kopia lustrzana https://github.com/pjalocha/esp32-ogn-tracker
Debugging the MAVlink interface
rodzic
630cb7f6f2
commit
b7012db511
52
main/gps.cpp
52
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<GPS_PosPipeSize; Idx++)
|
||||
{ if(Sec==Position[Idx].Sec) return Position+Idx; }
|
||||
{ int8_t PosSec = Position[Idx].Sec; if(Position[Idx].FracSec>=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
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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;
|
||||
|
|
34
main/ogn.h
34
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':'_';
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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=
|
||||
|
|
Ładowanie…
Reference in New Issue