AP_GPS: added logging of RXM_RAW messages for UBlox

this will allow for post-flight RTK processing of DF logs

libraries/AP_GPS/AP_GPS.cpp

@@ -98,6 +98,14 @@ const AP_Param::GroupInfo AP_GPS::var_info[] PROGMEM = {
     AP_GROUPINFO("SBP_LOGMASK", 8, AP_GPS, _sbp_logmask, 0xFF00),
 #endif
 
+#if GPS_RTK_AVAILABLE
+    // @Param: RXM_RAW
+    // @DisplayName: Raw data logging
+    // @Description: Enable logging of RXM raw data from uBlox which includes carrier phase and pseudo range information. This allows for post processing of dataflash logs for more precise positioning. Note that this requires a raw capable uBlox such as the 6P or 6T.
+    // @Values: 0:Disabled,1:log at 1MHz,5:log at 5MHz.
+    AP_GROUPINFO("RAW_DATA", 9, AP_GPS, _raw_data, 0),
+#endif
+
     AP_GROUPEND
 };
 

libraries/AP_GPS/AP_GPS.h

@@ -333,6 +333,7 @@ public:
 #endif
     AP_Int8 _sbas_mode;
     AP_Int8 _min_elevation;
+    AP_Int8 _raw_data;
     
     // handle sending of initialisation strings to the GPS
     void send_blob_start(uint8_t instance, const prog_char *_blob, uint16_t size);

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -47,8 +47,10 @@ extern const AP_HAL::HAL& hal;
  */
 #if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
 #define UBLOX_HW_LOGGING 1
+#define UBLOX_RXM_RAW_LOGGING 1
 #else
 #define UBLOX_HW_LOGGING 0
+#define UBLOX_RXM_RAW_LOGGING 0
 #endif
 
 AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) :
@@ -121,6 +123,11 @@ AP_GPS_UBLOX::send_next_rate_update(void)
     case 7:
         _request_version();
         break;
+#endif
+#if UBLOX_RXM_RAW_LOGGING
+    case 8:
+        _configure_message_rate(CLASS_RXM, MSG_RXM_RAW, gps._raw_data); // 24*16+8 bytes
+        break;
 #endif
     default:
         need_rate_update = false;
@@ -316,6 +323,33 @@ void AP_GPS_UBLOX::log_accuracy(void) {
 }
 #endif // UBLOX_HW_LOGGING
 
+#if UBLOX_RXM_RAW_LOGGING
+void AP_GPS_UBLOX::log_rxm_raw(const struct ubx_rxm_raw &raw)
+{
+    if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
+        return;
+    }
+    uint32_t now = hal.scheduler->millis();
+    for (uint8_t i=0; i<raw.numSV; i++) {
+        struct log_GPS_RAW pkt = {
+            LOG_PACKET_HEADER_INIT(LOG_GPS_RAW_MSG),
+            timestamp  : now,
+            iTOW       : raw.iTOW,
+            week       : raw.week,
+            numSV      : raw.numSV,
+            sv         : raw.svinfo[i].sv,
+            cpMes      : raw.svinfo[i].cpMes,
+            prMes      : raw.svinfo[i].prMes,
+            doMes      : raw.svinfo[i].doMes,
+            mesQI      : raw.svinfo[i].mesQI,
+            cno        : raw.svinfo[i].cno,
+            lli        : raw.svinfo[i].lli
+        };
+        gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
+    }
+}
+#endif // UBLOX_RXM_RAW_LOGGING
+
 void AP_GPS_UBLOX::unexpected_message(void)
 {
     Debug("Unexpected message 0x%02x 0x%02x", (unsigned)_class, (unsigned)_msg_id);
@@ -398,6 +432,13 @@ AP_GPS_UBLOX::_parse_gps(void)
     }
 #endif // UBLOX_HW_LOGGING
 
+#if UBLOX_RXM_RAW_LOGGING
+    if (_class == CLASS_RXM && _msg_id == MSG_RXM_RAW && gps._raw_data != 0) {
+        log_rxm_raw(_buffer.rxm_raw);
+        return false;
+    }
+#endif // UBLOX_RXM_RAW_LOGGING
+
     if (_class != CLASS_NAV) {
         unexpected_message();
         return false;
@@ -573,7 +614,7 @@ AP_GPS_UBLOX::_parse_gps(void)
  *  update checksum for a set of bytes
  */
 void
-AP_GPS_UBLOX::_update_checksum(uint8_t *data, uint8_t len, uint8_t &ck_a, uint8_t &ck_b)
+AP_GPS_UBLOX::_update_checksum(uint8_t *data, uint16_t len, uint8_t &ck_a, uint8_t &ck_b)
 {
     while (len--) {
         ck_a += *data;
@@ -587,7 +628,7 @@ AP_GPS_UBLOX::_update_checksum(uint8_t *data, uint8_t len, uint8_t &ck_a, uint8_
  *  send a ublox message
  */
 void
-AP_GPS_UBLOX::_send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size)
+AP_GPS_UBLOX::_send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint16_t size)
 {
     struct ubx_header header;
     uint8_t ck_a=0, ck_b=0;

libraries/AP_GPS/AP_GPS_UBLOX.h

@@ -38,6 +38,7 @@
  * would mean we would never detect it.
  */
 #define UBLOX_SET_BINARY "\265\142\006\001\003\000\001\006\001\022\117$PUBX,41,1,0003,0001,38400,0*26\r\n"
+#define UBLOX_MAX_RXM_RAW_SATS 16
 
 class AP_GPS_UBLOX : public AP_GPS_Backend
 {
@@ -200,6 +201,21 @@ private:
         uint8_t globalFlags;
         uint16_t reserved;
     };
+    struct PACKED ubx_rxm_raw {
+        int32_t iTOW;
+        int16_t week;
+        uint8_t numSV;
+        uint8_t reserved1;
+        struct ubx_rxm_raw_sv {
+            double cpMes;
+            double prMes;
+            float doMes;
+            uint8_t sv;
+            int8_t mesQI;
+            int8_t cno;
+            uint8_t lli;
+        } svinfo[UBLOX_MAX_RXM_RAW_SATS];
+    };
     // Receive buffer
     union PACKED {
         ubx_nav_posllh posllh;
@@ -212,6 +228,7 @@ private:
         ubx_mon_hw2 mon_hw2;
         ubx_cfg_sbas sbas;
         ubx_nav_svinfo_header svinfo_header;
+        ubx_rxm_raw rxm_raw;
         uint8_t bytes[];
     } _buffer;
 
@@ -222,6 +239,7 @@ private:
         CLASS_ACK = 0x05,
         CLASS_CFG = 0x06,
         CLASS_MON = 0x0A,
+        CLASS_RXM = 0x02,
         MSG_ACK_NACK = 0x00,
         MSG_ACK_ACK = 0x01,
         MSG_POSLLH = 0x2,
@@ -235,7 +253,8 @@ private:
         MSG_CFG_SBAS = 0x16,
         MSG_MON_HW = 0x09,
         MSG_MON_HW2 = 0x0B,
-        MSG_NAV_SVINFO = 0x30
+        MSG_NAV_SVINFO = 0x30,
+        MSG_RXM_RAW = 0x10
     };
     enum ubs_nav_fix_type {
         FIX_NONE = 0,
@@ -266,9 +285,9 @@ private:
     uint16_t        _payload_length;
     uint16_t        _payload_counter;
 
-	// 8 bit count of fix messages processed, used for periodic
-	// processing
-    uint8_t			_fix_count;
+    // 8 bit count of fix messages processed, used for periodic
+    // processing
+    uint8_t         _fix_count;
     uint8_t         _class;
 
     uint32_t        _last_vel_time;
@@ -296,8 +315,8 @@ private:
     void        _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
     void        _configure_gps(void);
     void        _configure_sbas(bool enable);
-    void        _update_checksum(uint8_t *data, uint8_t len, uint8_t &ck_a, uint8_t &ck_b);
-    void        _send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size);
+    void        _update_checksum(uint8_t *data, uint16_t len, uint8_t &ck_a, uint8_t &ck_b);
+    void        _send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint16_t size);
     void		send_next_rate_update(void);
     void        _request_version(void);
 
@@ -306,6 +325,7 @@ private:
     void log_mon_hw(void);
     void log_mon_hw2(void);
     void log_accuracy(void);
+    void log_rxm_raw(const struct ubx_rxm_raw &raw);
 };
 
 #endif // __AP_GPS_UBLOX_H__