From 4fcc489b83d9916f0d74a53c9f9ff1b48c23b039 Mon Sep 17 00:00:00 2001 From: Pawel Jalocha Date: Fri, 3 Jul 2020 11:29:30 +0100 Subject: [PATCH] Add takeoff/landing detection, for IGC file creation --- main/flight.h | 96 +++++++++++++++++++++++++++++++++++++++++++++++++++ main/ogn.h | 30 ++++++++++++++++ main/proc.cpp | 7 ++++ 3 files changed, 133 insertions(+) create mode 100644 main/flight.h diff --git a/main/flight.h b/main/flight.h new file mode 100644 index 0000000..0cc2c32 --- /dev/null +++ b/main/flight.h @@ -0,0 +1,96 @@ +#include +#include + +#include "ogn.h" + +// =========================================================================================== + +class FlightMonitor +{ public: + GPS_Position Takeoff; + GPS_Position Landing; + GPS_Position MaxAltitude; + // GPS_Position Recent; + // const char *IGCpath; + static const uint8_t MinHold = 5; // [sec] + static const uint16_t MinSpeed = 60; // [0.1m/s] + uint8_t HoldTime; + uint8_t TakeoffCount; + + public: + + void Clear(void) + { Takeoff.Clear(); + Landing.Clear(); + MaxAltitude.Clear(); + // Recent.Clear(); + // IGCpath=0; + HoldTime=0; + TakeoffCount=0; } + + static char NameCode(int Num) + { if(Num<=0) return '0'; + if(Num<10) return '0'+Num; + if(Num<36) return 'A'+(Num-10); + return '_'; } + + int ShortName(char *Name, const char *Serial) const + { int Len=0; + Name[Len++]='0'+Takeoff.Year%10; + Name[Len++]=NameCode(Takeoff.Month); + Name[Len++]=NameCode(Takeoff.Day); + Name[Len++]='O'; + Len+=Format_String(Name+Len, Serial); + Name[Len++]=NameCode(TakeoffCount); + Len+=Format_String(Name+Len, ".IGC"); + Name[Len]=0; + // printf("ShortName[%d]: %s\n", Len, Name); + return Len; } + + int LongName(char *Name, const char *Serial) const + { int Len=0; + Name[Len]=0; return 0; } + + static int FlightThresh(const GPS_Position &Position, uint16_t MinSpeed) + { if(!Position.isValid()) return -1; + uint16_t Speed=Position.Speed; // [0.1m/s] + int16_t Climb=Position.ClimbRate; // [0.1m/s] + uint8_t DOP=Position.PDOP; if(DOP==0) DOP=Position.HDOP; // [0.1] + Speed+=4*abs(Climb); + if(DOP>10) { Speed = (Speed*10)/DOP; } + return Speed>=MinSpeed; } + + bool inFlight(void) const { return Takeoff.isValid() && !Landing.isValid(); } + + int Process(const GPS_Position &Position) + { Position.Print(); + if(inFlight()) + { int Det=FlightThresh(Position, MinSpeed/2); // printf("FlightThres() => %d\n", Det); + if(Det<=0) + { HoldTime++; + if(HoldTime>=2*MinHold) + { Landing=Position; HoldTime=0; + char Name[16]; ShortName(Name, "XXX"); + printf("Landing #%d: %s\n", TakeoffCount, Name); + } + } + else HoldTime=0; + } + else + { int Det=FlightThresh(Position, MinSpeed); // printf("FlightThres() => %d\n", Det); + if(Det>0) + { HoldTime++; + if(HoldTime>=MinHold) + { Takeoff=Position; TakeoffCount++; + Landing.Clear(); HoldTime=0; + char Name[16]; ShortName(Name, "XXX"); + printf("Takeoff #%d: %s\n", TakeoffCount, Name); + } + } + else HoldTime=0; + } + return inFlight(); } + +} ; + +// =========================================================================================== diff --git a/main/ogn.h b/main/ogn.h index d6f06d3..7dc0310 100644 --- a/main/ogn.h +++ b/main/ogn.h @@ -1307,6 +1307,36 @@ class GPS_Position { int16_t LatAngle = calcLatAngle16(Latitude); LatitudeCosine = calcLatCosine(LatAngle); } + static int WriteIGC(char *Out, int32_t Coord, uint8_t DegSize, const char *SignChar) + { int Len=0; + bool Neg = Coord<0; if(Neg) Coord=(-Coord); + int32_t Deg = Coord/600000; + Len+=Format_UnsDec(Out+Len, Deg, DegSize); + Coord-=Deg*600000; Coord/=10; + Len+=Format_UnsDec(Out+Len, Coord, 5); + Out[Len++]=SignChar[Neg]; + return Len; } + + int WriteIGC(char *Out) + { if(!isValid()) return 0; + int Len=0; + Out[Len++] = 'B'; + Len+=Format_UnsDec(Out+Len, Hour, 2); + Len+=Format_UnsDec(Out+Len, Min, 2); + Len+=Format_UnsDec(Out+Len, Sec, 2); + Len+=WriteIGC(Out+Len, Latitude, 2, "NS"); + Len+=WriteIGC(Out+Len, Longitude, 3, "EW"); + Out[Len++] = FixMode>2 ? 'A':'V'; + if(hasBaro) + { int32_t Alt = StdAltitude/10; // [m] + if(Alt<0) { Alt = (-Alt); Out[Len++] = '-'; Len+=Format_UnsDec(Out+Len, (uint32_t)Alt, 4); } + else { Len+=Format_UnsDec(Out+Len, (uint32_t)Alt, 5); } + } else Len+=Format_String(Out+Len, " "); + int32_t Alt = (Altitude+GeoidSeparation)/10; // [m] + if(Alt<0) { Alt = (-Alt); Out[Len++] = '-'; Len+=Format_UnsDec(Out+Len, (uint32_t)Alt, 4); } + else { Len+=Format_UnsDec(Out+Len, (uint32_t)Alt, 5); } + Out[Len]=0; return Len; } + private: int8_t ReadLatitude(char Sign, const char *Value) diff --git a/main/proc.cpp b/main/proc.cpp index 62bd5ef..c086e7e 100644 --- a/main/proc.cpp +++ b/main/proc.cpp @@ -13,6 +13,8 @@ #include "fifo.h" +#include "flight.h" // flight status + #ifdef WITH_FLASHLOG // log own track to unused Flash pages (STM32 only) #include "flashlog.h" #endif @@ -77,6 +79,8 @@ static char Line[128]; // for printing out to the console, etc. static LDPC_Decoder Decoder; // decoder and error corrector for the OGN Gallager/LDPC code +FlightMonitor Flight; + // #define DEBUG_PRINT // ======================================================================================================================================= @@ -505,6 +509,9 @@ void vTaskPROC(void* pvParameters) Format_String(CONS_UART_Write, "s\n"); xSemaphoreGive(CONS_Mutex); #endif + + Flight.Process(*Position); // flight monitor: takeoff/landing + #ifdef WITH_GDL90 GDL_HEARTBEAT.Clear(); GDL_HEARTBEAT.Initialized=1;