2018-01-29 12:43:22 +00:00
# include <stdint.h>
2019-01-22 00:08:19 +00:00
# include "hal.h" // Hardware Abstraction Layer
2018-01-29 12:43:22 +00:00
2019-01-22 00:08:19 +00:00
# include "proc.h" // PROC task: decode/correct received packets
# include "ctrl.h" // CTRL task:
# include "log.h" // LOG task: packet logging
2018-01-29 12:43:22 +00:00
2019-01-22 00:08:19 +00:00
# include "ogn.h" // OGN packet structures, encoding/decoding/etc.
2018-01-29 12:43:22 +00:00
2019-01-22 00:08:19 +00:00
# include "rf.h" // RF task: transmission and reception of radio packets
# include "gps.h" // GPS task: get own time and position, set the GPS baudrate and navigation mode
2020-02-24 21:54:21 +00:00
# include "fifo.h"
2019-01-22 00:08:19 +00:00
# ifdef WITH_FLASHLOG // log own track to unused Flash pages (STM32 only)
2018-02-18 14:41:50 +00:00
# include "flashlog.h"
# endif
2020-05-06 16:31:43 +00:00
# ifdef WITH_SDLOG
# include "sdlog.h"
# endif
2018-02-18 14:41:50 +00:00
2020-02-24 21:54:21 +00:00
# ifdef WITH_SOUND
# include "sound.h"
# endif
2020-05-01 14:01:36 +00:00
# ifdef WITH_GDL90
# include "gdl90.h"
GDL90_HEARTBEAT GDL_HEARTBEAT ;
GDL90_REPORT GDL_REPORT ;
# endif
2019-01-22 00:08:19 +00:00
# ifdef WITH_LOOKOUT // traffic awareness and warnings
# include "lookout.h"
LookOut Look ;
2020-02-24 21:54:21 +00:00
# ifdef WITH_SOUND
const char * Dir [ 16 ] = { " N " , " NNE " , " NE " , " NEE " , " E " , " SEE " , " SE " , " SSE " , " S " , " SSW " , " SW " , " SWW " , " W " , " NWW " , " NW " , " NNW " } ;
const char * RelDir [ 8 ] = { " A " , " AR " , " R " , " BR " , " B " , " BL " , " L " , " AL " } ;
void Sound_TrafficWarn ( const LookOut_Target * Tgt )
{ if ( ! Tgt ) return ;
uint8_t WarnLevel = Tgt - > WarnLevel ;
// uint16_t DistMargin = Tgt->DistMargin; // [0.5m]
uint16_t TimeMargin = Tgt - > TimeMargin ; // [0.5s]
uint16_t HorDist = Tgt - > HorDist ; // [0.5]
uint16_t Bearing = Tgt - > getBearing ( ) ; //
int16_t RelBearing = Look . getRelBearing ( Tgt ) ;
xSemaphoreTake ( CONS_Mutex , portMAX_DELAY ) ;
Format_String ( CONS_UART_Write , " Traffic: " ) ;
CONS_UART_Write ( ' # ' ) ;
CONS_UART_Write ( ' 0 ' + WarnLevel ) ;
CONS_UART_Write ( ' ' ) ;
// Format_Hex(CONS_UART_Write, Bearing);
// CONS_UART_Write(' ');
uint16_t DirIdx = ( Bearing + 0x800 ) > > 12 ; DirIdx & = 0x0F ;
Format_String ( CONS_UART_Write , Dir [ DirIdx ] ) ;
CONS_UART_Write ( ' ' ) ;
uint16_t RelDirIdx = ( RelBearing + 0x1000 ) > > 13 ; RelDirIdx & = 0x07 ;
Format_String ( CONS_UART_Write , RelDir [ RelDirIdx ] ) ;
CONS_UART_Write ( ' ' ) ;
Format_UnsDec ( CONS_UART_Write , ( uint16_t ) ( HorDist / 2 ) ) ;
Format_String ( CONS_UART_Write , " m " ) ;
Format_UnsDec ( CONS_UART_Write , ( uint16_t ) ( TimeMargin / 2 ) ) ;
Format_String ( CONS_UART_Write , " s \n " ) ;
xSemaphoreGive ( CONS_Mutex ) ;
// SoundMsg("Traffic");
}
# endif
2019-01-22 00:08:19 +00:00
# endif
2020-02-24 21:54:21 +00:00
// static uint16_t PrevBattVolt = 0; // [mV]
static Delay < uint16_t , 32 > BatteryVoltagePipe ;
uint32_t BatteryVoltage = 0 ; // [1/256 mV] low-pass filtered battery voltage
int32_t BatteryVoltageRate = 0 ; // [1/256 mV/sec] low-pass filtered battery voltage rise/drop rate
2019-01-22 00:08:19 +00:00
static char Line [ 128 ] ; // for printing out to the console, etc.
2018-01-29 12:43:22 +00:00
2019-01-22 00:08:19 +00:00
static LDPC_Decoder Decoder ; // decoder and error corrector for the OGN Gallager/LDPC code
2018-01-29 12:43:22 +00:00
2020-07-03 10:29:30 +00:00
FlightMonitor Flight ;
2018-01-29 12:43:22 +00:00
// #define DEBUG_PRINT
2019-01-22 00:08:19 +00:00
// =======================================================================================================================================
# ifdef WITH_LOG
2020-08-29 17:54:47 +00:00
static int FlashLog ( OGN_RxPacket < OGN_Packet > * Packet , uint32_t Time )
{ OGN_LogPacket < OGN_Packet > * LogPacket = FlashLog_FIFO . getWrite ( ) ; if ( LogPacket = = 0 ) return - 1 ; // allocate new packet in the LOG_FIFO
2020-02-24 21:54:21 +00:00
LogPacket - > Packet = Packet - > Packet ; // copy the packet
2019-01-22 00:08:19 +00:00
LogPacket - > Flags = 0x80 ;
LogPacket - > setTime ( Time ) ;
LogPacket - > setCheck ( ) ;
2020-08-29 17:54:47 +00:00
FlashLog_FIFO . Write ( ) ; // finalize the write
2019-01-22 00:08:19 +00:00
return 1 ; }
2020-08-29 17:54:47 +00:00
static int FlashLog ( OGN_TxPacket < OGN_Packet > * Packet , uint32_t Time )
{ OGN_LogPacket < OGN_Packet > * LogPacket = FlashLog_FIFO . getWrite ( ) ; if ( LogPacket = = 0 ) return - 1 ;
2019-01-22 00:08:19 +00:00
LogPacket - > Packet = Packet - > Packet ;
LogPacket - > Flags = 0x00 ;
LogPacket - > setTime ( Time ) ;
LogPacket - > setCheck ( ) ;
2020-08-29 17:54:47 +00:00
FlashLog_FIFO . Write ( ) ;
2019-01-22 00:08:19 +00:00
return 1 ; }
# endif // WITH_LOG
2018-01-29 12:43:22 +00:00
// ---------------------------------------------------------------------------------------------------------------------------------------
2020-02-24 21:54:21 +00:00
// #ifdef WITH_ESP32
// const uint8_t RelayQueueSize = 32;
// #else
// const uint8_t RelayQueueSize = 16;
// #endif
2019-01-22 00:08:19 +00:00
2020-02-24 21:54:21 +00:00
OGN_PrioQueue < OGN_Packet , RelayQueueSize > RelayQueue ; // received packets and candidates to be relayed
2018-01-29 12:43:22 +00:00
2018-03-09 13:11:21 +00:00
# ifdef DEBUG_PRINT
2018-01-29 12:43:22 +00:00
static void PrintRelayQueue ( uint8_t Idx ) // for debug
{ uint8_t Len = 0 ;
// Len+=Format_String(Line+Len, "");
xSemaphoreTake ( CONS_Mutex , portMAX_DELAY ) ;
// Format_String(CONS_UART_Write, Line, Len);
Line [ Len + + ] = ' [ ' ; Len + = Format_Hex ( Line + Len , Idx ) ; Line [ Len + + ] = ' ] ' ; Line [ Len + + ] = ' ' ;
Len + = RelayQueue . Print ( Line + Len ) ;
Format_String ( CONS_UART_Write , Line ) ;
2018-03-09 13:11:21 +00:00
xSemaphoreGive ( CONS_Mutex ) ; }
# endif
2018-01-29 12:43:22 +00:00
2019-01-22 00:08:19 +00:00
static bool GetRelayPacket ( OGN_TxPacket < OGN_Packet > * Packet ) // prepare a packet to be relayed
2019-05-01 20:49:34 +00:00
{ if ( RelayQueue . Sum = = 0 ) return 0 ; // if no packets in the relay queue
XorShift32 ( RX_Random ) ; // produce a new random number
uint8_t Idx = RelayQueue . getRand ( RX_Random ) ; // get weight-random packet from the relay queue
if ( RelayQueue . Packet [ Idx ] . Rank = = 0 ) return 0 ; // should not happen ...
memcpy ( Packet - > Packet . Byte ( ) , RelayQueue [ Idx ] - > Byte ( ) , OGN_Packet : : Bytes ) ; // copy the packet
2020-02-24 21:54:21 +00:00
Packet - > Packet . Header . Relay = 1 ; // increment the relay count (in fact we only do single relay)
// Packet->Packet.calcAddrParity();
2019-05-01 20:49:34 +00:00
Packet - > Packet . Whiten ( ) ; Packet - > calcFEC ( ) ; // whiten and calc. the FEC code => packet ready for transmission
2018-01-29 12:43:22 +00:00
// PrintRelayQueue(Idx); // for debug
2019-05-01 20:49:34 +00:00
RelayQueue . decrRank ( Idx ) ; // reduce the rank of the packet selected for relay
2018-01-29 12:43:22 +00:00
return 1 ; }
2019-05-01 20:49:34 +00:00
static void CleanRelayQueue ( uint32_t Time , uint32_t Delay = 20 ) // remove "old" packets from the relay queue
{ RelayQueue . cleanTime ( ( Time - Delay ) % 60 ) ; } // remove packets 20(default) seconds into the past
2018-01-29 12:43:22 +00:00
// ---------------------------------------------------------------------------------------------------------------------------------------
2019-01-22 00:08:19 +00:00
static uint16_t InfoParmIdx = 0 ; // the round-robin index to info records in info packets
static int ReadInfo ( OGN1_Packet & Packet )
{ Packet . clrInfo ( ) ;
uint8_t ParmIdx ;
for ( ParmIdx = InfoParmIdx ; ; )
{ const char * Parm = Parameters . InfoParmValue ( ParmIdx ) ;
if ( Parm )
{ // printf("Parm[%d]=%s\n", ParmIdx, Parm);
if ( Parm [ 0 ] )
{ int Add = Packet . addInfo ( Parm , ParmIdx ) ; if ( Add = = 0 ) break ; }
}
ParmIdx + + ; if ( ParmIdx > = Parameters . InfoParmNum ) ParmIdx = 0 ;
if ( ParmIdx = = InfoParmIdx ) break ;
}
InfoParmIdx = ParmIdx ;
Packet . setInfoCheck ( ) ;
return Packet . Info . DataChars ; } // zero => no info parameters were stored
// ---------------------------------------------------------------------------------------------------------------------------------------
static void ReadStatus ( OGN_Packet & Packet )
2018-01-29 12:43:22 +00:00
{
2019-01-22 00:08:19 +00:00
// #ifdef WITH_JACEK
/*
xSemaphoreTake ( ADC1_Mutex , portMAX_DELAY ) ;
uint16_t MCU_Vtemp = ADC_Read_MCU_Vtemp ( ) ; // T = 25+(V25-Vtemp)/Avg_Slope; V25=1.43+/-0.1V, Avg_Slope=4.3+/-0.3mV/degC
uint16_t MCU_Vref = ADC_Read_MCU_Vref ( ) ; // VDD = 1.2*4096/Vref
MCU_Vtemp + = ADC_Read_MCU_Vtemp ( ) ; // measure again and average
MCU_Vref + = ADC_Read_MCU_Vref ( ) ;
# ifdef WITH_BATT_SENSE
uint16_t Vbatt = ADC_Read_Vbatt ( ) ; // measure voltage on PB1
Vbatt + = ADC_Read_Vbatt ( ) ;
# endif
xSemaphoreGive ( ADC1_Mutex ) ;
int16_t MCU_Temp = - 999 ; // [0.1degC]
uint16_t MCU_VCC = 0 ; // [0.01V]
if ( MCU_Vref )
{ MCU_Temp = 250 + ( ( ( ( int32_t ) 1430 - ( ( int32_t ) 1200 * ( int32_t ) MCU_Vtemp + ( MCU_Vref > > 1 ) ) / MCU_Vref ) * ( int32_t ) 37 + 8 ) > > 4 ) ; // [0.1degC]
MCU_VCC = ( ( ( uint32_t ) 240 < < 12 ) + ( MCU_Vref > > 1 ) ) / MCU_Vref ; } // [0.01V]
Packet . EncodeVoltage ( ( ( MCU_VCC < < 4 ) + 12 ) / 25 ) ; // [1/64V] write supply voltage to the status packet
# ifdef WITH_BATT_SENSE
if ( MCU_Vref )
Packet . EncodeVoltage ( ( ( int32_t ) 154 * ( int32_t ) Vbatt + ( MCU_Vref > > 1 ) ) / MCU_Vref ) ; // [1/64V] battery voltage assuming 1:1 divider form battery to PB1
# endif
*/
2018-01-29 12:43:22 +00:00
2019-01-22 00:08:19 +00:00
// Packet.clrHumidity();
2018-02-18 14:41:50 +00:00
# ifdef WITH_STM32
2019-01-22 00:08:19 +00:00
# ifdef WITH_JACEK
uint16_t MCU_Vbatt = Measure_Vbatt ( ) ; // [0.001V]
Packet . EncodeVoltage ( ( ( MCU_Vbatt < < 3 ) + 62 ) / 125 ) ; // [1/64V]
if ( MCU_Vbatt < 3600 )
{ uint16_t FlashLen = 3600 - MCU_Vbatt ; if ( FlashLen > 250 ) FlashLen = 250 ;
LED_BAT_Flash ( FlashLen ) ; }
# else // WITH_JACEK
2018-02-18 14:41:50 +00:00
uint16_t MCU_VCC = Measure_MCU_VCC ( ) ; // [0.001V]
2019-01-22 00:08:19 +00:00
Packet . EncodeVoltage ( ( ( MCU_VCC < < 3 ) + 62 ) / 125 ) ; // [1/64V]
# endif
int16_t MCU_Temp = Measure_MCU_Temp ( ) ; // [0.1degC]
# endif
# ifdef WITH_ESP32
// Packet.clrTemperature();
2020-02-24 21:54:21 +00:00
uint16_t BattVolt = BatterySense ( ) ; // [mV] measure battery voltage
if ( BatteryVoltage > 0 )
{ // int32_t PrevVolt = BatteryVoltage;
int32_t Rate = ( ( uint32_t ) BattVolt < < 8 ) - BatteryVoltage ; // [1/256 mV]
BatteryVoltage + = ( Rate + 32 ) > > 6 ; // [1/256 mV] low-pass battery voltage measurement
uint16_t Volt = ( BatteryVoltage + 16 ) > > 5 ;
int16_t Diff = Volt - BatteryVoltagePipe . Input ( Volt ) ;
BatteryVoltageRate = Diff ; }
// BatteryVoltageRate = BatteryVoltage - PrevVolt; }
// int16_t BattVoltDiff = BattVolt - PrevBattVolt;
// int32_t Diff = ((int32_t)BattVoltDiff<<8) - BatteryVoltageRate;
// BatteryVoltageRate += Diff/256; }
// BatteryVoltageRate = (((int32_t)BattVoltDiff<<8) + BatteryVoltageRate*255 + 128)>>8; }
else
{ BatteryVoltage = BattVolt < < 8 ;
// PrevBattVolt = BattVolt;
BatteryVoltagePipe . Clear ( BattVolt < < 3 ) ;
BatteryVoltageRate = 0 ; }
// PrevBattVolt = BattVolt;
Packet . EncodeVoltage ( ( ( BatteryVoltage > > 2 ) + 500 ) / 1000 ) ; // [1/64V] encode into the status packet
# ifdef DEBUG_PRINT
xSemaphoreTake ( CONS_Mutex , portMAX_DELAY ) ;
Format_String ( CONS_UART_Write , " Battery: " ) ;
// Format_UnsDec(CONS_UART_Write, BattVolt);
// CONS_UART_Write(' ');
// Format_UnsDec(CONS_UART_Write, PrevBattVolt);
// CONS_UART_Write(' ');
// Format_UnsDec(CONS_UART_Write, BatteryVoltage, 2);
// CONS_UART_Write(' ');
// Format_SignDec(CONS_UART_Write, BatteryVoltageRate, 2);
// CONS_UART_Write(' ');
Format_UnsDec ( CONS_UART_Write , ( 10 * BatteryVoltage + 128 ) > > 8 , 5 , 4 ) ;
Format_String ( CONS_UART_Write , " V " ) ;
Format_SignDec ( CONS_UART_Write , ( 600 * BatteryVoltageRate + 128 ) > > 8 , 3 , 1 ) ;
Format_String ( CONS_UART_Write , " mV/min \n " ) ;
xSemaphoreGive ( CONS_Mutex ) ;
# endif
2018-02-18 14:41:50 +00:00
# endif
2019-05-07 23:42:39 +00:00
if ( Packet . Status . Pressure = = 0 ) Packet . EncodeTemperature ( TRX . chipTemp * 10 ) ; // [0.1degC]
Packet . Status . RadioNoise = TRX . averRSSI ; // [-0.5dBm] write radio noise to the status packet
2018-01-29 12:43:22 +00:00
2020-02-24 21:54:21 +00:00
uint8_t TxPower = Parameters . getTxPower ( ) - 4 ;
if ( TxPower > 15 ) TxPower = 15 ;
Packet . Status . TxPower = TxPower ;
2018-01-29 12:43:22 +00:00
2018-02-18 14:41:50 +00:00
uint16_t RxRate = RX_OGN_Count64 + 1 ;
2018-01-29 12:43:22 +00:00
uint8_t RxRateLog2 = 0 ; RxRate > > = 1 ; while ( RxRate ) { RxRate > > = 1 ; RxRateLog2 + + ; }
2019-01-22 00:08:19 +00:00
Packet . Status . RxRate = RxRateLog2 ;
2020-02-24 21:54:21 +00:00
if ( Parameters . Verbose )
{ uint8_t Len = 0 ;
2020-05-07 21:41:26 +00:00
Len + = Format_String ( Line + Len , " $POGNR, " ) ; // NMEA report: radio status
Len + = Format_UnsDec ( Line + Len , RF_FreqPlan . Plan ) ; // which frequency plan
2018-01-29 12:43:22 +00:00
Line [ Len + + ] = ' , ' ;
2020-05-07 21:41:26 +00:00
Len + = Format_UnsDec ( Line + Len , RX_OGN_Count64 ) ; // number of OGN packets received
2018-01-29 12:43:22 +00:00
Line [ Len + + ] = ' , ' ;
Line [ Len + + ] = ' , ' ;
2019-05-07 23:42:39 +00:00
Len + = Format_SignDec ( Line + Len , - 5 * TRX . averRSSI , 2 , 1 ) ; // average RF level (over all channels)
2018-01-29 12:43:22 +00:00
Line [ Len + + ] = ' , ' ;
Len + = Format_UnsDec ( Line + Len , ( uint16_t ) TX_Credit ) ;
Line [ Len + + ] = ' , ' ;
2020-05-07 21:41:26 +00:00
Len + = Format_SignDec ( Line + Len , ( int16_t ) TRX . chipTemp ) ; // the temperature of the RF chip
2018-01-29 12:43:22 +00:00
Line [ Len + + ] = ' , ' ;
// Len+=Format_SignDec(Line+Len, MCU_Temp, 2, 1);
Line [ Len + + ] = ' , ' ;
2019-01-22 00:08:19 +00:00
# ifdef WITH_STM32
# ifdef WITH_JACEK
Len + = Format_UnsDec ( Line + Len , ( MCU_Vbatt + 5 ) / 10 , 3 , 2 ) ;
# else
2018-01-29 12:43:22 +00:00
// Len+=Format_UnsDec(Line+Len, (MCU_VCC+5)/10, 3, 2);
2019-01-22 00:08:19 +00:00
# endif
# endif
2018-01-29 12:43:22 +00:00
Len + = NMEA_AppendCheckCRNL ( Line , Len ) ; // append NMEA check-sum and CR+NL
// LogLine(Line);
// if(CONS_UART_Free()>=128)
{ xSemaphoreTake ( CONS_Mutex , portMAX_DELAY ) ;
2020-05-07 21:41:26 +00:00
Format_String ( CONS_UART_Write , Line , 0 , Len ) ; // send the NMEA out to the console
2018-01-29 12:43:22 +00:00
xSemaphoreGive ( CONS_Mutex ) ; }
# ifdef WITH_SDLOG
if ( Log_Free ( ) > = 128 )
{ xSemaphoreTake ( Log_Mutex , portMAX_DELAY ) ;
2020-05-07 21:41:26 +00:00
Format_String ( Log_Write , Line , 0 , Len ) ; // send the NMEA out to the log file
2018-01-29 12:43:22 +00:00
xSemaphoreGive ( Log_Mutex ) ; }
# endif
}
}
2019-01-22 00:08:19 +00:00
// static void ReadStatus(OGN_TxPacket<OGN_Packet> &StatPacket)
// { ReadStatus(StatPacket.Packet); }
2018-05-19 01:03:18 +00:00
2020-05-01 14:01:36 +00:00
# ifndef WITH_LOOKOUT // with LookOut the PFLAU is produced inside LookOut
2019-05-01 20:49:34 +00:00
static uint8_t WritePFLAU ( char * NMEA , uint8_t GPS = 1 ) // produce the (mostly dummy) PFLAU to satisfy XCsoar and LK8000
{ uint8_t Len = 0 ;
Len + = Format_String ( NMEA + Len , " $PFLAU, " ) ;
NMEA [ Len + + ] = ' 0 ' ;
NMEA [ Len + + ] = ' , ' ;
NMEA [ Len + + ] = ' 0 ' + GPS ; // TX status
NMEA [ Len + + ] = ' , ' ;
NMEA [ Len + + ] = ' 0 ' + GPS ; // GPS status
NMEA [ Len + + ] = ' , ' ;
NMEA [ Len + + ] = ' 1 ' ; // power status: one could monitor the supply
NMEA [ Len + + ] = ' , ' ;
NMEA [ Len + + ] = ' 0 ' ;
NMEA [ Len + + ] = ' , ' ;
NMEA [ Len + + ] = ' , ' ;
NMEA [ Len + + ] = ' 0 ' ;
NMEA [ Len + + ] = ' , ' ;
NMEA [ Len + + ] = ' , ' ;
Len + = NMEA_AppendCheckCRNL ( NMEA , Len ) ;
NMEA [ Len ] = 0 ;
return Len ; }
2020-05-01 14:01:36 +00:00
# endif
2019-05-01 20:49:34 +00:00
2018-05-19 01:03:18 +00:00
// ---------------------------------------------------------------------------------------------------------------------------------------
2019-01-22 00:08:19 +00:00
static void ProcessRxPacket ( OGN_RxPacket < OGN_Packet > * RxPacket , uint8_t RxPacketIdx , uint32_t RxTime ) // process every (correctly) received packet
2018-01-29 12:43:22 +00:00
{ int32_t LatDist = 0 , LonDist = 0 ; uint8_t Warn = 0 ;
2019-05-01 20:49:34 +00:00
if ( RxPacket - > Packet . Header . NonPos | | RxPacket - > Packet . Header . Encrypted ) return ; // status packet or encrypted: ignore
2018-01-29 12:43:22 +00:00
uint8_t MyOwnPacket = ( RxPacket - > Packet . Header . Address = = Parameters . Address )
& & ( RxPacket - > Packet . Header . AddrType = = Parameters . AddrType ) ;
2018-02-18 14:41:50 +00:00
if ( MyOwnPacket ) return ; // don't process my own (relayed) packets
2018-01-29 12:43:22 +00:00
bool DistOK = RxPacket - > Packet . calcDistanceVector ( LatDist , LonDist , GPS_Latitude , GPS_Longitude , GPS_LatCosine ) > = 0 ;
if ( DistOK )
2018-02-18 14:41:50 +00:00
{ RxPacket - > calcRelayRank ( GPS_Altitude / 10 ) ; // calculate the relay-rank (priority for relay)
2019-01-22 00:08:19 +00:00
OGN_RxPacket < OGN_Packet > * PrevRxPacket = RelayQueue . addNew ( RxPacketIdx ) ;
2020-02-24 21:54:21 +00:00
# ifdef WITH_POGNT
{ uint8_t Len = RxPacket - > WritePOGNT ( Line ) ; // print on the console as $POGNT
2020-05-06 16:31:43 +00:00
if ( Parameters . Verbose )
{ xSemaphoreTake ( CONS_Mutex , portMAX_DELAY ) ;
Format_String ( CONS_UART_Write , Line , 0 , Len ) ;
xSemaphoreGive ( CONS_Mutex ) ; }
# ifdef WITH_SDLOG
if ( Log_Free ( ) > = 128 )
{ xSemaphoreTake ( Log_Mutex , portMAX_DELAY ) ;
Format_String ( Log_Write , Line , 0 , Len ) ;
xSemaphoreGive ( Log_Mutex ) ; }
# endif
}
2020-02-24 21:54:21 +00:00
# endif
2019-01-22 00:08:19 +00:00
// Len=RxPacket->Packet.WriteAPRS(Line, RxTime); // print on the console as APRS message
// xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
// Format_String(CONS_UART_Write, Line, 0, Len);
// xSemaphoreGive(CONS_Mutex);
# ifdef WITH_LOOKOUT
const LookOut_Target * Tgt = Look . ProcessTarget ( RxPacket - > Packet ) ; // process the received target postion
if ( Tgt ) Warn = Tgt - > WarnLevel ; // remember warning level of this target
2020-05-01 14:01:36 +00:00
# ifdef WITH_GDL90
if ( Tgt )
{ Look . Write ( GDL_REPORT , Tgt ) ; // produce GDL90 report for this target
xSemaphoreTake ( CONS_Mutex , portMAX_DELAY ) ;
GDL_REPORT . Send ( CONS_UART_Write , 20 ) ; // transmit as traffic position report (not own-ship)
xSemaphoreGive ( CONS_Mutex ) ; }
# endif
2019-01-22 00:08:19 +00:00
# ifdef WITH_BEEPER
2020-02-24 21:54:21 +00:00
if ( KNOB_Tick > 12 ) Play ( Play_Vol_1 | Play_Oct_2 | ( 7 + 2 * Warn ) , 3 + 16 * Warn ) ;
2019-01-22 00:08:19 +00:00
# endif
2020-02-24 21:54:21 +00:00
# else // if not WITH_LOOKOUT
2018-01-29 12:43:22 +00:00
# ifdef WITH_BEEPER
2018-02-18 14:41:50 +00:00
if ( KNOB_Tick > 12 ) Play ( Play_Vol_1 | Play_Oct_2 | 7 , 3 ) ; // if Knob>12 => make a beep for every received packet
2018-01-29 12:43:22 +00:00
# endif
2019-01-22 00:08:19 +00:00
# endif // WITH_LOOKOUT
# ifdef WITH_LOG
bool Signif = PrevRxPacket ! = 0 ;
if ( ! Signif ) Signif = OGN_isSignif ( & ( RxPacket - > Packet ) , & ( PrevRxPacket - > Packet ) ) ;
2020-08-29 17:54:47 +00:00
if ( Signif ) FlashLog ( RxPacket , RxTime ) ; // log only significant packets
2019-01-22 00:08:19 +00:00
# endif
2018-01-29 12:43:22 +00:00
# ifdef WITH_PFLAA
2020-02-24 21:54:21 +00:00
if ( Parameters . Verbose // print PFLAA on the console for received packets
# ifdef WITH_LOOKOUT
& & ( ! Tgt )
# endif
)
{ uint8_t Len = RxPacket - > WritePFLAA ( Line , Warn , LatDist , LonDist , RxPacket - > Packet . DecodeAltitude ( ) - GPS_Altitude / 10 ) ;
xSemaphoreTake ( CONS_Mutex , portMAX_DELAY ) ;
Format_String ( CONS_UART_Write , Line , 0 , Len ) ;
2020-05-07 21:41:26 +00:00
xSemaphoreGive ( CONS_Mutex ) ;
# ifdef WITH_SDLOG
if ( Log_Free ( ) > = 128 )
{ xSemaphoreTake ( Log_Mutex , portMAX_DELAY ) ;
Format_String ( Log_Write , Line , 0 , Len ) ; // send the NMEA out to the log file
xSemaphoreGive ( Log_Mutex ) ; }
# endif
}
2018-01-29 12:43:22 +00:00
# endif
2019-05-05 20:51:49 +00:00
# ifdef WITH_MAVLINK
MAV_ADSB_VEHICLE MAV_RxReport ;
RxPacket - > Packet . Encode ( & MAV_RxReport ) ;
MAV_RxMsg : : Send ( sizeof ( MAV_RxReport ) , MAV_Seq + + , MAV_SysID , MAV_COMP_ID_ADSB , MAV_ID_ADSB_VEHICLE , ( const uint8_t * ) & MAV_RxReport , GPS_UART_Write ) ;
2020-02-24 21:54:21 +00:00
// xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
// MAV_RxMsg::Send(sizeof(MAV_RxReport), MAV_Seq++, MAV_SysID, MAV_COMP_ID_ADSB, MAV_ID_ADSB_VEHICLE, (const uint8_t *)&MAV_RxReport, CONS_UART_Write);
// xSemaphoreGive(CONS_Mutex);
2019-05-05 20:51:49 +00:00
# endif
2018-01-29 12:43:22 +00:00
}
}
static void DecodeRxPacket ( RFM_RxPktData * RxPkt )
{
uint8_t RxPacketIdx = RelayQueue . getNew ( ) ; // get place for this new packet
2019-01-22 00:08:19 +00:00
OGN_RxPacket < OGN_Packet > * RxPacket = RelayQueue [ RxPacketIdx ] ;
2018-01-29 12:43:22 +00:00
// PrintRelayQueue(RxPacketIdx); // for debug
// RxPacket->RxRSSI=RxPkt.RSSI;
// TickType_t ExecTime=xTaskGetTickCount();
2018-02-18 14:41:50 +00:00
{ RX_OGN_Packets + + ;
uint8_t Check = RxPkt - > Decode ( * RxPacket , Decoder ) ;
2018-01-29 12:43:22 +00:00
# ifdef DEBUG_PRINT
2018-02-18 14:41:50 +00:00
xSemaphoreTake ( CONS_Mutex , portMAX_DELAY ) ;
2019-01-22 00:08:19 +00:00
Format_String ( CONS_UART_Write , " DecodeRxPkt: " ) ;
2018-02-18 14:41:50 +00:00
Format_Hex ( CONS_UART_Write , RxPacket - > Packet . HeaderWord ) ;
CONS_UART_Write ( ' ' ) ;
2019-01-22 00:08:19 +00:00
Format_UnsDec ( CONS_UART_Write , ( uint16_t ) Check , 2 ) ;
2018-02-18 14:41:50 +00:00
CONS_UART_Write ( ' / ' ) ;
Format_UnsDec ( CONS_UART_Write , ( uint16_t ) RxPacket - > RxErr ) ;
2019-01-22 00:08:19 +00:00
Format_String ( CONS_UART_Write , " e \n " ) ;
2018-02-18 14:41:50 +00:00
xSemaphoreGive ( CONS_Mutex ) ;
2018-01-29 12:43:22 +00:00
# endif
2018-02-18 14:41:50 +00:00
if ( ( Check = = 0 ) & & ( RxPacket - > RxErr < 15 ) ) // what limit on number of detected bit errors ?
{ RxPacket - > Packet . Dewhiten ( ) ;
2019-01-22 00:08:19 +00:00
ProcessRxPacket ( RxPacket , RxPacketIdx , RxPkt - > Time ) ; }
2018-02-18 14:41:50 +00:00
}
2018-01-29 12:43:22 +00:00
}
// -------------------------------------------------------------------------------------------------------------------
# ifdef __cplusplus
extern " C "
# endif
void vTaskPROC ( void * pvParameters )
{
2018-02-18 14:41:50 +00:00
# ifdef WITH_FLASHLOG
uint16_t kB = FlashLog_OpenForWrite ( ) ;
xSemaphoreTake ( CONS_Mutex , portMAX_DELAY ) ;
Format_String ( CONS_UART_Write , " TaskPROC: " ) ;
Format_UnsDec ( CONS_UART_Write , kB ) ;
Format_String ( CONS_UART_Write , " KB FlashLog \n " ) ;
xSemaphoreGive ( CONS_Mutex ) ;
# endif
2018-01-29 12:43:22 +00:00
RelayQueue . Clear ( ) ;
2019-01-22 00:08:19 +00:00
# ifdef WITH_LOOKOUT
Look . Clear ( ) ;
# endif
2018-03-09 13:11:21 +00:00
2019-01-22 00:08:19 +00:00
OGN_TxPacket < OGN_Packet > PosPacket ; // position packet
OGN_Packet PrevLoggedPacket ; // most recent logged packet
uint32_t PosTime = 0 ; // [sec] when the position was recorded
OGN_TxPacket < OGN_Packet > StatPacket ; // status report packet
// OGN_TxPacket<OGN_Packet> InfoPacket; // information packet
2018-01-29 12:43:22 +00:00
for ( ; ; )
{ vTaskDelay ( 1 ) ;
2018-02-18 14:41:50 +00:00
RFM_RxPktData * RxPkt = RF_RxFIFO . getRead ( ) ; // check for new received packets
2020-02-24 21:54:21 +00:00
if ( RxPkt ) // if there is a new received packet
2018-01-29 12:43:22 +00:00
{
# ifdef DEBUG_PRINT
xSemaphoreTake ( CONS_Mutex , portMAX_DELAY ) ;
Format_UnsDec ( CONS_UART_Write , TimeSync_Time ( ) % 60 , 2 ) ;
CONS_UART_Write ( ' . ' ) ;
Format_UnsDec ( CONS_UART_Write , TimeSync_msTime ( ) , 3 ) ;
Format_String ( CONS_UART_Write , " RF_RxFIFO -> " ) ;
RxPkt - > Print ( CONS_UART_Write ) ;
// CONS_UART_Write('\r'); CONS_UART_Write('\n');
xSemaphoreGive ( CONS_Mutex ) ;
# endif
2018-02-18 14:41:50 +00:00
DecodeRxPacket ( RxPkt ) ; // decode and process the received packet
2020-02-24 21:54:21 +00:00
RF_RxFIFO . Read ( ) ; } // remove this packet from the queue
2018-01-29 12:43:22 +00:00
2018-02-18 14:41:50 +00:00
static uint32_t PrevSlotTime = 0 ; // remember previous time slot to detect a change
uint32_t SlotTime = TimeSync_Time ( ) ; // time slot
2020-02-24 21:54:21 +00:00
if ( TimeSync_msTime ( ) < 340 ) SlotTime - - ; // lasts up to 0.300sec after the PPS
2018-02-18 14:41:50 +00:00
if ( SlotTime = = PrevSlotTime ) continue ; // stil same time slot, go back to RX processing
PrevSlotTime = SlotTime ; // new slot started
2018-01-29 12:43:22 +00:00
// this part of the loop is executed only once per slot-time
2018-02-18 14:41:50 +00:00
uint8_t BestIdx ; int16_t BestResid ;
# ifdef WITH_MAVLINK
GPS_Position * Position = GPS_getPosition ( BestIdx , BestResid , ( SlotTime - 1 ) % 60 , 0 ) ;
# else
2020-02-24 21:54:21 +00:00
GPS_Position * Position = GPS_getPosition ( BestIdx , BestResid , SlotTime % 60 , 0 ) ; // get GPS position which isReady
2018-02-18 14:41:50 +00:00
# endif
2020-05-01 14:01:36 +00:00
// GPS_Position *Position = GPS_getPosition();
2018-02-18 14:41:50 +00:00
# ifdef DEBUG_PRINT
xSemaphoreTake ( CONS_Mutex , portMAX_DELAY ) ;
2020-02-24 21:54:21 +00:00
Format_String ( CONS_UART_Write , " getPos( " ) ;
2018-02-18 14:41:50 +00:00
Format_UnsDec ( CONS_UART_Write , SlotTime % 60 , 2 ) ;
2020-02-24 21:54:21 +00:00
Format_String ( CONS_UART_Write , " ) => " ) ;
2018-02-18 14:41:50 +00:00
Format_UnsDec ( CONS_UART_Write , ( uint16_t ) BestIdx ) ;
CONS_UART_Write ( ' : ' ) ;
Format_SignDec ( CONS_UART_Write , BestResid , 3 , 2 ) ;
Format_String ( CONS_UART_Write , " s \n " ) ;
xSemaphoreGive ( CONS_Mutex ) ;
# endif
2020-07-03 10:29:30 +00:00
2020-05-01 14:01:36 +00:00
# ifdef WITH_GDL90
GDL_HEARTBEAT . Clear ( ) ;
GDL_HEARTBEAT . Initialized = 1 ;
if ( Position )
{ if ( Position - > isTimeValid ( ) )
{ GDL_HEARTBEAT . UTCvalid = 1 ;
GDL_HEARTBEAT . setTimeStamp ( SlotTime ) ;
if ( Position - > isValid ( ) ) GDL_HEARTBEAT . PosValid = 1 ; }
}
GDL_REPORT . Clear ( ) ;
GDL_REPORT . setAddress ( Parameters . Address ) ;
GDL_REPORT . setAddrType ( Parameters . AddrType ! = 1 ) ;
GDL_REPORT . setAcftType ( Parameters . AcftType ) ;
if ( Parameters . Reg [ 0 ] ) GDL_REPORT . setAcftCall ( Parameters . Reg ) ;
// else GDL_REPORT.setAcftCall();
if ( Position & & Position - > isValid ( ) ) Position - > Encode ( GDL_REPORT ) ;
xSemaphoreTake ( CONS_Mutex , portMAX_DELAY ) ;
GDL_HEARTBEAT . Send ( CONS_UART_Write ) ;
GDL_REPORT . Send ( CONS_UART_Write ) ;
xSemaphoreGive ( CONS_Mutex ) ;
# endif
2020-07-04 15:49:55 +00:00
if ( Position )
{ Position - > EncodeStatus ( StatPacket . Packet ) ; // encode GPS altitude and pressure/temperature/humidity
Flight . Process ( * Position ) ; } // flight monitor: takeoff/landing
else
{ StatPacket . Packet . Status . FixQuality = 0 ; StatPacket . Packet . Status . Satellites = 0 ; } // or lack of the GPS lock
{ uint8_t SatSNR = ( GPS_SatSNR + 2 ) / 4 ; // encode number of satellites and SNR in the Status packet
2020-02-24 21:54:21 +00:00
if ( SatSNR > 8 ) { SatSNR - = 8 ; if ( SatSNR > 31 ) SatSNR = 31 ; }
else { SatSNR = 0 ; }
StatPacket . Packet . Status . SatSNR = SatSNR ; }
2020-07-04 15:49:55 +00:00
2020-09-21 00:43:27 +00:00
if ( Position & & Position - > isTimeValid ( ) & & Position - > isDateValid ( ) ) PosTime = Position - > getUnixTime ( ) ;
else PosTime = 0 ;
2020-02-24 21:54:21 +00:00
if ( Position & & Position - > isReady & & ( ! Position - > Sent ) & & Position - > isValid ( ) )
2019-01-22 00:08:19 +00:00
{ int16_t AverSpeed = GPS_AverageSpeed ( ) ; // [0.1m/s] average speed, including the vertical speed
2018-07-23 15:02:44 +00:00
if ( Parameters . FreqPlan = = 0 )
RF_FreqPlan . setPlan ( Position - > Latitude , Position - > Longitude ) ; // set the frequency plan according to the GPS position
else RF_FreqPlan . setPlan ( Parameters . FreqPlan ) ;
2018-01-29 12:43:22 +00:00
# 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 , " -> Sent \n " ) ;
xSemaphoreGive ( CONS_Mutex ) ;
# endif
2020-09-21 00:43:27 +00:00
// PosTime=Position->getUnixTime();
2018-01-29 12:43:22 +00:00
PosPacket . Packet . HeaderWord = 0 ;
PosPacket . Packet . Header . Address = Parameters . Address ; // set address
PosPacket . Packet . Header . AddrType = Parameters . AddrType ; // address-type
2019-06-02 08:02:57 +00:00
# ifdef WITH_ENCRYPT
if ( Parameters . Encrypt ) // if position encryption is requested
{ PosPacket . Packet . Header . Encrypted = 1 ; } // then set the flg in the header
# endif
2018-01-29 12:43:22 +00:00
PosPacket . Packet . calcAddrParity ( ) ; // parity of (part of) the header
2018-02-18 14:41:50 +00:00
if ( BestResid = = 0 ) Position - > Encode ( PosPacket . Packet ) ; // encode position/altitude/speed/etc. from GPS position
2020-02-24 21:54:21 +00:00
else // extrapolate the position when if not at an exact UTC second
{ while ( BestResid > = 50 ) BestResid - = 100 ; // remove full seconds
Position - > Encode ( PosPacket . Packet , BestResid ) ; }
2018-01-29 12:43:22 +00:00
PosPacket . Packet . Position . AcftType = Parameters . AcftType ; // aircraft-type
2020-02-24 21:54:21 +00:00
PosPacket . Packet . Position . Stealth = 0 ; // Parameters.Stealth;
# ifdef DEBUG_PRINT
{ uint8_t Len = PosPacket . Packet . WriteAPRS ( Line , PosTime ) ; // print on the console as APRS message
xSemaphoreTake ( CONS_Mutex , portMAX_DELAY ) ;
Format_String ( CONS_UART_Write , Line , 0 , Len ) ;
xSemaphoreGive ( CONS_Mutex ) ; }
# endif
2019-01-22 00:08:19 +00:00
OGN_TxPacket < OGN_Packet > * TxPacket = RF_TxFIFO . getWrite ( ) ;
2018-01-29 12:43:22 +00:00
TxPacket - > Packet = PosPacket . Packet ; // copy the position packet to the TxFIFO
2020-05-01 14:01:36 +00:00
2019-06-02 08:02:57 +00:00
# ifdef WITH_ENCRYPT
if ( Parameters . Encrypt ) TxPacket - > Packet . Encrypt ( Parameters . EncryptKey ) ; // if encryption is requested then encrypt
else TxPacket - > Packet . Whiten ( ) ; // otherwise only whiten
# else
TxPacket - > Packet . Whiten ( ) ;
# endif
TxPacket - > calcFEC ( ) ; // whiten and calculate FEC code
2018-01-29 12:43:22 +00:00
# ifdef DEBUG_PRINT
xSemaphoreTake ( CONS_Mutex , portMAX_DELAY ) ;
2020-02-24 21:54:21 +00:00
Format_UnsDec ( CONS_UART_Write , TimeSync_Time ( ) % 60 , 2 ) ;
2018-01-29 12:43:22 +00:00
CONS_UART_Write ( ' . ' ) ;
Format_UnsDec ( CONS_UART_Write , TimeSync_msTime ( ) , 3 ) ;
Format_String ( CONS_UART_Write , " TxFIFO <- " ) ;
Format_Hex ( CONS_UART_Write , TxPacket - > Packet . HeaderWord ) ;
CONS_UART_Write ( ' \r ' ) ; CONS_UART_Write ( ' \n ' ) ;
xSemaphoreGive ( CONS_Mutex ) ;
# endif
XorShift32 ( RX_Random ) ;
2020-02-24 21:54:21 +00:00
if ( ( AverSpeed > 10 ) | | ( ( RX_Random & 0x3 ) = = 0 ) ) // send only some positions if the speed is less than 1m/s
RF_TxFIFO . Write ( ) ; // complete the write into the TxFIFO
2018-01-29 12:43:22 +00:00
Position - > Sent = 1 ;
2020-05-01 14:01:36 +00:00
# ifdef WITH_FANET
2020-05-05 23:13:14 +00:00
if ( ( SlotTime & 0x07 ) = = ( RX_Random & 0x07 ) ) // every 8sec
2020-05-01 14:01:36 +00:00
{ FANET_Packet * FNTpkt = FNT_TxFIFO . getWrite ( ) ;
FNTpkt - > setAddress ( Parameters . Address ) ;
Position - > EncodeAirPos ( * FNTpkt , Parameters . AcftType , ! Parameters . Stealth ) ;
2020-05-05 23:13:14 +00:00
XorShift32 ( RX_Random ) ;
2020-05-01 14:01:36 +00:00
FNT_TxFIFO . Write ( ) ; }
# endif
2019-01-22 00:08:19 +00:00
# ifdef WITH_LOOKOUT
2020-02-24 21:54:21 +00:00
const LookOut_Target * Tgt = Look . ProcessOwn ( PosPacket . Packet ) ; // process own position, get the most dangerous target
2018-05-19 01:03:18 +00:00
# ifdef WITH_PFLAA
2020-02-24 21:54:21 +00:00
if ( Parameters . Verbose )
{ xSemaphoreTake ( CONS_Mutex , portMAX_DELAY ) ;
Look . WritePFLA ( CONS_UART_Write ) ; // produce PFLAU and PFLAA for all tracked targets
2020-05-07 21:41:26 +00:00
xSemaphoreGive ( CONS_Mutex ) ;
# ifdef WITH_SDLOG
if ( Log_Free ( ) > = 512 )
{ xSemaphoreTake ( Log_Mutex , portMAX_DELAY ) ;
Look . WritePFLA ( Log_Write ) ;
xSemaphoreGive ( Log_Mutex ) ; }
# endif
}
2020-02-24 21:54:21 +00:00
# else
if ( Parameters . Verbose )
{ uint8_t Len = Look . WritePFLAU ( Line ) ; // $PFLAU, overall status
xSemaphoreTake ( CONS_Mutex , portMAX_DELAY ) ;
Format_String ( CONS_UART_Write , Line , 0 , Len ) ;
2020-05-07 21:41:26 +00:00
xSemaphoreGive ( CONS_Mutex ) ;
# ifdef WITH_SDLOG
if ( Log_Free ( ) > = 128 )
{ xSemaphoreTake ( Log_Mutex , portMAX_DELAY ) ;
Format_String ( Log_Write , Line , 0 , Len ) ; // send the NMEA out to the log file
xSemaphoreGive ( Log_Mutex ) ; }
# endif
}
2018-05-19 01:03:18 +00:00
# endif // WITH_PFLAA
2019-01-22 00:08:19 +00:00
uint8_t Warn = 0 ;
if ( Tgt ) Warn = Tgt - > WarnLevel ; // what is the warning level ?
2020-02-24 21:54:21 +00:00
if ( ( Warn > 0 ) /* && (AverSpeed>=10) */ ) // if non-zero warning level and we seem to be moving
{ // int16_t RelBearing = Look.getRelBearing(Tgt); // relative bearing to the Target
// int8_t Bearing = (12*(int32_t)RelBearing+0x8000)>>16; // [-12..+12]
2019-01-22 00:08:19 +00:00
# ifdef WITH_BEEPER // make the sound according to the level
if ( Warn < = 1 )
{ if ( KNOB_Tick > 8 )
{ Play ( Play_Vol_1 | Play_Oct_1 | 4 , 200 ) ; }
}
else if ( Warn < = 2 )
{ if ( KNOB_Tick > 4 )
{ Play ( Play_Vol_3 | Play_Oct_1 | 8 , 150 ) ; Play ( Play_Oct_1 | 8 , 150 ) ;
Play ( Play_Vol_3 | Play_Oct_1 | 8 , 150 ) ; }
}
else if ( Warn < = 3 )
{ if ( KNOB_Tick > 2 )
{ Play ( Play_Vol_3 | Play_Oct_1 | 11 , 100 ) ; Play ( Play_Oct_1 | 11 , 100 ) ;
Play ( Play_Vol_3 | Play_Oct_1 | 11 , 100 ) ; Play ( Play_Oct_1 | 11 , 100 ) ;
Play ( Play_Vol_3 | Play_Oct_1 | 11 , 100 ) ; }
}
# endif // WITH_BEEPER
2020-02-24 21:54:21 +00:00
# ifdef WITH_SOUND
Sound_TrafficWarn ( Tgt ) ;
# endif
2019-01-22 00:08:19 +00:00
}
2020-05-01 14:01:36 +00:00
# else // WITH_LOOKOUT
if ( Parameters . Verbose )
{ uint8_t Len = Look . WritePFLAU ( Line ) ; // $PFLAU, overall status
xSemaphoreTake ( CONS_Mutex , portMAX_DELAY ) ;
Format_String ( CONS_UART_Write , Line , 0 , Len ) ;
2020-05-07 21:41:26 +00:00
xSemaphoreGive ( CONS_Mutex ) ;
# ifdef WITH_SDLOG
if ( Log_Free ( ) > = 128 )
{ xSemaphoreTake ( Log_Mutex , portMAX_DELAY ) ;
Format_String ( Log_Write , Line , 0 , Len ) ; // send the NMEA out to the log file
xSemaphoreGive ( Log_Mutex ) ; }
# endif
}
2019-01-22 00:08:19 +00:00
# endif // WITH_LOOKOUT
2018-02-18 14:41:50 +00:00
# ifdef WITH_FLASHLOG
bool Written = FlashLog_Process ( PosPacket . Packet , PosTime ) ;
// if(Written)
// { uint8_t Len=FlashLog_Print(Line);
// xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
// Format_String(CONS_UART_Write, Line);
// xSemaphoreGive(CONS_Mutex);
// }
# endif // WITH_FLASHLOG
2019-01-22 00:08:19 +00:00
# ifdef WITH_LOG
bool isSignif = OGN_isSignif ( & ( PosPacket . Packet ) , & PrevLoggedPacket ) ;
if ( isSignif )
2020-08-29 17:54:47 +00:00
{ FlashLog ( & PosPacket , PosTime ) ;
2019-01-22 00:08:19 +00:00
PrevLoggedPacket = PosPacket . Packet ; }
# endif
2018-01-29 12:43:22 +00:00
} else // if GPS position is not complete, contains no valid position, etc.
{ if ( ( SlotTime - PosTime ) > = 30 ) { PosPacket . Packet . Position . Time = 0x3F ; } // if no valid position for more than 30 seconds then set the time as unknown for the transmitted packet
2019-01-22 00:08:19 +00:00
OGN_TxPacket < OGN_Packet > * TxPacket = RF_TxFIFO . getWrite ( ) ;
2018-01-29 12:43:22 +00:00
TxPacket - > Packet = PosPacket . Packet ;
TxPacket - > Packet . Whiten ( ) ; TxPacket - > calcFEC ( ) ; // whiten and calculate FEC code
# ifdef DEBUG_PRINT
xSemaphoreTake ( CONS_Mutex , portMAX_DELAY ) ;
Format_String ( CONS_UART_Write , " TxFIFO: " ) ;
Format_Hex ( CONS_UART_Write , TxPacket - > Packet . HeaderWord ) ;
CONS_UART_Write ( ' \r ' ) ; CONS_UART_Write ( ' \n ' ) ;
xSemaphoreGive ( CONS_Mutex ) ;
# endif
XorShift32 ( RX_Random ) ;
if ( PosTime & & ( ( RX_Random & 0x3 ) = = 0 ) ) // send if some position in the packet and at 1/4 normal rate
RF_TxFIFO . Write ( ) ; // complete the write into the TxFIFO
if ( Position ) Position - > Sent = 1 ;
}
# ifdef DEBUG_PRINT
// char Line[128];
Line [ 0 ] = ' 0 ' + RF_TxFIFO . Full ( ) ; Line [ 1 ] = ' ' ; // print number of packets in the TxFIFO
RelayQueue . Print ( Line + 2 ) ; // dump the relay queue
xSemaphoreTake ( CONS_Mutex , portMAX_DELAY ) ;
Format_String ( CONS_UART_Write , Line ) ;
xSemaphoreGive ( CONS_Mutex ) ;
# endif
2020-05-01 14:01:36 +00:00
# ifdef WITH_FANET
2020-05-05 23:13:14 +00:00
if ( Parameters . Pilot [ 0 ] & & ( SlotTime & 0xFF ) = = ( RX_Random & 0xFF ) ) // every 256sec
2020-05-01 14:01:36 +00:00
{ FANET_Packet * FNTpkt = FNT_TxFIFO . getWrite ( ) ;
FNTpkt - > setAddress ( Parameters . Address ) ;
FNTpkt - > setName ( Parameters . Pilot ) ;
2020-05-05 23:13:14 +00:00
XorShift32 ( RX_Random ) ;
2020-05-01 14:01:36 +00:00
FNT_TxFIFO . Write ( ) ; }
# endif
2018-01-29 12:43:22 +00:00
StatPacket . Packet . HeaderWord = 0 ;
StatPacket . Packet . Header . Address = Parameters . Address ; // set address
StatPacket . Packet . Header . AddrType = Parameters . AddrType ; // address-type
2019-01-22 00:08:19 +00:00
StatPacket . Packet . Header . NonPos = 1 ;
2018-01-29 12:43:22 +00:00
StatPacket . Packet . calcAddrParity ( ) ; // parity of (part of) the header
StatPacket . Packet . Status . Hardware = HARDWARE_ID ;
StatPacket . Packet . Status . Firmware = SOFTWARE_ID ;
2019-01-22 00:08:19 +00:00
ReadStatus ( StatPacket . Packet ) ; // read status data and put them into the StatPacket
XorShift32 ( RX_Random ) ; // generate a new random number
if ( ( ( RX_Random & 0x0F ) = = 0 ) & & ( RF_TxFIFO . Full ( ) < 2 ) ) // decide whether to transmit the status/info packet
{ OGN_TxPacket < OGN_Packet > * StatusPacket = RF_TxFIFO . getWrite ( ) ; // ask for space in the Tx queue
uint8_t doTx = 1 ;
if ( RX_Random & 0x10 ) // decide to transmit info packet, not status
{ doTx = ReadInfo ( StatPacket . Packet ) ; } // and overwrite the StatPacket with the Info data
if ( doTx )
{
# ifdef WITH_LOG
2020-08-29 17:54:47 +00:00
FlashLog ( & StatPacket , PosTime ) ; // log the status packet
2019-01-22 00:08:19 +00:00
# endif
* StatusPacket = StatPacket ; // copy status packet into the Tx queue
StatusPacket - > Packet . Whiten ( ) ; // whiten for transmission
StatusPacket - > calcFEC ( ) ; // calc. the FEC code
RF_TxFIFO . Write ( ) ; // finalize write into the Tx queue
}
}
2018-01-29 12:43:22 +00:00
while ( RF_TxFIFO . Full ( ) < 2 )
2019-01-22 00:08:19 +00:00
{ OGN_TxPacket < OGN_Packet > * RelayPacket = RF_TxFIFO . getWrite ( ) ;
2018-01-29 12:43:22 +00:00
if ( ! GetRelayPacket ( RelayPacket ) ) break ;
// xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
// Format_String(CONS_UART_Write, "Relayed: ");
// Format_Hex(CONS_UART_Write, RelayPacket->Packet.HeaderWord);
// CONS_UART_Write('\r'); CONS_UART_Write('\n');
// xSemaphoreGive(CONS_Mutex);
# ifdef DEBUG_PRINT
xSemaphoreTake ( CONS_Mutex , portMAX_DELAY ) ;
Format_String ( CONS_UART_Write , " TxFIFO: " ) ;
Format_Hex ( CONS_UART_Write , RelayPacket - > Packet . HeaderWord ) ;
CONS_UART_Write ( ' \r ' ) ; CONS_UART_Write ( ' \n ' ) ;
xSemaphoreGive ( CONS_Mutex ) ;
# endif
RF_TxFIFO . Write ( ) ;
}
CleanRelayQueue ( SlotTime ) ;
}
}