From c3350d9784730f8dec04d89ef22bb26e1d5fb6c7 Mon Sep 17 00:00:00 2001 From: Pawel Jalocha Date: Thu, 17 May 2018 17:17:27 +0000 Subject: [PATCH] A hack to cope with the time specification in the MAV_GPS_RAW_INT message --- main/format.cpp | 32 ++++++++++++++++++++++++++++++++ main/format.h | 4 +++- main/gps.cpp | 25 +++++++++++++++---------- main/mavlink.h | 25 +++++++++++++------------ 4 files changed, 63 insertions(+), 23 deletions(-) diff --git a/main/format.cpp b/main/format.cpp index 42c6dd1..46fe1a6 100644 --- a/main/format.cpp +++ b/main/format.cpp @@ -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=0) return Val; if( (Digit>='A') && (Digit<='F') ) return Digit-'A'+10; diff --git a/main/format.h b/main/format.h index ec17957..f27c7b1 100644 --- a/main/format.h +++ b/main/format.h @@ -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]); } diff --git a/main/gps.cpp b/main/gps.cpp index ca23daa..fad6f11 100644 --- a/main/gps.cpp +++ b/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 diff --git a/main/mavlink.h b/main/mavlink.h index 8b3f6a8..f2b875d 100644 --- a/main/mavlink.h +++ b/main/mavlink.h @@ -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