diff --git a/main/gps.cpp b/main/gps.cpp index bca3c37..741a465 100644 --- a/main/gps.cpp +++ b/main/gps.cpp @@ -67,7 +67,7 @@ static union } ; } GPS_Burst; // for the autobaud on the GPS port -const int GPS_BurstTimeout = 200; // [ms] +const int GPS_BurstTimeout = 220; // [ms] static const uint8_t BaudRates=7; // number of possible baudrates choices static uint8_t BaudRateIdx=0; // actual choice @@ -166,7 +166,7 @@ static void GPS_BurstStart(void) // wh Format_UnsDec(CONS_UART_Write, TimeSync_Time()%60); CONS_UART_Write('.'); Format_UnsDec(CONS_UART_Write, TimeSync_msTime(),3); - Format_String(CONS_UART_Write, " -> GPS...\n"); + Format_String(CONS_UART_Write, " -> GPS_BurstStart()\n"); xSemaphoreGive(CONS_Mutex); #endif #ifdef WITH_GPS_CONFIG @@ -237,7 +237,8 @@ static void GPS_BurstComplete(void) // wh 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, " -> GPS_BurstComplete()\nGPS"); + CONS_UART_Write('0'+PosIdx); CONS_UART_Write(':'); CONS_UART_Write(' '); Format_String(CONS_UART_Write, Line); xSemaphoreGive(CONS_Mutex); #endif @@ -307,9 +308,8 @@ static void GPS_BurstComplete(void) // wh 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_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('.'); Format_UnsDec(CONS_UART_Write, (uint16_t)Position[PosIdx].FracSec, 2); @@ -321,11 +321,12 @@ static void GPS_BurstComplete(void) // wh } Position[NextPosIdx].Clear(); // clear the next position - int8_t Sec = Position[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 + // int8_t Sec = Position[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 } @@ -358,7 +359,8 @@ GPS_Position *GPS_getPosition(void) // ret 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; } return 0; } // ---------------------------------------------------------------------------- @@ -379,7 +381,7 @@ static void GPS_NMEA(void) // wh Format_String(CONS_UART_Write, " -> "); Format_Bytes(CONS_UART_Write, NMEA.Data, 6); CONS_UART_Write(' '); Format_Hex(CONS_UART_Write, GPS_Burst.Flags); - CONS_UART_Write('\r'); CONS_UART_Write('\n'); + CONS_UART_Write('\n'); xSemaphoreGive(CONS_Mutex); #endif if( NMEA.isP() || NMEA.isGxRMC() || NMEA.isGxGGA() || NMEA.isGxGSA() || NMEA.isGPTXT() ) @@ -491,13 +493,13 @@ static void GPS_MAV(void) // wh GPS_Status.MAV=1; LED_PCB_Flash(2); GPS_Status.BaudConfig = (GPS_getBaudRate() == GPS_TargetBaudRate); - int32_t TimeCorr_ms = (int32_t)Parameters.TimeCorr*1000; // apparently ArduPilot needs some time correction, as it "manually" converts from GPS to UTC time + int32_t TimeCorr_ms = (int32_t)Parameters.TimeCorr*1000; // [ms] apparently ArduPilot needs some time correction, as it "manually" converts from GPS to UTC time uint8_t MsgID = MAV.getMsgID(); if(MsgID==MAV_ID_HEARTBEAT) { const MAV_HEARTBEAT *Heartbeat = (const MAV_HEARTBEAT *)MAV.getPayload(); #ifdef DEBUG_PRINT xSemaphoreTake(CONS_Mutex, portMAX_DELAY); - Format_String(CONS_UART_Write, "MAV_HEARBEAT: "); + Format_String(CONS_UART_Write, "MAV_HEARTBEAT: "); Format_Hex(CONS_UART_Write, Heartbeat->system_status); Format_String(CONS_UART_Write, "\n"); xSemaphoreGive(CONS_Mutex); @@ -520,25 +522,25 @@ static void GPS_MAV(void) // wh Format_String(CONS_UART_Write, "\n"); xSemaphoreGive(CONS_Mutex); #endif - } else if(MsgID==MAV_ID_GLOBAL_POSITION_INT) + } 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(); - uint64_t UnixTime_ms = Pos->time_boot_ms + MAV_TimeOfs_ms; - Position[PosIdx].Read(Pos, UnixTime_ms); + uint64_t UnixTime_ms = Pos->time_boot_ms + MAV_TimeOfs_ms; // [ms] convert time-since-boot to UTC + Position[PosIdx].Read(Pos, UnixTime_ms); // read position/altitude/speed/etc. into GPS_Position structure #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, "\nGPS"); CONS_UART_Write('0'+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) + } else if(MsgID==MAV_ID_GPS_RAW_INT) // position form the GPS { MAV_GPS_RAW_INT *RawGPS = (MAV_GPS_RAW_INT *)MAV.getPayload(); - uint64_t UnixTime_ms = RawGPS->time_usec/1000; - if(UnixTime_ms<1000000000000) UnixTime_ms += MAV_TimeOfs_ms; + uint64_t UnixTime_ms = RawGPS->time_usec/1000; // [ms] UTC time, but for APM this is time-since-boot + if(UnixTime_ms<1000000000000) UnixTime_ms += MAV_TimeOfs_ms; // heuristic check and convert to true UTC // RawGPS->time_usec += (int64_t)MAV_TimeOfs_ms*1000; - Position[PosIdx].Read(RawGPS, UnixTime_ms); + Position[PosIdx].Read(RawGPS, UnixTime_ms); // read position/altitude/speed/etc. into GPS_Position structure #ifdef DEBUG_PRINT Position[PosIdx].PrintLine(Line); uint32_t UnixTime = (UnixTime_ms+500)/1000; @@ -549,7 +551,7 @@ 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, "\n"); + Format_String(CONS_UART_Write, "\nGPS"); CONS_UART_Write('0'+PosIdx); CONS_UART_Write(':'); CONS_UART_Write(' '); Format_String(CONS_UART_Write, Line); xSemaphoreGive(CONS_Mutex); #endif @@ -562,7 +564,7 @@ static void GPS_MAV(void) // wh 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, "\nGPS"); CONS_UART_Write('0'+PosIdx); CONS_UART_Write(':'); CONS_UART_Write(' '); Format_String(CONS_UART_Write, Line); xSemaphoreGive(CONS_Mutex); #endif diff --git a/main/gps.h b/main/gps.h index 5a90cd2..e9ec752 100644 --- a/main/gps.h +++ b/main/gps.h @@ -7,7 +7,7 @@ #include "lowpass2.h" -const uint8_t GPS_PosPipeSize = 4; // number of GPS positions held in a pipe +const uint8_t GPS_PosPipeSize = 4; // number of GPS positions held in a pipe, shall we do more for MAVlink which sends data more often ? 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) diff --git a/main/mavlink.h b/main/mavlink.h index f2b875d..589b355 100644 --- a/main/mavlink.h +++ b/main/mavlink.h @@ -160,13 +160,13 @@ class MAV_ADSB_VEHICLE // this message is sent by ADS-B or other traffic rece union { uint16_t flags; // validity: 1=coord. 2=alt. 4=heading 8=velocity 16=callsign 32=squawk 64=simulated struct - { bool CoordValid:1; - bool AltValid:1; - bool HeadingValid:1; - bool CallsignValid:1; - bool VelocityValid:1; - bool SquawkValid:1; - bool isSimulated:1; + { bool CoordValid:1; // #0 + bool AltValid:1; // #1 + bool HeadingValid:1; // #2 + bool CallsignValid:1; // #3 + bool VelocityValid:1; // #4 + bool SquawkValid:1; // #5 + bool isSimulated:1; // #6 } ; } ; uint16_t squawk; diff --git a/main/ogn.h b/main/ogn.h index 06a8254..c2de5f6 100644 --- a/main/ogn.h +++ b/main/ogn.h @@ -242,18 +242,24 @@ class OGN_Packet // Packet structure for the OGN tracker } 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; } + { MAV->ICAO_address = Header.Address; + MAV->lat = ((int64_t)50*DecodeLatitude()+1)/3; // convert coordinates to [1e-7deg] + MAV->lon = ((int64_t)50*DecodeLongitude()+1)/3; + MAV->altitude = 1000*DecodeAltitude(); // convert to [mm[ + MAV->heading = 10*DecodeHeading(); // [cdeg/s] + MAV->hor_velocity = 10*DecodeSpeed(); // [cm/s] + MAV->ver_velocity = 10*DecodeClimbRate(); // [cm/s] + MAV->flags = 0x1F; // all valid except for Squawk, not simulated + MAV->altitude_type = 1; // GPS altitude + static char Prefix[4] = { 'R', 'I', 'F', 'O' }; // prefix for Random, ICAO, Flarm and OGN address-types + MAV->callsign[0] = Prefix[Header.AddrType]; // create a call-sign from address-type and address + Format_Hex((char *)MAV->callsign+1, ( uint8_t)(Header.Address>>16)); // highest byte + Format_Hex((char *)MAV->callsign+3, (uint16_t)(Header.Address&0xFFFF)); // two lower bytes + MAV->callsign[7] = 0; // end-of-string for call-sign + MAV->squawk = 0; // what shall we put there for OGN ? + MAV->tslc = 1; // 1sec for now but should be more precise + MAV->emiter_type = 0; // could be more precise + } int8_t ReadAPRS(const char *Msg) // read an APRS position message { Clear(); @@ -1297,7 +1303,7 @@ class GPS_Position 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 hasTime :1; // full Unix Date/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 @@ -1432,7 +1438,7 @@ class GPS_Position int PrintLine(char *Out) const { int Len=0; // PrintDateTime(Out); - Out[Len++]=hasTime?'T':'_'; + Out[Len++]=hasTime?'U':'_'; Out[Len++]=hasGPS ?'G':'_'; Out[Len++]=hasBaro?'B':'_'; Out[Len++]=hasRMC ?'R':'_'; diff --git a/main/proc.cpp b/main/proc.cpp index 51a7fee..feb960f 100644 --- a/main/proc.cpp +++ b/main/proc.cpp @@ -106,6 +106,29 @@ static void ReadStatus(OGN_TxPacket &StatPacket) // r // --------------------------------------------------------------------------------------------------------------------------------------- +static uint8_t WritePFLAU(char *NMEA, uint8_t GPS=1) // produce the (mostly dummy) PFLAU to satisfy XCsoar and LK8000 +{ uint8_t Len=0; + Len+=Format_String(NMEA+Len, "$PFLAU,"); + NMEA[Len++]='0'; + NMEA[Len++]=','; + NMEA[Len++]='0'+GPS; // TX status + NMEA[Len++]=','; + NMEA[Len++]='0'+GPS; // GPS status + NMEA[Len++]=','; + NMEA[Len++]='1'; // power status: one could monitor the supply + NMEA[Len++]=','; + NMEA[Len++]='0'; + NMEA[Len++]=','; + NMEA[Len++]=','; + NMEA[Len++]='0'; + NMEA[Len++]=','; + NMEA[Len++]=','; + Len+=NMEA_AppendCheckCRNL(NMEA, Len); + NMEA[Len]=0; + return Len; } + +// --------------------------------------------------------------------------------------------------------------------------------------- + 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 @@ -284,6 +307,12 @@ void vTaskPROC(void* pvParameters) if( isMoving || ((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_PFLAA + { uint8_t Len=WritePFLAU(Line); + xSemaphoreTake(CONS_Mutex, portMAX_DELAY); + Format_String(CONS_UART_Write, Line, 0, Len); + xSemaphoreGive(CONS_Mutex); } +#endif // WITH_PFLAA #ifdef WITH_FLASHLOG bool Written=FlashLog_Process(PosPacket.Packet, PosTime); // if(Written) @@ -310,24 +339,24 @@ void vTaskPROC(void* pvParameters) RF_TxFIFO.Write(); // complete the write into the TxFIFO if(Position) Position->Sent=1; } -#ifdef WITH_MAVLINK - { MAV_HEARTBEAT MAV_HeartBeat; - // = { custom_mode:0, - // type:0, - // autopilot:0, - // base_mode:0, - // system_status:4, - // mavlink_version:1 - // }; - MAV_HeartBeat.custom_mode=0; - MAV_HeartBeat.type=0; - MAV_HeartBeat.autopilot=0; - MAV_HeartBeat.base_mode=0; - MAV_HeartBeat.system_status=4; - MAV_HeartBeat.mavlink_version=1; - MAV_RxMsg::Send(sizeof(MAV_HeartBeat), MAV_Seq++, MAV_SysID, MAV_COMP_ID_ADSB, MAV_ID_HEARTBEAT, (const uint8_t *)&MAV_HeartBeat, GPS_UART_Write); - } -#endif +// #ifdef WITH_MAVLINK +// { MAV_HEARTBEAT MAV_HeartBeat; +// // = { custom_mode:0, +// // type:0, +// // autopilot:0, +// // base_mode:0, +// // system_status:4, +// // mavlink_version:1 +// // }; +// MAV_HeartBeat.custom_mode=0; +// MAV_HeartBeat.type=0; +// MAV_HeartBeat.autopilot=0; +// MAV_HeartBeat.base_mode=0; +// MAV_HeartBeat.system_status=4; +// MAV_HeartBeat.mavlink_version=1; +// MAV_RxMsg::Send(sizeof(MAV_HeartBeat), MAV_Seq++, MAV_SysID, MAV_COMP_ID_ADSB, MAV_ID_HEARTBEAT, (const uint8_t *)&MAV_HeartBeat, GPS_UART_Write); +// } +// #endif #ifdef DEBUG_PRINT // char Line[128]; Line[0]='0'+RF_TxFIFO.Full(); Line[1]=' '; // print number of packets in the TxFIFO diff --git a/sdkconfig b/sdkconfig index 6b61293..cdded4b 100644 --- a/sdkconfig +++ b/sdkconfig @@ -198,7 +198,6 @@ CONFIG_ESP32_TIME_SYSCALL_USE_NONE= CONFIG_ESP32_RTC_CLOCK_SOURCE_INTERNAL_RC=y CONFIG_ESP32_RTC_CLOCK_SOURCE_EXTERNAL_CRYSTAL= CONFIG_ESP32_RTC_CLK_CAL_CYCLES=1024 -CONFIG_ESP32_RTC_XTAL_BOOTSTRAP_CYCLES=100 CONFIG_ESP32_DEEP_SLEEP_WAKEUP_DELAY=2000 CONFIG_ESP32_XTAL_FREQ_40= CONFIG_ESP32_XTAL_FREQ_26=