From f5e004fbb36b6a6aa2bd24b77da7685835c0b107 Mon Sep 17 00:00:00 2001 From: nick6x Date: Wed, 17 Aug 2016 12:24:36 -0700 Subject: [PATCH] -Seperate AFSK, DDS, and KISS into their own libraries to conserve memory. --- .../CrystalCalibration/CrystalCalibration.ino | 1 + examples/DDS/DDS.ino | 1 + examples/HAMBot/HAMBot.ino | 2 +- examples/PSK31Transmit/PSK31Transmit.ino | 1 + examples/QPSK63Transmit/QPSK63Transmit.ino | 1 + examples/SSTV/SSTV.ino | 2 +- examples/SSTV_M1_Static/SSTV_M1_Static.ino | 1 + src/SimpleFIFO.h | 89 --- src/inProgress/AFSK.cpp | 738 ------------------ src/inProgress/AFSK.h | 301 ------- src/inProgress/DDS.cpp | 175 ----- src/inProgress/DDS.h | 228 ------ src/inProgress/KISS.cpp | 88 --- src/inProgress/KISS.h | 35 - 14 files changed, 7 insertions(+), 1656 deletions(-) delete mode 100644 src/SimpleFIFO.h delete mode 100644 src/inProgress/AFSK.cpp delete mode 100644 src/inProgress/AFSK.h delete mode 100644 src/inProgress/DDS.cpp delete mode 100644 src/inProgress/DDS.h delete mode 100644 src/inProgress/KISS.cpp delete mode 100644 src/inProgress/KISS.h diff --git a/examples/CrystalCalibration/CrystalCalibration.ino b/examples/CrystalCalibration/CrystalCalibration.ino index 4a2d4c6..e21e791 100644 --- a/examples/CrystalCalibration/CrystalCalibration.ino +++ b/examples/CrystalCalibration/CrystalCalibration.ino @@ -15,6 +15,7 @@ #define DDS_DEBUG_SERIAL #include +#include #define PWM_PIN 3 #define RESET_PIN A3 diff --git a/examples/DDS/DDS.ino b/examples/DDS/DDS.ino index 9352a4d..9f30ca1 100644 --- a/examples/DDS/DDS.ino +++ b/examples/DDS/DDS.ino @@ -12,6 +12,7 @@ #define DDS_REFCLK_DEFAULT 9600 #include +#include #define PWM_PIN 3 #define RESET_PIN A3 diff --git a/examples/HAMBot/HAMBot.ino b/examples/HAMBot/HAMBot.ino index 40b0c81..d66048a 100755 --- a/examples/HAMBot/HAMBot.ino +++ b/examples/HAMBot/HAMBot.ino @@ -12,7 +12,7 @@ * '6' => turn robot right * '2' => move robot forward * '5' => tell robot to send morse code identity -/* +*/ #include // include the robot library #include diff --git a/examples/PSK31Transmit/PSK31Transmit.ino b/examples/PSK31Transmit/PSK31Transmit.ino index a4bb331..bcc26b0 100644 --- a/examples/PSK31Transmit/PSK31Transmit.ino +++ b/examples/PSK31Transmit/PSK31Transmit.ino @@ -11,6 +11,7 @@ */ #include +#include #include "varicode.h" #define PWM_PIN 3 diff --git a/examples/QPSK63Transmit/QPSK63Transmit.ino b/examples/QPSK63Transmit/QPSK63Transmit.ino index bc9858d..e68ae76 100644 --- a/examples/QPSK63Transmit/QPSK63Transmit.ino +++ b/examples/QPSK63Transmit/QPSK63Transmit.ino @@ -11,6 +11,7 @@ */ #include +#include #include "varicode.h" #define PWM_PIN 3 diff --git a/examples/SSTV/SSTV.ino b/examples/SSTV/SSTV.ino index c5959e3..634aade 100755 --- a/examples/SSTV/SSTV.ino +++ b/examples/SSTV/SSTV.ino @@ -51,7 +51,7 @@ void setup() { void loop() { - if(radio.waitForChannel(1000,2000, rssi)) { // Wait forever for calling frequency to open, then wait 2 seconds for breakers + if(radio.waitForChannel(1000,2000, -90)) { // Wait forever for calling frequency to open, then wait 2 seconds for breakers radio.setModeTransmit(); // Turn on the transmitter delay(250); // Wait a moment radio.SSTVTestPattern(MARTIN1); // send a MARTIN1 test pattern diff --git a/examples/SSTV_M1_Static/SSTV_M1_Static.ino b/examples/SSTV_M1_Static/SSTV_M1_Static.ino index 4082aa8..5ac9858 100644 --- a/examples/SSTV_M1_Static/SSTV_M1_Static.ino +++ b/examples/SSTV_M1_Static/SSTV_M1_Static.ino @@ -14,6 +14,7 @@ #define DDS_REFCLK_DEFAULT (34965/2) #include +#include #define PWM_PIN 3 #define RESET_PIN A3 diff --git a/src/SimpleFIFO.h b/src/SimpleFIFO.h deleted file mode 100644 index 6965ce6..0000000 --- a/src/SimpleFIFO.h +++ /dev/null @@ -1,89 +0,0 @@ -#ifndef SimpleFIFO_h -#define SimpleFIFO_h -/* -|| -|| @file SimpleFIFO.h -|| @version 1.2 -|| @author Alexander Brevig -|| @contact alexanderbrevig@gmail.com -|| -|| @description -|| | A simple FIFO class, mostly for primitive types but can be used with classes if assignment to int is allowed -|| | This FIFO is not dynamic, so be sure to choose an appropriate size for it -|| # -|| -|| @license -|| | Copyright (c) 2010 Alexander Brevig -|| | This library is free software; you can redistribute it and/or -|| | modify it under the terms of the GNU Lesser General Public -|| | License as published by the Free Software Foundation; version -|| | 2.1 of the License. -|| | -|| | This library is distributed in the hope that it will be useful, -|| | but WITHOUT ANY WARRANTY; without even the implied warranty of -|| | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -|| | Lesser General Public License for more details. -|| | -|| | You should have received a copy of the GNU Lesser General Public -|| | License along with this library; if not, write to the Free Software -|| | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA -|| # -|| -*/ -template -class SimpleFIFO { -public: - const int size; //speculative feature, in case it's needed - - SimpleFIFO(); - - T dequeue(); //get next element - bool enqueue( T element ); //add an element - T peek() const; //get the next element without releasing it from the FIFO - void flush(); //[1.1] reset to default state - - //how many elements are currently in the FIFO? - unsigned char count() { return numberOfElements; } - -private: -#ifndef SimpleFIFO_NONVOLATILE - volatile unsigned char numberOfElements; - volatile unsigned char nextIn; - volatile unsigned char nextOut; - volatile T raw[rawSize]; -#else - unsigned char numberOfElements; - unsigned char nextIn; - unsigned char nextOut; - T raw[rawSize]; -#endif -}; - -template -SimpleFIFO::SimpleFIFO() : size(rawSize) { - flush(); -} -template -bool SimpleFIFO::enqueue( T element ) { - if ( count() >= rawSize ) { return false; } - numberOfElements++; - nextIn %= size; - raw[nextIn] = element; - nextIn++; //advance to next index - return true; -} -template -T SimpleFIFO::dequeue() { - numberOfElements--; - nextOut %= size; - return raw[ nextOut++]; -} -template -T SimpleFIFO::peek() const { - return raw[ nextOut % size]; -} -template -void SimpleFIFO::flush() { - nextIn = nextOut = numberOfElements = 0; -} -#endif diff --git a/src/inProgress/AFSK.cpp b/src/inProgress/AFSK.cpp deleted file mode 100644 index 1fcfab6..0000000 --- a/src/inProgress/AFSK.cpp +++ /dev/null @@ -1,738 +0,0 @@ -#include -#include "HamShield.h" -#include "SimpleFIFO.h" -#include - -#define PHASE_BIT 8 -#define PHASE_INC 1 - -#define PHASE_MAX (SAMPLEPERBIT * PHASE_BIT) -#define PHASE_THRES (PHASE_MAX / 2) - -#define BIT_DIFFER(bitline1, bitline2) (((bitline1) ^ (bitline2)) & 0x01) -#define EDGE_FOUND(bitline) BIT_DIFFER((bitline), (bitline) >> 1) - -#define PPOOL_SIZE 2 - -#define AFSK_SPACE 2200 -#define AFSK_MARK 1200 - -// Timers -volatile unsigned long lastTx = 0; -volatile unsigned long lastTxEnd = 0; -volatile unsigned long lastRx = 0; - -#define T_BIT ((unsigned int)(SAMPLERATE/BITRATE)) - -#ifdef PACKET_PREALLOCATE -SimpleFIFO preallocPool; -AFSK::Packet preallocPackets[PPOOL_SIZE]; -#endif - -void AFSK::Encoder::process() { - // We're on the start of a byte position, so fetch one - if(bitPosition == 0) { - if(preamble) { // Still in preamble - currentByte = HDLC_PREAMBLE; - --preamble; // Decrement by one - } else { - if(!packet) { // We aren't on a packet, grab one - // Unless we already sent enough - if(maxTx-- == 0) { - stop(); - lastTxEnd = millis(); - return; - } - packet = pBuf.getPacket(); - if(!packet) { // There actually weren't any - stop(); // Stop transmitting and return - lastTxEnd = millis(); - return; - } - lastTx = millis(); - currentBytePos = 0; - nextByte = HDLC_FRAME; // Our next output should be a frame boundary - hdlc = true; - } - - // We ran out of actual data, provide an HDLC frame (idle) - if(currentBytePos == packet->len && nextByte == 0) { - // We also get here if nextByte isn't set, to handle empty frames - pBuf.freePacket(packet); - packet = pBuf.getPacket(); // Get the next, if any - //packet = NULL; - currentBytePos = 0; - nextByte = 0; - currentByte = HDLC_FRAME; - hdlc = true; - } else { - if(nextByte) { - // We queued up something other than the actual stream to be sent next - currentByte = nextByte; - nextByte = 0; - } else { - // Get the next byte to send, but if it's an HDLC frame, escape it - // and queue the real byte for the next cycle. - currentByte = packet->getByte(); - if(currentByte == HDLC_FRAME) { - nextByte = currentByte; - currentByte = HDLC_ESCAPE; - } else { - currentBytePos++; - } - hdlc = false; // If we get here, it will be NRZI bit stuffed - } - } - } - } - - // Pickup the last bit - currentBit = currentByte & 0x1; - - if(lastZero == 5) { - currentBit = 0; // Force a 0 bit output - } else { - currentByte >>= 1; // Bit shift it right, for the next round - ++bitPosition; // Note our increase in position - } - - // To handle NRZI 5 bit stuffing, count the bits - if(!currentBit || hdlc) - lastZero = 0; - else - ++lastZero; - - // NRZI and AFSK uses toggling 0s, "no change" on 1 - // So, if not a 1, toggle to the opposite tone - if(!currentBit) - currentTone = !currentTone; - - if(currentTone == 0) { - PORTD |= _BV(7); - dds->setFrequency(AFSK_SPACE); - } else { - PORTD &= ~_BV(7); - dds->setFrequency(AFSK_MARK); - } -} - -bool AFSK::Encoder::start() { - if(!done || sending) { - return false; - } - - if(randomWait > millis()) { - return false; - } - - // First real byte is a frame - currentBit = 0; - lastZero = 0; - bitPosition = 0; - //bitClock = 0; - preamble = 0b110000; // 6.7ms each, 23 = 153ms - done = false; - hdlc = true; - packet = 0x0; // No initial packet, find in the ISR - currentBytePos = 0; - maxTx = 3; - sending = true; - nextByte = 0; - dds->setFrequency(0); - dds->on(); - return true; -} - -void AFSK::Encoder::stop() { - randomWait = 0; - sending = false; - done = true; - dds->setFrequency(0); - dds->off(); -} - -AFSK::Decoder::Decoder() { - // Initialize the sampler delay line (phase shift) - //for(unsigned char i = 0; i < SAMPLEPERBIT/2; i++) - // delay_fifo.enqueue(0); -} - -bool AFSK::HDLCDecode::hdlcParse(bool bit, SimpleFIFO *fifo) { - bool ret = true; - - demod_bits <<= 1; - demod_bits |= bit ? 1 : 0; - - // Flag - if(demod_bits == HDLC_FRAME) { - fifo->enqueue(HDLC_FRAME); - rxstart = true; - currchar = 0; - bit_idx = 0; - return ret; - } - - // Reset - if((demod_bits & HDLC_RESET) == HDLC_RESET) { - rxstart = false; - lastRx = millis(); - return ret; - } - if(!rxstart) { - return ret; - } - - // Stuffed? - if((demod_bits & 0x3f) == 0x3e) - return ret; - - if(demod_bits & 0x01) - currchar |= 0x80; - - if(++bit_idx >= 8) { - if(currchar == HDLC_FRAME || - currchar == HDLC_RESET || - currchar == HDLC_ESCAPE) { - fifo->enqueue(HDLC_ESCAPE); - } - fifo->enqueue(currchar & 0xff); - currchar = 0; - bit_idx = 0; - } else { - currchar >>= 1; - } - - return ret; -} - -template -class FastRing { -private: - T ring[size]; - uint8_t position; -public: - FastRing(): position(0) {} - inline void write(T value) { - ring[(position++) & (size-1)] = value; - } - inline T read() const { - return ring[position & (size-1)]; - } - inline T readn(uint8_t n) const { - return ring[(position + (~n+1)) & (size-1)]; - } -}; -// Create a delay line that's half the length of the bit cycle (-90 degrees) -FastRing delayLine; - -// Handle the A/D converter interrupt (hopefully quickly :) -void AFSK::Decoder::process(int8_t curr_sample) { - // Run the same through the phase multiplier and butterworth filter - iir_x[0] = iir_x[1]; - iir_x[1] = ((int8_t)delayLine.read() * curr_sample) >> 2; - iir_y[0] = iir_y[1]; - iir_y[1] = iir_x[0] + iir_x[1] + (iir_y[0] >> 1) + (iir_y[0]>>3) + (iir_y[0]>>5); - - // Place this ADC sample into the delay line - delayLine.write(curr_sample); - - // Shift the bit into place based on the output of the discriminator - sampled_bits <<= 1; - sampled_bits |= (iir_y[1] > 0) ? 1 : 0; - - // If we found a 0/1 transition, adjust phases to track - if(EDGE_FOUND(sampled_bits)) { - if(curr_phase < PHASE_THRES) - curr_phase += PHASE_INC; - else - curr_phase -= PHASE_INC; - } - - // Move ahead in phase - curr_phase += PHASE_BIT; - - // If we've gone over the phase maximum, we should now have some data - if(curr_phase >= PHASE_MAX) { - curr_phase %= PHASE_MAX; - found_bits <<= 1; - - // If we have 3 bits or more set, it's a positive bit - register uint8_t bits = sampled_bits & 0x07; - if(bits == 0x07 || bits == 0x06 || bits == 0x05 || bits == 0x03) { - found_bits |= 1; - } - - hdlc.hdlcParse(!EDGE_FOUND(found_bits), &rx_fifo); // Process it - } -} - -// This routine uses a pre-allocated Packet structure -// to save on the memory requirements of the stream data -bool AFSK::Decoder::read() { - bool retVal = false; - if(!currentPacket) { // We failed a prior memory allocation - currentPacket = pBuf.makePacket(PACKET_MAX_LEN); - if(!currentPacket) // Still nothing - return false; - } - // While we have AFSK receive FIFO bytes... - while(rx_fifo.count()) { - // Grab the character - char c = rx_fifo.dequeue(); - bool escaped = false; - if(c == HDLC_ESCAPE) { // We received an escaped byte, mark it - escaped = true; - // Do we want to keep HDLC_ESCAPEs in the packet? - //currentPacket->append(HDLC_ESCAPE); // Append without FCS - c = rx_fifo.dequeue(); // Reset to the next character - } - - // Append all the bytes - // This will include unescaped HDLC_FRAME bytes - if(c != HDLC_FRAME || escaped) // Append frame if it is escaped - currentPacket->appendFCS(c); // Escaped characters and all else go into FCS - - if(currentPacket->len > PACKET_MAX_LEN) { - // We've now gone too far and picked up far too many bytes - // Cancel this frame, start back at the beginning - currentPacket->clear(); - continue; - } - - // We have a frame boundary, if it isn't escaped - // If it's escaped, it was part of the data stream - if(c == HDLC_FRAME && !escaped) { - if(!currentPacket->len) { - currentPacket->clear(); // There wasn't any data, restart stream - continue; - } else { - // We have some bytes in stream, check it meets minimum payload length - // Min payload is 1 (flag) + 14 (addressing) + 2 (control/PID) + 1 (flag) - if(currentPacket->len >= 16) { - // We should end up here with a valid FCS due to the appendFCS - if(currentPacket->crcOK()) { // Magic number for the CRC check passing - // Valid frame, so, let's filter for control + PID - // Maximum search distance is 71 bytes to end of the address fields - // Skip the HDLC frame start - bool filtered = false; - for(unsigned char i = 0; i < (currentPacket->len<70?currentPacket->len:71); ++i) { - if((currentPacket->getByte() & 0x1) == 0x1) { // Found a byte with LSB set - // which marks the final address payload - // next two bytes should be the control/PID - //if(currentPacket->getByte() == 0x03 && currentPacket->getByte() == 0xf0) { - filtered = true; - break; // Found it - //} - } - } - - if(!filtered) { - // Frame wasn't one we care about, discard - currentPacket->clear(); - continue; - } - - // It's all done and formatted, ready to go - currentPacket->ready = 1; - if(!pBuf.putPacket(currentPacket)) // Put it in the receive FIFO - pBuf.freePacket(currentPacket); // Out of FIFO space, so toss it - - // Allocate a new one of maximum length - currentPacket = pBuf.makePacket(PACKET_MAX_LEN); - retVal = true; - } - } - } - // Restart the stream - currentPacket->clear(); - } - } - return retVal; // This is true if we parsed a packet in this flow -} - -void AFSK::Decoder::start() { - // Do this in start to allocate our first packet - currentPacket = pBuf.makePacket(PACKET_MAX_LEN); -/* ASSR &= ~(_BV(EXCLK) | _BV(AS2)); - - // Do non-inverting PWM on pin OC2B (arduino pin 3) (p.159). - // OC2A (arduino pin 11) stays in normal port operation: - // COM2B1=1, COM2B0=0, COM2A1=0, COM2A0=0 - // Mode 1 - Phase correct PWM - TCCR2A = (TCCR2A | _BV(COM2B1)) & ~(_BV(COM2B0) | _BV(COM2A1) | _BV(COM2A0)) | - _BV(WGM21) | _BV(WGM20); - // No prescaler (p.162) - TCCR2B = (TCCR2B & ~(_BV(CS22) | _BV(CS21))) | _BV(CS20) | _BV(WGM22); - - OCR2A = pow(2,COMPARE_BITS)-1; - OCR2B = 0; - // Configure the ADC and Timer1 to trigger automatic interrupts - TCCR1A = 0; - TCCR1B = _BV(CS11) | _BV(WGM13) | _BV(WGM12); - ICR1 = ((F_CPU / 8) / REFCLK) - 1; - ADMUX = _BV(REFS0) | _BV(ADLAR) | 0; // Channel 0, shift result left (ADCH used) - DDRC &= ~_BV(0); - PORTC &= ~_BV(0); - DIDR0 |= _BV(0); - ADCSRB = _BV(ADTS2) | _BV(ADTS1) | _BV(ADTS0); - ADCSRA = _BV(ADEN) | _BV(ADSC) | _BV(ADATE) | _BV(ADIE) | _BV(ADPS2); // | _BV(ADPS0); */ -} - -AFSK::PacketBuffer::PacketBuffer() { - nextPacketIn = 0; - nextPacketOut = 0; - inBuffer = 0; - for(unsigned char i = 0; i < PACKET_BUFFER_SIZE; ++i) { - packets[i] = 0x0; - } -#ifdef PACKET_PREALLOCATE - for(unsigned char i = 0; i < PPOOL_SIZE; ++i) { - // Put some empty packets in the FIFO - preallocPool.enqueue(&preallocPackets[i]); - } -#endif -} - -unsigned char AFSK::PacketBuffer::readyCount() volatile { - unsigned char i; - unsigned int cnt = 0; - ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { - for(i = 0; i < PACKET_BUFFER_SIZE; ++i) { - if(packets[i] && packets[i]->ready) - ++cnt; - } - } - return cnt; -} - -// Return NULL on empty packet buffers -AFSK::Packet *AFSK::PacketBuffer::getPacket() volatile { - unsigned char i = 0; - AFSK::Packet *p = NULL; - - ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { - if(inBuffer == 0) { - return 0x0; - } - - do { - p = packets[nextPacketOut]; - if(p) { - packets[nextPacketOut] = 0x0; - --inBuffer; - } - nextPacketOut = ++nextPacketOut % PACKET_BUFFER_SIZE; - ++i; - } while(!p && iinit(dlen); - } - return p; // Passes through a null on failure. -} - -// Free a packet struct, mainly convenience -void AFSK::PacketBuffer::freePacket(Packet *p) { - if(!p) - return; - ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { -#ifdef PACKET_PREALLOCATE - preallocPool.enqueue(p); -#else - p->free(); - /*unsigned char i; - for(i = 0; i < PPOOL_SIZE; ++i) - if(p == &(pPool[i])) - break; - if(i < PPOOL_SIZE) - pStatus &= ~(1<= PACKET_BUFFER_SIZE) { - return false; - } - packets[nextPacketIn] = p; - nextPacketIn = ++nextPacketIn % PACKET_BUFFER_SIZE; - ++inBuffer; - } - return true; -} - -// Print a single byte to the data array -size_t AFSK::Packet::write(uint8_t c) { - return (appendFCS(c)?1:0); -} - -size_t AFSK::Packet::write(const uint8_t *ptr, size_t len) { - size_t i; - for(i = 0; i < len; ++i) - if(!appendFCS(ptr[i])) - break; - return i; -} - -// Add a callsign, flagged as source, destination, or digi -// Also tell the routine the SSID to use and if this is the final callsign -size_t AFSK::Packet::appendCallsign(const char *callsign, uint8_t ssid, bool final) { - uint8_t i; - for(i = 0; i < strlen(callsign) && i < 6; i++) { - appendFCS(callsign[i]<<1); - } - if(i < 6) { - for(;i<6;i++) { - appendFCS(' '<<1); - } - } - uint8_t ssidField = (ssid&0xf) << 1; - // TODO: Handle digis in the address C bit - if(final) { - ssidField |= 0b01100001; - } else { - ssidField |= 0b11100000; - } - appendFCS(ssidField); -} - -#ifdef PACKET_PARSER -// Process the AX25 frame and turn it into a bunch of useful strings -bool AFSK::Packet::parsePacket() { - uint8_t *d = dataPtr; - int i; - - // First 7 bytes are destination-ssid - for(i = 0; i < 6; i++) { - dstCallsign[i] = (*d++)>>1; - if(dstCallsign[i] == ' ') { - dstCallsign[i] = '\0'; - } - } - dstCallsign[6] = '\0'; - dstSSID = ((*d++)>>1) & 0xF; - - // Next 7 bytes are source-ssid - for(i = 0; i < 6; i++) { - srcCallsign[i] = (*d++)>>1; - if(srcCallsign[i] == ' ') { - srcCallsign[i] = '\0'; - } - } - srcCallsign[6] = '\0'; - srcSSID = *d++; // Don't shift yet, we need the LSB - - digipeater[0][0] = '\0'; // Set null in case we have none anyway - if((srcSSID & 1) == 0) { // Not the last address field - int digi; // Which digi we're on - for(digi = 0; digi < 8; digi++) { - for(i = 0; i < 6; i++) { - digipeater[digi][i] = (*d++)>>1; - if(digipeater[digi][i] == ' ') { - digipeater[digi][i] = '\0'; - } - } - uint8_t last = (*d) & 1; - digipeaterSSID[digi] = ((*d++)>>1) & 0xF; - if(last == 1) - break; - } - digipeater[digi][6] = '\0'; - for(digi += 1; digi<8; digi++) { // Empty out the rest of them - digipeater[digi][0] = '\0'; - } - } - - // Now handle the SSID itself - srcSSID >>= 1; - srcSSID &= 0xF; - - // After the address parsing, we end up on the control field - control = *d++; - // We have a PID if control type is U or I - // Control & 1 == 0 == I frame - // Control & 3 == 3 == U frame - if((control & 1) == 0 || (control & 3) == 3) - pid = *d++; - else pid = 0; - - // If there is no PID, we have no data - if(!pid) { - iFrameData = NULL; - return true; - } - - // At this point, we've walked far enough along that data is just at d - iFrameData = d; - - // Cheat a little by setting the first byte of the FCS to 0, making it a string - // First FCS byte is found at -2, HDLC flags aren't in this buffer - dataPtr[len-2] = '\0'; - - return true; -} -#endif - -void AFSK::Packet::printPacket(Stream *s) { - uint8_t i; -#ifdef PACKET_PARSER - if(!parsePacket()) { - s->print(F("Packet not valid")); - return; - } - - s->print(srcCallsign); - if(srcSSID > 0) { - s->write('-'); - s->print(srcSSID); - } - s->print(F(" > ")); - s->print(dstCallsign); - if(dstSSID > 0) { - s->write('-'); - s->print(dstSSID); - } - s->write(' '); - if(digipeater[0][0] != '\0') { - s->print(F("via ")); - for(i = 0; i < 8; i++) { - if(digipeater[i][0] == '\0') - break; - s->print(digipeater[i]); - if(digipeaterSSID[i] != 0) { - s->write('-'); - s->print(digipeaterSSID[i]); - } - if((digipeaterSSID[i] & _BV(7)) == _BV(7)) { - s->write('*'); // Digipeated already - } - // If we might have more, check to add a comma - if(i < 7 && digipeater[i+1][0] != '\0') { - s->write(','); - } - s->write(' '); - } - } - - // This is an S frame, we can only print control info - if(control & 3 == 1) { - switch((control>>2)&3) { - case 0: - s->print(F("RR")); - break; - case 1: - s->print(F("RNR")); - break; - case 2: - s->print(F("REJ")); - break; - case 3: // Undefined - s->print(F("unk")); - break; - } - // Use a + to indicate poll bit - if(control & _BV(4) == _BV(4)) { - s->write('+'); - } - } else if((control & 3) == 3) { // U Frame - s->print(F("U(")); - s->print(control, HEX); - s->write(','); - s->print(pid, HEX); - s->print(F(") ")); - } else if((control & 1) == 0) { // I Frame - s->print(F("I(")); - s->print(control, HEX); - s->write(','); - s->print(pid, HEX); - s->print(F(") ")); - } - s->print(F("len ")); - s->print(len); - s->print(F(": ")); - s->print((char *)iFrameData); - s->println(); -#else // no packet parser, do a rudimentary print - // Second 6 bytes are source callsign - for(i=7; i<13; i++) { - s->write(*(dataPtr+i)>>1); - } - // SSID - s->write('-'); - s->print((*(dataPtr+13) >> 1) & 0xF); - s->print(F(" -> ")); - // First 6 bytes are destination callsign - for(i=0; i<6; i++) { - s->write(*(dataPtr+i)>>1); - } - // SSID - s->write('-'); - s->print((*(dataPtr+6) >> 1) & 0xF); - // Control/PID next two bytes - // Skip those, print payload - for(i = 15; iwrite(*(dataPtr+i)); - } -#endif -} - -// Determine what we want to do on this ADC tick. -void AFSK::timer() { - static uint8_t tcnt = 0; - if(++tcnt == T_BIT && encoder.isSending()) { - PORTD |= _BV(6); - // Only run the encoder every 8th tick - // This is actually DDS RefClk / 1200 = 8, set as T_BIT - // A different refclk needs a different value - encoder.process(); - tcnt = 0; - PORTD &= ~_BV(6); - } else { - decoder.process(((int8_t)(ADCH - 128))); - } -} - -void AFSK::start(DDS *dds) { - afskEnabled = true; - encoder.setDDS(dds); - decoder.start(); -} diff --git a/src/inProgress/AFSK.h b/src/inProgress/AFSK.h deleted file mode 100644 index e7b2418..0000000 --- a/src/inProgress/AFSK.h +++ /dev/null @@ -1,301 +0,0 @@ -#ifndef _AFSK_H_ -#define _AFSK_H_ - -#include -#include -#include - -#define SAMPLERATE 9600 -#define BITRATE 1200 - -#define SAMPLEPERBIT (SAMPLERATE / BITRATE) - -#define RX_FIFO_LEN 16 - -#define PACKET_BUFFER_SIZE 2 -#define PACKET_STATIC 0 - -// Enable the packet parser which will tokenize the AX25 frame into easy strings -#define PACKET_PARSER - -// If this is set, all the packet buffers will be pre-allocated at compile time -// This will use more RAM, but can make it easier to do memory planning. -// TODO: Make this actually work right and not crash. -#define PACKET_PREALLOCATE - -// This is with all the digis, two addresses, and full payload -// Dst(7) + Src(7) + Digis(56) + Ctl(1) + PID(1) + Data(0-256) + FCS(2) -#define PACKET_MAX_LEN 330 -// Minimum is Dst + Src + Ctl + FCS -#define AX25_PACKET_HEADER_MINLEN 17 - -// HDLC framing bits -#define HDLC_FRAME 0x7E -#define HDLC_RESET 0x7F -#define HDLC_PREAMBLE 0x00 -#define HDLC_ESCAPE 0x1B -#define HDLC_TAIL 0x1C - -class AFSK { -private: - volatile bool afskEnabled; -public: - bool enabled() { return afskEnabled; }; - - class Packet:public Print { - public: - Packet():Print() {}; - virtual size_t write(uint8_t); - // Stock virtual method does what we want here. - //virtual size_t write(const char *); - virtual size_t write(const uint8_t *, size_t); - using Print::write; - unsigned char ready : 1; - unsigned char type : 2; - unsigned char freeData : 1; - unsigned short len; - unsigned short maxLen; - //void init(uint8_t *buf, unsigned int dlen, bool freeData); - void init(unsigned short dlen); - inline void free() { - if(freeData) - ::free(dataPtr); - } - inline const unsigned char getByte(void) { - return *readPos++; - } - inline const unsigned char getByte(uint16_t p) { - return *(dataPtr+p); - } - inline void start() { - fcs = 0xffff; - // No longer put an explicit frame start here - //*dataPos++ = HDLC_ESCAPE; - //*dataPos++ = HDLC_FRAME; - //len = 2; - len = 0; - } - - inline bool append(char c) { - if(len < maxLen) { - ++len; - *dataPos++ = c; - return true; - } - return false; - } - - #define UPDATE_FCS(d) e=fcs^(d); f=e^(e<<4); fcs=(fcs>>8)^(f<<8)^(f<<3)^(f>>4) - //#define UPDATE_FCS(d) s=(d)^(fcs>>8); t=s^(s>>4); fcs=(fcs<<8)^t^(t<<5)^(t<<12) - inline bool appendFCS(unsigned char c) { - register unsigned char e, f; - if(len < maxLen - 4) { // Leave room for FCS/HDLC - append(c); - UPDATE_FCS(c); - return true; - } - return false; - } - - size_t appendCallsign(const char *callsign, uint8_t ssid, bool final = false); - - inline void finish() { - append(~(fcs & 0xff)); - append(~((fcs>>8) & 0xff)); - // No longer append the frame boundaries themselves - //append(HDLC_ESCAPE); - //append(HDLC_FRAME); - ready = 1; - } - - inline void clear() { - fcs = 0xffff; - len = 0; - readPos = dataPtr; - dataPos = dataPtr; - } - - inline bool crcOK() { - return (fcs == 0xF0B8); - } -#ifdef PACKET_PARSER - bool parsePacket(); -#endif - void printPacket(Stream *s); - private: -#ifdef PACKET_PREALLOCATE - uint8_t dataPtr[PACKET_MAX_LEN]; // 256 byte I frame + headers max of 78 -#else - uint8_t *dataPtr; -#endif -#ifdef PACKET_PARSER - char srcCallsign[7]; - uint8_t srcSSID; - char dstCallsign[7]; - uint8_t dstSSID; - char digipeater[8][7]; - uint8_t digipeaterSSID[8]; - uint8_t *iFrameData; - uint8_t length; - uint8_t control; - uint8_t pid; -#endif - uint8_t *dataPos, *readPos; - unsigned short fcs; - }; - - - class PacketBuffer { - public: - // Initialize the buffers - PacketBuffer(); - // How many packets are in the buffer? - unsigned char count() volatile { return inBuffer; }; - // And how many of those are ready? - unsigned char readyCount() volatile; - // Retrieve the next packet - Packet *getPacket() volatile; - // Create a packet structure as needed - // This does not place it in the queue - static Packet *makePacket(unsigned short); - // Conveniently free packet memory - static void freePacket(Packet *); - // Place a packet into the buffer - bool putPacket(Packet *) volatile; - private: - volatile unsigned char inBuffer; - Packet * volatile packets[PACKET_BUFFER_SIZE]; - volatile unsigned char nextPacketIn; - volatile unsigned char nextPacketOut; - }; - - class Encoder { - public: - Encoder() { - randomWait = 1000; // At the very begin, wait at least one second - sending = false; - done = true; - packet = 0x0; - currentBytePos = 0; - nextByte = 0; - } - void setDDS(DDS *d) { dds = d; } - volatile inline bool isSending() volatile { - return sending; - } - volatile inline bool isDone() volatile { - return done; - } - volatile inline bool hasPackets() volatile { - return (pBuf.count() > 0); - } - inline bool putPacket(Packet *packet) { - return pBuf.putPacket(packet); - } - inline void setRandomWait() { - randomWait = 250 + (rand() % 1000) + millis(); - } - bool start(); - void stop(); - void process(); - private: - volatile bool sending; - byte currentByte; - byte currentBit : 1; - byte currentTone : 1; - byte lastZero : 3; - byte bitPosition : 3; - byte preamble : 6; - //byte bitClock; - bool hdlc; - byte nextByte; - byte maxTx; - Packet *packet; - PacketBuffer pBuf; - unsigned int currentBytePos; - volatile unsigned long randomWait; - volatile bool done; - DDS *dds; - }; - - class HDLCDecode { - public: - bool hdlcParse(bool, SimpleFIFO *fifo); - volatile bool rxstart; - private: - uint8_t demod_bits; - uint8_t bit_idx; - uint8_t currchar; - }; - - class Decoder { - public: - Decoder(); - void start(); - bool read(); - void process(int8_t); - inline bool dataAvailable() { - return (rx_fifo.count() > 0); - } - inline uint8_t getByte() { - return rx_fifo.dequeue(); - } - inline uint8_t packetCount() volatile { - return pBuf.count(); - } - inline Packet *getPacket() { - return pBuf.getPacket(); - } - inline bool isReceiving() volatile { - return hdlc.rxstart; - } - private: - Packet *currentPacket; - //SimpleFIFO delay_fifo; - SimpleFIFO rx_fifo; // This should be drained fairly often - int16_t iir_x[2]; - int16_t iir_y[2]; - uint8_t sampled_bits; - int8_t curr_phase; - uint8_t found_bits; - PacketBuffer pBuf; - HDLCDecode hdlc; - }; - -public: - inline bool read() { - return decoder.read(); - } - volatile inline bool txReady() volatile { - if(encoder.isDone() && encoder.hasPackets()) - return true; - return false; - } - volatile inline bool isDone() volatile { return encoder.isDone(); } - inline bool txStart() { - if(decoder.isReceiving()) { - encoder.setRandomWait(); - return false; - } - return encoder.start(); - } - inline bool putTXPacket(Packet *packet) { - bool ret = encoder.putPacket(packet); - if(!ret) // No room? - PacketBuffer::freePacket(packet); - return ret; - } - inline Packet *getRXPacket() { - return decoder.getPacket(); - } - inline uint8_t rxPacketCount() volatile { - return decoder.packetCount(); - } - //unsigned long lastTx; - //unsigned long lastRx; - void start(DDS *); - void timer(); - Encoder encoder; - Decoder decoder; -}; -#endif /* _AFSK_H_ */ diff --git a/src/inProgress/DDS.cpp b/src/inProgress/DDS.cpp deleted file mode 100644 index 8772fc7..0000000 --- a/src/inProgress/DDS.cpp +++ /dev/null @@ -1,175 +0,0 @@ -#include -#include "DDS.h" - -// To start the DDS, we use Timer1, set to the reference clock -// We use Timer2 for the PWM output, running as fast as feasible -void DDS::start() { - // Use the clkIO clock rate - ASSR &= ~(_BV(EXCLK) | _BV(AS2)); - - // First, the timer for the PWM output - // Setup the timer to use OC2B (pin 3) in fast PWM mode with a configurable top - // Run it without the prescaler -#ifdef DDS_PWM_PIN_3 - TCCR2A = (TCCR2A | _BV(COM2B1)) & ~(_BV(COM2B0) | _BV(COM2A1) | _BV(COM2A0)) | - _BV(WGM21) | _BV(WGM20); - TCCR2B = (TCCR2B & ~(_BV(CS22) | _BV(CS21))) | _BV(CS20) | _BV(WGM22); -#else - // Alternatively, use pin 11 - // Enable output compare on OC2A, toggle mode - TCCR2A = _BV(COM2A1) | _BV(WGM21) | _BV(WGM20); - //TCCR2A = (TCCR2A | _BV(COM2A1)) & ~(_BV(COM2A0) | _BV(COM2B1) | _BV(COM2B0)) | - // _BV(WGM21) | _BV(WGM20); - TCCR2B = _BV(CS20); -#endif - - // Set the top limit, which will be our duty cycle accuracy. - // Setting Comparator Bits smaller will allow for higher frequency PWM, - // with the loss of resolution. -#ifdef DDS_PWM_PIN_3 - OCR2A = pow(2,COMPARATOR_BITS)-1; - OCR2B = 0; -#else - OCR2A = 0; -#endif - -#ifdef DDS_USE_ONLY_TIMER2 - TIMSK2 |= _BV(TOIE2); -#endif - - // Second, setup Timer1 to trigger the ADC interrupt - // This lets us use decoding functions that run at the same reference - // clock as the DDS. - // We use ICR1 as TOP and prescale by 8 - TCCR1B = _BV(CS10) | _BV(WGM13) | _BV(WGM12); - TCCR1A = 0; - ICR1 = ((F_CPU / 1) / refclk) - 1; -#ifdef DDS_DEBUG_SERIAL - Serial.print(F("DDS SysClk: ")); - Serial.println(F_CPU/8); - Serial.print(F("DDS RefClk: ")); - Serial.println(refclk, DEC); - Serial.print(F("DDS ICR1: ")); - Serial.println(ICR1, DEC); -#endif - - // Configure the ADC here to automatically run and be triggered off Timer1 - ADMUX = _BV(REFS0) | _BV(ADLAR) | 0; // Channel 0, shift result left (ADCH used) - DDRC &= ~_BV(0); - PORTC &= ~_BV(0); - DIDR0 |= _BV(0); - ADCSRB = _BV(ADTS2) | _BV(ADTS1) | _BV(ADTS0); - ADCSRA = _BV(ADEN) | _BV(ADSC) | _BV(ADATE) | _BV(ADIE) | _BV(ADPS2); // | _BV(ADPS0); -} - -void DDS::stop() { - // TODO: Stop the timers. -#ifndef DDS_USE_ONLY_TIMER2 - TCCR1B = 0; -#endif - TCCR2B = 0; -} - -// Set our current sine wave frequency in Hz -ddsAccumulator_t DDS::calcFrequency(unsigned short freq) { - // Fo = (M*Fc)/2^N - // M = (Fo/Fc)*2^N - ddsAccumulator_t newStep; - if(refclk == DDS_REFCLK_DEFAULT) { - // Try to use precalculated values if possible - if(freq == 2200) { - newStep = (2200.0 / (DDS_REFCLK_DEFAULT+DDS_REFCLK_OFFSET)) * pow(2,ACCUMULATOR_BITS); - } else if (freq == 1200) { - newStep = (1200.0 / (DDS_REFCLK_DEFAULT+DDS_REFCLK_OFFSET)) * pow(2,ACCUMULATOR_BITS); - } else if(freq == 2400) { - newStep = (2400.0 / (DDS_REFCLK_DEFAULT+DDS_REFCLK_OFFSET)) * pow(2,ACCUMULATOR_BITS); - } else if (freq == 1500) { - newStep = (1500.0 / (DDS_REFCLK_DEFAULT+DDS_REFCLK_OFFSET)) * pow(2,ACCUMULATOR_BITS); - } else if (freq == 600) { - newStep = (600.0 / (DDS_REFCLK_DEFAULT+DDS_REFCLK_OFFSET)) * pow(2,ACCUMULATOR_BITS); - } - } else { - newStep = pow(2,ACCUMULATOR_BITS)*freq / (refclk+refclkOffset); - } - return newStep; -} - -// Degrees should be between -360 and +360 (others don't make much sense) -void DDS::setPhaseDeg(int16_t degrees) { - accumulator = degrees * (pow(2,ACCUMULATOR_BITS)/360.0); -} -void DDS::changePhaseDeg(int16_t degrees) { - accumulator += degrees * (pow(2,ACCUMULATOR_BITS)/360.0); -} - -// TODO: Clean this up a bit.. -void DDS::clockTick() { -/* if(running) { - accumulator += stepRate; - OCR2A = getDutyCycle(); - } - return;*/ - if(running) { - accumulator += stepRate; - if(timeLimited && tickDuration == 0) { -#ifndef DDS_PWM_PIN_3 - OCR2A = 0; -#else -#ifdef DDS_IDLE_HIGH - // Set the duty cycle to 50% - OCR2B = pow(2,COMPARATOR_BITS)/2; -#else - // Set duty cycle to 0, effectively off - OCR2B = 0; -#endif -#endif - running = false; - accumulator = 0; - } else { -#ifdef DDS_PWM_PIN_3 - OCR2B = getDutyCycle(); -#else - OCR2A = getDutyCycle(); -#endif - } - // Reduce our playback duration by one tick - tickDuration--; - } else { - // Hold it low -#ifndef DDS_PWM_PIN_3 - OCR2A = 0; -#else -#ifdef DDS_IDLE_HIGH - // Set the duty cycle to 50% - OCR2B = pow(2,COMPARATOR_BITS)/2; -#else - // Set duty cycle to 0, effectively off - OCR2B = 0; -#endif -#endif - } -} - -uint8_t DDS::getDutyCycle() { - #if ACCUMULATOR_BIT_SHIFT >= 24 - uint16_t phAng; - #else - uint8_t phAng; - #endif - if(amplitude == 0) // Shortcut out on no amplitude - return 128>>(8-COMPARATOR_BITS); - phAng = (accumulator >> ACCUMULATOR_BIT_SHIFT); - int8_t position = pgm_read_byte_near(ddsSineTable + phAng); //>>(8-COMPARATOR_BITS); - // Apply scaling and return - int16_t scaled = position; - // output = ((duty * amplitude) / 256) + 128 - // This keeps amplitudes centered around 50% duty - if(amplitude != 255) { // Amplitude is reduced, so do the full math - scaled *= amplitude; - scaled >>= 8+(8-COMPARATOR_BITS); - } else { // Otherwise, only shift for the comparator bits - scaled >>= (8-COMPARATOR_BITS); - } - scaled += 128>>(8-COMPARATOR_BITS); - return scaled; -} diff --git a/src/inProgress/DDS.h b/src/inProgress/DDS.h deleted file mode 100644 index 57cf40b..0000000 --- a/src/inProgress/DDS.h +++ /dev/null @@ -1,228 +0,0 @@ -#ifndef _DDS_H_ -#define _DDS_H_ - -#include - -// Use pin 3 for PWM? If not defined, use pin 11 -// Quality on pin 3 is higher than on 11, as it can be clocked faster -// when the COMPARATOR_BITS value is less than 8 -#define DDS_PWM_PIN_3 - -// Normally, we turn on timer2 and timer1, and have ADC sampling as our clock -// Define this to only use Timer2, and not start the ADC clock -// #define DDS_USE_ONLY_TIMER2 - -// Use a short (16 bit) accumulator. Phase accuracy is reduced, but speed -// is increased, along with a reduction in memory use. -#define SHORT_ACCUMULATOR - -#ifdef SHORT_ACCUMULATOR -#define ACCUMULATOR_BITS 16 -typedef uint16_t ddsAccumulator_t; -#else -#define ACCUMULATOR_BITS 32 -typedef uint32_t ddsAccumulator_t; -#endif - -// If defined, the timer will idle at 50% duty cycle -// This leaves it floating in the centre of the PWM/DAC voltage range -#define DDS_IDLE_HIGH - -// Select how fast the PWM is, at the expense of level accuracy. -// A faster PWM rate will make for easier filtering of the output wave, -// while a slower one will allow for more accurate voltage level outputs, -// but will increase the filtering requirements on the output. -// 8 = 62.5kHz PWM -// 7 = 125kHz PWM -// 6 = 250kHz PWM -#ifdef DDS_PWM_PIN_3 -#define COMPARATOR_BITS 6 -#else // When using pin 11, we always want 8 bits -#define COMPARATOR_BITS 8 -#endif - -// This is how often we'll perform a phase advance, as well as ADC sampling -// rate. The higher this value, the smoother the output wave will be, at the -// expense of CPU time. It maxes out around 62000 (TBD) -// May be overridden in the sketch to improve performance -#ifndef DDS_REFCLK_DEFAULT -#define DDS_REFCLK_DEFAULT 9600 -#endif -// As each Arduino crystal is a little different, this can be fine tuned to -// provide more accurate frequencies. Adjustments in the range of hundreds -// is a good start. -#ifndef DDS_REFCLK_OFFSET -#define DDS_REFCLK_OFFSET 0 -#endif - -#ifdef DDS_USE_ONLY_TIMER2 -// TODO: Figure out where this clock value is generated from -#define DDS_REFCLK_DEFAULT (62500/4) -#endif - -// Output some of the calculations and information about the DDS over serial -//#define DDS_DEBUG_SERIAL - -// When defined, use the 1024 element sine lookup table. This improves phase -// accuracy, at the cost of more flash and CPU requirements. -// #define DDS_TABLE_LARGE - -#ifdef DDS_TABLE_LARGE -// How many bits to keep from the accumulator to look up in this table -#define ACCUMULATOR_BIT_SHIFT (ACCUMULATOR_BITS-10) -static const int8_t ddsSineTable[1024] PROGMEM = { - 0, 0, 1, 2, 3, 3, 4, 5, 6, 7, 7, 8, 9, 10, 10, 11, 12, 13, 13, 14, 15, 16, 17, 17, 18, 19, 20, 20, 21, 22, 23, 24, - 24, 25, 26, 27, 27, 28, 29, 30, 30, 31, 32, 33, 33, 34, 35, 36, 36, 37, 38, 39, 39, 40, 41, 42, 42, 43, 44, 44, 45, 46, 47, 47, - 48, 49, 50, 50, 51, 52, 52, 53, 54, 55, 55, 56, 57, 57, 58, 59, 59, 60, 61, 61, 62, 63, 63, 64, 65, 65, 66, 67, 67, 68, 69, 69, - 70, 71, 71, 72, 73, 73, 74, 75, 75, 76, 76, 77, 78, 78, 79, 79, 80, 81, 81, 82, 82, 83, 84, 84, 85, 85, 86, 87, 87, 88, 88, 89, - 89, 90, 90, 91, 91, 92, 93, 93, 94, 94, 95, 95, 96, 96, 97, 97, 98, 98, 99, 99, 100, 100, 101, 101, 102, 102, 102, 103, 103, 104, 104, 105, - 105, 106, 106, 106, 107, 107, 108, 108, 108, 109, 109, 110, 110, 110, 111, 111, 112, 112, 112, 113, 113, 113, 114, 114, 114, 115, 115, 115, 116, 116, 116, 117, - 117, 117, 117, 118, 118, 118, 119, 119, 119, 119, 120, 120, 120, 120, 121, 121, 121, 121, 121, 122, 122, 122, 122, 123, 123, 123, 123, 123, 123, 124, 124, 124, - 124, 124, 124, 124, 125, 125, 125, 125, 125, 125, 125, 125, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, - 127, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 126, 125, 125, 125, 125, 125, 125, 125, 125, 124, 124, 124, - 124, 124, 124, 124, 123, 123, 123, 123, 123, 123, 122, 122, 122, 122, 121, 121, 121, 121, 121, 120, 120, 120, 120, 119, 119, 119, 119, 118, 118, 118, 117, 117, - 117, 117, 116, 116, 116, 115, 115, 115, 114, 114, 114, 113, 113, 113, 112, 112, 112, 111, 111, 110, 110, 110, 109, 109, 108, 108, 108, 107, 107, 106, 106, 106, - 105, 105, 104, 104, 103, 103, 102, 102, 102, 101, 101, 100, 100, 99, 99, 98, 98, 97, 97, 96, 96, 95, 95, 94, 94, 93, 93, 92, 91, 91, 90, 90, - 89, 89, 88, 88, 87, 87, 86, 85, 85, 84, 84, 83, 82, 82, 81, 81, 80, 79, 79, 78, 78, 77, 76, 76, 75, 75, 74, 73, 73, 72, 71, 71, - 70, 69, 69, 68, 67, 67, 66, 65, 65, 64, 63, 63, 62, 61, 61, 60, 59, 59, 58, 57, 57, 56, 55, 55, 54, 53, 52, 52, 51, 50, 50, 49, - 48, 47, 47, 46, 45, 44, 44, 43, 42, 42, 41, 40, 39, 39, 38, 37, 36, 36, 35, 34, 33, 33, 32, 31, 30, 30, 29, 28, 27, 27, 26, 25, - 24, 24, 23, 22, 21, 20, 20, 19, 18, 17, 17, 16, 15, 14, 13, 13, 12, 11, 10, 10, 9, 8, 7, 7, 6, 5, 4, 3, 3, 2, 1, 0, - 0, 0, -1, -2, -3, -3, -4, -5, -6, -7, -7, -8, -9, -10, -10, -11, -12, -13, -13, -14, -15, -16, -17, -17, -18, -19, -20, -20, -21, -22, -23, -24, - -24, -25, -26, -27, -27, -28, -29, -30, -30, -31, -32, -33, -33, -34, -35, -36, -36, -37, -38, -39, -39, -40, -41, -42, -42, -43, -44, -44, -45, -46, -47, -47, - -48, -49, -50, -50, -51, -52, -52, -53, -54, -55, -55, -56, -57, -57, -58, -59, -59, -60, -61, -61, -62, -63, -63, -64, -65, -65, -66, -67, -67, -68, -69, -69, - -70, -71, -71, -72, -73, -73, -74, -75, -75, -76, -76, -77, -78, -78, -79, -79, -80, -81, -81, -82, -82, -83, -84, -84, -85, -85, -86, -87, -87, -88, -88, -89, - -89, -90, -90, -91, -91, -92, -93, -93, -94, -94, -95, -95, -96, -96, -97, -97, -98, -98, -99, -99, -100, -100, -101, -101, -102, -102, -102, -103, -103, -104, -104, -105, - -105, -106, -106, -106, -107, -107, -108, -108, -108, -109, -109, -110, -110, -110, -111, -111, -112, -112, -112, -113, -113, -113, -114, -114, -114, -115, -115, -115, -116, -116, -116, -117, - -117, -117, -117, -118, -118, -118, -119, -119, -119, -119, -120, -120, -120, -120, -121, -121, -121, -121, -121, -122, -122, -122, -122, -123, -123, -123, -123, -123, -123, -124, -124, -124, - -124, -124, -124, -124, -125, -125, -125, -125, -125, -125, -125, -125, -126, -126, -126, -126, -126, -126, -126, -126, -126, -126, -126, -126, -126, -126, -126, -126, -126, -126, -126, -126, - -127, -126, -126, -126, -126, -126, -126, -126, -126, -126, -126, -126, -126, -126, -126, -126, -126, -126, -126, -126, -126, -125, -125, -125, -125, -125, -125, -125, -125, -124, -124, -124, - -124, -124, -124, -124, -123, -123, -123, -123, -123, -123, -122, -122, -122, -122, -121, -121, -121, -121, -121, -120, -120, -120, -120, -119, -119, -119, -119, -118, -118, -118, -117, -117, - -117, -117, -116, -116, -116, -115, -115, -115, -114, -114, -114, -113, -113, -113, -112, -112, -112, -111, -111, -110, -110, -110, -109, -109, -108, -108, -108, -107, -107, -106, -106, -106, - -105, -105, -104, -104, -103, -103, -102, -102, -102, -101, -101, -100, -100, -99, -99, -98, -98, -97, -97, -96, -96, -95, -95, -94, -94, -93, -93, -92, -91, -91, -90, -90, - -89, -89, -88, -88, -87, -87, -86, -85, -85, -84, -84, -83, -82, -82, -81, -81, -80, -79, -79, -78, -78, -77, -76, -76, -75, -75, -74, -73, -73, -72, -71, -71, - -70, -69, -69, -68, -67, -67, -66, -65, -65, -64, -63, -63, -62, -61, -61, -60, -59, -59, -58, -57, -57, -56, -55, -55, -54, -53, -52, -52, -51, -50, -50, -49, - -48, -47, -47, -46, -45, -44, -44, -43, -42, -42, -41, -40, -39, -39, -38, -37, -36, -36, -35, -34, -33, -33, -32, -31, -30, -30, -29, -28, -27, -27, -26, -25, - -24, -24, -23, -22, -21, -20, -20, -19, -18, -17, -17, -16, -15, -14, -13, -13, -12, -11, -10, -10, -9, -8, -7, -7, -6, -5, -4, -3, -3, -2, -1, 0 -}; -#else -#define ACCUMULATOR_BIT_SHIFT (ACCUMULATOR_BITS-8) -static const int8_t ddsSineTable[256] PROGMEM = { - 0, 3, 6, 9, 12, 16, 19, 22, 25, 28, 31, 34, 37, 40, 43, 46, 49, - 51, 54, 57, 60, 63, 65, 68, 71, 73, 76, 78, 81, 83, 85, 88, 90, - 92, 94, 96, 98, 100, 102, 104, 106, 107, 109, 111, 112, 113, 115, - 116, 117, 118, 120, 121, 122, 122, 123, 124, 125, 125, 126, 126, - 126, 127, 127, 127, 127, 127, 127, 127, 126, 126, 126, 125, 125, - 124, 123, 122, 122, 121, 120, 118, 117, 116, 115, 113, 112, 111, - 109, 107, 106, 104, 102, 100, 98, 96, 94, 92, 90, 88, 85, 83, 81, - 78, 76, 73, 71, 68, 65, 63, 60, 57, 54, 51, 49, 46, 43, 40, 37, - 34, 31, 28, 25, 22, 19, 16, 12, 9, 6, 3, 0, -3, -6, -9, -12, -16, - -19, -22, -25, -28, -31, -34, -37, -40, -43, -46, -49, -51, -54, - -57, -60, -63, -65, -68, -71, -73, -76, -78, -81, -83, -85, -88, - -90, -92, -94, -96, -98, -100, -102, -104, -106, -107, -109, -111, - -112, -113, -115, -116, -117, -118, -120, -121, -122, -122, -123, - -124, -125, -125, -126, -126, -126, -127, -127, -127, -127, -127, - -127, -127, -126, -126, -126, -125, -125, -124, -123, -122, -122, - -121, -120, -118, -117, -116, -115, -113, -112, -111, -109, -107, - -106, -104, -102, -100, -98, -96, -94, -92, -90, -88, -85, -83, - -81, -78, -76, -73, -71, -68, -65, -63, -60, -57, -54, -51, -49, - -46, -43, -40, -37, -34, -31, -28, -25, -22, -19, -16, -12, -9, -6, -3 - }; -#endif /* DDS_TABLE_LARGE */ - -class DDS { -public: - DDS(): refclk(DDS_REFCLK_DEFAULT), refclkOffset(DDS_REFCLK_OFFSET), - accumulator(0), running(false), - timeLimited(false), tickDuration(0), amplitude(255) - {}; - - // Start all of the timers needed - void start(); - // Is the DDS presently producing a tone? - const bool isRunning() { return running; }; - // Stop the DDS timers - void stop(); - - // Start and stop the PWM output - void on() { - timeLimited = false; - running = true; - } - // Provide a duration in ms for the tone - void on(unsigned short duration) { - // Duration in ticks from milliseconds is: - // t = (1/refclk) - tickDuration = (unsigned long)((unsigned long)duration * (unsigned long)refclk) / 1000; - timeLimited = true; - running = true; - } - void off() { - running = false; - } - - // Generate a tone for a specific amount of time - void play(unsigned short freq, unsigned short duration) { - setFrequency(freq); - on(duration); - } - // Blocking version - void playWait(unsigned short freq, unsigned short duration) { - play(freq, duration); - delay(duration); - } - - // Use these to get some calculated values for specific frequencies - // or to get the current frequency stepping rate. - ddsAccumulator_t calcFrequency(unsigned short freq); - ddsAccumulator_t getPhaseAdvance() { return stepRate; }; - - // Our maximum clock isn't very high, so our highest - // frequency supported will fit in a short. - void setFrequency(unsigned short freq) { stepRate = calcFrequency(freq); }; - void setPrecalcFrequency(ddsAccumulator_t freq) { stepRate = freq; }; - - // Handle phase shifts - void setPhaseDeg(int16_t degrees); - void changePhaseDeg(int16_t degrees); - - // Adjustable reference clock. This shoud be done before the timers are - // started, or they will need to be restarted. Frequencies will need to - // be set again to use the new clock. - void setReferenceClock(unsigned long ref) { - refclk = ref; - } - unsigned long getReferenceClock() { - return refclk; - } - - void setReferenceOffset(int16_t offset) { - refclkOffset = offset; - } - int16_t getReferenceOffset() { - return refclkOffset; - } - - uint8_t getDutyCycle(); - - // Set a scaling factor. To keep things quick, this is a power of 2 value. - // Set it with 0 for lowest (which will be off), 8 is highest. - void setAmplitude(unsigned char amp) { - amplitude = amp; - } - - // This is the function called by the ADC_vect ISR to produce the tones - void clockTick(); - -private: - volatile bool running; - volatile unsigned long tickDuration; - volatile bool timeLimited; - volatile unsigned char amplitude; - volatile ddsAccumulator_t accumulator; - volatile ddsAccumulator_t stepRate; - ddsAccumulator_t refclk; - int16_t refclkOffset; - static DDS *sDDS; -}; - -#endif /* _DDS_H_ */ diff --git a/src/inProgress/KISS.cpp b/src/inProgress/KISS.cpp deleted file mode 100644 index fdcd52a..0000000 --- a/src/inProgress/KISS.cpp +++ /dev/null @@ -1,88 +0,0 @@ -#include -#include "AFSK.h" -#include "KISS.h" - -//AFSK::Packet kissPacket; -bool inFrame = false; -uint8_t kissBuffer[PACKET_MAX_LEN]; -uint16_t kissLen = 0; - -// Inside the KISS loop, we basically wait for data to come in from the -// KISS equipment, and look if we have anything to relay along -void KISS::loop() { - static bool currentlySending = false; - if(radio->afsk.decoder.read() || radio->afsk.rxPacketCount()) { - // A true return means something was put onto the packet FIFO - // If we actually have data packets in the buffer, process them all now - while(radio->afsk.rxPacketCount()) { - AFSK::Packet *packet = radio->afsk.getRXPacket(); - if(packet) { - writePacket(packet); - AFSK::PacketBuffer::freePacket(packet); - } - } - } - // Check if we have incoming data to turn into a packet - while(io->available()) { - uint8_t c = (uint8_t)io->read(); - if(c == KISS_FEND) { - if(inFrame && kissLen > 0) { - int i; - AFSK::Packet *packet = AFSK::PacketBuffer::makePacket(PACKET_MAX_LEN); - packet->start(); - for(i = 0; i < kissLen; i++) { - packet->appendFCS(kissBuffer[i]); - } - packet->finish(); - radio->afsk.encoder.putPacket(packet); - } - kissLen = 0; - inFrame = false; - } - // We're inside the boundaries of a FEND - if(inFrame) { - // Unescape the incoming data - if(c == KISS_FESC) { - c = io->read(); - if(c == KISS_TFESC) { - c = KISS_FESC; - } else { - c = KISS_FEND; - } - } - kissBuffer[kissLen++] = c; - } - if(kissLen == 0 && c != KISS_FEND) { - if((c & 0xf) == 0) // First byte<3:0> should be a 0, otherwise we're having options - inFrame = true; - } - } - if(radio->afsk.txReady()) { - radio->setModeTransmit(); - currentlySending = true; - if(!radio->afsk.txStart()) { // Unable to start for some reason - radio->setModeReceive(); - currentlySending = false; - } - } - if(currentlySending && radio->afsk.encoder.isDone()) { - radio->setModeReceive(); - currentlySending = false; - } -} - -void KISS::writePacket(AFSK::Packet *p) { - int i; - io->write(KISS_FEND); - io->write((uint8_t)0); // Host to TNC port identifier - for(i = 0; i < p->len-2; i++) { - char c = p->getByte(i); - if(c == KISS_FEND || c == KISS_FESC) { - io->write(KISS_FESC); - io->write((c==KISS_FEND?KISS_TFEND:KISS_TFESC)); - } else { - io->write(c); - } - } - io->write(KISS_FEND); -} diff --git a/src/inProgress/KISS.h b/src/inProgress/KISS.h deleted file mode 100644 index d89ecaf..0000000 --- a/src/inProgress/KISS.h +++ /dev/null @@ -1,35 +0,0 @@ -#ifndef _KISS_H_ -#define _KISS_H_ - -#include -#include "AFSK.h" - -#define KISS_FEND 0xC0 -#define KISS_FESC 0xDB -#define KISS_TFEND 0xDC -#define KISS_TFESC 0xDD - -class KISS { -public: - KISS(Stream *_io, HamShield *h, DDS *d) : io(_io), radio(h), dds(d) {} - bool read(); - void writePacket(AFSK::Packet *); - void loop(); - inline void isr() { - static uint8_t tcnt = 0; - TIFR1 = _BV(ICF1); // Clear the timer flag - dds->clockTick(); - if(++tcnt == (DDS_REFCLK_DEFAULT/9600)) { - //PORTD |= _BV(2); // Diagnostic pin (D2) - radio->afsk.timer(); - tcnt = 0; - } - //PORTD &= ~(_BV(2)); - } -private: - Stream *io; - HamShield *radio; - DDS *dds; -}; - -#endif /* _KISS_H_ */