From 367ba6d4ce7670d0c402f2cc4ac8a436441f5e85 Mon Sep 17 00:00:00 2001 From: Pawel Jalocha Date: Sun, 18 Feb 2018 14:41:50 +0000 Subject: [PATCH] Correct the unique ID, add some string-valued parameters --- main/ctrl.cpp | 37 ++++--- main/format.cpp | 42 ++++++- main/format.h | 7 +- main/gps.cpp | 264 ++++++++++++++++++++++++++++++++++++------- main/gps.h | 11 +- main/hal.cpp | 155 ++++++++++++++++++++++---- main/hal.h | 44 +++++++- main/main.cpp | 31 ++++-- main/ogn.h | 277 ++++++++++++++++++++++++++++++++-------------- main/parameters.h | 242 +++++++++++++++++++++++++++++----------- main/proc.cpp | 166 ++++++++++++++++----------- main/rf.cpp | 2 +- sdkconfig | 4 + 13 files changed, 962 insertions(+), 320 deletions(-) diff --git a/main/ctrl.cpp b/main/ctrl.cpp index f5b0e41..700fa66 100644 --- a/main/ctrl.cpp +++ b/main/ctrl.cpp @@ -39,30 +39,34 @@ void PrintTasks(void (*CONS_UART_Write)(char)) // ======================================================================================================================== #ifdef WITH_OLED -int OLED_DisplayStatus(uint32_t Time, GPS_Position *GPS=0) +int OLED_DisplayStatus(uint32_t Time, uint8_t LineIdx=0) { char Line[20]; Format_String(Line , "OGN Tx/Rx "); Format_HHMMSS(Line+10, Time); - OLED_PutLine(0, Line); + OLED_PutLine(LineIdx++, Line); Parameters.Print(Line); - OLED_PutLine(1, Line); + OLED_PutLine(LineIdx++, Line); + return 0; } + +int OLED_DisplayPosition(GPS_Position *GPS=0, uint8_t LineIdx=2) +{ char Line[20]; if(GPS && GPS->isValid()) { Line[0]=' '; Format_SignDec(Line+1, GPS->Latitude /60, 6, 4); Line[9]=' '; Format_UnsDec (Line+10, GPS->Altitude /10, 5, 0); Line[15]='m'; - OLED_PutLine(2, Line); + OLED_PutLine(LineIdx , Line); Format_SignDec(Line, GPS->Longitude/60, 7, 4); Format_SignDec(Line+10, GPS->ClimbRate, 4, 1); - OLED_PutLine(3, Line); + OLED_PutLine(LineIdx+1, Line); Format_UnsDec (Line , GPS->Speed, 4, 1); Format_String(Line+5, "m/s "); Format_UnsDec (Line+10, GPS->Heading, 4, 1); Line[15]='^'; - OLED_PutLine(4, Line); + OLED_PutLine(LineIdx+2, Line); Format_String(Line, "0D/00sat DOP00.0"); Line[0]+=GPS->FixMode; Format_UnsDec(Line+3, GPS->Satellites, 2); Format_UnsDec(Line+12, (uint16_t)GPS->HDOP, 3, 1); - OLED_PutLine(5, Line); + OLED_PutLine(LineIdx+3, Line); } - else { OLED_PutLine(2, 0); OLED_PutLine(3, 0); OLED_PutLine(4, 0); } + else { OLED_PutLine(LineIdx, 0); OLED_PutLine(LineIdx+1, 0); OLED_PutLine(LineIdx+2, 0); OLED_PutLine(LineIdx+3, 0); } if(GPS && GPS->isDateValid()) { Format_UnsDec (Line , (uint16_t)GPS->Day, 2, 0); Line[2]='.'; Format_UnsDec (Line+ 3, (uint16_t)GPS->Month, 2, 0); Line[5]='.'; @@ -73,13 +77,13 @@ int OLED_DisplayStatus(uint32_t Time, GPS_Position *GPS=0) Format_UnsDec (Line+12, (uint16_t)GPS->Min, 2, 0); Format_UnsDec (Line+14, (uint16_t)GPS->Sec, 2, 0); } else Line[10]=0; - OLED_PutLine(6, Line); + OLED_PutLine(LineIdx+4, Line); Line[0]=0; - if(GPS && GPS->Baro) + if(GPS && GPS->hasBaro) { Format_String(Line , "0000.0hPa 00000m"); Format_UnsDec(Line , GPS->Pressure/4, 5, 1); Format_UnsDec(Line+10, GPS->StdAltitude/10, 5, 0); } - OLED_PutLine(7, Line); + OLED_PutLine(LineIdx+5, Line); return 0; } #endif @@ -102,7 +106,8 @@ static void ReadParameters(void) // read parameters requested by the user in th { PrintParameters(); if(NMEA.Parms==0) // if no parameter given { xSemaphoreTake(CONS_Mutex, portMAX_DELAY); // print a help message - Format_String(CONS_UART_Write, "$POGNS,,,
,,,,,,\n"); + // Format_String(CONS_UART_Write, "$POGNS,,,
,,,,,,\n"); + Format_String(CONS_UART_Write, "$POGNS[,=]\n"); xSemaphoreGive(CONS_Mutex); // return; } Parameters.ReadPOGNS(NMEA); @@ -131,6 +136,9 @@ static void ProcessCtrlC(void) // print system Format_String(CONS_UART_Write, "GPS: "); Format_UnsDec(CONS_UART_Write, GPS_getBaudRate(), 1); Format_String(CONS_UART_Write, "bps"); + CONS_UART_Write(','); + Format_UnsDec(CONS_UART_Write, GPS_PosPeriod, 3, 2); + CONS_UART_Write('s'); if(GPS_Status.PPS) Format_String(CONS_UART_Write, ",PPS"); if(GPS_Status.NMEA) Format_String(CONS_UART_Write, ",NMEA"); if(GPS_Status.UBX) Format_String(CONS_UART_Write, ",UBX"); @@ -174,10 +182,11 @@ void vTaskCTRL(void* pvParameters) GPS_Position *GPS = GPS_getPosition(); bool TimeChange = Time!=PrevTime; bool GPSchange = GPS!=PrevGPS; - if( (Time==PrevTime) && (GPS==PrevGPS) ) continue; + if( (!TimeChange) && (!GPSchange) ) continue; PrevTime=Time; PrevGPS=GPS; #ifdef WITH_OLED - OLED_DisplayStatus(Time, GPS); + if(TimeChange) OLED_DisplayStatus(Time, 0); + if(GPSchange) OLED_DisplayPosition(GPS, 2); #endif #ifdef DEBUG_PRINT if(!TimeChange || (Time%60)!=0) continue; diff --git a/main/format.cpp b/main/format.cpp index 5d83401..8fe8253 100644 --- a/main/format.cpp +++ b/main/format.cpp @@ -26,12 +26,27 @@ uint8_t Format_String(char *Str, const char *String) Str[OutLen++]=ch; } return OutLen; } -uint8_t Format_String(char *Str, const char *String, uint8_t Len) -{ uint8_t OutLen=0; - for(uint8_t Idx=0; Idx=Base) + { Dig=Value/Base; Value-=Dig*Base; } + else + { Dig=0; } + if(Pos==DecPoint) (*Output)('.'); + if( (Pos<=MinDigits) || (Dig>0) || (Pos<=DecPoint) ) + { (*Output)('0'+Dig); MinDigits=Pos; } + } +} + +void Format_SignDec( void (*Output)(char), int64_t Value, uint8_t MinDigits, uint8_t DecPoint) +{ if(Value<0) { (*Output)('-'); Value=(-Value); } + else { (*Output)('+'); } + Format_UnsDec(Output, (uint32_t)Value, MinDigits, DecPoint); } + // ------------------------------------------------------------------------------------------ uint8_t Format_UnsDec(char *Str, uint32_t Value, uint8_t MinDigits, uint8_t DecPoint) diff --git a/main/format.h b/main/format.h index 2d5ea17..461ae65 100644 --- a/main/format.h +++ b/main/format.h @@ -9,7 +9,7 @@ char HexDigit(uint8_t Val); inline void Format_Bytes ( void (*Output)(char), const char *Bytes, uint8_t Len) { Format_Bytes(Output, (const uint8_t *)Bytes, Len); } void Format_String( void (*Output)(char), const char *String); -inline void Format_String( void (*Output)(char), const char *String, uint8_t Len) { Format_Bytes(Output, (const uint8_t *)String, Len); } + void Format_String( void (*Output)(char), const char *String, uint8_t MinLen, uint8_t MaxLen); void Format_Hex( void (*Output)(char), uint8_t Byte ); void Format_Hex( void (*Output)(char), uint16_t Word ); @@ -21,8 +21,11 @@ void Format_SignDec( void (*Output)(char), int16_t Value, uint8_t MinDigits=1, void Format_UnsDec ( void (*Output)(char), uint32_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0); void Format_SignDec( void (*Output)(char), int32_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0); +void Format_UnsDec ( void (*Output)(char), uint64_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0); +void Format_SignDec( void (*Output)(char), int64_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0); + uint8_t Format_String(char *Str, const char *String); -uint8_t Format_String(char *Str, const char *String, uint8_t Len); +uint8_t Format_String(char *Str, const char *String, uint8_t MinLen, uint8_t MaxLen); uint8_t Format_UnsDec (char *Str, uint32_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0); uint8_t Format_SignDec(char *Str, int32_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0); diff --git a/main/gps.cpp b/main/gps.cpp index ff3422a..cc9692d 100644 --- a/main/gps.cpp +++ b/main/gps.cpp @@ -1,12 +1,15 @@ #include #include +#include "hal.h" #include "gps.h" +#include "ctrl.h" #include "nmea.h" #include "ubx.h" #ifdef WITH_MAVLINK #include "mavlink.h" +#include "atmosphere.h" #endif #include "ogn.h" @@ -18,6 +21,10 @@ // #define DEBUG_PRINT +#ifdef DEBUG_PRINT +static char Line[128]; +#endif + // ---------------------------------------------------------------------------- // void Debug_Print(uint8_t Byte) { while(!UART1_TxEmpty()) taskYIELD(); UART1_TxChar(Byte); } @@ -30,18 +37,21 @@ static UBX_RxMsg UBX; // UBX messages catcher static MAV_RxMsg MAV; // MAVlink message catcher #endif -static GPS_Position Position[4]; // GPS position pipe -static uint8_t PosIdx; // Pipe index, increments with every GPS position received +uint16_t GPS_PosPeriod = 0; -static TickType_t Burst_TickCount; // [msec] TickCount when the data burst from GPS started +const uint8_t PosPipeIdxMask = GPS_PosPipeSize-1; +static GPS_Position Position[GPS_PosPipeSize]; // GPS position pipe +static uint8_t PosIdx; // Pipe index, increments with every GPS position received - uint32_t GPS_TimeSinceLock; // [sec] time since the GPS has a lock - uint32_t GPS_FatTime = 0; // [sec] UTC date/time in FAT format - int32_t GPS_Altitude = 0; // [0.1m] last valid altitude - int32_t GPS_Latitude = 0; // - int32_t GPS_Longitude = 0; // - int16_t GPS_GeoidSepar= 0; // [0.1m] - uint16_t GPS_LatCosine = 3000; // +static TickType_t Burst_TickCount; // [msec] TickCount when the data burst from GPS started + + uint32_t GPS_TimeSinceLock; // [sec] time since the GPS has a lock + uint32_t GPS_FatTime = 0; // [sec] UTC date/time in FAT format + int32_t GPS_Altitude = 0; // [0.1m] last valid altitude + int32_t GPS_Latitude = 0; // + int32_t GPS_Longitude = 0; // + int16_t GPS_GeoidSepar= 0; // [0.1m] + uint16_t GPS_LatCosine = 3000; // Status GPS_Status; @@ -76,7 +86,7 @@ int16_t GPS_AverageSpeed(void) // get average speed based int16_t Speed=0; for(uint8_t Idx=0; Idx<4; Idx++) // loop over GPS positions { GPS_Position *Pos = Position+Idx; - if( !Pos->GPS || !Pos->isValid() ) continue; // skip invalid positions + if( !Pos->hasGPS || !Pos->isValid() ) continue; // skip invalid positions Speed += Pos->Speed +abs(Pos->ClimbRate); Count++; } if(Count==0) return -1; @@ -104,14 +114,14 @@ static void GPS_PPS_On(void) // called on rising edge o LED_PCB_Flash(50); // uint8_t Sec=GPS_Sec; Sec++; if(Sec>=60) Sec=0; GPS_Sec=Sec; // GPS_UnixTime++; -#ifdef WITH_MAVLINK - static MAV_SYSTEM_TIME MAV_Time; - MAV_Time.time_unix_usec = (uint64_t)1000000*TimeSync_Time(); - MAV_Time.time_boot_ms = TickCount; - xSemaphoreTake(CONS_Mutex, portMAX_DELAY); - MAV_RxMsg::Send(sizeof(MAV_Time), MAV_Seq++, MAV_SysID, MAV_COMP_ID_GPS, MAV_ID_SYSTEM_TIME, (const uint8_t *)&MAV_Time, CONS_UART_Write); - xSemaphoreGive(CONS_Mutex); -#endif +// #ifdef WITH_MAVLINK +// static MAV_SYSTEM_TIME MAV_Time; +// MAV_Time.time_unix_usec = (uint64_t)1000000*TimeSync_Time(); +// MAV_Time.time_boot_ms = TickCount; +// xSemaphoreTake(CONS_Mutex, portMAX_DELAY); +// MAV_RxMsg::Send(sizeof(MAV_Time), MAV_Seq++, MAV_SysID, MAV_COMP_ID_GPS, MAV_ID_SYSTEM_TIME, (const uint8_t *)&MAV_Time, CONS_UART_Write); +// xSemaphoreGive(CONS_Mutex); +// #endif } static void GPS_PPS_Off(void) // called on falling edge of PPS @@ -209,21 +219,37 @@ static void GPS_BurstStart(void) // wh static void GPS_BurstComplete(void) // when GPS has sent the essential data for position fix { +#ifdef WITH_MAVLINK + GPS_Position *GPS = Position+PosIdx; + if(GPS->hasTime && GPS->hasGPS && GPS->hasBaro) + { int32_t StdAlt1 = Atmosphere::StdAltitude((GPS->Pressure+2)/4); // [0.1m] we try to fix the cheap chinese ArduPilot with baro chip cs5607 instead of cs5611 + int32_t StdAlt2 = Atmosphere::StdAltitude((GPS->Pressure+1)/2); // [0.1m] the cx5607 is very close but gives pressure is twice as larger units + int32_t Alt = GPS->Altitude; // [0.1m] thus it appears to give pressure readout lower by a factor of two. + int32_t Delta1 = StdAlt1-Alt; // [0.1m] Here we check which pressure fits better the GPS altitude + int32_t Delta2 = StdAlt2-Alt; // [0.1m] + if(fabs(Delta1)StdAltitude=StdAlt1; } // + else { GPS->Pressure*=2; GPS->StdAltitude=StdAlt2; } + } +#endif #ifdef DEBUG_PRINT + Position[PosIdx].PrintLine(Line); xSemaphoreTake(CONS_Mutex, portMAX_DELAY); - Format_UnsDec(CONS_UART_Write, TimeSync_Time()%60); + 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, " -> ...complete\n"); + Format_String(CONS_UART_Write, Line); xSemaphoreGive(CONS_Mutex); #endif - if(Position[PosIdx].GPS) // GPS position data complete - { Position[PosIdx].Ready=1; + if(Position[PosIdx].hasGPS) // GPS position data complete + { Position[PosIdx].isReady=1; // mark this record as ready for processing => producing packets for transmission if(Position[PosIdx].isTimeValid()) // if time is valid already { if(Position[PosIdx].isDateValid()) // if date is valid as well { uint32_t UnixTime=Position[PosIdx].getUnixTime(); GPS_FatTime=Position[PosIdx].getFatTime(); - TimeSync_SoftPPS(Burst_TickCount, UnixTime, GPS_BurstDelay); // +#ifndef WITH_MAVLINK // with MAVlink we sync. with the SYSTEM_TIME message + TimeSync_SoftPPS(Burst_TickCount, UnixTime, GPS_BurstDelay); +#endif } } if(Position[PosIdx].isValid()) // position is complete and locked @@ -235,28 +261,65 @@ static void GPS_BurstComplete(void) // wh GPS_GeoidSepar=Position[PosIdx].GeoidSeparation; GPS_LatCosine=Position[PosIdx].LatitudeCosine; // GPS_FreqPlan=Position[PosIdx].getFreqPlan(); - if(GPS_TimeSinceLock==1) + if(GPS_TimeSinceLock==1) // if we just acquired the lock a moment ago { GPS_LockStart(); } - if(GPS_TimeSinceLock>1) - { uint8_t PrevIdx=(PosIdx+3)&3; - Position[PosIdx].calcDifferences(Position[PrevIdx]); + if(GPS_TimeSinceLock>1) // if the lock is more persistant + { uint8_t PrevIdx=(PosIdx+PosPipeIdxMask)&PosPipeIdxMask; + int16_t TimeDiff = Position[PosIdx].calcTimeDiff(Position[PrevIdx]); + for( ; ; ) + { if(TimeDiff>=95) break; + uint8_t PrevIdx2=(PrevIdx+PosPipeIdxMask)&PosPipeIdxMask; + if(PrevIdx2==PosIdx) break; + if(!Position[PrevIdx2].isValid()) break; + TimeDiff = Position[PosIdx].calcTimeDiff(Position[PrevIdx2]); + PrevIdx=PrevIdx2; } + TimeDiff=Position[PosIdx].calcDifferences(Position[PrevIdx]); +#ifdef DEBUG_PRINT + xSemaphoreTake(CONS_Mutex, portMAX_DELAY); + Format_String(CONS_UART_Write, "calcDiff() => "); + Format_UnsDec(CONS_UART_Write, (uint16_t)PosIdx); + Format_String(CONS_UART_Write, "->"); + Format_UnsDec(CONS_UART_Write, (uint16_t)PrevIdx); + CONS_UART_Write(' '); + Format_SignDec(CONS_UART_Write, TimeDiff, 3, 2); + Format_String(CONS_UART_Write, "s\n"); + xSemaphoreGive(CONS_Mutex); +#endif LED_PCB_Flash(100); } } else // complete but no valid lock { if(GPS_TimeSinceLock) { GPS_LockEnd(); GPS_TimeSinceLock=0; } } -#ifdef WITH_MAVLINK - static MAV_GPS_RAW_INT MAV_Position; - Position[PosIdx].Encode(MAV_Position); - xSemaphoreTake(CONS_Mutex, portMAX_DELAY); - MAV_RxMsg::Send(sizeof(MAV_Position), MAV_Seq++, MAV_SysID, MAV_COMP_ID_GPS, MAV_ID_GPS_RAW_INT, (const uint8_t *)&MAV_Position, CONS_UART_Write); - xSemaphoreGive(CONS_Mutex); -#endif +// #ifdef WITH_MAVLINK +// static MAV_GPS_RAW_INT MAV_Position; +// Position[PosIdx].Encode(MAV_Position); +// xSemaphoreTake(CONS_Mutex, portMAX_DELAY); +// MAV_RxMsg::Send(sizeof(MAV_Position), MAV_Seq++, MAV_SysID, MAV_COMP_ID_GPS, MAV_ID_GPS_RAW_INT, (const uint8_t *)&MAV_Position, CONS_UART_Write); +// xSemaphoreGive(CONS_Mutex); +// #endif } else // posiiton not complete, no GPS lock { if(GPS_TimeSinceLock) { GPS_LockEnd(); GPS_TimeSinceLock=0; } } - uint8_t NextPosIdx = (PosIdx+1)&3; // next position to be recorded + uint8_t NextPosIdx = (PosIdx+1)&PosPipeIdxMask; // next position to be recorded + if( Position[PosIdx].isTimeValid() && Position[NextPosIdx].isTimeValid() ) + { int16_t Period = Position[PosIdx].calcTimeDiff(Position[NextPosIdx]); + if(Period>0) GPS_PosPeriod = (Period+GPS_PosPipeSize/2)/(GPS_PosPipeSize-1); +#ifdef DEBUG_PRINT + xSemaphoreTake(CONS_Mutex, portMAX_DELAY); + Format_String(CONS_UART_Write,"GPS: "); + Format_UnsDec(CONS_UART_Write, (uint16_t)PosIdx); + CONS_UART_Write(' '); + Format_UnsDec(CONS_UART_Write, (uint16_t)Position[PosIdx].Sec, 2); + CONS_UART_Write('.'); + Format_UnsDec(CONS_UART_Write, (uint16_t)Position[PosIdx].FracSec, 2); + Format_String(CONS_UART_Write, "s "); + Format_SignDec(CONS_UART_Write, Period, 3, 2); + Format_String(CONS_UART_Write, "s\n"); + xSemaphoreGive(CONS_Mutex); +#endif + + } Position[NextPosIdx].Clear(); // clear the next position int8_t Sec = Position[PosIdx].Sec; // Sec++; if(Sec>=60) Sec=0; @@ -269,17 +332,32 @@ static void GPS_BurstComplete(void) // wh static void GPS_BurstEnd(void) // when GPS stops sending the data on the serial port { } -GPS_Position *GPS_getPosition(void) // return most recent GPS_Position which is complete +// ---------------------------------------------------------------------------- + +GPS_Position *GPS_getPosition(uint8_t &BestIdx, int16_t &BestRes, int8_t Sec, int8_t Frac) // return GPS position closest to the given Sec.Frac +{ int16_t TargetTime = Frac+(int16_t)Sec*100; + BestIdx=0; BestRes=0x7FFF; + for(uint8_t Idx=0; IdxisReady) continue; + int16_t Diff = TargetTime - (Pos->FracSec + (int16_t)Pos->Sec*100); + if(Diff<(-3000)) Diff+=6000; + else if(Diff>3000) Diff-=6000; + if(fabs(Diff)GPS) return PrevPos; - PrevIdx=(PrevIdx+3)&3; + if(PrevPos->isReady) return PrevPos; + PrevIdx=(PrevIdx+PosPipeIdxMask)&PosPipeIdxMask; PrevPos = Position+PrevIdx; - if(PrevPos->GPS) return PrevPos; + if(PrevPos->isReady) return PrevPos; return 0; } GPS_Position *GPS_getPosition(int8_t Sec) // return the GPS_Position which corresponds to given Sec (may be incomplete and not valid) -{ for(uint8_t Idx=0; Idx<4; Idx++) +{ for(uint8_t Idx=0; Idxsystem_status); + Format_String(CONS_UART_Write, "\n"); + xSemaphoreGive(CONS_Mutex); +#endif + } else if(MsgID==MAV_ID_SYSTEM_TIME) + { const MAV_SYSTEM_TIME *SysTime = (const MAV_SYSTEM_TIME *)MAV.getPayload(); + uint64_t UnixTime_ms = SysTime->time_unix_usec/1000 + TimeCorr_ms; // [ms] Unix Time + uint32_t UnixTime = UnixTime_ms/1000; // [ s] Unix Time + uint32_t UnixFrac = UnixTime_ms-(uint64_t)UnixTime*1000; // [ms] Second fraction of the Unix time + MAV_TimeOfs_ms=UnixTime_ms-SysTime->time_boot_ms; // [ms] difference between the Unix Time and the Ardupilot time-since-boot + TimeSync_SoftPPS(TickCount-UnixFrac, UnixTime, 70); +#ifdef DEBUG_PRINT + xSemaphoreTake(CONS_Mutex, portMAX_DELAY); + Format_String(CONS_UART_Write, "MAV_SYSTEM_TIME: "); + Format_UnsDec(CONS_UART_Write, UnixTime, 10); + CONS_UART_Write('.'); + Format_UnsDec(CONS_UART_Write, UnixFrac, 3); + CONS_UART_Write(' '); + Format_SignDec(CONS_UART_Write, MAV_TimeOfs_ms, 13, 3); + Format_String(CONS_UART_Write, "\n"); + xSemaphoreGive(CONS_Mutex); +#endif + } else if(MsgID==MAV_ID_GLOBAL_POSITION_INT) + { const MAV_GLOBAL_POSITION_INT *Pos = (const MAV_GLOBAL_POSITION_INT *)MAV.getPayload(); + uint64_t UnixTime_ms = Pos->time_boot_ms + MAV_TimeOfs_ms; + Position[PosIdx].Read(Pos, UnixTime_ms); +#ifdef DEBUG_PRINT + Position[PosIdx].PrintLine(Line); + xSemaphoreTake(CONS_Mutex, portMAX_DELAY); + Format_String(CONS_UART_Write, "MAV_GLOBAL_POSITION_INT: "); + Format_UnsDec(CONS_UART_Write, UnixTime_ms, 13, 3); + Format_String(CONS_UART_Write, "\n"); + Format_String(CONS_UART_Write, Line); + xSemaphoreGive(CONS_Mutex); +#endif + } else if(MsgID==MAV_ID_GPS_RAW_INT) + { MAV_GPS_RAW_INT *RawGPS = (MAV_GPS_RAW_INT *)MAV.getPayload(); + uint64_t UnixTime_ms = RawGPS->time_usec/1000 + MAV_TimeOfs_ms; + // RawGPS->time_usec += (int64_t)MAV_TimeOfs_ms*1000; + Position[PosIdx].Read(RawGPS, UnixTime_ms); +#ifdef DEBUG_PRINT + Position[PosIdx].PrintLine(Line); + uint32_t UnixTime = (UnixTime_ms+500)/1000; + int32_t TimeDiff = (int64_t)UnixTime_ms-(int64_t)UnixTime*1000; + xSemaphoreTake(CONS_Mutex, portMAX_DELAY); + Format_String(CONS_UART_Write, "MAV_GPS_RAW_INT: "); + Format_UnsDec(CONS_UART_Write, UnixTime_ms, 13, 3); + CONS_UART_Write(' '); + Format_SignDec(CONS_UART_Write, TimeDiff, 4, 3); + CONS_UART_Write(abs(TimeDiff)<250 ? '*':' '); + Format_String(CONS_UART_Write, "\n"); + Format_String(CONS_UART_Write, Line); + xSemaphoreGive(CONS_Mutex); +#endif + } else if(MsgID==MAV_ID_SCALED_PRESSURE) + { const MAV_SCALED_PRESSURE *Press = (const MAV_SCALED_PRESSURE *)MAV.getPayload(); + uint64_t UnixTime_ms = Press->time_boot_ms + MAV_TimeOfs_ms; + Position[PosIdx].Read(Press, UnixTime_ms); +#ifdef DEBUG_PRINT + Position[PosIdx].PrintLine(Line); + xSemaphoreTake(CONS_Mutex, portMAX_DELAY); + Format_String(CONS_UART_Write, "MAV_SCALED_PRESSURE: "); + Format_UnsDec(CONS_UART_Write, UnixTime_ms, 13, 3); + Format_String(CONS_UART_Write, "\n"); + Format_String(CONS_UART_Write, Line); + xSemaphoreGive(CONS_Mutex); +#endif + } else if(MsgID==MAV_ID_SYS_STATUS) + { const MAV_SYS_STATUS *Status = (const MAV_SYS_STATUS *)MAV.getPayload(); +#ifdef DEBUG_PRINT + xSemaphoreTake(CONS_Mutex, portMAX_DELAY); + Format_String(CONS_UART_Write, "MAV_SYS_STATUS: "); + Format_UnsDec(CONS_UART_Write, Status->battery_voltage, 4, 3); + Format_String(CONS_UART_Write, "V "); + Format_SignDec(CONS_UART_Write, Status->battery_current, 3, 2); + Format_String(CONS_UART_Write, "A\n"); + xSemaphoreGive(CONS_Mutex); +#endif + // } else if(MsgID==MAV_ID_STATUSTEXT) + // { + } +#ifdef DEBUG_PRINT + else + { xSemaphoreTake(CONS_Mutex, portMAX_DELAY); + Format_String(CONS_UART_Write, "MAV: MsgID="); + Format_UnsDec(CONS_UART_Write, (uint16_t)MAV.getMsgID(), 3); + Format_String(CONS_UART_Write, "\n"); + xSemaphoreGive(CONS_Mutex); + } +#endif } #endif @@ -552,7 +730,7 @@ void vTaskGPS(void* pvParameters) GPS_Burst.Flags=0; } - if(NoValidData>=1200) // if no valid data from GPS for 1sec + if(NoValidData>=2000) // if no valid data from GPS for 1sec { GPS_Status.Flags=0; GPS_Burst.Flags=0; // assume GPS state is unknown uint32_t NewBaudRate = GPS_nextBaudRate(); // switch to the next baud rate xSemaphoreTake(CONS_Mutex, portMAX_DELAY); diff --git a/main/gps.h b/main/gps.h index cb83320..5a90cd2 100644 --- a/main/gps.h +++ b/main/gps.h @@ -1,6 +1,4 @@ -#ifdef __cplusplus - #include "hal.h" #include "timesync.h" @@ -9,6 +7,8 @@ #include "lowpass2.h" +const uint8_t GPS_PosPipeSize = 4; // number of GPS positions held in a pipe + extern uint32_t GPS_FatTime; // [2 sec] UTC time in FAT format (for FatFS) extern int32_t GPS_Altitude; // [0.1m] altitude (height above Geoid) extern int32_t GPS_Latitude; // [0.0001/60 deg] @@ -16,6 +16,7 @@ extern int32_t GPS_Longitude; // [0.0001/60 deg] extern int16_t GPS_GeoidSepar; // [0.1m] extern uint16_t GPS_LatCosine; // [1.0/(1<<12)] Latitude Cosine for distance calculations extern uint32_t GPS_TimeSinceLock; // [sec] time since GPS has a valid lock +extern uint16_t GPS_PosPeriod; // [0.01sec] how often (which period) the GPS/MAV is sending the positions typedef union { uint8_t Flags; @@ -36,13 +37,11 @@ extern Status GPS_Status; uint32_t GPS_getBaudRate(void); // [bps] GPS_Position *GPS_getPosition(void); -GPS_Position *GPS_getPosition(int8_t Sec); +GPS_Position *GPS_getPosition(int8_t Sec); // return GPS position for given Sec +GPS_Position *GPS_getPosition(uint8_t &BestIdx, int16_t &BestRes, int8_t Sec, int8_t Frac); // return GPS position closest to the given Sec.Frac int16_t GPS_AverageSpeed(void); // [0.1m/s] calc. average speed based on most recent GPS positions -#endif // __cplusplus - - #ifdef __cplusplus extern "C" #endif diff --git a/main/hal.cpp b/main/hal.cpp index d6cd342..13561fb 100644 --- a/main/hal.cpp +++ b/main/hal.cpp @@ -1,4 +1,6 @@ +#include #include +#include // #include #include "freertos/FreeRTOS.h" @@ -34,7 +36,7 @@ // ====================================================================================================== /* -The HELTEC Automation board WiFi LoRa 32 with sx1276 (RFM95) for 868/915MHz +The HELTEC AUtomation board WiFi LoRa 32 with sx1278 (RFM95) Referenced: http://esp32.net/ Pinout: http://esp32.net/images/Heltec/WIFI-LoRa-32/Heltec_WIFI-LoRa-32_DiagramPinoutFromTop.jpg @@ -49,7 +51,7 @@ OLED datasheet: https://cdn-shop.adafruit.com/datasheets/SSD1306.pdf OLED example: https://github.com/yanbe/ssd1306-esp-idf-i2c OLED article: http://robotcantalk.blogspot.co.uk/2015/03/interfacing-arduino-with-ssd1306-driven.html -SX1276 pins: +SX1278 pins: 14 = GPIO14 = RST 5 = GPIO5 = SCK 18 = GPIO18 = CS = SS @@ -85,6 +87,8 @@ UART2 pins: */ #define PIN_LED_PCB GPIO_NUM_25 // status LED on the PCB +// #define PIN_LED_TX GPIO_NUM_?? +// #define PIN_LED_RX GPIO_NUM_?? #define PIN_RFM_RST GPIO_NUM_14 // Reset #define PIN_RFM_IRQ GPIO_NUM_26 // packet done on receive or transmit @@ -93,11 +97,13 @@ UART2 pins: #define PIN_RFM_MISO GPIO_NUM_19 // SPI MISO #define PIN_RFM_MOSI GPIO_NUM_27 // SPI MOSI - // wire colours for VK2828U GPS -#define PIN_GPS_TXD GPIO_NUM_12 // green -#define PIN_GPS_RXD GPIO_NUM_35 // blue + // VK2828U GPS MAVlink port +#define PIN_GPS_TXD GPIO_NUM_12 // green green +#define PIN_GPS_RXD GPIO_NUM_35 // blue yellow #define PIN_GPS_PPS GPIO_NUM_34 // white -#define PIN_GPS_ENA GPIO_NUM_13 // yellow -> well, I had a problem here, thus I tied the enable wire to 3.3V for the time being. +#define PIN_GPS_ENA GPIO_NUM_13 // yellow + +// Note: I had a problem GPS ENABLE on GPIO13, thus I tied the enable wire to 3.3V for the time being. #define CONS_UART UART_NUM_0 // UART0 for the console (the system does this for us) #define GPS_UART UART_NUM_1 // UART1 for GPS data read and dialog @@ -116,6 +122,25 @@ UART2 pins: uint64_t getUniqueID(void) { uint64_t ID=0; esp_err_t ret=esp_efuse_mac_get_default((uint8_t *)&ID); return ID; } +uint32_t getUniqueAddress(void) +{ uint32_t ID = getUniqueID()>>24; + ID &= 0x00FFFFFF; + ID = (ID>>16) | (ID&0x00FF00) | (ID<<16); + ID &= 0x00FFFFFF; + return ID; } + +// ====================================================================================================== + +#ifdef WITH_MAVLINK +uint8_t MAV_Seq=0; // sequence number for MAVlink message sent out +#endif + +// ====================================================================================================== + +// system_get_time() - return s 32-bit time in microseconds since the system start +// gettimeofday() +// xthal_get_ccount() - gets Xtal or master clock counts ? + // ====================================================================================================== FlashParameters Parameters; @@ -123,6 +148,7 @@ FlashParameters Parameters; //-------------------------------------------------------------------------------------------------------- // STatus LED +void LED_PCB_Dir (void) { gpio_set_direction(PIN_LED_PCB, GPIO_MODE_OUTPUT); } void LED_PCB_On (void) { gpio_set_level(PIN_LED_PCB, 1); } // LED is on GPIO25 void LED_PCB_Off (void) { gpio_set_level(PIN_LED_PCB, 0); } @@ -131,6 +157,15 @@ void LED_PCB_Off (void) { gpio_set_level(PIN_LED_PCB, 0); } SemaphoreHandle_t CONS_Mutex; +/* +bool CONS_InpReady(void) +{ struct timeval tv = { tv_sec:0, tv_usec:0} ; + fd_set fds; + FD_ZERO(&fds); + FD_SET(STDIN_FILENO, &fds); + select(STDIN_FILENO+1, &fds, NULL, NULL, &tv); + return (FD_ISSET(0, &fds)); } +*/ // int CONS_UART_Read (uint8_t &Byte) { return uart_read_bytes (CONS_UART, &Byte, 1, 0); } // non-blocking // void CONS_UART_Write (char Byte) { uart_write_bytes (CONS_UART, &Byte, 1); } // blocking ? void CONS_UART_Write (char Byte) { putchar(Byte); } @@ -155,14 +190,24 @@ bool GPS_PPS_isOn(void) { return gpio_get_level(PIN_GPS_PPS); } //-------------------------------------------------------------------------------------------------------- // RF chip +inline void RFM_RESET_Dir (void) { gpio_set_direction(PIN_RFM_RST, GPIO_MODE_OUTPUT); } inline void RFM_RESET_High(void) { gpio_set_level(PIN_RFM_RST, 1); } inline void RFM_RESET_Low (void) { gpio_set_level(PIN_RFM_RST, 0); } +#ifdef WITH_RFM95 void RFM_RESET(uint8_t On) { if(On) RFM_RESET_Low(); else RFM_RESET_High(); } +#endif -bool RFM_DIO0_isOn(void) { return gpio_get_level(PIN_RFM_IRQ); } +#ifdef WITH_RFM69 +void RFM_RESET(uint8_t On) +{ if(On) RFM_RESET_High(); + else RFM_RESET_Low(); } +#endif + +inline void RFM_IRQ_Dir (void) { gpio_set_direction(PIN_RFM_IRQ, GPIO_MODE_INPUT); } + bool RFM_IRQ_isOn(void) { return gpio_get_level(PIN_RFM_IRQ); } static spi_device_handle_t RFM_SPI; @@ -248,9 +293,18 @@ esp_err_t OLED_Clear(void) //-------------------------------------------------------------------------------------------------------- volatile uint8_t LED_PCB_Counter = 0; - void LED_PCB_Flash(uint8_t Time) { if(Time>LED_PCB_Counter) LED_PCB_Counter=Time; } // [ms] +#ifdef WITH_LED_TX +volatile uint8_t LED_TX_Counter = 0; +void LED_TX_Flash(uint8_t Time) { if(Time>LED_TX_Counter) LED_TX_Counter=Time; } // [ms] +#endif + +#ifdef WITH_LED_RX +volatile uint8_t LED_RX_Counter = 0; +void LED_RX_Flash(uint8_t Time) { if(Time>LED_RX_Counter) LED_RX_Counter=Time; } // [ms] +#endif + void LED_TimerCheck(uint8_t Ticks) { uint8_t Counter=LED_PCB_Counter; if(Counter) @@ -259,6 +313,24 @@ void LED_TimerCheck(uint8_t Ticks) if(Counter) LED_PCB_On(); else LED_PCB_Off(); LED_PCB_Counter=Counter; } +#ifdef WITH_LED_TX + Counter=LED_TX_Counter; + if(Counter) + { if(Ticksopen.handle, Param->open.rem_bda + case ESP_SPP_SRV_CLOSE_EVT: // connection closes for given handle + // Param->close.handle, Param->close.rem_bda + break; + case ESP_SPP_DATA_IND_EVT: // data is sent by the client + // Param->data_ind.handle, Param->data_ind.data, Param->data_ind.len + break; + case ESP_SPP_WRITE_EVT: // data has been sent to the cielnt + break; +*/ + default: + break; + } + xSemaphoreTake(CONS_Mutex, portMAX_DELAY); Format_String(CONS_UART_Write, "BT_SPP: "); - Format_Hex(CONS_UART_Write, (uint32_t)event); + Format_Hex(CONS_UART_Write, (uint32_t)Event); + CONS_UART_Write(' '); Format_String(CONS_UART_Write, "\n"); - xSemaphoreGive(CONS_Mutex); } + xSemaphoreGive(CONS_Mutex); +} int BT_SPP_Init(void) { esp_bt_controller_config_t BTconf = BT_CONTROLLER_INIT_CONFIG_DEFAULT(); - esp_err_t Err = esp_bt_controller_init(&BTconf); if(Err!=ESP_OK) return Err; + esp_err_t Err; + Err = esp_bt_controller_init(&BTconf); if(Err!=ESP_OK) return Err; Err = esp_bt_controller_enable(ESP_BT_MODE_CLASSIC_BT); if(Err!=ESP_OK) return Err; Err = esp_bluedroid_init(); if(Err!=ESP_OK) return Err; Err = esp_bluedroid_enable(); if(Err!=ESP_OK) return Err; @@ -379,6 +488,8 @@ int BT_SPP_Init(void) Err = esp_spp_init(esp_spp_mode); if(Err!=ESP_OK) return Err; return Err; } +#endif // WITH_BT_SPP + // ====================================================================================================== int NVS_Init(void) @@ -387,9 +498,9 @@ int NVS_Init(void) { nvs_flash_erase(); Err = nvs_flash_init(); } - if(Parameters.ReadFromNVS()!=ESP_OK) - { Parameters.setDefault(getUniqueID()); - Parameters.WriteToNVS(); } + // if(Parameters.ReadFromNVS()!=ESP_OK) + // { Parameters.setDefault(getUniqueID()); + // Parameters.WriteToNVS(); } return Err; } diff --git a/main/hal.h b/main/hal.h index f972c87..6b160ca 100644 --- a/main/hal.h +++ b/main/hal.h @@ -8,6 +8,10 @@ #include "freertos/task.h" #include "freertos/queue.h" +// ============================================================================================================ + +#define WITH_ESP32 + #define HARDWARE_ID 0x02 #define SOFTWARE_ID 0x01 @@ -15,6 +19,8 @@ #define WITH_RFM95 // RF chip selection // #define WITH_RFM69 +// #define WITH_LED_RX +// #define WITH_LED_TX // #define WITH_GPS_ENABLE // use GPS_ENABLE control line to turn the GPS ON/OFF #define WITH_GPS_PPS // use the PPS signal from GPS for precise time-sync. @@ -22,18 +28,31 @@ #define WITH_GPS_UBX // GPS understands UBX // #define WITH_GPS_MTK // GPS understands MTK // #define WITH_GPS_SRF +// #define WITH_MAVLINK -#define WITH_PFLAA +#define WITH_PFLAA // PFLAU and PFLAA for compatibility with XCsoar and LK8000 #define WITH_CONFIG // interpret the console input: $POGNS to change parameters #define WITH_OLED // OLED display on the I2C +// #define WITH_BT_SPP // Bluetooth serial port fo smartphone/tablet link + +// ============================================================================================================ + +#ifdef WITH_MAVLINK +const uint8_t MAV_SysID = 1; // System-ID for MAVlink messages we send out +extern uint8_t MAV_Seq; // sequence number for MAVlink message sent out +#endif + +// ============================================================================================================ + extern SemaphoreHandle_t CONS_Mutex; // console port Mutex +uint64_t getUniqueID(void); // get some unique ID of the CPU/chip +uint32_t getUniqueAddress(void); // get unique 24-bit address for the transmitted IF + #include "parameters.h" extern FlashParameters Parameters; -uint64_t getUniqueID(void); // get some unique ID of the CPU/chip - int CONS_UART_Read (uint8_t &Byte); // non-blocking void CONS_UART_Write (char Byte); // blocking int CONS_UART_Free (void); // how many bytes can be written to the transmit buffer @@ -48,7 +67,7 @@ bool GPS_PPS_isOn(void); void RFM_TransferBlock(uint8_t *Data, uint8_t Len); void RFM_RESET(uint8_t On); // RF module reset -bool RFM_DIO0_isOn(void); // query the IRQ state +bool RFM_IRQ_isOn(void); // query the IRQ state #ifdef WITH_OLED int OLED_SetContrast(uint8_t Contrast); @@ -57,15 +76,30 @@ int OLED_PutLine(uint8_t Line, const char *Text); void LED_PCB_On (void); // LED on the PCB for vizual indications void LED_PCB_Off (void); - void LED_PCB_Flash(uint8_t Time=100); // Flash the PCB LED for a period of [ms] + +#ifdef WITH_LED_TX +void LED_TX_On (void); +void LED_TX_Off (void); +void LED_TX_Flash (uint8_t Time=100); +#endif + +#ifdef WITH_LED_RX +void LED_TX_On (void); +void LED_TX_Off (void); +void LED_RX_Flash(uint8_t Time=100); +#endif + void LED_TimerCheck(uint8_t Ticks=1); void IO_Configuration(void); // Configure I/O int NVS_Init(void); // initialize non-volatile-storage in the Flash +#ifdef WITH_BT_SPP int BT_SPP_Init(void); +#endif + int SPIFFS_Register(const char *Path="/spiffs", const char *Label=0, size_t MaxOpenFiles=5); int SPIFFS_Info(size_t &Total, size_t &Used, const char *Label=0); diff --git a/main/main.cpp b/main/main.cpp index 43724d3..f73737e 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -1,28 +1,35 @@ // ====================================================================================================== -#include "freertos/FreeRTOS.h" +#include "freertos/FreeRTOS.h" // FreeeRTOS #include "freertos/task.h" -// #include "esp_system.h" -// #include "nvs_flash.h" -#include "hal.h" +#include "hal.h" // Hardware Abstraction Layer -#include "rf.h" -#include "proc.h" -#include "gps.h" -#include "ctrl.h" +#include "rf.h" // RF control/transmission/reception task +#include "proc.h" // GPS/RF process taskk +#include "gps.h" // GPS control data acquisiton +#include "ctrl.h" // Control task extern "C" void app_main(void) { // printf("OGN Tracker on ESP32\n"); - NVS_Init(); // initialize Non-Volatile-Storage in Flash and read the tracker parameters - SPIFFS_Register(); // initialize the file system in the Flash + CONS_Mutex = xSemaphoreCreateMutex(); // semaphore used for writing to the console - // BT_SPP_Init(); + NVS_Init(); // initialize Non-Volatile-Storage in Flash and read the tracker parameters - IO_Configuration(); // initialize the GPIO/UART/I2C/SPI for Radio, GPS, OLED, Baro + Parameters.setDefault(getUniqueAddress()); // set default parameter values + if(Parameters.ReadFromNVS()!=ESP_OK) // try to get parameters from NVS + { Parameters.WriteToNVS(); } // if did not work: try to save (default) parameters to NVS + + SPIFFS_Register(); // initialize the file system in the Flash + +#ifdef WITH_BT_SPP + BT_SPP_Init(); // start BT SPP +#endif + + IO_Configuration(); // initialize the GPIO/UART/I2C/SPI for Radio, GPS, OLED, Baro xTaskCreate(vTaskRF, "RF", 2048, 0, tskIDLE_PRIORITY+4, 0); xTaskCreate(vTaskPROC, "PROC", 2048, 0, tskIDLE_PRIORITY+3, 0); diff --git a/main/ogn.h b/main/ogn.h index 4d29b96..5c24b46 100644 --- a/main/ogn.h +++ b/main/ogn.h @@ -234,20 +234,19 @@ class OGN_Packet // Packet structure for the OGN tracker printf("\n"); } - void Encode(MAV_ADSB_VEHICLE &MAV) - { MAV.ICAO_address = HeaderWord&0x03FFFFFF; - MAV.lat = ((int64_t)50*DecodeLatitude()+1)/3; - MAV.lon = ((int64_t)50*DecodeLongitude()+1)/3; - MAV.altitude = 1000*DecodeAltitude(); - MAV.heading = 10*DecodeHeading(); - MAV.hor_velocity = 10*DecodeSpeed(); - MAV.ver_velocity = 10*DecodeClimbRate(); - MAV.flags = 0x17; - MAV.altitude_type = 1; - MAV.callsign[0] = 0; - MAV.tslc = 0; - MAV.emiter_type = 0; - } + void Encode(MAV_ADSB_VEHICLE *MAV) + { MAV->ICAO_address = HeaderWord&0x03FFFFFF; + MAV->lat = ((int64_t)50*DecodeLatitude()+1)/3; + MAV->lon = ((int64_t)50*DecodeLongitude()+1)/3; + MAV->altitude = 1000*DecodeAltitude(); + MAV->heading = 10*DecodeHeading(); + MAV->hor_velocity = 10*DecodeSpeed(); + MAV->ver_velocity = 10*DecodeClimbRate(); + MAV->flags = 0x17; + MAV->altitude_type = 1; + MAV->callsign[0] = 0; + MAV->tslc = 0; + MAV->emiter_type = 0; } int8_t ReadAPRS(const char *Msg) // read an APRS position message { Clear(); @@ -1259,10 +1258,14 @@ class GPS_Position union { uint8_t Flags; // bit #0 = GGA and RMC had same Time struct - { bool GPS :1; // all required GPS information has been supplied (but this is not the GPS lock status) - bool Baro :1; // barometric information has beed supplied - bool Ready :1; // is ready for the following treaement + { bool hasGPS :1; // all required GPS information has been supplied (but this is not the GPS lock status) + bool hasBaro :1; // barometric information has beed supplied + bool isReady :1; // is ready for the following treaement bool Sent :1; // has been transmitted + bool hasTime :1; // Time has been supplied + bool hasRMC :1; // GxRMC has been supplied + bool hasGGA :1; // GxGGA has been supplied + bool hasGSA :1; // GxGSA has been supplied } ; } ; @@ -1306,18 +1309,18 @@ class GPS_Position Latitude=0; Longitude=0; LatitudeCosine=3000; Altitude=0; GeoidSeparation=0; Speed=0; Heading=0; ClimbRate=0; TurnRate=0; - StdAltitude=0; Temperature=0; } + Temperature=0; Pressure=0; StdAltitude=0; } - void setDefaultDate() { Year=00; Month=1; Day=1; } - void setDefaultTime() { Hour=0; Min=0; Sec=0; FracSec=0; } + void setDefaultDate() { Year=00; Month=1; Day=1; } // default Date is 01-JAN-2000 + void setDefaultTime() { Hour=0; Min=0; Sec=0; FracSec=0; } // default Time is 00:00:00.00 - bool isTimeValid(void) const // is the GPS time-of-day valid ? + bool isTimeValid(void) const // is the GPS time-of-day valid { return (Hour>=0) && (Min>=0) && (Sec>=0); } // all data must have been correctly read: negative means not correctly read) bool isDateValid(void) const // is the GPS date valid ? { return (Year>=0) && (Month>=0) && (Day>=0); } - bool isValid(void) const // is GPS lock there ? + bool isValid(void) const // is GPS data is valid = GPS lock { if(!isTimeValid()) return 0; // is GPS time valid/present ? if(!isDateValid()) return 0; // is GPS date valid/present ? if(FixQuality==0) return 0; // Fix quality must be 1=GPS or 2=DGPS @@ -1393,8 +1396,22 @@ class GPS_Position printf("\n"); } int PrintLine(char *Out) const - { int Len=PrintDateTime(Out); - Len+=sprintf(Out+Len, " %d/%d/%02d", FixQuality, FixMode, Satellites); + { int Len=0; // PrintDateTime(Out); + Out[Len++]=hasTime?'T':'_'; + Out[Len++]=hasGPS ?'G':'_'; + Out[Len++]=hasBaro?'B':'_'; + Out[Len++]=hasRMC ?'R':'_'; + Out[Len++]=hasGGA ?'G':'_'; + Out[Len++]=hasGSA ?'G':'_'; + Out[Len++]=isValid() ?'V':'_'; + Out[Len++]=isTimeValid() ?'T':'_'; + Out[Len++]=isDateValid() ?'D':'_'; + + Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, (uint16_t)Hour, 2); + Out[Len++]=':'; Len+=Format_UnsDec(Out+Len, (uint16_t)Min, 2); + Out[Len++]=':'; Len+=Format_UnsDec(Out+Len, (uint16_t)Sec, 2); + Out[Len++]='.'; Len+=Format_UnsDec(Out+Len, (uint16_t)FracSec, 2); + Len+=sprintf(Out+Len, " %d/%d/%02d", FixQuality, FixMode, Satellites); Out[Len++]='/'; Len+=Format_UnsDec(Out+Len, PDOP, 2, 1); Out[Len++]='/'; Len+=Format_UnsDec(Out+Len, HDOP, 2, 1); Out[Len++]='/'; Len+=Format_UnsDec(Out+Len, VDOP, 2, 1); @@ -1406,6 +1423,9 @@ class GPS_Position Out[Len++]='/'; Len+=Format_SignDec(Out+Len, GeoidSeparation, 4, 1); Out[Len++]='m'; Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, Speed, 2, 1); Out[Len++]='m'; Out[Len++]='/'; Out[Len++]='s'; Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, Heading, 4, 1); Out[Len++]='d'; Out[Len++]='e'; Out[Len++]='g'; + Out[Len++]=' '; Len+=Format_SignDec(Out+Len, Temperature, 2, 1); Out[Len++]='C'; + Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, Pressure/4 ); Out[Len++]='P'; Out[Len++]='a'; + Out[Len++]=' '; Len+=Format_SignDec(Out+Len, StdAltitude, 2, 1); Out[Len++]='m'; Out[Len++]='\n'; Out[Len++]=0; return Len; } #endif // __AVR__ @@ -1427,7 +1447,7 @@ class GPS_Position int8_t ReadGGA(NMEA_RxMsg &RxMsg) { if(RxMsg.Parms<14) return -1; // no less than 14 paramaters - GPS = ReadTime((const char *)RxMsg.ParmPtr(0))>0; // read time and check if same as the RMC says + hasGPS = ReadTime((const char *)RxMsg.ParmPtr(0))>0; // read time and check if same as the RMC says FixQuality =Read_Dec1(*RxMsg.ParmPtr(5)); if(FixQuality<0) FixQuality=0; // fix quality: 0=invalid, 1=GPS, 2=DGPS Satellites=Read_Dec2((const char *)RxMsg.ParmPtr(6)); // number of satellites if(Satellites<0) Satellites=Read_Dec1(RxMsg.ParmPtr(6)[0]); @@ -1443,7 +1463,7 @@ class GPS_Position int8_t ReadGGA(const char *GGA) { if( (memcmp(GGA, "$GPGGA", 6)!=0) && (memcmp(GGA, "$GNGGA", 6)!=0) ) return -1; // check if the right sequence uint8_t Index[20]; if(IndexNMEA(Index, GGA)<14) return -2; // index parameters and check the sum - GPS = ReadTime(GGA+Index[0])>0; + hasGPS = ReadTime(GGA+Index[0])>0; FixQuality =Read_Dec1(GGA[Index[5]]); if(FixQuality<0) FixQuality=0; // fix quality Satellites=Read_Dec2(GGA+Index[6]); // number of satellites if(Satellites<0) Satellites=Read_Dec1(GGA[Index[6]]); @@ -1475,7 +1495,7 @@ class GPS_Position int ReadRMC(NMEA_RxMsg &RxMsg) { if(RxMsg.Parms<12) return -1; // no less than 12 parameters - GPS = ReadTime((const char *)RxMsg.ParmPtr(0))>0; // read time and check if same as the GGA says + hasGPS = ReadTime((const char *)RxMsg.ParmPtr(0))>0; // read time and check if same as the GGA says if(ReadDate((const char *)RxMsg.ParmPtr(8))<0) setDefaultDate(); // date ReadLatitude(*RxMsg.ParmPtr(3), (const char *)RxMsg.ParmPtr(2)); // Latitude ReadLongitude(*RxMsg.ParmPtr(5), (const char *)RxMsg.ParmPtr(4)); // Longitude @@ -1487,7 +1507,7 @@ class GPS_Position int8_t ReadRMC(const char *RMC) { if( (memcmp(RMC, "$GPRMC", 6)!=0) && (memcmp(RMC, "$GNRMC", 6)!=0) ) return -1; // check if the right sequence uint8_t Index[20]; if(IndexNMEA(Index, RMC)<12) return -2; // index parameters and check the sum - GPS = ReadTime(RMC+Index[0])>0; + hasGPS = ReadTime(RMC+Index[0])>0; if(ReadDate(RMC+Index[8])<0) setDefaultDate(); ReadLatitude( RMC[Index[3]], RMC+Index[2]); ReadLongitude(RMC[Index[5]], RMC+Index[4]); @@ -1496,56 +1516,96 @@ class GPS_Position calcLatitudeCosine(); return 1; } - int8_t calcDifferences(GPS_Position &RefPos) // calculate climb rate and turn rate with an earlier reference position + int16_t calcTimeDiff(GPS_Position &RefPos) const + { int16_t TimeDiff = (FracSec+(int16_t)Sec*100) - (RefPos.FracSec+(int16_t)RefPos.Sec*100); + if(TimeDiff<(-3000)) TimeDiff+=6000; + else if(TimeDiff>=3000) TimeDiff-=6000; + return TimeDiff; } // [0.01s] + + int16_t calcDifferences(GPS_Position &RefPos) // calculate climb rate and turn rate with an earlier reference position { ClimbRate=0; TurnRate=0; if(RefPos.FixQuality==0) return 0; - int TimeDiff=Sec-RefPos.Sec; if(TimeDiff<(-30)) TimeDiff+=60; - if(TimeDiff==0) return 0; - ClimbRate = Altitude-RefPos.Altitude; + int16_t TimeDiff = calcTimeDiff(RefPos); + if(TimeDiff<5) return 0; TurnRate = Heading-RefPos.Heading; if(TurnRate>1800) TurnRate-=3600; else if(TurnRate<(-1800)) TurnRate+=3600; - if(Baro && RefPos.Baro && (abs(Altitude-StdAltitude)<2500) ) + ClimbRate = Altitude-RefPos.Altitude; + if(hasBaro && RefPos.hasBaro && (abs(Altitude-StdAltitude)<2500) ) { ClimbRate = StdAltitude-RefPos.StdAltitude; } - if(TimeDiff==1) + if(TimeDiff==100) { } - else if(TimeDiff==2) + else if(TimeDiff==200) { ClimbRate=(ClimbRate+1)>>1; TurnRate=(TurnRate+1)>>1; } else - { ClimbRate/=TimeDiff; - TurnRate/=TimeDiff; } - return TimeDiff; } + { ClimbRate = ((int32_t)ClimbRate*100)/TimeDiff; + TurnRate = ((int32_t)TurnRate *100)/TimeDiff; } + return TimeDiff; } // [0.01s] - void Encode(MAV_GPS_RAW_INT &MAV) const - { MAV.time_usec = (int64_t)1000000*getUnixTime(); - MAV.lat = ((int64_t)50*Latitude+1)/3; - MAV.lon = ((int64_t)50*Longitude+1)/3; - MAV.alt = 100*Altitude; - MAV.vel = 10*Speed; - MAV.cog = 10*Heading;; - MAV.fix_type = 1+FixQuality; - MAV.eph = 10*HDOP; - MAV.epv = 10*VDOP; - MAV.satellites_visible = Satellites; } + void Write(MAV_GPS_RAW_INT *MAV) const + { MAV->time_usec = (int64_t)1000000*getUnixTime()+10000*FracSec; + MAV->lat = ((int64_t)50*Latitude+1)/3; + MAV->lon = ((int64_t)50*Longitude+1)/3; + MAV->alt = 100*Altitude; + MAV->vel = 10*Speed; + MAV->cog = 10*Heading;; + MAV->fix_type = 1+FixQuality; + MAV->eph = 10*HDOP; + MAV->epv = 10*VDOP; + MAV->satellites_visible = Satellites; } + + void Read(const MAV_GPS_RAW_INT *MAV, uint64_t UnixTime_ms=0) + { if(UnixTime_ms) setUnixTime_ms(UnixTime_ms); + Latitude = ((int64_t)MAV->lat*3+25)/50; + Longitude = ((int64_t)MAV->lon*3+25)/50; + Altitude = (MAV->alt+50)/100; // [0.1m] + Heading = (MAV->cog+5)/10; // [0.1deg] + Speed = (MAV->vel+5)/10; // [0.1m/s] + HDOP = (MAV->eph+5)/10; + VDOP = (MAV->epv+5)/10; + // GeoidSeparation = Parameters.GeoidSepar; + Satellites = MAV->satellites_visible; + FixMode = MAV->fix_type-1; + FixQuality = 1; + hasGPS = 1; } + + void Read(const MAV_GLOBAL_POSITION_INT *MAV, uint64_t UnixTime_ms=0) + { if(UnixTime_ms) setUnixTime_ms(UnixTime_ms); + Latitude = ((int64_t)MAV->lat*3+25)/50; + Longitude = ((int64_t)MAV->lon*3+25)/50; + Altitude = (MAV->alt+50)/100; // [0.1m] + ClimbRate = -MAV->vz/10; // [0.1m/s] + Heading = (uint32_t)((uint16_t)IntAtan2(MAV->vy, MAV->vx)*(uint32_t)450+0x1000)>>13; // [0.1degC] + Speed = IntSqrt((int32_t)MAV->vx*MAV->vx+(int32_t)MAV->vy*MAV->vy)/10; // [0.1m/s] + // GeoidSeparation = Parameters.GeoidSepar; + FixMode = 3; + FixQuality = 1; + hasGPS = 1; } + + void Read(const MAV_SCALED_PRESSURE *MAV, uint64_t UnixTime_ms=0) + { if(UnixTime_ms) setUnixTime_ms(UnixTime_ms); + Pressure = 100*4*MAV->press_abs; + Temperature = MAV->temperature/10; + hasBaro=1; } void Encode(OGN_Packet &Packet) const - { Packet.Position.FixQuality = FixQuality<3 ? FixQuality:3; - if((FixQuality>0)&&(FixMode>=2)) Packet.Position.FixMode = FixMode-2; + { Packet.Position.FixQuality = FixQuality<3 ? FixQuality:3; // + if((FixQuality>0)&&(FixMode>=2)) Packet.Position.FixMode = FixMode-2; // else Packet.Position.FixMode = 0; - if(PDOP>0) Packet.EncodeDOP(PDOP-10); // encode PDOP from GSA - else Packet.EncodeDOP(HDOP-10); // or if no GSA: use HDOP - int ShortTime=Sec; - if(FracSec>=50) { ShortTime+=1; if(ShortTime>=60) ShortTime-=60; } - Packet.Position.Time=ShortTime; - Packet.EncodeLatitude(Latitude); - Packet.EncodeLongitude(Longitude); - Packet.EncodeSpeed(Speed); - Packet.EncodeHeading(Heading); - Packet.EncodeClimbRate(ClimbRate); - Packet.EncodeTurnRate(TurnRate); - Packet.EncodeAltitude((Altitude+5)/10); - if(Baro) Packet.EncodeStdAltitude((StdAltitude+5)/10); - else Packet.clrBaro(); + if(PDOP>0) Packet.EncodeDOP(PDOP-10); // encode PDOP from GSA + else Packet.EncodeDOP(HDOP-10); // or if no GSA: use HDOP + int8_t ShortTime=Sec; // the 6-bit time field in the OGN packet + if(FracSec>=50) { ShortTime+=1; if(ShortTime>=60) ShortTime-=60; } // round to the closest full second + Packet.Position.Time=ShortTime; // Time + Packet.EncodeLatitude(Latitude); // Latitude + Packet.EncodeLongitude(Longitude); // Longitude + Packet.EncodeSpeed(Speed); // Speed + Packet.EncodeHeading(Heading); // Heading = track-over-ground + Packet.EncodeClimbRate(ClimbRate); // Climb rate + Packet.EncodeTurnRate(TurnRate); // Turn rate + Packet.EncodeAltitude((Altitude+5)/10); // Altitude + if(hasBaro) Packet.EncodeStdAltitude((StdAltitude+5)/10); // Pressure altitude + else Packet.clrBaro(); //or no-baro if pressure sensor data not there } void EncodeStatus(OGN_Packet &Packet) const @@ -1556,7 +1616,7 @@ class GPS_Position Packet.Status.FixQuality = FixQuality<3 ? FixQuality:3; Packet.Status.Satellites = Satellites<15 ? Satellites:15; Packet.EncodeAltitude((Altitude+5)/10); - if(Baro) + if(hasBaro) { Packet.EncodeTemperature(Temperature); Packet.Status.Pressure = (Pressure+16)>>5; } else @@ -1571,13 +1631,62 @@ class GPS_Position // return 3; } // => Australia + South America: upper half of 915MHz band // return 2; } // => USA/Canada: full 915MHz band + void Encode(OGN_Packet &Packet, int16_t dTime) const // Encode position which is extrapolated by the given fraction of a second + { Packet.Position.FixQuality = FixQuality<3 ? FixQuality:3; // + if((FixQuality>0)&&(FixMode>=2)) Packet.Position.FixMode = FixMode-2; // + else Packet.Position.FixMode = 0; + if(PDOP>0) Packet.EncodeDOP(PDOP-10); // encode PDOP from GSA + else Packet.EncodeDOP(HDOP-10); // or if no GSA: use HDOP + int32_t Lat, Lon, Alt; int16_t Head; + calcExtrapolation(Lat, Lon, Alt, Head, dTime); + int16_t ShortTime=Sec; // the 6-bit time field in the OGN packet + dTime += FracSec; + while(dTime>= 50 ) { dTime-=100; ShortTime++; if(ShortTime>=60) ShortTime-=60; } + while(dTime<(-50)) { dTime+=100; ShortTime--; if(ShortTime< 0) ShortTime+=60; } + Packet.Position.Time=ShortTime; // Time + Packet.EncodeLatitude(Lat); // Latitude + Packet.EncodeLongitude(Lon); // Longitude + Packet.EncodeSpeed(Speed); // Speed + Packet.EncodeHeading(Head); // Heading = track-over-ground + Packet.EncodeClimbRate(ClimbRate); // Climb rate + Packet.EncodeTurnRate(TurnRate); // Turn rate + Packet.EncodeAltitude((Alt+5)/10); // Altitude + if(hasBaro) Packet.EncodeStdAltitude((StdAltitude+(Alt-Altitude)+5)/10); // Pressure altitude + else Packet.clrBaro(); //or no-baro if pressure sensor data not there + } + + void calcExtrapolation(int32_t &Lat, int32_t &Lon, int32_t &Alt, int16_t &Head, int32_t dTime) const // extrapolate GPS position by a fraction of a second + { int16_t HeadAngle = ((int32_t)Heading<<12)/225; // [] + int16_t TurnAngle = (((dTime*TurnRate)/25)<<9)/225; // [] + HeadAngle += TurnAngle; + int32_t LatSpeed = ((int32_t)Speed*Icos(HeadAngle))>>12; // [0.1m/s] + int32_t LonSpeed = ((int32_t)Speed*Isin(HeadAngle))>>12; // [0.1m/s] + Lat = Latitude + calcLatitudeExtrapolation (dTime, LatSpeed); + Lon = Longitude + calcLongitudeExtrapolation(dTime, LonSpeed); + Alt = Altitude + calcAltitudeExtrapolation(dTime); + Head = Heading + (dTime*TurnRate)/100; + if(Head<0) Head+=3600; else if(Head>=3600) Head-=3600; } + + int32_t calcAltitudeExtrapolation(int32_t Time) const // [0.01s] + { return Time*ClimbRate/100; } // [0.1m] + + int32_t calcLatitudeExtrapolation(int32_t Time, int32_t LatSpeed) const // [0.01s] + { return (Time*LatSpeed*177)>>15; } // [0.1m] + + int32_t calcLongitudeExtrapolation(int32_t Time, int32_t LonSpeed) const // [0.01s] + { int16_t LatCosine = calcLatCosine(calcLatAngle16(Latitude)); + return calcLongitudeExtrapolation(Time, LonSpeed, LatCosine); } + + int32_t calcLongitudeExtrapolation(int32_t Time, int32_t LonSpeed, int16_t LatCosine) const // [0.01s] + { return (((int32_t)Time*LonSpeed*177)>>3)/LatCosine; } + // static int32_t calcLatDistance(int32_t Lat1, int32_t Lat2) // [m] distance along latitude // { return ((int64_t)(Lat2-Lat1)*0x2f684bda+0x80000000)>>32; } // static int32_t calcLatAngle32(int32_t Lat) // convert latitude to 32-bit integer angle // { return ((int64_t)Lat*2668799779u+0x4000000)>>27; } - static int16_t calcLatAngle16(int32_t Lat) // convert latitude to 16-bit integer angle + static int16_t calcLatAngle16(int32_t Lat) // convert latitude to 16-bit integer angle { return ((int64_t)Lat*1303125+0x80000000)>>32; } // static int32_t calcLatCosine(int32_t LatAngle) // calculate the cosine of the latitude 32-bit integer angle @@ -1667,28 +1776,28 @@ class GPS_Position else if(DOP>255) DOP=255; VDOP=DOP; return 0; } - int8_t ReadTime(const char *Value) + int8_t ReadTime(const char *Value) // read the Time field: HHMMSS.ss and check if it is a new one or the same one { int8_t Prev; int8_t Same=1; Prev=Hour; - Hour=Read_Dec2(Value); if(Hour<0) return -1; // read hour (two digits) + Hour=Read_Dec2(Value); if(Hour<0) return -1; // read hour (two digits), return when invalid if(Prev!=Hour) Same=0; Prev=Min; - Min=Read_Dec2(Value+2); if(Min<0) return -1; // read minute (two digits) + Min=Read_Dec2(Value+2); if(Min<0) return -1; // read minute (two digits), return when invalid if(Prev!=Min) Same=0; Prev=Sec; - Sec=Read_Dec2(Value+4); if(Sec<0) return -1; // read second (two digits) + Sec=Read_Dec2(Value+4); if(Sec<0) return -1; // read second (two digits), return when invalid if(Prev!=Sec) Same=0; Prev=FracSec; - if(Value[6]=='.') // is there a second fraction ? - { FracSec=Read_Dec2(Value+7); if(FracSec<0) return -1; } - if(Prev!=FracSec) Same=0; - return Same; } // return 1 when time did not change (both RMC and GGA were for same time) + if(Value[6]=='.') // is there a fraction + { FracSec=Read_Dec2(Value+7); if(FracSec<0) return -1; } // read the fraction, return when invalid + if(Prev!=FracSec) Same=0; // return 0 when time is valid but did not change + return Same; } // return 1 when time did not change (both RMC and GGA were for same time) - int8_t ReadDate(const char *Param) - { Day=Read_Dec2(Param); if(Day<0) return -1; // read calendar year (two digits - thus need to be extended to four) - Month=Read_Dec2(Param+2); if(Month<0) return -1; // read calendar month - Year=Read_Dec2(Param+4); if(Year<0) return -1; // read calendar day - return 0; } + int8_t ReadDate(const char *Param) // read the field DDMMYY + { Day=Read_Dec2(Param); if(Day<0) return -1; // read calendar year (two digits - thus need to be extended to four) + Month=Read_Dec2(Param+2); if(Month<0) return -1; // read calendar month + Year=Read_Dec2(Param+4); if(Year<0) return -1; // read calendar day + return 0; } // return 0 when field valid and was read correctly int8_t static IndexNMEA(uint8_t Index[20], const char *Seq) // index parameters and verify the NMEA checksum { if(Seq[0]!='$') return -1; @@ -1724,6 +1833,7 @@ class GPS_Position Hour = DayTime/SecsPerHour; DayTime -= (uint32_t)Hour*SecsPerHour; // Min = DayTime/SecsPerMin; DayTime -= (uint16_t)Min*SecsPerMin; Sec = DayTime; + FracSec=0; Days -= 10957+1; // [day] since 2000 minus 1 day Year = (Days*4)/((365*4)+1); // [year] since 1970 Days -= 365*Year + (Year/4); @@ -1731,7 +1841,12 @@ class GPS_Position Day = Days-(uint16_t)Month*31+1; Month++; uint32_t CheckTime = getUnixTime(); if(CheckTime #include -#include "nvs.h" -// #include "flashsize.h" -// #include "uniqueid.h" +#include "hal.h" -// #include "stm32f10x_flash.h" +#ifdef WITH_ESP32 +#include "nvs.h" +#endif + +#ifdef WITH_STM32 +#include "stm32f10x_flash.h" +#include "flashsize.h" +#endif #include "nmea.h" #include "format.h" @@ -26,19 +31,32 @@ class FlashParameters bool Stealth:1; } ; } ; + int16_t RFchipFreqCorr; // [10Hz] frequency correction for crystal frequency offset int8_t RFchipTxPower; // [dBm] highest bit set => HW module (up to +20dBm Tx power) int8_t RFchipTempCorr; // [degC] correction to the temperature measured in the RF chip + uint32_t CONbaud; // [bps] Console baud rate - uint16_t PressCorr; // [0.25Pa] pressure correction for the baro + + int16_t PressCorr; // [0.25Pa] pressure correction for the baro union - { uint16_t Flags; + { uint8_t Flags; struct { bool SaveToFlash:1; // Save parameters from the config file to Flash bool hasBT:1; // has BT interface on the console bool BT_ON:1; // BT on after power up } ; } ; // + int8_t TimeCorr; // [sec] it appears for ArduPilot you need to correct time by 3 seconds + + int16_t GeoidSepar; // [0.1m] Geoid-Separation, apparently ArduPilot MAVlink does not give this value (although present in the format) + uint16_t SoftPPSdelay; // [ms] + char Pilot[16]; + char Manuf[16]; + char Type[16]; + char Reg[16]; + char Base[16]; + char ICE[16]; // char BTname[8]; // char BTpin[4]; // char Pilot[16]; @@ -59,8 +77,11 @@ class FlashParameters public: // void setDefault(void) { setDefaults(UniqueID[0] ^ UniqueID[1] ^ UniqueID[2]); } - void setDefault(uint32_t UniqueID) - { AcftID = 0x07000000 | (UniqueID&0x00FFFFFF); + void setDefault(void) + { setDefault(getUniqueAddress()); } + + void setDefault(uint32_t UniqueAddr) + { AcftID = 0x07000000 | (UniqueAddr&0x00FFFFFF); RFchipFreqCorr = 0; // [10Hz] #ifdef WITH_RFM69W RFchipTxPower = 13; // [dBm] for RFM69W @@ -68,19 +89,20 @@ class FlashParameters RFchipTxPower = 0x80 | 14; // [dBm] for RFM69HW #endif RFchipTempCorr = 0; // [degC] - PressCorr = 0; // [0.25Pa] CONbaud = 115200; // [bps] + PressCorr = 0; // [0.25Pa] + TimeCorr = 0; // [sec] + GeoidSepar = 470; // [0.1m] + + Pilot[0] = 0; + Manuf[0] = 0; + Type[0] = 0; + Reg[0] = 0; + Base[0] = 0; + ICE[0] = 0; } - uint32_t static CheckSum(const uint32_t *Word, uint32_t Words) // calculate check-sum of pointed data - { uint32_t Check=CheckInit; - for(uint32_t Idx=0; Idx0) && (BaudRate<=230400) ) CONbaud = BaudRate; return 0; } +*/ + static bool isStringChar (char ch) // characters accepted as part of the string values + { if( (ch>='0') && (ch<='9') ) return 1; // numbers + if( (ch>='A') && (ch<='Z') ) return 1; // uppercase letters + if( (ch>='a') && (ch<='z') ) return 1; // lowercase letters + if(ch=='.') return 1; + if(ch=='-') return 1; + if(ch=='+') return 1; + if(ch=='_') return 1; + return 0; } - static char *SkipBlanks(char *Inp) + static int8_t Read_String(char *Value, const char *Inp, uint8_t MaxLen) + { const char *Val = SkipBlanks(Inp); + uint8_t Idx; + for(Idx=0; Idx' ') break; Inp++; } return Inp; } - bool ReadLine(char *Line) - { char *Name = SkipBlanks(Line); if((*Name)==0) return 0; - Line = Name; - for( ; ; ) - { char ch=(*Line); - if(ch<=' ') break; - if(ch=='=') break; - Line++; } - char *NameEnd=Line; - Line=SkipBlanks(Line); if((*Line)!='=') return 0; - char *Value = SkipBlanks(Line+1); if((*Value)<=' ') return 0; - *NameEnd=0; - return ReadParam(Name, Value); } - - bool ReadParam(const char *Name, const char *Value) + bool ReadParam(const char *Name, const char *Value) // interprete "Name = Value" line { if(strcmp(Name, "Address")==0) { uint32_t Addr=0; if(Read_Int(Addr, Value)<=0) return 0; Address=Addr; return 1; } @@ -237,16 +288,47 @@ class FlashParameters if(strcmp(Name, "PressCorr")==0) { int32_t Corr=0; if(Read_Float1(Corr, Value)<=0) return 0; PressCorr=4*Corr/10; return 1; } + if(strcmp(Name, "TimeCorr")==0) + { int32_t Corr=0; if(Read_Int(Corr, Value)<=0) return 0; + TimeCorr=Corr; return 1; } + if(strcmp(Name, "GeoidSepar")==0) + { return Read_Float1(GeoidSepar, Value)<=0; } + if(strcmp(Name, "Pilot")==0) + { return Read_String(Pilot, Value, 16)<=0; } + if(strcmp(Name, "Manuf")==0) + { return Read_String(Manuf, Value, 16 )<=0; } + if(strcmp(Name, "Type")==0) + { return Read_String(Type , Value, 16)<=0; } + if(strcmp(Name, "Reg")==0) + { return Read_String(Reg , Value, 16)<=0; } + if(strcmp(Name, "Base")==0) + { return Read_String(Base , Value, 16)<=0; } + if(strcmp(Name, "ICE")==0) + { return Read_String(ICE , Value, 16)<=0; } return 0; } + bool ReadLine(char *Line) // read a parameter line + { char *Name = (char *)SkipBlanks(Line); if((*Name)==0) return 0; // skip blanks and get to parameter name + Line = Name; + for( ; ; ) // step through the characters + { char ch=(*Line); // next character + if(ch<=' ') break; // break at blank + if(ch=='=') break; // or equal sign + Line++; } + char *NameEnd=Line; // remember where the parameter name ends + Line = (char *)SkipBlanks(Line); if((*Line)!='=') return 0; // next should be the equal sign + char *Value = (char *)SkipBlanks(Line+1); if((*Value)<=' ') return 0; + *NameEnd=0; // put NULL character just after the parameter name + return ReadParam(Name, Value); } + int ReadFile(FILE *File) - { char Line[80]; - size_t Lines=0; - for( ; ; ) - { if(fgets(Line, 80, File)==0) break; - if(strchr(Line, '\n')==0) break; - if(ReadLine(Line)) Lines++; } - return Lines; } + { char Line[80]; // line buffer + size_t Lines=0; // count interpreted lines + for( ; ; ) // loop over lines + { if(fgets(Line, 80, File)==0) break; // break on EOF or other trouble reading the file + if(strchr(Line, '\n')==0) break; // if no NL then break, line was too long + if(ReadLine(Line)) Lines++; } // interprete the line, count if positive + return Lines; } // return number of interpreted lines int ReadFile(const char *Name = "/spiffs/TRACKER.CFG") { FILE *File=fopen(Name, "rt"); if(File==0) return 0; @@ -254,50 +336,84 @@ class FlashParameters fclose(File); return Lines; } int Write_Hex(char *Line, const char *Name, uint32_t Value, uint8_t Digits) - { uint8_t Len=Format_String(Line, Name); - Len+=Format_String(Line+Len, "=0x"); + { uint8_t Len=Format_String(Line, Name, 12, 0); + Len+=Format_String(Line+Len, " = 0x"); Len+=Format_Hex(Line+Len, Value, Digits); Len+=Format_String(Line+Len, ";"); Line[Len]=0; return Len; } int Write_UnsDec(char *Line, const char *Name, uint32_t Value) - { uint8_t Len=Format_String(Line, Name); - Len+=Format_String(Line+Len, "="); + { uint8_t Len=Format_String(Line, Name, 12, 0); + Len+=Format_String(Line+Len, " = "); Len+=Format_UnsDec(Line+Len, Value); Len+=Format_String(Line+Len, ";"); Line[Len]=0; return Len; } int Write_SignDec(char *Line, const char *Name, int32_t Value) - { uint8_t Len=Format_String(Line, Name); - Len+=Format_String(Line+Len, "="); + { uint8_t Len=Format_String(Line, Name, 12, 0); + Len+=Format_String(Line+Len, " = "); Len+=Format_SignDec(Line+Len, Value); Len+=Format_String(Line+Len, ";"); Line[Len]=0; return Len; } int Write_Float1(char *Line, const char *Name, int32_t Value) - { uint8_t Len=Format_String(Line, Name); - Len+=Format_String(Line+Len, "="); + { uint8_t Len=Format_String(Line, Name, 12, 0); + Len+=Format_String(Line+Len, " = "); Len+=Format_SignDec(Line+Len, Value, 2, 1); Len+=Format_String(Line+Len, ";"); Line[Len]=0; return Len; } + int Write_String(char *Line, const char *Name, char *Value, uint8_t MaxLen=16) + { uint8_t Len=Format_String(Line, Name, 12, 0); + Len+=Format_String(Line+Len, " = "); + Len+=Format_String(Line+Len, Value, 0, MaxLen); + Line[Len]=0; return Len; } + int WriteFile(FILE *File) { char Line[80]; - Write_Hex (Line, "Address" , Address , 6); strcat(Line, " # [24-bit]\n"); if(fputs(Line, File)==EOF) return EOF; - Write_Hex (Line, "AddrType", AddrType, 1); strcat(Line, " # [2-bit]\n"); if(fputs(Line, File)==EOF) return EOF; - Write_Hex (Line, "AcftType", AcftType, 1); strcat(Line, " # [4-bit]\n"); if(fputs(Line, File)==EOF) return EOF; - Write_UnsDec (Line, "Console", CONbaud); strcat(Line, " # [bps]\n"); if(fputs(Line, File)==EOF) return EOF; - Write_SignDec(Line, "TxPower", getTxPower()); strcat(Line, " # [ dBm]\n"); if(fputs(Line, File)==EOF) return EOF; - Write_Float1 (Line, "FreqCorr", (int32_t)RFchipFreqCorr/10); strcat(Line, " # [kHz]\n"); if(fputs(Line, File)==EOF) return EOF; - Write_SignDec(Line, "TempCorr", (int32_t)RFchipTempCorr ); strcat(Line, " # [degC]\n"); if(fputs(Line, File)==EOF) return EOF; - Write_Float1 (Line, "PressCorr",(int32_t)PressCorr*10/4 ); strcat(Line, " # [Pa]\n"); if(fputs(Line, File)==EOF) return EOF; - return 8; } + Write_Hex (Line, "Address" , Address , 6); strcat(Line, " # [24-bit]\n"); if(fputs(Line, File)==EOF) return EOF; + Write_Hex (Line, "AddrType" , AddrType, 1); strcat(Line, " # [2-bit]\n"); if(fputs(Line, File)==EOF) return EOF; + Write_Hex (Line, "AcftType" , AcftType, 1); strcat(Line, " # [4-bit]\n"); if(fputs(Line, File)==EOF) return EOF; + Write_UnsDec (Line, "Console" , CONbaud ); strcat(Line, " # [ bps]\n"); if(fputs(Line, File)==EOF) return EOF; + Write_SignDec(Line, "TxPower" , getTxPower() ); strcat(Line, " # [ dBm]\n"); if(fputs(Line, File)==EOF) return EOF; + Write_Float1 (Line, "FreqCorr" , (int32_t)RFchipFreqCorr/10); strcat(Line, " # [ kHz]\n"); if(fputs(Line, File)==EOF) return EOF; + Write_SignDec(Line, "TempCorr" , (int32_t)RFchipTempCorr ); strcat(Line, " # [ degC]\n"); if(fputs(Line, File)==EOF) return EOF; + Write_Float1 (Line, "PressCorr" , (int32_t)PressCorr*10/4 ); strcat(Line, " # [ Pa]\n"); if(fputs(Line, File)==EOF) return EOF; + Write_SignDec(Line, "TimeCorr" , (int32_t)TimeCorr ); strcat(Line, " # [ s]\n"); if(fputs(Line, File)==EOF) return EOF; + Write_Float1 (Line, "GeoidSepar", GeoidSepar ); strcat(Line, " # [ m]\n"); if(fputs(Line, File)==EOF) return EOF; + Write_String (Line, "Pilot" , Pilot ); strcat(Line, " # [16char]\n"); if(fputs(Line, File)==EOF) return EOF; + Write_String (Line, "Manuf" , Manuf ); strcat(Line, " # [16char]\n"); if(fputs(Line, File)==EOF) return EOF; + Write_String (Line, "Type" , Type ); strcat(Line, " # [16char]\n"); if(fputs(Line, File)==EOF) return EOF; + Write_String (Line, "Reg" , Reg ); strcat(Line, " # [16char]\n"); if(fputs(Line, File)==EOF) return EOF; + Write_String (Line, "Base" , Base ); strcat(Line, " # [16char]\n"); if(fputs(Line, File)==EOF) return EOF; + Write_String (Line, "ICE" , ICE ); strcat(Line, " # [16char]\n"); if(fputs(Line, File)==EOF) return EOF; + return 16; } int WriteFile(const char *Name = "/spiffs/TRACKER.CFG") { FILE *File=fopen(Name, "wt"); if(File==0) return 0; int Lines=WriteFile(File); fclose(File); return Lines; } + void Write(void (*Output)(char)) + { char Line[80]; + Write_Hex (Line, "Address" , Address , 6); strcat(Line, " # [24-bit]\n"); Format_String(Output, Line); + Write_Hex (Line, "AddrType" , AddrType, 1); strcat(Line, " # [2-bit]\n"); Format_String(Output, Line); + Write_Hex (Line, "AcftType" , AcftType, 1); strcat(Line, " # [4-bit]\n"); Format_String(Output, Line); + Write_UnsDec (Line, "Console" , CONbaud ); strcat(Line, " # [ bps]\n"); Format_String(Output, Line); + Write_SignDec(Line, "TxPower" , getTxPower() ); strcat(Line, " # [ dBm]\n"); Format_String(Output, Line); + Write_Float1 (Line, "FreqCorr" , (int32_t)RFchipFreqCorr/10); strcat(Line, " # [ kHz]\n"); Format_String(Output, Line); + Write_SignDec(Line, "TempCorr" , (int32_t)RFchipTempCorr ); strcat(Line, " # [ degC]\n"); Format_String(Output, Line); + Write_Float1 (Line, "PressCorr" , (int32_t)PressCorr*10/4 ); strcat(Line, " # [ Pa]\n"); Format_String(Output, Line); + Write_SignDec(Line, "TimeCorr" , (int32_t)TimeCorr ); strcat(Line, " # [ s]\n"); Format_String(Output, Line); + Write_Float1 (Line, "GeoidSepar", GeoidSepar ); strcat(Line, " # [ m]\n"); Format_String(Output, Line); + Write_String (Line, "Pilot" , Pilot ); strcat(Line, " # [16char]\n"); Format_String(Output, Line); + Write_String (Line, "Manuf" , Manuf ); strcat(Line, " # [16char]\n"); Format_String(Output, Line); + Write_String (Line, "Type" , Type ); strcat(Line, " # [16char]\n"); Format_String(Output, Line); + Write_String (Line, "Reg" , Reg ); strcat(Line, " # [16char]\n"); Format_String(Output, Line); + Write_String (Line, "Base" , Base ); strcat(Line, " # [16char]\n"); Format_String(Output, Line); + Write_String (Line, "ICE" , ICE ); strcat(Line, " # [16char]\n"); Format_String(Output, Line); + } + } ; #endif // __PARAMETERS_H__ diff --git a/main/proc.cpp b/main/proc.cpp index c11e2cd..28c67d1 100644 --- a/main/proc.cpp +++ b/main/proc.cpp @@ -2,12 +2,17 @@ #include "hal.h" #include "proc.h" +#include "ctrl.h" #include "ogn.h" #include "rf.h" #include "gps.h" +#ifdef WITH_FLASHLOG +#include "flashlog.h" +#endif + static char Line[128]; // for printing out to serial port, etc. static LDPC_Decoder Decoder; // error corrector for the OGN Gallager code @@ -51,15 +56,18 @@ static void CleanRelayQueue(uint32_t Time, uint32_t Delay=20) static void ReadStatus(OGN_TxPacket &StatPacket) { - // 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] - // if(StatPacket.Packet.Status.Pressure==0) StatPacket.Packet.EncodeTemperature(MCU_Temp); // [0.1degC] +#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] StatPacket.Packet.Status.RadioNoise = RX_AverRSSI; // [-0.5dBm] write radio noise to the status packet StatPacket.Packet.Status.TxPower = Parameters.getTxPower()-4; - uint16_t RxRate = RX_OGN_Count64; + uint16_t RxRate = RX_OGN_Count64+1; uint8_t RxRateLog2=0; RxRate>>=1; while(RxRate) { RxRate>>=1; RxRateLog2++; } StatPacket.Packet.Status.RxRate = RxRateLog2; @@ -84,7 +92,7 @@ static void ReadStatus(OGN_TxPacket &StatPacket) // LogLine(Line); // if(CONS_UART_Free()>=128) { xSemaphoreTake(CONS_Mutex, portMAX_DELAY); - Format_String(CONS_UART_Write, Line, Len); // send the NMEA out to the console + Format_String(CONS_UART_Write, Line, 0, Len); // send the NMEA out to the console xSemaphoreGive(CONS_Mutex); } #ifdef WITH_SDLOG if(Log_Free()>=128) @@ -99,30 +107,21 @@ static void ReadStatus(OGN_TxPacket &StatPacket) static void ProcessRxPacket(OGN_RxPacket *RxPacket, uint8_t RxPacketIdx) // process every (correctly) received packet { int32_t LatDist=0, LonDist=0; uint8_t Warn=0; - if( RxPacket->Packet.Header.Other || RxPacket->Packet.Header.Encrypted ) return ; // status packet or encrypted: ignore + if( RxPacket->Packet.Header.Other || RxPacket->Packet.Header.Encrypted ) return ; // status packet or encrypted: ignore uint8_t MyOwnPacket = ( RxPacket->Packet.Header.Address == Parameters.Address ) && ( RxPacket->Packet.Header.AddrType == Parameters.AddrType ); + if(MyOwnPacket) return; // don't process my own (relayed) packets bool DistOK = RxPacket->Packet.calcDistanceVector(LatDist, LonDist, GPS_Latitude, GPS_Longitude, GPS_LatCosine)>=0; if(DistOK) - { if(!MyOwnPacket) - { 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 - if(!MyOwnPacket) - { xSemaphoreTake(CONS_Mutex, portMAX_DELAY); - Format_String(CONS_UART_Write, Line, 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 + { 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); #ifdef WITH_BEEPER - if(KNOB_Tick>12) Play(Play_Vol_1 | Play_Oct_2 | (7+2*Warn), 3+16*Warn); // if Knob>12 => make a beep for every received packet + if(KNOB_Tick>12) Play(Play_Vol_1 | Play_Oct_2 | 7, 3); // if Knob>12 => make a beep for every received packet #endif -#else // WITH_LOOKOUT -#ifdef WITH_BEEPER - if(KNOB_Tick>12) Play(Play_Vol_1 | Play_Oct_2 | 7, 3); // if Knob>12 => make a beep for every received packet -#endif -#endif // WITH_LOOKOUT #ifdef WITH_SDLOG if(Log_Free()>=128) { xSemaphoreTake(Log_Mutex, portMAX_DELAY); @@ -130,20 +129,18 @@ static void ProcessRxPacket(OGN_RxPacket *RxPacket, uint8_t RxPacketIdx) xSemaphoreGive(Log_Mutex); } #endif #ifdef WITH_PFLAA - if(!MyOwnPacket) - { 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, Len); - xSemaphoreGive(CONS_Mutex); } -#endif -#ifdef WITH_MAVLINK - if(!MyOwnPacket) - { 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); } + 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); #endif +// #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 } } @@ -155,22 +152,23 @@ static void DecodeRxPacket(RFM_RxPktData *RxPkt) // RxPacket->RxRSSI=RxPkt.RSSI; // TickType_t ExecTime=xTaskGetTickCount(); - RX_OGN_Packets++; - uint8_t Check = RxPkt->Decode(*RxPacket, Decoder); + { RX_OGN_Packets++; + uint8_t Check = RxPkt->Decode(*RxPacket, Decoder); #ifdef DEBUG_PRINT - 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); + 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); #endif - if( (Check==0) && (RxPacket->RxErr<15) ) // what limit on number of detected bit errors ? - { RxPacket->Packet.Dewhiten(); - ProcessRxPacket(RxPacket, RxPacketIdx); } + if( (Check==0) && (RxPacket->RxErr<15) ) // what limit on number of detected bit errors ? + { RxPacket->Packet.Dewhiten(); + ProcessRxPacket(RxPacket, RxPacketIdx); } + } } @@ -181,19 +179,24 @@ static void DecodeRxPacket(RFM_RxPktData *RxPkt) #endif void vTaskPROC(void* pvParameters) { +#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 RelayQueue.Clear(); - OGN_TxPacket PosPacket; // position packet - uint32_t PosTime=0; // [sec] when the position was recorded - OGN_TxPacket StatPacket; // status report packet + OGN_TxPacket PosPacket; // position packet + uint32_t PosTime=0; // [sec] when the position was recorded + OGN_TxPacket StatPacket; // status report packet for( ; ; ) { vTaskDelay(1); - static uint32_t PrevSlotTime=0; - uint32_t SlotTime = TimeSync_Time(); - if(TimeSync_msTime()<300) SlotTime--; - RFM_RxPktData *RxPkt = RF_RxFIFO.getRead(); // check for new received packets + RFM_RxPktData *RxPkt = RF_RxFIFO.getRead(); // check for new received packets if(RxPkt) { #ifdef DEBUG_PRINT @@ -206,15 +209,36 @@ void vTaskPROC(void* pvParameters) // CONS_UART_Write('\r'); CONS_UART_Write('\n'); xSemaphoreGive(CONS_Mutex); #endif - DecodeRxPacket(RxPkt); // decode and process the received packet + DecodeRxPacket(RxPkt); // decode and process the received packet RF_RxFIFO.Read(); } - if(SlotTime==PrevSlotTime) continue; - PrevSlotTime=SlotTime; + 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 // this part of the loop is executed only once per slot-time - GPS_Position *Position = GPS_getPosition(); + 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(); if(Position) Position->EncodeStatus(StatPacket.Packet); // encode GPS altitude and pressure - if( Position && Position->Ready && (!Position->Sent) && Position->GPS && Position->isValid() ) + if( Position && Position->isReady && (!Position->Sent) && Position->isReady && Position->isValid() ) { 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 /* @@ -232,7 +256,8 @@ void vTaskPROC(void* pvParameters) 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 - Position->Encode(PosPacket.Packet); // encode position/altitude/speed/etc. from GPS position + if(BestResid==0) Position->Encode(PosPacket.Packet); // encode position/altitude/speed/etc. from GPS position + else Position->Encode(PosPacket.Packet, BestResid); PosPacket.Packet.Position.Stealth = Parameters.Stealth; PosPacket.Packet.Position.AcftType = Parameters.AcftType; // aircraft-type OGN_TxPacket *TxPacket = RF_TxFIFO.getWrite(); @@ -252,7 +277,15 @@ void vTaskPROC(void* pvParameters) 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; - +#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 } 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(); @@ -293,8 +326,7 @@ void vTaskPROC(void* pvParameters) *StatusPacket = StatPacket; StatusPacket->Packet.Whiten(); StatusPacket->calcFEC(); - RF_TxFIFO.Write(); - } + RF_TxFIFO.Write(); } while(RF_TxFIFO.Full()<2) { OGN_TxPacket *RelayPacket = RF_TxFIFO.getWrite(); diff --git a/main/rf.cpp b/main/rf.cpp index 94960dc..7f7eb1f 100644 --- a/main/rf.cpp +++ b/main/rf.cpp @@ -167,7 +167,7 @@ extern "C" TRX.Deselect = RFM_Deselect; TRX.TransferByte = RFM_TransferByte; #endif - TRX.DIO0_isOn = RFM_DIO0_isOn; + TRX.DIO0_isOn = RFM_IRQ_isOn; TRX.RESET = RFM_RESET; RF_FreqPlan.setPlan(0); // 1 = Europe/Africa, 2 = USA/CA, 3 = Australia and South America diff --git a/sdkconfig b/sdkconfig index bad4548..22b32db 100644 --- a/sdkconfig +++ b/sdkconfig @@ -135,6 +135,8 @@ CONFIG_GATTC_ENABLE=y CONFIG_BLE_SMP_ENABLE=y CONFIG_BT_STACK_NO_LOG= CONFIG_BT_ACL_CONNECTIONS=4 +CONFIG_BT_ALLOCATION_FROM_SPIRAM_FIRST= +CONFIG_BT_BLE_DYNAMIC_ENV_MEMORY= CONFIG_SMP_ENABLE=y CONFIG_BT_RESERVE_DRAM=0x10000 @@ -344,6 +346,7 @@ CONFIG_LOG_COLORS=y # LWIP # CONFIG_L2_TO_L3_COPY= +CONFIG_LWIP_IRAM_OPTIMIZATION= CONFIG_LWIP_MAX_SOCKETS=4 CONFIG_LWIP_SO_REUSE= CONFIG_LWIP_SO_RCVBUF= @@ -515,6 +518,7 @@ CONFIG_SPIFFS_CACHE_STATS= CONFIG_SPIFFS_PAGE_CHECK=y CONFIG_SPIFFS_GC_MAX_RUNS=10 CONFIG_SPIFFS_GC_STATS= +CONFIG_SPIFFS_PAGE_SIZE=256 CONFIG_SPIFFS_OBJ_NAME_LEN=32 CONFIG_SPIFFS_USE_MAGIC=y CONFIG_SPIFFS_USE_MAGIC_LENGTH=y