From a382ce2b0c728a4a40037aeb59dda391013aa1a0 Mon Sep 17 00:00:00 2001
From: Michael du Breuil <michael@swiftradioplanes.com>
Date: Mon, 29 Jun 2015 15:04:35 -0700
Subject: [PATCH] AP_GPS: Add support for logging ublox RXM-RAWX messages

Can only record 32 satellites raw info before memory corruption occurs. (And raised the limit on normal RXM-RAW messages to 22)
---
 libraries/AP_GPS/AP_GPS_UBLOX.cpp | 47 ++++++++++++++++++++++++++++++-
 libraries/AP_GPS/AP_GPS_UBLOX.h   | 30 ++++++++++++++++++--
 2 files changed, 74 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp
index c32b27969e..ea1c17896b 100644
--- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp
+++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp
@@ -123,9 +123,12 @@ AP_GPS_UBLOX::send_next_rate_update(void)
     case 7:
         _configure_message_rate(CLASS_RXM, MSG_RXM_RAW, gps._raw_data);
         break;
+    case 8:
+        _configure_message_rate(CLASS_RXM, MSG_RXM_RAWX, gps._raw_data);
+        break;
 #endif
 #if UBLOX_VERSION_AUTODETECTION 
-    case 8:
+    case 9:
         _request_version();
         break;
 #endif
@@ -348,6 +351,45 @@ void AP_GPS_UBLOX::log_rxm_raw(const struct ubx_rxm_raw &raw)
         gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
     }
 }
+
+void AP_GPS_UBLOX::log_rxm_rawx(const struct ubx_rxm_rawx &raw)
+{
+    if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
+        return;
+    }
+    uint64_t now = hal.scheduler->micros64();
+
+    struct log_GPS_RAWH header = {
+        LOG_PACKET_HEADER_INIT(LOG_GPS_RAWH_MSG),
+        time_us    : now,
+        rcvTow     : raw.rcvTow,
+        week       : raw.week,
+        leapS      : raw.leapS,
+        numMeas    : raw.numMeas,
+        recStat    : raw.recStat
+    };
+    gps._DataFlash->WriteBlock(&header, sizeof(header));
+
+    for (uint8_t i=0; i<raw.numMeas; i++) {
+        struct log_GPS_RAWS pkt = {
+            LOG_PACKET_HEADER_INIT(LOG_GPS_RAWS_MSG),
+            time_us    : now,
+            prMes      : raw.svinfo[i].prMes,
+            cpMes      : raw.svinfo[i].cpMes,
+            doMes      : raw.svinfo[i].doMes,
+            gnssId     : raw.svinfo[i].gnssId,
+            svId       : raw.svinfo[i].svId,
+            freqId     : raw.svinfo[i].freqId,
+            locktime   : raw.svinfo[i].locktime,
+            cno        : raw.svinfo[i].cno,
+            prStdev    : raw.svinfo[i].prStdev,
+            cpStdev    : raw.svinfo[i].cpStdev,
+            doStdev    : raw.svinfo[i].doStdev,
+            trkStat    : raw.svinfo[i].trkStat
+        };
+        gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
+    }
+}
 #endif // UBLOX_RXM_RAW_LOGGING
 
 void AP_GPS_UBLOX::unexpected_message(void)
@@ -436,6 +478,9 @@ AP_GPS_UBLOX::_parse_gps(void)
     if (_class == CLASS_RXM && _msg_id == MSG_RXM_RAW && gps._raw_data != 0) {
         log_rxm_raw(_buffer.rxm_raw);
         return false;
+    } else if (_class == CLASS_RXM && _msg_id == MSG_RXM_RAWX && gps._raw_data != 0) {
+        log_rxm_rawx(_buffer.rxm_rawx);
+        return false;
     }
 #endif // UBLOX_RXM_RAW_LOGGING
 
diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h
index 2ce09f5119..c6e91aea34 100644
--- a/libraries/AP_GPS/AP_GPS_UBLOX.h
+++ b/libraries/AP_GPS/AP_GPS_UBLOX.h
@@ -42,7 +42,8 @@
 
 #if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
 #define UBLOX_RXM_RAW_LOGGING 1
-#define UBLOX_MAX_RXM_RAW_SATS 16
+#define UBLOX_MAX_RXM_RAW_SATS 22
+#define UBLOX_MAX_RXM_RAWX_SATS 32
 #else
 #define UBLOX_RXM_RAW_LOGGING 0
 #endif
@@ -224,6 +225,28 @@ private:
             uint8_t lli;
         } svinfo[UBLOX_MAX_RXM_RAW_SATS];
     };
+    struct PACKED ubx_rxm_rawx {
+        double rcvTow;
+        uint16_t week;
+        int8_t leapS;
+        uint8_t numMeas;
+        uint8_t recStat;
+        uint8_t reserved1[3];
+        struct ubx_rxm_rawx_sv {
+            double prMes;
+            double cpMes;
+            float doMes;
+            uint8_t gnssId;
+            uint8_t svId;
+            uint8_t freqId;
+            uint16_t locktime;
+            uint8_t cno;
+            uint8_t prStdev;
+            uint8_t cpStdev;
+            uint8_t doStdev;
+            uint8_t trkStat;
+        } svinfo[UBLOX_MAX_RXM_RAWX_SATS];
+    };
 #endif
     // Receive buffer
     union PACKED {
@@ -239,6 +262,7 @@ private:
         ubx_nav_svinfo_header svinfo_header;
 #if UBLOX_RXM_RAW_LOGGING
         ubx_rxm_raw rxm_raw;
+        ubx_rxm_rawx rxm_rawx;
 #endif
         uint8_t bytes[];
     } _buffer;
@@ -265,7 +289,8 @@ private:
         MSG_MON_HW = 0x09,
         MSG_MON_HW2 = 0x0B,
         MSG_NAV_SVINFO = 0x30,
-        MSG_RXM_RAW = 0x10
+        MSG_RXM_RAW = 0x10,
+        MSG_RXM_RAWX = 0x15
     };
     enum ubs_nav_fix_type {
         FIX_NONE = 0,
@@ -338,6 +363,7 @@ private:
     void log_mon_hw2(void);
     void log_accuracy(void);
     void log_rxm_raw(const struct ubx_rxm_raw &raw);
+    void log_rxm_rawx(const struct ubx_rxm_rawx &raw);
 };
 
 #endif // __AP_GPS_UBLOX_H__