From 840a4dee1e84d0c63a2aeea53a140bc9fa56ea1d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 4 Sep 2014 14:27:05 +1000 Subject: [PATCH] AP_GPS: added GPS_SBAS_MODE parameter allows SBAS to be enabled/disabled --- libraries/AP_GPS/AP_GPS.cpp | 7 +++++++ libraries/AP_GPS/AP_GPS.h | 1 + libraries/AP_GPS/AP_GPS_UBLOX.cpp | 20 ++++++++++++++++++++ libraries/AP_GPS/AP_GPS_UBLOX.h | 10 ++++++++++ 4 files changed, 38 insertions(+) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index f0f6a9469c..30f751741c 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -64,6 +64,13 @@ const AP_Param::GroupInfo AP_GPS::var_info[] PROGMEM = { AP_GROUPINFO("MIN_DGPS", 4, AP_GPS, _min_dgps, 100), #endif + // @Param: SBAS_MODE + // @DisplayName: SBAS Mode + // @Description: This sets the SBAS (satellite based augmentation system) mode if available on this GPS. If set to 2 then the SBAS mode is not changed in the GPS. Otherwise the GPS will be reconfigured to enable/disable SBAS. Disabling SBAS may be worthwhile in some parts of the world where an SBAS signal is available but the baseline is too long to be useful. + // @Values: 0:Disabled,1:Enabled,2:NoChange + // @User: Advanced + AP_GROUPINFO("SBAS_MODE", 5, AP_GPS, _sbas_mode, 2), + AP_GROUPEND }; diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index cff0045fcf..25d45e2cda 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -293,6 +293,7 @@ public: AP_Int8 _auto_switch; AP_Int8 _min_dgps; #endif + AP_Int8 _sbas_mode; // handle sending of initialisation strings to the GPS void send_blob_start(uint8_t instance, const prog_char *_blob, uint16_t size); diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 0cda3ee009..c3f6e39ce4 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -325,6 +325,21 @@ AP_GPS_UBLOX::_parse_gps(void) return false; } + if (_class == CLASS_CFG && _msg_id == MSG_CFG_SBAS && gps._sbas_mode != 2) { + Debug("Got SBAS settings %u %u %u 0x%x 0x%x\n", + (unsigned)_buffer.sbas.mode, + (unsigned)_buffer.sbas.usage, + (unsigned)_buffer.sbas.maxSBAS, + (unsigned)_buffer.sbas.scanmode2, + (unsigned)_buffer.sbas.scanmode1); + if (_buffer.sbas.mode != gps._sbas_mode) { + _buffer.sbas.mode = gps._sbas_mode; + _send_message(CLASS_CFG, MSG_CFG_SBAS, + &_buffer.sbas, + sizeof(_buffer.sbas)); + } + } + #if UBLOX_HW_LOGGING if (_class == CLASS_MON) { if (_msg_id == MSG_MON_HW) { @@ -458,6 +473,11 @@ AP_GPS_UBLOX::_parse_gps(void) _last_5hz_time = hal.scheduler->millis(); } + if (_fix_count == 50 && gps._sbas_mode != 2) { + // ask for SBAS settings every 20 seconds + Debug("Asking for SBAS setting\n"); + _send_message(CLASS_CFG, MSG_CFG_SBAS, NULL, 0); + } if (_fix_count == 100) { // ask for nav settings every 20 seconds Debug("Asking for engine setting\n"); diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index a9d96b7aed..01b6b998ab 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -86,6 +86,13 @@ private: uint32_t res3; uint32_t res4; }; + struct PACKED ubx_cfg_sbas { + uint8_t mode; + uint8_t usage; + uint8_t maxSBAS; + uint8_t scanmode2; + uint32_t scanmode1; + }; struct PACKED ubx_nav_posllh { uint32_t time; // GPS msToW @@ -197,6 +204,7 @@ private: ubx_mon_hw_60 mon_hw_60; ubx_mon_hw_68 mon_hw_68; ubx_mon_hw2 mon_hw2; + ubx_cfg_sbas sbas; uint8_t bytes[]; } _buffer; @@ -217,6 +225,7 @@ private: MSG_CFG_RATE = 0x08, MSG_CFG_SET_RATE = 0x01, MSG_CFG_NAV_SETTINGS = 0x24, + MSG_CFG_SBAS = 0x16, MSG_MON_HW = 0x09, MSG_MON_HW2 = 0x0B }; @@ -271,6 +280,7 @@ private: void _configure_navigation_rate(uint16_t rate_ms); 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 send_next_rate_update(void);