From 2293070a5b4de6ab455f76c8698b30145e8a2855 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Tue, 28 Oct 2014 12:44:07 -0700
Subject: [PATCH] AP_GPS: Add uBlox accuracy metrics interface and logging

---
 libraries/AP_GPS/AP_GPS.h         | 42 +++++++++++++++++++++++++++++++
 libraries/AP_GPS/AP_GPS_UBLOX.cpp | 26 +++++++++++++++++++
 libraries/AP_GPS/AP_GPS_UBLOX.h   |  1 +
 libraries/AP_GPS/GPS_Backend.cpp  |  3 +++
 4 files changed, 72 insertions(+)

diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h
index e83c451260..270351ae0b 100644
--- a/libraries/AP_GPS/AP_GPS.h
+++ b/libraries/AP_GPS/AP_GPS.h
@@ -135,7 +135,13 @@ public:
         uint16_t hdop;                      ///< horizontal dilution of precision in cm
         uint8_t num_sats;                   ///< Number of visible satelites        
         Vector3f velocity;                  ///< 3D velocitiy in m/s, in NED format
+        float speed_accuracy;
+        float horizontal_accuracy;
+        float vertical_accuracy;
         bool have_vertical_velocity:1;      ///< does this GPS give vertical velocity?
+        bool have_speed_accuracy:1;
+        bool have_horizontal_accuracy:1;
+        bool have_vertical_accuracy:1;
         uint32_t last_gps_time_ms;          ///< the system time we got the last GPS timestamp, milliseconds
     };
 
@@ -180,6 +186,42 @@ public:
         return location(primary_instance);
     }
 
+    bool speed_accuracy(uint8_t instance, float &sacc) const {
+        if(_GPS_STATE(instance).have_speed_accuracy) {
+            sacc = _GPS_STATE(instance).speed_accuracy;
+            return true;
+        }
+        return false;
+    }
+
+    bool speed_accuracy(float &sacc) const {
+        return speed_accuracy(primary_instance, sacc);
+    }
+
+    bool horizontal_accuracy(uint8_t instance, float &hacc) const {
+        if(_GPS_STATE(instance).have_horizontal_accuracy) {
+            hacc = _GPS_STATE(instance).horizontal_accuracy;
+            return true;
+        }
+        return false;
+    }
+
+    bool horizontal_accuracy(float &hacc) const {
+        return horizontal_accuracy(primary_instance, hacc);
+    }
+
+    bool vertical_accuracy(uint8_t instance, float &vacc) const {
+        if(_GPS_STATE(instance).have_vertical_accuracy) {
+            vacc = _GPS_STATE(instance).vertical_accuracy;
+            return true;
+        }
+        return false;
+    }
+
+    bool vertical_accuracy(float &vacc) const {
+        return vertical_accuracy(primary_instance, vacc);
+    }
+
     // 3D velocity in NED format
     const Vector3f &velocity(uint8_t instance) const {
         return _GPS_STATE(instance).velocity;
diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp
index 5e14534c22..3e1d963160 100644
--- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp
+++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp
@@ -287,6 +287,21 @@ void AP_GPS_UBLOX::log_mon_hw2(void)
     };
     gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
 }
+
+void AP_GPS_UBLOX::log_accuracy(void) {
+    if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
+        return;
+    }
+    struct log_Ubx3 pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_UBX3_MSG),
+        timestamp  : hal.scheduler->millis(),
+        instance   : state.instance,
+        hAcc     : state.horizontal_accuracy,
+        vAcc     : state.vertical_accuracy,
+        sAcc     : state.speed_accuracy
+    };
+    gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
+}
 #endif // UBLOX_HW_LOGGING
 
 void AP_GPS_UBLOX::unexpected_message(void)
@@ -390,6 +405,10 @@ AP_GPS_UBLOX::_parse_gps(void)
         state.location.lat = -353632610L;
         state.location.alt = 58400;
 #endif
+        state.horizontal_accuracy = _buffer.posllh.horizontal_accuracy*1.0e-3f;
+        state.vertical_accuracy = _buffer.posllh.vertical_accuracy*1.0e-3f;
+        state.have_horizontal_accuracy = true;
+        state.have_vertical_accuracy = true;
         break;
     case MSG_STATUS:
         Debug("MSG_STATUS fix_status=%u fix_type=%u",
@@ -462,6 +481,8 @@ AP_GPS_UBLOX::_parse_gps(void)
         state.velocity.x = _buffer.velned.ned_north * 0.01f;
         state.velocity.y = _buffer.velned.ned_east * 0.01f;
         state.velocity.z = _buffer.velned.ned_down * 0.01f;
+        state.have_speed_accuracy = true;
+        state.speed_accuracy = _buffer.velned.speed_accuracy*0.01f;
         _new_speed = true;
         break;
     default:
@@ -498,6 +519,11 @@ AP_GPS_UBLOX::_parse_gps(void)
 			_send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, NULL, 0);
             _fix_count = 0;
 		}
+
+#if UBLOX_HW_LOGGING
+        log_accuracy();
+#endif //UBLOX_HW_LOGGING
+
         return true;
     }
     return false;
diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h
index 01b6b998ab..5fa6ecc9bf 100644
--- a/libraries/AP_GPS/AP_GPS_UBLOX.h
+++ b/libraries/AP_GPS/AP_GPS_UBLOX.h
@@ -289,6 +289,7 @@ private:
     void write_logging_headers(void);
     void log_mon_hw(void);
     void log_mon_hw2(void);
+    void log_accuracy(void);
 };
 
 #endif // __AP_GPS_UBLOX_H__
diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp
index 32e3b27483..0d8caae9e1 100644
--- a/libraries/AP_GPS/GPS_Backend.cpp
+++ b/libraries/AP_GPS/GPS_Backend.cpp
@@ -23,6 +23,9 @@ AP_GPS_Backend::AP_GPS_Backend(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::
     gps(_gps),
     state(_state)
 {
+    state.have_speed_accuracy = false;
+    state.have_horizontal_accuracy = false;
+    state.have_vertical_accuracy = false;
 }
 
 int32_t AP_GPS_Backend::swap_int32(int32_t v) const