//
// Name: pc.c
//
//
// Revision History - update sysGetVersion():
//
// M. Gray 23 Jul 2001 V1.00 Initial release.
//
// M. Gray 25 Oct 2001 V1.01 Added '>' symbol to preface APRS status messages,
// added crossband repeater functions,
// reduced size of serial I/O buffers,
// changed GEOID height in GPGGA message to NULL field,
// changed order of telemetry data,
// corrected VSI (Vertical Speed Indicator),
// corrected spurious interrupt fault with processor,
// replaced GPS 2D/3D status with pdop/hdop value,
// removed heat sink temp from camera control module,
// fixed startup delay timer, and
// upgraded analog input to 20-bit ADC.
//
// M. Gray 01 May 2002 V1.02 Send GPGGA and GPRMC to video overlay on alternating seconds,
// added hour and minutes to bus voltage recording,
// command to turn ATV on and off,
// extended cut down to 10 seconds, and
// corrected analog static port 16/32 bit conversion problem.
//
// M. Gray 12 June 2002 V1.03 Send hex NMEA checksum as upper case characters,
// replaced \015\012 sequence with \n\r for clarity,
// updated plain text status strings on external serial port,
// added hysterisas to CTCSS tone decoder input,
// reversed output level for '52' and '58' DTMF commands,
// added TNC message feedback for DTMF commands, and
// cleaned up source code and documentation.
//
// M. Gray 30 June 2002 V1.04 Added BEGIN and END tags to binary memory dump,
// updated sysBootMessage() to use telemetry methods,
// added UTC time stamp to every LOG_TYPE_COORD record,
// removed UTC time stamp from bus voltage,
// added user abort to data download,
// moved log temp/bus voltage before coord,
// removed cursor control character '\014' from init string,
// added '412' DTMF command to send $GPGGA message on demand,
// record full 32-bit altitude value in native resolution,
// added different size log blocks to work around memory map limitations,
// expanded memory logging to 456KB out of 512KB, and
// removed logging from gpsParseMessage() to log corruption during gpsInit().
//
// M. Gray 5 Nov 2002 V1.05 Enabled configuration memory check after reboot (accidently disabled during debug),
// changed packet NMEA packets from flight time to UTC time,
// change packet APRS to report on secondary frequency using UTC time as trigger,
// added CW ident to cross band frequency,
// removed support for ISS transmit, and
// added support for cross link modes.
//
// M. Gray 22 Nov 2002 V1.06 Change temperature log rate to 15 seconds,
// added run time error handler, and
// added landing estimate module.
//
// M. Gray 9 Feb 2003 V1.07 Added port map list,
// added digital humidity/temp sensor,
// changed SSID to 0 for prediction packet,
// set prediction to APRS freq at 15 and 45 seconds,
// and add command to factory reset GPS engine.
//
// M. Gray 5 Apr 2003 V1.08 Added GATE,WIDE3-3 back to TNC messages,
// fixed NMEA-0183 checksum calculation, and
// removed heading for magnetic corection field in $GPRMC message.
//
// M. Gray 1 Jul 2003 V1.09 Added internal/external digital temperature sensors,
// upgraded to Rabbit 8.01 compiler, and
// and added support for new pressure sensor.
//
// M. Gray 18 Oct 2003 V1.10 Added support for short range transmitter to control remote playloads/cut down.
//
// M. Gray 13 Nov 2003 V1.11 Added DTMF pass through time out,
// change remote cut down command rate to 2 seconds,
// updated landing estimate polynomial, and
// added peak altitude display to cut down message.
//
// M. Gray 5 Mar 2004 V1.12 Added support for wireless link to ATV transmitter.
//
//
// COPYRIGHT (c) 2001-2003 Michael Gray, KD7LMO
//
// 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
//
// Disable debug and enable code storage in extended memory except for interrupt service methods.
#ifndef WIN32
#nodebug
#memmap xmem
#endif
// Define the size of the RS-232 buffers used for NMEA-0183 message and GPS.
#define CINBUFSIZE 255
#define COUTBUFSIZE 255
#define PORTD_BUFFER_SIZE 255
#define DINBUFSIZE PORTD_BUFFER_SIZE
#define DOUTBUFSIZE PORTD_BUFFER_SIZE
// To avoid confusion in the sizes of short, int, and long on 8, 16, and 32 bit processors,
// we will be very explicit. The typedefs MUST be checked when cross compiling or changing
// compilers.
typedef char int8;
typedef long int32;
// We simulate the Rabbit enviroment for cross compiling in Linux or Visual Studio.
// The primary reason is because they offer a much better IDE or development suite.
#ifdef WIN32
#pragma pack (1)
#pragma warning (disable: 4057)
#pragma warning (disable: 4100)
#pragma warning (disable: 4244)
#pragma warning (disable: 4305)
typedef unsigned char uint8;
typedef unsigned int uint16;
typedef int int16;
typedef unsigned long uint32;
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.H>
#define PADR 0
#define PBDR 1
#define PDDR 3
#define PEDR 4
#define TBM1R 10
#define TBL1R 11
#define TBCSR 12
#define TBCR 14
#define TAT1R 15
#define SPCR 17
#define PDFR 18
#define PDDDR 19
#define PDDCR 20
#define PDCR 21
#define PEDDR 22
#define PEFR 23
#define I0CR 24
#define TACR 25
#define TACSR 26
#define TAT4R 27
#define PECR 28
#define IB7CR 29
#define I1CR 30
#define TBCLR 31
#define TAT5R 32
#define SEC_TIMER 1
#define MS_TIMER 1000
#define PI 3.14159265358
uint16 PADRShadow, PDDRShadow, PEDRShadow, TAT1RShadow, TACSRShadow, TACRShadow, TAT4RShadow, TAT5RShadow, PDDDRShadow;
void WrPortI (uint16 port, uint16 *shadow, uint16 value) {};
void WrPortE (uint16 port, uint16 *shadow, uint16 value) {};
void BitWrPortI (uint16 port, uint16 *shadow, uint8 value, uint8 bit) {};
uint16 BitRdPortI (uint16 port, uint8 bit) { return 0; };
uint16 RdPortI (uint16 port) { return 0; };
uint16 RdPortE (uint16 port) { return 0; };
uint16 serCgetc () { return 0; };
void serAopen (uint16 baudrate) { };
void serCopen (uint16 baudrate) { };
void serDopen (uint16 baudrate) { };
void serAputc (uint8 value) { putchar(value); };
void serAputs (uint8 *str) { puts (str); };
void serCputc (uint8 value) { putchar(value); };
void serCputs (uint8 *str) { puts (str); };
void serDputc (uint8 value) { putchar(value); };
void serDputs (uint8 *str) { puts (str); };
void serDclose() {};
int serDwrFree() { return 0; };
uint8 serDgetc () { return 0; };
void clockDoublerOn() { };
void clockDoublerOff() { };
void SetVectExtern2000 (uint8 vector, void func()) {};
void SetVectIntern (uint8 vector, void func()) {};
uint8 VdGetFreeWd (uint8 timeOut) { return 0; };
void VdHitWd (uint8 value) {};
void root2xmem (uint32 src, void *dest, uint32 size) { };
void xmem2root (void *dest, uint32 src, uint32 size) { };
long xalloc (uint32 size) { return 0; };
void forceSoftReset() { };
void defineErrorHandler (void (*cbMethod)()) { };
#endif // END of cross platform support.
// Hardware I/O port map
//
// Port A
//
// 7 ANALOG_CSADC
// 6 ANALOG_CLK
// 5
// 4 RADIO_TX_FREQ_CH1, RADIO_TX2
// 3 ANALOG_DOUT
// 2 RADIO_TX_FREQ_CH2
// 1 LOCAL_IO_PORT1
// 0 LOCAL_IO_PORT0, RADIO_TX1
//
//
// Port B
//
// 7
// 6
// 5 DTMF D3
// 4 DTMF D2
// 3 DTMF D1
// 2 DTMF D0
// 1 DTMF Status
// 0 ANALOG_DIN
//
//
// Port D
//
// 7 HUM_TEMP_SCK / TEMP_CLK
// 6 HUM_TEMP_DATA / TEMP_DATA
// 5 RADIO_DATA_RX2
// 4 RADIO_DATA_RX1
// 3 RADIO_RX_FREQ_CH1
// 2 TNC_M1
// 1 TNC_DATA
// 0 TNC_M0
//
//
// Port E
//
// 7 RADIO_CTCSS_DETECT
// 6 RADIO_CARRIER_DETECT
// 5 REMOTE_TX
// 4 REMOTE_DATA
// 3 RADIO_AUDIO2
// 2 RADIO_AUDIO1
// 1 TNC Input Interrupt
// 0 TNC Input Interrupt
// Public methods and data structures for each class.
int8 analogGetBusVoltage();
int16 analogGetExtTemp();
int16 analogGetIntTemp();
void analogInit();
uint32 analogRead(uint8 channel);
void analogUpdate();
uint16 analogGetStatic();
void camAutoPan();
void camInit();
void camParseMessage();
void camReadMessage();
void camResetTimer();
void camUpdateTick();
uint8 cmdProcess();
void configCalcCRC();
void configDefault();
uint8 configInit();
typedef struct {
uint8 updateFlag, month, day, hours, minutes, seconds;
uint16 year;
int32 latitude, longitude, altitudeMeters, altitudeFeet;
uint16 vSpeed, hSpeed, heading, dop, status;
uint16 trackedSats, visibleSats;
int32 lastAltitude;
int16 vSpeedAccum;
} GPSPOSITION_STRUCT;
void gpsClearUpdate();
GPSPOSITION_STRUCT *gpsGetGPSData();
uint8 gpsInit();
uint8 gpsIsUpdate();
void gpsLogData();
void gpsParsePositionMessage();
void gpsReadMessage();
void gpsSendMessage(uint8 *, uint8);
void localIOInit();
void localIOStartPort1Timer();
void localIOUpdateTick();
// The record type identifiers.
#define LOG_TYPE_TIMESTAMP 0x10
#define LOG_TYPE_COORD 0x11
#define LOG_TYPE_TEMP_HUM 0x12
#define LOG_TYPE_BUSVOLTAGE 0x13
#define LOG_NEXT_BLOCK 0x14
#define LOG_TYPE_TEMP 0x15
void logBlockStart(uint8 recordID);
void logClear();
int32 logGetBlock1();
int32 logGetBlock2();
uint32 logGetBlockSize(int32 logBlock);
void logInit();
void logInt16 (int16 value);
void logInt32 (int32 value);
void logUint16 (uint16 value);
void logUint8 (uint8 value);
typedef struct {
float lat, lon;
int32 alt;
} COORD;
typedef struct {
float dist, head, trackError;
} COURSE;
typedef struct {
uint32 timeStamp;
uint16 timeInterval;
COURSE course;
COORD coord;
} NAV_WINDDATA;
void navCourse (COORD *coord1, COORD *coord2, COURSE *course);
void navDistRadial (COORD *current, COORD *next, float d, float tc);
float navGetDist (COURSE *course);
float navGetHead (COURSE *course);
uint16 navGetWindDataCount();
void navInit();
void navLaunch();
void navRadToDeg (COORD *coord);
void navSetDegFCoord (float lat, float lon, COORD *coord);
void navSetDegICoord (int32 lat, int32 lon, COORD *coord);
// Port A bits used to select transmit frequency.
#define RADIO_TX_FREQ_CH1 4
#define RADIO_TX_FREQ_CH2 2
// Port D bit used to select receive frequency.
#define RADIO_RX_FREQ_CH1 3
// Mode flags used to select transmit frequency.
#define RADIO_TX_APRS 0
#define RADIO_TX_DOWNLINK 1
#define RADIO_TX_CROSSBAND_LOW 2
#define RADIO_TX_CROSSBAND_HIGH 3
void radioDTMFDecode();
void radioInit();
void radioInitTone (uint16 *tone);
void radioSetTxFreq(uint8 mode);
void radioUpdate();
void radioUpdateTone();
/// Remote command to display text at coordinates (x, y) in color c.
#define COMMAND_TITLE 0x31
/// Remote command to execute auto focus camera sequence.
#define COMMAND_FOCUS 0x32
/// Remote command to set the frequency.
#define COMMAND_SET_FREQ 0x33
/// Remote command to set the PA state.
#define COMMAND_SET_PA 0x34
/// Remote command to set the time.
#define COMMAND_SET_TIME 0x35
void remoteTransmit (uint8 command, char *message, uint8 length);
void sysBootMessage(uint8, uint8);
uint16 sysCRC16 (uint8 *, uint16);
void sysDelay(uint32 delayMS);
char *sysGetVersion();
void sysInit();
void sysInterruptEnable();
uint8 sysNMEAChecksum (uint8 *, uint16);
#ifdef WIN32
void sysRuntimeErrHandler();
#else
root void sysRuntimeErrHandler();
#endif
void sysTimerISR();
void tempInit();
int16 tempGetExtTemp();
int16 tempGetIntTemp();
uint8 tempWriteAndGetAck(uint8 data);
uint8 tempReadWithAck (uint8 ack);
void tempMasterStart();
void tempMasterStop();
#define TLM_LOCAL 0
#define TLM_CAM_COMPUTER 1
#define TLM_GPS 2
#define TLM_STATUS 3
#define TLM_NMEA 4
#define TLM_PACKET 5
#define TLM_VER 6
#define TLM_MESSAGE 7
#define TLM_TYPE_STATUS 0x00
#define TLM_TYPE_GPGGA 0x01
#define TLM_TYPE_GPRMC 0x02
void tlmCreateMessage(uint8 *message);
void tlmCreateReport (uint8 reportType, uint8 packetType);
void tlmGPGGAPacket();
void tlmGPRMCPacket();
void tlmGPSStatus();
void tlmInit();
void tlmReport();
void tlmPositionReport();
void tlmPacketReport();
void tlmSetEstimatePacket();
void tlmStatus();
void tlmUpdate();
void tncExternalISR();
void tncInit();
void tncSendPacket(uint8 *);
void tncTimer();
// Boolean flags.
#define TRUE 1
#define FALSE 0
/*
* Class to handle the analog interface board. Utilizes the LTC2428 ADC.
*/
// Define the pins used for I/O control.
#define ANALOG_CSADC 7
#define ANALOG_CLK 6
#define ANALOG_DOUT 3
#define ANALOG_DIN 0
// Define the analog channel port numbers.
#define ANALOG_STATIC 0
#define ANALOG_BUSVOLT 2
#define ANALOG_EXTTEMP 3
#define ANALOG_INTTEMP 6
#define ANALOG_PITOT 7
// Return value to indicate ADC convertion is not complete.
#define ANALOG_NOT_READY 0xffffffff
// Last value read from ADC channel.
uint32 analogData[8];
// Index to the last channel read.
uint8 analogChannel, analogLastChannel;
/**
* The analog bus voltage in 0.1 volt units.
*
* @return analog bus voltage
*/
int8 analogGetBusVoltage()
{
return (uint8) ((int32) (153l * (analogData[ANALOG_BUSVOLT] & 0xfffff) / 1048575l));
}
/**
* External temperature in 0.1 degrees F.
*
* @return external temperature
*/
int16 analogGetExtTemp()
{
return (int16) ((int32) (5000l * (analogData[ANALOG_EXTTEMP] & 0xfffff) / 1048575l)) - 1235;
}
/**
* Internal temperature in 0.1 degrees F.
*
* @return internal temperature
*/
int16 analogGetIntTemp()
{
return (int16) ((int32) (5000l * (analogData[ANALOG_INTTEMP] & 0xfffff) / 1048575l)) - 1235;
}
/**
* Preasure in 0.01 PSI. Transfer function x = 5.0 VDC * 15 PSI / 0xfffff * 4.5 VDC
* 0.83 = 0.25 * (15 / 4.5) = Offset * (FullScalePSI / FullScaleV)
*
* @return preasure
*/
uint16 analogGetStatic()
{
return (uint16) ((uint32) ((200l * (analogData[ANALOG_STATIC] & 0xfffff) / 125829l) - 83l));
}
/**
* Initialize the analog interface board.
*/
void analogInit()
{
uint8 i;
// Clear the analog buffers.
for (i = 0; i < 8; ++i)
analogData[i] = 0;
// Set the ADC for external clocking.
BitWrPortI (PADR, &PADRShadow, 1, ANALOG_CSADC);
BitWrPortI (PADR, &PADRShadow, 0, ANALOG_CLK);
BitWrPortI (PADR, &PADRShadow, 0, ANALOG_CSADC);
// Start the conversion on the first channel.
analogChannel = 0;
analogLastChannel = 0;
analogRead (0);
}
/**
* Read the ADC on the analog interface board. This method returns the value
* of the last channel that was selected.
*
* @param channel number of <i>next</i> channel to read
*
* @return 4-bit status and 20-bit value of last channel programmed
*/
uint32 analogRead(uint8 channel)
{
uint8 i, mask;
uint32 data;
// Only process when we get the /EOC.
if (BitRdPortI(PBDR, ANALOG_DIN) == 1)
return ANALOG_NOT_READY;
// Shift 24-bits from the ADC. These include status and data.
data = 0;
for (i = 0; i < 24; ++i) {
BitWrPortI (PADR, &PADRShadow, 0, ANALOG_CLK);
BitWrPortI (PADR, &PADRShadow, 1, ANALOG_CLK);
data = data << 1;
data |= BitRdPortI (PBDR, ANALOG_DIN);
} // END for
BitWrPortI (PADR, &PADRShadow, 1, ANALOG_CSADC);
// Set the enable bit in the ADC control value.
channel |= 0x08;
// Select the next channel to process.
for (mask = 0x08; mask != 0; mask = mask >> 1) {
if ((channel & mask) != 0x00)
BitWrPortI (PADR, &PADRShadow, 1, ANALOG_DOUT);
else
BitWrPortI (PADR, &PADRShadow, 0, ANALOG_DOUT);
BitWrPortI (PADR, &PADRShadow, 0, ANALOG_CLK);
BitWrPortI (PADR, &PADRShadow, 1, ANALOG_CLK);
} // END for
BitWrPortI (PADR, &PADRShadow, 0, ANALOG_CLK);
BitWrPortI (PADR, &PADRShadow, 0, ANALOG_CSADC);
return data;
}
/**
* Update and read the analog channel values. This method is called periodically
* to check if the ADC conversion is complete. If it is, it reads the ADC value
* and selects the next channel to process.
*/
void analogUpdate()
{
uint32 data;
// Just exit if the conversion is not complete.
if ((data = analogRead(analogChannel)) == ANALOG_NOT_READY)
return;
// Save the data for the last channel we read.
analogData[analogLastChannel] = data;
// Save the new channel so we know where to put the results.
analogLastChannel = analogChannel;
// Sequentially select each channel in the system.
switch (analogChannel) {
case ANALOG_STATIC:
analogChannel = ANALOG_BUSVOLT;
break;
case ANALOG_BUSVOLT:
analogChannel = ANALOG_EXTTEMP;
break;
case ANALOG_EXTTEMP:
analogChannel = ANALOG_INTTEMP;
break;
case ANALOG_INTTEMP:
analogChannel = ANALOG_PITOT;
break;
case ANALOG_PITOT:
analogChannel = ANALOG_STATIC;
break;
} // END switch
}
void atvOverlayUpdate()
{
uint32 coord, coordMin, spd;
uint8 dirFlag;
char message[40], buffer[25];
GPSPOSITION_STRUCT *gpsData;
gpsData = gpsGetGPSData();
switch (gpsData->seconds) {
case 0:
if (gpsData->hours < 7)
buffer[0] = gpsData->hours + 17;
else
buffer[0] = gpsData->hours - 7;
buffer[1] = gpsData->minutes;
remoteTransmit (COMMAND_SET_TIME, buffer, strlen(buffer));
break;
case 1:
case 3:
strcpy (buffer, " http://kd7lmo.net");
remoteTransmit (COMMAND_TITLE, buffer, strlen(buffer));
break;
case 14:
case 16:
// Latitude value.
coord = gpsData->latitude;
if (gpsData->latitude < 0) {
coord = gpsData->latitude * -1;
dirFlag = 0;
} else {
dirFlag = 1;
coord = gpsData->latitude;
}
if (dirFlag == 1)
strcpy (message, "N");
else
strcpy (message, "S");
coordMin = (coord % 3600000) / 6;
sprintf (buffer, "%02ld%02ld.%03ld ", (uint32) (coord / 3600000), (uint32) (coordMin / 10000), (uint32) (coordMin % 10000) / 10);
strcat (message, buffer);
// Longitude value.
if (gpsData->longitude < 0) {
coord = gpsData->longitude * - 1;
dirFlag = 0;
} else {
dirFlag = 1;
coord = gpsData->longitude;
}
if (dirFlag == 1)
strcat (message, "E");
else
strcat (message, "W");
coordMin = (coord % 3600000) / 6;
sprintf (buffer, "%03ld%02ld.%03ld", (uint32) (coord / 3600000), (uint32) (coordMin / 10000), (uint32) (coordMin % 10000) / 10);
strcat (message, buffer);
remoteTransmit (COMMAND_TITLE, message, strlen(message));
break;
case 29:
case 31:
// Show GPS information if it is alive, otherwise show blank fields.
if ((gpsData->status & 0xe000) == 0xe000) {
sprintf (message, "%ld' %ld'/m ", gpsData->altitudeFeet, (int32) (gpsData->vSpeedAccum * 6000l / 12192l));
}
strcpy (message, "---' ---'/m ");
// GPS status
if ((gpsData->status & 0xe000) == 0xe000){
sprintf (buffer, "%d.%d 3D", gpsData->dop / 10, gpsData->dop % 10);
strcat (message, buffer);
} else
if ((gpsData->status & 0xe000) == 0xc000) {
sprintf (buffer, "%d.%d 2D", gpsData->dop / 10, gpsData->dop % 10);
strcat (message, buffer);
} else
strcat (message, "nogps");
remoteTransmit (COMMAND_TITLE, message, strlen(message));
break;
case 44:
case 46:
// Speed knots and heading.
spd = (int32) gpsData->hSpeed * 75000 / 385826;
sprintf (message, "%d.%dkts AT %d.%ddeg", (int16) (spd / 10), (int16) (spd % 10), gpsData->heading / 10, gpsData->heading % 10);
remoteTransmit (COMMAND_TITLE, message, strlen(message));
break;
} // END switch
}
/**
* Class to handle the camera control.
*/
#define CAM_START 1
#define CAM_HEAD2 2
#define CAM_HEAD3 3
#define CAM_DATA 4
#define CAM_MAX_BUFFER 100
#define CAM_PAN_DISABLE 0
#define CAM_PAN_UP_HOR_DOWN 1
#define CAM_PAN_UP_HOR 2
#define CAM_PAN_HOR_DOWN 3
#define CAM_PAN_UP_DOWN 4
#define CAM_UP_POS 0
#define CAM_HOR_POS 1
#define CAM_DOWN_POS 2
// Time periods in units of 5mS. 200 = 1 second
// Time between each pan command. (2 minutes)
#define CAM_PAN_PERIOD 24000
uint8 camMode, camIndex, camBuffer[CAM_MAX_BUFFER], camOutBuffer[CAM_MAX_BUFFER];
uint8 camPanMode, camPosition, camDownloadMode;
uint16 camAnalog1, camAnalog2, camAnalog3, camPanCountDown, camPanPeriod;
void camAutoPan()
{
// Reset the count down timer when it reachs 0.
if (camPanCountDown == 0) {
camPanCountDown = camPanPeriod;
// Determine the next camera position based on state machine.
switch (camPanMode) {
case CAM_PAN_UP_HOR_DOWN:
switch (camPosition) {
case CAM_UP_POS:
camPosition = CAM_HOR_POS;
serDputs ("fc horizon\n\r");
break;
case CAM_HOR_POS:
camPosition = CAM_DOWN_POS;
serDputs ("fc down\n\r");
break;
case CAM_DOWN_POS:
camPosition = CAM_UP_POS;
serDputs ("fc up\n\r");
break;
} // END camPosition switch
break;
case CAM_PAN_UP_HOR:
switch (camPosition) {
case CAM_UP_POS:
camPosition = CAM_HOR_POS;
serDputs ("fc horizon\n\r");
break;
case CAM_HOR_POS:
case CAM_DOWN_POS:
camPosition = CAM_UP_POS;
serDputs ("fc up\n\r");
break;
} // END camPosition switch
break;
case CAM_PAN_HOR_DOWN:
switch (camPosition) {
case CAM_HOR_POS:
camPosition = CAM_DOWN_POS;
serDputs ("fc down\n\r");
break;
case CAM_UP_POS:
case CAM_DOWN_POS:
camPosition = CAM_HOR_POS;
serDputs ("fc horizon\n\r");
break;
} // END camPosition switch
break;
case CAM_PAN_UP_DOWN:
switch (camPosition) {
case CAM_UP_POS:
camPosition = CAM_DOWN_POS;
serDputs ("fc down\n\r");
break;
case CAM_HOR_POS:
case CAM_DOWN_POS:
camPosition = CAM_UP_POS;
serDputs ("fc up\n\r");
break;
} // END camPosition switch
break;
} // END switch
} // END if
}
/**
* Initalize the camera control communications. The camera controller is a seperate module
* that is connected via an asyc serial port.
*/
void camInit()
{
camPanMode = CAM_PAN_UP_HOR_DOWN;
camPosition = CAM_HOR_POS;
camPanCountDown = 0;
camPanPeriod = CAM_PAN_PERIOD;
camMode = CAM_START;
camAnalog1 = 0;
camAnalog2 = 0;
camAnalog3 = 0;
strcpy (camOutBuffer, "no com from camera computer");
camDownloadMode = FALSE;
}
/**
* Parse and save the data elements that are in the string from the camera controller.
*/
void camParseMessage()
{
uint16 i;
i = 0;
// Save the raw camera control string.
strcpy (camOutBuffer, camBuffer);
// Skip to the first comma.
while (camBuffer[i] != 0 && camBuffer[i] != ',')
++i;
if (camBuffer[i] == 0)
return;
// Skip to the second comma.
++i;
while (camBuffer[i] != 0 && camBuffer[i] != ',')
++i;
if (camBuffer[i] == 0)
return;
++i;
camAnalog1 = atoi (camBuffer + i);
while (camBuffer[i] != 0 && camBuffer[i] != ',')
++i;
if (camBuffer[i] == 0)
return;
++i;
camAnalog2 = atoi (camBuffer + i);
while (camBuffer[i] != 0 && camBuffer[i] != ',')
++i;
if (camBuffer[i] == 0)
return;
camAnalog3 = atoi (camBuffer + i + 1);
// Save the results for telemetry display and include the parsed values if sucessful.
sprintf (camOutBuffer, "'%s', %d, %d, %d", camBuffer, camAnalog1, camAnalog2, camAnalog3);
}
/**
* Read the camera control message from the serial port. The message must start with $CC and end
* with a <CR> or <LF>.
*/
void camReadMessage()
{
uint32 i, j;
int32 logBase;
int serialData;
uint8 readyFlag, value, buffer[80];
while ((serialData = serDgetc()) != -1) {
value = (serialData & 0xff);
switch (camMode) {
case CAM_START:
if (value == '$')
camMode = CAM_HEAD2;
break;
case CAM_HEAD2:
if (value == 'C')
camMode = CAM_HEAD3;
else
camMode = CAM_START;
break;
case CAM_HEAD3:
switch (value) {
case 'D':
// Disable packets and the transmitter while downloading.
camDownloadMode = TRUE;
// Change the baud rate to allow for faster download and wait for the '*' character.
serDclose();
serDopen (57600);
readyFlag = FALSE;
while (!readyFlag) {
sysDelay (500);
serDputs ("*");
if (serDgetc() == '*')
readyFlag = TRUE;
}
// Send the size of the log blocks in bytes.
sprintf (buffer, "\n\rBEGIN %ld %ld\n\r", logGetBlockSize(logGetBlock1()), logGetBlockSize(logGetBlock2()));
serDputs (buffer);
// Dump each block of log memory.
logBase = logGetBlock1();
while (logBase != 0) {
i = 0;
// Dump a complete block of memory.
while (i < logGetBlockSize(logBase)) {
sprintf (buffer, "%06lx ", i);
serDputs (buffer);
for (j = 0; j < 16; ++j) {
xmem2root (&value, logBase + i, 1);
sprintf (buffer, "%02x ", value);
serDputs (buffer);
++i;
} // END for
serDputs ("\n\r");
// If the operator sends the '*' character, abort the download.
if (serDgetc() == '*') {
i = logGetBlockSize(logBase);
logBase = 0;
} // END if
} // END while i < logGetBlockSize()
if (logBase == logGetBlock1())
logBase = logGetBlock2();
else
logBase = 0;
} // END while logBase != 0
// Send the final message and wait for it to transmit before setting baud rate.
serDputs ("\n\rEND\n\rBaud rate changed, press '*' to continue.");
while (serDwrFree() != PORTD_BUFFER_SIZE);
serDclose();
serDopen (4800);
// Wait for the operator before continuing.
while (serDgetc() != '*');
// Enable the packet transmit.
camDownloadMode = FALSE;
break;
case 'C':
camMode = CAM_DATA;
camIndex = 0;
break;
case '!':
serDputs ("alive\n\r");
camMode = CAM_START;
break;
default:
camMode = CAM_START;
break;
} // END switch
break;
case CAM_DATA:
camBuffer[camIndex] = value;
// If we see a <CR> or <LF>, then NULL terminate the string.
if (value == 13 || value == 10) {
camBuffer[camIndex] = 0;
camParseMessage();
camMode = CAM_START;
}
// If we fill the buffer without a <CR> or <LF>, the data wasn't good.
if (++camIndex == CAM_MAX_BUFFER)
camMode = CAM_START;
break;
} // END switch
} // END while
}
/**
* Reset the camera control timer.
*/
void camResetTimer()
{
camPanCountDown = 0;
}
/**
* This method is called periodically to update the internal timer.
*/
void camUpdateTick()
{
if (camPanCountDown != 0)
--camPanCountDown;
}
/**
* Class to handle the configuration data.
*/
typedef struct {
uint16 crc;
uint8 analogFilter, analogReadCount, txDelay;
int32 internalTempOffset, externalTempOffset, busVoltageScale;
uint8 callSign[7], destCallSign[7], relayCallSign1[7], relayCallSign2[7];
uint8 callSignSSID, callSignLandingZoneSSID, relayCallSignSSID1, relayCallSignSSID2;
uint16 bootCount, flightTime;
} CONFIG_STRUCT;
#ifdef WIN32
CONFIG_STRUCT config;
#else
protected CONFIG_STRUCT config;
#endif
/**
* Calculate and set the configuration parameter block CRC.
*/
void configCalcCRC()
{
config.crc = sysCRC16 ((uint8 *) &config + 2, sizeof(CONFIG_STRUCT) - 2);
}
/**
* Set the default configuration parameters.
*/
void configDefault()
{
char buffer[40];
// Station ID, relay path, and destination call sign and SSID.
strcpy (config.callSign, "KD7LMO");
config.callSignSSID = 11;
config.callSignLandingZoneSSID = 0;
strcpy (config.destCallSign, "APRS ");
strcpy (config.relayCallSign1, "GATE ");
strcpy (config.relayCallSign2, "WIDE3 ");
config.relayCallSignSSID1 = 0;
config.relayCallSignSSID2 = 3;
// Number of TNC flag bytes sent before data stream starts. (350mS) 1 byte = 6.6mS
config.txDelay = 53;
// Count the number of system boots.
config.bootCount = 0;
// Flight operation time.
config.flightTime = 0;
serDputs ("done.\n\rAllocating log space...");
// Allocate and initialize the log subsystem.
logInit();
logClear();
// Let the user know what is going on.
sprintf (buffer, "done. blk1 0x%lx, blk2 0x%lx\n\r", logGetBlock1(), logGetBlock2());
serDputs (buffer);
}
/**
* Initialize configuration subsystem.
*/
uint8 configInit()
{
// Let the user know what is going on.
serDputs ("Checking configuration...");
// If the configuration CRC is not valid, then set default vaules.
if (config.crc != sysCRC16((uint8 *) &config + 2, sizeof(CONFIG_STRUCT) - 2)) {
serDputs ("invalid CRC.\n\rSetting defaults...");
configDefault();
navInit();
serDputs ("Setting CRC...");
configCalcCRC();
serDputs ("done.\n\r");
return FALSE;
}
// We just increment the boot counter to keep track of power on reset or power faults.
++config.bootCount;
configCalcCRC();
// Lets everyone know it is good.
serDputs ("good.\n\r");
return TRUE;
}
/**
* Class to handle the GPS receiver.
*/
#define GPS_START 0
#define GPS_START2 1
#define GPS_COMMAND1 2
#define GPS_COMMAND2 3
#define GPS_READMESSAGE 4
#define GPS_CHECKSUMMESSAGE 5
#define GPS_EOMCR 6
#define GPS_EOMLF 7
GPSPOSITION_STRUCT gpsPosition;
uint8 gpsMode, gpsIndex, gpsChecksum, gpsBuffer[80];
/**
* Clear the GPS update flag. The flag is set when a new GPS message is parsed
* and cleared by this method.
*
*/
void gpsClearUpdate()
{
gpsPosition.updateFlag = FALSE;
}
GPSPOSITION_STRUCT *gpsGetGPSData()
{
return &gpsPosition;
}
/**
* Configure the GPS receiver.
*/
uint8 gpsInit()
{
uint8 retryCount;
uint32 gpsStartTime;
serDputs ("Checking GPS status...");
// Clear the structure that stores the position message.
memset (&gpsPosition, 0, sizeof(GPSPOSITION_STRUCT));
// State machine used for parsing the binary GPS string.
gpsMode = GPS_START;
// Get everything set.
gpsStartTime = SEC_TIMER;
retryCount = 0;
gpsPosition.updateFlag = FALSE;
// Try 10 seconds to get a valid GPS engine response.
while (retryCount < 10) {
// Wait for 1 second to pass.
while (gpsStartTime == SEC_TIMER);
gpsStartTime = SEC_TIMER;
// Parse the incoming GPS message.
gpsReadMessage();
// Wait for the GPS message seconds value to change.
if (gpsPosition.updateFlag == TRUE) {
gpsPosition.updateFlag = FALSE;
serDputs ("good.\n\r");
return TRUE;
}
// Tell the GPS to send a status message once a second.
gpsSendMessage ("Hb\001", 3);
serDputs (".");
++retryCount;
} // END while
serDputs ("fault.\n\r");
return FALSE;
}
/**
* Determine if the GPS position has updated.
*
* @return true if updated; otherwise false
*/
uint8 gpsIsUpdate()
{
return gpsPosition.updateFlag;
}
/**
* Log the current GPS position and analog telemetry values.
*/
void gpsLogData()
{
uint8 status;
// Log the bus voltage once a minute.
if (gpsPosition.seconds == 0) {
logBlockStart (LOG_TYPE_BUSVOLTAGE);
logUint8 (analogGetBusVoltage());
} // END if
// Log the internal and external temperature every 15 seconds.
if (gpsPosition.seconds == 0 || gpsPosition.seconds == 15 || gpsPosition.seconds == 30 || gpsPosition.seconds == 45) {
logBlockStart (LOG_TYPE_TEMP);
logInt16 (tempGetIntTemp());
logInt16 (analogGetIntTemp());
logInt16 (tempGetExtTemp());
}
// Log the data.
logBlockStart (LOG_TYPE_COORD);
logUint8 (gpsPosition.hours);
logUint8 (gpsPosition.minutes);
logUint8 (gpsPosition.seconds);
logInt32 (gpsPosition.latitude);
logInt32 (gpsPosition.longitude);
logInt32 (gpsPosition.altitudeMeters);
if ((gpsPosition.status & 0xe000) == 0xe000)
status = 0x80;
else
status = 0x00;
logUint8 ((uint8) ((gpsPosition.dop & 0x7f) | status));
logUint8 ((uint8) ((gpsPosition.visibleSats << 4) | gpsPosition.trackedSats));
logUint16 ((uint16) ((analogData[ANALOG_STATIC] & 0xfffff) >> 4));
logUint16 ((uint16) ((analogData[ANALOG_PITOT] & 0xfffff) >> 4));
}
/**
* 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 = (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.altitudeMeters = ((int32) gpsBuffer[19] << 24) | ((int32) gpsBuffer[20] << 16) | ((int32) gpsBuffer[21] << 8) | gpsBuffer[22];
gpsPosition.altitudeFeet = gpsPosition.altitudeMeters * 100l / 3048l;
gpsPosition.vSpeed = (gpsBuffer[27] << 8) | gpsBuffer[28];
gpsPosition.hSpeed = (gpsBuffer[29] << 8) | gpsBuffer[30];
gpsPosition.heading = (gpsBuffer[31] << 8) | gpsBuffer[32];
gpsPosition.dop = (gpsBuffer[33] << 8) | gpsBuffer[34];
gpsPosition.visibleSats = gpsBuffer[35];
gpsPosition.trackedSats = gpsBuffer[36];
gpsPosition.status = (gpsBuffer[37] << 8) | gpsBuffer[38];
// Generate a VSI (Vertical Speed Indicator) by low pass filtering the change in altitude.
gpsPosition.vSpeedAccum = (int16) (gpsPosition.altitudeMeters - gpsPosition.lastAltitude) + gpsPosition.vSpeedAccum - (gpsPosition.vSpeedAccum >> 2);
gpsPosition.lastAltitude = gpsPosition.altitudeMeters;
}
/**
* Read and validate the GPS message. When a valid message has been read, the structure
* <b>gpsMessage</b> is updated.
*/
void gpsReadMessage()
{
uint16 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<CR><LF>
while ((value = serCgetc()) != -1)
switch (gpsMode) {
// Wait for the first @
case GPS_START:
if (value == '@')
gpsMode = GPS_START2;
break;
case GPS_START2:
if (value == '@')
gpsMode = GPS_COMMAND1;
else
gpsMode = GPS_START;
break;
case GPS_COMMAND1:
if (value == 'H')
gpsMode = GPS_COMMAND2;
else
gpsMode = GPS_START;
break;
case GPS_COMMAND2:
if (value == 'b') {
gpsMode = GPS_READMESSAGE;
gpsIndex = 0;
gpsChecksum = 0;
gpsChecksum ^= 'H';
gpsChecksum ^= 'b';
} else
gpsMode = GPS_START;
break;
case GPS_READMESSAGE:
gpsChecksum ^= value;
gpsBuffer[gpsIndex++] = (uint8) value;
if (gpsIndex == 47)
gpsMode = GPS_CHECKSUMMESSAGE;
break;
case GPS_CHECKSUMMESSAGE:
if (gpsChecksum == value)
gpsMode = GPS_EOMCR;
else
gpsMode = GPS_START;
break;
case GPS_EOMCR:
if (value == 13)
gpsMode = GPS_EOMLF;
else
gpsMode = GPS_START;
break;
case GPS_EOMLF:
// Once we have the last character, convert the binary message to something usable.
if (value == 10)
gpsParsePositionMessage();
gpsMode = GPS_START;
break;
} // END switch
}
/**
* Sends a binary message to the GPS receiver. The message
* contains only the data between the start of message '@@' and
* the checksum end EOM. THe rest is generated by this method.
*
* @param message pointer to array of binary data
* @param length length of message in bytes
*/
void gpsSendMessage (uint8 *message, uint8 length)
{
uint8 i, checksum;
// Send the message start characters.
serCputc ('@');
serCputc ('@');
// Send each character while calculating the checksum.
checksum = 0;
for (i = 0; i < length; ++i) {
checksum ^= message[i];
serCputc (message[i]);
} // END for
// Now send the checksum, <CR>, and <LF>.
serCputc (checksum);
serCputc (13);
serCputc (10);
}
/**
* Class to handle the local I/O functions.
*/
// Time periods in units of 5mS. 200 = 1 second
// Time to hold cut down I/O port high. (10 seconds).
#define LOCAL_IO_CUTDOWN_PERIOD 2000
// Defin the bit maps for the local I/O ports.
#define LOCAL_IO_PORT0 0
#define LOCAL_IO_PORT1 1
uint16 localIOCountDown;
/**
* Initalize the local I/O control and timers. NOTE: The I/O ports are set to low
* in the <b>rabbitInit</b> method.
*/
void localIOInit()
{
localIOCountDown = 0;
}
/**
* Turn on output 1 and start a timer that will turn it off after a period of time.
*/
void localIOStartPort1Timer()
{
BitWrPortI (PADR, &PADRShadow, 1, LOCAL_IO_PORT1);
localIOCountDown = LOCAL_IO_CUTDOWN_PERIOD;
}
/**
* This method is called periodically to update the internal timer.
*/
void localIOUpdateTick()
{
// Shut off port 1 after the timer expires.
if (localIOCountDown != 0)
if (--localIOCountDown == 0)
BitWrPortI (PADR, &PADRShadow, 0, LOCAL_IO_PORT1);
}
/**
* Class to handle the flight data recorder log.
*/
// The size of each log block. A log block can not exceed 0x3ffff bytes, so we break it into two sections.
#define LOG_SIZE_BLOCK1 0x40000
#define LOG_SIZE_BLOCK2 0x32000
#ifdef WIN32
uint8 logFlag;
int32 logPointer, logBlock1, logBlock2;
uint32 logIndex;
#else
protected uint8 logFlag;
protected int32 logPointer, logBlock1, logBlock2;
protected uint32 logIndex;
#endif
/**
* Verifies the log has space left, changes blocks if required, and saves the <b>recordID</b>.
*
* @param recordID of the next block
*
*/
void logBlockStart(uint8 recordID)
{
// Only write to the log if it is enabled.
if (!logFlag)
return;
// If we are in block 1 and out of space, switch to block 2.
if (logPointer == logBlock1 && logIndex + 32 > LOG_SIZE_BLOCK1) {
// Record a value so the decoder knows to switch blocks.
logUint8 (LOG_NEXT_BLOCK);
// Switch to block 2.
logPointer = logBlock2;
logIndex = 0;
} // END if
// If we are in block 2 and out of space, just stop logging.
if (logPointer == logBlock2 && logIndex + 32 > LOG_SIZE_BLOCK2) {
logFlag = FALSE;
return;
} // END if
// Just record the ID of this block.
logUint8 (recordID);
}
/**
* Clear and enable the data log.
*/
void logClear()
{
logPointer = logBlock1;
logIndex = 0;
logFlag = TRUE;
}
/**
* Return an extended memory pointer to log block 1.
*
* @return extended memory pointer
*/
int32 logGetBlock1()
{
return logBlock1;
}
/**
* Return an extended memory pointer to log block 2.
*
* @return extended memory pointer
*/
int32 logGetBlock2()
{
return logBlock2;
}
/**
* The size of the log block referenced by <b>logBase</b>.
*
* @return log block size in bytes
*/
uint32 logGetBlockSize(int32 logBase)
{
if (logBase == logBlock1)
return LOG_SIZE_BLOCK1;
if (logBase == logBlock2)
return LOG_SIZE_BLOCK2;
return 0;
}
/**
* Initialize the log system.
*/
void logInit()
{
logBlock1 = xalloc (LOG_SIZE_BLOCK1);
logBlock2 = xalloc (LOG_SIZE_BLOCK2);
}
/**
* Save the signed, 16-bit <b>value</b> in the log.
*/
void logInt16 (int16 value)
{
if (logFlag) {
root2xmem (logPointer + logIndex, &value, 2);
logIndex += 2;
}
}
/**
* Save the signed, 32-bit <b>value</b> in the log.
*/
void logInt32 (int32 value)
{
if (logFlag) {
root2xmem (logPointer + logIndex, &value, 4);
logIndex += 4;
}
}
/**
* Save the unsigned, 16-bit <b>value</b> in the log.
*/
void logUint16 (uint16 value)
{
if (logFlag) {
root2xmem (logPointer + logIndex, &value, 2);
logIndex += 2;
}
}
/**
* Save the unsigned, 8-bit <b>value</b> in the log.
*/
void logUint8 (uint8 value)
{
if (logFlag) {
root2xmem (logPointer + logIndex, &value, 1);
++logIndex;
}
}
/**
* Class to navigation.
*/
#define NAV_INTERVAL 500
#define NAV_BLOCKCOUNT 200
#ifdef WIN32
uint8 burstDetect;
int32 maxAltitude;
NAV_WINDDATA windData[NAV_BLOCKCOUNT];
#else
protected uint8 burstDetect;
protected int32 maxAltitude;
protected NAV_WINDDATA windData[NAV_BLOCKCOUNT];
#endif
/**
* Calculate the distance and heading from <b>coord1</b> to <b>coord2</b>. The
* coordinate values must be in radians. The result saved in <b>course</b> is
* in units of radians.
*
* @param coord1 start location
* @param coord2 end location
* @param course distance and heading
*/
void navCourse (COORD *coord1, COORD *coord2, COURSE *course)
{
float d, lat1, lon1, lat2, lon2, angle;
lat1 = coord1->lat;
lon1 = coord1->lon;
lat2 = coord2->lat;
lon2 = coord2->lon;
d = 2 * asin(sqrt( sin((lat1-lat2)/2)*sin((lat1-lat2)/2) + cos(lat1)*cos(lat2)*sin((lon1-lon2)/2)*sin((lon1-lon2)/2) ));
course->dist = d;
if (d < 0.00000027) {
course->head = 0;
return;
}
if (sin(lon2-lon1) < 0) {
angle = (sin(lat2)-sin(lat1)*cos(d)) / (sin(d)*cos(lat1));
if (angle < -1.0)
angle = -1.0;
if (angle > 1.0)
angle = 1.0;
course->head = acos(angle);
} else {
angle = (sin(lat2)-sin(lat1)*cos(d)) / (sin(d)*cos(lat1));
if (angle < -1.0)
angle = -1.0;
if (angle > 1.0)
angle = 1.0;
course->head = 2*PI - acos( angle );
}
}
/**
* Use a 4th order polynomial to calculate the descent rate in feet/second.
*
* @param alt altitude in feet
*
* @return descent in feet per second
*/
float navDescentRate (float alt)
{
return -1E-18*alt*alt*alt*alt + 3E-13*alt*alt*alt - 8E-09*alt*alt + 0.0004*alt + 13.522; // 4th order ANSR-13
// return -5E-18*alt*alt*alt*alt + 9E-13*alt*alt*alt - 4E-08*alt*alt + 0.0010*alt + 9.3197; // 4th order ANSR-9
// return -1E-18*alt*alt*alt*alt + 4E-13*alt*alt*alt - 2E-08*alt*alt + 0.0012*alt + 6.0886; // 4th order ANSR-8
}
/**
* Add the vector with a length <b>d</b> and true course <b>tc</b> to the
* lat/long <b>current</b> and save the result in <b>next</b>.
*
* @param current lat/lon coordinates
* @param next lat/lon after vector added
* @param d distance in radians
* @param tc true course in radians
*/
void navDistRadial (COORD *current, COORD *next, float d, float tc)
{
next->lat = asin(sin(current->lat) * cos(d) + cos(current->lat) * sin(d) * cos(tc));
next->lon = current->lon - asin(sin(tc) * sin(d) / cos(next->lat));
}
/**
* Return the course distance in nautical miles.
*
* @return distance
*/
float navGetDist (COURSE *course)
{
return course->dist * 180.0 * 60.0 / PI;
}
int32 navGetMaxAltitude()
{
return maxAltitude;
}
/**
* Get the number of wind data vectors currently saved.
*
* @return number of wind vectors
*/
uint16 navGetWindDataCount()
{
uint16 i;
for (i = 0; i < NAV_BLOCKCOUNT; ++i)
if (windData[i].coord.alt == 0)
return i;
return NAV_BLOCKCOUNT;
}
/**
* Initialize the navigation data arrays.
*/
void navInit()
{
uint16 i;
NAV_WINDDATA *wind;
for (i = 0; i < NAV_BLOCKCOUNT; ++i) {
wind = windData + i;
wind->timeStamp = 0;
wind->timeInterval = 0;
wind->course.dist = 0;
wind->course.head = 0;
wind->course.trackError = 0;
wind->coord.lat = 0;
wind->coord.lon = 0;
wind->coord.alt = 0;
} // END for
maxAltitude = 0;
burstDetect = FALSE;
}
void navLaunch(GPSPOSITION_STRUCT *gps)
{
navInit();
navSetDegICoord(gps->latitude, gps->longitude, &windData[0].coord);
windData[0].coord.alt = gps->altitudeFeet;
windData[0].timeStamp = gps->hours * 3600 + gps->minutes * 60 + gps->seconds;
}
/**
* Convert coordinates from radians to decimal degrees.
*
* @param coord pointer to coordinate pair
*/
void navRadToDeg (COORD *coord)
{
coord->lat *= 180.0 / PI;
coord->lon *= -180.0 / PI;
}
/**
* Convert coordinates (lat, lon) to radians and store in coord.
*
* @param lat in degrees where north is positive
* @param lon in degrees where east is positive
* @param coord coordinate pair in radians
*/
void navSetDegFCoord (float lat, float lon, COORD *coord)
{
coord->lat = lat * PI / 180.0;
coord->lon = -lon * PI / 180.0;
}
/**
* Convert coordinates (lat, lon) to radians and store in coord. The (lat, lon) pair
* is expected in milli-seconds of arc. The GPS engine provides the location in
* this format.
*
* @param lat in milli-seconds of arc where north is positive
* @param lon in milli-seconds of arc where east is positive
* @param coord coordinate pair in radians
*/
void navSetDegICoord (int32 lat, int32 lon, COORD *coord)
{
coord->lat = ((float) lat / 3600000.0) * PI / 180.0;
coord->lon = -((float) lon / 3600000.0) * PI / 180.0;
}
/**
* Course heading in true degrees.
*
* @return heading
*/
float navGetHead (COURSE *course)
{
return course->head * 180.0 / PI;
}
uint8 navUpdate (GPSPOSITION_STRUCT *gps, COORD *landingZone)
{
uint16 i;
int32 alt;
float segmentRate, distance;
COORD positionEstimate, next;
// Track the maximum altitude.
if (gps->altitudeFeet > maxAltitude)
maxAltitude = gps->altitudeFeet;
// Calculate the landing after burst.
if (burstDetect) {
// Find the last entry in the table.
for (i = 0; i < NAV_BLOCKCOUNT && gps->altitudeFeet > windData[i].coord.alt; ++i);
if (i < 2)
return FALSE;
--i;
alt = gps->altitudeFeet;
navSetDegICoord (gps->latitude, gps->longitude, &positionEstimate);
while (i != 0) {
segmentRate = ((float) (alt - windData[i].coord.alt)) / navDescentRate((float) windData[i].coord.alt);
distance = windData[i].course.dist * segmentRate / (float) windData[i].timeInterval;
navDistRadial (&positionEstimate, &next, distance, windData[i].course.head);
positionEstimate = next;
alt = windData[i].coord.alt;
--i;
}
*landingZone = positionEstimate;
return TRUE;
} // END if
// Set a flag to indicate we have a burst condition.
if (gps->altitudeFeet + 500 < maxAltitude) {
burstDetect = TRUE;
return FALSE;
}
// Find the last entry in the table.
for (i = 0; i < NAV_BLOCKCOUNT && windData[i].coord.alt != 0; ++i);
if (i == NAV_BLOCKCOUNT)
return FALSE;
// Calculate the wind speed for this altitude interval.
if (gps->altitudeFeet > windData[i - 1].coord.alt + NAV_INTERVAL) {
// Save the current position as the end point for this segment.
navSetDegICoord(gps->latitude, gps->longitude, &windData[i].coord);
windData[i].coord.alt = gps->altitudeFeet;
windData[i].timeStamp = gps->hours * 3600 + gps->minutes * 60 + gps->seconds;
// Calcualte the course and heading from the last element in this segment.
navCourse (&windData[i - 1].coord, &windData[i].coord, &windData[i].course);
// Calculate the time interval for this segment.
windData[i].timeInterval = (uint16) (windData[i].timeStamp - windData[i - 1].timeStamp);
} // END if
return FALSE;
}
/**
* Class to handle short range, local remote devices.
*/
// Port E bits used for control and data.
#define REMOTE_DATA 4
#define REMOTE_TX 5
#define REMOTE_MAX_MESSAGE 160
#define REMOTE_SELFTEST 0x21
#define REMOTE_CUTDOWN 0x22
// Control state machine.
uint8 remoteBit, remoteIndex, remoteMessageLen, remoteTransmitByte, remoteBuffer[REMOTE_MAX_MESSAGE], remoteMessageCount;
uint8 remoteCommand, remoteRepeatCount;
void remoteInit()
{
// Put the transmiter in low power mode.
BitWrPortI (PEDR, &PEDRShadow, 0, REMOTE_TX);
BitWrPortI (PEDR, &PEDRShadow, 0, REMOTE_DATA);
remoteRepeatCount = 0;
remoteMessageCount = 0;
remoteBit = 0;
remoteMessageLen = 0;
remoteIndex = REMOTE_MAX_MESSAGE;
remoteTransmitByte = 0x00;
}
/**
* Encode the <b>value</b> into a 16-bit manchester encoded bit stream in
* the <b>buffer</b>. Move the buffer pointer forward two bytes.
*
* @param value to encode
* @param buffer to write values to
*/
void remoteManchesterEncode(uint8 value, char **buffer)
{
uint8 i, encodedValue;
for (i = 0, encodedValue = 0; i < 4; ++i) {
encodedValue = (encodedValue << 2) | ((value & 0x80) ? 0x02 : 0x01);
value = value << 1;
} // END for
**buffer = encodedValue;
++(*buffer);
for (i = 0, encodedValue = 0; i < 4; ++i) {
encodedValue = (encodedValue << 2) | ((value & 0x80) ? 0x02 : 0x01);
value = value << 1;
} // END for
**buffer = encodedValue;
++(*buffer);
}
void remoteSend()
{
char buffer[10];
uint8 hold;
if (remoteRepeatCount == 0)
return;
--remoteRepeatCount;
sprintf (buffer, "%02x%02x%02x", remoteMessageCount, remoteMessageCount ^ 0xff, remoteRepeatCount);
++remoteMessageCount;
hold = remoteRepeatCount;
remoteRepeatCount = 0;
remoteTransmit (remoteCommand, buffer, 6);
remoteRepeatCount = hold;
}
void remoteSelfTest()
{
remoteCommand = REMOTE_SELFTEST;
remoteRepeatCount = 5;
}
void remoteCutDown()
{
remoteCommand = REMOTE_CUTDOWN;
remoteRepeatCount = 10;
}
/**
* This method is called at the 1200 baud bit rate (833uS) to send each bit in the
* remote message.
*/
void remoteTimer()
{
uint8 i;
// If the index into the buffer is at the end, there is nothing to do.
if (remoteIndex == REMOTE_MAX_MESSAGE)
return;
// The first thing we do is write the bit to prevent output jitter.
BitWrPortI (PEDR, &PEDRShadow, ((remoteTransmitByte & 0x80) ? 1 : 0), REMOTE_DATA);
// Shift out 8 bits in the register before we get the next byte.
if (++remoteBit != 8) {
remoteTransmitByte = remoteTransmitByte << 1;
return;
} // END if
// If we at the last byte, set the index to the EOM and shutdown the transmitter.
if (remoteIndex == remoteMessageLen) {
remoteIndex = REMOTE_MAX_MESSAGE;
BitWrPortI (PEDR, &PEDRShadow, 0, REMOTE_TX);
return;
}
// Get the next byte in the buffer.
remoteTransmitByte = remoteBuffer[remoteIndex++];
// Reset the bit counter.
remoteBit = 0;
}
/**
* Prepare the remote message for transmission. If a transmission is already in
* progress, the message is not sent.
*
* @param command 8-bit command value
* @param message variable length binary message to send
* @param length of the message in bytes
*/
void remoteTransmit (uint8 command, char *message, uint8 length)
{
uint8 i, *buffer;
uint8 text[80], crcBuffer[(REMOTE_MAX_MESSAGE - 20) / 2];
uint16 crc;
if (remoteRepeatCount != 0)
return;
// Ignore this message because we are already transmitting.
if (remoteIndex != REMOTE_MAX_MESSAGE)
return;
// Make sure the message isn't too long.
if (2 * length > REMOTE_MAX_MESSAGE - 16)
return;
if (command == COMMAND_TITLE)
message[20] = 0;
// serDputs ("*** Transmitting....\n\r");
// Turn on the the transmitter and key it.
BitWrPortI (PEDR, &PEDRShadow, 1, REMOTE_DATA);
BitWrPortI (PEDR, &PEDRShadow, 1, REMOTE_TX);
// Fill in the bit stream.
// Set a pointer to the output buffer.
buffer = remoteBuffer;
// Bit sync
remoteManchesterEncode (0xcc, &buffer);
remoteManchesterEncode (0xcc, &buffer);
// Frame sync.
remoteManchesterEncode (0x18, &buffer);
remoteManchesterEncode (0x47, &buffer);
remoteManchesterEncode (0x70, &buffer);
remoteManchesterEncode (0x25, &buffer);
// Command and length.
remoteManchesterEncode (command, &buffer);
remoteManchesterEncode (length, &buffer);
crcBuffer[0] = command;
crcBuffer[1] = length;
// Command data.
for (i = 0; i < length; ++i) {
remoteManchesterEncode (message[i], &buffer);
crcBuffer[i + 2] = message[i];
}
crc = sysCRC16 (crcBuffer, length + 2);
remoteManchesterEncode (crc >> 8, &buffer);
remoteManchesterEncode (crc & 0xff, &buffer);
// We will transmit 8 ones for the first byte.
remoteTransmitByte = 0xff;
// Counter to keep track of which bit we are sending.
remoteBit = 0;
// The length of the message in bytes.
remoteMessageLen = 2 * length + 20;
// An index into the buffer that contains the message.
remoteIndex = 0;
// for (i = 0; i < remoteMessageLen; ++i) {
// sprintf (text, "%d 0x%x\n\r", i, remoteBuffer[i]);
// serDputs (text);
// } // END if
}
/**
* Class to handle system and generic functions.
*/
/**
* Display the system start message via the TNC.
*/
void sysBootMessage(uint8 configStatus, uint8 gpsStatus)
{
uint8 buffer[80];
// Assemble a start up message for use on the TNC and RS-232 ports.
if (configStatus == TRUE)
sprintf (buffer, "%s - memory OK, boot count %d, ", sysGetVersion(), config.bootCount);
else
sprintf (buffer, "%s - memory initialized, ", sysGetVersion());
if (gpsStatus == TRUE)
strcat (buffer, "GPS active");
else
strcat (buffer, "GPS fault");
// Now send it to the RS-232 port and TNC.
serDputs (buffer);
serDputs ("\n\r");
tlmCreateMessage(buffer);
}
/**
* Calculate the CRC-16 CCITT of <b>buffer</b> that is <b>length</b> bytes long.
*
* @param buffer Pointer to data buffer.
* @param length number of bytes in data buffer
*
* @return CRC-16 of buffer[0 .. length]
*/
uint16 sysCRC16 (uint8 *buffer, uint16 length)
{
uint16 i, bit, crc, value;
crc = 0xffff;
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;
}
/**
* Wait <b>delayMS</b> before returning.
*
* @param delayMS delay time in milliseconds
*/
void sysDelay(uint32 delayMS)
{
uint32 timerTick;
timerTick = MS_TIMER;
while (MS_TIMER - timerTick < delayMS);
}
/**
* Return the software version string.
*
* @return software version string
*/
char *sysGetVersion()
{
return ">ANSR Payload Computer V1.12";
}
/**
* Configures the on chip ports. Rather than have each subsystem setup
* their ports, we do it all in one place to reduce the chance of making a mistake.
*/
void sysInit()
{
// We don't need to run that fast, so slow down to save power.
clockDoublerOff();
// Configure serial port C for GPS.
serCopen (9600);
// Configure serial port D for NMEA/flight computer control.
serDopen (4800);
serDputs ("\n\rInitializing...");
// ****** Configure PORT-A ******
// All outputs low.
WrPortI (PADR, &PADRShadow, 0xc0);
// Port A all outputs.
WrPortI (SPCR, NULL, 0x84);
// ****** Configure PORT-D ******
// All outputs LOW.
WrPortI(PDDR, &PDDRShadow, 0x00);
// Port D no alt TXA/TXB
WrPortI(PDFR, NULL, 0x00);
// Port D PD0-5 are outputs, PD6-7 inputs
WrPortI(PDDDR, &PDDDRShadow, 0x3f);
// PORT D PD0-5 push-pull, PD6-7 open drain
WrPortI(PDDCR, NULL, 0xc0);
// Clock PD0-3 bits on timer B1 interrupt.
WrPortI(PDCR, NULL, 0x02);
// ****** Configure PORT-E ******
// Port E, PE0, PE1, PE6, PE7 as inputs, PE2-PE5 as outputs
WrPortI (PEDDR, NULL, 0x3c);
// Port E all pins as I/O.
WrPortI (PEFR, NULL, 0x00);
// ****** Configure External Interrupt for TNC ******
// Set external ISR to priority 3.
SetVectExtern2000 (0x03, tncExternalISR);
// ****** Configure Timer A & B for TNC and radio ******
// Set the timer B ISR.
SetVectIntern (0x0b, tncTimer);
// Set the timer A4 ISR.
SetVectIntern (0x0a, sysTimerISR);
// Set Timer A1 value that feeds timer A5 and B.
// rate = (timerValue + 1) * 2 * clockPeriod
WrPortI (TAT1R, &TAT1RShadow, 191);
// Set timer A4 value that provides 5mS radio interrupt.
WrPortI (TAT5R, &TAT5RShadow, 168);
// Tell the world we are alive.
serDputs ("done\n\r");
}
/**
* Enable the Rabbit interrupts.
*/
void sysInterruptEnable()
{
// External interrupt priority 3 on INT0A, INT1A either edge
WrPortI (I0CR, NULL, 0x0f);
WrPortI (I1CR, NULL, 0x0f);
// Timer B clocked by timer A1, interrupt priority 2.
WrPortI (TBCR, NULL, 0x06);
// Set timer B compare registers to get the interrupt started.
WrPortI (TBM1R, NULL, 0);
WrPortI (TBL1R, NULL, 0);
// Clear the interrupt pending flag.
RdPortI (TBCSR);
// Enable Timer B and match register interrupts.
WrPortI (TBCSR, NULL, 0x03);
// Clear the interrupt pending flag.
RdPortI (TACSR);
// Timer A5 clocked by timer A1, all other timers clocked by pclk/2, interrupt priority 1.
WrPortI (TACR, &TACRShadow, 0x21);
// Enable timer A1 and timer A5 interrupts.
WrPortI (TACSR, &TACSRShadow, 0x21);
}
/**
* Calculate NMEA-0183 message checksum of <b>buffer</b> that is <b>length</b> bytes long.
*
* @param buffer Pointer to data buffer.
* @param length number of bytes in buffer.
*
* @return checksum of buffer
*/
uint8 sysNMEAChecksum (uint8 *buffer, uint16 length)
{
uint16 i;
uint8 checksum;
checksum = 0;
for (i = 0; i < length; ++i)
checksum ^= buffer[i];
return checksum;
}
#ifdef WIN32
void sysRuntimeErrHandler()
#else
root void sysRuntimeErrHandler()
#endif
{
static int error, xpc, addr;
#ifndef WIN32
// get all the relevant parameters off the stack
#asm
ld hl, (sp+@SP+2)
ld (error), hl ; get the runtime error code
ld hl, (sp+@SP+6)
ld (xpc), hl ; get the XPC where exception() was called
ld hl, (sp+@SP+8)
ld (addr), hl ; get the address exception() was called
#endasm
#endif
// This is a simple error handler, just reboot.
forceSoftReset();
}
/**
* System timer function that is called every 5mS.
*/
#ifdef WIN32
void sysTimerISR()
#else
interrupt void sysTimerISR()
#endif
{
// Clear the timer interrupt.
RdPortI (TACSR);
// Radio handler periodic timer.
radioUpdate();
// Local I/O periodic timer.
localIOUpdateTick();
// Camera control periodic timer.
camUpdateTick();
}
/**
* Class to handle the TNC functions.
*/
// Port D bits used for TNC control.
#define TNC_M0 0
#define TNC_DATA 1
#define TNC_M1 2
#define TNC_MAX_RX 300
#define TNC_MAX_MESSAGE 255
#define TNC_RX_FLAG 0
#define TNC_RX_DATA 1
#define TNC_RX_PARSE 2
#define TNC_TX_PREPARE 3
#define TNC_TX_SYNC 4
#define TNC_TX_DATA 5
#define TNC_TX_END 6
uint16 tncTimerCompare, tncIndex, tncLength, tncTxPacketCount, tncRxPacketCount, tncProcPacketCount;
uint8 tncBitCount, tncBitTime, tncShift, tncRx, tncLastBit, tncMode, tncTransmit;
uint8 tncBitStuff, tncBuffer[TNC_MAX_RX], tncSSIDOverrideFlag, tncRemoteTick;
struct {
uint8 sourceCallSign[7], sourceSSID;
uint8 destCallSign[7], destSSID;
uint8 message[TNC_MAX_MESSAGE];
} tncPacket;
#ifdef WIN32
void tncExternalISR()
#else
interrupt void tncExternalISR()
#endif
{
// The bit timer is synced on the falling edge of any receive bit. Two clock
// times later (408uS), we read and proces the bit value.
tncBitTime = 0;
}
/**
* Configure the TNC.
*/
void tncInit()
{
// Configure the internal state machine.
tncTimerCompare = 0;
tncShift = 0;
tncIndex = 0;
tncMode = TNC_RX_FLAG;
// Flag used to indicate the TNC is transmitting.
tncTransmit = FALSE;
// Flag to indicate we should select an alternate SID.
tncSSIDOverrideFlag = FALSE;
// Keep track of which tick we process the remote control transmitter.
tncRemoteTick = 0;
// Counter of all the packets we handle.
tncTxPacketCount = 0;
tncRxPacketCount = 0;
tncProcPacketCount = 0;
// Disable the CMX614 output.
BitWrPortI (PDDR, &PDDRShadow, 0, TNC_DATA);
BitWrPortI (PDDR, &PDDRShadow, 0, TNC_M0);
BitWrPortI (PDDR, &PDDRShadow, 1, TNC_M1);
}
/**
* Transfer the AX.25 packet to the <b>tncPacket</b> structure.
*
* @return TRUE if packet was valid; otherwise false
*/
int tncParsePacket()
{
uint16 i, j, crc;
// Make sure the CRC is good.
crc = sysCRC16 (tncBuffer, tncLength - 2);
if ((crc & 0xff) != tncBuffer[tncLength - 2])
return FALSE;
if ((crc >> 8) != tncBuffer[tncLength - 1])
return FALSE;
// Parse the dest call sign.
for (i = 0; i < 6 && tncBuffer[i] != 0x40; ++i)
tncPacket.destCallSign[i] = tncBuffer[i] >> 1;
tncPacket.destCallSign[i] = 0;
tncPacket.destSSID = (tncBuffer[6] >> 1) & 0x0f;
// Parse the source call sign.
for (i = 0; i < 6 && tncBuffer[7 + i] != 0x40; ++i)
tncPacket.sourceCallSign[i] = tncBuffer[7 + i] >> 1;
tncPacket.sourceCallSign[i] = 0;
tncPacket.sourceSSID = (tncBuffer[13] >> 1) & 0x0f;
// Skip over the optional repeater paths.
for (i = 13; i < 76 && i < tncLength && (tncBuffer[i] & 0x01) == 0x00; i += 7);
// Copy the message to our buffer.
for (j = 0, i += 3; i < tncLength && tncBuffer[i] != 0x0d; ++i, ++j)
tncPacket.message[j] = tncBuffer[i];
tncPacket.message[j] = 0;
return TRUE;
}
void tncSetAlternateSSID()
{
tncSSIDOverrideFlag = TRUE;
}
/**
* Send an AX.25 packet via the RF interface. This function enables the transmitter,
* prepares the output buffer, and sets the interrupt mode. The data is bit shifted
* in the timer interrupt service routine.
*
* @param message pointer to NULL terminate message string
*/
void tncSendPacket(uint8 *message)
{
uint16 i, crc;
uint8 *outBuffer;
// If we are currently sending, then ignore this packet.
if (tncMode == TNC_TX_PREPARE || tncMode == TNC_TX_SYNC || tncMode == TNC_TX_DATA || tncMode == TNC_TX_END)
return;
tncMode= TNC_TX_PREPARE;
// Turn on the MODEM TX mode.
BitWrPortI (PDDR, &PDDRShadow, 1, TNC_DATA);
BitWrPortI (PDDR, &PDDRShadow, 1, TNC_M0);
BitWrPortI (PDDR, &PDDRShadow, 0, TNC_M1);
// Set a pointer to our output buffer.
outBuffer = tncBuffer;
// Includes source (7), dest (7), control field (1), protocol ID (1), and end of message (1).
tncLength = 17;
// Set the destination address.
for (i = 0; i < 6; ++i)
*outBuffer++ = config.destCallSign[i] << 1;
// Set destiation to SSID-0
*outBuffer++ = 0x60;
// Set the source address.
for (i = 0; i < 6; ++i)
*outBuffer++ = config.callSign[i] << 1;
// Set the SSID.
if (tncSSIDOverrideFlag)
*outBuffer++ = 0x60 | (config.callSignLandingZoneSSID << 1);
else
*outBuffer++ = 0x60 | (config.callSignSSID << 1);
tncSSIDOverrideFlag = FALSE;
// Add relay path 1.
if (*config.relayCallSign1 != 0) {
for (i = 0; i < 6; ++i)
*outBuffer++ = config.relayCallSign1[i] << 1;
*outBuffer++ = 0x60 | (config.relayCallSignSSID1 << 1);
tncLength += 7;
} // END if
// Add relay path 2.
if (*config.relayCallSign2 != 0) {
for (i = 0; i < 6; ++i)
*outBuffer++ = config.relayCallSign2[i] << 1;
*outBuffer++ = 0x60 | (config.relayCallSignSSID2 << 1);
tncLength += 7;
} // END if
// Set bit-0 of the last SSID.
*(outBuffer - 1) |= 0x01;
// Set the control field (UI) and protocol ID.
*outBuffer++ = 0x03;
*outBuffer++ = 0xf0;
// Save the message.
while (*message != 0) {
*outBuffer++ = *message++;
++tncLength;
}
// Add the end of message character.
*outBuffer++ = 0x0d;
// Calculate and append the CRC.
crc = sysCRC16(tncBuffer, tncLength);
*outBuffer++ = crc & 0xff;
*outBuffer = (crc >> 8) & 0xff;
// Update the length to include the CRC bytes.
tncLength += 2;
// Prepare for the ISR.
tncBitCount = 0;
tncShift = 0x7e;
tncLastBit = 0;
tncIndex = 0;
tncMode = TNC_TX_SYNC;
}
#ifdef WIN32
void tncTimer()
#else
interrupt void tncTimer()
#endif
{
// Clear the interrupt pending flag.
RdPortI (TBCSR);
// Interrupt again in 208uS (4x 1200 baud).
tncTimerCompare = (tncTimerCompare + 7) & 0x3ff;
WrPortI (TBM1R, NULL, (tncTimerCompare >> 2) & 0xc0);
WrPortI (TBL1R, NULL, tncTimerCompare & 0xff);
// Keep track of when to process the remote transmitter.
if (++tncRemoteTick == 4) {
tncRemoteTick = 0;
remoteTimer();
} // END if
// We keep track of the sub bit-time to determine when to sample the bit.
tncBitTime = (tncBitTime + 1) & 0x03;
// If we are the mid-bit bit point, process the data.
if (tncBitTime == 2) {
// Process based on the rx or tx mode state machine.
switch (tncMode) {
case TNC_RX_FLAG:
case TNC_RX_DATA:
// NRZI decode the incoming data bit.
if ((RdPortI(PEDR) & 0x02) == 0x00) {
if (tncLastBit == 0)
tncShift = (tncShift >> 1) | 0x80;
else
tncShift = tncShift >> 1;
tncLastBit = 0;
} else {
if (tncLastBit == 1)
tncShift = (tncShift >> 1) | 0x80;
else
tncShift = tncShift >> 1;
tncLastBit = 1;
}
// Check for AX.25 packet flag.
if (tncShift == 0x7e) {
// If we captured 17 bytes and have a flag, then we might have a valid message.
if (tncIndex > 16) {
tncLength = tncIndex;
tncMode = TNC_RX_PARSE;
return;
} // END if
// Reset to the start of our message buffer.
tncBitCount = 0;
tncIndex = 0;
tncMode = TNC_RX_DATA;
tncBitStuff = 0;
return;
} // END if tncShift
// If we haven't seen the 0x7e start flag, then exit.
if (tncMode == TNC_RX_FLAG)
return;
// Check for bit stuffing.
tncBitStuff = (tncBitStuff >> 1) | (tncShift & 0x80);
if ((tncBitStuff & 0xfc) == 0x7c) {
tncBitStuff = 0;
return;
} else
tncRx = (tncRx >> 1) | (tncShift & 0x80);
// If we have 8 bits, then save it to the buffer.
if (++tncBitCount == 8) {
// If we over run the buffer, then reset everything.
if (tncIndex == TNC_MAX_RX) {
tncIndex = 0;
tncMode = TNC_RX_FLAG;
return;
} // END if
tncBuffer[tncIndex++] = tncRx;
tncBitCount = 0;
} // END if
break;
case TNC_TX_PREPARE:
break;
case TNC_TX_SYNC:
// The variable tncShift contains the lastest data byte.
// NRZI enocde the data stream.
if ((tncShift & 0x01) == 0x00)
if (tncLastBit == 0)
tncLastBit = 1;
else
tncLastBit = 0;
BitWrPortI (PDDR, &PDDRShadow, tncLastBit, TNC_DATA);
// 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 == config.txDelay) {
tncIndex = 0;
tncShift = tncBuffer[0];
tncBitStuff = 0;
tncMode = TNC_TX_DATA;
} // END if
} 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 (tncLastBit == 0)
tncLastBit = 1;
else
tncLastBit = 0;
BitWrPortI (PDDR, &PDDRShadow, tncLastBit, TNC_DATA);
tncBitStuff = 0x00;
return;
} // END if
// The variable tncShift contains the lastest data byte.
// NRZI enocde the data stream.
if ((tncShift & 0x01) == 0x00)
if (tncLastBit == 0)
tncLastBit = 1;
else
tncLastBit = 0;
BitWrPortI (PDDR, &PDDRShadow, tncLastBit, TNC_DATA);
// 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 (tncLastBit == 0)
tncLastBit = 1;
else
tncLastBit = 0;
BitWrPortI (PDDR, &PDDRShadow, tncLastBit, TNC_DATA);
// If all the bits were shifted, get the next one.
if (++tncBitCount == 8) {
tncBitCount = 0;
tncShift = 0x7e;
// Transmit two closing flags.
if (++tncIndex == 2) {
// Keep track of how many packets we have sent.
++tncTxPacketCount;
// Reset to the receive mode.
tncIndex = 0;
tncShift = 0;
tncMode = TNC_RX_FLAG;
// Change the MODEM to receive mode.
BitWrPortI (PDDR, &PDDRShadow, 0, TNC_M0);
BitWrPortI (PDDR, &PDDRShadow, 1, TNC_M1);
BitWrPortI (PDDR, &PDDRShadow, 0, TNC_DATA);
// Clear this flag to indicate we have sent the requested packet.
tncTransmit = FALSE;
// Select the main frequency.
radioSetTxFreq (RADIO_TX_DOWNLINK);
return;
} // END if
} else
tncShift = tncShift >> 1;
break;
} // END switch
} // END if bitTime
}
/**
* Class to handle radio functions.
*/
// Time periods in units of 5mS. 200 = 1 second
// The ID period for the cross band repeater. (5 minutes)
#define RADIO_ID_TIME 60000
// Time the carrier detect must be on or off before a transition is detected. (200 mS)
#define RADIO_SQUELCH_HYST 40
// Maximum transmit time. (2 minutes)
#define RADIO_TX_TIMEOUT 24000
// The amount of time to allow for the entry of a DTMF message. (10 seconds)
#define RADIO_DTMF_PERIOD 2000
// The amount of time required to change the transmit frequency. (50mS)
#define RADIO_FREQ_CHANGE_TIME 10
// The length of time to scan each receive channel. (300mS)
#define RADIO_SCAN_TIME 60
// The length of time after the DTMF pass through mode is activated before it is disabled.
#define RADIO_DTMF_PASS_THROUGH_PERIOD 24000
// Bit numbers used for radio I/O functions.
// NOTE: Port TX1A is the general I/O connector that is used for PTT with Maxcon radios.
#define RADIO_CARRIER_DETECT 6
#define RADIO_CTCSS_DETECT 7
#define RADIO_TX1 0
#define RADIO_TX2 4
#define RADIO_DATA_RX1 4
#define RADIO_DATA_RX2 5
#define RADIO_AUDIO1 2
#define RADIO_AUDIO2 3
// Operating mode.
#define RADIO_CROSSBAND_MODE 0x10
#define RADIO_CROSSLINK_MODE 0x11
// State of receive radio squelch.
#define RADIO_LOW 0x20
#define RADIO_LOW_HIGH 0x21
#define RADIO_HIGH_LOW 0x22
#define RADIO_HIGH 0x23
// Constants used to fill in tone generator structure.
#define RADIO_TONE_END 0x0000
#define RADIO_TONE387 0x8000
#define RADIO_TONE487 0x4000
#define RADIO_TONE1200 0x2000
#define RADIO_TONE2200 0x6000
#define RADIO_TONE0 0xc000
// The maximum number of DTMF digits accepted for a control message. 4-bits/digit, 32-bit word => 8 digits
#define RADIO_DTMF_BUFFERSIZE 8
// The dash and dot timing are set for 13WPM, the character spacing is set for 6.5WPM
#define RADIO_CODE_DOT RADIO_TONE1200 | 13
#define RADIO_CODE_DASH RADIO_TONE1200 | 39
#define RADIO_CODE_INTER_SYMBOL RADIO_TONE0 | 17
#define RADIO_CODE_CHARACTER_SPACE RADIO_TONE0 | 51
// Audio structure where the upper 3-bits is the tone, and lower 13-bits is the duration in units of 5mS.
const uint16 radioToneTimeOut[] = { RADIO_TONE0 | 75, RADIO_TONE387 | 75, RADIO_TONE487 | 75, RADIO_TONE387 | 75, RADIO_TONE487 | 75, RADIO_TONE387 | 75, RADIO_TONE487 | 75, RADIO_TONE_END };
const uint16 radioToneCourtesy[] = { RADIO_TONE0 | 160, RADIO_TONE2200 | 30, RADIO_TONE1200 | 30, RADIO_TONE0 | 75, RADIO_TONE_END };
// KD7LMO ident (-.- -.. --... .-.. -- ---)
const uint16 radioToneID[] = {
RADIO_CODE_DASH, RADIO_CODE_INTER_SYMBOL, RADIO_CODE_DOT, RADIO_CODE_INTER_SYMBOL, RADIO_CODE_DASH, RADIO_CODE_CHARACTER_SPACE,
RADIO_CODE_DASH, RADIO_CODE_INTER_SYMBOL, RADIO_CODE_DOT, RADIO_CODE_INTER_SYMBOL, RADIO_CODE_DOT, RADIO_CODE_CHARACTER_SPACE,
RADIO_CODE_DASH, RADIO_CODE_INTER_SYMBOL, RADIO_CODE_DASH, RADIO_CODE_INTER_SYMBOL, RADIO_CODE_DOT, RADIO_CODE_INTER_SYMBOL, RADIO_CODE_DOT, RADIO_CODE_INTER_SYMBOL, RADIO_CODE_DOT, RADIO_CODE_CHARACTER_SPACE,
RADIO_CODE_DOT, RADIO_CODE_INTER_SYMBOL, RADIO_CODE_DASH, RADIO_CODE_INTER_SYMBOL, RADIO_CODE_DOT, RADIO_CODE_INTER_SYMBOL, RADIO_CODE_DOT, RADIO_CODE_CHARACTER_SPACE,
RADIO_CODE_DASH, RADIO_CODE_INTER_SYMBOL, RADIO_CODE_DASH, RADIO_CODE_CHARACTER_SPACE,
RADIO_CODE_DASH, RADIO_CODE_INTER_SYMBOL, RADIO_CODE_DASH, RADIO_CODE_INTER_SYMBOL, RADIO_CODE_DASH, RADIO_CODE_CHARACTER_SPACE,
RADIO_TONE_END };
// Local variables
uint8 radioMode, radioLastSquelch, radioTransmit, radioTNCRequest, radioTone, radioAudio, radioTransmitTNCFlag;
uint8 radioLastDTMF, radioDTMFIndex, radioDTMFBuffer[RADIO_DTMF_BUFFERSIZE], radioDTMFReady, radioDTMFPassThrough;
uint8 radioSquelch, radioLastGPSPacketTime, radioAltRxFreq, radioFreqChangeTime, radioCarrierDetectChannel;
uint8 radioCrossLinkHighPower, radioScanChannel, radioToneDetect, radioDebugFlag, radioTLMFlag;
uint16 *radioTonePnt, radioLastIDTime, radioTxTimeOut, radioToneCounter;
uint16 radioLowSquelchTime, radioHighSquelchTime, radioDTMFCountDown, radioScanTime, radioListenTimeOut, radioListenTime;
uint16 radioDTMFPassThruTime;
/**
* Decode and process the DTMF digits. The digits are written to the <b>radioDTMFBuffer</b>
* in the <b>radioISR</b> method. When a command complete digit '#' is received,
* the <b>radioDTMFReady</b> flag is set TRUE.
*/
void radioDTMFDecode ()
{
uint8 i;
uint16 freq;
uint32 value;
char buffer[40];
// If the DTMF flag hasn't been set, then we don't have a DTMF command.
if (radioDTMFReady == FALSE)
return;
// Convert the DTMF decoder data into a 32-bit value.
// NOTE 0xa is the number '0', 0xb is the '*', 0xc is the '#'
value = 0;
for (i = 0; i < radioDTMFIndex; ++i) {
value = value << 4;
value = value | (radioDTMFBuffer[i] & 0x0f);
}
// Now decode each of the DTMF commands.
// NOTE: We should use a switch statement for the decoder, but the Rabbit compiler doesn't support
// a 32-bit value in the switch.
if (value == 0x5a) {
camPanMode = CAM_PAN_DISABLE;
tlmCreateMessage(">ATV Pan Disabled");
}
if (value == 0x55) {
camPanMode = CAM_PAN_UP_HOR_DOWN;
camResetTimer();
tlmCreateMessage(">ATV Auto Pan up/hor/down");
}
if (value == 0x51) {
camPanMode = CAM_PAN_UP_HOR;
camResetTimer();
tlmCreateMessage(">ATV Auto Pan up/hor");
}
if (value == 0x54) {
camPanMode = CAM_PAN_HOR_DOWN;
camResetTimer();
tlmCreateMessage(">ATV Auto Pan hor/down");
}
if (value == 0x57) {
camPanMode = CAM_PAN_UP_DOWN;
camResetTimer();
tlmCreateMessage(">ATV Auto Pan up/down");
}
if (value == 0x53) {
serDputs ("fc up\n\r");
camPanMode = CAM_PAN_DISABLE;
tlmCreateMessage(">ATV Pan up");
}
if (value == 0x56) {
serDputs ("fc horz\n\r");
camPanMode = CAM_PAN_DISABLE;
tlmCreateMessage(">ATV Pan hor");
}
if (value == 0x59) {
serDputs ("fc down\n\r");
camPanMode = CAM_PAN_DISABLE;
tlmCreateMessage(">ATV Pan down");
}
if (value == 0x52) {
// serDputs ("fc out 1 0\n\r");
buffer[0] = 0x00;
remoteTransmit (COMMAND_SET_PA, buffer, 1);
tlmCreateMessage(">ATV Power Off");
}
if (value == 0x555) {
buffer[0] = 0x00;
remoteTransmit (COMMAND_FOCUS, buffer, 1);
tlmCreateMessage(">ATV Focus");
}
if ((value & 0xfff000) == 0x552000) {
freq = 2000;
freq += (uint16) (((value >> 8) & 0x0f) * 100);
freq += (uint16) (((value >> 4) & 0x0f) * 10);
freq += (uint16) (value& 0x0f);
buffer[0] = (freq >> 8) & 0xff;
buffer[1] = freq & 0xff;
remoteTransmit (COMMAND_SET_FREQ, buffer, 2);
sprintf (buffer, ">ATV Freq %dMHz set", freq);
tlmCreateMessage (buffer);
} // END if
if (value == 0x58) {
// serDputs ("fc out 1 1\n\r");
buffer[0] = 0xff;
remoteTransmit (COMMAND_SET_PA, buffer, 1);
tlmCreateMessage(">ATV Power On");
}
if (value == 0x411)
tlmCreateReport(TLM_LOCAL, TLM_TYPE_STATUS);
if (value == 0x412)
tlmCreateReport(TLM_LOCAL, TLM_TYPE_GPGGA);
if (value == 0x413)
tlmCreateReport(TLM_LOCAL, TLM_TYPE_GPRMC);
if (value == 0x414)
tlmCreateReport(TLM_LOCAL, TLM_TYPE_STATUS | 0x80);
if (value == 0x471) {
radioMode = RADIO_CROSSBAND_MODE;
tlmCreateMessage(">Crossband Mode");
}
if (value == 0x472) {
radioMode = RADIO_CROSSLINK_MODE;
tlmCreateMessage(">ANSR-EOSS Crosslink Mode");
}
if (value == 0x473) {
radioCrossLinkHighPower = FALSE;
tlmCreateMessage(">Low Power ANSR-EOSS Crosslink Transmit");
}
if (value == 0x474) {
radioCrossLinkHighPower = TRUE;
tlmCreateMessage(">High Power ANSR-EOSS Crosslink Transmit");
}
if (value == 0x475) {
radioToneDetect = FALSE;
tlmCreateMessage(">Disabled CTCSS tone detect");
}
if (value == 0x476) {
radioToneDetect = TRUE;
tlmCreateMessage(">Enabled CTCSS tone detect");
}
if (value >= 0x4771 && value <= 0x4776) {
radioListenTimeOut = (uint16) (1000 * (value & 0x000f));
if (radioListenTime > radioListenTimeOut)
radioListenTime = radioListenTimeOut;
sprintf (buffer, ">Set %d second listen time", 5 * (value & 0x000f));
tlmCreateMessage(buffer);
}
if (value == 0x4778)
if (radioDebugFlag == TRUE) {
radioDebugFlag = FALSE;
tlmCreateMessage (">Disabled Radio Debug");
} else {
radioDebugFlag = TRUE;
tlmCreateMessage (">Enabled Radio Debug");
} // END if-else
if (value == 0x4779)
if (radioTLMFlag == TRUE) {
radioTLMFlag = FALSE;
tlmCreateMessage (">Disabled TLM Display");
} else {
radioTLMFlag = TRUE;
tlmCreateMessage (">Enabled TLM Display");
} // END if-else
if (value == 0x478) {
radioDTMFPassThrough = TRUE;
// Automatically disable the DTMF pass through mode after 2 minutes.
radioDTMFPassThruTime = RADIO_DTMF_PASS_THROUGH_PERIOD;
tlmCreateMessage(">Enabled DTMF pass through");
}
if (value == 0x479) {
radioDTMFPassThrough = FALSE;
tlmCreateMessage(">Disabled DTMF pass through");
}
if (value == 0x0000) {
// Cut down the balloon.
remoteCutDown();
// Send a packet to indicate the cutdown has been activated.
sprintf (buffer, ">ANSR %ld' peak altitude, cut down", navGetMaxAltitude());
tlmCreateMessage(buffer);
}
if (value == 0x31) {
// Execute the self test.
remoteSelfTest();
// Send a packet to indicate the cutdown has been activated.
tlmCreateMessage(">ANSR Remote Self Test");
}
if (value == 0x0)
forceSoftReset();
if (value == 0x0) {
gpsSendMessage ("Cf", 2);
// Send a packet to indicate the cutdown has been activated.
tlmCreateMessage(">GPS Engine Reset");
}
if (value == 0x0) {
// Clear the flight data recorder log.
logClear();
logBlockStart (LOG_TYPE_TIMESTAMP);
logUint8 (gpsPosition.month);
logUint8 (gpsPosition.day);
logUint16 (gpsPosition.year);
logUint8 (gpsPosition.hours);
logUint8 (gpsPosition.minutes);
logUint8 (gpsPosition.seconds);
config.flightTime = 0;
configCalcCRC();
// Send a packet to indicate the flight time has been reset.
tlmCreateReport(TLM_LOCAL, TLM_TYPE_STATUS);
navLaunch(&gpsPosition);
}
// Set the buffer back to the start.
radioDTMFIndex = 0;
// Set the flag to indicate we processed the current DTMF buffer.
radioDTMFReady = FALSE;
}
/**
* Initialize the radio subsystem. The radio subsystem includes transmit control,
* carrier detect, and audio routing to the DTMF decoder, TNC MODEM, and radio input.
*/
void radioInit()
{
// Store the state of the squelch line during the last radio ISR.
radioLastSquelch = RADIO_LOW;
// Track when we need to ID.
radioLastIDTime = 0;
// Timeout clock for DTMF pass through timer.
radioDTMFPassThruTime = 0;
// Flag used to indicate if the transmitter is keyed.
radioTransmit = FALSE;
// Flag used to indicate if a tone is currently transmitting.
radioTone = FALSE;
// Flag used to indicate if the receive audio is routed.
radioAudio = FALSE;
// Flag used to indicate a TNC transmit request has been made.
radioTNCRequest = FALSE;
// Flag used to indicate a TNC packet should be send when the input receiver carrier detect goes low.
radioTransmitTNCFlag = FALSE;
// Record the time the transmitter has been in repeat mode.
radioTxTimeOut = RADIO_TX_TIMEOUT;
// Operation mode - standard cross band operation.
radioMode = RADIO_CROSSBAND_MODE;
// Used to record the GPS time in seconds the last packet was sent.
radioLastGPSPacketTime = 0;
// Record the time used to change transmit frequencies.
radioFreqChangeTime = 0;
// Set the squelch hysterisa timers.
radioLowSquelchTime = RADIO_SQUELCH_HYST;
radioHighSquelchTime = RADIO_SQUELCH_HYST;
// Flag to indicate a DTMF string is ready for processing.
radioDTMFReady = FALSE;
// State machine used to one-shot a new DTMF character.
radioLastDTMF = 0;
// Index value used to store DTMF digits.
radioDTMFIndex = 0;
// Flag to pass DTMF tones through without blocking them.
radioDTMFPassThrough = FALSE;
// The scan time counter for each channel in cross link mode.
radioScanTime = RADIO_SCAN_TIME;
// The channel to scan.
radioScanChannel = 0;
// The channel to use for carrier detect.
radioCarrierDetectChannel = RADIO_CTCSS_DETECT;
// Flag to indicate we are using high power on the cross link.
radioCrossLinkHighPower = FALSE;
// Flag used to indicate if we should use carrier or CTCSS detect on 147.555 MHz freq.
radioToneDetect = FALSE;
// The amount of time to receive in carrier detect mode.
radioListenTimeOut = 1000;
// The carrier detect mode receive timer.
radioListenTime = radioListenTimeOut;
// Flag to indicate if debug information is shown.
radioDebugFlag = FALSE;
radioTLMFlag = FALSE;
// Program the control lines for default operation.
BitWrPortI (PADR, &PADRShadow, 0, RADIO_TX_FREQ_CH1);
BitWrPortI (PADR, &PADRShadow, 0, RADIO_TX_FREQ_CH2);
BitWrPortI (PDDR, &PDDRShadow, 0, RADIO_RX_FREQ_CH1);
BitWrPortI (PADR, &PADRShadow, 0, RADIO_TX1);
BitWrPortI (PADR, &PADRShadow, 0, RADIO_TX2);
BitWrPortI (PEDR, &PEDRShadow, 0, RADIO_AUDIO1);
BitWrPortI (PEDR, &PEDRShadow, 0, RADIO_AUDIO2);
BitWrPortI (PDDR, &PDDRShadow, 0, RADIO_DATA_RX1);
BitWrPortI (PDDR, &PDDRShadow, 1, RADIO_DATA_RX2);
}
/**
* Saves and starts playing the audio structure <b>tone</b>. The structure contains
* the frequency and duration of a tone or silent space. The tone generator is use
* to create a courtesy and status tone or a CW message.
*/
void radioInitTone (uint16 *tone)
{
radioTone = TRUE;
radioToneCounter = 1;
radioTonePnt = (uint16 *) tone;
}
/**
* Set the transmitter frequency.
*
* @param mode where mode is RADIO_TX_APRS, RADIO_TX_DOWNLINK
*/
void radioSetTxFreq(uint8 mode)
{
switch (mode) {
case RADIO_TX_APRS:
// Select the APRS frequency.
BitWrPortI (PADR, &PADRShadow, 1, RADIO_TX_FREQ_CH1);
BitWrPortI (PADR, &PADRShadow, 0, RADIO_TX_FREQ_CH2);
// Set the timer to allow the transmitter to change frequencies.
radioFreqChangeTime = RADIO_FREQ_CHANGE_TIME;
break;
case RADIO_TX_DOWNLINK:
// Select the downlink frequency frequency.
BitWrPortI (PADR, &PADRShadow, 0, RADIO_TX_FREQ_CH1);
BitWrPortI (PADR, &PADRShadow, 0, RADIO_TX_FREQ_CH2);
// Set the timer to allow the transmitter to change frequencies.
radioFreqChangeTime = RADIO_FREQ_CHANGE_TIME;
break;
case RADIO_TX_CROSSBAND_LOW:
// Select the downlink frequency frequency.
BitWrPortI (PADR, &PADRShadow, 0, RADIO_TX_FREQ_CH1);
BitWrPortI (PADR, &PADRShadow, 1, RADIO_TX_FREQ_CH2);
// Set the timer to allow the transmitter to change frequencies.
radioFreqChangeTime = RADIO_FREQ_CHANGE_TIME;
break;
case RADIO_TX_CROSSBAND_HIGH:
// Select the downlink frequency frequency.
BitWrPortI (PADR, &PADRShadow, 1, RADIO_TX_FREQ_CH1);
BitWrPortI (PADR, &PADRShadow, 1, RADIO_TX_FREQ_CH2);
// Set the timer to allow the transmitter to change frequencies.
radioFreqChangeTime = RADIO_FREQ_CHANGE_TIME;
break;
} // END switch
}
/**
* This method is called on a periodic basis (every 5mS) to process the radio operations. The
* operations include carrier detect, transmit timeout, and DTMF decoding.
*/
void radioUpdate()
{
uint8 radioDTMFDigit;
// Set the value of radioSquelch based on the current mode and state of the receive channel.
switch (radioMode) {
case RADIO_CROSSBAND_MODE:
// The CTCSS decoder output is not very stable, so debounce the output.
if (BitRdPortI(PEDR, RADIO_CTCSS_DETECT) == 0) {
radioHighSquelchTime = RADIO_SQUELCH_HYST;
if (radioLastSquelch == 0)
radioSquelch = RADIO_LOW;
else {
if (--radioLowSquelchTime == 0) {
radioLastSquelch = 0;
radioSquelch = RADIO_HIGH_LOW;
} // END if
}
} else {
radioLowSquelchTime = RADIO_SQUELCH_HYST;
if (radioLastSquelch == 1)
radioSquelch = RADIO_HIGH;
else {
if (--radioHighSquelchTime == 0) {
radioLastSquelch = 1;
radioSquelch = RADIO_LOW_HIGH;
} // END if
}
} // END if-else
break;
case RADIO_CROSSLINK_MODE:
// The CTCSS decoder / carrier detect output is not very stable, so debounce it.
if (BitRdPortI(PEDR, radioCarrierDetectChannel) == 0) {
radioHighSquelchTime = RADIO_SQUELCH_HYST;
if (radioLastSquelch == 0)
radioSquelch = RADIO_LOW;
else {
if (--radioLowSquelchTime == 0) {
radioLastSquelch = 0;
radioSquelch = RADIO_HIGH_LOW;
} // END if
}
} else {
radioLowSquelchTime = RADIO_SQUELCH_HYST;
if (radioLastSquelch == 1)
radioSquelch = RADIO_HIGH;
else {
if (--radioHighSquelchTime == 0) {
radioLastSquelch = 1;
radioSquelch = RADIO_LOW_HIGH;
} // END if
}
} // END if-else
// Reset the scan time counter if we detect any signal.
if (BitRdPortI(PEDR, radioCarrierDetectChannel) == 1)
radioScanTime = RADIO_SCAN_TIME;
// Select the other channel periodically to check for an incoming signal.
if (radioSquelch == RADIO_HIGH && radioScanChannel == 1) {
if (--radioListenTime == 0) {
radioListenTime = radioListenTimeOut;
radioScanTime = 1;
radioLastSquelch = 0;
} // END if
} else
radioListenTime = radioListenTimeOut;
// When the scan time ends, switch frequencies. ScanChannel 0: 145.560, 1: 147.555
if (--radioScanTime == 0) {
radioScanTime = RADIO_SCAN_TIME;
if (radioScanChannel == 0) {
radioScanChannel = 1;
if (radioToneDetect == TRUE)
radioCarrierDetectChannel = RADIO_CTCSS_DETECT;
else
radioCarrierDetectChannel = RADIO_CARRIER_DETECT;
// Listen on the 147.555 MHz freq.
BitWrPortI (PDDR, &PDDRShadow, 1, RADIO_RX_FREQ_CH1);
} else {
radioScanChannel = 0;
radioCarrierDetectChannel = RADIO_CTCSS_DETECT;
// Listen on the 145.560 MHz freq.
BitWrPortI (PDDR, &PDDRShadow, 0, RADIO_RX_FREQ_CH1);
}
} // END if-else
break;
} // END switch
// Turn on the transmitter if the receive radio squelch is opened.
if (radioSquelch == RADIO_LOW_HIGH) {
radioTransmit = TRUE;
// Select the desired output device.
if (radioMode == RADIO_CROSSLINK_MODE)
if (radioScanChannel == 1)
radioSetTxFreq (RADIO_TX_DOWNLINK);
else {
if (radioCrossLinkHighPower == FALSE)
radioSetTxFreq (RADIO_TX_CROSSBAND_LOW);
else
radioSetTxFreq (RADIO_TX_CROSSBAND_HIGH);
} // else
// If the ID timer has expired, reset it the first time we use the radio.
if (radioLastIDTime == 0)
radioLastIDTime = RADIO_ID_TIME;
} // END if
// Initiate the courtesy tone or ID if the radio squelch is closed.
if (radioSquelch == RADIO_HIGH_LOW) {
if (radioTransmitTNCFlag == TRUE) {
radioTransmitTNCFlag = FALSE;
radioTNCRequest = TRUE;
tncTransmit = TRUE;
} // END if
if (radioTxTimeOut != 0 && tncTransmit == FALSE) {
if (radioLastIDTime == 0)
radioInitTone ((uint16 *) radioToneID);
else
radioInitTone ((uint16 *) radioToneCourtesy);
} // END if
// Clear the DTMF buffer when the radio breaks.
radioDTMFIndex = 0;
} // END if
// If we get a TNC request flag and we aren't in receive mode, send the packet.
if (radioTransmitTNCFlag == TRUE && radioSquelch == RADIO_LOW && tncTransmit == FALSE && camDownloadMode == FALSE) {
radioTransmitTNCFlag = FALSE;
radioTNCRequest = TRUE;
tncTransmit = TRUE;
}
// If we are in tone mode, update the tone duration/frequency.
if (radioTone == TRUE)
radioUpdateTone();
// Turn off the transmitter when we aren't repeating, if we transmitted too long, or if we aren't sending a tone or packet.
if ((radioSquelch == RADIO_LOW || radioTxTimeOut == 0) && radioTransmit == TRUE && radioTone == FALSE && tncTransmit == FALSE)
radioTransmit = FALSE;
// Route the receive audio if we are transmitting and not sending a packet.
if (radioSquelch == RADIO_HIGH && radioTransmit == TRUE && tncTransmit == FALSE)
radioAudio = TRUE;
else
radioAudio = FALSE;
// If we repeat too long, then transmit a disconnect tone and turn off the transmitter.
if (radioSquelch == RADIO_LOW)
radioTxTimeOut = RADIO_TX_TIMEOUT;
else {
if (radioTxTimeOut != 0)
--radioTxTimeOut;
if (radioTxTimeOut == 0 && radioTone == FALSE && tncTransmit == FALSE && radioTransmit == TRUE)
radioInitTone ((uint16 *) radioToneTimeOut);
} // END if-else
// Transmit a packet at 10, 20, 40, and 50 seconds if we aren't in cross link mode.
if (radioMode == RADIO_CROSSBAND_MODE && radioTransmit == FALSE && tncTransmit == FALSE)
if (gpsPosition.seconds == 10 || gpsPosition.seconds == 20 || gpsPosition.seconds == 40 || gpsPosition.seconds == 50)
if (gpsPosition.seconds != radioLastGPSPacketTime) {
// Record the time of the last packet so we don't get duplicates if it takes less than a second.
radioLastGPSPacketTime = gpsPosition.seconds;
// Select the transmitter frequency.
radioSetTxFreq (RADIO_TX_APRS);
// Set a flag to turn on the transmitter.
radioTransmit = TRUE;
// Set a flag to indicate we are sending a packet.
tncTransmit = TRUE;
// Set a flag to tell the main loop to generate a TNC packet.
radioTNCRequest = TRUE;
} // END if
// Transmit an estimate packet at 55 seconds if we aren't in cross link mode.
if (radioMode == RADIO_CROSSBAND_MODE && radioTransmit == FALSE && tncTransmit == FALSE)
if (burstDetect == TRUE)
if (gpsPosition.seconds == 15 || gpsPosition.seconds == 45)
if (gpsPosition.seconds != radioLastGPSPacketTime) {
// Record the time of the last packet so we don't get duplicates if it takes less than a second.
radioLastGPSPacketTime = gpsPosition.seconds;
// Select the transmitter frequency.
radioSetTxFreq (RADIO_TX_APRS);
// Set a flag to indicate we are sending a packet.
tncTransmit = TRUE;
// Set the upper bit to indicate we want an estimate packet.
tlmSetEstimatePacket();
// Set a flag to tell the main loop to generate a TNC packet.
radioTNCRequest = TRUE;
} // END if
// Count down our ID timer.
if (tncTransmit == FALSE && radioLastIDTime != 0) {
--radioLastIDTime;
if (radioLastIDTime == 0 && radioTransmit == FALSE) {
radioTransmit = TRUE;
radioInitTone ((uint16 *) radioToneID);
}
} // END if
// Process the DTMF decoder
if (BitRdPortI (PBDR, 1) == 0)
radioLastDTMF = 0;
else {
// Handle a new DTMF value.
if (radioLastDTMF == 0) {
radioLastDTMF = 1;
// Decode the new character.
radioDTMFDigit = (RdPortI(PBDR) >> 2) & 0x0f;
// If the '#' key was pressed and we have one other digit process the command.
if (radioDTMFDigit == 0x0c && radioDTMFIndex != 0)
radioDTMFReady = TRUE;
// If this is the first character, start a timer that only allows so much time for a message.
if (radioDTMFIndex == 0)
radioDTMFCountDown = RADIO_DTMF_PERIOD;
// Save the new digit if the buffer isn't full and we don't have a command pending.
if (radioDTMFDigit != 0x0c && radioDTMFReady == FALSE && radioDTMFIndex != RADIO_DTMF_BUFFERSIZE)
radioDTMFBuffer[radioDTMFIndex++] = radioDTMFDigit;
} // END if
} // END else
// As soon as we start getting DTMF tones, turn off the audio feed through.
if (radioDTMFIndex != 0 && radioDTMFPassThrough == FALSE)
radioAudio = FALSE;
// Keep track of the time we need to change the transmit frequency.
if (radioFreqChangeTime != 0)
--radioFreqChangeTime;
// Based on the cross band mode, select the proper hardware channels.
switch (radioMode) {
case RADIO_CROSSBAND_MODE:
if (radioFreqChangeTime == 0)
BitWrPortI (PADR, &PADRShadow, radioTransmit, RADIO_TX1);
else
BitWrPortI (PADR, &PADRShadow, 0, RADIO_TX1);
BitWrPortI (PEDR, &PEDRShadow, radioAudio, RADIO_AUDIO2);
break;
case RADIO_CROSSLINK_MODE:
if (radioFreqChangeTime == 0)
BitWrPortI (PADR, &PADRShadow, radioTransmit, RADIO_TX1);
else
BitWrPortI (PADR, &PADRShadow, 0, RADIO_TX1);
if (radioScanChannel == 0) {
BitWrPortI (PEDR, &PEDRShadow, 0, RADIO_AUDIO1);
BitWrPortI (PEDR, &PEDRShadow, radioAudio, RADIO_AUDIO2);
} else {
if (radioToneDetect == FALSE) {
BitWrPortI (PEDR, &PEDRShadow, radioAudio, RADIO_AUDIO1);
BitWrPortI (PEDR, &PEDRShadow, 0, RADIO_AUDIO2);
} else {
BitWrPortI (PEDR, &PEDRShadow, 0, RADIO_AUDIO1);
BitWrPortI (PEDR, &PEDRShadow, radioAudio, RADIO_AUDIO2);
} // END if-else
} // END if-else
break;
} // END switch
// If the timeout period is reached, then start over.
if (radioDTMFCountDown != 0)
--radioDTMFCountDown;
else
if (radioDTMFReady == FALSE)
radioDTMFIndex = 0;
// Disable DTMF pass through mode after a period of time.
if (radioDTMFPassThruTime != 0)
if (--radioDTMFPassThruTime == 0)
radioDTMFPassThrough = FALSE;
}
/**
* This method is called on a periodic basis to set an audio tone based on
* the audio structure. The structure is saved in the <b>radioInitTone</b>
* method.
*/
void radioUpdateTone()
{
// Decrement the tone counter until we get to 0 and then set a new tone.
if (--radioToneCounter == 0) {
// The upper 3-bits determine the tone and the lower 13-bits are the duration in units of 5mS.
switch (*radioTonePnt & 0xe000) {
case RADIO_TONE_END:
radioTone = FALSE;
BitWrPortI (PDDR, &PDDRShadow, 0, TNC_DATA);
BitWrPortI (PDDR, &PDDRShadow, 0, TNC_M0);
BitWrPortI (PDDR, &PDDRShadow, 1, TNC_M1);
break;
case RADIO_TONE0:
BitWrPortI (PDDR, &PDDRShadow, 0, TNC_DATA);
BitWrPortI (PDDR, &PDDRShadow, 0, TNC_M0);
BitWrPortI (PDDR, &PDDRShadow, 1, TNC_M1);
break;
case RADIO_TONE387:
BitWrPortI (PDDR, &PDDRShadow, 1, TNC_DATA);
BitWrPortI (PDDR, &PDDRShadow, 0, TNC_M0);
BitWrPortI (PDDR, &PDDRShadow, 0, TNC_M1);
break;
case RADIO_TONE487:
BitWrPortI (PDDR, &PDDRShadow, 0, TNC_DATA);
BitWrPortI (PDDR, &PDDRShadow, 0, TNC_M0);
BitWrPortI (PDDR, &PDDRShadow, 0, TNC_M1);
break;
case RADIO_TONE1200:
BitWrPortI (PDDR, &PDDRShadow, 1, TNC_DATA);
BitWrPortI (PDDR, &PDDRShadow, 1, TNC_M0);
BitWrPortI (PDDR, &PDDRShadow, 0, TNC_M1);
break;
case RADIO_TONE2200:
BitWrPortI (PDDR, &PDDRShadow, 0, TNC_DATA);
BitWrPortI (PDDR, &PDDRShadow, 1, TNC_M0);
BitWrPortI (PDDR, &PDDRShadow, 0, TNC_M1);
break;
} // END switch
// Get the duration this tone will play in units of 5mS.
radioToneCounter = (*radioTonePnt++ & 0x1fff);
}
}
/**
* Class to handle digital temperature sensors.
*/
// Define the I2C clock/data lines.
#define TEMP_CLK 7
#define TEMP_DATA 6
#define tempClockHigh() BitWrPortI(PDDDR, &PDDDRShadow, 0, TEMP_CLK)
#define tempClockLow() BitWrPortI(PDDDR, &PDDDRShadow, 1, TEMP_CLK)
#define tempClock() BitRdPortI(PDDR, TEMP_CLK)
#define tempDataHigh() BitWrPortI(PDDDR, &PDDDRShadow, 0, TEMP_DATA)
#define tempDataLow() BitWrPortI(PDDDR, &PDDDRShadow, 1, TEMP_DATA)
#define tempData() BitRdPortI(PDDR, TEMP_DATA)
/**
* Get the digital temperature sensor reading in units of 0.1 degrees F.
*
* @return signed temperature value
*/
int16 tempReadTemp(uint8 channel)
{
uint8 returnCode, retryCount;
int32 temp;
retryCount = 0;
while (retryCount < 5) {
tempMasterStart();
returnCode = tempWriteAndGetAck (channel);
temp = tempReadWithAck (TRUE) << 8;
temp = temp | tempReadWithAck (FALSE);
tempMasterStop();
if (returnCode == 0)
return (int16) ((temp * 9 / 64) + 320);
++retryCount;
}
return 0;
}
/**
* Get the digital temperature sensor reading in units of 0.1 degrees F.
*
* @return signed temperature value
*/
int16 tempGetExtTemp()
{
return tempReadTemp (0x91);
}
/**
* Get the digital temperature sensor reading in units of 0.1 degrees F.
*
* @return signed temperature value
*/
int16 tempGetIntTemp()
{
return tempReadTemp (0x95);
}
/**
* Initialize the digital temperarture sensor.
*/
void tempInit()
{
uint8 i;
tempDataHigh();
tempClockLow();
for (i = 0; i < 4; ++i)
tempMasterStop();
}
/**
* Send the I2C start sequence.
*/
void tempMasterStart()
{
tempClockHigh();
tempDataHigh();
tempDataLow();
tempClockLow();
tempDataHigh();
}
/**
* Send the I2C stop sequence.
*/
void tempMasterStop()
{
tempDataLow();
tempClockHigh();
tempDataHigh();
}
/**
* Send a data byte and read the slave ACK status.
*
* @param data byte value to send
*
* @return slave ACK state 1 or 0
*/
uint8 tempWriteAndGetAck(uint8 data)
{
uint8 i, returnState;
// Bit bang out each bit, MSB first.
for (i = 0; i < 8; ++i) {
if ((data & 0x80) == 0x80)
tempDataHigh();
else
tempDataLow();
tempClockHigh();
tempClockLow();
data = data << 1;
} // END for
tempDataHigh();
tempClockHigh();
returnState = tempData();
tempClockLow();
return returnState;
}
/**
* Read the I2C slave device and set the optional ACK bit.
*
* @param if true, set the ACK after reading the data
*
* @return value read from I2C slave
*/
uint8 tempReadWithAck (uint8 ack)
{
uint8 i, value;
value = 0;
for (i = 0; i < 8; ++i) {
value = value << 1;
tempClockHigh();
value |= (tempData() & 0x01);
tempClockLow();
} // END for
tempDataHigh();
if (ack) {
tempDataLow();
tempClockHigh();
tempClockLow();
tempDataHigh();
} // END if
return value;
}
/**
* Class to handle telemetry.
*/
uint8 tlmReportString[80];
uint8 tlmReportType, tlmPositionReportType, tlmPacketType;
/**
* Create a telemetry message that will be sent when the transmitter is clear.
*
* @param message string to send
*/
void tlmCreateMessage(uint8 *message)
{
// Save a copy of the message.
strncpy (tlmReportString, message, sizeof(tlmReportString));
tlmReportString[sizeof(tlmReportString) - 1] = 0;
// Set the internal state machine to transmit when the input carrier detect is clear.
tlmReportType = TLM_MESSAGE;
tlmPacketType = TLM_TYPE_STATUS;
radioTransmitTNCFlag = TRUE;
}
/**
* Create a telemetry report that will be sent when the transmitter is clear.
*
* @param reportType telemetry report, i.e. TLM_LOCAL, TLM_GPS, etc.
*/
void tlmCreateReport(uint8 reportType, uint8 packetType)
{
// Set the internal state machine to transmit when the input carrier detect is clear.
tlmReportType = reportType;
tlmPacketType = packetType;
radioTransmitTNCFlag = TRUE;
}
/**
* Create a landing estimate packet.
*/
void tlmLandingEstimate()
{
float coord;
uint8 dirFlag, buffer[80], message[80];
uint32 coordMin;
COORD landingZone;
// Calculate the landing zone.
if (!navUpdate(&gpsPosition, &landingZone)) {
radioTransmit = TRUE;
tncSendPacket (">ANSR Unable to compute landing zone");
return;
}
// Generate the GPGGA message.
sprintf (message, "$GPGGA,%02d%02d%02d,", gpsPosition.hours, gpsPosition.minutes, gpsPosition.seconds);
// Set a flag to turn on the transmitter after we have calculated the landing zone.
radioTransmit = TRUE;
// Convert lat/lon to degrees.
navRadToDeg (&landingZone);
if (landingZone.lat < 0) {
coord = -landingZone.lat;
dirFlag = 0;
} else {
dirFlag = 1;
coord = landingZone.lat;
}
coordMin = (uint32) ((coord - floor(coord)) * 600000.0);
sprintf (buffer, "%d%02ld.%04ld,", (uint16) coord, (uint32) (coordMin / 10000), (uint32) (coordMin % 10000));
strcat (message, buffer);
if (dirFlag == 1)
strcat (message, "N,");
else
strcat (message, "S,");
// Longitude value.
if (landingZone.lon < 0) {
coord = -landingZone.lon;
dirFlag = 0;
} else {
dirFlag = 1;
coord = landingZone.lon;
}
coordMin = (uint32) ((coord - floor(coord)) * 600000.0);
sprintf (buffer, "%d%02ld.%04ld,", (uint16) coord, (uint32) (coordMin / 10000), (uint32) (coordMin % 10000));
strcat (message, buffer);
if (dirFlag == 1)
strcat (message, "E,");
else
strcat (message, "W,");
// GPS status where 0: not available, 1: available
if ((gpsPosition.status & 0x8000) == 0x8000)
strcat (message, "1,");
else
strcat (message, "0,");
// Number of visibile birds.
sprintf (buffer, "%02d,", gpsPosition.visibleSats);
strcat (message, buffer);
// DOP
sprintf (buffer, "%d.%01d,", gpsPosition.dop / 10, gpsPosition.dop % 10);
strcat (message, buffer);
// Altitude in meters.
sprintf (buffer, "%ld.0,M,,M,,", (int32) (gpsPosition.altitudeMeters / 100l));
strcat (message, buffer);
// Checksum, we add 1 to skip over the $ character.
sprintf (buffer, "*%02X", sysNMEAChecksum(message + 1, strlen(message) - 1));
strcat (message, buffer);
// Set the SSID of the landing zone.
tncSetAlternateSSID();
tncSendPacket (message);
}
/**
* Create a NMEA $GPGGA packet and send it on the TNC.
*/
void tlmGPGGAPacket()
{
uint32 coord, coordMin;
uint8 dirFlag, buffer[80], message[80];
// Generate the GPGGA message.
sprintf (message, "$GPGGA,");
// UTC is replaced with flight time
sprintf (buffer, "%02d%02d%02d,", gpsPosition.hours, gpsPosition.minutes, gpsPosition.seconds);
strcat (message, buffer);
// Latitude value.
coord = gpsPosition.latitude;
if (gpsPosition.latitude < 0) {
coord = gpsPosition.latitude * -1;
dirFlag = 0;
} else {
dirFlag = 1;
coord = gpsPosition.latitude;
}
coordMin = (coord % 3600000) / 6;
sprintf (buffer, "%02ld%02ld.%04ld,", (uint32) (coord / 3600000), (uint32) (coordMin / 10000), (uint32) (coordMin % 10000));
strcat (message, buffer);
if (dirFlag == 1)
strcat (message, "N,");
else
strcat (message, "S,");
// Longitude value.
if (gpsPosition.longitude < 0) {
coord = gpsPosition.longitude * - 1;
dirFlag = 0;
} else {
dirFlag = 1;
coord = gpsPosition.longitude;
}
coordMin = (coord % 3600000) / 6;
sprintf (buffer, "%03ld%02ld.%04ld,", (uint32) (coord / 3600000), (uint32) (coordMin / 10000), (uint32) (coordMin % 10000));
strcat (message, buffer);
if (dirFlag == 1)
strcat (message, "E,");
else
strcat (message, "W,");
// GPS status where 0: not available, 1: available
if ((gpsPosition.status & 0x8000) == 0x8000)
strcat (message, "1,");
else
strcat (message, "0,");
// Number of visibile birds.
sprintf (buffer, "%02d,", gpsPosition.visibleSats);
strcat (message, buffer);
// DOP
sprintf (buffer, "%d.%01d,", gpsPosition.dop / 10, gpsPosition.dop % 10);
strcat (message, buffer);
// Altitude in meters.
sprintf (buffer, "%ld.0,M,,M,,", (int32) (gpsPosition.altitudeMeters / 100l));
strcat (message, buffer);
// Checksum, we add 1 to skip over the $ character.
sprintf (buffer, "*%02X", sysNMEAChecksum(message + 1, strlen(message) - 1));
strcat (message, buffer);
tncSendPacket (message);
}
/**
* Create a NMEA $GPRMC packet and send it on the TNC.
*/
void tlmGPRMCPacket()
{
uint32 coord, coordMin, temp;
uint8 dirFlag, buffer[80], message[80];
// Generate the GPRMC message.
sprintf (message, "$GPRMC,");
// UTC is replaced with flight time
sprintf (buffer, "%02d%02d%02d,", gpsPosition.hours, gpsPosition.minutes, gpsPosition.seconds);
strcat (message, buffer);
// GPS status.
if ((gpsPosition.status & 0x8000) == 0x8000)
strcat (message, "A,");
else
strcat (message, "V,");
// Latitude value.
coord = gpsPosition.latitude;
if (gpsPosition.latitude < 0) {
coord = gpsPosition.latitude * -1;
dirFlag = 0;
} else {
dirFlag = 1;
coord = gpsPosition.latitude;
}
coordMin = (coord % 3600000) / 6;
sprintf (buffer, "%02ld%02ld.%04ld,", (uint32) (coord / 3600000), (uint32) (coordMin / 10000), (uint32) (coordMin % 10000));
strcat (message, buffer);
if (dirFlag == 1)
strcat (message, "N,");
else
strcat (message, "S,");
// Longitude value.
if (gpsPosition.longitude < 0) {
coord = gpsPosition.longitude * - 1;
dirFlag = 0;
} else {
dirFlag = 1;
coord = gpsPosition.longitude;
}
coordMin = (coord % 3600000) / 6;
sprintf (buffer, "%03ld%02ld.%04ld,", (uint32) (coord / 3600000), (uint32) (coordMin / 10000), (uint32) (coordMin % 10000));
strcat (message, buffer);
if (dirFlag == 1)
strcat (message, "E,");
else
strcat (message, "W,");
// Speed knots and heading.
temp = (int32) gpsPosition.hSpeed * 75000 / 385826;
sprintf (buffer, "%d.%d,%d.%d,", (int16) (temp / 10), (int16) (temp % 10), gpsPosition.heading / 10, gpsPosition.heading % 10);
strcat (message, buffer);
// Date
sprintf (buffer, "%02d%02d%02d,,", gpsPosition.day, gpsPosition.month, gpsPosition.year % 100);
strcat (message, buffer);
// Checksum, skip over the $ character.
sprintf (buffer, "*%02X", sysNMEAChecksum(message + 1, strlen(message) - 1));
strcat (message, buffer);
tncSendPacket (message);
}
/**
* Initialize the telemetry subsystem.
*/
void tlmInit()
{
// Select the default TNC and NMEA reports.
tlmReportType = TLM_LOCAL;
tlmPositionReportType = TLM_NMEA;
tlmPacketType = TLM_TYPE_STATUS;
strcpy (tlmReportString, "");
}
/**
* Standard telemetry status packet.
*/
void tlmLocal()
{
int16 temp;
uint8 buffer[100], message[100];
// Status message header.
sprintf (message, ">ANSR ");
// Current flight time.
sprintf (buffer, "%02d:%02d:%02d ", config.flightTime / 3600, (config.flightTime / 60) % 60, config.flightTime % 60);
strcat (message, buffer);
// Show GPS information if it is alive, otherwise show blank fields.
if ((gpsPosition.status & 0xe000) == 0xe000) {
sprintf (buffer, "%ld' %ld'/min ", gpsPosition.altitudeFeet, (int32) (gpsPosition.vSpeedAccum * 6000l / 12192l));
strcat (message, buffer);
} else
strcat (message, "---' ---'/min ");
// GPS status
if ((gpsPosition.status & 0xe000) == 0xe000){
sprintf (buffer, "%d.%dpdop ", gpsPosition.dop / 10, gpsPosition.dop % 10);
strcat (message, buffer);
} else
if ((gpsPosition.status & 0xe000) == 0xc000) {
sprintf (buffer, "%d.%dhdop ", gpsPosition.dop / 10, gpsPosition.dop % 10);
strcat (message, buffer);
} else
strcat (message, "nogps ");
// Payload computer bus voltage
temp = analogGetBusVoltage();
sprintf (buffer, "%d.%dV ", temp / 10, temp % 10);
strcat (message, buffer);
// Internal temp for digital sensor
temp = tempGetIntTemp();
sprintf (buffer, "%d.%dF ", temp / 10, abs(temp % 10));
strcat (message, buffer);
// Internal temp for analog sensor
temp = analogGetIntTemp();
sprintf (buffer, "%d.%dF ", temp / 10, abs(temp % 10));
strcat (message, buffer);
// External temp
temp = tempGetExtTemp();
sprintf (buffer, "%d.%dF ", temp / 10, abs(temp % 10));
strcat (message, buffer);
// Static preasure
// temp = analogGetStatic();
// sprintf (buffer, "%d.%02dpsi ", temp / 100, temp % 100);
// strcat (message, buffer);
// Camera computer bus voltage
// sprintf (buffer, "%.1fV ", (float) camAnalog1 / 47.627907);
// strcat (message, buffer);
// Internal camera temp
// sprintf (buffer, "%.0fF ", (double) camAnalog2 / 5.613384);
// strcat (message, buffer);
// sprintf (buffer, "%d", navGetWindDataCount());
// strcat (message, buffer);
// Report the information.
tncSendPacket (message);
}
/**
* Display low-level telemetry information from the GPS receiver.
*/
void tlmGPSStatus()
{
uint8 message[100], buffer[100];
// Date stamp.
sprintf (message, "%02d/%02d/%04d ", gpsPosition.month, gpsPosition.day, gpsPosition.year);
// Time stamp.
sprintf (buffer, "%02d:%02d:%02d ", gpsPosition.hours, gpsPosition.minutes, gpsPosition.seconds);
strcat (message, buffer);
// Show our position in the world.
sprintf (buffer, "%ld %ld %ld ", gpsPosition.latitude, gpsPosition.longitude, gpsPosition.altitudeFeet);
strcat (message, buffer);
// Show our speed.
sprintf (buffer, "%d %d ", gpsPosition.hSpeed, gpsPosition.vSpeedAccum);
strcat (message, buffer);
// GPS receiver status.
sprintf (buffer, "%d %d %03d %x", gpsPosition.visibleSats, gpsPosition.trackedSats, gpsPosition.dop, gpsPosition.status);
strcat (message, buffer);
tncSendPacket (message);
}
/**
* Send the user readable hex data stream of a TNC message to the NMEA serial port.
*/
void tlmPacketReport()
{
uint8 i, buffer[10];
serDputs (tncPacket.sourceCallSign);
serDputs (" > ");
serDputs (tncPacket.destCallSign);
serDputs (" msg: ");
serDputs (tncPacket.message);
serDputs ("\n\rbin: ");
for (i = 0; i < tncLength; ++i) {
sprintf (buffer, "%02x ", tncBuffer[i]);
serDputs (buffer);
if ((i % 24) == 23)
serDputs ("\n\r");
}
serDputs ("\n\r");
}
void tlmSetEstimatePacket()
{
tlmPacketType |= 0x80;
}
/**
* NMEA GPS position report.
*/
void tlmPositionReport()
{
uint32 coord, coordMin, temp;
uint8 dirFlag, buffer[80], message[80], latLong[40];
// Generate latitude information that is used in both NMEA messages.
// Latitude value.
coord = gpsPosition.latitude;
if (gpsPosition.latitude < 0) {
coord = gpsPosition.latitude * -1;
dirFlag = 0;
} else {
dirFlag = 1;
coord = gpsPosition.latitude;
}
coordMin = (coord % 3600000) / 6;
sprintf (latLong, "%02ld%02ld.%04ld,", (uint32) (coord / 3600000), (uint32) (coordMin / 10000), (uint32) (coordMin % 10000));
if (dirFlag == 1)
strcat (latLong, "N,");
else
strcat (latLong, "S,");
// Longitude value.
if (gpsPosition.longitude < 0) {
coord = gpsPosition.longitude * - 1;
dirFlag = 0;
} else {
dirFlag = 1;
coord = gpsPosition.longitude;
}
coordMin = (coord % 3600000) / 6;
sprintf (buffer, "%03ld%02ld.%04ld,", (uint32) (coord / 3600000), (uint32) (coordMin / 10000), (uint32) (coordMin % 10000));
strcat (latLong, buffer);
if (dirFlag == 1)
strcat (latLong, "E,");
else
strcat (latLong, "W,");
if ((config.flightTime & 0x01) == 0x00) {
// Generate the GPGGA message.
sprintf (message, "GPGGA,");
// UTC is replaced with flight time
sprintf (buffer, "%02d%02d%02d,", config.flightTime / 3600, (config.flightTime / 60) % 60, config.flightTime % 60);
strcat (message, buffer);
// Add in the lat, and long information.
strcat (message, latLong);
// Always indicate the status is valid for the ATV overlay.
strcat (message, "1,");
// Number of visibile birds.
sprintf (buffer, "%02d,", gpsPosition.visibleSats);
strcat (message, buffer);
// DOP
sprintf (buffer, "%d.%01d,", gpsPosition.dop / 10, gpsPosition.dop % 10);
strcat (message, buffer);
sprintf (buffer, "%ld.0,M,,M,,", gpsPosition.altitudeFeet);
strcat (message, buffer);
sprintf (buffer, "*%02X", sysNMEAChecksum(message, strlen(message)));
strcat (message, buffer);
serDputc ('$');
serDputs (message);
serDputs ("\n\r");
}
if ((config.flightTime & 0x01) == 0x01) {
// Generate the GPRMC message.
sprintf (message, "GPRMC,");
// UTC is replaced with flight time
sprintf (buffer, "%02d%02d%02d,", config.flightTime / 3600, (config.flightTime / 60) % 60, config.flightTime % 60);
strcat (message, buffer);
// Always indicate the status is valid for the ATV overlay.
strcat (message, "A,");
// Add in the lat/long information.
strcat (message, latLong);
// Speed (MPH) and heading.
temp = (int32) gpsPosition.hSpeed * 15000 / 67056;
sprintf (buffer, "%d.%d,%d.%d,", (int16) (temp / 10), (int16) (temp % 10), gpsPosition.heading / 10, gpsPosition.heading % 10);
strcat (message, buffer);
// Date
sprintf (buffer, "%02d%02d%02d,,", gpsPosition.day, gpsPosition.month, gpsPosition.year % 100);
strcat (message, buffer);
sprintf (buffer, "*%02X", sysNMEAChecksum(message, strlen(message)));
strcat (message, buffer);
serDputc ('$');
serDputs (message);
serDputs ("\n\r");
}
}
/**
* Create the telemetry status packet and transmit it via the RF link.
*/
void tlmReport()
{
// Select the telemetry stream based on the desired report type.
switch (tlmReportType) {
// Standard report.
case TLM_LOCAL:
tlmLocal();
break;
// Data stream from camera controller.
case TLM_CAM_COMPUTER:
tncSendPacket (camOutBuffer);
break;
// Telemetry diagnostic information.
case TLM_GPS:
tlmGPSStatus();
break;
case TLM_STATUS:
tlmStatus();
break;
case TLM_VER:
tncSendPacket (sysGetVersion());
tlmReportType = TLM_LOCAL;
break;
case TLM_MESSAGE:
tncSendPacket (tlmReportString);
tlmReportType = TLM_LOCAL;
break;
} // END switch
}
/**
* Send the telemetry status report on the TNC.
*/
void tlmStatus()
{
uint8 buffer[80], message[80];
// Boot counter.
sprintf (message, "Boot: %d ", config.bootCount);
// Packet counters.
sprintf (buffer, "Tx: 0x%04x Rx: 0x%04x Rx-PC:0x%04x", tncTxPacketCount, tncRxPacketCount, tncProcPacketCount);
strcat (message, buffer);
tncSendPacket (message);
}
/**
* Update the telemetry measurands. This routine is called once a second.
*/
void tlmUpdate()
{
// Update the flight time
++config.flightTime;
configCalcCRC();
}
/**
* Class to process TNC commands.
*/
// The maximum number of tokens or entries in a TNC command.
#define CMD_MAX_TOKENS 5
// An array of indexes that points to each token in a string.
uint8 cmdBuffer[TNC_MAX_MESSAGE];
/**
* This routine takes the contents of the cmdBuffer[] and parses the commands.
* The cmdBuffer[] is filled by the serial ISR. This routine was modeled after
* Java's StringTokenizer class. Tokens are delimted by a space, tab, or comma.
*
* @return true if command was valid and processed; otherwise false
*/
uint8 cmdProcess()
{
uint8 token[CMD_MAX_TOKENS];
uint8 i, tokenCount;
// Get everything set.
i = 0;
tokenCount = 0;
// We are going to fill the token[] with the index of each token in the command buffer,
// NULL terminate the token(s), and return the number of tokens in tokenCount.
while (cmdBuffer[i] != 0) {
// Skip over the white space to the start of the token.
while (cmdBuffer[i] == ' ' || cmdBuffer[i] == ',' || cmdBuffer[i] == 9)
++i;
// If we aren't at the end of the command, set this token.
if (cmdBuffer[i] != 0)
if (tokenCount != CMD_MAX_TOKENS)
token[tokenCount++] = i;
else
return FALSE;
// Find the end of the token.
while (cmdBuffer[i] != ' ' && cmdBuffer[i] != ',' && cmdBuffer[i] != 9 && cmdBuffer[i] != 0) {
cmdBuffer[i] = tolower(cmdBuffer[i]);
++i;
} // END while
// If we aren't at the end of the line, terminate the token.
if (cmdBuffer[i] != 0)
cmdBuffer[i++] = 0;
}
// We need at least two tokens. (Header & Mnemonic)
if (tokenCount < 2)
return FALSE;
// Check for a valid header.
if (strcmp(cmdBuffer + token[0], "fc") != 0)
return FALSE;
// Process the commands that only have a mnemonic.
// Display firmware version. (fc ver)
if (strcmp(cmdBuffer + token[1], "ver") == 0) {
tlmCreateReport (TLM_VER, TLM_TYPE_STATUS);
return TRUE;
}
// Reset the processor. (fc reset)
if (strcmp(cmdBuffer + token[1], "reset") == 0)
forceSoftReset();
// Proces the commands that have one operand.
// We need at least three tokens. (Header, Mnemonic, and Operand)
if (tokenCount < 3)
return FALSE;
// Control the flight timer (fc time reset)
if (strcmp (cmdBuffer + token[1], "time") == 0)
if (strcmp (cmdBuffer + token[2], "reset") == 0) {
logIndex = 0;
logFlag = TRUE;
logBlockStart (LOG_TYPE_TIMESTAMP);
logUint8 (gpsPosition.month);
logUint8 (gpsPosition.day);
logUint16 (gpsPosition.year);
logUint8 (gpsPosition.hours);
logUint8 (gpsPosition.minutes);
logUint8 (gpsPosition.seconds);
config.flightTime = 0;
configCalcCRC();
tlmCreateReport (TLM_LOCAL, TLM_TYPE_STATUS);
return TRUE;
}
// Select the automatic camera control mode.
if (strcmp (cmdBuffer + token[1], "cam") == 0)
switch (atoi(cmdBuffer + token[2])) {
case 0:
camPanMode = CAM_PAN_DISABLE;
return TRUE;
case 1:
camPanMode = CAM_PAN_UP_HOR_DOWN;
camResetTimer();
return TRUE;
case 2:
camPanMode = CAM_PAN_UP_HOR;
camResetTimer();
return TRUE;
case 3:
camPanMode = CAM_PAN_HOR_DOWN;
camResetTimer();
return TRUE;
case 4:
camPanMode = CAM_PAN_UP_DOWN;
camResetTimer();
return TRUE;
} // END switch
// Select the telemetry mode based on the mode select value 0 through 9.
if (strcmp (cmdBuffer + token[1], "tlm") == 0) {
if (cmdBuffer[token[2]] == '0') {
tlmCreateReport (TLM_LOCAL, TLM_TYPE_STATUS);
return TRUE;
}
if (cmdBuffer[token[2]] == '1') {
tlmCreateReport (TLM_CAM_COMPUTER, TLM_TYPE_STATUS);
return TRUE;
}
if (cmdBuffer[token[2]] == '2') {
tlmCreateReport (TLM_GPS, TLM_TYPE_STATUS);
return TRUE;
}
if (cmdBuffer[token[2]] == '3') {
tlmCreateReport (TLM_STATUS, TLM_TYPE_STATUS);
return TRUE;
}
if (cmdBuffer[token[2]] == '8') {
tlmCreateReport (TLM_NMEA, TLM_TYPE_STATUS);
return TRUE;
}
if (cmdBuffer[token[2]] == '9') {
tlmCreateReport (TLM_PACKET, TLM_TYPE_STATUS);
return TRUE;
}
} // END if
return FALSE;
}
main()
{
uint8 configStatus, gpsStatus;
uint16 count;
uint32 timerTick;
char buffer[80];
COORD landingZone;
// Configure the hardware system including I/O, serial ports, and timers.
sysInit();
// Initialize the subsystems.
configStatus = configInit();
tncInit();
gpsStatus = gpsInit();
radioInit();
tlmInit();
camInit();
localIOInit();
analogInit();
tempInit();
remoteInit();
// Turn on the interrupts.
sysInterruptEnable();
// Set the run time error handler.
defineErrorHandler (sysRuntimeErrHandler);
// Send a message to indicate we have started.
sysBootMessage(configStatus, gpsStatus);
// Record the time we start our main loop.
timerTick = SEC_TIMER;
count = 0;
// This is the main program loop that runs until power down.
while(1) {
// If the system timer has changed by one second, update the periodic telemetry.
if (timerTick != SEC_TIMER) {
++timerTick;
// Update the telemetry measurands.
tlmUpdate();
// Send the once per second position report to the student payload computer.
if (radioTLMFlag == TRUE) {
sprintf (buffer, "%u %d\n\r", (uint16) ((analogData[ANALOG_STATIC] & 0xfffff) >> 4), analogGetStatic());
serDputs (buffer);
} else
if (tlmPositionReportType == TLM_NMEA)
tlmPositionReport();
// Send any required remote packets on odd seconds.
if ((timerTick & 0x01) == 0x01)
remoteSend();
} // END if timerTick
// Check for a GPS message in the serial buffer.
gpsReadMessage();
// If the GPS message has been updated, save it to the log and update the navigation system.
if (gpsIsUpdate()) {
gpsLogData();
atvOverlayUpdate();
if (!burstDetect)
navUpdate(&gpsPosition, &landingZone);
gpsClearUpdate();
}
// Check for a camera control message in the serial buffer.
camReadMessage();
// Update the analog channels
analogUpdate();
// Handle the automatic camera panning functions
camAutoPan();
// Process DTMF commands.
radioDTMFDecode();
// If a TNC packet request has been made by the radio system, generate the packet now.
// We process this request outside the timer interrupt routine because it may take longer than 5mS.
if (radioTNCRequest == TRUE) {
radioTNCRequest = FALSE;
// Rotate through each packet type.
switch (tlmPacketType) {
case TLM_TYPE_STATUS | 0x80:
case TLM_TYPE_GPGGA | 0x80:
case TLM_TYPE_GPRMC | 0x80:
tlmPacketType &= ~0x80;
tlmLandingEstimate();
break;
case TLM_TYPE_STATUS:
tlmReport();
// Only change reports types if we are in the local telemetry mode.
if (tlmReportType == TLM_LOCAL)
tlmPacketType = TLM_TYPE_GPGGA;
break;
case TLM_TYPE_GPGGA:
tlmGPGGAPacket();
tlmPacketType = TLM_TYPE_GPRMC;
break;
case TLM_TYPE_GPRMC:
tlmGPRMCPacket();
tlmPacketType = TLM_TYPE_STATUS;
break;
} // END switch
} // END if
// Debug messages.
//sprintf (buffer, "%u %u %u %u %u %u %u\n\r", radioLowSquelchTime, radioHighSquelchTime, radioSquelch, tncTransmit, radioLastIDTime, radioTxTimeOut, radioToneCounter);
//serDputs (buffer);
// sprintf (buffer, "%d %d %d %d %d\n\r", gpsMode, gpsIndex, gpsBuffer[0], gpsBuffer[1], gpsBuffer[2]);
// serDputs(buffer);
if (radioDebugFlag == TRUE) {
sprintf (buffer, "%x %u %u %u %u %u %d %d\n\r", radioMode, radioListenTime, radioListenTimeOut, radioScanTime, radioScanChannel, radioSquelch, radioToneDetect, radioAudio);
serDputs (buffer);
} // END if
//sprintf (buffer, "%u %u %u %u %u %u\n\r", radioDTMFIndex, radioDTMFCountDown, radioDTMFBuffer[0], radioDTMFBuffer[1], radioDTMFBuffer[2], radioDTMFBuffer[3]);
//serDputs (buffer);
// Check for a packet in the TNC buffer.
if (tncMode == TNC_RX_PARSE) {
if (tncParsePacket() == TRUE) {
// Keep track of the total valid packets received.
++tncRxPacketCount;
if (tlmPositionReportType == TLM_PACKET)
tlmPacketReport();
// Only process messages that were sent to this station.
if (strcmp(tncPacket.destCallSign, config.callSign) == 0) {
// Keep track of the packets that were addressed to us.
++tncProcPacketCount;
// Copy the message for processing.
strcpy (cmdBuffer, tncPacket.message);
// If the command was not processed locally pass it to the student payload.
if (cmdProcess() == FALSE) {
serDputs (tncPacket.message);
serDputs ("\n\r");
}
}
} // END if
tncMode = TNC_RX_FLAG;
tncIndex = 0;
} // END if
}; // END while()
}