kopia lustrzana https://github.com/pjalocha/esp32-ogn-tracker
116 wiersze
4.5 KiB
C++
116 wiersze
4.5 KiB
C++
#include <stdint.h>
|
|
#include <stdlib.h>
|
|
|
|
#include "hal.h"
|
|
#include "aero.h"
|
|
|
|
class AEROmsg
|
|
{ public:
|
|
|
|
static const uint8_t MaxLen=96; // maximum length
|
|
static const uint8_t MaxParms=16; // maximum number of parameters (commas)
|
|
|
|
uint8_t Data[MaxLen]; // the message itself
|
|
uint8_t Len; // number of bytes
|
|
uint8_t Parms; // number of commas
|
|
uint8_t Parm[MaxParms]; // offset to each comma
|
|
uint8_t State; // bits: 0:loading, 1:complete, 2:locked,
|
|
|
|
public:
|
|
void Clear(void) // Clear the frame: discard all data, ready for next message
|
|
{ State=0; Len=0; Parms=0; }
|
|
|
|
void ProcessByte(uint8_t Byte) // pass all bytes through this call and it will build the frame
|
|
{
|
|
if(isComplete()) return; // if already a complete frame, ignore extra bytes
|
|
if(Len==0) // if data is empty
|
|
{ if(Byte!='#') return; // then ignore all bytes but '#'
|
|
Data[Len++]=Byte; // start storing the frame
|
|
setLoading(); Parms=0; // set state to "isLoading", clear checksum
|
|
} else // if not empty (being loaded)
|
|
{ if((Byte=='\r')||(Byte=='\n')) // if CR (or NL ?) then frame is complete
|
|
{ setComplete(); if(Len<MaxLen) Data[Len]=0;
|
|
return; }
|
|
else if(Byte<=' ') // other control bytes treat as errors
|
|
{ Clear(); return; } // and drop the frame
|
|
else if(Byte==',') // save comma positions to later get back to the fields
|
|
{ if(Parms<MaxParms) Parm[Parms++]=Len+1; }
|
|
if(Len<MaxLen) { Data[Len++]=Byte; } // store data but if too much then treat as an error
|
|
else Clear(); // if too long, then drop the frame completely
|
|
}
|
|
return; }
|
|
|
|
uint8_t isLoading(void) const { return State &0x01; }
|
|
void setLoading(void) { State|=0x01; }
|
|
|
|
uint8_t isComplete(void) const { return State &0x02; }
|
|
void setComplete(void) { State|=0x02; }
|
|
|
|
uint8_t isLocked(void) const { return State&0x04; }
|
|
|
|
uint8_t isEmpty(void) const { return Len==0; }
|
|
|
|
uint8_t isChecked(void) const // is the AERO checksum OK ?
|
|
{ if(Data[Len-5]!=',') return 0;
|
|
uint16_t CRC; if(Read_Hex(CRC, (const char *)(Data+(Len-4)))!=4) return 0;
|
|
return CRC == CRC16(Data, Len-5); }
|
|
|
|
static uint16_t CRC16(uint16_t CRC, uint8_t Byte)
|
|
{ uint8_t X = (CRC>>8) ^ Byte; X ^= X>>4;
|
|
CRC = (CRC<<8) ^ ((uint16_t)(X<<12)) ^ ((uint16_t)(X <<5)) ^ ((uint16_t)X);
|
|
return CRC; }
|
|
|
|
static uint16_t CRC16(const uint8_t *Data, uint8_t Len)
|
|
{ uint16_t CRC = 0xFFFF;
|
|
while(Len--)
|
|
{ CRC = CRC16(CRC, *Data++); }
|
|
return (CRC>>8) | (CRC<<8); }
|
|
|
|
} ;
|
|
|
|
static AEROmsg AERO;
|
|
|
|
static void ADSB_AERO(bool GoodCRC)
|
|
{
|
|
// #ifdef DEBUG_PRINT
|
|
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
|
// Format_UnsDec(CONS_UART_Write, TimeSync_Time()%60);
|
|
// CONS_UART_Write('.');
|
|
// Format_UnsDec(CONS_UART_Write, TimeSync_msTime(),3);
|
|
// Format_String(CONS_UART_Write, " -> ");
|
|
Format_String(CONS_UART_Write, (const char *)AERO.Data, AERO.Len, 0);
|
|
if(!GoodCRC) Format_String(CONS_UART_Write, " <- bad !");
|
|
Format_String(CONS_UART_Write, "\n");
|
|
xSemaphoreGive(CONS_Mutex);
|
|
// #endif
|
|
|
|
}
|
|
|
|
#ifdef __cplusplus
|
|
extern "C"
|
|
#endif
|
|
void vTaskAERO(void* pvParameters)
|
|
{
|
|
|
|
vTaskDelay(5); // put some initial delay for lighter startup load
|
|
|
|
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
|
Format_String(CONS_UART_Write, "TaskAERO:");
|
|
Format_String(CONS_UART_Write, "\n");
|
|
xSemaphoreGive(CONS_Mutex);
|
|
|
|
for( ; ; ) // main task loop: every milisecond (RTOS time tick)
|
|
{ vTaskDelay(1); //
|
|
for( ; ; ) // loop over bytes in the AERO UART buffer
|
|
{ uint8_t Byte; int Err=AERO_UART_Read(Byte); if(Err<=0) break; // get Byte from serial port, if no bytes then break this l$
|
|
AERO.ProcessByte(Byte); // process through the AERO interpreter
|
|
if(AERO.isComplete()) // AERO completely received ?
|
|
{ ADSB_AERO(AERO.isChecked());
|
|
AERO.Clear(); break; }
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|