From 024e35dc6a0356adfc801a023d5ec208cf3996cb Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 5 Dec 2012 09:59:16 -0800 Subject: altos: Add Pico Beacon code as ao_aprs.c Pico Beacon hooks a GPS to an AD9954 DDS radio chip with a PIC. It directly synthesizes the necessary AX.25 packets to do APRS reporting. We're going to appropriate the code for use in Mega Metrum to (optionally) broadcast APRS packets. http://ad7zj.net/kd7lmo/aprsbeacon_code.html Signed-off-by: Keith Packard ( --- src/drivers/ao_aprs.c | 3102 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 3102 insertions(+) create mode 100644 src/drivers/ao_aprs.c (limited to 'src') diff --git a/src/drivers/ao_aprs.c b/src/drivers/ao_aprs.c new file mode 100644 index 00000000..79cea49a --- /dev/null +++ b/src/drivers/ao_aprs.c @@ -0,0 +1,3102 @@ +/** + * http://ad7zj.net/kd7lmo/aprsbeacon_code.html + * + * @mainpage Pico Beacon + * + * @section overview_sec Overview + * + * The Pico Beacon is an APRS based tracking beacon that operates in the UHF 420-450MHz band. The device utilizes a + * Microchip PIC 18F2525 embedded controller, Motorola M12+ GPS engine, and Analog Devices AD9954 DDS. The device is capable + * of generating a 1200bps A-FSK and 9600 bps FSK AX.25 compliant APRS (Automatic Position Reporting System) message. + + + * + * @section history_sec Revision History + * + * @subsection v305 V3.05 + * 23 Dec 2006, Change include; (1) change printf format width to conform to ANSI standard when new CCS 4.xx compiler released. + * + * + * @subsection v304 V3.04 + * 10 Jan 2006, Change include; (1) added amplitude control to engineering mode, + * (2) corrected number of bytes reported in log, + * (3) add engineering command to set high rate position reports (5 seconds), and + * (4) corrected size of LOG_COORD block when searching for end of log. + * + * @subsection v303 V3.03 + * 15 Sep 2005, Change include; (1) removed AD9954 setting SDIO as input pin, + * (2) additional comments and Doxygen tags, + * (3) integration and test code calculates DDS FTW, + * (4) swapped bus and reference analog input ports (hardware change), + * (5) added message that indicates we are reading flash log and reports length, + * (6) report bus voltage in 10mV steps, and + * (7) change log type enumerated values to XORed nibbles for error detection. + * + * + * @subsection v302 V3.02 + * 6 Apr 2005, Change include; (1) corrected tracked satellite count in NMEA-0183 $GPGGA message, + * (2) Doxygen documentation clean up and additions, and + * (3) added integration and test code to baseline. + * + * + * @subsection v301 V3.01 + * 13 Jan 2005, Renamed project and files to Pico Beacon. + * + * + * @subsection v300 V3.00 + * 15 Nov 2004, Change include; (1) Micro Beacon extreme hardware changes including integral transmitter, + * (2) PIC18F2525 processor, + * (3) AD9954 DDS support functions, + * (4) added comments and formatting for doxygen, + * (5) process GPS data with native Motorola protocol, + * (6) generate plain text $GPGGA and $GPRMC messages, + * (7) power down GPS 5 hours after lock, + * (8) added flight data recorder, and + * (9) added diagnostics terminal mode. + * + * + * @subsection v201 V2.01 + * 30 Jan 2004, Change include; (1) General clean up of in-line documentation, and + * (2) changed temperature resolution to 0.1 degrees F. + * + * + * @subsection v200 V2.00 + * 26 Oct 2002, Change include; (1) Micro Beacon II hardware changes including PIC18F252 processor, + * (2) serial EEPROM, + * (3) GPS power control, + * (4) additional ADC input, and + * (5) LM60 temperature sensor. + * + * + * @subsection v101 V1.01 + * 5 Dec 2001, Change include; (1) Changed startup message, and + * (2) applied SEPARATE pragma to several methods for memory usage. + * + * + * @subsection v100 V1.00 + * 25 Sep 2001, Initial release. Flew ANSR-3 and ANSR-4. + * + + + * + * + * @section copyright_sec Copyright + * + * Copyright (c) 2001-2009 Michael Gray, KD7LMO + + + * + * + * @section gpl_sec GNU General Public License + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + + + * + * + * @section design Design Details + * + * Provides design details on a variety of the components that make up the Pico Beacon. + * + * @subpage power + */ + +/** + * @page power Power Consumption + * + * Measured DC power consumption. + * + * 3VDC prime power current + + * + * 7mA Held in reset + + * 18mA Processor running, all I/O off + + * 110mA GPS running + + * 120mA GPS running w/antenna + + * 250mA DDS running and GPS w/antenna + + * 420mA DDS running, GPS w/antenna, and PA chain on with no RF + + * 900mA Transmit + + * + */ + +// Hardware specific configuration. +#include <18f2525.h> +#device ADC=10 + +// NOTE: Even though we are using an external clock, we set the HS oscillator mode to +// make the PIC 18F252 work with our external clock which is a clipped 1V P-P sine wave. +#fuses HS,NOWDT,NOPROTECT,NOPUT,NOBROWNOUT,NOLVP + +// C runtime library definitions. +#include +#include + +// These compiler directives set the clock, SPI/I2C ports, and I/O configuration. + +// TCXO frequency +#use delay(clock=19200000) + +// Engineering and data extracation port. +#use rs232(baud=57600, xmit=PIN_B7, rcv=PIN_B6, STREAM=PC_HOST) + +// GPS engine +#use rs232(baud=9600, xmit=PIN_C6, rcv=PIN_C7) + +#use i2c (master, scl=PIN_C3, sda=PIN_C4) + +#use fast_io(A) +#use fast_io(B) +#use fast_io(C) + +// We define types that are used for all variables. These are declared +// because each processor has a different sizes for int and long. +// The PIC compiler defines int8_t, int16_t, and int32_t. + +/// Boolean value { false, true } +typedef boolean bool_t; + +/// Signed 8-bit number in the range -128 through 127. +typedef signed int8 int8_t; + +/// Unsigned 8-bit number in the range 0 through 255. +typedef unsigned int8 uint8_t; + +/// Signed 16-bit number in the range -32768 through 32767. +typedef signed int16 int16_t; + +/// Unsigned 16-bit number in the range 0 through 65535. +typedef unsigned int16 uint16_t; + +/// Signed 32-bit number in the range -2147483648 through 2147483647. +typedef signed int32 int32_t; + +/// Unsigned 32-bit number in the range 0 through 4294967296. +typedef unsigned int32 uint32_t; + +// Function and structure prototypes. These are declared at the start of +// the file much like a C++ header file. + +// Map I/O pin names to hardware pins. + +/// Heartbeat LED - Port A2 +#define IO_LED PIN_A2 + +/// AD9954 DDS Profile Select 0 - Port A3 +#define IO_PS0 PIN_A3 + +/// UHF amplifier and PA chain - Port A4 +#define IO_PTT PIN_A4 + +/// AD9954 DDS Update - Port A5 +#define IO_UPDATE PIN_A5 + +/// AD9954 CS (Chip Select) - Port B0 +#define IO_CS PIN_B0 + +/// GPS Engine Power - Port B1 +#define IO_GPS_PWR PIN_B1 + +/// AD9954 DDS Profile Select 1 - Port C0 +#define IO_PS1 PIN_C0 + +/// AD9954 DDS OSK (Output Shift Key) - Port C2 +#define IO_OSK PIN_C2 + +/// GPS engine serial transmit pin - Port C6 +#define IO_GPS_TXD PIN_C6 + +// Public methods, constants, and data structures for each class. + +/// Operational modes of the AD9954 DDS for the ddsSetMode function. +enum DDS_MODE +{ + /// Device has not been initialized. + DDS_MODE_NOT_INITIALIZED, + + /// Device in lowest power down mode. + DDS_MODE_POWERDOWN, + + /// Generate FM modulated audio tones. + DDS_MODE_AFSK, + + /// Generate true FSK tones. + DDS_MODE_FSK +}; + +void ddsInit(); +void ddsSetAmplitude (uint8_t amplitude); +void ddsSetOutputScale (uint16_t amplitude); +void ddsSetFSKFreq (uint32_t ftw0, uint32_t ftw1); +void ddsSetFreq (uint32_t freq); +void ddsSetFTW (uint32_t ftw); +void ddsSetMode (DDS_MODE mode); + +void flashErase(); +uint8_t flashGetByte (); +void flashReadBlock(uint32_t address, uint8_t *block, uint16_t length); +void flashSendByte(uint8_t value); +void flashSendAddress(uint32_t address); +void flashWriteBlock(uint32_t address, uint8_t *block, uint8_t length); + +/// Type of GPS fix. +enum GPS_FIX_TYPE +{ + /// No GPS FIX + GPS_NO_FIX, + + /// 2D (Latitude/Longitude) fix. + GPS_2D_FIX, + + /// 3D (Latitude/Longitude/Altitude) fix. + GPS_3D_FIX +}; + +/// GPS Position information. +typedef struct +{ + /// Flag that indicates the position information has been updated since it was last checked. + bool_t updateFlag; + + /// Month in UTC time. + uint8_t month; + + /// Day of month in UTC time. + uint8_t day; + + /// Hours in UTC time. + uint8_t hours; + + /// Minutes in UTC time. + uint8_t minutes; + + /// Seconds in UTC time. + uint8_t seconds; + + /// Year in UTC time. + uint16_t year; + + /// Latitude in milli arc-seconds where + is North, - is South. + int32_t latitude; + + /// Longitude in milli arc-seconds where + is East, - is West. + int32_t longitude; + + /// Altitude in cm + int32_t altitudeCM; + + /// Calculated altitude in feet + int32_t altitudeFeet; + + /// 3D speed in cm/second. + uint16_t vSpeed; + + /// 2D speed in cm/second. + uint16_t hSpeed; + + /// Heading units of 0.1 degrees. + uint16_t heading; + + /// DOP (Dilution of Precision) + uint16_t dop; + + /// 16-bit number that represents status of GPS engine. + uint16_t status; + + /// Number of tracked satellites used in the fix position. + uint8_t trackedSats; + + /// Number of visible satellites. + uint8_t visibleSats; +} GPSPOSITION_STRUCT; + +void gpsInit(); +bool_t gpsIsReady(); +GPS_FIX_TYPE gpsGetFixType(); +int32_t gpsGetPeakAltitude(); +void gpsPowerOn(); +bool_t gpsSetup(); +void gpsUpdate(); + +int16_t lm92GetTemp(); + +/// Define the log record types. +enum LOG_TYPE +{ + /// Time stamp the log was started. + LOG_BOOTED = 0xb4, + + /// GPS coordinates. + LOG_COORD = 0xa5, + + /// Temperature + LOG_TEMPERATURE = 0x96, + + /// Bus voltage. + LOG_VOLTAGE = 0x87 +}; + +void logInit(); +uint32_t logGetAddress(); +void logType (LOG_TYPE type); +void logUint8 (uint8_t value); +void logInt16 (int16_t value); + +bool_t serialHasData(); +void serialInit(); +uint8_t serialRead(); +void serialUpdate(); + +uint16_t sysCRC16(uint8_t *buffer, uint8_t length, uint16_t crc); +void sysInit(); +void sysLogVoltage(); + +/// 0% duty cycle (LED Off) constant for function timeSetDutyCycle +#define TIME_DUTYCYCLE_0 0 + +/// 10% duty cycle constant for function timeSetDutyCycle +#define TIME_DUTYCYCLE_10 1 + +/// 70% duty cycle constant for function timeSetDutyCycle +#define TIME_DUTYCYCLE_70 7 + +uint8_t timeGetTicks(); +void timeInit(); +void timeSetDutyCycle (uint8_t dutyCycle); +void timeUpdate(); + +/// Operational modes of the TNC for the tncSetMode function. +enum TNC_DATA_MODE +{ + /// No operation waiting for setup and configuration. + TNC_MODE_STANDBY, + + /// 1200 bps using A-FSK (Audio FSK) tones. + TNC_MODE_1200_AFSK, + + /// 9600 bps using true FSK tones. + TNC_MODE_9600_FSK +}; + +void tncInit(); +bool_t tncIsFree(); +void tncHighRate(bool_t state); +void tncSetMode (TNC_DATA_MODE dataMode); +void tnc1200TimerTick(); +void tnc9600TimerTick(); +void tncTxByte (uint8_t value); +void tncTxPacket(TNC_DATA_MODE dataMode); + +/** + * @defgroup ADC Analog To Digital Converter + * + * Control and manage the on board PIC A/D converter. + * + * @{ + */ + +/// Filtered voltages using a single pole, low pass filter. +uint16_t adcMainBusVolt; + +/// PIC ADC Channel number of the reference voltage. +#define ADC_REF 0 + +/// PIC ADC Channel number of the main bus voltage. +#define ADC_MAINBUS 1 + +/// Input diode drop in units of 0.01 volts. +#define MAIN_BUS_VOLT_OFFSET 20 + +/** + * Intialize the ADC subsystem. + */ +void adcInit() +{ + // Setup the ADC. + setup_adc_ports(AN0_TO_AN1); + setup_adc( ADC_CLOCK_DIV_32 ); + + // Zero the ADC filters. + adcMainBusVolt = 0; +} + +/** + * Filtered main bus voltage in 10mV resolution. + * + * @return voltage in 10mV steps + */ +uint16_t adcGetMainBusVolt() +{ + uint32_t volts; + + volts = (uint32_t) (adcMainBusVolt >> 3); + + volts = (volts * 330l) / 1023l; + + return (uint16_t) volts + MAIN_BUS_VOLT_OFFSET; +} + +/** + * Get the current ADC value for the main bus voltage. + * + * @return ADC value in the range 0 to 1023 + */ +uint16_t adcRawBusVolt() +{ + set_adc_channel(ADC_MAINBUS); + delay_us(50); + return read_adc(); +} + +/** + * Get the current ADC value for the reference source voltage. + * + * @return ADC value in the range 0 to 1023 + */ +uint16_t adcRawRefVolt() +{ + set_adc_channel(ADC_REF); + delay_us(50); + return read_adc(); +} + +/** + * Read and filter the ADC channels for bus voltages. + */ +void adcUpdate(void) +{ + // Filter the bus voltage using a single pole low pass filter. + set_adc_channel(ADC_MAINBUS); + delay_us(50); + adcMainBusVolt = read_adc() + adcMainBusVolt - (adcMainBusVolt >> 3); +} + +/** @} */ + + +/** + * @defgroup diag Diagnostics and Control + * + * Functions for diagnostics and control of the hardware and flight data recorder. + * + * @{ + */ + +/// Number of bytes per line to display when reading flight data recorder. +#define DIAG_BYTES_PER_LINE 32 + +/** + * Process the command to erase the data logger flash. + */ +void diagEraseFlash() +{ + // Confirm we want to erase the flash with the key sequence 'yes' . + fprintf (PC_HOST, "Are you sure (yes)? "); + + if (fgetc(PC_HOST) != 'y') + return; + + if (fgetc(PC_HOST) != 'e') + return; + + if (fgetc(PC_HOST) != 's') + return; + + if (fgetc(PC_HOST) != 13) + return; + + // User feedback and erase the part. + fprintf (PC_HOST, "Erasing flash..."); + + flashErase(); + + fprintf (PC_HOST, "done.\n\r"); +} + +/** + * Display the engineering mode menu. + */ +void diagMenu() +{ + // User interface. + fprintf (PC_HOST, "Options: (e)rase Flash, (r)ead Flash\n\r"); + fprintf (PC_HOST, " Toggle (L)ED\n\r"); + fprintf (PC_HOST, " (P)TT - Push To Transmit\n\r"); + fprintf (PC_HOST, " (f)requencey down, (F)requency up - 1KHz step\n\r"); + fprintf (PC_HOST, " (c)hannel down, (C)hannel up - 25KHz step\n\r"); + fprintf (PC_HOST, " (a)mplitude down, (A)mplitude up - 0.5 dB steps\n\r"); + fprintf (PC_HOST, " e(x)it engineering mode\n\r"); +} + +/** + * Process the command to dump the contents of the data logger flash. + */ +void diagReadFlash() +{ + bool_t dataFoundFlag, userStopFlag; + uint8_t i, buffer[DIAG_BYTES_PER_LINE]; + uint32_t address; + + // Set the initial conditions to read the flash. + address = 0x0000; + userStopFlag = false; + + do + { + // Read each block from the flash device. + flashReadBlock (address, buffer, DIAG_BYTES_PER_LINE); + + // This flag will get set if any data byte is not equal to 0xff (erase flash state) + dataFoundFlag = false; + + // Display the address. + fprintf (PC_HOST, "%08lx ", address); + + // Display each byte in the line. + for (i = 0; i < DIAG_BYTES_PER_LINE; ++i) + { + fprintf (PC_HOST, "%02x", buffer[i]); + + // Set this flag if the cell is not erased. + if (buffer[i] != 0xff) + dataFoundFlag = true; + + // Any key will abort the transfer. + if (kbhit(PC_HOST)) + userStopFlag = true; + } // END for + + // at the end of each line. + fprintf (PC_HOST, "\n\r"); + + // Advance to the next block of memory. + address += DIAG_BYTES_PER_LINE; + } while (dataFoundFlag && !userStopFlag); + + // Feedback to let the user know why the transfer stopped. + if (userStopFlag) + fprintf (PC_HOST, "User aborted download!\n\r"); +} + +void diag1PPS() +{ + uint16_t timeStamp, lastTimeStamp; + + lastTimeStamp = 0x0000; + + gpsPowerOn(); + + for (;;) + { + timeStamp = CCP_2; + + if (timeStamp != lastTimeStamp) + { + delay_ms (10); + + timeStamp = CCP_2; + + fprintf (PC_HOST, "%lu %lu\n\r", timeStamp, (timeStamp - lastTimeStamp)); + + lastTimeStamp = timeStamp; + } + } +} + +/** + * Process diagnostic commands through the debug RS-232 port. + */ +void diagPort() +{ + bool_t diagDoneFlag, ledFlag, paFlag, showSettingsFlag; + uint8_t command, amplitude; + uint32_t freqHz; + + // If the input is low, we aren't connected to the RS-232 device so continue to boot. + if (!input(PIN_B6)) + return; + + fprintf (PC_HOST, "Engineering Mode\n\r"); + fprintf (PC_HOST, "Application Built %s %s\n\r", __DATE__, __TIME__); + + // Current state of the status LED. + ledFlag = false; + output_bit (IO_LED, ledFlag); + + // This flag indicates we are ready to leave the diagnostics mode. + diagDoneFlag = false; + + // Current state of the PA. + paFlag = false; + + // Flag that indicate we should show the current carrier frequency. + showSettingsFlag = false; + + // Set the initial carrier frequency and amplitude. + freqHz = 445950000; + amplitude = 0; + + // Wait for the exit command. + while (!diagDoneFlag) + { + // Wait for the user command. + command = fgetc(PC_HOST); + + // Decode and process the key stroke. + switch (command) + { + case 'e': + diagEraseFlash(); + logInit(); + break; + + case 'l': + case 'L': + ledFlag = (ledFlag ? false : true); + output_bit (IO_LED, ledFlag); + break; + + case 'h': + case 'H': + case '?': + diagMenu(); + break; + + case 'r': + diagReadFlash(); + break; + + case 't': + tncHighRate (true); + fprintf (PC_HOST, "Set high rate TNC.\n\r"); + break; + + case 'f': + freqHz -= 1000; + ddsSetFreq (freqHz); + + // Display the new frequency. + showSettingsFlag = true; + break; + + case 'F': + freqHz += 1000; + ddsSetFreq (freqHz); + + // Display the new frequency. + showSettingsFlag = true; + break; + + case 'c': + freqHz -= 25000; + ddsSetFreq (freqHz); + + // Display the new frequency. + showSettingsFlag = true; + break; + + case 'C': + freqHz += 25000; + ddsSetFreq (freqHz); + + // Display the new frequency. + showSettingsFlag = true; + break; + + case 'p': + case 'P': + ddsSetFreq (freqHz); + + paFlag = (paFlag ? false : true); + output_bit (IO_PTT, paFlag); + output_bit (IO_OSK, paFlag); + + if (paFlag) + { + ddsSetMode (DDS_MODE_AFSK); + ddsSetAmplitude (amplitude); + } else + ddsSetMode (DDS_MODE_POWERDOWN); + + break; + + case 'a': + if (amplitude != 200) + { + amplitude += 5; + ddsSetAmplitude (amplitude); + + // Display the new amplitude. + showSettingsFlag = true; + } + break; + + case 'A': + if (amplitude != 0) + { + amplitude -= 5; + ddsSetAmplitude (amplitude); + + // Display the new amplitude. + showSettingsFlag = true; + } + break; + + case 'g': + diag1PPS(); + break; + + case 'x': + diagDoneFlag = true; + break; + + default: + fprintf (PC_HOST, "Invalid command. (H)elp for menu.\n\r"); + break; + } // END switch + + // Display the results of any user requests or commands. + if (showSettingsFlag) + { + showSettingsFlag = false; + + fprintf (PC_HOST, "%03ld.%03ld MHz ", freqHz / 1000000, (freqHz / 1000) % 1000); + fprintf (PC_HOST, "%d.%01ddBc\n\r", amplitude / 10, amplitude % 10); + + } // END if + + } // END while + + // Let the user know we are done with this mode. + fprintf (PC_HOST, "Exit diagnostic mode.\n\r"); + + return; +} + +/** @} */ + + +/** + * @defgroup DDS AD9954 DDS (Direct Digital Synthesizer) + * + * Functions to control the Analog Devices AD9954 DDS. + * + * @{ + */ + +/// AD9954 CFR1 - Control functions including RAM, profiles, OSK, sync, sweep, SPI, and power control settings. +#define DDS_AD9954_CFR1 0x00 + +/// AD9954 CFR2 - Control functions including sync, PLL multiplier, VCO range, and charge pump current. +#define DDS_AD9954_CFR2 0x01 + +/// AD9954 ASF - Auto ramp rate speed control and output scale factor (0x0000 to 0x3fff). +#define DDS_AD9954_ASF 0x02 + +/// AD9954 ARR - Amplitude ramp rate for OSK function. +#define DDS_AD9954_ARR 0x03 + +/// AD9954 FTW0 - Frequency tuning word 0. +#define DDS_AD9954_FTW0 0x04 + +/// AD9954 FTW1 - Frequency tuning word 1 +#define DDS_AD9954_FTW1 0x06 + +/// AD9954 NLSCW - Negative Linear Sweep Control Word used for spectral shaping in FSK mode +#define DDS_AD9954_NLSCW 0x07 + +/// AD9954 PLSCW - Positive Linear Sweep Control Word used for spectral shaping in FSK mode +#define DDS_AD9954_PLSCW 0x08 + +/// AD9954 RSCW0 - RAM Segment Control Word 0 +#define DDS_AD9954_RWCW0 0x07 + +/// AD9954 RSCW0 - RAM Segment Control Word 1 +#define DDS_AD9954_RWCW1 0x08 + +/// AD9954 RAM segment +#define DDS_RAM 0x0b + +/// Current operational mode. +DDS_MODE ddsMode; + +/// Number of digits in DDS frequency to FTW conversion. +#define DDS_FREQ_TO_FTW_DIGITS 9 + +/// Array of multiplication factors used to convert frequency to the FTW. +const uint32_t DDS_MULT[DDS_FREQ_TO_FTW_DIGITS] = { 11, 7, 7, 3, 4, 8, 4, 9, 1 }; + +/// Array of divisors used to convert frequency to the FTW. +const uint32_t DDS_DIVISOR[DDS_FREQ_TO_FTW_DIGITS - 1] = { 10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000 }; + +/// Lookup table to convert dB amplitude scale in 0.5 steps to a linear DDS scale factor. +const uint16_t DDS_AMP_TO_SCALE[] = +{ + 16383, 15467, 14601, 13785, 13013, 12286, 11598, 10949, 10337, 9759, 9213, 8697, + 8211, 7752, 7318, 6909, 6522, 6157, 5813, 5488, 5181, 4891, 4617, 4359, 4115, 3885, 3668, 3463, + 3269, 3086, 2913, 2750, 2597, 2451, 2314, 2185, 2062, 1947, 1838, 1735, 1638 +}; + + +/// Frequency Word List - 4.0KHz FM frequency deviation at 81.15MHz (445.950MHz) +const uint32_t freqTable[256] = +{ + 955418300, 955419456, 955420611, 955421765, 955422916, 955424065, 955425210, 955426351, + 955427488, 955428618, 955429743, 955430861, 955431971, 955433073, 955434166, 955435249, + 955436322, 955437385, 955438435, 955439474, 955440500, 955441513, 955442511, 955443495, + 955444464, 955445417, 955446354, 955447274, 955448176, 955449061, 955449926, 955450773, + 955451601, 955452408, 955453194, 955453960, 955454704, 955455426, 955456126, 955456803, + 955457457, 955458088, 955458694, 955459276, 955459833, 955460366, 955460873, 955461354, + 955461809, 955462238, 955462641, 955463017, 955463366, 955463688, 955463983, 955464250, + 955464489, 955464701, 955464884, 955465040, 955465167, 955465266, 955465337, 955465380, + 955465394, 955465380, 955465337, 955465266, 955465167, 955465040, 955464884, 955464701, + 955464489, 955464250, 955463983, 955463688, 955463366, 955463017, 955462641, 955462238, + 955461809, 955461354, 955460873, 955460366, 955459833, 955459276, 955458694, 955458088, + 955457457, 955456803, 955456126, 955455426, 955454704, 955453960, 955453194, 955452408, + 955451601, 955450773, 955449926, 955449061, 955448176, 955447274, 955446354, 955445417, + 955444464, 955443495, 955442511, 955441513, 955440500, 955439474, 955438435, 955437385, + 955436322, 955435249, 955434166, 955433073, 955431971, 955430861, 955429743, 955428618, + 955427488, 955426351, 955425210, 955424065, 955422916, 955421765, 955420611, 955419456, + 955418300, 955417144, 955415989, 955414836, 955413684, 955412535, 955411390, 955410249, + 955409113, 955407982, 955406857, 955405740, 955404629, 955403528, 955402435, 955401351, + 955400278, 955399216, 955398165, 955397126, 955396100, 955395088, 955394089, 955393105, + 955392136, 955391183, 955390246, 955389326, 955388424, 955387540, 955386674, 955385827, + 955385000, 955384192, 955383406, 955382640, 955381896, 955381174, 955380474, 955379797, + 955379143, 955378513, 955377906, 955377324, 955376767, 955376235, 955375728, 955375246, + 955374791, 955374362, 955373959, 955373583, 955373234, 955372912, 955372618, 955372350, + 955372111, 955371900, 955371716, 955371560, 955371433, 955371334, 955371263, 955371220, + 955371206, 955371220, 955371263, 955371334, 955371433, 955371560, 955371716, 955371900, + 955372111, 955372350, 955372618, 955372912, 955373234, 955373583, 955373959, 955374362, + 955374791, 955375246, 955375728, 955376235, 955376767, 955377324, 955377906, 955378513, + 955379143, 955379797, 955380474, 955381174, 955381896, 955382640, 955383406, 955384192, + 955385000, 955385827, 955386674, 955387540, 955388424, 955389326, 955390246, 955391183, + 955392136, 955393105, 955394089, 955395088, 955396100, 955397126, 955398165, 955399216, + 955400278, 955401351, 955402435, 955403528, 955404629, 955405740, 955406857, 955407982, + 955409113, 955410249, 955411390, 955412535, 955413684, 955414836, 955415989, 955417144 +}; + +/** + * Initialize the DDS regsiters and RAM. + */ +void ddsInit() +{ + // Setup the SPI port for the DDS interface. + setup_spi( SPI_MASTER | SPI_L_TO_H | SPI_CLK_DIV_4 | SPI_XMIT_L_TO_H ); + + // Set the initial DDS mode. The ddsSetMode function uses this value to make the desired DDS selections. + ddsMode = DDS_MODE_NOT_INITIALIZED; + + // Set the DDS operational mode. + ddsSetMode (DDS_MODE_POWERDOWN); + + // Set the output to full scale. + ddsSetOutputScale (0x3fff); + + // CFR2 (Control Function Register No. 2) + output_low (IO_CS); + spi_write (DDS_AD9954_CFR2); + + spi_write (0x00); // Unused register bits + spi_write (0x00); + spi_write (0x9c); // 19x reference clock multipler, high VCO range, nominal charge pump current + output_high (IO_CS); + + // ARR (Amplitude Ramp Rate) to 15mS for OSK + output_low (IO_CS); + spi_write (DDS_AD9954_ARR); + + spi_write (83); + output_high (IO_CS); + + // Strobe the part so we apply the updates. + output_high (IO_UPDATE); + output_low (IO_UPDATE); +} + +/** + * Set DDS amplitude value in the range 0 to 16383 where 16383 is full scale. This value is a + * linear multiplier and needs to be scale for RF output power in log scale. + * + * @param scale in the range 0 to 16383 + */ +void ddsSetOutputScale (uint16_t scale) +{ + // Set ASF (Amplitude Scale Factor) + output_low (IO_CS); + spi_write (DDS_AD9954_ASF); + + spi_write ((scale >> 8) & 0xff); + spi_write (scale & 0xff); + + output_high (IO_CS); + + // Strobe the DDS to set the amplitude. + output_high (IO_UPDATE); + output_low (IO_UPDATE); +} + +/** + * Set the DDS amplitude in units of dBc of full scale where 1 is 0.1 dB. For example, a value of 30 is 3dBc + * or a value of 85 is 8.5dBc. + * + * @param amplitude in 0.1 dBc of full scale + */ +void ddsSetAmplitude (uint8_t amplitude) +{ + // Range limit based on the lookup table size. + if (amplitude > 200) + return; + + // Set the linear DDS ASF (Amplitude Scale Factor) based on the dB lookup table. + ddsSetOutputScale (DDS_AMP_TO_SCALE[amplitude / 5]); + + // Toggle the DDS output low and then high to force it to ramp to the new output level setting. + output_low (IO_OSK); + delay_ms(25); + + output_high (IO_OSK); + delay_ms(25); +} + +/** + * Set DDS frequency tuning word. The output frequency is equal to RefClock * (ftw / 2 ^ 32). + * + * @param ftw Frequency Tuning Word + */ +void ddsSetFTW (uint32_t ftw) +{ + // Set FTW0 (Frequency Tuning Word 0) + output_low (IO_CS); + spi_write (DDS_AD9954_FTW0); + + spi_write ((ftw >> 24) & 0xff); + spi_write ((ftw >> 16) & 0xff); + spi_write ((ftw >> 8) & 0xff); + spi_write (ftw & 0xff); + + output_high (IO_CS); + + // Strobe the DDS to set the frequency. + output_high (IO_UPDATE); + output_low (IO_UPDATE); +} + +/** + * Convert frequency in hertz to 32-bit DDS FTW (Frequency Tune Word). + * + * @param freq frequency in Hertz + * + */ +void ddsSetFreq(uint32_t freq) +{ + uint8_t i; + uint32_t ftw; + + // To avoid rounding errors with floating point math, we do a long multiply on the data. + ftw = freq * DDS_MULT[0]; + + for (i = 0; i < DDS_FREQ_TO_FTW_DIGITS - 1; ++i) + ftw += (freq * DDS_MULT[i+1]) / DDS_DIVISOR[i]; + + ddsSetFTW (ftw); +} + +/** + * Set DDS frequency tuning word for the FSK 0 and 1 values. The output frequency is equal + * to RefClock * (ftw / 2 ^ 32). + * + * @param ftw0 frequency tuning word for the FSK 0 value + * @param ftw1 frequency tuning word for the FSK 1 value + */ +void ddsSetFSKFreq (uint32_t ftw0, uint32_t ftw1) +{ + // Set FTW0 (Frequency Tuning Word 0) + output_low (IO_CS); + spi_write (DDS_AD9954_FTW0); + + spi_write ((ftw0 >> 24) & 0xff); + spi_write ((ftw0 >> 16) & 0xff); + spi_write ((ftw0 >> 8) & 0xff); + spi_write (ftw0 & 0xff); + + output_high (IO_CS); + + // Set FTW0 (Frequency Tuning Word 1) + output_low (IO_CS); + spi_write (DDS_AD9954_FTW1); + + spi_write ((ftw1 >> 24) & 0xff); + spi_write ((ftw1 >> 16) & 0xff); + spi_write ((ftw1 >> 8) & 0xff); + spi_write (ftw1 & 0xff); + + output_high (IO_CS); + + // Strobe the DDS to set the frequency. + output_high (IO_UPDATE); + output_low (IO_UPDATE); +} + +/** + * Set the DDS to run in A-FSK, FSK, or PSK31 mode + * + * @param mode DDS_MODE_APRS, DDS_MODE_PSK31, or DDS_MODE_HF_APRS constant + */ +void ddsSetMode (DDS_MODE mode) +{ + // Save the current mode. + ddsMode = mode; + + switch (mode) + { + case DDS_MODE_POWERDOWN: + // CFR1 (Control Function Register No. 1) + output_low (IO_CS); + spi_write (DDS_AD9954_CFR1); + + spi_write (0x00); + spi_write (0x00); + spi_write (0x00); + spi_write (0xf0); // Power down all subsystems. + output_high (IO_CS); + break; + + case DDS_MODE_AFSK: + // CFR1 (Control Function Register No. 1) + output_low (IO_CS); + spi_write (DDS_AD9954_CFR1); + + spi_write (0x03); // OSK Enable and Auto OSK keying + spi_write (0x00); + spi_write (0x00); + spi_write (0x40); // Power down comparator circuit + output_high (IO_CS); + break; + + case DDS_MODE_FSK: + // CFR1 (Control Function Register No. 1) + output_low (IO_CS); + spi_write (DDS_AD9954_CFR1); + + spi_write (0x03); // Clear RAM Enable, OSK Enable, Auto OSK keying + spi_write (0x00); + spi_write (0x00); + spi_write (0x40); // Power down comparator circuit + output_high (IO_CS); + + // NOTE: The sweep rate requires 1/4 of a bit time (26uS) to transition. + // 6KHz delta = 70641 counts = (6KHz / 364.8MHz) * 2 ^ 32 + // SYNC_CLK = 91.2MHz 1/91.2MHz * 70641 * 1/29 = 26.7uS + + // NLSCW (Negative Linear Sweep Control Word) + output_low (IO_CS); + spi_write (DDS_AD9954_NLSCW); + + spi_write (1); // Falling sweep ramp rate word + spi_write (0x00); // Delta frequency tuning word + spi_write (0x00); + spi_write (0x00); + spi_write (250); + output_high (IO_CS); + + // PLSCW (Positive Linear Sweep Control Word) + output_low (IO_CS); + spi_write (DDS_AD9954_PLSCW); + + spi_write (1); // Rising sweep ramp rate word + spi_write (0x00); // Delta frequency tuning word + spi_write (0x00); + spi_write (0x00); + spi_write (250); + output_high (IO_CS); + break; + } // END switch + + // Strobe the DDS to change the mode. + output_high (IO_UPDATE); + output_low (IO_UPDATE); +} + +/** @} */ + +/** + * @defgroup flash Flash Manager + * + * Functions to control the ST MP25P80 serial flash device. + * + * @{ + */ + +/// Flash Chip Select - Port B3 +#define FLASH_CS PIN_B3 + +/// Flash Clock - Port B5 +#define FLASH_CLK PIN_B5 + +/// Flash Data Input - Port B4 +#define FLASH_D PIN_B4 + +/// Flash Data Output - Port B2 +#define FLASH_Q PIN_B2 + +/** + * Determine if a flash write or erase operation is currently in progress. + * + * @return true if write/erase in progress + */ +bool_t flashIsWriteInProgress() +{ + uint8_t status; + + output_low (FLASH_CS); + + // Read Status Register (RDSR) flash command. + flashSendByte (0x05); + + status = flashGetByte(); + + output_high (FLASH_CS); + + return (((status & 0x01) == 0x01) ? true : false); +} + +/** + * Read a block of memory from the flash device. + * + * @param address of desired location in the range 0x00000 to 0xFFFFF (1MB) + * @param block pointer to locate of data block + * @param length number of bytes to read + */ +void flashReadBlock(uint32_t address, uint8_t *block, uint16_t length) +{ + uint16_t i; + + output_low (FLASH_CS); + + // Read Data Byte(s) (READ) flash command. + flashSendByte (0x03); + flashSendAddress (address); + + for (i = 0; i < length; ++i) + *block++ = flashGetByte(); + + output_high (FLASH_CS); +} + +/** + * Write a block of memory to the flash device. + * + * @param address of desired location in the range 0x00000 to 0xFFFFF (1MB) + * @param block pointer data block to write + * @param length number of bytes to write + */ +void flashWriteBlock(uint32_t address, uint8_t *block, uint8_t length) +{ + uint8_t i; + + output_low (FLASH_CS); + // Write Enable (WREN) flash command. + flashSendByte (0x06); + output_high (FLASH_CS); + + output_low (FLASH_CS); + // Page Program (PP) flash command. + flashSendByte (0x02); + flashSendAddress (address); + + for (i = 0; i < length; ++i) + { + // Send each byte in the data block. + flashSendByte (*block++); + + // Track the address in the flash device. + ++address; + + // If we cross a page boundary (a page is 256 bytes) we need to stop and send the address again. + if ((address & 0xff) == 0x00) + { + output_high (FLASH_CS); + + // Write this block of data. + while (flashIsWriteInProgress()); + + output_low (FLASH_CS); + // Write Enable (WREN) flash command. + flashSendByte (0x06); + output_high (FLASH_CS); + + output_low (FLASH_CS); + // Page Program (PP) flash command. + flashSendByte (0x02); + flashSendAddress (address); + } // END if + } // END for + + output_high (FLASH_CS); + + // Wait for the final write operation to complete. + while (flashIsWriteInProgress()); +} + +/** + * Erase the entire flash device (all locations set to 0xff). + */ +void flashErase() +{ + output_low (FLASH_CS); + // Write Enable (WREN) flash command. + flashSendByte (0x06); + output_high (FLASH_CS); + + output_low (FLASH_CS); + // Bulk Erase (BE) flash command. + flashSendByte (0xc7); + output_high (FLASH_CS); + + while (flashIsWriteInProgress()); +} + +/** + * Read a single byte from the flash device through the serial interface. This function + * only controls the clock line. The chip select must be configured before calling + * this function. + * + * @return byte read from device + */ +uint8_t flashGetByte() +{ + uint8_t i, value; + + value = 0; + + // Bit bang the 8-bits. + for (i = 0; i < 8; ++i) + { + // Data is ready on the rising edge of the clock. + output_high (FLASH_CLK); + + // MSB is first, so shift left. + value = value << 1; + + if (input (FLASH_Q)) + value = value | 0x01; + + output_low (FLASH_CLK); + } // END for + + return value; +} + +/** + * Initialize the flash memory subsystem. + */ +void flashInit() +{ + // I/O lines to control flash. + output_high (FLASH_CS); + output_low (FLASH_CLK); + output_low (FLASH_D); +} + +/** + * Write a single byte to the flash device through the serial interface. This function + * only controls the clock line. The chip select must be configured before calling + * this function. + * + * @param value byte to write to device + */ +void flashSendByte(uint8_t value) +{ + uint8_t i; + + // Bit bang the 8-bits. + for (i = 0; i < 8; ++i) + { + // Drive the data input pin. + if ((value & 0x80) == 0x80) + output_high (FLASH_D); + else + output_low (FLASH_D); + + // MSB is first, so shift leeft. + value = value << 1; + + // Data is accepted on the rising edge of the clock. + output_high (FLASH_CLK); + output_low (FLASH_CLK); + } // END for +} + +/** + * Write the 24-bit address to the flash device through the serial interface. This function + * only controls the clock line. The chip select must be configured before calling + * this function. + * + * @param address 24-bit flash device address + */ +void flashSendAddress(uint32_t address) +{ + uint8_t i; + + // Bit bang the 24-bits. + for (i = 0; i < 24; ++i) + { + // Drive the data input pin. + if ((address & 0x800000) == 0x800000) + output_high (FLASH_D); + else + output_low (FLASH_D); + + // MSB is first, so shift left. + address = address << 1; + + // Data is accepted on the rising edge of the clock. + output_high (FLASH_CLK); + output_low (FLASH_CLK); + } // END for +} + +/** @} */ + +/** + * @defgroup GPS Motorola M12+ GPS Engine + * + * Functions to control the Motorola M12+ GPS engine in native binary protocol mode. + * + * @{ + */ + +/// The maximum length of a binary GPS engine message. +#define GPS_BUFFER_SIZE 50 + +/// GPS parse engine state machine values. +enum GPS_PARSE_STATE_MACHINE +{ + /// 1st start character '@' + GPS_START1, + + /// 2nd start character '@' + GPS_START2, + + /// Upper case 'A' - 'Z' message type + GPS_COMMAND1, + + /// Lower case 'a' - 'z' message type + GPS_COMMAND2, + + /// 0 - xx bytes based on message type 'Aa' + GPS_READMESSAGE, + + /// 8-bit checksum + GPS_CHECKSUMMESSAGE, + + /// End of message - Carriage Return + GPS_EOMCR, + + /// End of message - Line Feed + GPS_EOMLF +}; + +/// Index into gpsBuffer used to store message data. +uint8_t gpsIndex; + +/// State machine used to parse the GPS message stream. +GPS_PARSE_STATE_MACHINE gpsParseState; + +/// Buffer to store data as it is read from the GPS engine. +uint8_t gpsBuffer[GPS_BUFFER_SIZE]; + +/// Peak altitude detected while GPS is in 3D fix mode. +int32_t gpsPeakAltitude; + +/// Checksum used to verify binary message from GPS engine. +uint8_t gpsChecksum; + +/// Last verified GPS message received. +GPSPOSITION_STRUCT gpsPosition; + +/** + * Get the type of fix. + * + * @return gps fix type enumeration + */ +GPS_FIX_TYPE gpsGetFixType() +{ + // The upper 3-bits determine the fix type. + switch (gpsPosition.status & 0xe000) + { + case 0xe000: + return GPS_3D_FIX; + + case 0xc000: + return GPS_2D_FIX; + + default: + return GPS_NO_FIX; + } // END switch +} + +/** + * Peak altitude detected while GPS is in 3D fix mode since the system was booted. + * + * @return altitude in feet + */ +int32_t gpsGetPeakAltitude() +{ + return gpsPeakAltitude; +} + +/** + * Initialize the GPS subsystem. + */ +void gpsInit() +{ + // Initial parse state. + gpsParseState = GPS_START1; + + // Assume we start at sea level. + gpsPeakAltitude = 0; + + // Clear the structure that stores the position message. + memset (&gpsPosition, 0, sizeof(GPSPOSITION_STRUCT)); + + // Setup the timers used to measure the 1-PPS time period. + setup_timer_3(T3_INTERNAL | T3_DIV_BY_1); + setup_ccp2 (CCP_CAPTURE_RE | CCP_USE_TIMER3); +} + +/** + * Determine if new GPS message is ready to process. This function is a one shot and + * typically returns true once a second for each GPS position fix. + * + * @return true if new message available; otherwise false + */ +bool_t gpsIsReady() +{ + if (gpsPosition.updateFlag) + { + gpsPosition.updateFlag = false; + return true; + } // END if + + return false; +} + +/** + * Calculate NMEA-0183 message checksum of buffer that is length bytes long. + * + * @param buffer pointer to data buffer. + * @param length number of bytes in buffer. + * + * @return checksum of buffer + */ +uint8_t gpsNMEAChecksum (uint8_t *buffer, uint8_t length) +{ + uint8_t i, checksum; + + checksum = 0; + + for (i = 0; i < length; ++i) + checksum ^= buffer[i]; + + return checksum; +} + +/** + * Verify the GPS engine is sending the @@Hb position report message. If not, + * configure the GPS engine to send the desired report. + * + * @return true if GPS engine operation; otherwise false + */ +bool_t gpsSetup() +{ + uint8_t startTime, retryCount; + + // We wait 10 seconds for the GPS engine to respond to our message request. + startTime = timeGetTicks(); + retryCount = 0; + + while (++retryCount < 10) + { + // Read the serial FIFO and process the GPS messages. + gpsUpdate(); + + // If a GPS data set is available, then GPS is operational. + if (gpsIsReady()) + { + timeSetDutyCycle (TIME_DUTYCYCLE_10); + return true; + } + + if (timeGetTicks() > startTime) + { + puts ("@@Hb\001\053\015\012"); + startTime += 10; + } // END if + + } // END while + + return false; +} + +/** + * Parse the Motorola @@Hb (Short position/message) report. + */ +void gpsParsePositionMessage() +{ + // Convert the binary stream into data elements. We will scale to the desired units + // as the values are used. + gpsPosition.updateFlag = true; + + gpsPosition.month = gpsBuffer[0]; + gpsPosition.day = gpsBuffer[1]; + gpsPosition.year = ((uint16_t) gpsBuffer[2] << 8) | gpsBuffer[3]; + gpsPosition.hours = gpsBuffer[4]; + gpsPosition.minutes = gpsBuffer[5]; + gpsPosition.seconds = gpsBuffer[6]; + gpsPosition.latitude = ((int32) gpsBuffer[11] << 24) | ((int32) gpsBuffer[12] << 16) | ((int32) gpsBuffer[13] << 8) | (int32) gpsBuffer[14]; + gpsPosition.longitude = ((int32) gpsBuffer[15] << 24) | ((int32) gpsBuffer[16] << 16) | ((int32) gpsBuffer[17] << 8) | gpsBuffer[18]; + gpsPosition.altitudeCM = ((int32) gpsBuffer[19] << 24) | ((int32) gpsBuffer[20] << 16) | ((int32) gpsBuffer[21] << 8) | gpsBuffer[22]; + gpsPosition.altitudeFeet = gpsPosition.altitudeCM * 100l / 3048l; + gpsPosition.vSpeed = ((uint16_t) gpsBuffer[27] << 8) | gpsBuffer[28]; + gpsPosition.hSpeed = ((uint16_t) gpsBuffer[29] << 8) | gpsBuffer[30]; + gpsPosition.heading = ((uint16_t) gpsBuffer[31] << 8) | gpsBuffer[32]; + gpsPosition.dop = ((uint16_t) gpsBuffer[33] << 8) | gpsBuffer[34]; + gpsPosition.visibleSats = gpsBuffer[35]; + gpsPosition.trackedSats = gpsBuffer[36]; + gpsPosition.status = ((uint16_t) gpsBuffer[37] << 8) | gpsBuffer[38]; + + // Update the peak altitude if we have a valid 3D fix. + if (gpsGetFixType() == GPS_3D_FIX) + if (gpsPosition.altitudeFeet > gpsPeakAltitude) + gpsPeakAltitude = gpsPosition.altitudeFeet; +} + +/** + * Turn on the GPS engine power and serial interface. + */ +void gpsPowerOn() +{ + // 3.0 VDC LDO control line. + output_high (IO_GPS_PWR); + + // Enable the UART and the transmit line. +#asm + bsf 0xFAB.7 +#endasm +} + +/** + * Turn off the GPS engine power and serial interface. + */ +void gpsPowerOff() +{ + // Disable the UART and the transmit line. +#asm + bcf 0xFAB.7 +#endasm + + // 3.0 VDC LDO control line. + output_low (IO_GPS_PWR); +} + +/** + * Read the serial FIFO and process complete GPS messages. + */ +void gpsUpdate() +{ + uint8_t value; + + // This state machine handles each characters as it is read from the GPS serial port. + // We are looking for the GPS mesage @@Hb ... C + while (serialHasData()) + { + // Get the character value. + value = serialRead(); + + // Process based on the state machine. + switch (gpsParseState) + { + case GPS_START1: + if (value == '@') + gpsParseState = GPS_START2; + break; + + case GPS_START2: + if (value == '@') + gpsParseState = GPS_COMMAND1; + else + gpsParseState = GPS_START1; + break; + + case GPS_COMMAND1: + if (value == 'H') + gpsParseState = GPS_COMMAND2; + else + gpsParseState = GPS_START1; + break; + + case GPS_COMMAND2: + if (value == 'b') + { + gpsParseState = GPS_READMESSAGE; + gpsIndex = 0; + gpsChecksum = 0; + gpsChecksum ^= 'H'; + gpsChecksum ^= 'b'; + } else + gpsParseState = GPS_START1; + break; + + case GPS_READMESSAGE: + gpsChecksum ^= value; + gpsBuffer[gpsIndex++] = value; + + if (gpsIndex == 47) + gpsParseState = GPS_CHECKSUMMESSAGE; + + break; + + case GPS_CHECKSUMMESSAGE: + if (gpsChecksum == value) + gpsParseState = GPS_EOMCR; + else + gpsParseState = GPS_START1; + break; + + case GPS_EOMCR: + if (value == 13) + gpsParseState = GPS_EOMLF; + else + gpsParseState = GPS_START1; + break; + + case GPS_EOMLF: + // Once we have the last character, convert the binary message to something usable. + if (value == 10) + gpsParsePositionMessage(); + + gpsParseState = GPS_START1; + break; + } // END switch + } // END while +} + +/** @} */ + + +/** + * @defgroup log Flight Data Recorder + * + * Functions to manage and control the flight data recorder + * + * @{ + */ + +/// Number of bytes to buffer before writing to flash memory. +#define LOG_WRITE_BUFFER_SIZE 40 + +/// Last used address in flash memory. +uint32_t logAddress; + +/// Temporary buffer that holds data before it is written to flash device. +uint8_t logBuffer[LOG_WRITE_BUFFER_SIZE]; + +/// Current index into log buffer. +uint8_t logIndex; + +/** + * Last used address in flash memory. This location is where the next log data will + * be written. + * + * @return 24-bit flash memory address + */ +uint32_t logGetAddress() +{ + return logAddress; +} + +/** + * Write the contents of the temporary log buffer to the flash device. If the buffer + * is empty, nothing is done. + */ +void logFlush() +{ + // We only need to write if there is data. + if (logIndex != 0) + { + flashWriteBlock (logAddress, logBuffer, logIndex); + logAddress += logIndex; + logIndex = 0; + } // END if +} + +/** + * Prepare the flight data recorder for logging. + */ +void logInit() +{ + uint8_t buffer[8]; + bool_t endFound; + + fprintf (PC_HOST, "Searching for end of flash log..."); + + logAddress = 0x0000; + endFound = false; + + // Read each logged data block from flash to determine how long it is. + do + { + // Read the data log entry type. + flashReadBlock (logAddress, buffer, 1); + + // Based on the log entry type, we'll skip over the data contained in the entry. + switch (buffer[0]) + { + case LOG_BOOTED: + logAddress += 7; + break; + + case LOG_COORD: + logAddress += 26; + break; + + case LOG_TEMPERATURE: + logAddress += 3; + break; + + case LOG_VOLTAGE: + logAddress += 5; + break; + + case 0xff: + endFound = true; + break; + + default: + ++logAddress; + } // END switch + } while (logAddress < 0x100000 && !endFound); + + fprintf (PC_HOST, "done. Log contains %ld bytes.\n\r", logAddress); + + logIndex = 0; +} + +/** + * Start a entry in the data log. + * + * @param type of log entry, i.e. LOG_BOOTED, LOG_COORD, etc. + */ +void logType (LOG_TYPE type) +{ + // Only add the new entry if there is space. + if (logAddress >= 0x100000) + return; + + // Write the old entry first. + logFlush(); + + // Save the type and set the log buffer pointer. + logBuffer[0] = type; + logIndex = 1; +} + +/** + * Save an unsigned, 8-bit value in the log. + * + * @param value unsigned, 8-bit value + */ +void logUint8 (uint8_t value) +{ + logBuffer[logIndex++] = value; +} + +/** + * Save a signed, 16-bit value in the log. + * + * @param value signed, 16-bit value + */ +void logInt16 (int16_t value) +{ + logBuffer[logIndex++] = (value >> 8) & 0xff; + logBuffer[logIndex++] = value & 0xff; +} + +/** + * Save an unsigned, 16-bit value in the log. + * + * @param value unsigned, 16-bit value + */ +void logUint16 (uint16_t value) +{ + logBuffer[logIndex++] = (value >> 8) & 0xff; + logBuffer[logIndex++] = value & 0xff; +} + +/** + * Save a signed, 32-bit value in the log. + * + * @param value signed, 32-bit value + */ +void logInt32 (int32_t value) +{ + logBuffer[logIndex++] = (value >> 24) & 0xff; + logBuffer[logIndex++] = (value >> 16) & 0xff; + logBuffer[logIndex++] = (value >> 8) & 0xff; + logBuffer[logIndex++] = value & 0xff; +} + +/** @} */ + +/** + * @defgroup LM92 LM92 temperature sensor + * + * Read and control the National Semiconductor LM92 I2C temperature sensor + * + * @{ + */ + +/** + * Read the LM92 temperature value in 0.1 degrees F. + * + * @return 0.1 degrees F + */ +int16_t lm92GetTemp() +{ + int16_t value; + int32_t temp; + + // Set the SDA and SCL to input pins to control the LM92. + set_tris_c (0x9a); + + // Read the temperature register value. + i2c_start(); + i2c_write(0x97); + value = ((int16_t) i2c_read() << 8); + value = value | ((int16_t) i2c_read() & 0x00f8); + i2c_stop(); + + // Set the SDA and SCL back to outputs for use with the AD9954 because we share common clock pins. + set_tris_c (0x82); + + // LM92 register 0.0625degC/bit 9 10 9 + // ------------- * -------------- * - * -- = -- + 320 + // 8 5 64 + + // Convert to degrees F. + temp = (int32_t) value; + temp = ((temp * 9l) / 64l) + 320; + + return (int16_t) temp; +} + +/** @} */ + + +/** + * @defgroup serial Serial Port FIFO + * + * FIFO for the built-in serial port. + * + * @{ + */ + +/// Size of serial port FIFO in bytes. It must be a power of 2, i.e. 2, 4, 8, 16, etc. +#define SERIAL_BUFFER_SIZE 64 + +/// Mask to wrap around at end of circular buffer. (SERIAL_BUFFER_SIZE - 1) +#define SERIAL_BUFFER_MASK 0x3f + +/// Index to the next free location in the buffer. +uint8_t serialHead; + +/// Index to the next oldest data in the buffer. +uint8_t serialTail; + +/// Circular buffer (FIFO) to hold serial data. +uint8_t serialBuffer[SERIAL_BUFFER_SIZE]; + +/** + * Determine if the FIFO contains data. + * + * @return true if data present; otherwise false + */ +bool_t serialHasData() +{ + if (serialHead == serialTail) + return false; + + return true; +} + +/** + * Initialize the serial processor. + */ +void serialInit() +{ + serialHead = 0; + serialTail = 0; +} + +/** + * Get the oldest character from the FIFO. + * + * @return oldest character; 0 if FIFO is empty + */ +uint8_t serialRead() +{ + uint8_t value; + + // Make sure we have something to return. + if (serialHead == serialTail) + return 0; + + // Save the value. + value = serialBuffer[serialTail]; + + // Update the pointer. + serialTail = (serialTail + 1) & SERIAL_BUFFER_MASK; + + return value; +} + +/** + * Read and store any characters in the PIC serial port in a FIFO. + */ +void serialUpdate() +{ + // If there isn't a character in the PIC buffer, just leave. + while (kbhit()) + { + // Save the value in the FIFO. + serialBuffer[serialHead] = getc(); + + // Move the pointer to the next open space. + serialHead = (serialHead + 1) & SERIAL_BUFFER_MASK; + } +} + +/** @} */ + +/** + * @defgroup sys System Library Functions + * + * Generic system functions similiar to the run-time C library. + * + * @{ + */ + +/** + * Calculate the CRC-16 CCITT of buffer that is length bytes long. + * The crc parameter allow the calculation on the CRC on multiple buffers. + * + * @param buffer Pointer to data buffer. + * @param length number of bytes in data buffer + * @param crc starting value + * + * @return CRC-16 of buffer[0 .. length] + */ +uint16_t sysCRC16(uint8_t *buffer, uint8_t length, uint16_t crc) +{ + uint8_t i, bit, value; + + for (i = 0; i < length; ++i) + { + value = buffer[i]; + + for (bit = 0; bit < 8; ++bit) + { + crc ^= (value & 0x01); + crc = ( crc & 0x01 ) ? ( crc >> 1 ) ^ 0x8408 : ( crc >> 1 ); + value = value >> 1; + } // END for + } // END for + + return crc ^ 0xffff; +} + +/** + * Initialize the system library and global resources. + */ +void sysInit() +{ + gpsPowerOff (); + output_high (IO_LED); + + output_high (IO_CS); + output_low (IO_PS1); + output_low (IO_PS0); + output_low (IO_OSK); + output_low (IO_UPDATE); + output_low (IO_PTT); + output_low (IO_GPS_TXD); + + // Configure the port direction (input/output). + set_tris_a (0xc3); + set_tris_b (0x44); + set_tris_c (0x82); + + // Display a startup message during boot. + fprintf (PC_HOST, "System booted.\n\r"); +} + +/** + * Log the current GPS position. + */ +void sysLogGPSData() +{ + // Log the data. + logType (LOG_COORD); + logUint8 (gpsPosition.hours); + logUint8 (gpsPosition.minutes); + logUint8 (gpsPosition.seconds); + logInt32 (gpsPosition.latitude); + logInt32 (gpsPosition.longitude); + logInt32 (gpsPosition.altitudeCM); + + logUint16 (gpsPosition.vSpeed); + logUint16 (gpsPosition.hSpeed); + logUint16 (gpsPosition.heading); + + logUint16 (gpsPosition.status); + + logUint8 ((uint8_t) (gpsPosition.dop & 0xff)); + logUint8 ((uint8_t) ((gpsPosition.visibleSats << 4) | gpsPosition.trackedSats)); +} + +/** + * Log the ADC values of the bus and reference voltage values. + */ +void sysLogVoltage() +{ + logType (LOG_VOLTAGE); + logUint16 (adcRawBusVolt()); + logUint16 (adcRawRefVolt()); +} + +/** @} */ + +/** + * @defgroup rtc Real Time Interrupt tick + * + * Manage the built-in real time interrupt. The interrupt clock PRI is 104uS (9600 bps). + * + * @{ + */ + +/// A counter that ticks every 100mS. +uint8_t timeTicks; + +/// Counts the number of 104uS interrupts for a 100mS time period. +uint16_t timeInterruptCount; + +/// Counts the number of 100mS time periods in 1 second. +uint8_t time100ms; + +/// System time in seconds. +uint8_t timeSeconds; + +/// System time in minutes. +uint8_t timeMinutes; + +/// System time in hours. +uint8_t timeHours; + +/// Desired LED duty cycle 0 to 9 where 0 = 0% and 9 = 90%. +uint8_t timeDutyCycle; + +/// Current value of the timer 1 compare register used to generate 104uS interrupt rate (9600bps). +uint16_t timeCompare; + +/// 16-bit NCO where the upper 8-bits are used to index into the frequency generation table. +uint16_t timeNCO; + +/// Audio tone NCO update step (phase). +uint16_t timeNCOFreq; + +/// Counter used to deciminate down from the 104uS to 833uS interrupt rate. (9600 to 1200 baud) +uint8_t timeLowRateCount; + +/// Current TNC mode (standby, 1200bps A-FSK, or 9600bps FSK) +TNC_DATA_MODE tncDataMode; + +/// Flag set true once per second. +bool_t timeUpdateFlag; + +/// Flag that indicate the flight time should run. +bool_t timeRunFlag; + +/// The change in the CCP_1 register for each 104uS (9600bps) interrupt period. +#define TIME_RATE 125 + +/** + * Running 8-bit counter that ticks every 100mS. + * + * @return 100mS time tick + */ +uint8_t timeGetTicks() +{ + return timeTicks; +} + +/** + * Initialize the real-time clock. + */ +void timeInit() +{ + timeTicks = 0; + timeInterruptCount = 0; + time100mS = 0; + timeSeconds = 0; + timeMinutes = 0; + timeHours = 0; + timeDutyCycle = TIME_DUTYCYCLE_70; + timeCompare = TIME_RATE; + timeUpdateFlag = false; + timeNCO = 0x00; + timeLowRateCount = 0; + timeNCOFreq = 0x2000; + tncDataMode = TNC_MODE_STANDBY; + timeRunFlag = false; + + // Configure CCP1 to interrupt at 1mS for PSK31 or 833uS for 1200 baud APRS + CCP_1 = TIME_RATE; + set_timer1(timeCompare); + setup_ccp1( CCP_COMPARE_INT ); + setup_timer_1( T1_INTERNAL | T1_DIV_BY_4 ); +} + +/** + * Function return true once a second based on real-time clock. + * + * @return true on one second tick; otherwise false + */ +bool_t timeIsUpdate() +{ + if (timeUpdateFlag) + { + timeUpdateFlag = false; + return true; + } // END if + + return false; +} + +/** + * Set the blink duty cycle of the heartbeat LED. The LED blinks at a 1Hz rate. + * + * @param dutyCycle TIME_DUTYCYCLE_xx constant + */ +void timeSetDutyCycle (uint8_t dutyCycle) +{ + timeDutyCycle = dutyCycle; +} + +/** + * Set a flag to indicate the flight time should run. This flag is typically set when the payload + * lifts off. + */ +void timeSetRunFlag() +{ + timeRunFlag = true; +} + +#INT_CCP1 +/** + * Timer interrupt handler called every 104uS (9600 times/second). + */ +void timeUpdate() +{ + // Setup the next interrupt for the operational mode. + timeCompare += TIME_RATE; + CCP_1 = timeCompare; + + switch (tncDataMode) + { + case TNC_MODE_STANDBY: + break; + + case TNC_MODE_1200_AFSK: + ddsSetFTW (freqTable[timeNCO >> 8]); + + timeNCO += timeNCOFreq; + + if (++timeLowRateCount == 8) + { + timeLowRateCount = 0; + tnc1200TimerTick(); + } // END if + break; + + case TNC_MODE_9600_FSK: + tnc9600TimerTick(); + break; + } // END switch + + // Read the GPS serial port and save any incoming characters. + serialUpdate(); + + // Count the number of milliseconds required for the tenth second counter. + if (++timeInterruptCount == 960) + { + timeInterruptCount = 0; + + // This timer just ticks every 100mS and is used for general timing. + ++timeTicks; + + // Roll the counter over every second. + if (++time100mS == 10) + { + time100mS = 0; + + // We set this flag true every second. + timeUpdateFlag = true; + + // Maintain a Real Time Clock. + if (timeRunFlag) + if (++timeSeconds == 60) + { + timeSeconds = 0; + + if (++timeMinutes == 60) + { + timeMinutes = 0; + ++timeHours; + } // END if timeMinutes + } // END if timeSeconds + } // END if time100mS + + // Flash the status LED at timeDutyCycle % per second. We use the duty cycle for mode feedback. + if (time100mS >= timeDutyCycle) + output_low (IO_LED); + else + output_high (IO_LED); + } // END if +} + +/** @} */ + +/** + * @defgroup tnc TNC (Terminal Node Controller) + * + * Functions that provide a subset of the TNC functions. + * + * @{ + */ + +/// The number of start flag bytes to send before the packet message. (360bits * 1200bps = 300mS) +#define TNC_TX_DELAY 45 + +/// The size of the TNC output buffer. +#define TNC_BUFFER_SIZE 80 + +/// States that define the current mode of the 1200 bps (A-FSK) state machine. +enum TNC_TX_1200BPS_STATE +{ + /// Stand by state ready to accept new message. + TNC_TX_READY, + + /// 0x7E bit stream pattern used to define start of APRS message. + TNC_TX_SYNC, + + /// Transmit the AX.25 header that contains the source/destination call signs, APRS path, and flags. + TNC_TX_HEADER, + + /// Transmit the message data. + TNC_TX_DATA, + + /// Transmit the end flag sequence. + TNC_TX_END +}; + +/// Enumeration of the messages we can transmit. +enum TNC_MESSAGE_TYPE +{ + /// Startup message that contains software version information. + TNC_BOOT_MESSAGE, + + /// Plain text status message. + TNC_STATUS, + + /// Message that contains GPS NMEA-0183 $GPGGA message. + TNC_GGA, + + /// Message that contains GPS NMEA-0183 $GPRMC message. + TNC_RMC +}; + +/// AX.25 compliant packet header that contains destination, station call sign, and path. +/// 0x76 for SSID-11, 0x78 for SSID-12 +uint8_t TNC_AX25_HEADER[30] = { + 'A' << 1, 'P' << 1, 'R' << 1, 'S' << 1, ' ' << 1, ' ' << 1, 0x60, \ + 'K' << 1, 'D' << 1, '7' << 1, 'L' << 1, 'M' << 1, 'O' << 1, 0x76, \ + 'G' << 1, 'A' << 1, 'T' << 1, 'E' << 1, ' ' << 1, ' ' << 1, 0x60, \ + 'W' << 1, 'I' << 1, 'D' << 1, 'E' << 1, '3' << 1, ' ' << 1, 0x67, \ + 0x03, 0xf0 }; + + +/// The next bit to transmit. +uint8_t tncTxBit; + +/// Current mode of the 1200 bps state machine. +TNC_TX_1200BPS_STATE tncMode; + +/// Counter for each bit (0 - 7) that we are going to transmit. +uint8_t tncBitCount; + +/// A shift register that holds the data byte as we bit shift it for transmit. +uint8_t tncShift; + +/// Index into the APRS header and data array for each byte as we transmit it. +uint8_t tncIndex; + +/// The number of bytes in the message portion of the AX.25 message. +uint8_t tncLength; + +/// A copy of the last 5 bits we've transmitted to determine if we need to bit stuff on the next bit. +uint8_t tncBitStuff; + +/// Pointer to TNC buffer as we save each byte during message preparation. +uint8_t *tncBufferPnt; + +/// The type of message to tranmit in the next packet. +TNC_MESSAGE_TYPE tncPacketType; + +/// Buffer to hold the message portion of the AX.25 packet as we prepare it. +uint8_t tncBuffer[TNC_BUFFER_SIZE]; + +/// Flag that indicates we want to transmit every 5 seconds. +bool_t tncHighRateFlag; + +/** + * Initialize the TNC internal variables. + */ +void tncInit() +{ + tncTxBit = 0; + tncMode = TNC_TX_READY; + tncPacketType = TNC_BOOT_MESSAGE; + tncHighRateFlag = false; +} + +/** + * Determine if the hardware if ready to transmit a 1200 baud packet. + * + * @return true if ready; otherwise false + */ +bool_t tncIsFree() +{ + if (tncMode == TNC_TX_READY) + return true; + + return false; +} + +void tncHighRate(bool_t state) +{ + tncHighRateFlag = state; +} + +/** + * Configure the TNC for the desired data mode. + * + * @param dataMode enumerated type that specifies 1200bps A-FSK or 9600bps FSK + */ +void tncSetMode(TNC_DATA_MODE dataMode) +{ + switch (dataMode) + { + case TNC_MODE_1200_AFSK: + ddsSetMode (DDS_MODE_AFSK); + break; + + case TNC_MODE_9600_FSK: + ddsSetMode (DDS_MODE_FSK); + + // FSK tones at 445.947 and 445.953 MHz + ddsSetFSKFreq (955382980, 955453621); + break; + } // END switch + + tncDataMode = dataMode; +} + +/** + * Determine if the seconds value timeSeconds is a valid time slot to transmit + * a message. Time seconds is in UTC. + * + * @param timeSeconds UTC time in seconds + * + * @return true if valid time slot; otherwise false + */ +bool_t tncIsTimeSlot (uint8_t timeSeconds) +{ + if (tncHighRateFlag) + { + if ((timeSeconds % 5) == 0) + return true; + + return false; + } // END if + + switch (timeSeconds) + { + case 0: + case 15: + case 30: + case 45: + return true; + + default: + return false; + } // END switch +} + +/** + * Method that is called every 833uS to transmit the 1200bps A-FSK data stream. + * The provides the pre and postamble as well as the bit stuffed data stream. + */ +void tnc1200TimerTick() +{ + // Set the A-FSK frequency. + if (tncTxBit == 0x00) + timeNCOFreq = 0x2000; + else + timeNCOFreq = 0x3aab; + + switch (tncMode) + { + case TNC_TX_READY: + // Generate a test signal alteranting between high and low tones. + tncTxBit = (tncTxBit == 0 ? 1 : 0); + break; + + case TNC_TX_SYNC: + // The variable tncShift contains the lastest data byte. + // NRZI enocde the data stream. + if ((tncShift & 0x01) == 0x00) + if (tncTxBit == 0) + tncTxBit = 1; + else + tncTxBit = 0; + + // When the flag is done, determine if we need to send more or data. + if (++tncBitCount == 8) + { + tncBitCount = 0; + tncShift = 0x7e; + + // Once we transmit x mS of flags, send the data. + // txDelay bytes * 8 bits/byte * 833uS/bit = x mS + if (++tncIndex == TNC_TX_DELAY) + { + tncIndex = 0; + tncShift = TNC_AX25_HEADER[0]; + tncBitStuff = 0; + tncMode = TNC_TX_HEADER; + } // END if + } else + tncShift = tncShift >> 1; + break; + + case TNC_TX_HEADER: + // Determine if we have sent 5 ones in a row, if we have send a zero. + if (tncBitStuff == 0x1f) + { + if (tncTxBit == 0) + tncTxBit = 1; + else + tncTxBit = 0; + + tncBitStuff = 0x00; + return; + } // END if + + // The variable tncShift contains the lastest data byte. + // NRZI enocde the data stream. + if ((tncShift & 0x01) == 0x00) + if (tncTxBit == 0) + tncTxBit = 1; + else + tncTxBit = 0; + + // Save the data stream so we can determine if bit stuffing is + // required on the next bit time. + tncBitStuff = ((tncBitStuff << 1) | (tncShift & 0x01)) & 0x1f; + + // If all the bits were shifted, get the next byte. + if (++tncBitCount == 8) + { + tncBitCount = 0; + + // After the header is sent, then send the data. + if (++tncIndex == sizeof(TNC_AX25_HEADER)) + { + tncIndex = 0; + tncShift = tncBuffer[0]; + tncMode = TNC_TX_DATA; + } else + tncShift = TNC_AX25_HEADER[tncIndex]; + + } else + tncShift = tncShift >> 1; + + break; + + case TNC_TX_DATA: + // Determine if we have sent 5 ones in a row, if we have send a zero. + if (tncBitStuff == 0x1f) + { + if (tncTxBit == 0) + tncTxBit = 1; + else + tncTxBit = 0; + + tncBitStuff = 0x00; + return; + } // END if + + // The variable tncShift contains the lastest data byte. + // NRZI enocde the data stream. + if ((tncShift & 0x01) == 0x00) + if (tncTxBit == 0) + tncTxBit = 1; + else + tncTxBit = 0; + + // Save the data stream so we can determine if bit stuffing is + // required on the next bit time. + tncBitStuff = ((tncBitStuff << 1) | (tncShift & 0x01)) & 0x1f; + + // If all the bits were shifted, get the next byte. + if (++tncBitCount == 8) + { + tncBitCount = 0; + + // If everything was sent, transmit closing flags. + if (++tncIndex == tncLength) + { + tncIndex = 0; + tncShift = 0x7e; + tncMode = TNC_TX_END; + } else + tncShift = tncBuffer[tncIndex]; + + } else + tncShift = tncShift >> 1; + + break; + + case TNC_TX_END: + // The variable tncShift contains the lastest data byte. + // NRZI enocde the data stream. + if ((tncShift & 0x01) == 0x00) + if (tncTxBit == 0) + tncTxBit = 1; + else + tncTxBit = 0; + + // If all the bits were shifted, get the next one. + if (++tncBitCount == 8) + { + tncBitCount = 0; + tncShift = 0x7e; + + // Transmit two closing flags. + if (++tncIndex == 2) + { + tncMode = TNC_TX_READY; + + // Tell the TNC time interrupt to stop generating the frequency words. + tncDataMode = TNC_MODE_STANDBY; + + // Key off the DDS. + output_low (IO_OSK); + output_low (IO_PTT); + ddsSetMode (DDS_MODE_POWERDOWN); + + return; + } // END if + } else + tncShift = tncShift >> 1; + + break; + } // END switch +} + +/** + * Method that is called every 104uS to transmit the 9600bps FSK data stream. + */ +void tnc9600TimerTick() +{ + +} + +/** + * Write character to the TNC buffer. Maintain the pointer + * and length to the buffer. The pointer tncBufferPnt and tncLength + * must be set before calling this function for the first time. + * + * @param character to save to telemetry buffer + */ +void tncTxByte (uint8_t character) +{ + *tncBufferPnt++ = character; + ++tncLength; +} + +/** + * Generate the GPS NMEA standard UTC time stamp. Data is written through the tncTxByte + * callback function. + */ +void tncNMEATime() +{ + // UTC of position fix. + printf (tncTxByte, "%02d%02d%02d,", gpsPosition.hours, gpsPosition.minutes, gpsPosition.seconds); +} + +/** + * Generate the GPS NMEA standard latitude/longitude fix. Data is written through the tncTxByte + * callback function. + */ +void tncNMEAFix() +{ + uint8_t dirChar; + uint32_t coord, coordMin; + + // Latitude value. + coord = gpsPosition.latitude; + + if (gpsPosition.latitude < 0) + { + coord = gpsPosition.latitude * -1; + dirChar = 'S'; + } else { + coord = gpsPosition.latitude; + dirChar = 'N'; + } + + coordMin = (coord % 3600000) / 6; + printf (tncTxByte, "%02ld%02ld.%04ld,%c,", (uint32_t) (coord / 3600000), (uint32_t) (coordMin / 10000), (uint32_t) (coordMin % 10000), dirChar); + + + // Longitude value. + if (gpsPosition.longitude < 0) + { + coord = gpsPosition.longitude * - 1; + dirChar = 'W'; + } else { + coord = gpsPosition.longitude; + dirChar = 'E'; + } + + coordMin = (coord % 3600000) / 6; + printf (tncTxByte, "%03ld%02ld.%04ld,%c,", (uint32_t) (coord / 3600000), (uint32_t) (coordMin / 10000), (uint32_t) (coordMin % 10000), dirChar); + +} + +/** + * Generate the GPS NMEA-0183 $GPGGA packet. Data is written through the tncTxByte + * callback function. + */ +void tncGPGGAPacket() +{ + // Generate the GPGGA message. + printf (tncTxByte, "$GPGGA,"); + + // Standard NMEA time. + tncNMEATime(); + + // Standard NMEA-0183 latitude/longitude. + tncNMEAFix(); + + // GPS status where 0: not available, 1: available + if (gpsGetFixType() != GPS_NO_FIX) + printf (tncTxByte, "1,"); + else + printf (tncTxByte, "0,"); + + // Number of visible birds. + printf (tncTxByte, "%02d,", gpsPosition.trackedSats); + + // DOP + printf (tncTxByte, "%ld.%01ld,", gpsPosition.dop / 10, gpsPosition.dop % 10); + + // Altitude in meters. + printf (tncTxByte, "%ld.%02ld,M,,M,,", (int32_t) (gpsPosition.altitudeCM / 100l), (int32_t) (gpsPosition.altitudeCM % 100)); + + // Checksum, we add 1 to skip over the $ character. + printf (tncTxByte, "*%02X", gpsNMEAChecksum(tncBuffer + 1, tncLength - 1)); +} + +/** + * Generate the GPS NMEA-0183 $GPRMC packet. Data is written through the tncTxByte + * callback function. + */ +void tncGPRMCPacket() +{ + uint32_t temp; + + // Generate the GPRMC message. + printf (tncTxByte, "$GPRMC,"); + + // Standard NMEA time. + tncNMEATime(); + + // GPS status. + if (gpsGetFixType() != GPS_NO_FIX) + printf (tncTxByte, "A,"); + else + printf (tncTxByte, "V,"); + + // Standard NMEA-0183 latitude/longitude. + tncNMEAFix(); + + // Speed knots and heading. + temp = (int32_t) gpsPosition.hSpeed * 75000 / 385826; + printf (tncTxByte, "%ld.%ld,%ld.%ld,", (int16_t) (temp / 10), (int16_t) (temp % 10), gpsPosition.heading / 10, gpsPosition.heading % 10); + + // Date + printf (tncTxByte, "%02d%02d%02ld,,", gpsPosition.day, gpsPosition.month, gpsPosition.year % 100); + + // Checksum, skip over the $ character. + printf (tncTxByte, "*%02X", gpsNMEAChecksum(tncBuffer + 1, tncLength - 1)); +} + +/** + * Generate the plain text status packet. Data is written through the tncTxByte + * callback function. + */ +void tncStatusPacket(int16_t temperature) +{ + uint16_t voltage; + + // Plain text telemetry. + printf (tncTxByte, ">ANSR "); + + // Display the flight time. + printf (tncTxByte, "%02U:%02U:%02U ", timeHours, timeMinutes, timeSeconds); + + // Altitude in feet. + printf (tncTxByte, "%ld' ", gpsPosition.altitudeFeet); + + // Peak altitude in feet. + printf (tncTxByte, "%ld'pk ", gpsGetPeakAltitude()); + + // GPS hdop or pdop + printf (tncTxByte, "%lu.%lu", gpsPosition.dop / 10, gpsPosition.dop % 10); + + // The text 'pdop' for a 3D fix, 'hdop' for a 2D fix, and 'dop' for no fix. + switch (gpsGetFixType()) + { + case GPS_NO_FIX: + printf (tncTxByte, "dop "); + break; + + case GPS_2D_FIX: + printf (tncTxByte, "hdop "); + break; + + + case GPS_3D_FIX: + printf (tncTxByte, "pdop "); + break; + } // END switch + + // Number of satellites in the solution. + printf (tncTxByte, "%utrk ", gpsPosition.trackedSats); + + // Display main bus voltage. + voltage = adcGetMainBusVolt(); + printf (tncTxByte, "%lu.%02luvdc ", voltage / 100, voltage % 100); + + // Display internal temperature. + printf (tncTxByte, "%ld.%01ldF ", temperature / 10, abs(temperature % 10)); + + // Print web address link. + printf (tncTxByte, "www.kd7lmo.net"); +} + +/** + * Prepare an AX.25 data packet. Each time this method is called, it automatically + * rotates through 1 of 3 messages. + * + * @param dataMode enumerated type that specifies 1200bps A-FSK or 9600bps FSK + */ +void tncTxPacket(TNC_DATA_MODE dataMode) +{ + int16_t temperature; + uint16_t crc; + + // Only transmit if there is not another message in progress. + if (tncMode != TNC_TX_READY) + return; + + // Log the battery and reference voltage before we start the RF chain. + sysLogVoltage(); + + // We need to read the temperature sensor before we setup the DDS since they share a common clock pin. + temperature = lm92GetTemp(); + + // Log the system temperature every time we transmit a packet. + logType (LOG_TEMPERATURE); + logInt16 (temperature); + + // Configure the DDS for the desired operational. + tncSetMode (dataMode); + + // Set a pointer to our TNC output buffer. + tncBufferPnt = tncBuffer; + + // Set the message length counter. + tncLength = 0; + + // Determine the contents of the packet. + switch (tncPacketType) + { + case TNC_BOOT_MESSAGE: + printf (tncTxByte, ">ANSR Pico Beacon - V3.05"); + + // Select the next packet we will generate. + tncPacketType = TNC_STATUS; + break; + + case TNC_STATUS: + tncStatusPacket(temperature); + + // Select the next packet we will generate. + tncPacketType = TNC_GGA; + break; + + case TNC_GGA: + tncGPGGAPacket(); + + // Select the next packet we will generate. + tncPacketType = TNC_RMC; + break; + + case TNC_RMC: + tncGPRMCPacket(); + + // Select the next packet we will generate. + tncPacketType = TNC_STATUS; + break; + } + + // Add the end of message character. + printf (tncTxByte, "\015"); + + // Calculate the CRC for the header and message. + crc = sysCRC16(TNC_AX25_HEADER, sizeof(TNC_AX25_HEADER), 0xffff); + crc = sysCRC16(tncBuffer, tncLength, crc ^ 0xffff); + + // Save the CRC in the message. + *tncBufferPnt++ = crc & 0xff; + *tncBufferPnt = (crc >> 8) & 0xff; + + // Update the length to include the CRC bytes. + tncLength += 2; + + // Prepare the variables that are used in the real-time clock interrupt. + tncBitCount = 0; + tncShift = 0x7e; + tncTxBit = 0; + tncIndex = 0; + tncMode = TNC_TX_SYNC; + + // Turn on the PA chain. + output_high (IO_PTT); + + // Wait for the PA chain to power up. + delay_ms (10); + + // Key the DDS. + output_high (IO_OSK); + + // Log the battery and reference voltage just after we key the transmitter. + sysLogVoltage(); +} + +/** @} */ + +uint32_t counter; + +uint8_t bitIndex; +uint8_t streamIndex; +uint8_t value; + +uint8_t bitStream[] = { 0x10, 0x20, 0x30 }; + +void init() +{ + counter = 0; + bitIndex = 0; + streamIndex = 0; + value = bitStream[0]; +} + +void test() +{ + counter += 0x10622d; + + CCP_1 = (uint16_t) ((counter >> 16) & 0xffff); + + if ((value & 0x80) == 0x80) + setup_ccp1 (CCP_COMPARE_SET_ON_MATCH); + else + setup_ccp1 (CCP_COMPARE_CLR_ON_MATCH); + + if (++bitIndex == 8) + { + bitIndex = 0; + + if (++streamIndex == sizeof(bitStream)) + { + streamIndex = 0; + } + + value = bitStream[streamIndex]; + } else + value = value << 1; +} + +// This is where we go after reset. +void main() +{ + uint8_t i, utcSeconds, lockLostCounter; + +test(); + + // Configure the basic systems. + sysInit(); + + // Wait for the power converter chains to stabilize. + delay_ms (100); + + // Setup the subsystems. + adcInit(); + flashInit(); + gpsInit(); + logInit(); + timeInit(); + serialInit(); + tncInit(); + + // Program the DDS. + ddsInit(); + + // Turn off the LED after everything is configured. + output_low (IO_LED); + + // Check for the diagnostics plug, otherwise we'll continue to boot. + diagPort(); + + // Setup our interrupts. + enable_interrupts(GLOBAL); + enable_interrupts(INT_CCP1); + + // Turn on the GPS engine. + gpsPowerOn(); + + // Allow the GPS engine to boot. + delay_ms (250); + + // Initialize the GPS engine. + while (!gpsSetup()); + + // Charge the ADC filters. + for (i = 0; i < 32; ++i) + adcUpdate(); + + // Log startup event. + logType (LOG_BOOTED); + logUint8 (gpsPosition.month); + logUint8 (gpsPosition.day); + logUint8 (gpsPosition.year & 0xff); + + logUint8 (gpsPosition.hours); + logUint8 (gpsPosition.minutes); + logUint8 (gpsPosition.seconds); + + // Transmit software version packet on start up. + tncTxPacket(TNC_MODE_1200_AFSK); + + // Counters to send packets if the GPS time stamp is not available. + lockLostCounter = 5; + utcSeconds = 55; + + // This is the main loop that process GPS data and waits for the once per second timer tick. + for (;;) + { + // Read the GPS engine serial port FIFO and process the GPS data. + gpsUpdate(); + + if (gpsIsReady()) + { + // Start the flight timer when we get a valid 3D fix. + if (gpsGetFixType() == GPS_3D_FIX) + timeSetRunFlag(); + + // Generate our packets based on the GPS time. + if (tncIsTimeSlot(gpsPosition.seconds)) + tncTxPacket(TNC_MODE_1200_AFSK); + + // Sync the internal clock to GPS UTC time. + utcSeconds = gpsPosition.seconds; + + // This counter is reset every time we receive the GPS message. + lockLostCounter = 0; + + // Log the data to flash. + sysLogGPSData(); + } // END if gpsIsReady + + // Processing that occurs once a second. + if (timeIsUpdate()) + { + // We maintain the UTC time in seconds if we shut off the GPS engine or it fails. + if (++utcSeconds == 60) + utcSeconds = 0; + + // If we loose information for more than 5 seconds, + // we will determine when to send a packet based on internal time. + if (lockLostCounter == 5) + { + if (tncIsTimeSlot(utcSeconds)) + tncTxPacket(TNC_MODE_1200_AFSK); + } else + ++lockLostCounter; + + // Update the ADC filters. + adcUpdate(); + + if (timeHours == 5 && timeMinutes == 0 && timeSeconds == 0) + gpsPowerOff(); + + } // END if timeIsUpdate + + } // END for +} + + + -- cgit v1.2.3 From d65751fded3321b8a350e4140c44f87fec95aab2 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 5 Dec 2012 19:30:27 -0800 Subject: altos: Make aprs code output encoded packets to stdout This generates a .wav file containing a single APRS packet. This has been tested and appears to be successfully decoded by an APRS receiver. Signed-off-by: Keith Packard --- src/drivers/ao_aprs.c | 1704 ++++--------------------------------------------- 1 file changed, 123 insertions(+), 1581 deletions(-) (limited to 'src') diff --git a/src/drivers/ao_aprs.c b/src/drivers/ao_aprs.c index 79cea49a..be7abaf5 100644 --- a/src/drivers/ao_aprs.c +++ b/src/drivers/ao_aprs.c @@ -139,96 +139,22 @@ * */ -// Hardware specific configuration. -#include <18f2525.h> -#device ADC=10 - -// NOTE: Even though we are using an external clock, we set the HS oscillator mode to -// make the PIC 18F252 work with our external clock which is a clipped 1V P-P sine wave. -#fuses HS,NOWDT,NOPROTECT,NOPUT,NOBROWNOUT,NOLVP - -// C runtime library definitions. -#include -#include - -// These compiler directives set the clock, SPI/I2C ports, and I/O configuration. - -// TCXO frequency -#use delay(clock=19200000) - -// Engineering and data extracation port. -#use rs232(baud=57600, xmit=PIN_B7, rcv=PIN_B6, STREAM=PC_HOST) - -// GPS engine -#use rs232(baud=9600, xmit=PIN_C6, rcv=PIN_C7) - -#use i2c (master, scl=PIN_C3, sda=PIN_C4) - -#use fast_io(A) -#use fast_io(B) -#use fast_io(C) - -// We define types that are used for all variables. These are declared -// because each processor has a different sizes for int and long. -// The PIC compiler defines int8_t, int16_t, and int32_t. - -/// Boolean value { false, true } -typedef boolean bool_t; - -/// Signed 8-bit number in the range -128 through 127. -typedef signed int8 int8_t; - -/// Unsigned 8-bit number in the range 0 through 255. -typedef unsigned int8 uint8_t; - -/// Signed 16-bit number in the range -32768 through 32767. -typedef signed int16 int16_t; - -/// Unsigned 16-bit number in the range 0 through 65535. -typedef unsigned int16 uint16_t; - -/// Signed 32-bit number in the range -2147483648 through 2147483647. -typedef signed int32 int32_t; - -/// Unsigned 32-bit number in the range 0 through 4294967296. -typedef unsigned int32 uint32_t; - -// Function and structure prototypes. These are declared at the start of -// the file much like a C++ header file. - -// Map I/O pin names to hardware pins. - -/// Heartbeat LED - Port A2 -#define IO_LED PIN_A2 - -/// AD9954 DDS Profile Select 0 - Port A3 -#define IO_PS0 PIN_A3 - -/// UHF amplifier and PA chain - Port A4 -#define IO_PTT PIN_A4 - -/// AD9954 DDS Update - Port A5 -#define IO_UPDATE PIN_A5 - -/// AD9954 CS (Chip Select) - Port B0 -#define IO_CS PIN_B0 - -/// GPS Engine Power - Port B1 -#define IO_GPS_PWR PIN_B1 - -/// AD9954 DDS Profile Select 1 - Port C0 -#define IO_PS1 PIN_C0 - -/// AD9954 DDS OSK (Output Shift Key) - Port C2 -#define IO_OSK PIN_C2 - -/// GPS engine serial transmit pin - Port C6 -#define IO_GPS_TXD PIN_C6 +#include +#include +#include +#include +#include +#include + +typedef int bool_t; +typedef int32_t int32; +#define false 0 +#define true 1 // Public methods, constants, and data structures for each class. /// Operational modes of the AD9954 DDS for the ddsSetMode function. -enum DDS_MODE +typedef enum { /// Device has not been initialized. DDS_MODE_NOT_INITIALIZED, @@ -241,7 +167,7 @@ enum DDS_MODE /// Generate true FSK tones. DDS_MODE_FSK -}; +} DDS_MODE; void ddsInit(); void ddsSetAmplitude (uint8_t amplitude); @@ -251,15 +177,8 @@ void ddsSetFreq (uint32_t freq); void ddsSetFTW (uint32_t ftw); void ddsSetMode (DDS_MODE mode); -void flashErase(); -uint8_t flashGetByte (); -void flashReadBlock(uint32_t address, uint8_t *block, uint16_t length); -void flashSendByte(uint8_t value); -void flashSendAddress(uint32_t address); -void flashWriteBlock(uint32_t address, uint8_t *block, uint8_t length); - /// Type of GPS fix. -enum GPS_FIX_TYPE +typedef enum { /// No GPS FIX GPS_NO_FIX, @@ -269,7 +188,7 @@ enum GPS_FIX_TYPE /// 3D (Latitude/Longitude/Altitude) fix. GPS_3D_FIX -}; +} GPS_FIX_TYPE; /// GPS Position information. typedef struct @@ -329,6 +248,8 @@ typedef struct uint8_t visibleSats; } GPSPOSITION_STRUCT; +GPSPOSITION_STRUCT gpsPosition; + void gpsInit(); bool_t gpsIsReady(); GPS_FIX_TYPE gpsGetFixType(); @@ -337,47 +258,7 @@ void gpsPowerOn(); bool_t gpsSetup(); void gpsUpdate(); -int16_t lm92GetTemp(); - -/// Define the log record types. -enum LOG_TYPE -{ - /// Time stamp the log was started. - LOG_BOOTED = 0xb4, - - /// GPS coordinates. - LOG_COORD = 0xa5, - - /// Temperature - LOG_TEMPERATURE = 0x96, - - /// Bus voltage. - LOG_VOLTAGE = 0x87 -}; - -void logInit(); -uint32_t logGetAddress(); -void logType (LOG_TYPE type); -void logUint8 (uint8_t value); -void logInt16 (int16_t value); - -bool_t serialHasData(); -void serialInit(); -uint8_t serialRead(); -void serialUpdate(); - uint16_t sysCRC16(uint8_t *buffer, uint8_t length, uint16_t crc); -void sysInit(); -void sysLogVoltage(); - -/// 0% duty cycle (LED Off) constant for function timeSetDutyCycle -#define TIME_DUTYCYCLE_0 0 - -/// 10% duty cycle constant for function timeSetDutyCycle -#define TIME_DUTYCYCLE_10 1 - -/// 70% duty cycle constant for function timeSetDutyCycle -#define TIME_DUTYCYCLE_70 7 uint8_t timeGetTicks(); void timeInit(); @@ -385,7 +266,7 @@ void timeSetDutyCycle (uint8_t dutyCycle); void timeUpdate(); /// Operational modes of the TNC for the tncSetMode function. -enum TNC_DATA_MODE +typedef enum { /// No operation waiting for setup and configuration. TNC_MODE_STANDBY, @@ -395,7 +276,7 @@ enum TNC_DATA_MODE /// 9600 bps using true FSK tones. TNC_MODE_9600_FSK -}; +} TNC_DATA_MODE; void tncInit(); bool_t tncIsFree(); @@ -406,395 +287,8 @@ void tnc9600TimerTick(); void tncTxByte (uint8_t value); void tncTxPacket(TNC_DATA_MODE dataMode); -/** - * @defgroup ADC Analog To Digital Converter - * - * Control and manage the on board PIC A/D converter. - * - * @{ - */ - -/// Filtered voltages using a single pole, low pass filter. -uint16_t adcMainBusVolt; - -/// PIC ADC Channel number of the reference voltage. -#define ADC_REF 0 - -/// PIC ADC Channel number of the main bus voltage. -#define ADC_MAINBUS 1 - -/// Input diode drop in units of 0.01 volts. -#define MAIN_BUS_VOLT_OFFSET 20 - -/** - * Intialize the ADC subsystem. - */ -void adcInit() -{ - // Setup the ADC. - setup_adc_ports(AN0_TO_AN1); - setup_adc( ADC_CLOCK_DIV_32 ); - - // Zero the ADC filters. - adcMainBusVolt = 0; -} - -/** - * Filtered main bus voltage in 10mV resolution. - * - * @return voltage in 10mV steps - */ -uint16_t adcGetMainBusVolt() -{ - uint32_t volts; - - volts = (uint32_t) (adcMainBusVolt >> 3); - - volts = (volts * 330l) / 1023l; - - return (uint16_t) volts + MAIN_BUS_VOLT_OFFSET; -} - -/** - * Get the current ADC value for the main bus voltage. - * - * @return ADC value in the range 0 to 1023 - */ -uint16_t adcRawBusVolt() -{ - set_adc_channel(ADC_MAINBUS); - delay_us(50); - return read_adc(); -} - -/** - * Get the current ADC value for the reference source voltage. - * - * @return ADC value in the range 0 to 1023 - */ -uint16_t adcRawRefVolt() -{ - set_adc_channel(ADC_REF); - delay_us(50); - return read_adc(); -} - -/** - * Read and filter the ADC channels for bus voltages. - */ -void adcUpdate(void) -{ - // Filter the bus voltage using a single pole low pass filter. - set_adc_channel(ADC_MAINBUS); - delay_us(50); - adcMainBusVolt = read_adc() + adcMainBusVolt - (adcMainBusVolt >> 3); -} - -/** @} */ - - -/** - * @defgroup diag Diagnostics and Control - * - * Functions for diagnostics and control of the hardware and flight data recorder. - * - * @{ - */ - -/// Number of bytes per line to display when reading flight data recorder. -#define DIAG_BYTES_PER_LINE 32 - -/** - * Process the command to erase the data logger flash. - */ -void diagEraseFlash() -{ - // Confirm we want to erase the flash with the key sequence 'yes' . - fprintf (PC_HOST, "Are you sure (yes)? "); - - if (fgetc(PC_HOST) != 'y') - return; - - if (fgetc(PC_HOST) != 'e') - return; - - if (fgetc(PC_HOST) != 's') - return; - - if (fgetc(PC_HOST) != 13) - return; - - // User feedback and erase the part. - fprintf (PC_HOST, "Erasing flash..."); - - flashErase(); - - fprintf (PC_HOST, "done.\n\r"); -} - -/** - * Display the engineering mode menu. - */ -void diagMenu() -{ - // User interface. - fprintf (PC_HOST, "Options: (e)rase Flash, (r)ead Flash\n\r"); - fprintf (PC_HOST, " Toggle (L)ED\n\r"); - fprintf (PC_HOST, " (P)TT - Push To Transmit\n\r"); - fprintf (PC_HOST, " (f)requencey down, (F)requency up - 1KHz step\n\r"); - fprintf (PC_HOST, " (c)hannel down, (C)hannel up - 25KHz step\n\r"); - fprintf (PC_HOST, " (a)mplitude down, (A)mplitude up - 0.5 dB steps\n\r"); - fprintf (PC_HOST, " e(x)it engineering mode\n\r"); -} - -/** - * Process the command to dump the contents of the data logger flash. - */ -void diagReadFlash() -{ - bool_t dataFoundFlag, userStopFlag; - uint8_t i, buffer[DIAG_BYTES_PER_LINE]; - uint32_t address; - - // Set the initial conditions to read the flash. - address = 0x0000; - userStopFlag = false; - - do - { - // Read each block from the flash device. - flashReadBlock (address, buffer, DIAG_BYTES_PER_LINE); - - // This flag will get set if any data byte is not equal to 0xff (erase flash state) - dataFoundFlag = false; - - // Display the address. - fprintf (PC_HOST, "%08lx ", address); - - // Display each byte in the line. - for (i = 0; i < DIAG_BYTES_PER_LINE; ++i) - { - fprintf (PC_HOST, "%02x", buffer[i]); - - // Set this flag if the cell is not erased. - if (buffer[i] != 0xff) - dataFoundFlag = true; - - // Any key will abort the transfer. - if (kbhit(PC_HOST)) - userStopFlag = true; - } // END for - - // at the end of each line. - fprintf (PC_HOST, "\n\r"); - - // Advance to the next block of memory. - address += DIAG_BYTES_PER_LINE; - } while (dataFoundFlag && !userStopFlag); - - // Feedback to let the user know why the transfer stopped. - if (userStopFlag) - fprintf (PC_HOST, "User aborted download!\n\r"); -} - -void diag1PPS() -{ - uint16_t timeStamp, lastTimeStamp; - - lastTimeStamp = 0x0000; - - gpsPowerOn(); - - for (;;) - { - timeStamp = CCP_2; - - if (timeStamp != lastTimeStamp) - { - delay_ms (10); - - timeStamp = CCP_2; - - fprintf (PC_HOST, "%lu %lu\n\r", timeStamp, (timeStamp - lastTimeStamp)); - - lastTimeStamp = timeStamp; - } - } -} - -/** - * Process diagnostic commands through the debug RS-232 port. - */ -void diagPort() -{ - bool_t diagDoneFlag, ledFlag, paFlag, showSettingsFlag; - uint8_t command, amplitude; - uint32_t freqHz; - - // If the input is low, we aren't connected to the RS-232 device so continue to boot. - if (!input(PIN_B6)) - return; - - fprintf (PC_HOST, "Engineering Mode\n\r"); - fprintf (PC_HOST, "Application Built %s %s\n\r", __DATE__, __TIME__); - - // Current state of the status LED. - ledFlag = false; - output_bit (IO_LED, ledFlag); - - // This flag indicates we are ready to leave the diagnostics mode. - diagDoneFlag = false; - - // Current state of the PA. - paFlag = false; - - // Flag that indicate we should show the current carrier frequency. - showSettingsFlag = false; - - // Set the initial carrier frequency and amplitude. - freqHz = 445950000; - amplitude = 0; - - // Wait for the exit command. - while (!diagDoneFlag) - { - // Wait for the user command. - command = fgetc(PC_HOST); - - // Decode and process the key stroke. - switch (command) - { - case 'e': - diagEraseFlash(); - logInit(); - break; - - case 'l': - case 'L': - ledFlag = (ledFlag ? false : true); - output_bit (IO_LED, ledFlag); - break; - - case 'h': - case 'H': - case '?': - diagMenu(); - break; - - case 'r': - diagReadFlash(); - break; - - case 't': - tncHighRate (true); - fprintf (PC_HOST, "Set high rate TNC.\n\r"); - break; - - case 'f': - freqHz -= 1000; - ddsSetFreq (freqHz); - - // Display the new frequency. - showSettingsFlag = true; - break; - - case 'F': - freqHz += 1000; - ddsSetFreq (freqHz); - - // Display the new frequency. - showSettingsFlag = true; - break; - - case 'c': - freqHz -= 25000; - ddsSetFreq (freqHz); - - // Display the new frequency. - showSettingsFlag = true; - break; - - case 'C': - freqHz += 25000; - ddsSetFreq (freqHz); - - // Display the new frequency. - showSettingsFlag = true; - break; - - case 'p': - case 'P': - ddsSetFreq (freqHz); - - paFlag = (paFlag ? false : true); - output_bit (IO_PTT, paFlag); - output_bit (IO_OSK, paFlag); - - if (paFlag) - { - ddsSetMode (DDS_MODE_AFSK); - ddsSetAmplitude (amplitude); - } else - ddsSetMode (DDS_MODE_POWERDOWN); - - break; - - case 'a': - if (amplitude != 200) - { - amplitude += 5; - ddsSetAmplitude (amplitude); - - // Display the new amplitude. - showSettingsFlag = true; - } - break; - - case 'A': - if (amplitude != 0) - { - amplitude -= 5; - ddsSetAmplitude (amplitude); - - // Display the new amplitude. - showSettingsFlag = true; - } - break; - - case 'g': - diag1PPS(); - break; - - case 'x': - diagDoneFlag = true; - break; - - default: - fprintf (PC_HOST, "Invalid command. (H)elp for menu.\n\r"); - break; - } // END switch - - // Display the results of any user requests or commands. - if (showSettingsFlag) - { - showSettingsFlag = false; - - fprintf (PC_HOST, "%03ld.%03ld MHz ", freqHz / 1000000, (freqHz / 1000) % 1000); - fprintf (PC_HOST, "%d.%01ddBc\n\r", amplitude / 10, amplitude % 10); - - } // END if - - } // END while - - // Let the user know we are done with this mode. - fprintf (PC_HOST, "Exit diagnostic mode.\n\r"); - - return; -} - /** @} */ - /** * @defgroup DDS AD9954 DDS (Direct Digital Synthesizer) * @@ -894,89 +388,6 @@ const uint32_t freqTable[256] = 955409113, 955410249, 955411390, 955412535, 955413684, 955414836, 955415989, 955417144 }; -/** - * Initialize the DDS regsiters and RAM. - */ -void ddsInit() -{ - // Setup the SPI port for the DDS interface. - setup_spi( SPI_MASTER | SPI_L_TO_H | SPI_CLK_DIV_4 | SPI_XMIT_L_TO_H ); - - // Set the initial DDS mode. The ddsSetMode function uses this value to make the desired DDS selections. - ddsMode = DDS_MODE_NOT_INITIALIZED; - - // Set the DDS operational mode. - ddsSetMode (DDS_MODE_POWERDOWN); - - // Set the output to full scale. - ddsSetOutputScale (0x3fff); - - // CFR2 (Control Function Register No. 2) - output_low (IO_CS); - spi_write (DDS_AD9954_CFR2); - - spi_write (0x00); // Unused register bits - spi_write (0x00); - spi_write (0x9c); // 19x reference clock multipler, high VCO range, nominal charge pump current - output_high (IO_CS); - - // ARR (Amplitude Ramp Rate) to 15mS for OSK - output_low (IO_CS); - spi_write (DDS_AD9954_ARR); - - spi_write (83); - output_high (IO_CS); - - // Strobe the part so we apply the updates. - output_high (IO_UPDATE); - output_low (IO_UPDATE); -} - -/** - * Set DDS amplitude value in the range 0 to 16383 where 16383 is full scale. This value is a - * linear multiplier and needs to be scale for RF output power in log scale. - * - * @param scale in the range 0 to 16383 - */ -void ddsSetOutputScale (uint16_t scale) -{ - // Set ASF (Amplitude Scale Factor) - output_low (IO_CS); - spi_write (DDS_AD9954_ASF); - - spi_write ((scale >> 8) & 0xff); - spi_write (scale & 0xff); - - output_high (IO_CS); - - // Strobe the DDS to set the amplitude. - output_high (IO_UPDATE); - output_low (IO_UPDATE); -} - -/** - * Set the DDS amplitude in units of dBc of full scale where 1 is 0.1 dB. For example, a value of 30 is 3dBc - * or a value of 85 is 8.5dBc. - * - * @param amplitude in 0.1 dBc of full scale - */ -void ddsSetAmplitude (uint8_t amplitude) -{ - // Range limit based on the lookup table size. - if (amplitude > 200) - return; - - // Set the linear DDS ASF (Amplitude Scale Factor) based on the dB lookup table. - ddsSetOutputScale (DDS_AMP_TO_SCALE[amplitude / 5]); - - // Toggle the DDS output low and then high to force it to ramp to the new output level setting. - output_low (IO_OSK); - delay_ms(25); - - output_high (IO_OSK); - delay_ms(25); -} - /** * Set DDS frequency tuning word. The output frequency is equal to RefClock * (ftw / 2 ^ 32). * @@ -984,20 +395,10 @@ void ddsSetAmplitude (uint8_t amplitude) */ void ddsSetFTW (uint32_t ftw) { - // Set FTW0 (Frequency Tuning Word 0) - output_low (IO_CS); - spi_write (DDS_AD9954_FTW0); - - spi_write ((ftw >> 24) & 0xff); - spi_write ((ftw >> 16) & 0xff); - spi_write ((ftw >> 8) & 0xff); - spi_write (ftw & 0xff); - - output_high (IO_CS); - - // Strobe the DDS to set the frequency. - output_high (IO_UPDATE); - output_low (IO_UPDATE); + static int id; + int x = ftw - freqTable[0]; + putchar (x > 0 ? 0xff : 0x0); +// printf ("%d %d\n", id++, x > 0 ? 1 : 0); } /** @@ -1014,365 +415,32 @@ void ddsSetFreq(uint32_t freq) // To avoid rounding errors with floating point math, we do a long multiply on the data. ftw = freq * DDS_MULT[0]; - for (i = 0; i < DDS_FREQ_TO_FTW_DIGITS - 1; ++i) - ftw += (freq * DDS_MULT[i+1]) / DDS_DIVISOR[i]; - - ddsSetFTW (ftw); -} - -/** - * Set DDS frequency tuning word for the FSK 0 and 1 values. The output frequency is equal - * to RefClock * (ftw / 2 ^ 32). - * - * @param ftw0 frequency tuning word for the FSK 0 value - * @param ftw1 frequency tuning word for the FSK 1 value - */ -void ddsSetFSKFreq (uint32_t ftw0, uint32_t ftw1) -{ - // Set FTW0 (Frequency Tuning Word 0) - output_low (IO_CS); - spi_write (DDS_AD9954_FTW0); - - spi_write ((ftw0 >> 24) & 0xff); - spi_write ((ftw0 >> 16) & 0xff); - spi_write ((ftw0 >> 8) & 0xff); - spi_write (ftw0 & 0xff); - - output_high (IO_CS); - - // Set FTW0 (Frequency Tuning Word 1) - output_low (IO_CS); - spi_write (DDS_AD9954_FTW1); - - spi_write ((ftw1 >> 24) & 0xff); - spi_write ((ftw1 >> 16) & 0xff); - spi_write ((ftw1 >> 8) & 0xff); - spi_write (ftw1 & 0xff); - - output_high (IO_CS); - - // Strobe the DDS to set the frequency. - output_high (IO_UPDATE); - output_low (IO_UPDATE); -} - -/** - * Set the DDS to run in A-FSK, FSK, or PSK31 mode - * - * @param mode DDS_MODE_APRS, DDS_MODE_PSK31, or DDS_MODE_HF_APRS constant - */ -void ddsSetMode (DDS_MODE mode) -{ - // Save the current mode. - ddsMode = mode; - - switch (mode) - { - case DDS_MODE_POWERDOWN: - // CFR1 (Control Function Register No. 1) - output_low (IO_CS); - spi_write (DDS_AD9954_CFR1); - - spi_write (0x00); - spi_write (0x00); - spi_write (0x00); - spi_write (0xf0); // Power down all subsystems. - output_high (IO_CS); - break; - - case DDS_MODE_AFSK: - // CFR1 (Control Function Register No. 1) - output_low (IO_CS); - spi_write (DDS_AD9954_CFR1); - - spi_write (0x03); // OSK Enable and Auto OSK keying - spi_write (0x00); - spi_write (0x00); - spi_write (0x40); // Power down comparator circuit - output_high (IO_CS); - break; - - case DDS_MODE_FSK: - // CFR1 (Control Function Register No. 1) - output_low (IO_CS); - spi_write (DDS_AD9954_CFR1); - - spi_write (0x03); // Clear RAM Enable, OSK Enable, Auto OSK keying - spi_write (0x00); - spi_write (0x00); - spi_write (0x40); // Power down comparator circuit - output_high (IO_CS); - - // NOTE: The sweep rate requires 1/4 of a bit time (26uS) to transition. - // 6KHz delta = 70641 counts = (6KHz / 364.8MHz) * 2 ^ 32 - // SYNC_CLK = 91.2MHz 1/91.2MHz * 70641 * 1/29 = 26.7uS - - // NLSCW (Negative Linear Sweep Control Word) - output_low (IO_CS); - spi_write (DDS_AD9954_NLSCW); - - spi_write (1); // Falling sweep ramp rate word - spi_write (0x00); // Delta frequency tuning word - spi_write (0x00); - spi_write (0x00); - spi_write (250); - output_high (IO_CS); - - // PLSCW (Positive Linear Sweep Control Word) - output_low (IO_CS); - spi_write (DDS_AD9954_PLSCW); - - spi_write (1); // Rising sweep ramp rate word - spi_write (0x00); // Delta frequency tuning word - spi_write (0x00); - spi_write (0x00); - spi_write (250); - output_high (IO_CS); - break; - } // END switch - - // Strobe the DDS to change the mode. - output_high (IO_UPDATE); - output_low (IO_UPDATE); -} - -/** @} */ - -/** - * @defgroup flash Flash Manager - * - * Functions to control the ST MP25P80 serial flash device. - * - * @{ - */ - -/// Flash Chip Select - Port B3 -#define FLASH_CS PIN_B3 - -/// Flash Clock - Port B5 -#define FLASH_CLK PIN_B5 - -/// Flash Data Input - Port B4 -#define FLASH_D PIN_B4 - -/// Flash Data Output - Port B2 -#define FLASH_Q PIN_B2 - -/** - * Determine if a flash write or erase operation is currently in progress. - * - * @return true if write/erase in progress - */ -bool_t flashIsWriteInProgress() -{ - uint8_t status; - - output_low (FLASH_CS); - - // Read Status Register (RDSR) flash command. - flashSendByte (0x05); - - status = flashGetByte(); - - output_high (FLASH_CS); - - return (((status & 0x01) == 0x01) ? true : false); -} - -/** - * Read a block of memory from the flash device. - * - * @param address of desired location in the range 0x00000 to 0xFFFFF (1MB) - * @param block pointer to locate of data block - * @param length number of bytes to read - */ -void flashReadBlock(uint32_t address, uint8_t *block, uint16_t length) -{ - uint16_t i; - - output_low (FLASH_CS); - - // Read Data Byte(s) (READ) flash command. - flashSendByte (0x03); - flashSendAddress (address); - - for (i = 0; i < length; ++i) - *block++ = flashGetByte(); - - output_high (FLASH_CS); -} - -/** - * Write a block of memory to the flash device. - * - * @param address of desired location in the range 0x00000 to 0xFFFFF (1MB) - * @param block pointer data block to write - * @param length number of bytes to write - */ -void flashWriteBlock(uint32_t address, uint8_t *block, uint8_t length) -{ - uint8_t i; - - output_low (FLASH_CS); - // Write Enable (WREN) flash command. - flashSendByte (0x06); - output_high (FLASH_CS); - - output_low (FLASH_CS); - // Page Program (PP) flash command. - flashSendByte (0x02); - flashSendAddress (address); - - for (i = 0; i < length; ++i) - { - // Send each byte in the data block. - flashSendByte (*block++); - - // Track the address in the flash device. - ++address; - - // If we cross a page boundary (a page is 256 bytes) we need to stop and send the address again. - if ((address & 0xff) == 0x00) - { - output_high (FLASH_CS); - - // Write this block of data. - while (flashIsWriteInProgress()); - - output_low (FLASH_CS); - // Write Enable (WREN) flash command. - flashSendByte (0x06); - output_high (FLASH_CS); - - output_low (FLASH_CS); - // Page Program (PP) flash command. - flashSendByte (0x02); - flashSendAddress (address); - } // END if - } // END for - - output_high (FLASH_CS); - - // Wait for the final write operation to complete. - while (flashIsWriteInProgress()); -} - -/** - * Erase the entire flash device (all locations set to 0xff). - */ -void flashErase() -{ - output_low (FLASH_CS); - // Write Enable (WREN) flash command. - flashSendByte (0x06); - output_high (FLASH_CS); - - output_low (FLASH_CS); - // Bulk Erase (BE) flash command. - flashSendByte (0xc7); - output_high (FLASH_CS); - - while (flashIsWriteInProgress()); -} - -/** - * Read a single byte from the flash device through the serial interface. This function - * only controls the clock line. The chip select must be configured before calling - * this function. - * - * @return byte read from device - */ -uint8_t flashGetByte() -{ - uint8_t i, value; - - value = 0; - - // Bit bang the 8-bits. - for (i = 0; i < 8; ++i) - { - // Data is ready on the rising edge of the clock. - output_high (FLASH_CLK); - - // MSB is first, so shift left. - value = value << 1; - - if (input (FLASH_Q)) - value = value | 0x01; + for (i = 0; i < DDS_FREQ_TO_FTW_DIGITS - 1; ++i) + ftw += (freq * DDS_MULT[i+1]) / DDS_DIVISOR[i]; - output_low (FLASH_CLK); - } // END for - - return value; -} - -/** - * Initialize the flash memory subsystem. - */ -void flashInit() -{ - // I/O lines to control flash. - output_high (FLASH_CS); - output_low (FLASH_CLK); - output_low (FLASH_D); + ddsSetFTW (ftw); } /** - * Write a single byte to the flash device through the serial interface. This function - * only controls the clock line. The chip select must be configured before calling - * this function. + * Set DDS frequency tuning word for the FSK 0 and 1 values. The output frequency is equal + * to RefClock * (ftw / 2 ^ 32). * - * @param value byte to write to device + * @param ftw0 frequency tuning word for the FSK 0 value + * @param ftw1 frequency tuning word for the FSK 1 value */ -void flashSendByte(uint8_t value) +void ddsSetFSKFreq (uint32_t ftw0, uint32_t ftw1) { - uint8_t i; - - // Bit bang the 8-bits. - for (i = 0; i < 8; ++i) - { - // Drive the data input pin. - if ((value & 0x80) == 0x80) - output_high (FLASH_D); - else - output_low (FLASH_D); - - // MSB is first, so shift leeft. - value = value << 1; - - // Data is accepted on the rising edge of the clock. - output_high (FLASH_CLK); - output_low (FLASH_CLK); - } // END for +// printf ("ftw0 %d ftw1 %d\n", ftw0, ftw1); } -/** - * Write the 24-bit address to the flash device through the serial interface. This function - * only controls the clock line. The chip select must be configured before calling - * this function. +/** + * Set the DDS to run in A-FSK, FSK, or PSK31 mode * - * @param address 24-bit flash device address + * @param mode DDS_MODE_APRS, DDS_MODE_PSK31, or DDS_MODE_HF_APRS constant */ -void flashSendAddress(uint32_t address) +void ddsSetMode (DDS_MODE mode) { - uint8_t i; - - // Bit bang the 24-bits. - for (i = 0; i < 24; ++i) - { - // Drive the data input pin. - if ((address & 0x800000) == 0x800000) - output_high (FLASH_D); - else - output_low (FLASH_D); - - // MSB is first, so shift left. - address = address << 1; - - // Data is accepted on the rising edge of the clock. - output_high (FLASH_CLK); - output_low (FLASH_CLK); - } // END for +// printf ("mode %d\n", mode); } /** @} */ @@ -1389,7 +457,7 @@ void flashSendAddress(uint32_t address) #define GPS_BUFFER_SIZE 50 /// GPS parse engine state machine values. -enum GPS_PARSE_STATE_MACHINE +typedef enum { /// 1st start character '@' GPS_START1, @@ -1414,7 +482,7 @@ enum GPS_PARSE_STATE_MACHINE /// End of message - Line Feed GPS_EOMLF -}; +} GPS_PARSE_STATE_MACHINE; /// Index into gpsBuffer used to store message data. uint8_t gpsIndex; @@ -1480,8 +548,8 @@ void gpsInit() memset (&gpsPosition, 0, sizeof(GPSPOSITION_STRUCT)); // Setup the timers used to measure the 1-PPS time period. - setup_timer_3(T3_INTERNAL | T3_DIV_BY_1); - setup_ccp2 (CCP_CAPTURE_RE | CCP_USE_TIMER3); +// setup_timer_3(T3_INTERNAL | T3_DIV_BY_1); +// setup_ccp2 (CCP_CAPTURE_RE | CCP_USE_TIMER3); } /** @@ -1492,6 +560,7 @@ void gpsInit() */ bool_t gpsIsReady() { + return true; if (gpsPosition.updateFlag) { gpsPosition.updateFlag = false; @@ -1538,12 +607,12 @@ bool_t gpsSetup() while (++retryCount < 10) { // Read the serial FIFO and process the GPS messages. - gpsUpdate(); +// gpsUpdate(); // If a GPS data set is available, then GPS is operational. if (gpsIsReady()) { - timeSetDutyCycle (TIME_DUTYCYCLE_10); +// timeSetDutyCycle (TIME_DUTYCYCLE_10); return true; } @@ -1597,12 +666,8 @@ void gpsParsePositionMessage() void gpsPowerOn() { // 3.0 VDC LDO control line. - output_high (IO_GPS_PWR); +// output_high (IO_GPS_PWR); - // Enable the UART and the transmit line. -#asm - bsf 0xFAB.7 -#endasm } /** @@ -1610,394 +675,13 @@ void gpsPowerOn() */ void gpsPowerOff() { - // Disable the UART and the transmit line. -#asm - bcf 0xFAB.7 -#endasm - // 3.0 VDC LDO control line. - output_low (IO_GPS_PWR); -} - -/** - * Read the serial FIFO and process complete GPS messages. - */ -void gpsUpdate() -{ - uint8_t value; - - // This state machine handles each characters as it is read from the GPS serial port. - // We are looking for the GPS mesage @@Hb ... C - while (serialHasData()) - { - // Get the character value. - value = serialRead(); - - // Process based on the state machine. - switch (gpsParseState) - { - case GPS_START1: - if (value == '@') - gpsParseState = GPS_START2; - break; - - case GPS_START2: - if (value == '@') - gpsParseState = GPS_COMMAND1; - else - gpsParseState = GPS_START1; - break; - - case GPS_COMMAND1: - if (value == 'H') - gpsParseState = GPS_COMMAND2; - else - gpsParseState = GPS_START1; - break; - - case GPS_COMMAND2: - if (value == 'b') - { - gpsParseState = GPS_READMESSAGE; - gpsIndex = 0; - gpsChecksum = 0; - gpsChecksum ^= 'H'; - gpsChecksum ^= 'b'; - } else - gpsParseState = GPS_START1; - break; - - case GPS_READMESSAGE: - gpsChecksum ^= value; - gpsBuffer[gpsIndex++] = value; - - if (gpsIndex == 47) - gpsParseState = GPS_CHECKSUMMESSAGE; - - break; - - case GPS_CHECKSUMMESSAGE: - if (gpsChecksum == value) - gpsParseState = GPS_EOMCR; - else - gpsParseState = GPS_START1; - break; - - case GPS_EOMCR: - if (value == 13) - gpsParseState = GPS_EOMLF; - else - gpsParseState = GPS_START1; - break; - - case GPS_EOMLF: - // Once we have the last character, convert the binary message to something usable. - if (value == 10) - gpsParsePositionMessage(); - - gpsParseState = GPS_START1; - break; - } // END switch - } // END while -} - -/** @} */ - - -/** - * @defgroup log Flight Data Recorder - * - * Functions to manage and control the flight data recorder - * - * @{ - */ - -/// Number of bytes to buffer before writing to flash memory. -#define LOG_WRITE_BUFFER_SIZE 40 - -/// Last used address in flash memory. -uint32_t logAddress; - -/// Temporary buffer that holds data before it is written to flash device. -uint8_t logBuffer[LOG_WRITE_BUFFER_SIZE]; - -/// Current index into log buffer. -uint8_t logIndex; - -/** - * Last used address in flash memory. This location is where the next log data will - * be written. - * - * @return 24-bit flash memory address - */ -uint32_t logGetAddress() -{ - return logAddress; -} - -/** - * Write the contents of the temporary log buffer to the flash device. If the buffer - * is empty, nothing is done. - */ -void logFlush() -{ - // We only need to write if there is data. - if (logIndex != 0) - { - flashWriteBlock (logAddress, logBuffer, logIndex); - logAddress += logIndex; - logIndex = 0; - } // END if -} - -/** - * Prepare the flight data recorder for logging. - */ -void logInit() -{ - uint8_t buffer[8]; - bool_t endFound; - - fprintf (PC_HOST, "Searching for end of flash log..."); - - logAddress = 0x0000; - endFound = false; - - // Read each logged data block from flash to determine how long it is. - do - { - // Read the data log entry type. - flashReadBlock (logAddress, buffer, 1); - - // Based on the log entry type, we'll skip over the data contained in the entry. - switch (buffer[0]) - { - case LOG_BOOTED: - logAddress += 7; - break; - - case LOG_COORD: - logAddress += 26; - break; - - case LOG_TEMPERATURE: - logAddress += 3; - break; - - case LOG_VOLTAGE: - logAddress += 5; - break; - - case 0xff: - endFound = true; - break; - - default: - ++logAddress; - } // END switch - } while (logAddress < 0x100000 && !endFound); - - fprintf (PC_HOST, "done. Log contains %ld bytes.\n\r", logAddress); - - logIndex = 0; -} - -/** - * Start a entry in the data log. - * - * @param type of log entry, i.e. LOG_BOOTED, LOG_COORD, etc. - */ -void logType (LOG_TYPE type) -{ - // Only add the new entry if there is space. - if (logAddress >= 0x100000) - return; - - // Write the old entry first. - logFlush(); - - // Save the type and set the log buffer pointer. - logBuffer[0] = type; - logIndex = 1; -} - -/** - * Save an unsigned, 8-bit value in the log. - * - * @param value unsigned, 8-bit value - */ -void logUint8 (uint8_t value) -{ - logBuffer[logIndex++] = value; -} - -/** - * Save a signed, 16-bit value in the log. - * - * @param value signed, 16-bit value - */ -void logInt16 (int16_t value) -{ - logBuffer[logIndex++] = (value >> 8) & 0xff; - logBuffer[logIndex++] = value & 0xff; -} - -/** - * Save an unsigned, 16-bit value in the log. - * - * @param value unsigned, 16-bit value - */ -void logUint16 (uint16_t value) -{ - logBuffer[logIndex++] = (value >> 8) & 0xff; - logBuffer[logIndex++] = value & 0xff; -} - -/** - * Save a signed, 32-bit value in the log. - * - * @param value signed, 32-bit value - */ -void logInt32 (int32_t value) -{ - logBuffer[logIndex++] = (value >> 24) & 0xff; - logBuffer[logIndex++] = (value >> 16) & 0xff; - logBuffer[logIndex++] = (value >> 8) & 0xff; - logBuffer[logIndex++] = value & 0xff; -} - -/** @} */ - -/** - * @defgroup LM92 LM92 temperature sensor - * - * Read and control the National Semiconductor LM92 I2C temperature sensor - * - * @{ - */ - -/** - * Read the LM92 temperature value in 0.1 degrees F. - * - * @return 0.1 degrees F - */ -int16_t lm92GetTemp() -{ - int16_t value; - int32_t temp; - - // Set the SDA and SCL to input pins to control the LM92. - set_tris_c (0x9a); - - // Read the temperature register value. - i2c_start(); - i2c_write(0x97); - value = ((int16_t) i2c_read() << 8); - value = value | ((int16_t) i2c_read() & 0x00f8); - i2c_stop(); - - // Set the SDA and SCL back to outputs for use with the AD9954 because we share common clock pins. - set_tris_c (0x82); - - // LM92 register 0.0625degC/bit 9 10 9 - // ------------- * -------------- * - * -- = -- + 320 - // 8 5 64 - - // Convert to degrees F. - temp = (int32_t) value; - temp = ((temp * 9l) / 64l) + 320; - - return (int16_t) temp; +// output_low (IO_GPS_PWR); } /** @} */ -/** - * @defgroup serial Serial Port FIFO - * - * FIFO for the built-in serial port. - * - * @{ - */ - -/// Size of serial port FIFO in bytes. It must be a power of 2, i.e. 2, 4, 8, 16, etc. -#define SERIAL_BUFFER_SIZE 64 - -/// Mask to wrap around at end of circular buffer. (SERIAL_BUFFER_SIZE - 1) -#define SERIAL_BUFFER_MASK 0x3f - -/// Index to the next free location in the buffer. -uint8_t serialHead; - -/// Index to the next oldest data in the buffer. -uint8_t serialTail; - -/// Circular buffer (FIFO) to hold serial data. -uint8_t serialBuffer[SERIAL_BUFFER_SIZE]; - -/** - * Determine if the FIFO contains data. - * - * @return true if data present; otherwise false - */ -bool_t serialHasData() -{ - if (serialHead == serialTail) - return false; - - return true; -} - -/** - * Initialize the serial processor. - */ -void serialInit() -{ - serialHead = 0; - serialTail = 0; -} - -/** - * Get the oldest character from the FIFO. - * - * @return oldest character; 0 if FIFO is empty - */ -uint8_t serialRead() -{ - uint8_t value; - - // Make sure we have something to return. - if (serialHead == serialTail) - return 0; - - // Save the value. - value = serialBuffer[serialTail]; - - // Update the pointer. - serialTail = (serialTail + 1) & SERIAL_BUFFER_MASK; - - return value; -} - -/** - * Read and store any characters in the PIC serial port in a FIFO. - */ -void serialUpdate() -{ - // If there isn't a character in the PIC buffer, just leave. - while (kbhit()) - { - // Save the value in the FIFO. - serialBuffer[serialHead] = getc(); - - // Move the pointer to the next open space. - serialHead = (serialHead + 1) & SERIAL_BUFFER_MASK; - } -} - -/** @} */ - /** * @defgroup sys System Library Functions * @@ -2035,65 +719,6 @@ uint16_t sysCRC16(uint8_t *buffer, uint8_t length, uint16_t crc) return crc ^ 0xffff; } -/** - * Initialize the system library and global resources. - */ -void sysInit() -{ - gpsPowerOff (); - output_high (IO_LED); - - output_high (IO_CS); - output_low (IO_PS1); - output_low (IO_PS0); - output_low (IO_OSK); - output_low (IO_UPDATE); - output_low (IO_PTT); - output_low (IO_GPS_TXD); - - // Configure the port direction (input/output). - set_tris_a (0xc3); - set_tris_b (0x44); - set_tris_c (0x82); - - // Display a startup message during boot. - fprintf (PC_HOST, "System booted.\n\r"); -} - -/** - * Log the current GPS position. - */ -void sysLogGPSData() -{ - // Log the data. - logType (LOG_COORD); - logUint8 (gpsPosition.hours); - logUint8 (gpsPosition.minutes); - logUint8 (gpsPosition.seconds); - logInt32 (gpsPosition.latitude); - logInt32 (gpsPosition.longitude); - logInt32 (gpsPosition.altitudeCM); - - logUint16 (gpsPosition.vSpeed); - logUint16 (gpsPosition.hSpeed); - logUint16 (gpsPosition.heading); - - logUint16 (gpsPosition.status); - - logUint8 ((uint8_t) (gpsPosition.dop & 0xff)); - logUint8 ((uint8_t) ((gpsPosition.visibleSats << 4) | gpsPosition.trackedSats)); -} - -/** - * Log the ADC values of the bus and reference voltage values. - */ -void sysLogVoltage() -{ - logType (LOG_VOLTAGE); - logUint16 (adcRawBusVolt()); - logUint16 (adcRawRefVolt()); -} - /** @} */ /** @@ -2166,11 +791,10 @@ void timeInit() { timeTicks = 0; timeInterruptCount = 0; - time100mS = 0; +// time100mS = 0; timeSeconds = 0; timeMinutes = 0; timeHours = 0; - timeDutyCycle = TIME_DUTYCYCLE_70; timeCompare = TIME_RATE; timeUpdateFlag = false; timeNCO = 0x00; @@ -2178,12 +802,6 @@ void timeInit() timeNCOFreq = 0x2000; tncDataMode = TNC_MODE_STANDBY; timeRunFlag = false; - - // Configure CCP1 to interrupt at 1mS for PSK31 or 833uS for 1200 baud APRS - CCP_1 = TIME_RATE; - set_timer1(timeCompare); - setup_ccp1( CCP_COMPARE_INT ); - setup_timer_1( T1_INTERNAL | T1_DIV_BY_4 ); } /** @@ -2202,16 +820,6 @@ bool_t timeIsUpdate() return false; } -/** - * Set the blink duty cycle of the heartbeat LED. The LED blinks at a 1Hz rate. - * - * @param dutyCycle TIME_DUTYCYCLE_xx constant - */ -void timeSetDutyCycle (uint8_t dutyCycle) -{ - timeDutyCycle = dutyCycle; -} - /** * Set a flag to indicate the flight time should run. This flag is typically set when the payload * lifts off. @@ -2221,7 +829,6 @@ void timeSetRunFlag() timeRunFlag = true; } -#INT_CCP1 /** * Timer interrupt handler called every 104uS (9600 times/second). */ @@ -2229,7 +836,7 @@ void timeUpdate() { // Setup the next interrupt for the operational mode. timeCompare += TIME_RATE; - CCP_1 = timeCompare; +// CCP_1 = timeCompare; switch (tncDataMode) { @@ -2252,46 +859,6 @@ void timeUpdate() tnc9600TimerTick(); break; } // END switch - - // Read the GPS serial port and save any incoming characters. - serialUpdate(); - - // Count the number of milliseconds required for the tenth second counter. - if (++timeInterruptCount == 960) - { - timeInterruptCount = 0; - - // This timer just ticks every 100mS and is used for general timing. - ++timeTicks; - - // Roll the counter over every second. - if (++time100mS == 10) - { - time100mS = 0; - - // We set this flag true every second. - timeUpdateFlag = true; - - // Maintain a Real Time Clock. - if (timeRunFlag) - if (++timeSeconds == 60) - { - timeSeconds = 0; - - if (++timeMinutes == 60) - { - timeMinutes = 0; - ++timeHours; - } // END if timeMinutes - } // END if timeSeconds - } // END if time100mS - - // Flash the status LED at timeDutyCycle % per second. We use the duty cycle for mode feedback. - if (time100mS >= timeDutyCycle) - output_low (IO_LED); - else - output_high (IO_LED); - } // END if } /** @} */ @@ -2311,7 +878,7 @@ void timeUpdate() #define TNC_BUFFER_SIZE 80 /// States that define the current mode of the 1200 bps (A-FSK) state machine. -enum TNC_TX_1200BPS_STATE +typedef enum { /// Stand by state ready to accept new message. TNC_TX_READY, @@ -2327,10 +894,10 @@ enum TNC_TX_1200BPS_STATE /// Transmit the end flag sequence. TNC_TX_END -}; +} TNC_TX_1200BPS_STATE; /// Enumeration of the messages we can transmit. -enum TNC_MESSAGE_TYPE +typedef enum { /// Startup message that contains software version information. TNC_BOOT_MESSAGE, @@ -2343,13 +910,13 @@ enum TNC_MESSAGE_TYPE /// Message that contains GPS NMEA-0183 $GPRMC message. TNC_RMC -}; +} TNC_MESSAGE_TYPE; /// AX.25 compliant packet header that contains destination, station call sign, and path. /// 0x76 for SSID-11, 0x78 for SSID-12 uint8_t TNC_AX25_HEADER[30] = { 'A' << 1, 'P' << 1, 'R' << 1, 'S' << 1, ' ' << 1, ' ' << 1, 0x60, \ - 'K' << 1, 'D' << 1, '7' << 1, 'L' << 1, 'M' << 1, 'O' << 1, 0x76, \ + 'K' << 1, 'D' << 1, '7' << 1, 'S' << 1, 'Q' << 1, 'G' << 1, 0x76, \ 'G' << 1, 'A' << 1, 'T' << 1, 'E' << 1, ' ' << 1, ' ' << 1, 0x60, \ 'W' << 1, 'I' << 1, 'D' << 1, 'E' << 1, '3' << 1, ' ' << 1, 0x67, \ 0x03, 0xf0 }; @@ -2631,8 +1198,8 @@ void tnc1200TimerTick() tncDataMode = TNC_MODE_STANDBY; // Key off the DDS. - output_low (IO_OSK); - output_low (IO_PTT); +// output_low (IO_OSK); +// output_low (IO_PTT); ddsSetMode (DDS_MODE_POWERDOWN); return; @@ -2665,6 +1232,19 @@ void tncTxByte (uint8_t character) ++tncLength; } +static void +tncPrintf(char *fmt, ...) +{ + va_list ap; + int c; + + va_start(ap, fmt); + c = vsprintf(tncBufferPnt, fmt, ap); + va_end(ap); + tncBufferPnt += c; + tncLength += c; +} + /** * Generate the GPS NMEA standard UTC time stamp. Data is written through the tncTxByte * callback function. @@ -2672,7 +1252,7 @@ void tncTxByte (uint8_t character) void tncNMEATime() { // UTC of position fix. - printf (tncTxByte, "%02d%02d%02d,", gpsPosition.hours, gpsPosition.minutes, gpsPosition.seconds); + tncPrintf ("%02d%02d%02d,", gpsPosition.hours, gpsPosition.minutes, gpsPosition.seconds); } /** @@ -2697,7 +1277,7 @@ void tncNMEAFix() } coordMin = (coord % 3600000) / 6; - printf (tncTxByte, "%02ld%02ld.%04ld,%c,", (uint32_t) (coord / 3600000), (uint32_t) (coordMin / 10000), (uint32_t) (coordMin % 10000), dirChar); + tncPrintf ("%02ld%02ld.%04ld,%c,", (uint32_t) (coord / 3600000), (uint32_t) (coordMin / 10000), (uint32_t) (coordMin % 10000), dirChar); // Longitude value. @@ -2711,8 +1291,8 @@ void tncNMEAFix() } coordMin = (coord % 3600000) / 6; - printf (tncTxByte, "%03ld%02ld.%04ld,%c,", (uint32_t) (coord / 3600000), (uint32_t) (coordMin / 10000), (uint32_t) (coordMin % 10000), dirChar); - + tncPrintf ("%03ld%02ld.%04ld,%c,", (uint32_t) (coord / 3600000), (uint32_t) (coordMin / 10000), (uint32_t) (coordMin % 10000), dirChar); + } /** @@ -2722,7 +1302,7 @@ void tncNMEAFix() void tncGPGGAPacket() { // Generate the GPGGA message. - printf (tncTxByte, "$GPGGA,"); + tncPrintf ("$GPGGA,"); // Standard NMEA time. tncNMEATime(); @@ -2732,21 +1312,21 @@ void tncGPGGAPacket() // GPS status where 0: not available, 1: available if (gpsGetFixType() != GPS_NO_FIX) - printf (tncTxByte, "1,"); + tncPrintf ("1,"); else - printf (tncTxByte, "0,"); + tncPrintf ("0,"); // Number of visible birds. - printf (tncTxByte, "%02d,", gpsPosition.trackedSats); + tncPrintf ("%02d,", gpsPosition.trackedSats); // DOP - printf (tncTxByte, "%ld.%01ld,", gpsPosition.dop / 10, gpsPosition.dop % 10); + tncPrintf ("%ld.%01ld,", gpsPosition.dop / 10, gpsPosition.dop % 10); // Altitude in meters. - printf (tncTxByte, "%ld.%02ld,M,,M,,", (int32_t) (gpsPosition.altitudeCM / 100l), (int32_t) (gpsPosition.altitudeCM % 100)); + tncPrintf ("%ld.%02ld,M,,M,,", (int32_t) (gpsPosition.altitudeCM / 100l), (int32_t) (gpsPosition.altitudeCM % 100)); // Checksum, we add 1 to skip over the $ character. - printf (tncTxByte, "*%02X", gpsNMEAChecksum(tncBuffer + 1, tncLength - 1)); + tncPrintf ("*%02X", gpsNMEAChecksum(tncBuffer + 1, tncLength - 1)); } /** @@ -2758,29 +1338,29 @@ void tncGPRMCPacket() uint32_t temp; // Generate the GPRMC message. - printf (tncTxByte, "$GPRMC,"); + tncPrintf ("$GPRMC,"); // Standard NMEA time. tncNMEATime(); // GPS status. if (gpsGetFixType() != GPS_NO_FIX) - printf (tncTxByte, "A,"); + tncPrintf ("A,"); else - printf (tncTxByte, "V,"); + tncPrintf ("V,"); // Standard NMEA-0183 latitude/longitude. tncNMEAFix(); // Speed knots and heading. temp = (int32_t) gpsPosition.hSpeed * 75000 / 385826; - printf (tncTxByte, "%ld.%ld,%ld.%ld,", (int16_t) (temp / 10), (int16_t) (temp % 10), gpsPosition.heading / 10, gpsPosition.heading % 10); + tncPrintf ("%ld.%ld,%ld.%ld,", (int16_t) (temp / 10), (int16_t) (temp % 10), gpsPosition.heading / 10, gpsPosition.heading % 10); // Date - printf (tncTxByte, "%02d%02d%02ld,,", gpsPosition.day, gpsPosition.month, gpsPosition.year % 100); + tncPrintf ("%02d%02d%02ld,,", gpsPosition.day, gpsPosition.month, gpsPosition.year % 100); // Checksum, skip over the $ character. - printf (tncTxByte, "*%02X", gpsNMEAChecksum(tncBuffer + 1, tncLength - 1)); + tncPrintf ("*%02X", gpsNMEAChecksum(tncBuffer + 1, tncLength - 1)); } /** @@ -2792,49 +1372,49 @@ void tncStatusPacket(int16_t temperature) uint16_t voltage; // Plain text telemetry. - printf (tncTxByte, ">ANSR "); + tncPrintf (">ANSR "); // Display the flight time. - printf (tncTxByte, "%02U:%02U:%02U ", timeHours, timeMinutes, timeSeconds); + tncPrintf ("%02U:%02U:%02U ", timeHours, timeMinutes, timeSeconds); // Altitude in feet. - printf (tncTxByte, "%ld' ", gpsPosition.altitudeFeet); + tncPrintf ("%ld' ", gpsPosition.altitudeFeet); // Peak altitude in feet. - printf (tncTxByte, "%ld'pk ", gpsGetPeakAltitude()); + tncPrintf ("%ld'pk ", gpsGetPeakAltitude()); // GPS hdop or pdop - printf (tncTxByte, "%lu.%lu", gpsPosition.dop / 10, gpsPosition.dop % 10); + tncPrintf ("%lu.%lu", gpsPosition.dop / 10, gpsPosition.dop % 10); // The text 'pdop' for a 3D fix, 'hdop' for a 2D fix, and 'dop' for no fix. switch (gpsGetFixType()) { case GPS_NO_FIX: - printf (tncTxByte, "dop "); + tncPrintf ("dop "); break; case GPS_2D_FIX: - printf (tncTxByte, "hdop "); + tncPrintf ("hdop "); break; case GPS_3D_FIX: - printf (tncTxByte, "pdop "); + tncPrintf ("pdop "); break; } // END switch // Number of satellites in the solution. - printf (tncTxByte, "%utrk ", gpsPosition.trackedSats); + tncPrintf ("%utrk ", gpsPosition.trackedSats); // Display main bus voltage. - voltage = adcGetMainBusVolt(); - printf (tncTxByte, "%lu.%02luvdc ", voltage / 100, voltage % 100); +// voltage = adcGetMainBusVolt(); +// tncPrintf ("%lu.%02luvdc ", voltage / 100, voltage % 100); // Display internal temperature. - printf (tncTxByte, "%ld.%01ldF ", temperature / 10, abs(temperature % 10)); +// tncPrintf ("%ld.%01ldF ", temperature / 10, abs(temperature % 10)); // Print web address link. - printf (tncTxByte, "www.kd7lmo.net"); + tncPrintf ("www.altusmetrum.org"); } /** @@ -2852,16 +1432,6 @@ void tncTxPacket(TNC_DATA_MODE dataMode) if (tncMode != TNC_TX_READY) return; - // Log the battery and reference voltage before we start the RF chain. - sysLogVoltage(); - - // We need to read the temperature sensor before we setup the DDS since they share a common clock pin. - temperature = lm92GetTemp(); - - // Log the system temperature every time we transmit a packet. - logType (LOG_TEMPERATURE); - logInt16 (temperature); - // Configure the DDS for the desired operational. tncSetMode (dataMode); @@ -2875,7 +1445,7 @@ void tncTxPacket(TNC_DATA_MODE dataMode) switch (tncPacketType) { case TNC_BOOT_MESSAGE: - printf (tncTxByte, ">ANSR Pico Beacon - V3.05"); + tncPrintf (">MegaMetrum v1.0 Beacon"); // Select the next packet we will generate. tncPacketType = TNC_STATUS; @@ -2904,7 +1474,7 @@ void tncTxPacket(TNC_DATA_MODE dataMode) } // Add the end of message character. - printf (tncTxByte, "\015"); + tncPrintf ("\015"); // Calculate the CRC for the header and message. crc = sysCRC16(TNC_AX25_HEADER, sizeof(TNC_AX25_HEADER), 0xffff); @@ -2925,20 +1495,23 @@ void tncTxPacket(TNC_DATA_MODE dataMode) tncMode = TNC_TX_SYNC; // Turn on the PA chain. - output_high (IO_PTT); +// output_high (IO_PTT); // Wait for the PA chain to power up. - delay_ms (10); +// delay_ms (10); // Key the DDS. - output_high (IO_OSK); +// output_high (IO_OSK); // Log the battery and reference voltage just after we key the transmitter. - sysLogVoltage(); +// sysLogVoltage(); + while (tncMode != TNC_TX_READY) + timeUpdate(); } /** @} */ +#if 0 uint32_t counter; uint8_t bitIndex; @@ -2959,7 +1532,7 @@ void test() { counter += 0x10622d; - CCP_1 = (uint16_t) ((counter >> 16) & 0xffff); +// CCP_1 = (uint16_t) ((counter >> 16) & 0xffff); if ((value & 0x80) == 0x80) setup_ccp1 (CCP_COMPARE_SET_ON_MATCH); @@ -2979,68 +1552,37 @@ void test() } else value = value << 1; } +#endif // This is where we go after reset. -void main() +int main(int argc, char **argv) { uint8_t i, utcSeconds, lockLostCounter; -test(); +//test(); // Configure the basic systems. - sysInit(); +// sysInit(); // Wait for the power converter chains to stabilize. - delay_ms (100); +// delay_ms (100); // Setup the subsystems. - adcInit(); - flashInit(); +// adcInit(); +// flashInit(); gpsInit(); - logInit(); - timeInit(); - serialInit(); +// logInit(); +// timeInit(); +// serialInit(); tncInit(); // Program the DDS. - ddsInit(); - - // Turn off the LED after everything is configured. - output_low (IO_LED); - - // Check for the diagnostics plug, otherwise we'll continue to boot. - diagPort(); - - // Setup our interrupts. - enable_interrupts(GLOBAL); - enable_interrupts(INT_CCP1); - - // Turn on the GPS engine. - gpsPowerOn(); - - // Allow the GPS engine to boot. - delay_ms (250); - - // Initialize the GPS engine. - while (!gpsSetup()); - - // Charge the ADC filters. - for (i = 0; i < 32; ++i) - adcUpdate(); - - // Log startup event. - logType (LOG_BOOTED); - logUint8 (gpsPosition.month); - logUint8 (gpsPosition.day); - logUint8 (gpsPosition.year & 0xff); - - logUint8 (gpsPosition.hours); - logUint8 (gpsPosition.minutes); - logUint8 (gpsPosition.seconds); +// ddsInit(); // Transmit software version packet on start up. tncTxPacket(TNC_MODE_1200_AFSK); + exit(0); // Counters to send packets if the GPS time stamp is not available. lockLostCounter = 5; utcSeconds = 55; @@ -3049,7 +1591,7 @@ test(); for (;;) { // Read the GPS engine serial port FIFO and process the GPS data. - gpsUpdate(); +// gpsUpdate(); if (gpsIsReady()) { @@ -3068,7 +1610,7 @@ test(); lockLostCounter = 0; // Log the data to flash. - sysLogGPSData(); +// sysLogGPSData(); } // END if gpsIsReady // Processing that occurs once a second. @@ -3088,7 +1630,7 @@ test(); ++lockLostCounter; // Update the ADC filters. - adcUpdate(); +// adcUpdate(); if (timeHours == 5 && timeMinutes == 0 && timeSeconds == 0) gpsPowerOff(); -- cgit v1.2.3 From 0c2c47dd7af2fc95de852178c4244daba02f44ed Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 5 Dec 2012 19:44:09 -0800 Subject: altos: Add test scaffolding for APRS This moves some test code out of ao_aprs.c and into ao_aprs_test.c, and then adds Makefile fragments to compile and run the resulting program, creating a wav file as output Signed-off-by: Keith Packard --- src/drivers/ao_aprs.c | 163 +++++------------------------------------------- src/test/Makefile | 14 ++++- src/test/ao_aprs_test.c | 145 ++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 174 insertions(+), 148 deletions(-) create mode 100644 src/test/ao_aprs_test.c (limited to 'src') diff --git a/src/drivers/ao_aprs.c b/src/drivers/ao_aprs.c index be7abaf5..df68278c 100644 --- a/src/drivers/ao_aprs.c +++ b/src/drivers/ao_aprs.c @@ -139,11 +139,10 @@ * */ -#include -#include -#include -#include -#include +#ifndef AO_APRS_TEST +#include +#endif + #include typedef int bool_t; @@ -395,10 +394,8 @@ const uint32_t freqTable[256] = */ void ddsSetFTW (uint32_t ftw) { - static int id; int x = ftw - freqTable[0]; putchar (x > 0 ? 0xff : 0x0); -// printf ("%d %d\n", id++, x > 0 ? 1 : 0); } /** @@ -1003,6 +1000,8 @@ void tncSetMode(TNC_DATA_MODE dataMode) // FSK tones at 445.947 and 445.953 MHz ddsSetFSKFreq (955382980, 955453621); break; + case TNC_MODE_STANDBY: + break; } // END switch tncDataMode = dataMode; @@ -1061,11 +1060,12 @@ void tnc1200TimerTick() case TNC_TX_SYNC: // The variable tncShift contains the lastest data byte. // NRZI enocde the data stream. - if ((tncShift & 0x01) == 0x00) + if ((tncShift & 0x01) == 0x00) { if (tncTxBit == 0) tncTxBit = 1; else tncTxBit = 0; + } // When the flag is done, determine if we need to send more or data. if (++tncBitCount == 8) @@ -1101,11 +1101,12 @@ void tnc1200TimerTick() // The variable tncShift contains the lastest data byte. // NRZI enocde the data stream. - if ((tncShift & 0x01) == 0x00) + if ((tncShift & 0x01) == 0x00) { if (tncTxBit == 0) tncTxBit = 1; else tncTxBit = 0; + } // Save the data stream so we can determine if bit stuffing is // required on the next bit time. @@ -1145,11 +1146,12 @@ void tnc1200TimerTick() // The variable tncShift contains the lastest data byte. // NRZI enocde the data stream. - if ((tncShift & 0x01) == 0x00) + if ((tncShift & 0x01) == 0x00) { if (tncTxBit == 0) tncTxBit = 1; else tncTxBit = 0; + } // Save the data stream so we can determine if bit stuffing is // required on the next bit time. @@ -1177,11 +1179,12 @@ void tnc1200TimerTick() case TNC_TX_END: // The variable tncShift contains the lastest data byte. // NRZI enocde the data stream. - if ((tncShift & 0x01) == 0x00) + if ((tncShift & 0x01) == 0x00) { if (tncTxBit == 0) tncTxBit = 1; else tncTxBit = 0; + } // If all the bits were shifted, get the next one. if (++tncBitCount == 8) @@ -1239,7 +1242,7 @@ tncPrintf(char *fmt, ...) int c; va_start(ap, fmt); - c = vsprintf(tncBufferPnt, fmt, ap); + c = vsprintf((char *) tncBufferPnt, fmt, ap); va_end(ap); tncBufferPnt += c; tncLength += c; @@ -1369,7 +1372,7 @@ void tncGPRMCPacket() */ void tncStatusPacket(int16_t temperature) { - uint16_t voltage; +// uint16_t voltage; // Plain text telemetry. tncPrintf (">ANSR "); @@ -1425,7 +1428,7 @@ void tncStatusPacket(int16_t temperature) */ void tncTxPacket(TNC_DATA_MODE dataMode) { - int16_t temperature; + int16_t temperature = 20; uint16_t crc; // Only transmit if there is not another message in progress. @@ -1510,135 +1513,3 @@ void tncTxPacket(TNC_DATA_MODE dataMode) } /** @} */ - -#if 0 -uint32_t counter; - -uint8_t bitIndex; -uint8_t streamIndex; -uint8_t value; - -uint8_t bitStream[] = { 0x10, 0x20, 0x30 }; - -void init() -{ - counter = 0; - bitIndex = 0; - streamIndex = 0; - value = bitStream[0]; -} - -void test() -{ - counter += 0x10622d; - -// CCP_1 = (uint16_t) ((counter >> 16) & 0xffff); - - if ((value & 0x80) == 0x80) - setup_ccp1 (CCP_COMPARE_SET_ON_MATCH); - else - setup_ccp1 (CCP_COMPARE_CLR_ON_MATCH); - - if (++bitIndex == 8) - { - bitIndex = 0; - - if (++streamIndex == sizeof(bitStream)) - { - streamIndex = 0; - } - - value = bitStream[streamIndex]; - } else - value = value << 1; -} -#endif - -// This is where we go after reset. -int main(int argc, char **argv) -{ - uint8_t i, utcSeconds, lockLostCounter; - -//test(); - - // Configure the basic systems. -// sysInit(); - - // Wait for the power converter chains to stabilize. -// delay_ms (100); - - // Setup the subsystems. -// adcInit(); -// flashInit(); - gpsInit(); -// logInit(); -// timeInit(); -// serialInit(); - tncInit(); - - // Program the DDS. -// ddsInit(); - - // Transmit software version packet on start up. - tncTxPacket(TNC_MODE_1200_AFSK); - - exit(0); - // Counters to send packets if the GPS time stamp is not available. - lockLostCounter = 5; - utcSeconds = 55; - - // This is the main loop that process GPS data and waits for the once per second timer tick. - for (;;) - { - // Read the GPS engine serial port FIFO and process the GPS data. -// gpsUpdate(); - - if (gpsIsReady()) - { - // Start the flight timer when we get a valid 3D fix. - if (gpsGetFixType() == GPS_3D_FIX) - timeSetRunFlag(); - - // Generate our packets based on the GPS time. - if (tncIsTimeSlot(gpsPosition.seconds)) - tncTxPacket(TNC_MODE_1200_AFSK); - - // Sync the internal clock to GPS UTC time. - utcSeconds = gpsPosition.seconds; - - // This counter is reset every time we receive the GPS message. - lockLostCounter = 0; - - // Log the data to flash. -// sysLogGPSData(); - } // END if gpsIsReady - - // Processing that occurs once a second. - if (timeIsUpdate()) - { - // We maintain the UTC time in seconds if we shut off the GPS engine or it fails. - if (++utcSeconds == 60) - utcSeconds = 0; - - // If we loose information for more than 5 seconds, - // we will determine when to send a packet based on internal time. - if (lockLostCounter == 5) - { - if (tncIsTimeSlot(utcSeconds)) - tncTxPacket(TNC_MODE_1200_AFSK); - } else - ++lockLostCounter; - - // Update the ADC filters. -// adcUpdate(); - - if (timeHours == 5 && timeMinutes == 0 && timeSeconds == 0) - gpsPowerOff(); - - } // END if timeIsUpdate - - } // END for -} - - - diff --git a/src/test/Makefile b/src/test/Makefile index 0dcdc949..092bf360 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -1,7 +1,8 @@ vpath % ..:../core:../drivers:../util PROGS=ao_flight_test ao_flight_test_baro ao_flight_test_accel ao_flight_test_noisy_accel ao_flight_test_mm \ - ao_gps_test ao_gps_test_skytraq ao_convert_test ao_convert_pa_test ao_fec_test + ao_gps_test ao_gps_test_skytraq ao_convert_test ao_convert_pa_test ao_fec_test \ + ao_aprs_test INCS=ao_kalman.h ao_ms5607.h ao_log.h ao_data.h altitude-pa.h altitude.h @@ -9,7 +10,7 @@ KALMAN=make-kalman CFLAGS=-I.. -I. -I../core -I../drivers -O0 -g -Wall -all: $(PROGS) +all: $(PROGS) ao_aprs_data.wav clean: rm -f $(PROGS) run-out.baro run-out.full @@ -49,5 +50,14 @@ ao_kalman.h: $(KALMAN) ao_fec_test: ao_fec_test.c ao_fec_tx.c ao_fec_rx.c cc $(CFLAGS) -DAO_FEC_DEBUG=1 -o $@ ao_fec_test.c ../core/ao_fec_tx.c ../core/ao_fec_rx.c -lm +ao_aprs_test: ao_aprs_test.c ao_aprs.c + cc $(CFLAGS) -o $@ ao_aprs_test.c + +SOX_INPUT_ARGS=--type raw --encoding unsigned-integer -b 8 -c 1 -r 9600 +SOX_OUTPUT_ARGS=--type wav + +ao_aprs_data.wav: ao_aprs_test + ./ao_aprs_test | sox $(SOX_INPUT_ARGS) - $(SOX_OUTPUT_ARGS) $@ + check: ao_fec_test ao_flight_test ao_flight_test_baro run-tests ./ao_fec_test && ./run-tests \ No newline at end of file diff --git a/src/test/ao_aprs_test.c b/src/test/ao_aprs_test.c new file mode 100644 index 00000000..d791e930 --- /dev/null +++ b/src/test/ao_aprs_test.c @@ -0,0 +1,145 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program 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 + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include +#include +#include +#include +#include + +#include + +#define AO_APRS_TEST + +#include + +/* + * @section copyright_sec Copyright + * + * Copyright (c) 2001-2009 Michael Gray, KD7LMO + + + * + * + * @section gpl_sec GNU General Public License + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + + */ + +// This is where we go after reset. +int main(int argc, char **argv) +{ + uint8_t utcSeconds, lockLostCounter; + +//test(); + + // Configure the basic systems. +// sysInit(); + + // Wait for the power converter chains to stabilize. +// delay_ms (100); + + // Setup the subsystems. +// adcInit(); +// flashInit(); + gpsInit(); +// logInit(); +// timeInit(); +// serialInit(); + tncInit(); + + // Program the DDS. +// ddsInit(); + + // Transmit software version packet on start up. + tncTxPacket(TNC_MODE_1200_AFSK); + + exit(0); + // Counters to send packets if the GPS time stamp is not available. + lockLostCounter = 5; + utcSeconds = 55; + + // This is the main loop that process GPS data and waits for the once per second timer tick. + for (;;) + { + // Read the GPS engine serial port FIFO and process the GPS data. +// gpsUpdate(); + + if (gpsIsReady()) + { + // Start the flight timer when we get a valid 3D fix. + if (gpsGetFixType() == GPS_3D_FIX) + timeSetRunFlag(); + + // Generate our packets based on the GPS time. + if (tncIsTimeSlot(gpsPosition.seconds)) + tncTxPacket(TNC_MODE_1200_AFSK); + + // Sync the internal clock to GPS UTC time. + utcSeconds = gpsPosition.seconds; + + // This counter is reset every time we receive the GPS message. + lockLostCounter = 0; + + // Log the data to flash. +// sysLogGPSData(); + } // END if gpsIsReady + + // Processing that occurs once a second. + if (timeIsUpdate()) + { + // We maintain the UTC time in seconds if we shut off the GPS engine or it fails. + if (++utcSeconds == 60) + utcSeconds = 0; + + // If we loose information for more than 5 seconds, + // we will determine when to send a packet based on internal time. + if (lockLostCounter == 5) + { + if (tncIsTimeSlot(utcSeconds)) + tncTxPacket(TNC_MODE_1200_AFSK); + } else + ++lockLostCounter; + + // Update the ADC filters. +// adcUpdate(); + + if (timeHours == 5 && timeMinutes == 0 && timeSeconds == 0) + gpsPowerOff(); + + } // END if timeIsUpdate + + } // END for +} + + + + -- cgit v1.2.3 From 8b1f186a574c22cebd9daba9d352ec82556c3b28 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 5 Dec 2012 20:10:54 -0800 Subject: altos: Generate all of the APRS messages Note that two of them are in NMEA form, which some receivers appear not to parse Signed-off-by: Keith Packard --- src/drivers/ao_aprs.c | 8 ++++++-- src/test/ao_aprs_test.c | 35 ++++++++++++++--------------------- 2 files changed, 20 insertions(+), 23 deletions(-) (limited to 'src') diff --git a/src/drivers/ao_aprs.c b/src/drivers/ao_aprs.c index df68278c..0a41d5fd 100644 --- a/src/drivers/ao_aprs.c +++ b/src/drivers/ao_aprs.c @@ -395,7 +395,7 @@ const uint32_t freqTable[256] = void ddsSetFTW (uint32_t ftw) { int x = ftw - freqTable[0]; - putchar (x > 0 ? 0xff : 0x0); + putchar (x > 0 ? 0xc0 : 0x40); } /** @@ -1243,6 +1243,10 @@ tncPrintf(char *fmt, ...) va_start(ap, fmt); c = vsprintf((char *) tncBufferPnt, fmt, ap); + if (*fmt == '\015') + fprintf (stderr, "\n"); + else + vfprintf(stderr, fmt, ap); va_end(ap); tncBufferPnt += c; tncLength += c; @@ -1378,7 +1382,7 @@ void tncStatusPacket(int16_t temperature) tncPrintf (">ANSR "); // Display the flight time. - tncPrintf ("%02U:%02U:%02U ", timeHours, timeMinutes, timeSeconds); + tncPrintf ("%02u:%02u:%02u ", timeHours, timeMinutes, timeSeconds); // Altitude in feet. tncPrintf ("%ld' ", gpsPosition.altitudeFeet); diff --git a/src/test/ao_aprs_test.c b/src/test/ao_aprs_test.c index d791e930..1c0b252b 100644 --- a/src/test/ao_aprs_test.c +++ b/src/test/ao_aprs_test.c @@ -54,42 +54,34 @@ */ -// This is where we go after reset. -int main(int argc, char **argv) +static void +audio_gap(int secs) { - uint8_t utcSeconds, lockLostCounter; - -//test(); + int samples = secs * 9600; - // Configure the basic systems. -// sysInit(); - - // Wait for the power converter chains to stabilize. -// delay_ms (100); + while (samples--) + putchar(0x7f); +} - // Setup the subsystems. -// adcInit(); -// flashInit(); +// This is where we go after reset. +int main(int argc, char **argv) +{ + uint8_t utcSeconds, lockLostCounter, i; gpsInit(); -// logInit(); -// timeInit(); -// serialInit(); tncInit(); - // Program the DDS. -// ddsInit(); - + audio_gap(1); // Transmit software version packet on start up. tncTxPacket(TNC_MODE_1200_AFSK); - exit(0); // Counters to send packets if the GPS time stamp is not available. lockLostCounter = 5; utcSeconds = 55; // This is the main loop that process GPS data and waits for the once per second timer tick. - for (;;) + for (i = 0; i < 5; i++) { + audio_gap(10); // Read the GPS engine serial port FIFO and process the GPS data. // gpsUpdate(); @@ -138,6 +130,7 @@ int main(int argc, char **argv) } // END if timeIsUpdate } // END for + return 0; } -- cgit v1.2.3 From 03f844ddcd95166211451fda0b20f9b15496294e Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 5 Dec 2012 20:11:35 -0800 Subject: altos: Add missing ao_aprs.h file This has defines for the planned APRS interface Signed-off-by: Keith Packard --- src/drivers/ao_aprs.h | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) create mode 100644 src/drivers/ao_aprs.h (limited to 'src') diff --git a/src/drivers/ao_aprs.h b/src/drivers/ao_aprs.h new file mode 100644 index 00000000..fe3c6349 --- /dev/null +++ b/src/drivers/ao_aprs.h @@ -0,0 +1,27 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program 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 + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_APRS_H_ +#define _AO_APRS_H_ + +void +ao_aprs_send(void); + +void +ao_aprs_init(void); + +#endif /* _AO_APRS_H_ */ -- cgit v1.2.3 From fe820a8a2dc6248b5edb96a9521536d41b936116 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 5 Dec 2012 21:01:59 -0800 Subject: Signed-off-by: Keith Packard altos: Switch APRS to standard position reporting form Stop using NMEA sentences for position --- src/drivers/ao_aprs.c | 55 ++++++++++++++++++++++++++++++++++++++++++++++++- src/test/ao_aprs_test.c | 2 +- 2 files changed, 55 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/ao_aprs.c b/src/drivers/ao_aprs.c index 0a41d5fd..cea802bb 100644 --- a/src/drivers/ao_aprs.c +++ b/src/drivers/ao_aprs.c @@ -1370,6 +1370,58 @@ void tncGPRMCPacket() tncPrintf ("*%02X", gpsNMEAChecksum(tncBuffer + 1, tncLength - 1)); } +/** + * Generate the plain text position packet. Data is written through the tncTxByte + * callback function + */ +void tncPositionPacket(void) +{ + int32_t latitude = 45.4694766 * 10000000; + int32_t longitude = -122.7376250 * 10000000; + uint32_t altitude = 10000; + uint16_t lat_deg; + uint16_t lon_deg; + uint16_t lat_min; + uint16_t lat_frac; + uint16_t lon_min; + uint16_t lon_frac; + + char lat_sign = 'N', lon_sign = 'E'; + +// tncPrintf (">ANSR "); + if (latitude < 0) { + lat_sign = 'S'; + latitude = -latitude; + } + + if (longitude < 0) { + lon_sign = 'W'; + longitude = -longitude; + } + + lat_deg = latitude / 10000000; + latitude -= lat_deg * 10000000; + latitude *= 60; + lat_min = latitude / 10000000; + latitude -= lat_min * 10000000; + lat_frac = (latitude + 50000) / 100000; + + lon_deg = longitude / 10000000; + longitude -= lon_deg * 10000000; + longitude *= 60; + lon_min = longitude / 10000000; + longitude -= lon_min * 10000000; + lon_frac = (longitude + 50000) / 100000; + + tncPrintf ("=%02u%02u.%02u%c\\%03u%02u.%02u%cO", + lat_deg, lat_min, lat_frac, lat_sign, + lon_deg, lon_min, lon_frac, lon_sign); + + tncPrintf (" /A=%06u", altitude * 100 / 3048); +} + + + /** * Generate the plain text status packet. Data is written through the tncTxByte * callback function. @@ -1466,7 +1518,8 @@ void tncTxPacket(TNC_DATA_MODE dataMode) break; case TNC_GGA: - tncGPGGAPacket(); + tncPositionPacket(); +// tncGPGGAPacket(); // Select the next packet we will generate. tncPacketType = TNC_RMC; diff --git a/src/test/ao_aprs_test.c b/src/test/ao_aprs_test.c index 1c0b252b..cec4d617 100644 --- a/src/test/ao_aprs_test.c +++ b/src/test/ao_aprs_test.c @@ -79,7 +79,7 @@ int main(int argc, char **argv) utcSeconds = 55; // This is the main loop that process GPS data and waits for the once per second timer tick. - for (i = 0; i < 5; i++) + for (i = 0; i < 3; i++) { audio_gap(10); // Read the GPS engine serial port FIFO and process the GPS data. -- cgit v1.2.3 From 3e1254c4f3261f66d8070250898fe906eb80d8f2 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 5 Dec 2012 21:08:19 -0800 Subject: altos: Strip out everything but the basic position reporting from APRS Any useful data will be sent over the digital link; APRS is strictly for position tracking Signed-off-by: Keith Packard --- src/drivers/ao_aprs.c | 208 +----------------------------------------------- src/test/ao_aprs_test.c | 62 +-------------- 2 files changed, 4 insertions(+), 266 deletions(-) (limited to 'src') diff --git a/src/drivers/ao_aprs.c b/src/drivers/ao_aprs.c index cea802bb..3a51ae90 100644 --- a/src/drivers/ao_aprs.c +++ b/src/drivers/ao_aprs.c @@ -1252,124 +1252,6 @@ tncPrintf(char *fmt, ...) tncLength += c; } -/** - * Generate the GPS NMEA standard UTC time stamp. Data is written through the tncTxByte - * callback function. - */ -void tncNMEATime() -{ - // UTC of position fix. - tncPrintf ("%02d%02d%02d,", gpsPosition.hours, gpsPosition.minutes, gpsPosition.seconds); -} - -/** - * Generate the GPS NMEA standard latitude/longitude fix. Data is written through the tncTxByte - * callback function. - */ -void tncNMEAFix() -{ - uint8_t dirChar; - uint32_t coord, coordMin; - - // Latitude value. - coord = gpsPosition.latitude; - - if (gpsPosition.latitude < 0) - { - coord = gpsPosition.latitude * -1; - dirChar = 'S'; - } else { - coord = gpsPosition.latitude; - dirChar = 'N'; - } - - coordMin = (coord % 3600000) / 6; - tncPrintf ("%02ld%02ld.%04ld,%c,", (uint32_t) (coord / 3600000), (uint32_t) (coordMin / 10000), (uint32_t) (coordMin % 10000), dirChar); - - - // Longitude value. - if (gpsPosition.longitude < 0) - { - coord = gpsPosition.longitude * - 1; - dirChar = 'W'; - } else { - coord = gpsPosition.longitude; - dirChar = 'E'; - } - - coordMin = (coord % 3600000) / 6; - tncPrintf ("%03ld%02ld.%04ld,%c,", (uint32_t) (coord / 3600000), (uint32_t) (coordMin / 10000), (uint32_t) (coordMin % 10000), dirChar); - -} - -/** - * Generate the GPS NMEA-0183 $GPGGA packet. Data is written through the tncTxByte - * callback function. - */ -void tncGPGGAPacket() -{ - // Generate the GPGGA message. - tncPrintf ("$GPGGA,"); - - // Standard NMEA time. - tncNMEATime(); - - // Standard NMEA-0183 latitude/longitude. - tncNMEAFix(); - - // GPS status where 0: not available, 1: available - if (gpsGetFixType() != GPS_NO_FIX) - tncPrintf ("1,"); - else - tncPrintf ("0,"); - - // Number of visible birds. - tncPrintf ("%02d,", gpsPosition.trackedSats); - - // DOP - tncPrintf ("%ld.%01ld,", gpsPosition.dop / 10, gpsPosition.dop % 10); - - // Altitude in meters. - tncPrintf ("%ld.%02ld,M,,M,,", (int32_t) (gpsPosition.altitudeCM / 100l), (int32_t) (gpsPosition.altitudeCM % 100)); - - // Checksum, we add 1 to skip over the $ character. - tncPrintf ("*%02X", gpsNMEAChecksum(tncBuffer + 1, tncLength - 1)); -} - -/** - * Generate the GPS NMEA-0183 $GPRMC packet. Data is written through the tncTxByte - * callback function. - */ -void tncGPRMCPacket() -{ - uint32_t temp; - - // Generate the GPRMC message. - tncPrintf ("$GPRMC,"); - - // Standard NMEA time. - tncNMEATime(); - - // GPS status. - if (gpsGetFixType() != GPS_NO_FIX) - tncPrintf ("A,"); - else - tncPrintf ("V,"); - - // Standard NMEA-0183 latitude/longitude. - tncNMEAFix(); - - // Speed knots and heading. - temp = (int32_t) gpsPosition.hSpeed * 75000 / 385826; - tncPrintf ("%ld.%ld,%ld.%ld,", (int16_t) (temp / 10), (int16_t) (temp % 10), gpsPosition.heading / 10, gpsPosition.heading % 10); - - // Date - tncPrintf ("%02d%02d%02ld,,", gpsPosition.day, gpsPosition.month, gpsPosition.year % 100); - - // Checksum, skip over the $ character. - tncPrintf ("*%02X", gpsNMEAChecksum(tncBuffer + 1, tncLength - 1)); -} - /** * Generate the plain text position packet. Data is written through the tncTxByte * callback function @@ -1420,62 +1302,6 @@ void tncPositionPacket(void) tncPrintf (" /A=%06u", altitude * 100 / 3048); } - - -/** - * Generate the plain text status packet. Data is written through the tncTxByte - * callback function. - */ -void tncStatusPacket(int16_t temperature) -{ -// uint16_t voltage; - - // Plain text telemetry. - tncPrintf (">ANSR "); - - // Display the flight time. - tncPrintf ("%02u:%02u:%02u ", timeHours, timeMinutes, timeSeconds); - - // Altitude in feet. - tncPrintf ("%ld' ", gpsPosition.altitudeFeet); - - // Peak altitude in feet. - tncPrintf ("%ld'pk ", gpsGetPeakAltitude()); - - // GPS hdop or pdop - tncPrintf ("%lu.%lu", gpsPosition.dop / 10, gpsPosition.dop % 10); - - // The text 'pdop' for a 3D fix, 'hdop' for a 2D fix, and 'dop' for no fix. - switch (gpsGetFixType()) - { - case GPS_NO_FIX: - tncPrintf ("dop "); - break; - - case GPS_2D_FIX: - tncPrintf ("hdop "); - break; - - - case GPS_3D_FIX: - tncPrintf ("pdop "); - break; - } // END switch - - // Number of satellites in the solution. - tncPrintf ("%utrk ", gpsPosition.trackedSats); - - // Display main bus voltage. -// voltage = adcGetMainBusVolt(); -// tncPrintf ("%lu.%02luvdc ", voltage / 100, voltage % 100); - - // Display internal temperature. -// tncPrintf ("%ld.%01ldF ", temperature / 10, abs(temperature % 10)); - - // Print web address link. - tncPrintf ("www.altusmetrum.org"); -} - /** * Prepare an AX.25 data packet. Each time this method is called, it automatically * rotates through 1 of 3 messages. @@ -1484,7 +1310,6 @@ void tncStatusPacket(int16_t temperature) */ void tncTxPacket(TNC_DATA_MODE dataMode) { - int16_t temperature = 20; uint16_t crc; // Only transmit if there is not another message in progress. @@ -1500,38 +1325,7 @@ void tncTxPacket(TNC_DATA_MODE dataMode) // Set the message length counter. tncLength = 0; - // Determine the contents of the packet. - switch (tncPacketType) - { - case TNC_BOOT_MESSAGE: - tncPrintf (">MegaMetrum v1.0 Beacon"); - - // Select the next packet we will generate. - tncPacketType = TNC_STATUS; - break; - - case TNC_STATUS: - tncStatusPacket(temperature); - - // Select the next packet we will generate. - tncPacketType = TNC_GGA; - break; - - case TNC_GGA: - tncPositionPacket(); -// tncGPGGAPacket(); - - // Select the next packet we will generate. - tncPacketType = TNC_RMC; - break; - - case TNC_RMC: - tncGPRMCPacket(); - - // Select the next packet we will generate. - tncPacketType = TNC_STATUS; - break; - } + tncPositionPacket(); // Add the end of message character. tncPrintf ("\015"); diff --git a/src/test/ao_aprs_test.c b/src/test/ao_aprs_test.c index cec4d617..947a02b4 100644 --- a/src/test/ao_aprs_test.c +++ b/src/test/ao_aprs_test.c @@ -66,71 +66,15 @@ audio_gap(int secs) // This is where we go after reset. int main(int argc, char **argv) { - uint8_t utcSeconds, lockLostCounter, i; gpsInit(); tncInit(); audio_gap(1); - // Transmit software version packet on start up. + + /* Transmit one packet */ tncTxPacket(TNC_MODE_1200_AFSK); - // Counters to send packets if the GPS time stamp is not available. - lockLostCounter = 5; - utcSeconds = 55; - - // This is the main loop that process GPS data and waits for the once per second timer tick. - for (i = 0; i < 3; i++) - { - audio_gap(10); - // Read the GPS engine serial port FIFO and process the GPS data. -// gpsUpdate(); - - if (gpsIsReady()) - { - // Start the flight timer when we get a valid 3D fix. - if (gpsGetFixType() == GPS_3D_FIX) - timeSetRunFlag(); - - // Generate our packets based on the GPS time. - if (tncIsTimeSlot(gpsPosition.seconds)) - tncTxPacket(TNC_MODE_1200_AFSK); - - // Sync the internal clock to GPS UTC time. - utcSeconds = gpsPosition.seconds; - - // This counter is reset every time we receive the GPS message. - lockLostCounter = 0; - - // Log the data to flash. -// sysLogGPSData(); - } // END if gpsIsReady - - // Processing that occurs once a second. - if (timeIsUpdate()) - { - // We maintain the UTC time in seconds if we shut off the GPS engine or it fails. - if (++utcSeconds == 60) - utcSeconds = 0; - - // If we loose information for more than 5 seconds, - // we will determine when to send a packet based on internal time. - if (lockLostCounter == 5) - { - if (tncIsTimeSlot(utcSeconds)) - tncTxPacket(TNC_MODE_1200_AFSK); - } else - ++lockLostCounter; - - // Update the ADC filters. -// adcUpdate(); - - if (timeHours == 5 && timeMinutes == 0 && timeSeconds == 0) - gpsPowerOff(); - - } // END if timeIsUpdate - - } // END for - return 0; + exit(0); } -- cgit v1.2.3 From d717edd18a35376811d6be0d0c7522ee8cc426f9 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 5 Dec 2012 21:13:37 -0800 Subject: altos: Reduce printf calls in APRS packet generation Merge all of the data into a single printf call Signed-off-by: Keith Packard --- src/drivers/ao_aprs.c | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) (limited to 'src') diff --git a/src/drivers/ao_aprs.c b/src/drivers/ao_aprs.c index 3a51ae90..b45ef8c5 100644 --- a/src/drivers/ao_aprs.c +++ b/src/drivers/ao_aprs.c @@ -1270,7 +1270,6 @@ void tncPositionPacket(void) char lat_sign = 'N', lon_sign = 'E'; -// tncPrintf (">ANSR "); if (latitude < 0) { lat_sign = 'S'; latitude = -latitude; @@ -1295,11 +1294,10 @@ void tncPositionPacket(void) longitude -= lon_min * 10000000; lon_frac = (longitude + 50000) / 100000; - tncPrintf ("=%02u%02u.%02u%c\\%03u%02u.%02u%cO", + tncPrintf ("=%02u%02u.%02u%c\\%03u%02u.%02u%cO /A=%06u\015", lat_deg, lat_min, lat_frac, lat_sign, - lon_deg, lon_min, lon_frac, lon_sign); - - tncPrintf (" /A=%06u", altitude * 100 / 3048); + lon_deg, lon_min, lon_frac, lon_sign, + altitude * 100 / 3048); } /** @@ -1327,9 +1325,6 @@ void tncTxPacket(TNC_DATA_MODE dataMode) tncPositionPacket(); - // Add the end of message character. - tncPrintf ("\015"); - // Calculate the CRC for the header and message. crc = sysCRC16(TNC_AX25_HEADER, sizeof(TNC_AX25_HEADER), 0xffff); crc = sysCRC16(tncBuffer, tncLength, crc ^ 0xffff); -- cgit v1.2.3 From 0bb7200f85db1bc6e39e72e671be9a7aef9c8f09 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 5 Dec 2012 21:22:55 -0800 Subject: altos: Remove more unused APRS code Getting down to a reasonable amount of code. Signed-off-by: Keith Packard --- src/drivers/ao_aprs.c | 545 ++---------------------------------------------- src/test/ao_aprs_test.c | 3 +- 2 files changed, 17 insertions(+), 531 deletions(-) (limited to 'src') diff --git a/src/drivers/ao_aprs.c b/src/drivers/ao_aprs.c index b45ef8c5..c1a800a9 100644 --- a/src/drivers/ao_aprs.c +++ b/src/drivers/ao_aprs.c @@ -152,110 +152,12 @@ typedef int32_t int32; // Public methods, constants, and data structures for each class. -/// Operational modes of the AD9954 DDS for the ddsSetMode function. -typedef enum -{ - /// Device has not been initialized. - DDS_MODE_NOT_INITIALIZED, - - /// Device in lowest power down mode. - DDS_MODE_POWERDOWN, - - /// Generate FM modulated audio tones. - DDS_MODE_AFSK, - - /// Generate true FSK tones. - DDS_MODE_FSK -} DDS_MODE; - void ddsInit(); void ddsSetAmplitude (uint8_t amplitude); void ddsSetOutputScale (uint16_t amplitude); void ddsSetFSKFreq (uint32_t ftw0, uint32_t ftw1); void ddsSetFreq (uint32_t freq); void ddsSetFTW (uint32_t ftw); -void ddsSetMode (DDS_MODE mode); - -/// Type of GPS fix. -typedef enum -{ - /// No GPS FIX - GPS_NO_FIX, - - /// 2D (Latitude/Longitude) fix. - GPS_2D_FIX, - - /// 3D (Latitude/Longitude/Altitude) fix. - GPS_3D_FIX -} GPS_FIX_TYPE; - -/// GPS Position information. -typedef struct -{ - /// Flag that indicates the position information has been updated since it was last checked. - bool_t updateFlag; - - /// Month in UTC time. - uint8_t month; - - /// Day of month in UTC time. - uint8_t day; - - /// Hours in UTC time. - uint8_t hours; - - /// Minutes in UTC time. - uint8_t minutes; - - /// Seconds in UTC time. - uint8_t seconds; - - /// Year in UTC time. - uint16_t year; - - /// Latitude in milli arc-seconds where + is North, - is South. - int32_t latitude; - - /// Longitude in milli arc-seconds where + is East, - is West. - int32_t longitude; - - /// Altitude in cm - int32_t altitudeCM; - - /// Calculated altitude in feet - int32_t altitudeFeet; - - /// 3D speed in cm/second. - uint16_t vSpeed; - - /// 2D speed in cm/second. - uint16_t hSpeed; - - /// Heading units of 0.1 degrees. - uint16_t heading; - - /// DOP (Dilution of Precision) - uint16_t dop; - - /// 16-bit number that represents status of GPS engine. - uint16_t status; - - /// Number of tracked satellites used in the fix position. - uint8_t trackedSats; - - /// Number of visible satellites. - uint8_t visibleSats; -} GPSPOSITION_STRUCT; - -GPSPOSITION_STRUCT gpsPosition; - -void gpsInit(); -bool_t gpsIsReady(); -GPS_FIX_TYPE gpsGetFixType(); -int32_t gpsGetPeakAltitude(); -void gpsPowerOn(); -bool_t gpsSetup(); -void gpsUpdate(); uint16_t sysCRC16(uint8_t *buffer, uint8_t length, uint16_t crc); @@ -264,27 +166,12 @@ void timeInit(); void timeSetDutyCycle (uint8_t dutyCycle); void timeUpdate(); -/// Operational modes of the TNC for the tncSetMode function. -typedef enum -{ - /// No operation waiting for setup and configuration. - TNC_MODE_STANDBY, - - /// 1200 bps using A-FSK (Audio FSK) tones. - TNC_MODE_1200_AFSK, - - /// 9600 bps using true FSK tones. - TNC_MODE_9600_FSK -} TNC_DATA_MODE; - void tncInit(); bool_t tncIsFree(); void tncHighRate(bool_t state); -void tncSetMode (TNC_DATA_MODE dataMode); void tnc1200TimerTick(); -void tnc9600TimerTick(); void tncTxByte (uint8_t value); -void tncTxPacket(TNC_DATA_MODE dataMode); +void tncTxPacket(void); /** @} */ @@ -296,42 +183,6 @@ void tncTxPacket(TNC_DATA_MODE dataMode); * @{ */ -/// AD9954 CFR1 - Control functions including RAM, profiles, OSK, sync, sweep, SPI, and power control settings. -#define DDS_AD9954_CFR1 0x00 - -/// AD9954 CFR2 - Control functions including sync, PLL multiplier, VCO range, and charge pump current. -#define DDS_AD9954_CFR2 0x01 - -/// AD9954 ASF - Auto ramp rate speed control and output scale factor (0x0000 to 0x3fff). -#define DDS_AD9954_ASF 0x02 - -/// AD9954 ARR - Amplitude ramp rate for OSK function. -#define DDS_AD9954_ARR 0x03 - -/// AD9954 FTW0 - Frequency tuning word 0. -#define DDS_AD9954_FTW0 0x04 - -/// AD9954 FTW1 - Frequency tuning word 1 -#define DDS_AD9954_FTW1 0x06 - -/// AD9954 NLSCW - Negative Linear Sweep Control Word used for spectral shaping in FSK mode -#define DDS_AD9954_NLSCW 0x07 - -/// AD9954 PLSCW - Positive Linear Sweep Control Word used for spectral shaping in FSK mode -#define DDS_AD9954_PLSCW 0x08 - -/// AD9954 RSCW0 - RAM Segment Control Word 0 -#define DDS_AD9954_RWCW0 0x07 - -/// AD9954 RSCW0 - RAM Segment Control Word 1 -#define DDS_AD9954_RWCW1 0x08 - -/// AD9954 RAM segment -#define DDS_RAM 0x0b - -/// Current operational mode. -DDS_MODE ddsMode; - /// Number of digits in DDS frequency to FTW conversion. #define DDS_FREQ_TO_FTW_DIGITS 9 @@ -398,287 +249,8 @@ void ddsSetFTW (uint32_t ftw) putchar (x > 0 ? 0xc0 : 0x40); } -/** - * Convert frequency in hertz to 32-bit DDS FTW (Frequency Tune Word). - * - * @param freq frequency in Hertz - * - */ -void ddsSetFreq(uint32_t freq) -{ - uint8_t i; - uint32_t ftw; - - // To avoid rounding errors with floating point math, we do a long multiply on the data. - ftw = freq * DDS_MULT[0]; - - for (i = 0; i < DDS_FREQ_TO_FTW_DIGITS - 1; ++i) - ftw += (freq * DDS_MULT[i+1]) / DDS_DIVISOR[i]; - - ddsSetFTW (ftw); -} - -/** - * Set DDS frequency tuning word for the FSK 0 and 1 values. The output frequency is equal - * to RefClock * (ftw / 2 ^ 32). - * - * @param ftw0 frequency tuning word for the FSK 0 value - * @param ftw1 frequency tuning word for the FSK 1 value - */ -void ddsSetFSKFreq (uint32_t ftw0, uint32_t ftw1) -{ -// printf ("ftw0 %d ftw1 %d\n", ftw0, ftw1); -} - -/** - * Set the DDS to run in A-FSK, FSK, or PSK31 mode - * - * @param mode DDS_MODE_APRS, DDS_MODE_PSK31, or DDS_MODE_HF_APRS constant - */ -void ddsSetMode (DDS_MODE mode) -{ -// printf ("mode %d\n", mode); -} - -/** @} */ - -/** - * @defgroup GPS Motorola M12+ GPS Engine - * - * Functions to control the Motorola M12+ GPS engine in native binary protocol mode. - * - * @{ - */ - -/// The maximum length of a binary GPS engine message. -#define GPS_BUFFER_SIZE 50 - -/// GPS parse engine state machine values. -typedef enum -{ - /// 1st start character '@' - GPS_START1, - - /// 2nd start character '@' - GPS_START2, - - /// Upper case 'A' - 'Z' message type - GPS_COMMAND1, - - /// Lower case 'a' - 'z' message type - GPS_COMMAND2, - - /// 0 - xx bytes based on message type 'Aa' - GPS_READMESSAGE, - - /// 8-bit checksum - GPS_CHECKSUMMESSAGE, - - /// End of message - Carriage Return - GPS_EOMCR, - - /// End of message - Line Feed - GPS_EOMLF -} GPS_PARSE_STATE_MACHINE; - -/// Index into gpsBuffer used to store message data. -uint8_t gpsIndex; - -/// State machine used to parse the GPS message stream. -GPS_PARSE_STATE_MACHINE gpsParseState; - -/// Buffer to store data as it is read from the GPS engine. -uint8_t gpsBuffer[GPS_BUFFER_SIZE]; - -/// Peak altitude detected while GPS is in 3D fix mode. -int32_t gpsPeakAltitude; - -/// Checksum used to verify binary message from GPS engine. -uint8_t gpsChecksum; - -/// Last verified GPS message received. -GPSPOSITION_STRUCT gpsPosition; - -/** - * Get the type of fix. - * - * @return gps fix type enumeration - */ -GPS_FIX_TYPE gpsGetFixType() -{ - // The upper 3-bits determine the fix type. - switch (gpsPosition.status & 0xe000) - { - case 0xe000: - return GPS_3D_FIX; - - case 0xc000: - return GPS_2D_FIX; - - default: - return GPS_NO_FIX; - } // END switch -} - -/** - * Peak altitude detected while GPS is in 3D fix mode since the system was booted. - * - * @return altitude in feet - */ -int32_t gpsGetPeakAltitude() -{ - return gpsPeakAltitude; -} - -/** - * Initialize the GPS subsystem. - */ -void gpsInit() -{ - // Initial parse state. - gpsParseState = GPS_START1; - - // Assume we start at sea level. - gpsPeakAltitude = 0; - - // Clear the structure that stores the position message. - memset (&gpsPosition, 0, sizeof(GPSPOSITION_STRUCT)); - - // Setup the timers used to measure the 1-PPS time period. -// setup_timer_3(T3_INTERNAL | T3_DIV_BY_1); -// setup_ccp2 (CCP_CAPTURE_RE | CCP_USE_TIMER3); -} - -/** - * Determine if new GPS message is ready to process. This function is a one shot and - * typically returns true once a second for each GPS position fix. - * - * @return true if new message available; otherwise false - */ -bool_t gpsIsReady() -{ - return true; - if (gpsPosition.updateFlag) - { - gpsPosition.updateFlag = false; - return true; - } // END if - - return false; -} - -/** - * Calculate NMEA-0183 message checksum of buffer that is length bytes long. - * - * @param buffer pointer to data buffer. - * @param length number of bytes in buffer. - * - * @return checksum of buffer - */ -uint8_t gpsNMEAChecksum (uint8_t *buffer, uint8_t length) -{ - uint8_t i, checksum; - - checksum = 0; - - for (i = 0; i < length; ++i) - checksum ^= buffer[i]; - - return checksum; -} - -/** - * Verify the GPS engine is sending the @@Hb position report message. If not, - * configure the GPS engine to send the desired report. - * - * @return true if GPS engine operation; otherwise false - */ -bool_t gpsSetup() -{ - uint8_t startTime, retryCount; - - // We wait 10 seconds for the GPS engine to respond to our message request. - startTime = timeGetTicks(); - retryCount = 0; - - while (++retryCount < 10) - { - // Read the serial FIFO and process the GPS messages. -// gpsUpdate(); - - // If a GPS data set is available, then GPS is operational. - if (gpsIsReady()) - { -// timeSetDutyCycle (TIME_DUTYCYCLE_10); - return true; - } - - if (timeGetTicks() > startTime) - { - puts ("@@Hb\001\053\015\012"); - startTime += 10; - } // END if - - } // END while - - return false; -} - -/** - * Parse the Motorola @@Hb (Short position/message) report. - */ -void gpsParsePositionMessage() -{ - // Convert the binary stream into data elements. We will scale to the desired units - // as the values are used. - gpsPosition.updateFlag = true; - - gpsPosition.month = gpsBuffer[0]; - gpsPosition.day = gpsBuffer[1]; - gpsPosition.year = ((uint16_t) gpsBuffer[2] << 8) | gpsBuffer[3]; - gpsPosition.hours = gpsBuffer[4]; - gpsPosition.minutes = gpsBuffer[5]; - gpsPosition.seconds = gpsBuffer[6]; - gpsPosition.latitude = ((int32) gpsBuffer[11] << 24) | ((int32) gpsBuffer[12] << 16) | ((int32) gpsBuffer[13] << 8) | (int32) gpsBuffer[14]; - gpsPosition.longitude = ((int32) gpsBuffer[15] << 24) | ((int32) gpsBuffer[16] << 16) | ((int32) gpsBuffer[17] << 8) | gpsBuffer[18]; - gpsPosition.altitudeCM = ((int32) gpsBuffer[19] << 24) | ((int32) gpsBuffer[20] << 16) | ((int32) gpsBuffer[21] << 8) | gpsBuffer[22]; - gpsPosition.altitudeFeet = gpsPosition.altitudeCM * 100l / 3048l; - gpsPosition.vSpeed = ((uint16_t) gpsBuffer[27] << 8) | gpsBuffer[28]; - gpsPosition.hSpeed = ((uint16_t) gpsBuffer[29] << 8) | gpsBuffer[30]; - gpsPosition.heading = ((uint16_t) gpsBuffer[31] << 8) | gpsBuffer[32]; - gpsPosition.dop = ((uint16_t) gpsBuffer[33] << 8) | gpsBuffer[34]; - gpsPosition.visibleSats = gpsBuffer[35]; - gpsPosition.trackedSats = gpsBuffer[36]; - gpsPosition.status = ((uint16_t) gpsBuffer[37] << 8) | gpsBuffer[38]; - - // Update the peak altitude if we have a valid 3D fix. - if (gpsGetFixType() == GPS_3D_FIX) - if (gpsPosition.altitudeFeet > gpsPeakAltitude) - gpsPeakAltitude = gpsPosition.altitudeFeet; -} - -/** - * Turn on the GPS engine power and serial interface. - */ -void gpsPowerOn() -{ - // 3.0 VDC LDO control line. -// output_high (IO_GPS_PWR); - -} - -/** - * Turn off the GPS engine power and serial interface. - */ -void gpsPowerOff() -{ - // 3.0 VDC LDO control line. -// output_low (IO_GPS_PWR); -} - /** @} */ - /** * @defgroup sys System Library Functions * @@ -759,9 +331,6 @@ uint16_t timeNCOFreq; /// Counter used to deciminate down from the 104uS to 833uS interrupt rate. (9600 to 1200 baud) uint8_t timeLowRateCount; -/// Current TNC mode (standby, 1200bps A-FSK, or 9600bps FSK) -TNC_DATA_MODE tncDataMode; - /// Flag set true once per second. bool_t timeUpdateFlag; @@ -797,7 +366,6 @@ void timeInit() timeNCO = 0x00; timeLowRateCount = 0; timeNCOFreq = 0x2000; - tncDataMode = TNC_MODE_STANDBY; timeRunFlag = false; } @@ -833,29 +401,16 @@ void timeUpdate() { // Setup the next interrupt for the operational mode. timeCompare += TIME_RATE; -// CCP_1 = timeCompare; - - switch (tncDataMode) - { - case TNC_MODE_STANDBY: - break; - case TNC_MODE_1200_AFSK: - ddsSetFTW (freqTable[timeNCO >> 8]); + ddsSetFTW (freqTable[timeNCO >> 8]); - timeNCO += timeNCOFreq; + timeNCO += timeNCOFreq; - if (++timeLowRateCount == 8) - { - timeLowRateCount = 0; - tnc1200TimerTick(); - } // END if - break; - - case TNC_MODE_9600_FSK: - tnc9600TimerTick(); - break; - } // END switch + if (++timeLowRateCount == 8) + { + timeLowRateCount = 0; + tnc1200TimerTick(); + } // END if } /** @} */ @@ -981,32 +536,6 @@ void tncHighRate(bool_t state) tncHighRateFlag = state; } -/** - * Configure the TNC for the desired data mode. - * - * @param dataMode enumerated type that specifies 1200bps A-FSK or 9600bps FSK - */ -void tncSetMode(TNC_DATA_MODE dataMode) -{ - switch (dataMode) - { - case TNC_MODE_1200_AFSK: - ddsSetMode (DDS_MODE_AFSK); - break; - - case TNC_MODE_9600_FSK: - ddsSetMode (DDS_MODE_FSK); - - // FSK tones at 445.947 and 445.953 MHz - ddsSetFSKFreq (955382980, 955453621); - break; - case TNC_MODE_STANDBY: - break; - } // END switch - - tncDataMode = dataMode; -} - /** * Determine if the seconds value timeSeconds is a valid time slot to transmit * a message. Time seconds is in UTC. @@ -1197,14 +726,6 @@ void tnc1200TimerTick() { tncMode = TNC_TX_READY; - // Tell the TNC time interrupt to stop generating the frequency words. - tncDataMode = TNC_MODE_STANDBY; - - // Key off the DDS. -// output_low (IO_OSK); -// output_low (IO_PTT); - ddsSetMode (DDS_MODE_POWERDOWN); - return; } // END if } else @@ -1222,36 +743,6 @@ void tnc9600TimerTick() } -/** - * Write character to the TNC buffer. Maintain the pointer - * and length to the buffer. The pointer tncBufferPnt and tncLength - * must be set before calling this function for the first time. - * - * @param character to save to telemetry buffer - */ -void tncTxByte (uint8_t character) -{ - *tncBufferPnt++ = character; - ++tncLength; -} - -static void -tncPrintf(char *fmt, ...) -{ - va_list ap; - int c; - - va_start(ap, fmt); - c = vsprintf((char *) tncBufferPnt, fmt, ap); - if (*fmt == '\015') - fprintf (stderr, "\n"); - else - vfprintf(stderr, fmt, ap); - va_end(ap); - tncBufferPnt += c; - tncLength += c; -} - /** * Generate the plain text position packet. Data is written through the tncTxByte * callback function @@ -1267,6 +758,7 @@ void tncPositionPacket(void) uint16_t lat_frac; uint16_t lon_min; uint16_t lon_frac; + int c; char lat_sign = 'N', lon_sign = 'E'; @@ -1294,10 +786,12 @@ void tncPositionPacket(void) longitude -= lon_min * 10000000; lon_frac = (longitude + 50000) / 100000; - tncPrintf ("=%02u%02u.%02u%c\\%03u%02u.%02u%cO /A=%06u\015", - lat_deg, lat_min, lat_frac, lat_sign, - lon_deg, lon_min, lon_frac, lon_sign, - altitude * 100 / 3048); + c = sprintf ((char *) tncBufferPnt, "=%02u%02u.%02u%c\\%03u%02u.%02u%cO /A=%06u\015", + lat_deg, lat_min, lat_frac, lat_sign, + lon_deg, lon_min, lon_frac, lon_sign, + altitude * 100 / 3048); + tncBufferPnt += c; + tncLength += c; } /** @@ -1306,17 +800,10 @@ void tncPositionPacket(void) * * @param dataMode enumerated type that specifies 1200bps A-FSK or 9600bps FSK */ -void tncTxPacket(TNC_DATA_MODE dataMode) +void tncTxPacket(void) { uint16_t crc; - // Only transmit if there is not another message in progress. - if (tncMode != TNC_TX_READY) - return; - - // Configure the DDS for the desired operational. - tncSetMode (dataMode); - // Set a pointer to our TNC output buffer. tncBufferPnt = tncBuffer; diff --git a/src/test/ao_aprs_test.c b/src/test/ao_aprs_test.c index 947a02b4..93dab577 100644 --- a/src/test/ao_aprs_test.c +++ b/src/test/ao_aprs_test.c @@ -66,13 +66,12 @@ audio_gap(int secs) // This is where we go after reset. int main(int argc, char **argv) { - gpsInit(); tncInit(); audio_gap(1); /* Transmit one packet */ - tncTxPacket(TNC_MODE_1200_AFSK); + tncTxPacket(); exit(0); } -- cgit v1.2.3 From b79f448818126258174044a23db5b4f330fd5986 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 5 Dec 2012 21:25:29 -0800 Subject: altos: More APRS trimming Signed-off-by: Keith Packard --- src/drivers/ao_aprs.c | 63 --------------------------------------------------- 1 file changed, 63 deletions(-) (limited to 'src') diff --git a/src/drivers/ao_aprs.c b/src/drivers/ao_aprs.c index c1a800a9..86f5f650 100644 --- a/src/drivers/ao_aprs.c +++ b/src/drivers/ao_aprs.c @@ -167,8 +167,6 @@ void timeSetDutyCycle (uint8_t dutyCycle); void timeUpdate(); void tncInit(); -bool_t tncIsFree(); -void tncHighRate(bool_t state); void tnc1200TimerTick(); void tncTxByte (uint8_t value); void tncTxPacket(void); @@ -504,9 +502,6 @@ TNC_MESSAGE_TYPE tncPacketType; /// Buffer to hold the message portion of the AX.25 packet as we prepare it. uint8_t tncBuffer[TNC_BUFFER_SIZE]; -/// Flag that indicates we want to transmit every 5 seconds. -bool_t tncHighRateFlag; - /** * Initialize the TNC internal variables. */ @@ -515,56 +510,6 @@ void tncInit() tncTxBit = 0; tncMode = TNC_TX_READY; tncPacketType = TNC_BOOT_MESSAGE; - tncHighRateFlag = false; -} - -/** - * Determine if the hardware if ready to transmit a 1200 baud packet. - * - * @return true if ready; otherwise false - */ -bool_t tncIsFree() -{ - if (tncMode == TNC_TX_READY) - return true; - - return false; -} - -void tncHighRate(bool_t state) -{ - tncHighRateFlag = state; -} - -/** - * Determine if the seconds value timeSeconds is a valid time slot to transmit - * a message. Time seconds is in UTC. - * - * @param timeSeconds UTC time in seconds - * - * @return true if valid time slot; otherwise false - */ -bool_t tncIsTimeSlot (uint8_t timeSeconds) -{ - if (tncHighRateFlag) - { - if ((timeSeconds % 5) == 0) - return true; - - return false; - } // END if - - switch (timeSeconds) - { - case 0: - case 15: - case 30: - case 45: - return true; - - default: - return false; - } // END switch } /** @@ -735,14 +680,6 @@ void tnc1200TimerTick() } // END switch } -/** - * Method that is called every 104uS to transmit the 9600bps FSK data stream. - */ -void tnc9600TimerTick() -{ - -} - /** * Generate the plain text position packet. Data is written through the tncTxByte * callback function -- cgit v1.2.3 From 684f53d67379cf2ae696fab93d81e49208dfa43c Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 5 Dec 2012 21:34:05 -0800 Subject: altos: Remove APRS sine-wave table We're generating a lovely square wave, which appears to be decoded just fine thankyouverymuch. Signed-off-by: Keith Packard --- src/drivers/ao_aprs.c | 99 +-------------------------------------------------- 1 file changed, 1 insertion(+), 98 deletions(-) (limited to 'src') diff --git a/src/drivers/ao_aprs.c b/src/drivers/ao_aprs.c index 86f5f650..937be734 100644 --- a/src/drivers/ao_aprs.c +++ b/src/drivers/ao_aprs.c @@ -173,82 +173,6 @@ void tncTxPacket(void); /** @} */ -/** - * @defgroup DDS AD9954 DDS (Direct Digital Synthesizer) - * - * Functions to control the Analog Devices AD9954 DDS. - * - * @{ - */ - -/// Number of digits in DDS frequency to FTW conversion. -#define DDS_FREQ_TO_FTW_DIGITS 9 - -/// Array of multiplication factors used to convert frequency to the FTW. -const uint32_t DDS_MULT[DDS_FREQ_TO_FTW_DIGITS] = { 11, 7, 7, 3, 4, 8, 4, 9, 1 }; - -/// Array of divisors used to convert frequency to the FTW. -const uint32_t DDS_DIVISOR[DDS_FREQ_TO_FTW_DIGITS - 1] = { 10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000 }; - -/// Lookup table to convert dB amplitude scale in 0.5 steps to a linear DDS scale factor. -const uint16_t DDS_AMP_TO_SCALE[] = -{ - 16383, 15467, 14601, 13785, 13013, 12286, 11598, 10949, 10337, 9759, 9213, 8697, - 8211, 7752, 7318, 6909, 6522, 6157, 5813, 5488, 5181, 4891, 4617, 4359, 4115, 3885, 3668, 3463, - 3269, 3086, 2913, 2750, 2597, 2451, 2314, 2185, 2062, 1947, 1838, 1735, 1638 -}; - - -/// Frequency Word List - 4.0KHz FM frequency deviation at 81.15MHz (445.950MHz) -const uint32_t freqTable[256] = -{ - 955418300, 955419456, 955420611, 955421765, 955422916, 955424065, 955425210, 955426351, - 955427488, 955428618, 955429743, 955430861, 955431971, 955433073, 955434166, 955435249, - 955436322, 955437385, 955438435, 955439474, 955440500, 955441513, 955442511, 955443495, - 955444464, 955445417, 955446354, 955447274, 955448176, 955449061, 955449926, 955450773, - 955451601, 955452408, 955453194, 955453960, 955454704, 955455426, 955456126, 955456803, - 955457457, 955458088, 955458694, 955459276, 955459833, 955460366, 955460873, 955461354, - 955461809, 955462238, 955462641, 955463017, 955463366, 955463688, 955463983, 955464250, - 955464489, 955464701, 955464884, 955465040, 955465167, 955465266, 955465337, 955465380, - 955465394, 955465380, 955465337, 955465266, 955465167, 955465040, 955464884, 955464701, - 955464489, 955464250, 955463983, 955463688, 955463366, 955463017, 955462641, 955462238, - 955461809, 955461354, 955460873, 955460366, 955459833, 955459276, 955458694, 955458088, - 955457457, 955456803, 955456126, 955455426, 955454704, 955453960, 955453194, 955452408, - 955451601, 955450773, 955449926, 955449061, 955448176, 955447274, 955446354, 955445417, - 955444464, 955443495, 955442511, 955441513, 955440500, 955439474, 955438435, 955437385, - 955436322, 955435249, 955434166, 955433073, 955431971, 955430861, 955429743, 955428618, - 955427488, 955426351, 955425210, 955424065, 955422916, 955421765, 955420611, 955419456, - 955418300, 955417144, 955415989, 955414836, 955413684, 955412535, 955411390, 955410249, - 955409113, 955407982, 955406857, 955405740, 955404629, 955403528, 955402435, 955401351, - 955400278, 955399216, 955398165, 955397126, 955396100, 955395088, 955394089, 955393105, - 955392136, 955391183, 955390246, 955389326, 955388424, 955387540, 955386674, 955385827, - 955385000, 955384192, 955383406, 955382640, 955381896, 955381174, 955380474, 955379797, - 955379143, 955378513, 955377906, 955377324, 955376767, 955376235, 955375728, 955375246, - 955374791, 955374362, 955373959, 955373583, 955373234, 955372912, 955372618, 955372350, - 955372111, 955371900, 955371716, 955371560, 955371433, 955371334, 955371263, 955371220, - 955371206, 955371220, 955371263, 955371334, 955371433, 955371560, 955371716, 955371900, - 955372111, 955372350, 955372618, 955372912, 955373234, 955373583, 955373959, 955374362, - 955374791, 955375246, 955375728, 955376235, 955376767, 955377324, 955377906, 955378513, - 955379143, 955379797, 955380474, 955381174, 955381896, 955382640, 955383406, 955384192, - 955385000, 955385827, 955386674, 955387540, 955388424, 955389326, 955390246, 955391183, - 955392136, 955393105, 955394089, 955395088, 955396100, 955397126, 955398165, 955399216, - 955400278, 955401351, 955402435, 955403528, 955404629, 955405740, 955406857, 955407982, - 955409113, 955410249, 955411390, 955412535, 955413684, 955414836, 955415989, 955417144 -}; - -/** - * Set DDS frequency tuning word. The output frequency is equal to RefClock * (ftw / 2 ^ 32). - * - * @param ftw Frequency Tuning Word - */ -void ddsSetFTW (uint32_t ftw) -{ - int x = ftw - freqTable[0]; - putchar (x > 0 ? 0xc0 : 0x40); -} - -/** @} */ - /** * @defgroup sys System Library Functions * @@ -400,7 +324,7 @@ void timeUpdate() // Setup the next interrupt for the operational mode. timeCompare += TIME_RATE; - ddsSetFTW (freqTable[timeNCO >> 8]); + putchar ((timeNCO >> 8) < 0x80 ? 0xc0 : 0x40); timeNCO += timeNCOFreq; @@ -446,22 +370,6 @@ typedef enum TNC_TX_END } TNC_TX_1200BPS_STATE; -/// Enumeration of the messages we can transmit. -typedef enum -{ - /// Startup message that contains software version information. - TNC_BOOT_MESSAGE, - - /// Plain text status message. - TNC_STATUS, - - /// Message that contains GPS NMEA-0183 $GPGGA message. - TNC_GGA, - - /// Message that contains GPS NMEA-0183 $GPRMC message. - TNC_RMC -} TNC_MESSAGE_TYPE; - /// AX.25 compliant packet header that contains destination, station call sign, and path. /// 0x76 for SSID-11, 0x78 for SSID-12 uint8_t TNC_AX25_HEADER[30] = { @@ -471,7 +379,6 @@ uint8_t TNC_AX25_HEADER[30] = { 'W' << 1, 'I' << 1, 'D' << 1, 'E' << 1, '3' << 1, ' ' << 1, 0x67, \ 0x03, 0xf0 }; - /// The next bit to transmit. uint8_t tncTxBit; @@ -496,9 +403,6 @@ uint8_t tncBitStuff; /// Pointer to TNC buffer as we save each byte during message preparation. uint8_t *tncBufferPnt; -/// The type of message to tranmit in the next packet. -TNC_MESSAGE_TYPE tncPacketType; - /// Buffer to hold the message portion of the AX.25 packet as we prepare it. uint8_t tncBuffer[TNC_BUFFER_SIZE]; @@ -509,7 +413,6 @@ void tncInit() { tncTxBit = 0; tncMode = TNC_TX_READY; - tncPacketType = TNC_BOOT_MESSAGE; } /** -- cgit v1.2.3 From 933d654ec917d9794e87407a7e579438bb738d54 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 5 Dec 2012 21:37:47 -0800 Subject: altos: Remove a bunch of time bits from the APRS code Signed-off-by: Keith Packard --- src/drivers/ao_aprs.c | 81 --------------------------------------------------- 1 file changed, 81 deletions(-) (limited to 'src') diff --git a/src/drivers/ao_aprs.c b/src/drivers/ao_aprs.c index 937be734..7e9013a0 100644 --- a/src/drivers/ao_aprs.c +++ b/src/drivers/ao_aprs.c @@ -161,7 +161,6 @@ void ddsSetFTW (uint32_t ftw); uint16_t sysCRC16(uint8_t *buffer, uint8_t length, uint16_t crc); -uint8_t timeGetTicks(); void timeInit(); void timeSetDutyCycle (uint8_t dutyCycle); void timeUpdate(); @@ -220,30 +219,6 @@ uint16_t sysCRC16(uint8_t *buffer, uint8_t length, uint16_t crc) * @{ */ -/// A counter that ticks every 100mS. -uint8_t timeTicks; - -/// Counts the number of 104uS interrupts for a 100mS time period. -uint16_t timeInterruptCount; - -/// Counts the number of 100mS time periods in 1 second. -uint8_t time100ms; - -/// System time in seconds. -uint8_t timeSeconds; - -/// System time in minutes. -uint8_t timeMinutes; - -/// System time in hours. -uint8_t timeHours; - -/// Desired LED duty cycle 0 to 9 where 0 = 0% and 9 = 90%. -uint8_t timeDutyCycle; - -/// Current value of the timer 1 compare register used to generate 104uS interrupt rate (9600bps). -uint16_t timeCompare; - /// 16-bit NCO where the upper 8-bits are used to index into the frequency generation table. uint16_t timeNCO; @@ -253,67 +228,14 @@ uint16_t timeNCOFreq; /// Counter used to deciminate down from the 104uS to 833uS interrupt rate. (9600 to 1200 baud) uint8_t timeLowRateCount; -/// Flag set true once per second. -bool_t timeUpdateFlag; - -/// Flag that indicate the flight time should run. -bool_t timeRunFlag; - -/// The change in the CCP_1 register for each 104uS (9600bps) interrupt period. -#define TIME_RATE 125 - -/** - * Running 8-bit counter that ticks every 100mS. - * - * @return 100mS time tick - */ -uint8_t timeGetTicks() -{ - return timeTicks; -} - /** * Initialize the real-time clock. */ void timeInit() { - timeTicks = 0; - timeInterruptCount = 0; -// time100mS = 0; - timeSeconds = 0; - timeMinutes = 0; - timeHours = 0; - timeCompare = TIME_RATE; - timeUpdateFlag = false; timeNCO = 0x00; timeLowRateCount = 0; timeNCOFreq = 0x2000; - timeRunFlag = false; -} - -/** - * Function return true once a second based on real-time clock. - * - * @return true on one second tick; otherwise false - */ -bool_t timeIsUpdate() -{ - if (timeUpdateFlag) - { - timeUpdateFlag = false; - return true; - } // END if - - return false; -} - -/** - * Set a flag to indicate the flight time should run. This flag is typically set when the payload - * lifts off. - */ -void timeSetRunFlag() -{ - timeRunFlag = true; } /** @@ -321,9 +243,6 @@ void timeSetRunFlag() */ void timeUpdate() { - // Setup the next interrupt for the operational mode. - timeCompare += TIME_RATE; - putchar ((timeNCO >> 8) < 0x80 ? 0xc0 : 0x40); timeNCO += timeNCOFreq; -- cgit v1.2.3 From 74969483736381858484dca9ebb528d9d2d73f5b Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 5 Dec 2012 22:23:46 -0800 Subject: altos: Start restructuring APRS code to create and send packets Signed-off-by: Keith Packard --- src/drivers/ao_aprs.c | 52 +++++++++++++++++++++++-------------------------- src/test/ao_aprs_test.c | 52 +++++++++++++++++++++++++++++++++++++++++++++---- 2 files changed, 72 insertions(+), 32 deletions(-) (limited to 'src') diff --git a/src/drivers/ao_aprs.c b/src/drivers/ao_aprs.c index 7e9013a0..b8d17bd9 100644 --- a/src/drivers/ao_aprs.c +++ b/src/drivers/ao_aprs.c @@ -238,22 +238,6 @@ void timeInit() timeNCOFreq = 0x2000; } -/** - * Timer interrupt handler called every 104uS (9600 times/second). - */ -void timeUpdate() -{ - putchar ((timeNCO >> 8) < 0x80 ? 0xc0 : 0x40); - - timeNCO += timeNCOFreq; - - if (++timeLowRateCount == 8) - { - timeLowRateCount = 0; - tnc1200TimerTick(); - } // END if -} - /** @} */ /** @@ -553,6 +537,28 @@ void tncPositionPacket(void) tncLength += c; } +static int16_t +tncFill(uint8_t *buf, int16_t len) +{ + int16_t l = 0; + uint8_t b; + uint8_t bit; + + while (tncMode != TNC_TX_READY && l < len) { + b = 0; + for (bit = 0; bit < 8; bit++) { + b = b << 1 | (timeNCO >> 15); + timeNCO += timeNCOFreq; + } + *buf++ = b; + l++; + tnc1200TimerTick(); + } + if (tncMode == TNC_TX_READY) + l = -l; + return l; +} + /** * Prepare an AX.25 data packet. Each time this method is called, it automatically * rotates through 1 of 3 messages. @@ -589,19 +595,9 @@ void tncTxPacket(void) tncIndex = 0; tncMode = TNC_TX_SYNC; - // Turn on the PA chain. -// output_high (IO_PTT); - - // Wait for the PA chain to power up. -// delay_ms (10); - - // Key the DDS. -// output_high (IO_OSK); + timeInit(); - // Log the battery and reference voltage just after we key the transmitter. -// sysLogVoltage(); - while (tncMode != TNC_TX_READY) - timeUpdate(); + ao_radio_send_lots(tncFill); } /** @} */ diff --git a/src/test/ao_aprs_test.c b/src/test/ao_aprs_test.c index 93dab577..d350ca0d 100644 --- a/src/test/ao_aprs_test.c +++ b/src/test/ao_aprs_test.c @@ -25,6 +25,27 @@ #define AO_APRS_TEST +typedef int16_t (*ao_radio_fill_func)(uint8_t *buffer, int16_t len); + +#define DEBUG 0 +#if DEBUG +void +ao_aprs_bit(uint8_t bit) +{ + static int seq = 0; + printf ("%6d %d\n", seq++, bit ? 1 : 0); +} +#else +void +ao_aprs_bit(uint8_t bit) +{ + putchar (bit ? 0xc0 : 0x40); +} +#endif + +void +ao_radio_send_lots(ao_radio_fill_func fill); + #include /* @@ -57,10 +78,12 @@ static void audio_gap(int secs) { +#if !DEBUG int samples = secs * 9600; while (samples--) - putchar(0x7f); + ao_aprs_bit(0); +#endif } // This is where we go after reset. @@ -76,6 +99,27 @@ int main(int argc, char **argv) exit(0); } - - - +void +ao_radio_send_lots(ao_radio_fill_func fill) +{ + int16_t len; + uint8_t done = 0; + uint8_t buf[16], *b, c; + uint8_t bit; + + while (!done) { + len = (*fill)(buf, sizeof (buf)); + if (len < 0) { + done = 1; + len = -len; + } + b = buf; + while (len--) { + c = *b++; + for (bit = 0; bit < 8; bit++) { + ao_aprs_bit(c & 0x80); + c <<= 1; + } + } + } +} -- cgit v1.2.3 From 51ef826372f466f44901c4c609ed6a987d30fda4 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 5 Dec 2012 23:39:47 -0800 Subject: altos: Prepare APRS for use within altos itself Make all variables static, const-ify constants, change the public name of the single entry point. Signed-off-by: Keith Packard --- src/drivers/ao_aprs.c | 76 +++++++++++++++++++------------------------------ src/drivers/ao_aprs.h | 3 -- src/test/ao_aprs_test.c | 4 +-- 3 files changed, 30 insertions(+), 53 deletions(-) (limited to 'src') diff --git a/src/drivers/ao_aprs.c b/src/drivers/ao_aprs.c index b8d17bd9..1a074ba5 100644 --- a/src/drivers/ao_aprs.c +++ b/src/drivers/ao_aprs.c @@ -152,23 +152,10 @@ typedef int32_t int32; // Public methods, constants, and data structures for each class. -void ddsInit(); -void ddsSetAmplitude (uint8_t amplitude); -void ddsSetOutputScale (uint16_t amplitude); -void ddsSetFSKFreq (uint32_t ftw0, uint32_t ftw1); -void ddsSetFreq (uint32_t freq); -void ddsSetFTW (uint32_t ftw); +static void timeInit(void); -uint16_t sysCRC16(uint8_t *buffer, uint8_t length, uint16_t crc); - -void timeInit(); -void timeSetDutyCycle (uint8_t dutyCycle); -void timeUpdate(); - -void tncInit(); -void tnc1200TimerTick(); -void tncTxByte (uint8_t value); -void tncTxPacket(void); +static void tncInit(void); +static void tnc1200TimerTick(void); /** @} */ @@ -190,7 +177,7 @@ void tncTxPacket(void); * * @return CRC-16 of buffer[0 .. length] */ -uint16_t sysCRC16(uint8_t *buffer, uint8_t length, uint16_t crc) +static uint16_t sysCRC16(const uint8_t *buffer, uint8_t length, uint16_t crc) { uint8_t i, bit, value; @@ -220,21 +207,17 @@ uint16_t sysCRC16(uint8_t *buffer, uint8_t length, uint16_t crc) */ /// 16-bit NCO where the upper 8-bits are used to index into the frequency generation table. -uint16_t timeNCO; +static uint16_t timeNCO; /// Audio tone NCO update step (phase). -uint16_t timeNCOFreq; - -/// Counter used to deciminate down from the 104uS to 833uS interrupt rate. (9600 to 1200 baud) -uint8_t timeLowRateCount; +static uint16_t timeNCOFreq; /** * Initialize the real-time clock. */ -void timeInit() +static void timeInit() { timeNCO = 0x00; - timeLowRateCount = 0; timeNCOFreq = 0x2000; } @@ -252,7 +235,7 @@ void timeInit() #define TNC_TX_DELAY 45 /// The size of the TNC output buffer. -#define TNC_BUFFER_SIZE 80 +#define TNC_BUFFER_SIZE 40 /// States that define the current mode of the 1200 bps (A-FSK) state machine. typedef enum @@ -275,44 +258,43 @@ typedef enum /// AX.25 compliant packet header that contains destination, station call sign, and path. /// 0x76 for SSID-11, 0x78 for SSID-12 -uint8_t TNC_AX25_HEADER[30] = { - 'A' << 1, 'P' << 1, 'R' << 1, 'S' << 1, ' ' << 1, ' ' << 1, 0x60, \ - 'K' << 1, 'D' << 1, '7' << 1, 'S' << 1, 'Q' << 1, 'G' << 1, 0x76, \ - 'G' << 1, 'A' << 1, 'T' << 1, 'E' << 1, ' ' << 1, ' ' << 1, 0x60, \ - 'W' << 1, 'I' << 1, 'D' << 1, 'E' << 1, '3' << 1, ' ' << 1, 0x67, \ +static const uint8_t TNC_AX25_HEADER[] = { + 'A' << 1, 'P' << 1, 'A' << 1, 'M' << 1, ' ' << 1, ' ' << 1, 0x60, \ + 'K' << 1, 'D' << 1, '7' << 1, 'S' << 1, 'Q' << 1, 'G' << 1, 0x78, \ + 'W' << 1, 'I' << 1, 'D' << 1, 'E' << 1, '2' << 1, ' ' << 1, 0x65, \ 0x03, 0xf0 }; /// The next bit to transmit. -uint8_t tncTxBit; +static uint8_t tncTxBit; /// Current mode of the 1200 bps state machine. -TNC_TX_1200BPS_STATE tncMode; +static TNC_TX_1200BPS_STATE tncMode; /// Counter for each bit (0 - 7) that we are going to transmit. -uint8_t tncBitCount; +static uint8_t tncBitCount; /// A shift register that holds the data byte as we bit shift it for transmit. -uint8_t tncShift; +static uint8_t tncShift; /// Index into the APRS header and data array for each byte as we transmit it. -uint8_t tncIndex; +static uint8_t tncIndex; /// The number of bytes in the message portion of the AX.25 message. -uint8_t tncLength; +static uint8_t tncLength; /// A copy of the last 5 bits we've transmitted to determine if we need to bit stuff on the next bit. -uint8_t tncBitStuff; +static uint8_t tncBitStuff; /// Pointer to TNC buffer as we save each byte during message preparation. -uint8_t *tncBufferPnt; +static uint8_t *tncBufferPnt; /// Buffer to hold the message portion of the AX.25 packet as we prepare it. -uint8_t tncBuffer[TNC_BUFFER_SIZE]; +static uint8_t tncBuffer[TNC_BUFFER_SIZE]; /** * Initialize the TNC internal variables. */ -void tncInit() +static void tncInit() { tncTxBit = 0; tncMode = TNC_TX_READY; @@ -322,7 +304,7 @@ void tncInit() * Method that is called every 833uS to transmit the 1200bps A-FSK data stream. * The provides the pre and postamble as well as the bit stuffed data stream. */ -void tnc1200TimerTick() +static void tnc1200TimerTick() { // Set the A-FSK frequency. if (tncTxBit == 0x00) @@ -487,10 +469,9 @@ void tnc1200TimerTick() } /** - * Generate the plain text position packet. Data is written through the tncTxByte - * callback function + * Generate the plain text position packet. */ -void tncPositionPacket(void) +static void tncPositionPacket(void) { int32_t latitude = 45.4694766 * 10000000; int32_t longitude = -122.7376250 * 10000000; @@ -565,10 +546,13 @@ tncFill(uint8_t *buf, int16_t len) * * @param dataMode enumerated type that specifies 1200bps A-FSK or 9600bps FSK */ -void tncTxPacket(void) +void ao_aprs_send(void) { uint16_t crc; + timeInit(); + tncInit(); + // Set a pointer to our TNC output buffer. tncBufferPnt = tncBuffer; @@ -595,8 +579,6 @@ void tncTxPacket(void) tncIndex = 0; tncMode = TNC_TX_SYNC; - timeInit(); - ao_radio_send_lots(tncFill); } diff --git a/src/drivers/ao_aprs.h b/src/drivers/ao_aprs.h index fe3c6349..a033fa0b 100644 --- a/src/drivers/ao_aprs.h +++ b/src/drivers/ao_aprs.h @@ -21,7 +21,4 @@ void ao_aprs_send(void); -void -ao_aprs_init(void); - #endif /* _AO_APRS_H_ */ diff --git a/src/test/ao_aprs_test.c b/src/test/ao_aprs_test.c index d350ca0d..d0cfb52d 100644 --- a/src/test/ao_aprs_test.c +++ b/src/test/ao_aprs_test.c @@ -89,12 +89,10 @@ audio_gap(int secs) // This is where we go after reset. int main(int argc, char **argv) { - tncInit(); - audio_gap(1); /* Transmit one packet */ - tncTxPacket(); + ao_aprs_send(); exit(0); } -- cgit v1.2.3 From c1e6fa32b856b91afa355cd272d2d7287d3ccca1 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 6 Dec 2012 10:12:11 -0800 Subject: altos: Hook APRS up to the radio This adds an arbitrary-length packet writing function to the radio code. Signed-off-by: Keith Packard --- src/core/ao.h | 5 ++ src/drivers/ao_aprs.c | 48 +++++------- src/drivers/ao_cc1120.c | 175 +++++++++++++++++++++++++++++++++++++------ src/megametrum-v0.1/Makefile | 1 + src/test/ao_aprs_test.c | 6 ++ 5 files changed, 183 insertions(+), 52 deletions(-) (limited to 'src') diff --git a/src/core/ao.h b/src/core/ao.h index 54018b37..d6e27707 100644 --- a/src/core/ao.h +++ b/src/core/ao.h @@ -529,6 +529,11 @@ ao_radio_recv_abort(void); void ao_radio_test(uint8_t on); +typedef int16_t (*ao_radio_fill_func)(uint8_t *buffer, int16_t len); + +void +ao_radio_send_lots(ao_radio_fill_func fill); + /* * Compute the packet length as follows: * diff --git a/src/drivers/ao_aprs.c b/src/drivers/ao_aprs.c index 1a074ba5..e273908f 100644 --- a/src/drivers/ao_aprs.c +++ b/src/drivers/ao_aprs.c @@ -145,11 +145,6 @@ #include -typedef int bool_t; -typedef int32_t int32; -#define false 0 -#define true 1 - // Public methods, constants, and data structures for each class. static void timeInit(void); @@ -285,9 +280,6 @@ static uint8_t tncLength; /// A copy of the last 5 bits we've transmitted to determine if we need to bit stuff on the next bit. static uint8_t tncBitStuff; -/// Pointer to TNC buffer as we save each byte during message preparation. -static uint8_t *tncBufferPnt; - /// Buffer to hold the message portion of the AX.25 packet as we prepare it. static uint8_t tncBuffer[TNC_BUFFER_SIZE]; @@ -471,18 +463,18 @@ static void tnc1200TimerTick() /** * Generate the plain text position packet. */ -static void tncPositionPacket(void) +static int tncPositionPacket(void) { - int32_t latitude = 45.4694766 * 10000000; - int32_t longitude = -122.7376250 * 10000000; - uint32_t altitude = 10000; + int32_t latitude = ao_gps_data.latitude; + int32_t longitude = ao_gps_data.longitude; + int32_t altitude = ao_gps_data.altitude; + uint16_t lat_deg; uint16_t lon_deg; uint16_t lat_min; uint16_t lat_frac; uint16_t lon_min; uint16_t lon_frac; - int c; char lat_sign = 'N', lon_sign = 'E'; @@ -510,12 +502,15 @@ static void tncPositionPacket(void) longitude -= lon_min * 10000000; lon_frac = (longitude + 50000) / 100000; - c = sprintf ((char *) tncBufferPnt, "=%02u%02u.%02u%c\\%03u%02u.%02u%cO /A=%06u\015", - lat_deg, lat_min, lat_frac, lat_sign, - lon_deg, lon_min, lon_frac, lon_sign, - altitude * 100 / 3048); - tncBufferPnt += c; - tncLength += c; + if (altitude < 0) + altitude = 0; + + altitude = altitude * (int32_t) 1000 / (int32_t) 3048; + + return sprintf ((char *) tncBuffer, "=%02u%02u.%02u%c\\%03u%02u.%02u%cO /A=%06u\015", + lat_deg, lat_min, lat_frac, lat_sign, + lon_deg, lon_min, lon_frac, lon_sign, + altitude); } static int16_t @@ -553,24 +548,15 @@ void ao_aprs_send(void) timeInit(); tncInit(); - // Set a pointer to our TNC output buffer. - tncBufferPnt = tncBuffer; - - // Set the message length counter. - tncLength = 0; - - tncPositionPacket(); + tncLength = tncPositionPacket(); // Calculate the CRC for the header and message. crc = sysCRC16(TNC_AX25_HEADER, sizeof(TNC_AX25_HEADER), 0xffff); crc = sysCRC16(tncBuffer, tncLength, crc ^ 0xffff); // Save the CRC in the message. - *tncBufferPnt++ = crc & 0xff; - *tncBufferPnt = (crc >> 8) & 0xff; - - // Update the length to include the CRC bytes. - tncLength += 2; + tncBuffer[tncLength++] = crc & 0xff; + tncBuffer[tncLength++] = (crc >> 8) & 0xff; // Prepare the variables that are used in the real-time clock interrupt. tncBitCount = 0; diff --git a/src/drivers/ao_cc1120.c b/src/drivers/ao_cc1120.c index f27958f9..ed26e28d 100644 --- a/src/drivers/ao_cc1120.c +++ b/src/drivers/ao_cc1120.c @@ -323,12 +323,12 @@ static const uint16_t packet_rx_setup[] = { /* * For our RDF beacon, set the symbol rate to 2kBaud (for a 1kHz tone) * - * (2**20 - DATARATE_M) * 2 ** DATARATE_E + * (2**20 + DATARATE_M) * 2 ** DATARATE_E * Rdata = -------------------------------------- * fosc * 2 ** 39 * - * DATARATE_M = 511705 - * DATARATE_E = 6 + * DATARATE_M = 25166 + * DATARATE_E = 5 * * To make the tone last for 200ms, we need 2000 * .2 = 400 bits or 50 bytes */ @@ -358,7 +358,64 @@ static const uint16_t rdf_setup[] = { (0 << CC1120_PKT_CFG0_UART_SWAP_EN)), }; -static uint8_t ao_radio_mode; +/* + * APRS deviation is 5kHz + * + * fdev = fosc >> 24 * (256 + dev_m) << dev_e + * + * 32e6Hz / (2 ** 24) * (256 + 71) * (2 ** 3) = 4989 + */ + +#define APRS_DEV_E 3 +#define APRS_DEV_M 71 +#define APRS_PACKET_LEN 50 + +/* + * For our APRS beacon, set the symbol rate to 9.6kBaud (8x oversampling for 1200 baud data rate) + * + * (2**20 + DATARATE_M) * 2 ** DATARATE_E + * Rdata = -------------------------------------- * fosc + * 2 ** 39 + * + * DATARATE_M = 239914 + * DATARATE_E = 7 + * + * Rdata = 9599.998593330383301 + * + */ +#define APRS_DRATE_E 5 +#define APRS_DRATE_M 25166 + +static const uint16_t aprs_setup[] = { + CC1120_DEVIATION_M, APRS_DEV_M, + CC1120_MODCFG_DEV_E, ((CC1120_MODCFG_DEV_E_MODEM_MODE_NORMAL << CC1120_MODCFG_DEV_E_MODEM_MODE) | + (CC1120_MODCFG_DEV_E_MOD_FORMAT_2_GFSK << CC1120_MODCFG_DEV_E_MOD_FORMAT) | + (APRS_DEV_E << CC1120_MODCFG_DEV_E_DEV_E)), + CC1120_DRATE2, ((APRS_DRATE_E << CC1120_DRATE2_DATARATE_E) | + (((APRS_DRATE_M >> 16) & CC1120_DRATE2_DATARATE_M_19_16_MASK) << CC1120_DRATE2_DATARATE_M_19_16)), + CC1120_DRATE1, ((APRS_DRATE_M >> 8) & 0xff), + CC1120_DRATE0, ((APRS_DRATE_M >> 0) & 0xff), + CC1120_PKT_CFG2, ((CC1120_PKT_CFG2_CCA_MODE_ALWAYS_CLEAR << CC1120_PKT_CFG2_CCA_MODE) | + (CC1120_PKT_CFG2_PKT_FORMAT_NORMAL << CC1120_PKT_CFG2_PKT_FORMAT)), + CC1120_PKT_CFG1, ((0 << CC1120_PKT_CFG1_WHITE_DATA) | + (CC1120_PKT_CFG1_ADDR_CHECK_CFG_NONE << CC1120_PKT_CFG1_ADDR_CHECK_CFG) | + (CC1120_PKT_CFG1_CRC_CFG_DISABLED << CC1120_PKT_CFG1_CRC_CFG) | + (0 << CC1120_PKT_CFG1_APPEND_STATUS)), +}; + +#define AO_PKT_CFG0_INFINITE ((0 << CC1120_PKT_CFG0_RESERVED7) | \ + (CC1120_PKT_CFG0_LENGTH_CONFIG_INFINITE << CC1120_PKT_CFG0_LENGTH_CONFIG) | \ + (0 << CC1120_PKT_CFG0_PKG_BIT_LEN) | \ + (0 << CC1120_PKT_CFG0_UART_MODE_EN) | \ + (0 << CC1120_PKT_CFG0_UART_SWAP_EN)) + +#define AO_PKT_CFG0_FIXED ((0 << CC1120_PKT_CFG0_RESERVED7) | \ + (CC1120_PKT_CFG0_LENGTH_CONFIG_FIXED << CC1120_PKT_CFG0_LENGTH_CONFIG) | \ + (0 << CC1120_PKT_CFG0_PKG_BIT_LEN) | \ + (0 << CC1120_PKT_CFG0_UART_MODE_EN) | \ + (0 << CC1120_PKT_CFG0_UART_SWAP_EN)) + +static uint16_t ao_radio_mode; #define AO_RADIO_MODE_BITS_PACKET 1 #define AO_RADIO_MODE_BITS_PACKET_TX 2 @@ -366,17 +423,22 @@ static uint8_t ao_radio_mode; #define AO_RADIO_MODE_BITS_TX_FINISH 8 #define AO_RADIO_MODE_BITS_PACKET_RX 16 #define AO_RADIO_MODE_BITS_RDF 32 +#define AO_RADIO_MODE_BITS_APRS 64 +#define AO_RADIO_MODE_BITS_INFINITE 128 +#define AO_RADIO_MODE_BITS_FIXED 256 #define AO_RADIO_MODE_NONE 0 #define AO_RADIO_MODE_PACKET_TX_BUF (AO_RADIO_MODE_BITS_PACKET | AO_RADIO_MODE_BITS_PACKET_TX | AO_RADIO_MODE_BITS_TX_BUF) #define AO_RADIO_MODE_PACKET_TX_FINISH (AO_RADIO_MODE_BITS_PACKET | AO_RADIO_MODE_BITS_PACKET_TX | AO_RADIO_MODE_BITS_TX_FINISH) #define AO_RADIO_MODE_PACKET_RX (AO_RADIO_MODE_BITS_PACKET | AO_RADIO_MODE_BITS_PACKET_RX) #define AO_RADIO_MODE_RDF (AO_RADIO_MODE_BITS_RDF | AO_RADIO_MODE_BITS_TX_FINISH) +#define AO_RADIO_MODE_APRS_BUF (AO_RADIO_MODE_BITS_APRS | AO_RADIO_MODE_BITS_INFINITE) +#define AO_RADIO_MODE_APRS_FINISH (AO_RADIO_MODE_BITS_APRS | AO_RADIO_MODE_BITS_FIXED) static void -ao_radio_set_mode(uint8_t new_mode) +ao_radio_set_mode(uint16_t new_mode) { - uint8_t changes; + uint16_t changes; int i; if (new_mode == ao_radio_mode) @@ -404,6 +466,17 @@ ao_radio_set_mode(uint8_t new_mode) if (changes & AO_RADIO_MODE_BITS_RDF) for (i = 0; i < sizeof (rdf_setup) / sizeof (rdf_setup[0]); i += 2) ao_radio_reg_write(rdf_setup[i], rdf_setup[i+1]); + + if (changes & AO_RADIO_MODE_BITS_APRS) + for (i = 0; i < sizeof (aprs_setup) / sizeof (aprs_setup[0]); i += 2) + ao_radio_reg_write(aprs_setup[i], aprs_setup[i+1]); + + if (changes & AO_RADIO_MODE_BITS_INFINITE) + ao_radio_reg_write(CC1120_PKT_CFG0, AO_PKT_CFG0_INFINITE); + + if (changes & AO_RADIO_MODE_BITS_FIXED) + ao_radio_reg_write(CC1120_PKT_CFG0, AO_PKT_CFG0_FIXED); + ao_radio_mode = new_mode; } @@ -430,11 +503,21 @@ ao_radio_setup(void) ao_radio_configured = 1; } +static void +ao_radio_set_len(uint8_t len) +{ + static uint8_t last_len; + + if (len != last_len) { + ao_radio_reg_write(CC1120_PKT_LEN, len); + last_len = len; + } +} + static void ao_radio_get(uint8_t len) { static uint32_t last_radio_setting; - static uint8_t last_len; ao_mutex_get(&ao_radio_mutex); if (!ao_radio_configured) @@ -445,10 +528,7 @@ ao_radio_get(uint8_t len) ao_radio_reg_write(CC1120_FREQ0, ao_config.radio_setting); last_radio_setting = ao_config.radio_setting; } - if (len != last_len) { - ao_radio_reg_write(CC1120_PKT_LEN, len); - last_len = len; - } + ao_radio_set_len(len); } #define ao_radio_put() ao_mutex_put(&ao_radio_mutex) @@ -562,6 +642,24 @@ ao_radio_test_cmd(void) } } +static uint8_t +ao_radio_wait_tx(uint8_t wait_fifo) +{ + uint8_t fifo_space = 0; + + do { + ao_radio_wake = 0; + ao_arch_block_interrupts(); + while (!ao_radio_wake) + ao_sleep(&ao_radio_wake); + ao_arch_release_interrupts(); + if (!wait_fifo) + return 0; + fifo_space = ao_radio_tx_fifo_space(); + } while (!fifo_space); + return fifo_space; +} + static uint8_t tx_data[(AO_RADIO_MAX_SEND + 4) * 2]; void @@ -601,16 +699,51 @@ ao_radio_send(const void *d, uint8_t size) ao_exti_enable(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN); } - do { - ao_radio_wake = 0; - ao_arch_block_interrupts(); - while (!ao_radio_wake) - ao_sleep(&ao_radio_wake); - ao_arch_release_interrupts(); - if (!encode_len) - break; - fifo_space = ao_radio_tx_fifo_space(); - } while (!fifo_space); + fifo_space = ao_radio_wait_tx(encode_len != 0); + } + ao_radio_put(); +} + +#define AO_RADIO_LOTS 64 + +void +ao_radio_send_lots(ao_radio_fill_func fill) +{ + uint8_t buf[AO_RADIO_LOTS], *b; + int cnt; + int total = 0; + uint8_t done = 0; + uint8_t started = 0; + uint8_t fifo_space; + + ao_radio_get(0xff); + fifo_space = CC1120_FIFO_SIZE; + while (!done) { + cnt = (*fill)(buf, sizeof(buf)); + if (cnt < 0) { + done = 1; + cnt = -cnt; + } + total += cnt; + if (done) { + ao_radio_set_len(total & 0xff); + ao_radio_set_mode(AO_RADIO_MODE_APRS_FINISH); + } else + ao_radio_set_mode(AO_RADIO_MODE_APRS_BUF); + b = buf; + while (cnt) { + uint8_t this_len = cnt; + if (this_len > fifo_space) + this_len = fifo_space; + ao_radio_fifo_write(b, this_len); + b += this_len; + cnt -= this_len; + if (!started) { + ao_radio_start_tx(); + started = 1; + } + fifo_space = ao_radio_wait_tx(!done || cnt); + } } ao_radio_put(); } diff --git a/src/megametrum-v0.1/Makefile b/src/megametrum-v0.1/Makefile index 7d6c7388..25d0ed03 100644 --- a/src/megametrum-v0.1/Makefile +++ b/src/megametrum-v0.1/Makefile @@ -90,6 +90,7 @@ ALTOS_SRC = \ ao_packet.c \ ao_companion.c \ ao_pyro.c \ + ao_aprs.c \ $(PROFILE) \ $(SAMPLE_PROFILE) \ $(STACK_GUARD) diff --git a/src/test/ao_aprs_test.c b/src/test/ao_aprs_test.c index d0cfb52d..f16c94e8 100644 --- a/src/test/ao_aprs_test.c +++ b/src/test/ao_aprs_test.c @@ -23,6 +23,8 @@ #include +struct ao_telemetry_location ao_gps_data; + #define AO_APRS_TEST typedef int16_t (*ao_radio_fill_func)(uint8_t *buffer, int16_t len); @@ -91,6 +93,10 @@ int main(int argc, char **argv) { audio_gap(1); + ao_gps_data.latitude = 45.4694766 * 10000000; + ao_gps_data.longitude = -122.7376250 * 10000000; + ao_gps_data.altitude = 83; + /* Transmit one packet */ ao_aprs_send(); -- cgit v1.2.3 From f661da527fb4a3a492f5322e2a718d441e1cde83 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 6 Dec 2012 10:23:39 -0800 Subject: altos: Hook up APRS to telemetry loop Send APRS packet once every 2 seconds Signed-off-by: Keith Packard --- src/core/ao_telemetry.c | 21 ++++++++++++++++++++- src/drivers/ao_aprs.h | 2 ++ src/megametrum-v0.1/ao_pins.h | 1 + 3 files changed, 23 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/core/ao_telemetry.c b/src/core/ao_telemetry.c index 52ac9489..79d1bb81 100644 --- a/src/core/ao_telemetry.c +++ b/src/core/ao_telemetry.c @@ -22,6 +22,12 @@ static __pdata uint16_t ao_telemetry_interval; static __pdata uint8_t ao_rdf = 0; static __pdata uint16_t ao_rdf_time; +#if HAS_APRS +static __pdata uint16_t ao_aprs_time; + +#include +#endif + #if defined(MEGAMETRUM) #define AO_SEND_MEGA 1 #endif @@ -288,6 +294,9 @@ ao_telemetry(void) while (ao_telemetry_interval == 0) ao_sleep(&telemetry); time = ao_rdf_time = ao_time(); +#if HAS_APRS + ao_aprs_time = time; +#endif while (ao_telemetry_interval) { @@ -325,6 +334,12 @@ ao_telemetry(void) #endif ao_radio_rdf(); } +#if HAS_APRS + if (ao_rdf && (int16_t) (ao_time() - ao_aprs_time) >= 0) { + ao_aprs_time = ao_time() + AO_APRS_INTERVAL_TICKS; + ao_aprs_send(); + } +#endif #endif time += ao_telemetry_interval; delay = time - ao_time(); @@ -389,8 +404,12 @@ ao_rdf_set(uint8_t rdf) ao_rdf = rdf; if (rdf == 0) ao_radio_rdf_abort(); - else + else { ao_rdf_time = ao_time() + AO_RDF_INTERVAL_TICKS; +#if HAS_APRS + ao_aprs_time = ao_time() + AO_APRS_INTERVAL_TICKS; +#endif + } } __xdata struct ao_task ao_telemetry_task; diff --git a/src/drivers/ao_aprs.h b/src/drivers/ao_aprs.h index a033fa0b..e00dd75b 100644 --- a/src/drivers/ao_aprs.h +++ b/src/drivers/ao_aprs.h @@ -18,6 +18,8 @@ #ifndef _AO_APRS_H_ #define _AO_APRS_H_ +#define AO_APRS_INTERVAL_TICKS AO_SEC_TO_TICKS(2) + void ao_aprs_send(void); diff --git a/src/megametrum-v0.1/ao_pins.h b/src/megametrum-v0.1/ao_pins.h index f07dc26e..083c1b6f 100644 --- a/src/megametrum-v0.1/ao_pins.h +++ b/src/megametrum-v0.1/ao_pins.h @@ -70,6 +70,7 @@ #define HAS_BEEP 1 #define HAS_RADIO 1 #define HAS_TELEMETRY 1 +#define HAS_APRS 1 #define HAS_SPI_1 1 #define SPI_1_PA5_PA6_PA7 1 /* Barometer */ -- cgit v1.2.3 From 1f84c0adbfa494ddc7dbe276796d999560be9438 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 6 Dec 2012 10:28:14 -0800 Subject: altos: Allow telemetry, rdf and APRS to be individually controlled But, only when APRS is available so that TeleMetrum and TeleMini don't change behaviour Signed-off-by: Keith Packard --- src/core/ao.h | 5 +++++ src/core/ao_config.c | 2 +- src/core/ao_telemetry.c | 31 +++++++++++++++++++++---------- 3 files changed, 27 insertions(+), 11 deletions(-) (limited to 'src') diff --git a/src/core/ao.h b/src/core/ao.h index d6e27707..fa873efe 100644 --- a/src/core/ao.h +++ b/src/core/ao.h @@ -717,6 +717,11 @@ struct ao_config { #define AO_IGNITE_MODE_APOGEE 1 #define AO_IGNITE_MODE_MAIN 2 +#define AO_RADIO_ENABLE_CORE 1 +#define AO_RADIO_ENABLE_APRS 2 +#define AO_RADIO_DISABLE_TELEMETRY 4 +#define AO_RADIO_DISABLE_RDF 8 + #define AO_PAD_ORIENTATION_ANTENNA_UP 0 #define AO_PAD_ORIENTATION_ANTENNA_DOWN 1 diff --git a/src/core/ao_config.c b/src/core/ao_config.c index e85ddcb4..df40ff90 100644 --- a/src/core/ao_config.c +++ b/src/core/ao_config.c @@ -128,7 +128,7 @@ _ao_config_get(void) if (minor < 6) ao_config.pad_orientation = AO_CONFIG_DEFAULT_PAD_ORIENTATION; if (minor < 8) - ao_config.radio_enable = TRUE; + ao_config.radio_enable = AO_RADIO_ENABLE_CORE; if (minor < 9) ao_xmemset(&ao_config.aes_key, '\0', AO_AES_LEN); if (minor < 10) diff --git a/src/core/ao_telemetry.c b/src/core/ao_telemetry.c index 79d1bb81..4ff98f9b 100644 --- a/src/core/ao_telemetry.c +++ b/src/core/ao_telemetry.c @@ -300,27 +300,35 @@ ao_telemetry(void) while (ao_telemetry_interval) { +#if HAS_APRS + if (!(ao_config.radio_enable & AO_RADIO_DISABLE_TELEMETRY)) +#endif + { #ifdef AO_SEND_ALL_BARO - ao_send_baro(); + ao_send_baro(); #endif #ifdef AO_SEND_MEGA - ao_send_mega_sensor(); - ao_send_mega_data(); + ao_send_mega_sensor(); + ao_send_mega_data(); #else - ao_send_sensor(); + ao_send_sensor(); #endif #if HAS_COMPANION - if (ao_companion_running) - ao_send_companion(); + if (ao_companion_running) + ao_send_companion(); #endif - ao_send_configuration(); + ao_send_configuration(); #if HAS_GPS - ao_send_location(); - ao_send_satellite(); + ao_send_location(); + ao_send_satellite(); #endif + } #ifndef AO_SEND_ALL_BARO if (ao_rdf && +#if HAS_APRS + !(ao_config.radio_enable & AO_RADIO_DISABLE_RDF) && +#endif (int16_t) (ao_time() - ao_rdf_time) >= 0) { #if HAS_IGNITE_REPORT @@ -335,7 +343,10 @@ ao_telemetry(void) ao_radio_rdf(); } #if HAS_APRS - if (ao_rdf && (int16_t) (ao_time() - ao_aprs_time) >= 0) { + if (ao_rdf && + (ao_config.radio_enable & AO_RADIO_ENABLE_APRS) && + (int16_t) (ao_time() - ao_aprs_time) >= 0) + { ao_aprs_time = ao_time() + AO_APRS_INTERVAL_TICKS; ao_aprs_send(); } -- cgit v1.2.3 From 75912f8af04cecc0bbffecb2072d465c3744d4e8 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 6 Dec 2012 10:30:46 -0800 Subject: altos: Send APRS packets even during ascent If you're using APRS, presumably you want to watch the rocket going up too. Signed-off-by: Keith Packard --- src/core/ao_telemetry.c | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) (limited to 'src') diff --git a/src/core/ao_telemetry.c b/src/core/ao_telemetry.c index 4ff98f9b..cfc72e04 100644 --- a/src/core/ao_telemetry.c +++ b/src/core/ao_telemetry.c @@ -343,8 +343,7 @@ ao_telemetry(void) ao_radio_rdf(); } #if HAS_APRS - if (ao_rdf && - (ao_config.radio_enable & AO_RADIO_ENABLE_APRS) && + if ((ao_config.radio_enable & AO_RADIO_ENABLE_APRS) && (int16_t) (ao_time() - ao_aprs_time) >= 0) { ao_aprs_time = ao_time() + AO_APRS_INTERVAL_TICKS; @@ -417,9 +416,6 @@ ao_rdf_set(uint8_t rdf) ao_radio_rdf_abort(); else { ao_rdf_time = ao_time() + AO_RDF_INTERVAL_TICKS; -#if HAS_APRS - ao_aprs_time = ao_time() + AO_APRS_INTERVAL_TICKS; -#endif } } -- cgit v1.2.3 From f8a704268f0978a39b9c7983e049ef55914f7280 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 7 Dec 2012 10:15:25 -0800 Subject: altos: Fix up APRS packet sending code in cc1120 driver This fixes the FIFO management, ensuring that the data are streamed into the radio fast enough to keep the packet continuous. Sounds like it works, but testing with an actual APRS receiver is required. Signed-off-by: Keith Packard --- src/drivers/ao_cc1120.c | 73 +++++++++++++++++++++++++++++++++++++------------ 1 file changed, 56 insertions(+), 17 deletions(-) (limited to 'src') diff --git a/src/drivers/ao_cc1120.c b/src/drivers/ao_cc1120.c index ed26e28d..7654af85 100644 --- a/src/drivers/ao_cc1120.c +++ b/src/drivers/ao_cc1120.c @@ -261,7 +261,7 @@ ao_radio_idle(void) #define PACKET_DEV_M 80 /* - * For our packet data, set the symbol rate to 38360 Baud + * For our packet data, set the symbol rate to 38400 Baud * * (2**20 + DATARATE_M) * 2 ** DATARATE_E * Rdata = -------------------------------------- * fosc @@ -383,8 +383,8 @@ static const uint16_t rdf_setup[] = { * Rdata = 9599.998593330383301 * */ -#define APRS_DRATE_E 5 -#define APRS_DRATE_M 25166 +#define APRS_DRATE_E 7 +#define APRS_DRATE_M 239914 static const uint16_t aprs_setup[] = { CC1120_DEVIATION_M, APRS_DEV_M, @@ -432,8 +432,9 @@ static uint16_t ao_radio_mode; #define AO_RADIO_MODE_PACKET_TX_FINISH (AO_RADIO_MODE_BITS_PACKET | AO_RADIO_MODE_BITS_PACKET_TX | AO_RADIO_MODE_BITS_TX_FINISH) #define AO_RADIO_MODE_PACKET_RX (AO_RADIO_MODE_BITS_PACKET | AO_RADIO_MODE_BITS_PACKET_RX) #define AO_RADIO_MODE_RDF (AO_RADIO_MODE_BITS_RDF | AO_RADIO_MODE_BITS_TX_FINISH) -#define AO_RADIO_MODE_APRS_BUF (AO_RADIO_MODE_BITS_APRS | AO_RADIO_MODE_BITS_INFINITE) -#define AO_RADIO_MODE_APRS_FINISH (AO_RADIO_MODE_BITS_APRS | AO_RADIO_MODE_BITS_FIXED) +#define AO_RADIO_MODE_APRS_BUF (AO_RADIO_MODE_BITS_APRS | AO_RADIO_MODE_BITS_INFINITE | AO_RADIO_MODE_BITS_TX_BUF) +#define AO_RADIO_MODE_APRS_LAST_BUF (AO_RADIO_MODE_BITS_APRS | AO_RADIO_MODE_BITS_FIXED | AO_RADIO_MODE_BITS_TX_BUF) +#define AO_RADIO_MODE_APRS_FINISH (AO_RADIO_MODE_BITS_APRS | AO_RADIO_MODE_BITS_FIXED | AO_RADIO_MODE_BITS_TX_FINISH) static void ao_radio_set_mode(uint16_t new_mode) @@ -642,17 +643,23 @@ ao_radio_test_cmd(void) } } +static void +ao_radio_wait_isr(void) +{ + ao_radio_wake = 0; + ao_arch_block_interrupts(); + while (!ao_radio_wake) + ao_sleep(&ao_radio_wake); + ao_arch_release_interrupts(); +} + static uint8_t ao_radio_wait_tx(uint8_t wait_fifo) { uint8_t fifo_space = 0; do { - ao_radio_wake = 0; - ao_arch_block_interrupts(); - while (!ao_radio_wake) - ao_sleep(&ao_radio_wake); - ao_arch_release_interrupts(); + ao_radio_wait_isr(); if (!wait_fifo) return 0; fifo_space = ao_radio_tx_fifo_space(); @@ -725,25 +732,47 @@ ao_radio_send_lots(ao_radio_fill_func fill) cnt = -cnt; } total += cnt; - if (done) { + + /* At the last buffer, set the total length */ + if (done) ao_radio_set_len(total & 0xff); - ao_radio_set_mode(AO_RADIO_MODE_APRS_FINISH); - } else - ao_radio_set_mode(AO_RADIO_MODE_APRS_BUF); + b = buf; while (cnt) { uint8_t this_len = cnt; + + /* Wait for some space in the fifo */ + while ((fifo_space = ao_radio_tx_fifo_space()) == 0) { + ao_radio_wake = 0; + ao_arch_block_interrupts(); + while (!ao_radio_wake) + ao_sleep(&ao_radio_wake); + ao_arch_release_interrupts(); + } if (this_len > fifo_space) this_len = fifo_space; + + cnt -= this_len; + + if (done) { + if (cnt) + ao_radio_set_mode(AO_RADIO_MODE_APRS_LAST_BUF); + else + ao_radio_set_mode(AO_RADIO_MODE_APRS_FINISH); + } else + ao_radio_set_mode(AO_RADIO_MODE_APRS_BUF); + ao_radio_fifo_write(b, this_len); b += this_len; - cnt -= this_len; + if (!started) { ao_radio_start_tx(); started = 1; - } - fifo_space = ao_radio_wait_tx(!done || cnt); + } else + ao_exti_enable(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN); } + /* Wait for the transmitter to go idle */ + ao_radio_wait_isr(); } ao_radio_put(); } @@ -1140,11 +1169,21 @@ ao_radio_test_recv() } } +#include + +static void +ao_radio_aprs() +{ + ao_packet_slave_stop(); + ao_aprs_send(); +} + #endif static const struct ao_cmds ao_radio_cmds[] = { { ao_radio_test_cmd, "C <1 start, 0 stop, none both>\0Radio carrier test" }, #if CC1120_DEBUG + { ao_radio_aprs, "G\0Send APRS packet" }, { ao_radio_show, "R\0Show CC1120 status" }, { ao_radio_beep, "b\0Emit an RDF beacon" }, { ao_radio_packet, "p\0Send a test packet" }, -- cgit v1.2.3 From b28323ce91d23db5e1c3cbd1309c72aafcfbe235 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 7 Dec 2012 17:18:32 -0800 Subject: altos: Make APRS interval configurable This provides a separate configuration value for APRS, allowing the interval between APRS reports to vary. Signed-off-by: Keith Packard --- src/core/ao.h | 8 ++++---- src/core/ao_config.c | 27 +++++++++++++++++++++++++++ src/core/ao_telemetry.c | 5 ++--- src/drivers/ao_aprs.h | 2 -- 4 files changed, 33 insertions(+), 9 deletions(-) (limited to 'src') diff --git a/src/core/ao.h b/src/core/ao.h index fa873efe..df5bbf48 100644 --- a/src/core/ao.h +++ b/src/core/ao.h @@ -684,7 +684,7 @@ extern __xdata uint8_t ao_force_freq; #endif #define AO_CONFIG_MAJOR 1 -#define AO_CONFIG_MINOR 12 +#define AO_CONFIG_MINOR 13 #define AO_AES_LEN 16 @@ -711,6 +711,7 @@ struct ao_config { #if AO_PYRO_NUM struct ao_pyro pyro[AO_PYRO_NUM]; /* minor version 12 */ #endif + uint16_t aprs_interval; /* minor version 13 */ }; #define AO_IGNITE_MODE_DUAL 0 @@ -718,9 +719,8 @@ struct ao_config { #define AO_IGNITE_MODE_MAIN 2 #define AO_RADIO_ENABLE_CORE 1 -#define AO_RADIO_ENABLE_APRS 2 -#define AO_RADIO_DISABLE_TELEMETRY 4 -#define AO_RADIO_DISABLE_RDF 8 +#define AO_RADIO_DISABLE_TELEMETRY 2 +#define AO_RADIO_DISABLE_RDF 4 #define AO_PAD_ORIENTATION_ANTENNA_UP 0 #define AO_PAD_ORIENTATION_ANTENNA_DOWN 1 diff --git a/src/core/ao_config.c b/src/core/ao_config.c index 63158158..0aac16a6 100644 --- a/src/core/ao_config.c +++ b/src/core/ao_config.c @@ -139,6 +139,8 @@ _ao_config_get(void) if (minor < 12) memset(&ao_config.pyro, '\0', sizeof (ao_config.pyro)); #endif + if (minor < 13) + ao_config.aprs_interval = 0; ao_config.minor = AO_CONFIG_MINOR; ao_config_dirty = 1; } @@ -498,6 +500,27 @@ ao_config_key_set(void) __reentrant } #endif +#if HAS_APRS + +void +ao_config_aprs_show(void) +{ + printf ("APRS interval: %d\n", ao_config.aprs_interval); +} + +void +ao_config_aprs_set(void) +{ + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + _ao_config_edit_start(); + ao_config.aprs_interval = ao_cmd_lex_i; + _ao_config_edit_finish(); +} + +#endif /* HAS_APRS */ + struct ao_config_var { __code char *str; void (*set)(void) __reentrant; @@ -553,6 +576,10 @@ __code struct ao_config_var ao_config_vars[] = { #if AO_PYRO_NUM { "P \0Configure pyro channels", ao_pyro_set, ao_pyro_show }, +#endif +#if HAS_APRS + { "A \0APRS packet interval (0 disable)", + ao_config_aprs_set, ao_config_aprs_show }, #endif { "s\0Show", ao_config_show, 0 }, diff --git a/src/core/ao_telemetry.c b/src/core/ao_telemetry.c index cfc72e04..8d440e15 100644 --- a/src/core/ao_telemetry.c +++ b/src/core/ao_telemetry.c @@ -299,7 +299,6 @@ ao_telemetry(void) #endif while (ao_telemetry_interval) { - #if HAS_APRS if (!(ao_config.radio_enable & AO_RADIO_DISABLE_TELEMETRY)) #endif @@ -343,10 +342,10 @@ ao_telemetry(void) ao_radio_rdf(); } #if HAS_APRS - if ((ao_config.radio_enable & AO_RADIO_ENABLE_APRS) && + if (ao_config.aprs_interval != 0 && (int16_t) (ao_time() - ao_aprs_time) >= 0) { - ao_aprs_time = ao_time() + AO_APRS_INTERVAL_TICKS; + ao_aprs_time = ao_time() + AO_SEC_TO_TICKS(ao_config.aprs_interval); ao_aprs_send(); } #endif diff --git a/src/drivers/ao_aprs.h b/src/drivers/ao_aprs.h index e00dd75b..a033fa0b 100644 --- a/src/drivers/ao_aprs.h +++ b/src/drivers/ao_aprs.h @@ -18,8 +18,6 @@ #ifndef _AO_APRS_H_ #define _AO_APRS_H_ -#define AO_APRS_INTERVAL_TICKS AO_SEC_TO_TICKS(2) - void ao_aprs_send(void); -- cgit v1.2.3 From 1f797066857b171b19829e2bb7187b8faf37d07c Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 7 Dec 2012 17:20:02 -0800 Subject: altos: Use configured callsign in APRS packets Instead of hard-coding my own call sign... Signed-off-by: Keith Packard --- src/drivers/ao_aprs.c | 23 +++++++++++++++++++++-- 1 file changed, 21 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/ao_aprs.c b/src/drivers/ao_aprs.c index e273908f..e3abe52e 100644 --- a/src/drivers/ao_aprs.c +++ b/src/drivers/ao_aprs.c @@ -253,12 +253,30 @@ typedef enum /// AX.25 compliant packet header that contains destination, station call sign, and path. /// 0x76 for SSID-11, 0x78 for SSID-12 -static const uint8_t TNC_AX25_HEADER[] = { +static uint8_t TNC_AX25_HEADER[] = { 'A' << 1, 'P' << 1, 'A' << 1, 'M' << 1, ' ' << 1, ' ' << 1, 0x60, \ - 'K' << 1, 'D' << 1, '7' << 1, 'S' << 1, 'Q' << 1, 'G' << 1, 0x78, \ + 'N' << 1, '0' << 1, 'C' << 1, 'A' << 1, 'L' << 1, 'L' << 1, 0x78, \ 'W' << 1, 'I' << 1, 'D' << 1, 'E' << 1, '2' << 1, ' ' << 1, 0x65, \ 0x03, 0xf0 }; +#define TNC_CALLSIGN_OFF 7 +#define TNC_CALLSIGN_LEN 6 + +static void +tncSetCallsign(void) +{ + uint8_t i; + + for (i = 0; i < TNC_CALLSIGN_LEN; i++) { + if (!ao_config.callsign[i]) + break; + TNC_AX25_HEADER[TNC_CALLSIGN_OFF + i] = ao_config.callsign[i] << 1; + } + for (; i < TNC_CALLSIGN_LEN; i++) + TNC_AX25_HEADER[TNC_CALLSIGN_OFF + i] = ' ' << 1; + +} + /// The next bit to transmit. static uint8_t tncTxBit; @@ -547,6 +565,7 @@ void ao_aprs_send(void) timeInit(); tncInit(); + tncSetCallsign(); tncLength = tncPositionPacket(); -- cgit v1.2.3 From 4339d5c8e6373119e5377fe5c883b6b0e6ce37f6 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 7 Dec 2012 17:38:17 -0800 Subject: altos: Fix aprs test to not allow callsign configuration There's no configuration to take a callsign from... Signed-off-by: Keith Packard --- src/drivers/ao_aprs.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/ao_aprs.c b/src/drivers/ao_aprs.c index e3abe52e..93c4af3f 100644 --- a/src/drivers/ao_aprs.c +++ b/src/drivers/ao_aprs.c @@ -265,6 +265,7 @@ static uint8_t TNC_AX25_HEADER[] = { static void tncSetCallsign(void) { +#ifndef AO_APRS_TEST uint8_t i; for (i = 0; i < TNC_CALLSIGN_LEN; i++) { @@ -274,7 +275,7 @@ tncSetCallsign(void) } for (; i < TNC_CALLSIGN_LEN; i++) TNC_AX25_HEADER[TNC_CALLSIGN_OFF + i] = ' ' << 1; - +#endif } /// The next bit to transmit. -- cgit v1.2.3 From cf47efdc86f0b421fcf4389669fbecf6fa3f5934 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 7 Dec 2012 22:49:34 -0800 Subject: altos: Stop including profiling and stack guard code in megametrum These take CPU time and memory and are intended only for debugging Signed-off-by: Keith Packard --- src/megametrum-v0.1/Makefile | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) (limited to 'src') diff --git a/src/megametrum-v0.1/Makefile b/src/megametrum-v0.1/Makefile index 25d0ed03..a5fdcbb2 100644 --- a/src/megametrum-v0.1/Makefile +++ b/src/megametrum-v0.1/Makefile @@ -37,12 +37,12 @@ INC = \ #PROFILE=ao_profile.c #PROFILE_DEF=-DAO_PROFILE=1 -SAMPLE_PROFILE=ao_sample_profile.c \ - ao_sample_profile_timer.c -SAMPLE_PROFILE_DEF=-DHAS_SAMPLE_PROFILE=1 +#SAMPLE_PROFILE=ao_sample_profile.c \ +# ao_sample_profile_timer.c +#SAMPLE_PROFILE_DEF=-DHAS_SAMPLE_PROFILE=1 -STACK_GUARD=ao_mpu_stm.c -STACK_GUARD_DEF=-DHAS_STACK_GUARD=1 +#STACK_GUARD=ao_mpu_stm.c +#STACK_GUARD_DEF=-DHAS_STACK_GUARD=1 ALTOS_SRC = \ ao_interrupt.c \ -- cgit v1.2.3 From 6b4cfd8719e3fd4a2904369e176182c870a3b43c Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 16 Dec 2012 13:29:31 -0800 Subject: altos: Round APRS data correctly Apply rounding once at the start of the computation, then truncate after that. Signed-off-by: Keith Packard --- src/drivers/ao_aprs.c | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/drivers/ao_aprs.c b/src/drivers/ao_aprs.c index 93c4af3f..03bcab05 100644 --- a/src/drivers/ao_aprs.c +++ b/src/drivers/ao_aprs.c @@ -507,24 +507,32 @@ static int tncPositionPacket(void) longitude = -longitude; } + /* Round latitude and longitude by 0.005 minutes */ + latitude = latitude + 833; + if (latitude > 900000000) + latitude = 900000000; + longitude = longitude + 833; + if (longitude > 1800000000) + longitude = 1800000000; + lat_deg = latitude / 10000000; latitude -= lat_deg * 10000000; latitude *= 60; lat_min = latitude / 10000000; latitude -= lat_min * 10000000; - lat_frac = (latitude + 50000) / 100000; + lat_frac = latitude / 100000; lon_deg = longitude / 10000000; longitude -= lon_deg * 10000000; longitude *= 60; lon_min = longitude / 10000000; longitude -= lon_min * 10000000; - lon_frac = (longitude + 50000) / 100000; + lon_frac = longitude / 100000; if (altitude < 0) altitude = 0; - altitude = altitude * (int32_t) 1000 / (int32_t) 3048; + altitude = (altitude * (int32_t) 10000 + (3048/2)) / (int32_t) 3048; return sprintf ((char *) tncBuffer, "=%02u%02u.%02u%c\\%03u%02u.%02u%cO /A=%06u\015", lat_deg, lat_min, lat_frac, lat_sign, -- cgit v1.2.3 From 9bc701ce1132f04ec90ef22e6a7a90c67918737b Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 16 Dec 2012 13:30:20 -0800 Subject: altos: Document what HAS_BOOT_RADIO does in the m25 driver HAS_BOOT_RADIO causes the m25 driver to abort any ongoing receive in case that is holding the SPI bus. Signed-off-by: Keith Packard --- src/drivers/ao_m25.c | 1 + 1 file changed, 1 insertion(+) (limited to 'src') diff --git a/src/drivers/ao_m25.c b/src/drivers/ao_m25.c index 9603c1de..518765b2 100644 --- a/src/drivers/ao_m25.c +++ b/src/drivers/ao_m25.c @@ -100,6 +100,7 @@ static __xdata uint8_t ao_m25_mutex; static __xdata uint8_t ao_m25_instruction[4]; #if HAS_BOOT_RADIO +/* Kick any radio listeners off so the flash can be written */ extern uint8_t ao_radio_in_recv; static void ao_boot_radio(void) { -- cgit v1.2.3 From 4e3ac3f2038cc3a43252fc8f820a1373a637ab83 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 16 Dec 2012 13:31:45 -0800 Subject: altos: Test APRS rounding by using coordinates near the boundary This selects lat/lon and altitude near the rounding boundary to check that the resulting APRS data is correctly computed. Signed-off-by: Keith Packard --- src/test/ao_aprs_test.c | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/test/ao_aprs_test.c b/src/test/ao_aprs_test.c index f16c94e8..3b31f2d3 100644 --- a/src/test/ao_aprs_test.c +++ b/src/test/ao_aprs_test.c @@ -93,13 +93,16 @@ int main(int argc, char **argv) { audio_gap(1); - ao_gps_data.latitude = 45.4694766 * 10000000; - ao_gps_data.longitude = -122.7376250 * 10000000; - ao_gps_data.altitude = 83; + ao_gps_data.latitude = (45.0 + 28.25 / 60.0) * 10000000; + ao_gps_data.longitude = (-(122 + 44.2649 / 60.0)) * 10000000; + ao_gps_data.altitude = 84; /* Transmit one packet */ ao_aprs_send(); + tncBuffer[strlen((char *) tncBuffer) - 2] = '\0'; + fprintf(stderr, "packet: %s\n", tncBuffer); + exit(0); } -- cgit v1.2.3 From 22a58b0f9b82ea8c7abeda79ca7a4cd21c3dc93c Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 16 Dec 2012 16:04:05 -0800 Subject: altos: Wire up another CC1120 GPIO to get MARC status changes When the radio drops out of RX or TX mode due to an error, it changes the MARC status, and sends pulse down a configured GPIO. Use this to tell when something 'bad' happened during TX or RX so that we can recover from losing the SPI bus in the middle of transmission or reception. Without this, the radio would change state and we'd never know, leaving the radio code waiting for an interrupt that would never arrive. Signed-off-by: Keith Packard --- src/drivers/ao_cc1120.c | 142 ++++++++++++++++++++++++++---------- src/drivers/ao_m25.c | 14 +--- src/megadongle-v0.1/ao_pins.h | 1 - src/megametrum-v0.1/ao_megametrum.c | 2 + src/megametrum-v0.1/ao_pins.h | 16 +++- 5 files changed, 120 insertions(+), 55 deletions(-) (limited to 'src') diff --git a/src/drivers/ao_cc1120.c b/src/drivers/ao_cc1120.c index 7654af85..3e894f76 100644 --- a/src/drivers/ao_cc1120.c +++ b/src/drivers/ao_cc1120.c @@ -24,10 +24,12 @@ #define AO_RADIO_MAX_RECV sizeof(struct ao_packet) #define AO_RADIO_MAX_SEND sizeof(struct ao_packet) -uint8_t ao_radio_wake; -uint8_t ao_radio_mutex; -uint8_t ao_radio_abort; -uint8_t ao_radio_in_recv; +static uint8_t ao_radio_mutex; + +static uint8_t ao_radio_wake; /* radio ready. Also used as sleep address */ +static uint8_t ao_radio_abort; /* radio operation should abort */ +static uint8_t ao_radio_mcu_wake; /* MARC status change */ +static uint8_t ao_radio_marc_status; /* Last read MARC status value */ #define CC1120_DEBUG AO_FEC_DEBUG #define CC1120_TRACE 0 @@ -218,13 +220,32 @@ ao_radio_recv_abort(void) #define ao_radio_rdf_value 0x55 static uint8_t -ao_radio_marc_status(void) +ao_radio_get_marc_status(void) { return ao_radio_reg_read(CC1120_MARC_STATUS1); } static void -ao_radio_tx_isr(void) +ao_radio_mcu_wakeup_isr(void) +{ + ao_radio_mcu_wake = 1; + ao_wakeup(&ao_radio_wake); +} + + +static void +ao_radio_check_marc_status(void) +{ + ao_radio_mcu_wake = 0; + ao_radio_marc_status = ao_radio_get_marc_status(); + + /* Anyt other than 'tx/rx finished' means an error occurred */ + if (ao_radio_marc_status & ~(CC1120_MARC_STATUS1_TX_FINISHED|CC1120_MARC_STATUS1_RX_FINISHED)) + ao_radio_abort = 1; +} + +static void +ao_radio_isr(void) { ao_exti_disable(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN); ao_radio_wake = 1; @@ -234,8 +255,9 @@ ao_radio_tx_isr(void) static void ao_radio_start_tx(void) { - ao_exti_set_callback(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN, ao_radio_tx_isr); + ao_exti_set_callback(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN, ao_radio_isr); ao_exti_enable(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN); + ao_exti_enable(AO_CC1120_MCU_WAKEUP_PORT, AO_CC1120_MCU_WAKEUP_PIN); ao_radio_strobe(CC1120_STX); } @@ -247,6 +269,8 @@ ao_radio_idle(void) if ((state >> CC1120_STATUS_STATE) == CC1120_STATUS_STATE_IDLE) break; } + /* Flush any pending TX bytes */ + ao_radio_strobe(CC1120_SFTX); } /* @@ -294,18 +318,19 @@ static const uint16_t packet_setup[] = { (0 << CC1120_PKT_CFG0_PKG_BIT_LEN) | (0 << CC1120_PKT_CFG0_UART_MODE_EN) | (0 << CC1120_PKT_CFG0_UART_SWAP_EN)), + AO_CC1120_MARC_GPIO_IOCFG, CC1120_IOCFG_GPIO_CFG_MARC_MCU_WAKEUP, }; static const uint16_t packet_tx_setup[] = { CC1120_PKT_CFG2, ((CC1120_PKT_CFG2_CCA_MODE_ALWAYS_CLEAR << CC1120_PKT_CFG2_CCA_MODE) | (CC1120_PKT_CFG2_PKT_FORMAT_NORMAL << CC1120_PKT_CFG2_PKT_FORMAT)), - CC1120_IOCFG2, CC1120_IOCFG_GPIO_CFG_RX0TX1_CFG, + AO_CC1120_INT_GPIO_IOCFG, CC1120_IOCFG_GPIO_CFG_RX0TX1_CFG, }; static const uint16_t packet_rx_setup[] = { CC1120_PKT_CFG2, ((CC1120_PKT_CFG2_CCA_MODE_ALWAYS_CLEAR << CC1120_PKT_CFG2_CCA_MODE) | (CC1120_PKT_CFG2_PKT_FORMAT_SYNCHRONOUS_SERIAL << CC1120_PKT_CFG2_PKT_FORMAT)), - CC1120_IOCFG2, CC1120_IOCFG_GPIO_CFG_CLKEN_SOFT, + AO_CC1120_INT_GPIO_IOCFG, CC1120_IOCFG_GPIO_CFG_CLKEN_SOFT, }; /* @@ -455,10 +480,10 @@ ao_radio_set_mode(uint16_t new_mode) ao_radio_reg_write(packet_tx_setup[i], packet_tx_setup[i+1]); if (changes & AO_RADIO_MODE_BITS_TX_BUF) - ao_radio_reg_write(CC1120_IOCFG2, CC1120_IOCFG_GPIO_CFG_TXFIFO_THR); + ao_radio_reg_write(AO_CC1120_INT_GPIO_IOCFG, CC1120_IOCFG_GPIO_CFG_TXFIFO_THR); if (changes & AO_RADIO_MODE_BITS_TX_FINISH) - ao_radio_reg_write(CC1120_IOCFG2, CC1120_IOCFG_GPIO_CFG_RX0TX1_CFG); + ao_radio_reg_write(AO_CC1120_INT_GPIO_IOCFG, CC1120_IOCFG_GPIO_CFG_RX0TX1_CFG); if (changes & AO_RADIO_MODE_BITS_PACKET_RX) for (i = 0; i < sizeof (packet_rx_setup) / sizeof (packet_rx_setup[0]); i += 2) @@ -551,9 +576,11 @@ ao_rdf_run(void) ao_radio_start_tx(); ao_arch_block_interrupts(); - while (!ao_radio_wake && !ao_radio_abort) + while (!ao_radio_wake && !ao_radio_abort && !ao_radio_mcu_wake) ao_sleep(&ao_radio_wake); ao_arch_release_interrupts(); + if (ao_radio_mcu_wake) + ao_radio_check_marc_status(); if (!ao_radio_wake) ao_radio_idle(); ao_radio_put(); @@ -646,11 +673,12 @@ ao_radio_test_cmd(void) static void ao_radio_wait_isr(void) { - ao_radio_wake = 0; ao_arch_block_interrupts(); - while (!ao_radio_wake) + while (!ao_radio_wake && !ao_radio_mcu_wake && !ao_radio_abort) ao_sleep(&ao_radio_wake); ao_arch_release_interrupts(); + if (ao_radio_mcu_wake) + ao_radio_check_marc_status(); } static uint8_t @@ -663,7 +691,7 @@ ao_radio_wait_tx(uint8_t wait_fifo) if (!wait_fifo) return 0; fifo_space = ao_radio_tx_fifo_space(); - } while (!fifo_space); + } while (!fifo_space && !ao_radio_abort); return fifo_space; } @@ -688,6 +716,7 @@ ao_radio_send(const void *d, uint8_t size) while (encode_len) { this_len = encode_len; + ao_radio_wake = 0; if (this_len > fifo_space) { this_len = fifo_space; ao_radio_set_mode(AO_RADIO_MODE_PACKET_TX_BUF); @@ -707,6 +736,10 @@ ao_radio_send(const void *d, uint8_t size) } fifo_space = ao_radio_wait_tx(encode_len != 0); + if (ao_radio_abort) { + ao_radio_idle(); + break; + } } ao_radio_put(); } @@ -742,13 +775,12 @@ ao_radio_send_lots(ao_radio_fill_func fill) uint8_t this_len = cnt; /* Wait for some space in the fifo */ - while ((fifo_space = ao_radio_tx_fifo_space()) == 0) { + while (!ao_radio_abort && (fifo_space = ao_radio_tx_fifo_space()) == 0) { ao_radio_wake = 0; - ao_arch_block_interrupts(); - while (!ao_radio_wake) - ao_sleep(&ao_radio_wake); - ao_arch_release_interrupts(); + ao_radio_wait_isr(); } + if (ao_radio_abort) + break; if (this_len > fifo_space) this_len = fifo_space; @@ -771,7 +803,12 @@ ao_radio_send_lots(ao_radio_fill_func fill) } else ao_exti_enable(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN); } + if (ao_radio_abort) { + ao_radio_idle(); + break; + } /* Wait for the transmitter to go idle */ + ao_radio_wake = 0; ao_radio_wait_isr(); } ao_radio_put(); @@ -822,14 +859,21 @@ ao_radio_rx_isr(void) static uint16_t ao_radio_rx_wait(void) { - ao_arch_block_interrupts(); - rx_waiting = 1; - while (rx_data_cur - rx_data_consumed < AO_FEC_DECODE_BLOCK && - !ao_radio_abort) { - ao_sleep(&ao_radio_wake); - } - rx_waiting = 0; - ao_arch_release_interrupts(); + do { + if (ao_radio_mcu_wake) + ao_radio_check_marc_status(); + ao_arch_block_interrupts(); + rx_waiting = 1; + while (rx_data_cur - rx_data_consumed < AO_FEC_DECODE_BLOCK && + !ao_radio_abort && + !ao_radio_mcu_wake) + { + if (ao_sleep(&ao_radio_wake)) + ao_radio_abort = 1; + } + rx_waiting = 0; + ao_arch_release_interrupts(); + } while (ao_radio_mcu_wake); if (ao_radio_abort) return 0; rx_data_consumed += AO_FEC_DECODE_BLOCK; @@ -865,30 +909,53 @@ ao_radio_recv(__xdata void *d, uint8_t size) rx_data_consumed = 0; rx_ignore = 2; + /* Must be set before changing the frequency; any abort + * after the frequency is set needs to terminate the read + * so that the registers can be reprogrammed + */ ao_radio_abort = 0; - ao_radio_in_recv = 1; + /* configure interrupt pin */ ao_radio_get(len); ao_radio_set_mode(AO_RADIO_MODE_PACKET_RX); ao_radio_wake = 0; + ao_radio_mcu_wake = 0; stm_spi2.cr2 = 0; /* clear any RXNE */ (void) stm_spi2.dr; - ao_exti_set_callback(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN, ao_radio_rx_isr); + /* Have the radio signal when the preamble quality goes high */ + ao_radio_reg_write(AO_CC1120_INT_GPIO_IOCFG, CC1120_IOCFG_GPIO_CFG_PQT_REACHED); + ao_exti_set_mode(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN, + AO_EXTI_MODE_RISING|AO_EXTI_PRIORITY_HIGH); + ao_exti_set_callback(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN, ao_radio_isr); ao_exti_enable(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN); + ao_exti_enable(AO_CC1120_MCU_WAKEUP_PORT, AO_CC1120_MCU_WAKEUP_PIN); ao_radio_strobe(CC1120_SRX); + /* Wait for the preamble to appear */ + ao_radio_wait_isr(); + if (ao_radio_abort) + goto abort; + + ao_radio_reg_write(AO_CC1120_INT_GPIO_IOCFG, CC1120_IOCFG_GPIO_CFG_CLKEN_SOFT); + ao_exti_set_mode(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN, + AO_EXTI_MODE_FALLING|AO_EXTI_PRIORITY_HIGH); + + ao_exti_set_callback(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN, ao_radio_rx_isr); + ao_exti_enable(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN); + ao_radio_burst_read_start(CC1120_SOFT_RX_DATA_OUT); ret = ao_fec_decode(rx_data, rx_data_count, d, size + 2, ao_radio_rx_wait); ao_radio_burst_read_stop(); +abort: ao_radio_strobe(CC1120_SIDLE); /* Convert from 'real' rssi to cc1111-style values */ @@ -901,11 +968,6 @@ ao_radio_recv(__xdata void *d, uint8_t size) ((uint8_t *) d)[size] = (uint8_t) rssi; - ao_radio_in_recv = 0; - - if (ao_radio_abort) - ao_delay(1); - #if AO_PROFILE rx_last_done_tick = rx_done_tick; rx_done_tick = ao_profile_tick(); @@ -1125,7 +1187,7 @@ static void ao_radio_show(void) { printf ("Status: %02x\n", status); printf ("CHIP_RDY: %d\n", (status >> CC1120_STATUS_CHIP_RDY) & 1); printf ("STATE: %s\n", cc1120_state_name[(status >> CC1120_STATUS_STATE) & CC1120_STATUS_STATE_MASK]); - printf ("MARC: %02x\n", ao_radio_marc_status()); + printf ("MARC: %02x\n", ao_radio_get_marc_status()); for (i = 0; i < AO_NUM_CC1120_REG; i++) printf ("\t%02x %-20.20s\n", ao_radio_reg_read(ao_cc1120_reg[i].addr), ao_cc1120_reg[i].name); @@ -1215,7 +1277,13 @@ ao_radio_init(void) ao_enable_port(AO_CC1120_INT_PORT); ao_exti_setup(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN, AO_EXTI_MODE_FALLING|AO_EXTI_PRIORITY_HIGH, - ao_radio_tx_isr); + ao_radio_isr); + + /* Enable the hacked up GPIO3 pin */ + ao_enable_port(AO_CC1120_MCU_WAKEUP_PORT); + ao_exti_setup(AO_CC1120_MCU_WAKEUP_PORT, AO_CC1120_MCU_WAKEUP_PIN, + AO_EXTI_MODE_FALLING|AO_EXTI_PRIORITY_MED, + ao_radio_mcu_wakeup_isr); ao_cmd_register(&ao_radio_cmds[0]); } diff --git a/src/drivers/ao_m25.c b/src/drivers/ao_m25.c index 518765b2..9f696ace 100644 --- a/src/drivers/ao_m25.c +++ b/src/drivers/ao_m25.c @@ -99,19 +99,7 @@ static __xdata uint8_t ao_m25_mutex; static __xdata uint8_t ao_m25_instruction[4]; -#if HAS_BOOT_RADIO -/* Kick any radio listeners off so the flash can be written */ -extern uint8_t ao_radio_in_recv; - -static void ao_boot_radio(void) { - if (ao_radio_in_recv) - ao_radio_recv_abort(); -} -#else -#define ao_boot_radio() -#endif - -#define M25_SELECT(cs) do { ao_boot_radio(); ao_spi_get_mask(AO_M25_SPI_CS_PORT,cs,AO_M25_SPI_BUS, AO_SPI_SPEED_FAST); } while (0) +#define M25_SELECT(cs) ao_spi_get_mask(AO_M25_SPI_CS_PORT,cs,AO_M25_SPI_BUS, AO_SPI_SPEED_FAST) #define M25_DESELECT(cs) ao_spi_put_mask(AO_M25_SPI_CS_PORT,cs,AO_M25_SPI_BUS) #define M25_BLOCK_SHIFT 16 diff --git a/src/megadongle-v0.1/ao_pins.h b/src/megadongle-v0.1/ao_pins.h index cabe9ee2..efbcd775 100644 --- a/src/megadongle-v0.1/ao_pins.h +++ b/src/megadongle-v0.1/ao_pins.h @@ -140,7 +140,6 @@ #define AO_CC1120_INT_PIN 14 #define AO_CC1120_INT_GPIO 2 -#define HAS_BOOT_RADIO 1 /* * Profiling Viterbi decoding diff --git a/src/megametrum-v0.1/ao_megametrum.c b/src/megametrum-v0.1/ao_megametrum.c index cb1eb417..fbdab64a 100644 --- a/src/megametrum-v0.1/ao_megametrum.c +++ b/src/megametrum-v0.1/ao_megametrum.c @@ -53,7 +53,9 @@ main(void) ao_exti_init(); ao_adc_init(); +#if HAS_BEEP ao_beep_init(); +#endif ao_cmd_init(); #if HAS_MS5607 diff --git a/src/megametrum-v0.1/ao_pins.h b/src/megametrum-v0.1/ao_pins.h index 083c1b6f..ab4cf7df 100644 --- a/src/megametrum-v0.1/ao_pins.h +++ b/src/megametrum-v0.1/ao_pins.h @@ -67,7 +67,7 @@ #define HAS_EEPROM 1 #define USE_INTERNAL_FLASH 0 #define HAS_USB 1 -#define HAS_BEEP 1 +#define HAS_BEEP 0 #define HAS_RADIO 1 #define HAS_TELEMETRY 1 #define HAS_APRS 1 @@ -282,11 +282,19 @@ struct ao_adc { #define AO_CC1120_SPI_CS_PIN 5 #define AO_CC1120_SPI_BUS AO_SPI_2_PB13_PB14_PB15 -#define AO_CC1120_INT_PORT (&stm_gpioc) -#define AO_CC1120_INT_PIN 14 +#define AO_CC1120_INT_PORT (&stm_gpioc) +#define AO_CC1120_INT_PIN 14 +#define AO_CC1120_MCU_WAKEUP_PORT (&stm_gpioc) +#define AO_CC1120_MCU_WAKEUP_PIN (0) #define AO_CC1120_INT_GPIO 2 -#define HAS_BOOT_RADIO 1 +#define AO_CC1120_INT_GPIO_IOCFG CC1120_IOCFG2 + +#define AO_CC1120_MARC_GPIO 3 +#define AO_CC1120_MARC_GPIO_IOCFG CC1120_IOCFG3 + + +#define HAS_BOOT_RADIO 0 /* * Mag sensor (hmc5883) -- cgit v1.2.3 From dd7c30324461b2aed83b86bfe4323180664123cf Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 16 Dec 2012 16:08:33 -0800 Subject: altos: Add new MARC status pin interrupt bits to megadongle Signed-off-by: Keith Packard --- src/megadongle-v0.1/ao_pins.h | 7 +++++++ 1 file changed, 7 insertions(+) (limited to 'src') diff --git a/src/megadongle-v0.1/ao_pins.h b/src/megadongle-v0.1/ao_pins.h index efbcd775..d810cd4c 100644 --- a/src/megadongle-v0.1/ao_pins.h +++ b/src/megadongle-v0.1/ao_pins.h @@ -139,7 +139,14 @@ #define AO_CC1120_INT_PORT (&stm_gpioc) #define AO_CC1120_INT_PIN 14 +#define AO_CC1120_MCU_WAKEUP_PORT (&stm_gpioc) +#define AO_CC1120_MCU_WAKEUP_PIN (0) + #define AO_CC1120_INT_GPIO 2 +#define AO_CC1120_INT_GPIO_IOCFG CC1120_IOCFG2 + +#define AO_CC1120_MARC_GPIO 3 +#define AO_CC1120_MARC_GPIO_IOCFG CC1120_IOCFG3 /* * Profiling Viterbi decoding -- cgit v1.2.3 From b6c9e8ffc87481a23ba90fa22df7c9421e2cd6a6 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 16 Dec 2012 16:52:15 -0800 Subject: altos: Re-enable beeper on megametrum I turned it off during radio testing and forgot to fix that before committing... Signed-off-by: Keith Packard --- src/megametrum-v0.1/ao_pins.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/megametrum-v0.1/ao_pins.h b/src/megametrum-v0.1/ao_pins.h index ab4cf7df..b1a70ea2 100644 --- a/src/megametrum-v0.1/ao_pins.h +++ b/src/megametrum-v0.1/ao_pins.h @@ -67,7 +67,7 @@ #define HAS_EEPROM 1 #define USE_INTERNAL_FLASH 0 #define HAS_USB 1 -#define HAS_BEEP 0 +#define HAS_BEEP 1 #define HAS_RADIO 1 #define HAS_TELEMETRY 1 #define HAS_APRS 1 -- cgit v1.2.3 From b1d37be4c024e9690107c693d9819229025966fa Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 17 Dec 2012 17:03:41 -0800 Subject: altos: Average MPU6000 values on ground for later use Having long-term ground averages recorded to the eeprom file will make post-flight analysis of the data better. Signed-off-by: Keith Packard --- src/core/ao_data.h | 21 +++++++++++ src/core/ao_log.h | 14 +++++-- src/core/ao_sample.c | 95 ++++++++++++++++++++++++++++++++++++++++++++++- src/core/ao_sqrt.c | 2 + src/test/ao_flight_test.c | 17 ++++++++- 5 files changed, 142 insertions(+), 7 deletions(-) (limited to 'src') diff --git a/src/core/ao_data.h b/src/core/ao_data.h index 6fdd19cb..7e2f85d8 100644 --- a/src/core/ao_data.h +++ b/src/core/ao_data.h @@ -288,4 +288,25 @@ typedef int16_t accel_t; #endif +#if !HAS_GYRO && HAS_MPU6000 + +#define HAS_GYRO 1 + +typedef int16_t gyro_t; +typedef int32_t angle_t; + +/* Y axis is aligned with the direction of motion (along) */ +/* X axis is aligned in the other board axis (across) */ +/* Z axis is aligned perpendicular to the board (through) */ + +#define ao_data_along(packet) ((packet)->mpu6000.accel_y) +#define ao_data_across(packet) ((packet)->mpu6000.accel_x) +#define ao_data_through(packet) ((packet)->mpu6000.accel_z) + +#define ao_data_roll(packet) ((packet)->mpu6000.gyro_y) +#define ao_data_pitch(packet) ((packet)->mpu6000.gyro_x) +#define ao_data_yaw(packet) ((packet)->mpu6000.gyro_z) + +#endif + #endif /* _AO_DATA_H_ */ diff --git a/src/core/ao_log.h b/src/core/ao_log.h index 4eaed420..93b01778 100644 --- a/src/core/ao_log.h +++ b/src/core/ao_log.h @@ -199,10 +199,16 @@ struct ao_log_mega { union { /* 4 */ /* AO_LOG_FLIGHT */ struct { - uint16_t flight; /* 4 */ - int16_t ground_accel; /* 6 */ - uint32_t ground_pres; /* 8 */ - } flight; /* 12 */ + uint16_t flight; /* 4 */ + int16_t ground_accel; /* 6 */ + uint32_t ground_pres; /* 8 */ + int16_t ground_accel_along; /* 16 */ + int16_t ground_accel_across; /* 12 */ + int16_t ground_accel_through; /* 14 */ + int16_t ground_gyro_roll; /* 18 */ + int16_t ground_gyro_pitch; /* 20 */ + int16_t ground_gyro_yaw; /* 22 */ + } flight; /* 24 */ /* AO_LOG_STATE */ struct { uint16_t state; diff --git a/src/core/ao_sample.c b/src/core/ao_sample.c index 985c0940..7a1eff8e 100644 --- a/src/core/ao_sample.c +++ b/src/core/ao_sample.c @@ -37,6 +37,16 @@ __pdata alt_t ao_sample_height; #if HAS_ACCEL __pdata accel_t ao_sample_accel; #endif +#if HAS_GYRO +__pdata accel_t ao_sample_accel_along; +__pdata accel_t ao_sample_accel_across; +__pdata accel_t ao_sample_accel_through; +__pdata gyro_t ao_sample_roll; +__pdata gyro_t ao_sample_pitch; +__pdata gyro_t ao_sample_yaw; +__pdata angle_t ao_sample_angle; +__pdata angle_t ao_sample_roll_angle; +#endif __data uint8_t ao_sample_data; @@ -53,6 +63,15 @@ __pdata accel_t ao_accel_2g; /* factory accel calibration */ __pdata int32_t ao_accel_scale; /* sensor to m/s² conversion */ #endif +#if HAS_GYRO +__pdata accel_t ao_ground_accel_along; +__pdata accel_t ao_ground_accel_across; +__pdata accel_t ao_ground_accel_through; +__pdata gyro_t ao_ground_pitch; +__pdata gyro_t ao_ground_yaw; +__pdata gyro_t ao_ground_roll; +#endif + static __pdata uint8_t ao_preflight; /* in preflight mode */ static __pdata uint16_t nsamples; @@ -60,6 +79,14 @@ __pdata int32_t ao_sample_pres_sum; #if HAS_ACCEL __pdata int32_t ao_sample_accel_sum; #endif +#if HAS_GYRO +__pdata int32_t ao_sample_accel_along_sum; +__pdata int32_t ao_sample_accel_across_sum; +__pdata int32_t ao_sample_accel_through_sum; +__pdata int32_t ao_sample_pitch_sum; +__pdata int32_t ao_sample_yaw_sum; +__pdata int32_t ao_sample_roll_sum; +#endif static void ao_sample_preflight_add(void) @@ -68,6 +95,14 @@ ao_sample_preflight_add(void) ao_sample_accel_sum += ao_sample_accel; #endif ao_sample_pres_sum += ao_sample_pres; +#if HAS_GYRO + ao_sample_accel_along_sum += ao_sample_accel_along; + ao_sample_accel_across_sum += ao_sample_accel_across; + ao_sample_accel_through_sum += ao_sample_accel_through; + ao_sample_pitch_sum += ao_sample_pitch; + ao_sample_yaw_sum += ao_sample_yaw; + ao_sample_roll_sum += ao_sample_roll; +#endif ++nsamples; } @@ -80,8 +115,23 @@ ao_sample_preflight_set(void) #endif ao_ground_pres = ao_sample_pres_sum >> 9; ao_ground_height = pres_to_altitude(ao_ground_pres); - nsamples = 0; ao_sample_pres_sum = 0; +#if HAS_GYRO + ao_ground_accel_along = ao_sample_accel_along_sum >> 9; + ao_ground_accel_across = ao_sample_accel_across_sum >> 9; + ao_ground_accel_through = ao_sample_accel_through_sum >> 9; + ao_ground_pitch = ao_sample_pitch_sum >> 9; + ao_ground_yaw = ao_sample_yaw_sum >> 9; + ao_ground_roll = ao_sample_roll_sum >> 9; + ao_sample_accel_along_sum = 0; + ao_sample_accel_across_sum = 0; + ao_sample_accel_through_sum = 0; + ao_sample_pitch_sum = 0; + ao_sample_yaw_sum = 0; + ao_sample_roll_sum = 0; + ao_sample_angle = 0; +#endif + nsamples = 0; } static void @@ -122,6 +172,24 @@ ao_sample_preflight_update(void) ao_sample_preflight_set(); } +#if HAS_GYRO + +static int32_t p_filt; +static int32_t y_filt; + +static gyro_t inline ao_gyro(void) { + gyro_t p = ao_sample_pitch - ao_ground_pitch; + gyro_t y = ao_sample_yaw - ao_ground_yaw; + + p_filt = p_filt - (p_filt >> 6) + p; + y_filt = y_filt - (y_filt >> 6) + y; + + p = p_filt >> 6; + y = y_filt >> 6; + return ao_sqrt(p*p + y*y); +} +#endif + uint8_t ao_sample(void) { @@ -147,6 +215,12 @@ ao_sample(void) ao_sample_accel = ao_data_accel_invert(ao_sample_accel); ao_data_set_accel(ao_data, ao_sample_accel); #endif +#if HAS_GYRO + ao_sample_accel_along = ao_data_along(ao_data); + ao_sample_pitch = ao_data_pitch(ao_data); + ao_sample_yaw = ao_data_yaw(ao_data); + ao_sample_roll = ao_data_roll(ao_data); +#endif if (ao_preflight) ao_sample_preflight(); @@ -154,6 +228,10 @@ ao_sample(void) if (ao_flight_state < ao_flight_boost) ao_sample_preflight_update(); ao_kalman(); +#if HAS_GYRO + ao_sample_angle += ao_gyro(); + ao_sample_roll_angle += (ao_sample_roll - ao_ground_roll); +#endif } ao_sample_data = ao_data_ring_next(ao_sample_data); } @@ -170,6 +248,21 @@ ao_sample_init(void) #if HAS_ACCEL ao_sample_accel_sum = 0; ao_sample_accel = 0; +#endif +#if HAS_GYRO + ao_sample_accel_along_sum = 0; + ao_sample_accel_across_sum = 0; + ao_sample_accel_through_sum = 0; + ao_sample_accel_along = 0; + ao_sample_accel_across = 0; + ao_sample_accel_through = 0; + ao_sample_pitch_sum = 0; + ao_sample_yaw_sum = 0; + ao_sample_roll_sum = 0; + ao_sample_pitch = 0; + ao_sample_yaw = 0; + ao_sample_roll = 0; + ao_sample_angle = 0; #endif ao_sample_data = ao_data_head; ao_preflight = TRUE; diff --git a/src/core/ao_sqrt.c b/src/core/ao_sqrt.c index 09c2e319..3a550eaa 100644 --- a/src/core/ao_sqrt.c +++ b/src/core/ao_sqrt.c @@ -15,7 +15,9 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. */ +#ifndef AO_FLIGHT_TEST #include "ao.h" +#endif /* Adapted from int_sqrt.c in the linux kernel, which is licensed GPLv2 */ /** diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index acdf4d92..cdd1f236 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -236,10 +236,14 @@ extern int32_t ao_accel_scale; extern alt_t ao_ground_height; extern alt_t ao_sample_alt; +double ao_sample_qangle; + int ao_sample_prev_tick; uint16_t prev_tick; + #include "ao_kalman.c" +#include "ao_sqrt.c" #include "ao_sample.c" #include "ao_flight.c" @@ -309,7 +313,7 @@ ao_mpu6000_accel(int16_t sensor) } static double -ao_mpu6000_gyro(int16_t sensor) +ao_mpu6000_gyro(int32_t sensor) { return sensor / 32767.0 * MPU6000_GYRO_FULLSCALE; } @@ -370,6 +374,7 @@ ao_insert(void) if (!ao_summary) { printf("%7.2f height %8.2f accel %8.3f " #if MEGAMETRUM + "roll %8.3f angle %8.3f qangle %8.3f " "accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f " #endif "state %-8.8s k_height %8.2f k_speed %8.3f k_accel %8.3f avg_height %5d drogue %4d main %4d error %5d\n", @@ -377,6 +382,9 @@ ao_insert(void) height, accel, #if MEGAMETRUM + ao_mpu6000_gyro(ao_sample_roll_angle) / 100.0, + ao_mpu6000_gyro(ao_sample_angle) / 100.0, + ao_sample_qangle, ao_mpu6000_accel(ao_data_static.mpu6000.accel_x), ao_mpu6000_accel(ao_data_static.mpu6000.accel_y), ao_mpu6000_accel(ao_data_static.mpu6000.accel_z), @@ -715,12 +723,14 @@ update_orientation (double rate_x, double rate_y, double rate_z, int tick) q_dot.q2 = 0.5 * (ao_orient.q0 * rate_y + ao_orient.q3 * rate_x - ao_orient.q1 * rate_z) + lambda * ao_orient.q2; q_dot.q3 = 0.5 * (ao_orient.q0 * rate_z + ao_orient.q1 * rate_y - ao_orient.q2 * rate_x) + lambda * ao_orient.q3; +#if 0 printf ("update_orientation %g %g %g (%g s)\n", rate_x, rate_y, rate_z, dt); printf ("q_dot (%g) %g %g %g\n", q_dot.q0, q_dot.q1, q_dot.q2, q_dot.q3); +#endif ao_orient.q0 += q_dot.q0 * dt; ao_orient.q1 += q_dot.q1 * dt; @@ -731,6 +741,8 @@ update_orientation (double rate_x, double rate_y, double rate_z, int tick) ao_quat_rot(&ao_current, &ao_up, &ao_orient); + ao_sample_qangle = 180 / M_PI * acos(ao_current.q3 * sqrt(2)); +#if 0 printf ("orient (%g) %g %g %g current (%g) %g %g %g\n", ao_orient.q0, ao_orient.q1, @@ -740,6 +752,7 @@ update_orientation (double rate_x, double rate_y, double rate_z, int tick) ao_current.q1, ao_current.q2, ao_current.q3); +#endif } #endif @@ -845,7 +858,7 @@ ao_sleep(void *wchan) double rate_y = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_y - ao_ground_mpu6000.gyro_y); double rate_z = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_z - ao_ground_mpu6000.gyro_z); - update_orientation(rate_x, rate_z, rate_y, tick); + update_orientation(rate_x * M_PI / 180, rate_z * M_PI / 180, rate_y * M_PI / 180, tick); } ao_records_read++; ao_insert(); -- cgit v1.2.3 From 57487e78b90465a21c87cf30deb0aeaba0887332 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 18 Dec 2012 23:15:20 -0800 Subject: altos: Actually record ground averages for 6dof sensor This gets the long-term averages for the 6dof sensors recorded into the first flight log record. Signed-off-by: Keith Packard --- src/core/ao_log.h | 6 +++--- src/core/ao_log_mega.c | 8 ++++++++ src/core/ao_sample.c | 8 +++++--- src/core/ao_sample.h | 8 ++++++++ 4 files changed, 24 insertions(+), 6 deletions(-) (limited to 'src') diff --git a/src/core/ao_log.h b/src/core/ao_log.h index 93b01778..036d6f2d 100644 --- a/src/core/ao_log.h +++ b/src/core/ao_log.h @@ -205,9 +205,9 @@ struct ao_log_mega { int16_t ground_accel_along; /* 16 */ int16_t ground_accel_across; /* 12 */ int16_t ground_accel_through; /* 14 */ - int16_t ground_gyro_roll; /* 18 */ - int16_t ground_gyro_pitch; /* 20 */ - int16_t ground_gyro_yaw; /* 22 */ + int16_t ground_roll; /* 18 */ + int16_t ground_pitch; /* 20 */ + int16_t ground_yaw; /* 22 */ } flight; /* 24 */ /* AO_LOG_STATE */ struct { diff --git a/src/core/ao_log_mega.c b/src/core/ao_log_mega.c index ac1590db..e03687ad 100644 --- a/src/core/ao_log_mega.c +++ b/src/core/ao_log_mega.c @@ -94,6 +94,14 @@ ao_log(void) log.tick = ao_sample_tick; #if HAS_ACCEL log.u.flight.ground_accel = ao_ground_accel; +#endif +#if HAS_GYRO + log.u.flight.ground_accel_along = ao_ground_accel_along; + log.u.flight.ground_accel_across = ao_ground_accel_across; + log.u.flight.ground_accel_through = ao_ground_accel_through; + log.u.flight.ground_pitch = ao_ground_pitch; + log.u.flight.ground_yaw = ao_ground_yaw; + log.u.flight.ground_roll = ao_ground_roll; #endif log.u.flight.ground_pres = ao_ground_pres; log.u.flight.flight = ao_flight_number; diff --git a/src/core/ao_sample.c b/src/core/ao_sample.c index 7a1eff8e..dec44f9f 100644 --- a/src/core/ao_sample.c +++ b/src/core/ao_sample.c @@ -172,8 +172,8 @@ ao_sample_preflight_update(void) ao_sample_preflight_set(); } +#if 0 #if HAS_GYRO - static int32_t p_filt; static int32_t y_filt; @@ -189,6 +189,7 @@ static gyro_t inline ao_gyro(void) { return ao_sqrt(p*p + y*y); } #endif +#endif uint8_t ao_sample(void) @@ -217,6 +218,8 @@ ao_sample(void) #endif #if HAS_GYRO ao_sample_accel_along = ao_data_along(ao_data); + ao_sample_accel_across = ao_data_across(ao_data); + ao_sample_accel_through = ao_data_through(ao_data); ao_sample_pitch = ao_data_pitch(ao_data); ao_sample_yaw = ao_data_yaw(ao_data); ao_sample_roll = ao_data_roll(ao_data); @@ -229,8 +232,7 @@ ao_sample(void) ao_sample_preflight_update(); ao_kalman(); #if HAS_GYRO - ao_sample_angle += ao_gyro(); - ao_sample_roll_angle += (ao_sample_roll - ao_ground_roll); + /* do quaternion stuff here... */ #endif } ao_sample_data = ao_data_ring_next(ao_sample_data); diff --git a/src/core/ao_sample.h b/src/core/ao_sample.h index 9336bdf9..a2dac979 100644 --- a/src/core/ao_sample.h +++ b/src/core/ao_sample.h @@ -111,6 +111,14 @@ extern __pdata accel_t ao_ground_accel; /* startup acceleration */ extern __pdata accel_t ao_accel_2g; /* factory accel calibration */ extern __pdata int32_t ao_accel_scale; /* sensor to m/s² conversion */ #endif +#if HAS_GYRO +extern __pdata accel_t ao_ground_accel_along; +extern __pdata accel_t ao_ground_accel_across; +extern __pdata accel_t ao_ground_accel_through; +extern __pdata gyro_t ao_ground_pitch; +extern __pdata gyro_t ao_ground_yaw; +extern __pdata gyro_t ao_ground_roll; +#endif void ao_sample_init(void); -- cgit v1.2.3 From a6e116515f5e4522adbfcd1900885c2a6034b57c Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 28 Dec 2012 19:34:33 -0700 Subject: altos: Fix cc1120 debug code to build on megadongle RDF function had changed, and APRS isn't available on megadongle. Signed-off-by: Keith Packard --- src/drivers/ao_cc1120.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/ao_cc1120.c b/src/drivers/ao_cc1120.c index 3e894f76..63d2f955 100644 --- a/src/drivers/ao_cc1120.c +++ b/src/drivers/ao_cc1120.c @@ -1195,7 +1195,7 @@ static void ao_radio_show(void) { } static void ao_radio_beep(void) { - ao_radio_rdf(RDF_PACKET_LEN); + ao_radio_rdf(); } static void ao_radio_packet(void) { @@ -1231,6 +1231,7 @@ ao_radio_test_recv() } } +#if HAS_APRS #include static void @@ -1239,13 +1240,16 @@ ao_radio_aprs() ao_packet_slave_stop(); ao_aprs_send(); } +#endif #endif static const struct ao_cmds ao_radio_cmds[] = { { ao_radio_test_cmd, "C <1 start, 0 stop, none both>\0Radio carrier test" }, #if CC1120_DEBUG +#if HAS_APRS { ao_radio_aprs, "G\0Send APRS packet" }, +#endif { ao_radio_show, "R\0Show CC1120 status" }, { ao_radio_beep, "b\0Emit an RDF beacon" }, { ao_radio_packet, "p\0Send a test packet" }, -- cgit v1.2.3 From b70ca5eaf1c3d60bd9adf6835e1247f4147ca9c8 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 28 Dec 2012 19:35:46 -0700 Subject: altos: Fix MegaDongle CC1120 chip select pin It's on A0, not C5 Signed-off-by: Keith Packard --- src/megadongle-v0.1/ao_pins.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/megadongle-v0.1/ao_pins.h b/src/megadongle-v0.1/ao_pins.h index d810cd4c..5a5eaa30 100644 --- a/src/megadongle-v0.1/ao_pins.h +++ b/src/megadongle-v0.1/ao_pins.h @@ -132,8 +132,8 @@ #define AO_RADIO_CAL_DEFAULT 0x6ca333 #define AO_FEC_DEBUG 0 -#define AO_CC1120_SPI_CS_PORT (&stm_gpioc) -#define AO_CC1120_SPI_CS_PIN 5 +#define AO_CC1120_SPI_CS_PORT (&stm_gpioa) +#define AO_CC1120_SPI_CS_PIN 0 #define AO_CC1120_SPI_BUS AO_SPI_2_PB13_PB14_PB15 #define AO_CC1120_INT_PORT (&stm_gpioc) -- cgit v1.2.3