From a525f3057347abb1ff2ab94f8bccec596f747960 Mon Sep 17 00:00:00 2001 From: Ben Kurtz Date: Fri, 26 Oct 2018 18:54:08 -0700 Subject: [PATCH] AP_GPS: update implementation of SBP2 flags Some flags were out of date or named incorrectly. Updates based on protocol version 2.4.1, documented here: https://github.com/swift-nav/libsbp/raw/b02e8d835c31f11585ca220fb02b083c12f57247/docs/sbp.pdf --- libraries/AP_GPS/AP_GPS_SBP2.cpp | 8 ++++---- libraries/AP_GPS/AP_GPS_SBP2.h | 19 +++++++++++-------- 2 files changed, 15 insertions(+), 12 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_SBP2.cpp b/libraries/AP_GPS/AP_GPS_SBP2.cpp index 3cf5534756..06834940e9 100644 --- a/libraries/AP_GPS/AP_GPS_SBP2.cpp +++ b/libraries/AP_GPS/AP_GPS_SBP2.cpp @@ -271,14 +271,14 @@ AP_GPS_SBP2::_attempt_state_update() // // Check Flags for Valid Messages // - if (last_gps_time.flags.fix_mode == 0 || - last_vel_ned.flags.fix_mode == 0 || + if (last_gps_time.flags.time_src == 0 || + last_vel_ned.flags.vel_mode == 0 || last_pos_llh.flags.fix_mode == 0 || last_dops.flags.fix_mode == 0) { Debug("Message Marked as Invalid. NO FIX! Flags: {GPS_TIME: %d, VEL_NED: %d, POS_LLH: %d, DOPS: %d}", - last_gps_time.flags.fix_mode, - last_vel_ned.flags.fix_mode, + last_gps_time.flags.time_src, + last_vel_ned.flags.vel_mode, last_pos_llh.flags.fix_mode, last_dops.flags.fix_mode); diff --git a/libraries/AP_GPS/AP_GPS_SBP2.h b/libraries/AP_GPS/AP_GPS_SBP2.h index ef174066c9..676b76bce3 100644 --- a/libraries/AP_GPS/AP_GPS_SBP2.h +++ b/libraries/AP_GPS/AP_GPS_SBP2.h @@ -88,7 +88,8 @@ private: uint8_t res : 5; uint8_t protocol_minor : 8; uint8_t protocol_major : 8; - uint8_t res2 : 7; + uint8_t res2 : 6; + bool ext_antenna_short : 1; bool ext_antenna : 1; }; // 4 bytes @@ -98,7 +99,7 @@ private: uint32_t tow; //< GPS Time of Week rounded to the nearest ms (unit: ms) int32_t ns; //< Nanosecond remainder of rounded tow (unit: ns) struct PACKED flags { - uint8_t fix_mode:3; //< Fix mode (0: invalid, 1: SPP, 2: DGNSS, 3: Float RTX, 4: Fixed RTX + uint8_t time_src:3; //< Fix mode (0: invalid, 1: GNSS Solution, 2: Propagated uint8_t res:5; //< Reserved } flags; }; // 11 bytes @@ -112,7 +113,7 @@ private: uint16_t hdop; //< Horizontal Dilution of Precision (unit: 0.01) uint16_t vdop; //< Vertical Dilution of Precision (unit: 0.01) struct PACKED flags { - uint8_t fix_mode:3; //< Fix mode (0: invalid, 1: SPP, 2: DGNSS, 3: Float RTX, 4: Fixed RTX + uint8_t fix_mode:3; //< Fix mode (0: invalid, 1: SPP, 2: DGNSS, 3: Float RTX, 4: Fixed RTX, 5: Undefined, 6: SBAS Position uint8_t res:4; //< Reserved bool raim_repair:1; //< Solution from RAIM? } flags; @@ -128,9 +129,9 @@ private: uint16_t v_accuracy; //< Vertical position accuracy estimate (unit: mm) uint8_t n_sats; //< Number of satellites used in solution struct PACKED flags { - uint8_t fix_mode:3; //< Fix mode (0: invalid, 1: SPP, 2: DGNSS, 3: Float RTX, 4: Fixed RTX - uint8_t res:4; //< Reserved - bool raim_repair:1; //< Solution from RAIM? + uint8_t fix_mode:3; //< Fix mode (0: invalid, 1: SPP, 2: DGNSS, 3: Float RTX, 4: Fixed RTX, 5: Dead Reckoning, 6: SBAS Position + uint8_t ins_mode:2; //< Inertial navigation mode (0: none, 1: INS used) + uint8_t res:3; //< Reserved } flags; }; // 34 bytes @@ -144,8 +145,9 @@ private: uint16_t v_accuracy; //< Vertical velocity accuracy estimate (unit: mm/s) uint8_t n_sats; //< Number of satellites used in solution struct PACKED flags { - uint8_t fix_mode:3; //< Fix mode (0: invalid, 1: SPP, 2: DGNSS, 3: Float RTX, 4: Fixed RTX - uint8_t res:5; //< Reserved + uint8_t vel_mode:3; //< Velocity mode (0: Invalid, 1: Measured Doppler derived, 2: Computed Doppler derived, 3: Dead reckoning) + uint8_t ins_mode:2; //< Inertial navigation mode (0: none, 1: INS used) + uint8_t res:3; //< Reserved } flags; }; // 22 bytes @@ -159,6 +161,7 @@ private: uint8_t quality:1; //< Time quality values (0: Unknown - don't have nav solution, 1: Good (< 1 microsecond)) uint8_t res:6; //< Reserved } flags; + uint8_t pin; //< Pin number (0-9) }; // 12 bytes void _sbp_process();