kopia lustrzana https://github.com/pjalocha/esp32-ogn-tracker
A hack to cope with the time specification in the MAV_GPS_RAW_INT message
Pawel Jalocha
rodzic
0718171974
commit
c3350d9784
|
|
@ -14,7 +14,9 @@ void Format_Bytes( void (*Output)(char), const uint8_t *Bytes, uint8_t Len)
|
|||
void Format_String( void (*Output)(char), const char *String)
|
||||
{ for( ; ; )
|
||||
{ uint8_t ch = (*String++); if(ch==0) break;
|
||||
#ifdef WITH_AUTOCR
|
||||
if(ch=='\n') (*Output)('\r');
|
||||
#endif
|
||||
(*Output)(ch); }
|
||||
}
|
||||
|
||||
|
|
@ -22,7 +24,9 @@ uint8_t Format_String(char *Out, const char *String)
|
|||
{ uint8_t OutLen=0;
|
||||
for( ; ; )
|
||||
{ char ch = (*String++); if(ch==0) break;
|
||||
#ifdef WITH_AUTOCR
|
||||
if(ch=='\n') Out[OutLen++]='\r';
|
||||
#endif
|
||||
Out[OutLen++]=ch; }
|
||||
// Out[OutLen]=0;
|
||||
return OutLen; }
|
||||
|
|
@ -32,7 +36,9 @@ void Format_String( void (*Output)(char), const char *String, uint8_t MinLen, ui
|
|||
uint8_t Idx;
|
||||
for(Idx=0; Idx<MaxLen; Idx++)
|
||||
{ char ch = String[Idx]; if(ch==0) break;
|
||||
#ifdef WITH_AUTOCR
|
||||
if(ch=='\n') (*Output)('\r');
|
||||
#endif
|
||||
(*Output)(ch); }
|
||||
for( ; Idx<MinLen; Idx++)
|
||||
(*Output)(' ');
|
||||
|
|
@ -44,7 +50,9 @@ uint8_t Format_String(char *Out, const char *String, uint8_t MinLen, uint8_t Max
|
|||
uint8_t Idx;
|
||||
for(Idx=0; Idx<MaxLen; Idx++)
|
||||
{ char ch = String[Idx]; if(ch==0) break;
|
||||
#ifdef WITH_AUTOCR
|
||||
if(ch=='\n') Out[OutLen++]='\r';
|
||||
#endif
|
||||
Out[OutLen++]=ch; }
|
||||
for( ; Idx<MinLen; Idx++)
|
||||
Out[OutLen++]=' ';
|
||||
|
|
@ -166,6 +174,30 @@ uint8_t Format_Hex( char *Output, uint32_t Word, uint8_t Digits)
|
|||
|
||||
// ------------------------------------------------------------------------------------------
|
||||
|
||||
uint8_t Format_Latitude(char *Out, int32_t Lat)
|
||||
{ uint8_t Len=0;
|
||||
char Sign='N';
|
||||
if(Lat<0) { Sign='S'; Lat=(-Lat); }
|
||||
uint32_t Deg=Lat/600000;
|
||||
Lat -= 600000*Deg;
|
||||
Len+=Format_UnsDec(Out+Len, Deg, 2, 0);
|
||||
Len+=Format_UnsDec(Out+Len, Lat, 6, 4);
|
||||
Out[Len++]=Sign;
|
||||
return Len; }
|
||||
|
||||
uint8_t Format_Longitude(char *Out, int32_t Lon)
|
||||
{ uint8_t Len=0;
|
||||
char Sign='E';
|
||||
if(Lon<0) { Sign='W'; Lon=(-Lon); }
|
||||
uint32_t Deg=Lon/600000;
|
||||
Lon -= 600000*Deg;
|
||||
Len+=Format_UnsDec(Out+Len, Deg, 3, 0);
|
||||
Len+=Format_UnsDec(Out+Len, Lon, 6, 4);
|
||||
Out[Len++]=Sign;
|
||||
return Len; }
|
||||
|
||||
// ------------------------------------------------------------------------------------------
|
||||
|
||||
int8_t Read_Hex1(char Digit)
|
||||
{ int8_t Val=Read_Dec1(Digit); if(Val>=0) return Val;
|
||||
if( (Digit>='A') && (Digit<='F') ) return Digit-'A'+10;
|
||||
|
|
|
|||
|
|
@ -37,8 +37,10 @@ uint8_t Format_Hex( char *Output, uint32_t Word, uint8_t Digits);
|
|||
|
||||
uint8_t Format_HHMMSS(char *Out, uint32_t Time);
|
||||
|
||||
uint8_t Format_Latitude (char *Out, int32_t Lat); // [1/600000deg] => DDMM.MMMMs
|
||||
uint8_t Format_Longitude(char *Out, int32_t Lon); // [1/600000deg] => DDDMM.MMMMs
|
||||
|
||||
int8_t Read_Hex1(char Digit);
|
||||
int16_t Read_Hex2(const char *Inp);
|
||||
|
||||
int8_t Read_Dec1(char Digit); // convert single digit into an integer
|
||||
inline int8_t Read_Dec1(const char *Inp) { return Read_Dec1(Inp[0]); }
|
||||
|
|
|
|||
25
main/gps.cpp
25
main/gps.cpp
|
|
@ -194,7 +194,7 @@ static void GPS_BurstStart(void) // wh
|
|||
Len += NMEA_AppendCheck(GPS_Cmd, Len);
|
||||
GPS_Cmd[Len]=0;
|
||||
// Serial.println(GPS_Cmd);
|
||||
Format_String(GPS_UART_Write, GPS_Cmd, Len);
|
||||
Format_String(GPS_UART_Write, GPS_Cmd, Len, 0);
|
||||
GPS_UART_Write('\r'); GPS_UART_Write('\n');
|
||||
#endif
|
||||
#ifdef WITH_GPS_SRF
|
||||
|
|
@ -206,7 +206,7 @@ static void GPS_BurstStart(void) // wh
|
|||
Len += NMEA_AppendCheck(GPS_Cmd, Len);
|
||||
GPS_Cmd[Len]=0;
|
||||
// Serial.println(GPS_Cmd);
|
||||
Format_String(GPS_UART_Write, GPS_Cmd, Len);
|
||||
Format_String(GPS_UART_Write, GPS_Cmd, Len, 0);
|
||||
GPS_UART_Write('\r'); GPS_UART_Write('\n');
|
||||
#endif
|
||||
}
|
||||
|
|
@ -383,17 +383,21 @@ static void GPS_NMEA(void) // wh
|
|||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
if( NMEA.isP() || NMEA.isGxRMC() || NMEA.isGxGGA() || NMEA.isGxGSA() || NMEA.isGPTXT() )
|
||||
{ static char CRNL[3] = "\r\n";
|
||||
{ // static char CRNL[3] = "\r\n";
|
||||
// if(CONS_UART_Free()>=128)
|
||||
{ xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_Bytes(CONS_UART_Write, NMEA.Data, NMEA.Len);
|
||||
Format_Bytes(CONS_UART_Write, (const uint8_t *)CRNL, 2);
|
||||
Format_String(CONS_UART_Write, (const char *)NMEA.Data, 0, NMEA.Len);
|
||||
CONS_UART_Write('\n');
|
||||
// Format_Bytes(CONS_UART_Write, NMEA.Data, NMEA.Len);
|
||||
// Format_Bytes(CONS_UART_Write, (const uint8_t *)CRNL, 2);
|
||||
xSemaphoreGive(CONS_Mutex); }
|
||||
#ifdef WITH_SDLOG
|
||||
if(Log_Free()>=128)
|
||||
{ xSemaphoreTake(Log_Mutex, portMAX_DELAY);
|
||||
Format_Bytes(Log_Write, NMEA.Data, NMEA.Len);
|
||||
Format_Bytes(Log_Write, (const uint8_t *)CRNL, 2);
|
||||
Format_String(Log_Write, (const char *)NMEA.Data, 0, NMEA.Len);
|
||||
Log_Write('\n');
|
||||
// Format_Bytes(Log_Write, NMEA.Data, NMEA.Len);
|
||||
// Format_Bytes(Log_Write, (const uint8_t *)CRNL, 2);
|
||||
xSemaphoreGive(Log_Mutex); }
|
||||
#endif
|
||||
}
|
||||
|
|
@ -481,8 +485,8 @@ static void GPS_UBX(void)
|
|||
#endif // WITH_GPS_UBX
|
||||
|
||||
#ifdef WITH_MAVLINK
|
||||
static void GPS_MAV(void) // when GPS gets an MAV packet
|
||||
{ static int64_t MAV_TimeOfs_ms=0;
|
||||
static void GPS_MAV(void) // when GPS gets an MAV packet
|
||||
{ static int64_t MAV_TimeOfs_ms=0; // [ms] diff. between UTC time and boot time reported in MAV messages
|
||||
TickType_t TickCount=xTaskGetTickCount();
|
||||
GPS_Status.MAV=1;
|
||||
LED_PCB_Flash(2);
|
||||
|
|
@ -531,7 +535,8 @@ static void GPS_MAV(void) // w
|
|||
#endif
|
||||
} else if(MsgID==MAV_ID_GPS_RAW_INT)
|
||||
{ MAV_GPS_RAW_INT *RawGPS = (MAV_GPS_RAW_INT *)MAV.getPayload();
|
||||
uint64_t UnixTime_ms = RawGPS->time_usec/1000 + MAV_TimeOfs_ms;
|
||||
uint64_t UnixTime_ms = RawGPS->time_usec/1000;
|
||||
if(UnixTime_ms<(uint64)1000000000000) UnixTime_ms += MAV_TimeOfs_ms;
|
||||
// RawGPS->time_usec += (int64_t)MAV_TimeOfs_ms*1000;
|
||||
Position[PosIdx].Read(RawGPS, UnixTime_ms);
|
||||
#ifdef DEBUG_PRINT
|
||||
|
|
|
|||
|
|
@ -95,7 +95,7 @@ class MAV_SYS_STATUS
|
|||
|
||||
class MAV_GPS_RAW_INT
|
||||
{ public:
|
||||
uint64_t time_usec; // [usec] Time
|
||||
uint64_t time_usec; // [usec] Time (appears to be boot time, not UTC)
|
||||
int32_t lat; // [1e-7deg] Latitude
|
||||
int32_t lon; // [1e-7deg] Longitude
|
||||
int32_t alt; // [mm] Altitude AMSL
|
||||
|
|
@ -112,8 +112,8 @@ class MAV_GPS_RAW_INT
|
|||
uint8_t satellites_visible; // [] Number of satellites
|
||||
public:
|
||||
void Print(void) const
|
||||
{ printf("GPS_RAW_INT: [%+9.5f, %+10.5f]deg %+5.1fm %3.1fm/s %05.1fdeg %d/%dsat\n",
|
||||
1e-7*lat, 1e-7*lon, 1e-3*alt, 0.01*vel, 0.01*cog, fix_type, satellites_visible); }
|
||||
{ printf("GPS_RAW_INT: %14.3fsec [%+9.5f, %+10.5f]deg %+5.1fm %3.1fm/s %05.1fdeg %d/%dsat\n",
|
||||
1e-6*time_usec, 1e-7*lat, 1e-7*lon, 1e-3*alt, 0.01*vel, 0.01*cog, fix_type, satellites_visible); }
|
||||
} ;
|
||||
|
||||
class MAV_GLOBAL_POSITION_INT
|
||||
|
|
@ -121,17 +121,17 @@ class MAV_GLOBAL_POSITION_INT
|
|||
uint32_t time_boot_ms; // [ms]
|
||||
int32_t lat; // [1e-7deg]
|
||||
int32_t lon; // [1e-7deg]
|
||||
int32_t alt; // [mm]
|
||||
int32_t relative_alt; // [mm]
|
||||
int16_t vx; // [cm]
|
||||
int16_t vy; // [cm]
|
||||
int16_t vz; // [cm]
|
||||
int16_t hdg; // [0.01deg]
|
||||
int32_t alt; // [mm] AMSL
|
||||
int32_t relative_alt; // [mm] Above-takeoff ?
|
||||
int16_t vx; // [cm/s] along latitude: positive => north
|
||||
int16_t vy; // [cm/s] along longitude: positive => east
|
||||
int16_t vz; // [cm/s] sink rate: positive => down
|
||||
int16_t hdg; // [0.01deg] yaw angle
|
||||
public:
|
||||
void Print(void) const
|
||||
{ uint16_t Track = IntAtan2(vy, vx);
|
||||
printf("GLOBAL_POSITION_INT: [%+9.5f, %+10.5f]deg %5.1f(%+4.1f)m %3.1fm/s %05.1f/%05.1fdeg %+4.1fm/s\n",
|
||||
1e-7*lat, 1e-7*lon, 1e-3*alt, 1e-3*relative_alt,
|
||||
printf("GLOBAL_POSITION_INT: %10.3fsec [%+9.5f, %+10.5f]deg %5.1f(%+4.1f)m %3.1fm/s %05.1f/%05.1fdeg %+4.1fm/s\n",
|
||||
1e-3*time_boot_ms, 1e-7*lat, 1e-7*lon, 1e-3*alt, 1e-3*relative_alt,
|
||||
0.01*IntSqrt((int32_t)vx*vx+(int32_t)vy*vy), 360.0/0x10000*Track, 0.01*hdg,
|
||||
0.01*vz); }
|
||||
} ;
|
||||
|
|
@ -144,7 +144,8 @@ class MAV_SCALED_PRESSURE
|
|||
int16_t temperature; // [0.01degC]
|
||||
public:
|
||||
void Print(void) const
|
||||
{ printf("SCALED_PRESSURE: %8.3f [sec] %7.2f %+4.2f [hPa] %+5.2f [degC]\n", 1e-3*time_boot_ms, 2*press_abs, 2*press_diff, 0.01*temperature); }
|
||||
{ printf("SCALED_PRESSURE: %8.3f [sec] %7.2f %+4.2f [hPa] %+5.2f [degC]\n",
|
||||
1e-3*time_boot_ms, 2*press_abs, 2*press_diff, 0.01*temperature); } // with ms5607 there is a bug/feature: pressure is twice as low
|
||||
} ;
|
||||
|
||||
class MAV_ADSB_VEHICLE // this message is sent by ADS-B or other traffic receiver
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue