From 2293070a5b4de6ab455f76c8698b30145e8a2855 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger 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