2018-01-29 12:43:22 +00:00
# include <stdint.h>
# include "hal.h"
2018-02-25 18:47:03 +00:00
2018-01-29 12:43:22 +00:00
# include "proc.h"
2018-02-18 14:41:50 +00:00
# include "ctrl.h"
2018-01-29 12:43:22 +00:00
# include "ogn.h"
# include "rf.h"
# include "gps.h"
2018-02-18 14:41:50 +00:00
# ifdef WITH_FLASHLOG
# include "flashlog.h"
# endif
2018-01-29 12:43:22 +00:00
static char Line [ 128 ] ; // for printing out to serial port, etc.
static LDPC_Decoder Decoder ; // error corrector for the OGN Gallager code
// #define DEBUG_PRINT
// ==================================================================
// ---------------------------------------------------------------------------------------------------------------------------------------
static OGN_PrioQueue < 16 > RelayQueue ; // received packets and candidates to be relayed
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 ) ;
xSemaphoreGive ( CONS_Mutex ) ;
}
static bool GetRelayPacket ( OGN_TxPacket * Packet ) // prepare a packet to be relayed
{ if ( RelayQueue . Sum = = 0 ) return 0 ;
XorShift32 ( RX_Random ) ;
uint8_t Idx = RelayQueue . getRand ( RX_Random ) ;
if ( RelayQueue . Packet [ Idx ] . Rank = = 0 ) return 0 ;
memcpy ( Packet - > Packet . Byte ( ) , RelayQueue [ Idx ] - > Byte ( ) , OGN_Packet : : Bytes ) ;
Packet - > Packet . Header . RelayCount + = 1 ;
Packet - > Packet . Whiten ( ) ; Packet - > calcFEC ( ) ;
// PrintRelayQueue(Idx); // for debug
RelayQueue . decrRank ( Idx ) ;
return 1 ; }
static void CleanRelayQueue ( uint32_t Time , uint32_t Delay = 20 )
{ RelayQueue . cleanTime ( ( Time - Delay ) % 60 ) ; }
// ---------------------------------------------------------------------------------------------------------------------------------------
static void ReadStatus ( OGN_TxPacket & StatPacket )
{
2018-02-18 14:41:50 +00:00
# ifdef WITH_STM32
uint16_t MCU_VCC = Measure_MCU_VCC ( ) ; // [0.001V]
StatPacket . Packet . EncodeVoltage ( ( ( MCU_VCC < < 3 ) + 62 ) / 125 ) ; // [1/64V]
int16_t MCU_Temp = Measure_MCU_Temp ( ) ; // [0.1degC]
# endif
if ( StatPacket . Packet . Status . Pressure = = 0 ) StatPacket . Packet . EncodeTemperature ( RF_Temp * 10 ) ; // [0.1degC]
2018-01-29 12:43:22 +00:00
StatPacket . Packet . Status . RadioNoise = RX_AverRSSI ; // [-0.5dBm] write radio noise to the status packet
StatPacket . Packet . Status . TxPower = Parameters . getTxPower ( ) - 4 ;
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 + + ; }
StatPacket . Packet . Status . RxRate = RxRateLog2 ;
{ uint8_t Len = 0 ;
Len + = Format_String ( Line + Len , " $POGNR, " ) ; // NMEA report: radio status
Len + = Format_UnsDec ( Line + Len , RF_FreqPlan . Plan ) ; // which frequency plan
Line [ Len + + ] = ' , ' ;
Len + = Format_UnsDec ( Line + Len , RX_OGN_Count64 ) ; // number of OGN packets received
Line [ Len + + ] = ' , ' ;
Line [ Len + + ] = ' , ' ;
Len + = Format_SignDec ( Line + Len , - 5 * RX_AverRSSI , 2 , 1 ) ; // average RF level (over all channels)
Line [ Len + + ] = ' , ' ;
Len + = Format_UnsDec ( Line + Len , ( uint16_t ) TX_Credit ) ;
Line [ Len + + ] = ' , ' ;
Len + = Format_SignDec ( Line + Len , ( int16_t ) RF_Temp ) ; // the temperature of the RF chip
Line [ Len + + ] = ' , ' ;
// Len+=Format_SignDec(Line+Len, MCU_Temp, 2, 1);
Line [ Len + + ] = ' , ' ;
// Len+=Format_UnsDec(Line+Len, (MCU_VCC+5)/10, 3, 2);
Len + = NMEA_AppendCheckCRNL ( Line , Len ) ; // append NMEA check-sum and CR+NL
// LogLine(Line);
// if(CONS_UART_Free()>=128)
{ xSemaphoreTake ( CONS_Mutex , portMAX_DELAY ) ;
2018-02-18 14:41:50 +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 ) ;
Format_String ( Log_Write , Line , Len ) ; // send the NMEA out to the log file
xSemaphoreGive ( Log_Mutex ) ; }
# endif
}
}
// ---------------------------------------------------------------------------------------------------------------------------------------
static void ProcessRxPacket ( OGN_RxPacket * RxPacket , uint8_t RxPacketIdx ) // process every (correctly) received packet
{ int32_t LatDist = 0 , LonDist = 0 ; uint8_t Warn = 0 ;
2018-02-18 14:41:50 +00:00
if ( RxPacket - > Packet . Header . Other | | 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)
RelayQueue . addNew ( RxPacketIdx ) ;
uint8_t Len = RxPacket - > WritePOGNT ( Line ) ; // print on the console as $POGNT
xSemaphoreTake ( CONS_Mutex , portMAX_DELAY ) ;
Format_String ( CONS_UART_Write , Line , 0 , Len ) ;
xSemaphoreGive ( CONS_Mutex ) ;
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
# ifdef WITH_SDLOG
if ( Log_Free ( ) > = 128 )
{ xSemaphoreTake ( Log_Mutex , portMAX_DELAY ) ;
Format_String ( Log_Write , Line , Len ) ;
xSemaphoreGive ( Log_Mutex ) ; }
# endif
# ifdef WITH_PFLAA
2018-02-18 14:41:50 +00:00
Len = RxPacket - > Packet . WritePFLAA ( Line , Warn , LatDist , LonDist , RxPacket - > Packet . DecodeAltitude ( ) - GPS_Altitude / 10 ) ; // print on the console
xSemaphoreTake ( CONS_Mutex , portMAX_DELAY ) ;
Format_String ( CONS_UART_Write , Line , 0 , Len ) ;
xSemaphoreGive ( CONS_Mutex ) ;
2018-01-29 12:43:22 +00:00
# endif
2018-02-18 14:41:50 +00:00
// #ifdef WITH_MAVLINK
// MAV_ADSB_VEHICLE MAV_RxReport;
// RxPacket->Packet.Encode(&MAV_RxReport);
// 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);
// #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
OGN_RxPacket * RxPacket = RelayQueue [ RxPacketIdx ] ;
// 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 ) ;
Format_String ( CONS_UART_Write , " RxPacket: " ) ;
Format_Hex ( CONS_UART_Write , RxPacket - > Packet . HeaderWord ) ;
CONS_UART_Write ( ' ' ) ;
Format_UnsDec ( CONS_UART_Write , ( uint16_t ) Check ) ;
CONS_UART_Write ( ' / ' ) ;
Format_UnsDec ( CONS_UART_Write , ( uint16_t ) RxPacket - > RxErr ) ;
Format_String ( CONS_UART_Write , " \n " ) ;
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 ( ) ;
ProcessRxPacket ( RxPacket , RxPacketIdx ) ; }
}
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 ( ) ;
2018-02-18 14:41:50 +00:00
OGN_TxPacket PosPacket ; // position packet
uint32_t PosTime = 0 ; // [sec] when the position was recorded
OGN_TxPacket StatPacket ; // status report 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
2018-01-29 12:43:22 +00:00
if ( RxPkt )
{
# 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
2018-01-29 12:43:22 +00:00
RF_RxFIFO . Read ( ) ; }
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
if ( TimeSync_msTime ( ) < 300 ) SlotTime - - ; // lasts up to 0.300sec after the PPS
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
GPS_Position * Position = GPS_getPosition ( BestIdx , BestResid , SlotTime % 60 , 0 ) ;
# endif
# ifdef DEBUG_PRINT
xSemaphoreTake ( CONS_Mutex , portMAX_DELAY ) ;
Format_String ( CONS_UART_Write , " getPos() => " ) ;
Format_UnsDec ( CONS_UART_Write , SlotTime % 60 , 2 ) ;
CONS_UART_Write ( ' ' ) ;
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
// GPS_Position *Position = GPS_getPosition();
2018-01-29 12:43:22 +00:00
if ( Position ) Position - > EncodeStatus ( StatPacket . Packet ) ; // encode GPS altitude and pressure
2018-02-18 14:41:50 +00:00
if ( Position & & Position - > isReady & & ( ! Position - > Sent ) & & Position - > isReady & & Position - > isValid ( ) )
2018-01-29 12:43:22 +00:00
{ int16_t AverSpeed = GPS_AverageSpeed ( ) ; // [0.1m/s] average speed, including the vertical speed
RF_FreqPlan . setPlan ( Position - > Latitude , Position - > Longitude ) ; // set the frequency plan according to the GPS position
/*
# 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
*/
PosTime = Position - > getUnixTime ( ) ;
PosPacket . Packet . HeaderWord = 0 ;
PosPacket . Packet . Header . Address = Parameters . Address ; // set address
PosPacket . Packet . Header . AddrType = Parameters . AddrType ; // address-type
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
else Position - > Encode ( PosPacket . Packet , BestResid ) ;
2018-01-29 12:43:22 +00:00
PosPacket . Packet . Position . Stealth = Parameters . Stealth ;
PosPacket . Packet . Position . AcftType = Parameters . AcftType ; // aircraft-type
OGN_TxPacket * TxPacket = RF_TxFIFO . getWrite ( ) ;
TxPacket - > Packet = PosPacket . Packet ; // copy the position packet to the TxFIFO
TxPacket - > Packet . Whiten ( ) ; TxPacket - > calcFEC ( ) ; // whiten and calculate FEC code
# 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 , " 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 ( ( 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
Position - > Sent = 1 ;
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
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
OGN_TxPacket * TxPacket = RF_TxFIFO . getWrite ( ) ;
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
StatPacket . Packet . HeaderWord = 0 ;
StatPacket . Packet . Header . Address = Parameters . Address ; // set address
StatPacket . Packet . Header . AddrType = Parameters . AddrType ; // address-type
StatPacket . Packet . Header . Other = 1 ;
StatPacket . Packet . calcAddrParity ( ) ; // parity of (part of) the header
StatPacket . Packet . Status . Hardware = HARDWARE_ID ;
StatPacket . Packet . Status . Firmware = SOFTWARE_ID ;
ReadStatus ( StatPacket ) ;
XorShift32 ( RX_Random ) ;
if ( ( ( RX_Random & 0x1F ) = = 0 ) & & ( RF_TxFIFO . Full ( ) < 2 ) )
{ OGN_TxPacket * StatusPacket = RF_TxFIFO . getWrite ( ) ;
* StatusPacket = StatPacket ;
StatusPacket - > Packet . Whiten ( ) ;
StatusPacket - > calcFEC ( ) ;
2018-02-18 14:41:50 +00:00
RF_TxFIFO . Write ( ) ; }
2018-01-29 12:43:22 +00:00
while ( RF_TxFIFO . Full ( ) < 2 )
{ OGN_TxPacket * RelayPacket = RF_TxFIFO . getWrite ( ) ;
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 ) ;
}
}