A hack to cope with the time specification in the MAV_GPS_RAW_INT message

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

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]); }

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_String(CONS_UART_Write, (const char *)NMEA.Data, 0, NMEA.Len);
-      Format_Bytes(CONS_UART_Write, (const uint8_t *)CRNL, 2);
+      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_String(Log_Write, (const char *)NMEA.Data, 0, NMEA.Len);
-      Format_Bytes(Log_Write, (const uint8_t *)CRNL, 2);
+      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 void GPS_MAV(void)                                                  // when GPS gets an MAV packet
-{ static int64_t MAV_TimeOfs_ms=0;
+{ 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

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",
+   { printf("GPS_RAW_INT: %14.3fsec [%+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); }
+           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           alt;  // [mm]         AMSL
-    int32_t  relative_alt;  // [mm]
+    int32_t  relative_alt;  // [mm]         Above-takeoff ?
-    int16_t            vx;  // [cm]
+    int16_t            vx;  // [cm/s]       along latitude: positive => north
-    int16_t            vy;  // [cm]
+    int16_t            vy;  // [cm/s]       along longitude: positive => east
-    int16_t            vz;  // [cm]
+    int16_t            vz;  // [cm/s]       sink rate: positive => down
-    int16_t           hdg;  // [0.01deg]
+    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",
+     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-7*lat, 1e-7*lon, 1e-3*alt, 1e-3*relative_alt,
+           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