mirror
/
esp32-ogn-tracker
kopia lustrzana https://github.com/pjalocha/esp32-ogn-tracker
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność

Debugging the MAVlink interface

pull/5/head
Pawel Jalocha 2018-05-19 01:03:18 +00:00
rodzic 630cb7f6f2
commit b7012db511
6 zmienionych plików z 102 dodań i 66 usunięć
Pokaż statystyki Ściągnij plik aktualizacji Ściągnij plik porównania Expand all files Collapse all files

52
main/gps.cpp
Wyświetl plik

@ -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

2
main/gps.h
Wyświetl plik

@ -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)

14
main/mavlink.h
Wyświetl plik

@ -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
Wyświetl plik

@ -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':'_';

65
main/proc.cpp
Wyświetl plik

@ -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

1
sdkconfig
Wyświetl plik

@ -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=