From a35330351f1f7313d0604f7d8bac24ea4372d6c2 Mon Sep 17 00:00:00 2001 From: Pawel Jalocha Date: Sat, 26 Sep 2020 02:00:23 +0100 Subject: [PATCH] Produce IGC files, B-records only for now --- main/flight.h | 14 +++-- main/gps.cpp | 147 ++++++++++++++++++++++++++----------------------- main/gps.h | 14 ++++- main/hal.h | 1 + main/main.cpp | 12 ++-- main/ogn.h | 6 +- main/proc.cpp | 4 +- main/proc.h | 4 +- main/sdlog.cpp | 117 ++++++++++++++++++++++++++++++++++++++- main/sdlog.h | 6 ++ 10 files changed, 233 insertions(+), 92 deletions(-) diff --git a/main/flight.h b/main/flight.h index 728c9ff..b5085a4 100644 --- a/main/flight.h +++ b/main/flight.h @@ -15,7 +15,7 @@ class FlightMonitor static const uint8_t MinHold = 5; // [sec] minimum hold time before takeoff declared static const uint16_t MinSpeed = 60; // [0.1m/s] minimum speed to trigger the takeoff uint8_t HoldTime; - uint8_t TakeoffCount; // count take-off/landing cycles for the IGC file name + // uint8_t TakeoffCount; // count take-off/landing cycles for the IGC file name public: @@ -26,7 +26,8 @@ class FlightMonitor // Recent.Clear(); // IGCpath=0; HoldTime=0; - TakeoffCount=0; } + // TakeoffCount=0; + } static char NameCode(int Num) // coding of numbers in IGC file names { if(Num<=0) return '0'; @@ -34,14 +35,17 @@ class FlightMonitor if(Num<36) return 'A'+(Num-10); return '_'; } - int ShortName(char *Name, const char *Serial) const // produce short IGC file name (a three-character Serial) + int ShortName(char *Name, uint8_t TakeoffNum, const char *Serial) const // produce short IGC file name (a three-character Serial) + { return ShortName(Name, Takeoff, TakeoffNum, Serial); } + + static int ShortName(char *Name, const GPS_Position &Takeoff, uint8_t TakeoffNum, const char *Serial) { int Len=0; Name[Len++]='0'+Takeoff.Year%10; // Year (last digit) Name[Len++]=NameCode(Takeoff.Month); // encoded month Name[Len++]=NameCode(Takeoff.Day); // encoded day Name[Len++]='O'; // OGN Len+=Format_String(Name+Len, Serial); // three-digit serial - Name[Len++]=NameCode(TakeoffCount); // flight of the day + Name[Len++]=NameCode(TakeoffNum); // flight of the day Len+=Format_String(Name+Len, ".IGC"); // extension Name[Len]=0; // printf("ShortName[%d]: %s\n", Len, Name); @@ -81,7 +85,7 @@ class FlightMonitor if(Det>0) // if criteria satisfied { HoldTime++; // count the holding time if(HoldTime>=MinHold) // if enough - { Takeoff=Position; TakeoffCount++; // declare takeoff + { Takeoff=Position; // TakeoffCount++; // declare takeoff Landing.Clear(); HoldTime=0; // clear the landing position // char Name[16]; ShortName(Name, "XXX"); // printf("Takeoff #%d: %s\n", TakeoffCount, Name); diff --git a/main/gps.cpp b/main/gps.cpp index cf96725..b5973e1 100644 --- a/main/gps.cpp +++ b/main/gps.cpp @@ -41,13 +41,13 @@ static UBX_RxMsg UBX; // UBX messages catcher static MAV_RxMsg MAV; // MAVlink message catcher #endif -uint16_t GPS_PosPeriod = 0; // [0.01s] time between succecive GPS readouts +uint16_t GPS_PosPeriod = 0; // [0.01s] time between succecive GPS readouts -// uint8_t GPS_PowerMode = 2; // 0=shutdown, 1=reduced power, 2=normal +// uint8_t GPS_PowerMode = 2; // 0=shutdown, 1=reduced power, 2=normal const uint8_t PosPipeIdxMask = GPS_PosPipeSize-1; -static uint8_t PosIdx; // Pipe index, increments with every GPS position received -static GPS_Position Position[GPS_PosPipeSize]; // GPS position pipe +uint8_t GPS_PosIdx; // Pipe index, increments with every GPS position received +GPS_Position GPS_Pos[GPS_PosPipeSize]; // GPS position pipe static TickType_t PPS_Tick; // [msec] System Tick when the PPS arrived static TickType_t Burst_Tick; // [msec] System Tick when the data burst from GPS started @@ -94,6 +94,10 @@ uint16_t MAVLINK_BattCurr = 0; // [10mA] uint8_t MAVLINK_BattCap = 0; // [%] #endif +EventGroupHandle_t GPS_Event = 0; + +FlightMonitor Flight; + // ---------------------------------------------------------------------------- static char GPS_Cmd[64]; @@ -158,7 +162,7 @@ int16_t GPS_AverageSpeed(void) // get average speed based { uint8_t Count=0; int16_t Speed=0; for(uint8_t Idx=0; IdxhasGPS || !Pos->isValid() ) continue; // skip invalid positions Speed += Pos->Speed +abs(Pos->ClimbRate); Count++; } @@ -363,7 +367,7 @@ static void GPS_Random_Update(uint8_t Bit) { GPS_Random = (GPS_Random<<1) | (Bit&1); } static void GPS_Random_Update(GPS_Position *Pos) -{ if(Position==0) return; +{ if(Pos==0) return; GPS_Random_Update(Pos->Altitude); GPS_Random_Update(Pos->Speed); GPS_Random_Update(Pos->Latitude); @@ -374,7 +378,7 @@ static void GPS_Random_Update(GPS_Position *Pos) static void GPS_BurstComplete(void) // when GPS has sent the essential data for position fix { #ifdef WITH_MAVLINK - GPS_Position *GPS = Position+PosIdx; + GPS_Position *GPS = GPS_Pos+GPS_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 @@ -386,7 +390,7 @@ static void GPS_BurstComplete(void) // wh } #endif #ifdef DEBUG_PRINT - Position[PosIdx].PrintLine(Line); // print out the GPS position in a single-line format + GPS_Pos[GPS_PosIdx].PrintLine(Line); // print out the GPS position in a single-line format xSemaphoreTake(CONS_Mutex, portMAX_DELAY); Format_UnsDec(CONS_UART_Write, TimeSync_Time()%60, 2); CONS_UART_Write('.'); @@ -394,19 +398,19 @@ static void GPS_BurstComplete(void) // wh Format_String(CONS_UART_Write, " -> GPS_BurstComplete() GPS:"); Format_Hex(CONS_UART_Write, GPS_Status.Flags); Format_String(CONS_UART_Write, "\nGPS["); - CONS_UART_Write('0'+PosIdx); CONS_UART_Write(']'); CONS_UART_Write(' '); + CONS_UART_Write('0'+GPS_PosIdx); CONS_UART_Write(']'); CONS_UART_Write(' '); Format_String(CONS_UART_Write, Line); xSemaphoreGive(CONS_Mutex); #endif - GPS_Random_Update(Position+PosIdx); - 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(); + GPS_Random_Update(GPS_Pos+GPS_PosIdx); + if(GPS_Pos[GPS_PosIdx].hasGPS) // GPS position data complete + { GPS_Pos[GPS_PosIdx].isReady=1; // mark this record as ready for processing => producing packets for transmission + if(GPS_Pos[GPS_PosIdx].isTimeValid()) // if time is valid already + { if(GPS_Pos[GPS_PosIdx].isDateValid()) // if date is valid as well + { uint32_t UnixTime=GPS_Pos[GPS_PosIdx].getUnixTime(); + GPS_FatTime=GPS_Pos[GPS_PosIdx].getFatTime(); #ifndef WITH_MAVLINK // with MAVlink we sync. with the SYSTEM_TIME message - int32_t msDiff = Position[PosIdx].FracSec; + int32_t msDiff = GPS_Pos[GPS_PosIdx].FracSec; if(msDiff>=50) { msDiff-=100; UnixTime++; } // [0.01s] msDiff*=10; // [ms] // if(abs(msDiff)<=200) // if (almost) full-second burst @@ -418,35 +422,35 @@ static void GPS_BurstComplete(void) // wh #endif } } - if(Position[PosIdx].isValid()) // position is complete and locked + if(GPS_Pos[GPS_PosIdx].isValid()) // position is complete and locked { if(Parameters.manGeoidSepar) // if GeoidSepar is "manual" - this implies the GPS does not correct for it - { Position[PosIdx].GeoidSeparation = Parameters.GeoidSepar; // copy the manually set GeoidSepar - Position[PosIdx].Altitude -= Parameters.GeoidSepar; } // correct the Altitude - we likely need a separate flag for this - Position[PosIdx].calcLatitudeCosine(); + { GPS_Pos[GPS_PosIdx].GeoidSeparation = Parameters.GeoidSepar; // copy the manually set GeoidSepar + GPS_Pos[GPS_PosIdx].Altitude -= Parameters.GeoidSepar; } // correct the Altitude - we likely need a separate flag for this + GPS_Pos[GPS_PosIdx].calcLatitudeCosine(); GPS_TimeSinceLock++; - GPS_Altitude=Position[PosIdx].Altitude; - GPS_Latitude=Position[PosIdx].Latitude; - GPS_Longitude=Position[PosIdx].Longitude; - GPS_GeoidSepar=Position[PosIdx].GeoidSeparation; - GPS_LatCosine=Position[PosIdx].LatitudeCosine; - // GPS_FreqPlan=Position[PosIdx].getFreqPlan(); + GPS_Altitude=GPS_Pos[GPS_PosIdx].Altitude; + GPS_Latitude=GPS_Pos[GPS_PosIdx].Latitude; + GPS_Longitude=GPS_Pos[GPS_PosIdx].Longitude; + GPS_GeoidSepar=GPS_Pos[GPS_PosIdx].GeoidSeparation; + GPS_LatCosine=GPS_Pos[GPS_PosIdx].LatitudeCosine; + // GPS_FreqPlan=GPS_Pos[GPS_PosIdx].getFreqPlan(); if(GPS_TimeSinceLock==1) // if we just acquired the lock a moment ago { GPS_LockStart(); } if(GPS_TimeSinceLock>1) // if the lock is more persistant - { uint8_t PrevIdx=(PosIdx+PosPipeIdxMask)&PosPipeIdxMask; - int16_t TimeDiff = Position[PosIdx].calcTimeDiff(Position[PrevIdx]); + { uint8_t PrevIdx=(GPS_PosIdx+PosPipeIdxMask)&PosPipeIdxMask; + int16_t TimeDiff = GPS_Pos[GPS_PosIdx].calcTimeDiff(GPS_Pos[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]); + if(PrevIdx2==GPS_PosIdx) break; + if(!GPS_Pos[PrevIdx2].isValid()) break; + TimeDiff = GPS_Pos[GPS_PosIdx].calcTimeDiff(GPS_Pos[PrevIdx2]); PrevIdx=PrevIdx2; } - TimeDiff=Position[PosIdx].calcDifferentials(Position[PrevIdx]); + TimeDiff=GPS_Pos[GPS_PosIdx].calcDifferentials(GPS_Pos[PrevIdx]); #ifdef DEBUG_PRINT xSemaphoreTake(CONS_Mutex, portMAX_DELAY); Format_String(CONS_UART_Write, "calcDiff() => "); - Format_UnsDec(CONS_UART_Write, (uint16_t)PosIdx); + Format_UnsDec(CONS_UART_Write, (uint16_t)GPS_PosIdx); Format_String(CONS_UART_Write, "->"); Format_UnsDec(CONS_UART_Write, (uint16_t)PrevIdx); CONS_UART_Write(' '); @@ -461,7 +465,7 @@ static void GPS_BurstComplete(void) // wh } // #ifdef WITH_MAVLINK // static MAV_GPS_RAW_INT MAV_Position; -// Position[PosIdx].Encode(MAV_Position); +// GPS_Pos[GPS_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); @@ -470,31 +474,33 @@ static void GPS_BurstComplete(void) // wh else // posiiton not complete, no GPS lock { if(GPS_TimeSinceLock) { GPS_LockEnd(); GPS_TimeSinceLock=0; } } - uint8_t NextPosIdx = (PosIdx+1)&PosPipeIdxMask; // next position to be recorded - if( Position[PosIdx].isTimeValid() && Position[NextPosIdx].isTimeValid() ) - { int32_t Period = Position[PosIdx].calcTimeDiff(Position[NextPosIdx]); + uint8_t NextPosIdx = (GPS_PosIdx+1)&PosPipeIdxMask; // next position to be recorded + if( GPS_Pos[GPS_PosIdx].isTimeValid() && GPS_Pos[NextPosIdx].isTimeValid() ) + { int32_t Period = GPS_Pos[GPS_PosIdx].calcTimeDiff(GPS_Pos[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["); - CONS_UART_Write('0'+PosIdx); CONS_UART_Write(']'); CONS_UART_Write(' '); - Format_UnsDec(CONS_UART_Write, (uint16_t)Position[PosIdx].Sec, 2); + CONS_UART_Write('0'+GPS_PosIdx); CONS_UART_Write(']'); CONS_UART_Write(' '); + Format_UnsDec(CONS_UART_Write, (uint16_t)GPS_Pos[GPS_PosIdx].Sec, 2); CONS_UART_Write('.'); - Format_UnsDec(CONS_UART_Write, (uint16_t)Position[PosIdx].FracSec, 2); + Format_UnsDec(CONS_UART_Write, (uint16_t)GPS_Pos[GPS_PosIdx].FracSec, 2); Format_String(CONS_UART_Write, "s "); Format_UnsDec(CONS_UART_Write, GPS_PosPeriod, 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; // + GPS_Pos[NextPosIdx].Clear(); // clear the next position + // int8_t Sec = GPS_Pos[GPS_PosIdx].Sec; // // Sec++; if(Sec>=60) Sec=0; - // Position[NextPosIdx].Sec=Sec; // set the correct time for the next position - Position[NextPosIdx].copyTime(Position[PosIdx]); // copy time from current position - Position[NextPosIdx].incrTime(); // increment time by 1 sec - // Position[NextPosIdx].copyDate(Position[PosIdx]); - PosIdx=NextPosIdx; // advance the index + // GPS_Pos[NextPosIdx].Sec=Sec; // set the correct time for the next position + GPS_Pos[NextPosIdx].copyTime(GPS_Pos[GPS_PosIdx]); // copy time from current position + GPS_Pos[NextPosIdx].incrTime(); // increment time by 1 sec + Flight.Process(GPS_Pos[GPS_PosIdx]); + // GPS_Pos[NextPosIdx].copyDate(GPS_Pos[GPS_PosIdx]); + GPS_PosIdx=NextPosIdx; // advance the index + xEventGroupSetBits(GPS_Event, GPSevt_NewPos); } static void GPS_BurstEnd(void) // when GPS stops sending the data on the serial port @@ -506,28 +512,28 @@ GPS_Position *GPS_getPosition(uint8_t &BestIdx, int16_t &BestRes, int8_t Sec, in { int16_t TargetTime = Frac+(int16_t)Sec*100; // target time including the seconds BestIdx=0; BestRes=0x7FFF; for(uint8_t Idx=0; IdxisReady) continue; // skip those not-ready yet int16_t Diff = TargetTime - (Pos->FracSec + (int16_t)Pos->Sec*100); // difference from the target time if(Diff<(-3000)) Diff+=6000; // wrap-around 60 sec else if(Diff>=3000) Diff-=6000; if(abs(Diff)isReady) return PrevPos; PrevIdx=(PrevIdx+PosPipeIdxMask)&PosPipeIdxMask; - PrevPos = Position+PrevIdx; + PrevPos = GPS_Pos+PrevIdx; 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=50) { PosSec++; if(PosSec>=60) PosSec-=60; } - if(Sec==PosSec) return Position+Idx; } + { int8_t PosSec = GPS_Pos[Idx].Sec; if(GPS_Pos[Idx].FracSec>=50) { PosSec++; if(PosSec>=60) PosSec-=60; } + if(Sec==PosSec) return GPS_Pos+Idx; } return 0; } // ---------------------------------------------------------------------------- @@ -537,7 +543,7 @@ static void GPS_NMEA(void) // wh GPS_Status.BaudConfig = (GPS_getBaudRate() == GPS_TargetBaudRate); LED_PCB_Flash(10); // Flash the LED for 2 ms if(NMEA.isGxGSV()) ProcessGSV(NMEA); // process satellite data - Position[PosIdx].ReadNMEA(NMEA); // read position elements from NMEA + GPS_Pos[GPS_PosIdx].ReadNMEA(NMEA); // read position elements from NMEA if(NMEA.isGxRMC()) GPS_Burst.GxRMC=1; if(NMEA.isGxGGA()) GPS_Burst.GxGGA=1; if(NMEA.isGxGSA()) GPS_Burst.GxGSA=1; @@ -593,7 +599,7 @@ static void GPS_UBX(void) GPS_Status.BaudConfig = (GPS_getBaudRate() == GPS_TargetBaudRate); LED_PCB_Flash(10); // DumpUBX(); - // Position[PosIdx].ReadUBX(UBX); + // GPS_Pos[GPS_PosIdx].ReadUBX(UBX); #ifdef WITH_GPS_UBX_PASS { xSemaphoreTake(CONS_Mutex, portMAX_DELAY); // send ther UBX packet to the console UBX.Send(CONS_UART_Write); @@ -719,8 +725,8 @@ static void GPS_MAV(void) // wh uint8_t MsgID = MAV.getMsgID(); uint64_t UnixTime_ms = MAV_getUnixTime(); // get the time from the MAVlink message if( (MsgID!=MAV_ID_SYSTEM_TIME) && UnixTime_ms) - { if(Position[PosIdx].hasTime) - { uint64_t PrevUnixTime_ms = Position[PosIdx].getUnixTime_ms(); + { if(GPS_Pos[GPS_PosIdx].hasTime) + { uint64_t PrevUnixTime_ms = GPS_Pos[GPS_PosIdx].getUnixTime_ms(); int32_t TimeDiff_ms = UnixTime_ms-PrevUnixTime_ms; #ifdef DEBUG_PRINT xSemaphoreTake(CONS_Mutex, portMAX_DELAY); @@ -761,21 +767,21 @@ static void GPS_MAV(void) // wh #endif } else if(MsgID==MAV_ID_GLOBAL_POSITION_INT) // position based on GPS and inertial sensors { const MAV_GLOBAL_POSITION_INT *Pos = (const MAV_GLOBAL_POSITION_INT *)MAV.getPayload(); - Position[PosIdx].Read(Pos, UnixTime_ms); // read position/altitude/speed/etc. into GPS_Position structure + GPS_Pos[GPS_PosIdx].Read(Pos, UnixTime_ms); // read position/altitude/speed/etc. into GPS_Position structure #ifdef DEBUG_PRINT - Position[PosIdx].PrintLine(Line); + GPS_Pos[GPS_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, "\nGPS"); CONS_UART_Write('0'+PosIdx); CONS_UART_Write(':'); CONS_UART_Write(' '); + Format_String(CONS_UART_Write, "\nGPS"); CONS_UART_Write('0'+GPS_PosIdx); CONS_UART_Write(':'); CONS_UART_Write(' '); Format_String(CONS_UART_Write, Line); xSemaphoreGive(CONS_Mutex); #endif } else if(MsgID==MAV_ID_GPS_RAW_INT) // position form the GPS { const MAV_GPS_RAW_INT *RawGPS = (const MAV_GPS_RAW_INT *)MAV.getPayload(); - Position[PosIdx].Read(RawGPS, UnixTime_ms); // read position/altitude/speed/etc. into GPS_Position structure + GPS_Pos[GPS_PosIdx].Read(RawGPS, UnixTime_ms); // read position/altitude/speed/etc. into GPS_Position structure #ifdef DEBUG_PRINT - Position[PosIdx].PrintLine(Line); + GPS_Pos[GPS_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); @@ -784,20 +790,20 @@ static void GPS_MAV(void) // wh CONS_UART_Write(' '); Format_SignDec(CONS_UART_Write, TimeDiff, 4, 3); CONS_UART_Write(abs(TimeDiff)<250 ? '*':' '); - Format_String(CONS_UART_Write, "\nGPS"); CONS_UART_Write('0'+PosIdx); CONS_UART_Write(':'); CONS_UART_Write(' '); + Format_String(CONS_UART_Write, "\nGPS"); CONS_UART_Write('0'+GPS_PosIdx); CONS_UART_Write(':'); CONS_UART_Write(' '); 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); + GPS_Pos[GPS_PosIdx].Read(Press, UnixTime_ms); #ifdef DEBUG_PRINT - Position[PosIdx].PrintLine(Line); + GPS_Pos[GPS_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, "\nGPS"); CONS_UART_Write('0'+PosIdx); CONS_UART_Write(':'); CONS_UART_Write(' '); + Format_String(CONS_UART_Write, "\nGPS"); CONS_UART_Write('0'+GPS_PosIdx); CONS_UART_Write(':'); CONS_UART_Write(' '); Format_String(CONS_UART_Write, Line); xSemaphoreGive(CONS_Mutex); #endif @@ -868,6 +874,7 @@ static void GPS_MAV(void) // wh #endif void vTaskGPS(void* pvParameters) { + GPS_Event = xEventGroupCreate(); GPS_Status.Flags = 0; // PPS_TickCount=0; @@ -892,8 +899,8 @@ void vTaskGPS(void* pvParameters) MAV.Clear(); #endif for(uint8_t Idx=0; Idx<4; Idx++) - Position[Idx].Clear(); - PosIdx=0; + GPS_Pos[Idx].Clear(); + GPS_PosIdx=0; TickType_t RefTick = xTaskGetTickCount(); for( ; ; ) // main task loop: every milisecond (RTOS time tick) diff --git a/main/gps.h b/main/gps.h index 86bd25f..9e0e3db 100644 --- a/main/gps.h +++ b/main/gps.h @@ -2,10 +2,11 @@ #include "hal.h" #include "timesync.h" - #include "ogn.h" - #include "lowpass2.h" +#include "flight.h" + +// extern uint8_t GPS_PowerMode; // 0=shutdown, 1=reduced, 2=normal #ifdef WITH_ESP32 const uint8_t GPS_PosPipeSize = 8; // number of GPS positions held in a pipe @@ -13,7 +14,8 @@ const uint8_t GPS_PosPipeSize = 8; // number of GPS positions held in const uint8_t GPS_PosPipeSize = 4; // number of GPS positions held in a pipe #endif -// extern uint8_t GPS_PowerMode; // 0=shutdown, 1=reduced, 2=normal +extern uint8_t GPS_PosIdx; // Pipe index, increments with every GPS position received +extern GPS_Position GPS_Pos[GPS_PosPipeSize]; // GPS position 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) @@ -56,6 +58,12 @@ extern uint16_t MAVLINK_BattCurr; // [10mA] extern uint8_t MAVLINK_BattCap; // [%] #endif +extern EventGroupHandle_t GPS_Event; +const EventBits_t GPSevt_PPS = 0x01; +const EventBits_t GPSevt_NewPos = 0x02; + +extern FlightMonitor Flight; + #ifdef __cplusplus extern "C" #endif diff --git a/main/hal.h b/main/hal.h index 03ca8e2..ccb6e00 100644 --- a/main/hal.h +++ b/main/hal.h @@ -7,6 +7,7 @@ #include "freertos/semphr.h" #include "freertos/task.h" #include "freertos/queue.h" +#include "freertos/event_groups.h" #include "fifo.h" diff --git a/main/main.cpp b/main/main.cpp index af60249..a0d8c12 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -76,15 +76,18 @@ void app_main(void) } #endif - xTaskCreate(vTaskRF, "RF", 2048, 0, tskIDLE_PRIORITY+4, 0); -#ifdef WITH_LOG - xTaskCreate(vTaskLOG , "LOG", 4096, 0, tskIDLE_PRIORITY+1, 0); -#endif #ifdef WITH_SDLOG Log_Mutex = xSemaphoreCreateMutex(); xTaskCreate(vTaskSDLOG, "SDLOG", 4096, 0, tskIDLE_PRIORITY+1, 0); #endif + +#ifdef WITH_LOG + xTaskCreate(vTaskLOG , "LOG", 4096, 0, tskIDLE_PRIORITY+1, 0); +#endif + + xTaskCreate(vTaskRF, "RF", 2048, 0, tskIDLE_PRIORITY+4, 0); xTaskCreate(vTaskPROC, "PROC", 2048, 0, tskIDLE_PRIORITY+3, 0); + xTaskCreate(vTaskGPS, "GPS", 2048, 0, tskIDLE_PRIORITY+4, 0); #if defined(WITH_BMP180) || defined(WITH_BMP280) || defined(WITH_BME280) || defined(WITH_MS5607) || defined(WITH_MS5611) xTaskCreate(vTaskSENS, "SENS", 2048, 0, tskIDLE_PRIORITY+4, 0); @@ -92,6 +95,7 @@ void app_main(void) #ifdef WITH_BMX055 xTaskCreate(vTaskIMU, "IMU", 2048, 0, tskIDLE_PRIORITY+4, 0); #endif + #ifdef WITH_KNOB xTaskCreate(vTaskKNOB, "KNOB", 2048, 0, tskIDLE_PRIORITY+3, 0); #endif diff --git a/main/ogn.h b/main/ogn.h index 5b44d87..007f4c1 100644 --- a/main/ogn.h +++ b/main/ogn.h @@ -1455,13 +1455,13 @@ class GPS_Position: public GPS_Time Out[Len++]=SignChar[Neg]; return Len; } - int WriteHHMMSS(char *Out) + int WriteHHMMSS(char *Out) const { Format_UnsDec(Out , Hour, 2); Format_UnsDec(Out+2, Min , 2); Format_UnsDec(Out+4, Sec , 2); return 6; } - int WriteIGC(char *Out) + int WriteIGC(char *Out) const { // if(!isValid()) return 0; int Len=0; Out[Len++] = 'B'; @@ -1482,7 +1482,7 @@ class GPS_Position: public GPS_Time 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, " "); - Out[Len]=0; return Len; } + Out[Len++]='\n'; Out[Len]=0; return Len; } private: diff --git a/main/proc.cpp b/main/proc.cpp index ea68822..96e6d17 100644 --- a/main/proc.cpp +++ b/main/proc.cpp @@ -77,7 +77,7 @@ 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; +// FlightMonitor Flight; // #define DEBUG_PRINT @@ -531,7 +531,7 @@ void vTaskPROC(void* pvParameters) #endif if(Position) { Position->EncodeStatus(StatPacket.Packet); // encode GPS altitude and pressure/temperature/humidity - Flight.Process(*Position); } // flight monitor: takeoff/landing + /* Flight.Process(*Position); */ } // flight monitor: takeoff/landing else { StatPacket.Packet.Status.FixQuality=0; StatPacket.Packet.Status.Satellites=0; } // or lack of the GPS lock diff --git a/main/proc.h b/main/proc.h index 1e0a95d..55df5fe 100644 --- a/main/proc.h +++ b/main/proc.h @@ -1,4 +1,4 @@ -#include "flight.h" +// #include "flight.h" #ifdef WITH_LOOKOUT // traffic awareness and warnings #include "lookout.h" @@ -8,7 +8,7 @@ extern LookOut Look; extern uint32_t BatteryVoltage; // [1/256 mV] averaged extern int32_t BatteryVoltageRate; // [1/256 mV] averaged -extern FlightMonitor Flight; +// extern FlightMonitor Flight; #ifdef WITH_ESP32 const uint8_t RelayQueueSize = 32; diff --git a/main/sdlog.cpp b/main/sdlog.cpp index cef7ef8..436464e 100644 --- a/main/sdlog.cpp +++ b/main/sdlog.cpp @@ -11,6 +11,8 @@ #include "timesync.h" #include "fifo.h" +// ============================================================================================ + static char LogFileName[32]; static FILE *LogFile = 0; @@ -74,6 +76,96 @@ static int WriteLog(size_t MaxBlock=FIFOsize/2) // process th Count+=Write; } return Count; } +// ============================================================================================ + +const char *IGC_Path = "/sdcard/IGC"; +const int IGC_PathLen = 11; +// constexpr int IGC_PathLen = strlen(IGC_Path); +const char *IGC_Serial = "XXX"; + char IGC_FileName[32]; +static FILE *IGC_File=0; + uint8_t IGC_FlightNum=0; + +static void IGC_Close(void) +{ if(IGC_File) // if a file open, then close it and increment the flight number + { xSemaphoreTake(CONS_Mutex, portMAX_DELAY); + Format_String(CONS_UART_Write, "IGC_Close: "); + Format_String(CONS_UART_Write, IGC_FileName); + Format_String(CONS_UART_Write, "\n"); + xSemaphoreGive(CONS_Mutex); + fclose(IGC_File); IGC_File=0; IGC_FlightNum++; } +} + +static int IGC_Open(void) +{ IGC_Close(); // close the previous file, if open + if(!SD_isMounted()) return -1; // -1 => SD not mounted + memcpy(IGC_FileName, IGC_Path, IGC_PathLen); // copy path + IGC_FileName[IGC_PathLen]='/'; // slash after the path + Flight.ShortName(IGC_FileName+IGC_PathLen+1, IGC_FlightNum, IGC_Serial); // full name + IGC_File=fopen(IGC_FileName, "rt"); // attempt to open for read, just to try if the file is already there + if(IGC_File) { IGC_Close(); return -2; } // -2 => file already exists + IGC_File=fopen(IGC_FileName, "wt"); // open for write + if(IGC_File==0) // failed: maybe sub-dir does not exist ? + { if(mkdir(IGC_Path, 0777)<0) return -3; // -3 => can't create sub-dir + IGC_File=fopen(IGC_FileName, "wt"); } // retry to open for write + if(IGC_File) + { xSemaphoreTake(CONS_Mutex, portMAX_DELAY); + Format_String(CONS_UART_Write, "IGC_Open: "); + Format_String(CONS_UART_Write, IGC_FileName); + Format_String(CONS_UART_Write, "\n"); + xSemaphoreGive(CONS_Mutex); } + return IGC_File ? 0:-4; } // -4 => can't open for write + +static char Line[192]; + +static int IGC_Header(void) // write the top of the IGC file +{ + return 0; } + +static int IGC_Log(const GPS_Position &Pos) // log GPS position as B-record +{ int Len=Pos.WriteIGC(Line); +#ifdef DEBUG_PRINT + xSemaphoreTake(CONS_Mutex, portMAX_DELAY); + Format_String(CONS_UART_Write, Line); + xSemaphoreGive(CONS_Mutex); +#endif + int Written=fwrite(Line, 1, Len, IGC_File); + if(Written!=Len) IGC_Close(); + return Written; } + +static void IGC_Check(void) // check if new GPS position +{ static uint8_t PrevPosIdx=0; + if(GPS_PosIdx==PrevPosIdx) return; + PrevPosIdx=GPS_PosIdx; + const uint8_t PosPipeIdxMask = GPS_PosPipeSize-1; // get the GPS position just before in the pipe + uint8_t PosIdx = GPS_PosIdx-1; PosIdx&=PosPipeIdxMask; + bool inFlight = Flight.inFlight(); // in-flight or on-the-ground ? +#ifdef DEBUG_PRINT + GPS_Pos[PosIdx].PrintLine(Line); + xSemaphoreTake(CONS_Mutex, portMAX_DELAY); + Format_String(CONS_UART_Write, "IGC_Check() ["); + CONS_UART_Write('0'+GPS_PosIdx); + CONS_UART_Write(inFlight?'^':'_'); + Format_String(CONS_UART_Write, "] "); + Format_String(CONS_UART_Write, Line); + xSemaphoreGive(CONS_Mutex); +#endif + if(IGC_File) // if IGC file already open + { IGC_Log(GPS_Pos[PosIdx]); // log position + if(!inFlight) // if no longer in flight + { IGC_Close(); } // then close the IGC file + } + else // if IGC file is not open + { if(inFlight) // and in-flight + { for(int Try=0; Try<8; Try++) + { int Err=IGC_Open(); if(Err!=(-2)) break; } // try to open a new IGC file but don't overwrite the old ones + if(IGC_File) { IGC_Header(); IGC_Log(GPS_Pos[PosIdx]); } // if open succesfully then write header and first B-record + } + } +} + +// ============================================================================================ + #ifdef WITH_SDLOG extern "C" @@ -82,17 +174,36 @@ extern "C" for( ; ; ) { if(!SD_isMounted()) // if SD ia not mounted: - { vTaskDelay(5000); SD_Mount(); continue; } // try to (Re)mount it after a delay of 5sec + { vTaskDelay(5000); SD_Mount(); IGC_Check(); continue; } // try to (Re)mount it after a delay of 5sec + + // if(GPS_Event) + // { EventBits_t GPSevt = xEventGroupWaitBits(GPS_Event, GPSevt_NewPos, pdTRUE, pdFALSE, 100); + // if(GPSevt&GPSevt_NewPos) LogIGC(); } + if(!LogFile) // when SD mounted and log file not open: { Log_Open(); // try to (re)open it - if(!LogFile) { SD_Unmount(); vTaskDelay(1000); continue; } // if can't be open then unmount the SD and retry at a delay of 1sec + if(!LogFile) { IGC_Check(); SD_Unmount(); vTaskDelay(1000); continue; } // if can't be open then unmount the SD and retry at a delay of 1sec } - if(Log_FIFO.Full()0); // write the console output to the log file if(Write<0) { SD_Unmount(); vTaskDelay(1000); continue; } // if write fails then unmount the SD card and (re)try after a delay of 1sec // if(Write==0) vTaskDelay(100); + IGC_Check(); Log_Check(); } // make sure the log is well saved by regular close-reopen } +/* +extern "C" + void vTaskIGC(void* pvParameters) +{ + for( ; ; ) + { EventBits_t GPSevt = xEventGroupWaitBits(GPS_Event, GPSevt_NewPos, pdTRUE, pdFALSE, 2000); + if((GPSevt&GPSevt_NewPos)==0) continue; + + } +} +*/ + #endif // WITH_SDLOG diff --git a/main/sdlog.h b/main/sdlog.h index 4b5a73e..9ac8deb 100644 --- a/main/sdlog.h +++ b/main/sdlog.h @@ -14,4 +14,10 @@ extern SemaphoreHandle_t Log_Mutex; #endif void vTaskSDLOG(void* pvParameters); +/* +#ifdef __cplusplus + extern "C" +#endif + void vTaskIGC(void* pvParameters); +*/ #endif // __SDLOG_H__