/* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . */ // // u-blox GPS driver for ArduPilot // Origin code by Michael Smith, Jordi Munoz and Jose Julio, DIYDrones.com // Substantially rewritten for new GPS driver structure by Andrew Tridgell // #include "AP_GPS.h" #include "AP_GPS_UBLOX.h" #include #include #include #include "RTCM3_Parser.h" #include #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH #define UBLOX_SPEED_CHANGE 1 #else #define UBLOX_SPEED_CHANGE 0 #endif #define UBLOX_DEBUGGING 0 #define UBLOX_FAKE_3DLOCK 0 #ifndef CONFIGURE_PPS_PIN #define CONFIGURE_PPS_PIN 0 #endif // this is number of epochs per output. A higher value will reduce // the uart bandwidth needed and allow for higher latency #define RTK_MB_RTCM_RATE 1 // use this to enable debugging of moving baseline configs #define UBLOX_MB_DEBUGGING 0 extern const AP_HAL::HAL& hal; #if UBLOX_DEBUGGING #if defined(HAL_BUILD_AP_PERIPH) extern "C" { void can_printf(const char *fmt, ...); } # define Debug(fmt, args ...) do {can_printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args);} while(0) #else # define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0) #endif #else # define Debug(fmt, args ...) #endif #if UBLOX_MB_DEBUGGING #if defined(HAL_BUILD_AP_PERIPH) extern "C" { void can_printf(const char *fmt, ...); } # define MB_Debug(fmt, args ...) do {can_printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args);} while(0) #else # define MB_Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0) #endif #else # define MB_Debug(fmt, args ...) #endif AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port, AP_GPS::GPS_Role _role) : AP_GPS_Backend(_gps, _state, _port), _next_message(STEP_PVT), _ublox_port(255), _unconfigured_messages(CONFIG_ALL), _hardware_generation(UBLOX_UNKNOWN_HARDWARE_GENERATION), next_fix(AP_GPS::NO_FIX), noReceivedHdop(true), role(_role) { // stop any config strings that are pending gps.send_blob_start(state.instance, nullptr, 0); // start the process of updating the GPS rates _request_next_config(); #if CONFIGURE_PPS_PIN _unconfigured_messages |= CONFIG_TP5; #endif #if GPS_MOVING_BASELINE if (role == AP_GPS::GPS_ROLE_MB_BASE && !mb_use_uart2()) { rtcm3_parser = new RTCM3_Parser; if (rtcm3_parser == nullptr) { GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "u-blox %d: failed RTCMv3 parser allocation", state.instance + 1); } _unconfigured_messages |= CONFIG_RTK_MOVBASE; } if (role == AP_GPS::GPS_ROLE_MB_ROVER) { _unconfigured_messages |= CONFIG_RTK_MOVBASE; state.gps_yaw_configured = true; } #endif } AP_GPS_UBLOX::~AP_GPS_UBLOX() { #if GPS_MOVING_BASELINE delete rtcm3_parser; #endif } #if GPS_MOVING_BASELINE /* config for F9 GPS in moving baseline base role See ZED-F9P integration manual section 3.1.5.6.1 */ const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_MB_Base_uart1[] { { ConfigKey::CFG_UART1OUTPROT_RTCM3X, 1}, { ConfigKey::CFG_UART2OUTPROT_RTCM3X, 0}, { ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART1, 0}, { ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART2, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART1, RTK_MB_RTCM_RATE}, { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART1, RTK_MB_RTCM_RATE}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART1, RTK_MB_RTCM_RATE}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART1, RTK_MB_RTCM_RATE}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART1, RTK_MB_RTCM_RATE}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART1, RTK_MB_RTCM_RATE}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART1, RTK_MB_RTCM_RATE}, { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART2, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART2, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART2, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART2, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART2, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART2, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART2, 0}, }; const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_MB_Base_uart2[] { { ConfigKey::CFG_UART2_ENABLED, 1}, { ConfigKey::CFG_UART2_BAUDRATE, 460800}, { ConfigKey::CFG_UART2OUTPROT_RTCM3X, 1}, { ConfigKey::CFG_UART1OUTPROT_RTCM3X, 0}, { ConfigKey::CFG_UART1INPROT_RTCM3X, 1}, { ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART2, 0}, { ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART1, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART2, RTK_MB_RTCM_RATE}, { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART2, RTK_MB_RTCM_RATE}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART2, RTK_MB_RTCM_RATE}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART2, RTK_MB_RTCM_RATE}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART2, RTK_MB_RTCM_RATE}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART2, RTK_MB_RTCM_RATE}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART2, RTK_MB_RTCM_RATE}, { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART1, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART1, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART1, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART1, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART1, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART1, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART1, 0}, }; /* config for F9 GPS in moving baseline rover role See ZED-F9P integration manual section 3.1.5.6.1. Note that we list the RTCM msg types as 0 to prevent getting RTCM data from a GPS previously configured as a base */ const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_MB_Rover_uart1[] { { ConfigKey::CFG_UART2OUTPROT_RTCM3X, 0}, { ConfigKey::CFG_UART1INPROT_RTCM3X, 1}, { ConfigKey::CFG_UART2INPROT_RTCM3X, 0}, { ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART1, 1}, { ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART2, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART1, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART1, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART1, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART1, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART1, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART1, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART1, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART2, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART2, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART2, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART2, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART2, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART2, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART2, 0}, }; const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_MB_Rover_uart2[] { { ConfigKey::CFG_UART2_ENABLED, 1}, { ConfigKey::CFG_UART2_BAUDRATE, 460800}, { ConfigKey::CFG_UART2OUTPROT_RTCM3X, 0}, { ConfigKey::CFG_UART2INPROT_RTCM3X, 1}, { ConfigKey::CFG_UART1INPROT_RTCM3X, 0}, { ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART1, 1}, { ConfigKey::MSGOUT_UBX_NAV_RELPOSNED_UART2, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART2, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART2, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART2, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART2, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART2, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART2, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART2, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_0_UART1, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE4072_1_UART1, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1077_UART1, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1087_UART1, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1097_UART1, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART1, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART1, 0}, }; #endif // GPS_MOVING_BASELINE void AP_GPS_UBLOX::_request_next_config(void) { // don't request config if we shouldn't configure the GPS if (gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE) { return; } // Ensure there is enough space for the largest possible outgoing message if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+sizeof(struct ubx_cfg_nav_rate)+2)) { // not enough space - do it next time return; } if (_unconfigured_messages == CONFIG_RATE_SOL && havePvtMsg) { /* we don't need SOL if we have PVT and TIMEGPS. This is needed as F9P doesn't support the SOL message */ _unconfigured_messages &= ~CONFIG_RATE_SOL; } Debug("Unconfigured messages: 0x%x Current message: %u\n", (unsigned)_unconfigured_messages, (unsigned)_next_message); // check AP_GPS_UBLOX.h for the enum that controls the order. // This switch statement isn't maintained against the enum in order to reduce code churn switch (_next_message++) { case STEP_PVT: if(!_request_message_rate(CLASS_NAV, MSG_PVT)) { _next_message--; } break; case STEP_TIMEGPS: if(!_request_message_rate(CLASS_NAV, MSG_TIMEGPS)) { _next_message--; } break; case STEP_PORT: _request_port(); break; case STEP_POLL_SVINFO: // not required once we know what generation we are on if(_hardware_generation == UBLOX_UNKNOWN_HARDWARE_GENERATION) { if (!_send_message(CLASS_NAV, MSG_NAV_SVINFO, 0, 0)) { _next_message--; } } break; case STEP_POLL_SBAS: if (gps._sbas_mode != AP_GPS::SBAS_Mode::DoNotChange) { _send_message(CLASS_CFG, MSG_CFG_SBAS, nullptr, 0); } else { _unconfigured_messages &= ~CONFIG_SBAS; } break; case STEP_POLL_NAV: if (!_send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, nullptr, 0)) { _next_message--; } break; case STEP_POLL_GNSS: if (!_send_message(CLASS_CFG, MSG_CFG_GNSS, nullptr, 0)) { _next_message--; } break; case STEP_POLL_TP5: #if CONFIGURE_PPS_PIN if (!_send_message(CLASS_CFG, MSG_CFG_TP5, nullptr, 0)) { _next_message--; } #endif break; case STEP_NAV_RATE: if (!_send_message(CLASS_CFG, MSG_CFG_RATE, nullptr, 0)) { _next_message--; } break; case STEP_POSLLH: if(!_request_message_rate(CLASS_NAV, MSG_POSLLH)) { _next_message--; } break; case STEP_STATUS: if(!_request_message_rate(CLASS_NAV, MSG_STATUS)) { _next_message--; } break; case STEP_SOL: if(!_request_message_rate(CLASS_NAV, MSG_SOL)) { _next_message--; } break; case STEP_VELNED: if(!_request_message_rate(CLASS_NAV, MSG_VELNED)) { _next_message--; } break; case STEP_DOP: if(! _request_message_rate(CLASS_NAV, MSG_DOP)) { _next_message--; } break; case STEP_MON_HW: if(!_request_message_rate(CLASS_MON, MSG_MON_HW)) { _next_message--; } break; case STEP_MON_HW2: if(!_request_message_rate(CLASS_MON, MSG_MON_HW2)) { _next_message--; } break; case STEP_RAW: #if UBLOX_RXM_RAW_LOGGING if(gps._raw_data == 0) { _unconfigured_messages &= ~CONFIG_RATE_RAW; } else if(!_request_message_rate(CLASS_RXM, MSG_RXM_RAW)) { _next_message--; } #else _unconfigured_messages & = ~CONFIG_RATE_RAW; #endif break; case STEP_RAWX: #if UBLOX_RXM_RAW_LOGGING if(gps._raw_data == 0) { _unconfigured_messages &= ~CONFIG_RATE_RAW; } else if(!_request_message_rate(CLASS_RXM, MSG_RXM_RAWX)) { _next_message--; } #else _unconfigured_messages & = ~CONFIG_RATE_RAW; #endif break; case STEP_VERSION: if(!_have_version && !hal.util->get_soft_armed()) { _request_version(); } else { _unconfigured_messages &= ~CONFIG_VERSION; } break; case STEP_TMODE: if (supports_F9_config()) { if (!_configure_valget(ConfigKey::TMODE_MODE)) { _next_message--; } } break; case STEP_RTK_MOVBASE: #if GPS_MOVING_BASELINE if (supports_F9_config()) { static_assert(sizeof(active_config.done_mask)*8 >= ARRAY_SIZE(config_MB_Base_uart1), "done_mask too small, base1"); static_assert(sizeof(active_config.done_mask)*8 >= ARRAY_SIZE(config_MB_Base_uart2), "done_mask too small, base2"); static_assert(sizeof(active_config.done_mask)*8 >= ARRAY_SIZE(config_MB_Rover_uart1), "done_mask too small, rover1"); static_assert(sizeof(active_config.done_mask)*8 >= ARRAY_SIZE(config_MB_Rover_uart2), "done_mask too small, rover2"); if (role == AP_GPS::GPS_ROLE_MB_BASE) { const config_list *list = mb_use_uart2()?config_MB_Base_uart2:config_MB_Base_uart1; uint8_t list_length = mb_use_uart2()?ARRAY_SIZE(config_MB_Base_uart2):ARRAY_SIZE(config_MB_Base_uart1); if (!_configure_config_set(list, list_length, CONFIG_RTK_MOVBASE)) { _next_message--; } } if (role == AP_GPS::GPS_ROLE_MB_ROVER) { const config_list *list = mb_use_uart2()?config_MB_Rover_uart2:config_MB_Rover_uart1; uint8_t list_length = mb_use_uart2()?ARRAY_SIZE(config_MB_Rover_uart2):ARRAY_SIZE(config_MB_Rover_uart1); if (!_configure_config_set(list, list_length, CONFIG_RTK_MOVBASE)) { _next_message--; } } } #endif break; default: // this case should never be reached, do a full reset if it is hit _next_message = STEP_PVT; break; } } void AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) { uint8_t desired_rate; switch(msg_class) { case CLASS_NAV: switch(msg_id) { case MSG_POSLLH: desired_rate = havePvtMsg ? 0 : RATE_POSLLH; if(rate == desired_rate) { _unconfigured_messages &= ~CONFIG_RATE_POSLLH; } else { _configure_message_rate(msg_class, msg_id, desired_rate); _unconfigured_messages |= CONFIG_RATE_POSLLH; _cfg_needs_save = true; } break; case MSG_STATUS: desired_rate = havePvtMsg ? 0 : RATE_STATUS; if(rate == desired_rate) { _unconfigured_messages &= ~CONFIG_RATE_STATUS; } else { _configure_message_rate(msg_class, msg_id, desired_rate); _unconfigured_messages |= CONFIG_RATE_STATUS; _cfg_needs_save = true; } break; case MSG_SOL: desired_rate = havePvtMsg ? 0 : RATE_SOL; if(rate == desired_rate) { _unconfigured_messages &= ~CONFIG_RATE_SOL; } else { _configure_message_rate(msg_class, msg_id, desired_rate); _unconfigured_messages |= CONFIG_RATE_SOL; _cfg_needs_save = true; } break; case MSG_PVT: if(rate == RATE_PVT) { _unconfigured_messages &= ~CONFIG_RATE_PVT; } else { _configure_message_rate(msg_class, msg_id, RATE_PVT); _unconfigured_messages |= CONFIG_RATE_PVT; _cfg_needs_save = true; } break; case MSG_TIMEGPS: if(rate == RATE_TIMEGPS) { _unconfigured_messages &= ~CONFIG_RATE_TIMEGPS; } else { _configure_message_rate(msg_class, msg_id, RATE_TIMEGPS); _unconfigured_messages |= CONFIG_RATE_TIMEGPS; _cfg_needs_save = true; } break; case MSG_VELNED: desired_rate = havePvtMsg ? 0 : RATE_VELNED; if(rate == desired_rate) { _unconfigured_messages &= ~CONFIG_RATE_VELNED; } else { _configure_message_rate(msg_class, msg_id, desired_rate); _unconfigured_messages |= CONFIG_RATE_VELNED; _cfg_needs_save = true; } break; case MSG_DOP: if(rate == RATE_DOP) { _unconfigured_messages &= ~CONFIG_RATE_DOP; } else { _configure_message_rate(msg_class, msg_id, RATE_DOP); _unconfigured_messages |= CONFIG_RATE_DOP; _cfg_needs_save = true; } break; } break; case CLASS_MON: switch(msg_id) { case MSG_MON_HW: if(rate == RATE_HW) { _unconfigured_messages &= ~CONFIG_RATE_MON_HW; } else { _configure_message_rate(msg_class, msg_id, RATE_HW); _unconfigured_messages |= CONFIG_RATE_MON_HW; _cfg_needs_save = true; } break; case MSG_MON_HW2: if(rate == RATE_HW2) { _unconfigured_messages &= ~CONFIG_RATE_MON_HW2; } else { _configure_message_rate(msg_class, msg_id, RATE_HW2); _unconfigured_messages |= CONFIG_RATE_MON_HW2; _cfg_needs_save = true; } break; } break; #if UBLOX_RXM_RAW_LOGGING case CLASS_RXM: switch(msg_id) { case MSG_RXM_RAW: if(rate == gps._raw_data) { _unconfigured_messages &= ~CONFIG_RATE_RAW; } else { _configure_message_rate(msg_class, msg_id, gps._raw_data); _unconfigured_messages |= CONFIG_RATE_RAW; _cfg_needs_save = true; } break; case MSG_RXM_RAWX: if(rate == gps._raw_data) { _unconfigured_messages &= ~CONFIG_RATE_RAW; } else { _configure_message_rate(msg_class, msg_id, gps._raw_data); _unconfigured_messages |= CONFIG_RATE_RAW; _cfg_needs_save = true; } break; } break; #endif // UBLOX_RXM_RAW_LOGGING } } // Requests the ublox driver to identify what port we are using to communicate void AP_GPS_UBLOX::_request_port(void) { if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+2)) { // not enough space - do it next time return; } _send_message(CLASS_CFG, MSG_CFG_PRT, nullptr, 0); } // Ensure there is enough space for the largest possible outgoing message // Process bytes available from the stream // // The stream is assumed to contain only messages we recognise. If it // contains other messages, and those messages contain the preamble // bytes, it is possible for this code to fail to synchronise to the // stream immediately. Without buffering the entire message and // re-processing it from the top, this is unavoidable. The parser // attempts to avoid this when possible. // bool AP_GPS_UBLOX::read(void) { uint8_t data; int16_t numc; bool parsed = false; uint32_t millis_now = AP_HAL::millis(); // walk through the gps configuration at 1 message per second if (millis_now - _last_config_time >= _delay_time) { _request_next_config(); _last_config_time = millis_now; if (_unconfigured_messages) { // send the updates faster until fully configured if (!havePvtMsg && (_unconfigured_messages & CONFIG_REQUIRED_INITIAL)) { _delay_time = 300; } else { _delay_time = 750; } } else { _delay_time = 2000; } } if(!_unconfigured_messages && gps._save_config && !_cfg_saved && _num_cfg_save_tries < 5 && (millis_now - _last_cfg_sent_time) > 5000 && !hal.util->get_soft_armed()) { //save the configuration sent until now if (gps._save_config == 1 || (gps._save_config == 2 && _cfg_needs_save)) { _save_cfg(); } } numc = port->available(); for (int16_t i = 0; i < numc; i++) { // Process bytes received // read the next byte data = port->read(); #if GPS_MOVING_BASELINE if (rtcm3_parser) { if (rtcm3_parser->read(data)) { // we've found a RTCMv3 packet. We stop parsing at // this point and reset u-blox parse state. We need to // stop parsing to give the higher level driver a // chance to send the RTCMv3 packet to another (rover) // GPS _step = 0; break; } } #endif reset: switch(_step) { // Message preamble detection // // If we fail to match any of the expected bytes, we reset // the state machine and re-consider the failed byte as // the first byte of the preamble. This improves our // chances of recovering from a mismatch and makes it less // likely that we will be fooled by the preamble appearing // as data in some other message. // case 1: if (PREAMBLE2 == data) { _step++; break; } _step = 0; Debug("reset %u", __LINE__); FALLTHROUGH; case 0: if(PREAMBLE1 == data) _step++; break; // Message header processing // // We sniff the class and message ID to decide whether we // are going to gather the message bytes or just discard // them. // // We always collect the length so that we can avoid being // fooled by preamble bytes in messages. // case 2: _step++; _class = data; _ck_b = _ck_a = data; // reset the checksum accumulators break; case 3: _step++; _ck_b += (_ck_a += data); // checksum byte _msg_id = data; break; case 4: _step++; _ck_b += (_ck_a += data); // checksum byte _payload_length = data; // payload length low byte break; case 5: _step++; _ck_b += (_ck_a += data); // checksum byte _payload_length += (uint16_t)(data<<8); if (_payload_length > sizeof(_buffer)) { Debug("large payload %u", (unsigned)_payload_length); // assume any payload bigger then what we know about is noise _payload_length = 0; _step = 0; goto reset; } _payload_counter = 0; // prepare to receive payload if (_payload_length == 0) { // bypass payload and go straight to checksum _step++; } break; // Receive message data // case 6: _ck_b += (_ck_a += data); // checksum byte if (_payload_counter < sizeof(_buffer)) { _buffer[_payload_counter] = data; } if (++_payload_counter == _payload_length) _step++; break; // Checksum and message processing // case 7: _step++; if (_ck_a != data) { Debug("bad cka %x should be %x", data, _ck_a); _step = 0; goto reset; } break; case 8: _step = 0; if (_ck_b != data) { Debug("bad ckb %x should be %x", data, _ck_b); break; // bad checksum } #if GPS_MOVING_BASELINE if (rtcm3_parser) { // this is a uBlox packet, discard any partial RTCMv3 state rtcm3_parser->reset(); } #endif if (_parse_gps()) { parsed = true; } break; } } return parsed; } // Private Methods ///////////////////////////////////////////////////////////// void AP_GPS_UBLOX::log_mon_hw(void) { #if HAL_LOGGING_ENABLED if (!should_log()) { return; } struct log_Ubx1 pkt = { LOG_PACKET_HEADER_INIT(LOG_GPS_UBX1_MSG), time_us : AP_HAL::micros64(), instance : state.instance, noisePerMS : _buffer.mon_hw_60.noisePerMS, jamInd : _buffer.mon_hw_60.jamInd, aPower : _buffer.mon_hw_60.aPower, agcCnt : _buffer.mon_hw_60.agcCnt, config : _unconfigured_messages, }; if (_payload_length == 68) { pkt.noisePerMS = _buffer.mon_hw_68.noisePerMS; pkt.jamInd = _buffer.mon_hw_68.jamInd; pkt.aPower = _buffer.mon_hw_68.aPower; pkt.agcCnt = _buffer.mon_hw_68.agcCnt; } AP::logger().WriteBlock(&pkt, sizeof(pkt)); #endif } void AP_GPS_UBLOX::log_mon_hw2(void) { #if HAL_LOGGING_ENABLED if (!should_log()) { return; } struct log_Ubx2 pkt = { LOG_PACKET_HEADER_INIT(LOG_GPS_UBX2_MSG), time_us : AP_HAL::micros64(), instance : state.instance, ofsI : _buffer.mon_hw2.ofsI, magI : _buffer.mon_hw2.magI, ofsQ : _buffer.mon_hw2.ofsQ, magQ : _buffer.mon_hw2.magQ, }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); #endif } #if UBLOX_RXM_RAW_LOGGING void AP_GPS_UBLOX::log_rxm_raw(const struct ubx_rxm_raw &raw) { #if HAL_LOGGING_ENABLED if (!should_log()) { return; } uint64_t now = AP_HAL::micros64(); for (uint8_t i=0; i> 28) & 0x07; // mask off the storage size switch (key_size) { case 0x1: // bit case 0x2: // byte return 1; case 0x3: // 2 bytes return 2; case 0x4: // 4 bytes return 4; case 0x5: // 8 bytes return 8; default: break; } // invalid return 0; } /* find index of a config key in the active_config list, or -1 */ int8_t AP_GPS_UBLOX::find_active_config_index(ConfigKey key) const { #if GPS_MOVING_BASELINE if (active_config.list == nullptr) { return -1; } for (uint8_t i=0; i UBLOX_M8 || GNSS_GALILEO !=_buffer.gnss.configBlock[i].gnssId)) { _buffer.gnss.configBlock[i].resTrkCh = (_buffer.gnss.numTrkChHw - 3) / (gnssCount * 2); _buffer.gnss.configBlock[i].maxTrkCh = _buffer.gnss.numTrkChHw; } else { if(GNSS_SBAS ==_buffer.gnss.configBlock[i].gnssId) { _buffer.gnss.configBlock[i].resTrkCh = 1; _buffer.gnss.configBlock[i].maxTrkCh = 3; } if(GNSS_GALILEO ==_buffer.gnss.configBlock[i].gnssId) { _buffer.gnss.configBlock[i].resTrkCh = (_buffer.gnss.numTrkChHw - 3) / (gnssCount * 2); _buffer.gnss.configBlock[i].maxTrkCh = 8; //Per the M8 receiver description UBX-13003221 - R16, 4.1.1.3 it is not recommended to set the number of galileo channels higher then eigh } } _buffer.gnss.configBlock[i].flags = _buffer.gnss.configBlock[i].flags | 0x00000001; } else { _buffer.gnss.configBlock[i].resTrkCh = 0; _buffer.gnss.configBlock[i].maxTrkCh = 0; _buffer.gnss.configBlock[i].flags = _buffer.gnss.configBlock[i].flags & 0xFFFFFFFE; } } if (memcmp(&start_gnss, &_buffer.gnss, sizeof(start_gnss))) { _send_message(CLASS_CFG, MSG_CFG_GNSS, &_buffer.gnss, 4 + (8 * _buffer.gnss.numConfigBlocks)); _unconfigured_messages |= CONFIG_GNSS; _cfg_needs_save = true; } else { _unconfigured_messages &= ~CONFIG_GNSS; } } else { _unconfigured_messages &= ~CONFIG_GNSS; } return false; #endif case MSG_CFG_SBAS: if (gps._sbas_mode != AP_GPS::SBAS_Mode::DoNotChange) { 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)); _unconfigured_messages |= CONFIG_SBAS; _cfg_needs_save = true; } else { _unconfigured_messages &= ~CONFIG_SBAS; } } else { _unconfigured_messages &= ~CONFIG_SBAS; } return false; case MSG_CFG_MSG: if(_payload_length == sizeof(ubx_cfg_msg_rate_6)) { // can't verify the setting without knowing the port // request the port again if(_ublox_port >= UBLOX_MAX_PORTS) { _request_port(); return false; } _verify_rate(_buffer.msg_rate_6.msg_class, _buffer.msg_rate_6.msg_id, _buffer.msg_rate_6.rates[_ublox_port]); } else { _verify_rate(_buffer.msg_rate.msg_class, _buffer.msg_rate.msg_id, _buffer.msg_rate.rate); } return false; case MSG_CFG_PRT: _ublox_port = _buffer.prt.portID; return false; case MSG_CFG_RATE: if(_buffer.nav_rate.measure_rate_ms != gps._rate_ms[state.instance] || _buffer.nav_rate.nav_rate != 1 || _buffer.nav_rate.timeref != 0) { _configure_rate(); _unconfigured_messages |= CONFIG_RATE_NAV; _cfg_needs_save = true; } else { _unconfigured_messages &= ~CONFIG_RATE_NAV; } return false; #if CONFIGURE_PPS_PIN case MSG_CFG_TP5: { // configure the PPS pin for 1Hz, zero delay Debug("Got TP5 ver=%u 0x%04x %u\n", (unsigned)_buffer.nav_tp5.version, (unsigned)_buffer.nav_tp5.flags, (unsigned)_buffer.nav_tp5.freqPeriod); const uint16_t desired_flags = 0x003f; const uint16_t desired_period_hz = 1; if (_buffer.nav_tp5.flags != desired_flags || _buffer.nav_tp5.freqPeriod != desired_period_hz) { _buffer.nav_tp5.tpIdx = 0; _buffer.nav_tp5.reserved1[0] = 0; _buffer.nav_tp5.reserved1[1] = 0; _buffer.nav_tp5.antCableDelay = 0; _buffer.nav_tp5.rfGroupDelay = 0; _buffer.nav_tp5.freqPeriod = desired_period_hz; _buffer.nav_tp5.freqPeriodLock = desired_period_hz; _buffer.nav_tp5.pulseLenRatio = 1; _buffer.nav_tp5.pulseLenRatioLock = 2; _buffer.nav_tp5.userConfigDelay = 0; _buffer.nav_tp5.flags = desired_flags; _send_message(CLASS_CFG, MSG_CFG_TP5, &_buffer.nav_tp5, sizeof(_buffer.nav_tp5)); _unconfigured_messages |= CONFIG_TP5; _cfg_needs_save = true; } else { _unconfigured_messages &= ~CONFIG_TP5; } return false; } #endif // CONFIGURE_PPS_PIN case MSG_CFG_VALGET: { uint8_t cfg_len = _payload_length - sizeof(ubx_cfg_valget); const uint8_t *cfg_data = (const uint8_t *)(&_buffer) + sizeof(ubx_cfg_valget); while (cfg_len >= 5) { ConfigKey id; memcpy(&id, cfg_data, sizeof(uint32_t)); cfg_len -= 4; cfg_data += 4; switch (id) { case ConfigKey::TMODE_MODE: { uint8_t mode = cfg_data[0]; if (mode != 0) { // ask for mode 0, to disable TIME mode mode = 0; _configure_valset(ConfigKey::TMODE_MODE, &mode); _cfg_needs_save = true; _unconfigured_messages |= CONFIG_TMODE_MODE; } else { _unconfigured_messages &= ~CONFIG_TMODE_MODE; } break; } default: break; } #if GPS_MOVING_BASELINE // see if it is in active config list int8_t cfg_idx = find_active_config_index(id); if (cfg_idx >= 0) { const uint8_t key_size = config_key_size(id); if (cfg_len < key_size || memcmp(&active_config.list[cfg_idx].value, cfg_data, key_size) != 0) { _configure_valset(id, &active_config.list[cfg_idx].value); _unconfigured_messages |= active_config.unconfig_bit; active_config.done_mask &= ~(1U << cfg_idx); _cfg_needs_save = true; } else { active_config.done_mask |= (1U << cfg_idx); if (active_config.done_mask == (1U<= AP_GPS::GPS_OK_FIX_2D) { state.last_gps_time_ms = AP_HAL::millis(); state.time_week_ms = _buffer.solution.itow; state.time_week = _buffer.solution.week; } #if UBLOX_FAKE_3DLOCK next_fix = state.status; state.num_sats = 10; state.time_week = 1721; state.time_week_ms = AP_HAL::millis() + 3*60*60*1000 + 37000; state.last_gps_time_ms = AP_HAL::millis(); state.hdop = 130; #endif break; #if GPS_MOVING_BASELINE case MSG_RELPOSNED: { // note that we require the yaw to come from a fixed solution, not a float solution // yaw from a float solution would only be acceptable with a very large separation between // GPS modules const uint32_t valid_mask = static_cast(RELPOSNED::relPosHeadingValid) | static_cast(RELPOSNED::relPosValid) | static_cast(RELPOSNED::gnssFixOK) | static_cast(RELPOSNED::isMoving) | static_cast(RELPOSNED::carrSolnFixed); const uint32_t invalid_mask = static_cast(RELPOSNED::refPosMiss) | static_cast(RELPOSNED::refObsMiss) | static_cast(RELPOSNED::carrSolnFloat); _check_new_itow(_buffer.relposned.iTOW); if (_buffer.relposned.iTOW != _last_relposned_itow+200) { // useful for looking at packet loss on links MB_Debug("RELPOSNED ITOW %u %u\n", unsigned(_buffer.relposned.iTOW), unsigned(_last_relposned_itow)); } _last_relposned_itow = _buffer.relposned.iTOW; MB_Debug("RELPOSNED flags: %lx valid: %lx invalid: %lx\n", _buffer.relposned.flags, valid_mask, invalid_mask); if (((_buffer.relposned.flags & valid_mask) == valid_mask) && ((_buffer.relposned.flags & invalid_mask) == 0)) { if (calculate_moving_base_yaw(_buffer.relposned.relPosHeading * 1e-5, _buffer.relposned.relPosLength * 0.01, _buffer.relposned.relPosD*0.01)) { state.have_gps_yaw_accuracy = true; state.gps_yaw_accuracy = _buffer.relposned.accHeading * 1e-5; _last_relposned_ms = AP_HAL::millis(); } state.relPosHeading = _buffer.relposned.relPosHeading * 1e-5; state.relPosLength = _buffer.relposned.relPosLength * 0.01; state.relPosD = _buffer.relposned.relPosD * 0.01; state.accHeading = _buffer.relposned.accHeading * 1e-5; state.relposheading_ts = AP_HAL::millis(); } else { state.have_gps_yaw_accuracy = false; } } break; #endif // GPS_MOVING_BASELINE case MSG_PVT: Debug("MSG_PVT"); havePvtMsg = true; // position _check_new_itow(_buffer.pvt.itow); _last_pvt_itow = _buffer.pvt.itow; _last_pos_time = _buffer.pvt.itow; state.location.lng = _buffer.pvt.lon; state.location.lat = _buffer.pvt.lat; state.location.alt = _buffer.pvt.h_msl / 10; switch (_buffer.pvt.fix_type) { case 0: state.status = AP_GPS::NO_FIX; break; case 1: state.status = AP_GPS::NO_FIX; break; case 2: state.status = AP_GPS::GPS_OK_FIX_2D; break; case 3: state.status = AP_GPS::GPS_OK_FIX_3D; if (_buffer.pvt.flags & 0b00000010) // diffsoln state.status = AP_GPS::GPS_OK_FIX_3D_DGPS; if (_buffer.pvt.flags & 0b01000000) // carrsoln - float state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT; if (_buffer.pvt.flags & 0b10000000) // carrsoln - fixed state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED; break; case 4: GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Unexpected state %d", _buffer.pvt.flags); state.status = AP_GPS::GPS_OK_FIX_3D; break; case 5: state.status = AP_GPS::NO_FIX; break; default: state.status = AP_GPS::NO_FIX; break; } next_fix = state.status; _new_position = true; state.horizontal_accuracy = _buffer.pvt.h_acc*1.0e-3f; state.vertical_accuracy = _buffer.pvt.v_acc*1.0e-3f; state.have_horizontal_accuracy = true; state.have_vertical_accuracy = true; // SVs state.num_sats = _buffer.pvt.num_sv; // velocity _last_vel_time = _buffer.pvt.itow; state.ground_speed = _buffer.pvt.gspeed*0.001f; // m/s state.ground_course = wrap_360(_buffer.pvt.head_mot * 1.0e-5f); // Heading 2D deg * 100000 state.have_vertical_velocity = true; state.velocity.x = _buffer.pvt.velN * 0.001f; state.velocity.y = _buffer.pvt.velE * 0.001f; state.velocity.z = _buffer.pvt.velD * 0.001f; state.have_speed_accuracy = true; state.speed_accuracy = _buffer.pvt.s_acc*0.001f; _new_speed = true; // dop if(noReceivedHdop) { state.hdop = _buffer.pvt.p_dop; state.vdop = _buffer.pvt.p_dop; } state.last_gps_time_ms = AP_HAL::millis(); // time state.time_week_ms = _buffer.pvt.itow; #if UBLOX_FAKE_3DLOCK state.location.lng = 1491652300L; state.location.lat = -353632610L; state.location.alt = 58400; state.vertical_accuracy = 0; state.horizontal_accuracy = 0; state.status = AP_GPS::GPS_OK_FIX_3D; state.num_sats = 10; state.time_week = 1721; state.time_week_ms = AP_HAL::millis() + 3*60*60*1000 + 37000; state.last_gps_time_ms = AP_HAL::millis(); state.hdop = 130; state.speed_accuracy = 0; next_fix = state.status; #endif break; case MSG_TIMEGPS: Debug("MSG_TIMEGPS"); _check_new_itow(_buffer.timegps.itow); if (_buffer.timegps.valid & UBX_TIMEGPS_VALID_WEEK_MASK) { state.time_week = _buffer.timegps.week; } break; case MSG_VELNED: Debug("MSG_VELNED"); if (havePvtMsg) { _unconfigured_messages |= CONFIG_RATE_VELNED; break; } _check_new_itow(_buffer.velned.itow); _last_vel_time = _buffer.velned.itow; state.ground_speed = _buffer.velned.speed_2d*0.01f; // m/s state.ground_course = wrap_360(_buffer.velned.heading_2d * 1.0e-5f); // Heading 2D deg * 100000 state.have_vertical_velocity = true; 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.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x))); state.ground_speed = state.velocity.xy().length(); state.have_speed_accuracy = true; state.speed_accuracy = _buffer.velned.speed_accuracy*0.01f; #if UBLOX_FAKE_3DLOCK state.speed_accuracy = 0; #endif _new_speed = true; break; case MSG_NAV_SVINFO: { Debug("MSG_NAV_SVINFO\n"); static const uint8_t HardwareGenerationMask = 0x07; _check_new_itow(_buffer.svinfo_header.itow); _hardware_generation = _buffer.svinfo_header.globalFlags & HardwareGenerationMask; switch (_hardware_generation) { case UBLOX_5: case UBLOX_6: // only 7 and newer support CONFIG_GNSS _unconfigured_messages &= ~CONFIG_GNSS; break; case UBLOX_7: case UBLOX_M8: #if UBLOX_SPEED_CHANGE port->begin(4000000U); Debug("Changed speed to 4Mhz for SPI-driven UBlox\n"); #endif break; default: hal.console->printf("Wrong Ublox Hardware Version%u\n", _hardware_generation); break; }; _unconfigured_messages &= ~CONFIG_VERSION; /* We don't need that anymore */ _configure_message_rate(CLASS_NAV, MSG_NAV_SVINFO, 0); break; } default: Debug("Unexpected NAV message 0x%02x", (unsigned)_msg_id); if (++_disable_counter == 0) { Debug("Disabling NAV message 0x%02x", (unsigned)_msg_id); _configure_message_rate(CLASS_NAV, _msg_id, 0); } return false; } if (state.have_gps_yaw) { // when we are a rover we want to ensure we have both the new // PVT and the new RELPOSNED message so that we give a // consistent view if (AP_HAL::millis() - _last_relposned_ms > 400) { // we have stopped receiving valid RELPOSNED messages, disable yaw reporting state.have_gps_yaw = false; } else if (_last_relposned_itow != _last_pvt_itow) { // wait until ITOW matches return false; } } // we only return true when we get new position and speed data // this ensures we don't use stale data if (_new_position && _new_speed && _last_vel_time == _last_pos_time) { _new_speed = _new_position = false; return true; } return false; } // UBlox auto configuration /* * update checksum for a set of bytes */ void AP_GPS_UBLOX::_update_checksum(uint8_t *data, uint16_t len, uint8_t &ck_a, uint8_t &ck_b) { while (len--) { ck_a += *data; ck_b += ck_a; data++; } } /* * send a ublox message */ bool AP_GPS_UBLOX::_send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint16_t size) { if (port->txspace() < (sizeof(struct ubx_header) + 2 + size)) { return false; } struct ubx_header header; uint8_t ck_a=0, ck_b=0; header.preamble1 = PREAMBLE1; header.preamble2 = PREAMBLE2; header.msg_class = msg_class; header.msg_id = msg_id; header.length = size; _update_checksum((uint8_t *)&header.msg_class, sizeof(header)-2, ck_a, ck_b); _update_checksum((uint8_t *)msg, size, ck_a, ck_b); port->write((const uint8_t *)&header, sizeof(header)); port->write((const uint8_t *)msg, size); port->write((const uint8_t *)&ck_a, 1); port->write((const uint8_t *)&ck_b, 1); return true; } /* * requests the given message rate for a specific message class * and msg_id * returns true if it sent the request, false if waiting on knowing the port */ bool AP_GPS_UBLOX::_request_message_rate(uint8_t msg_class, uint8_t msg_id) { // Without knowing what communication port is being used it isn't possible to verify // always ensure we have a port before sending the request if(_ublox_port >= UBLOX_MAX_PORTS) { _request_port(); return false; } else { struct ubx_cfg_msg msg; msg.msg_class = msg_class; msg.msg_id = msg_id; return _send_message(CLASS_CFG, MSG_CFG_MSG, &msg, sizeof(msg)); } } /* * configure a UBlox GPS for the given message rate for a specific * message class and msg_id */ bool AP_GPS_UBLOX::_configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) { if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+sizeof(struct ubx_cfg_msg_rate)+2)) { return false; } struct ubx_cfg_msg_rate msg; msg.msg_class = msg_class; msg.msg_id = msg_id; msg.rate = rate; return _send_message(CLASS_CFG, MSG_CFG_MSG, &msg, sizeof(msg)); } /* * configure F9 based key/value pair - VALSET */ bool AP_GPS_UBLOX::_configure_valset(ConfigKey key, const void *value) { if (!supports_F9_config()) { return false; } const uint8_t len = config_key_size(key); struct ubx_cfg_valset msg {}; uint8_t buf[sizeof(msg)+len]; if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+sizeof(buf)+2)) { return false; } msg.version = 1; msg.layers = 7; // all layers msg.transaction = 0; msg.key = uint32_t(key); memcpy(buf, &msg, sizeof(msg)); memcpy(&buf[sizeof(msg)], value, len); auto ret = _send_message(CLASS_CFG, MSG_CFG_VALSET, buf, sizeof(buf)); return ret; } /* * configure F9 based key/value pair - VALGET */ bool AP_GPS_UBLOX::_configure_valget(ConfigKey key) { if (!supports_F9_config()) { return false; } struct { struct ubx_cfg_valget msg; ConfigKey key; } msg {}; if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+sizeof(msg)+2)) { return false; } msg.msg.version = 0; msg.msg.layers = 0; // ram msg.key = key; return _send_message(CLASS_CFG, MSG_CFG_VALGET, &msg, sizeof(msg)); } /* * configure F9 based key/value pair for a complete config list */ bool AP_GPS_UBLOX::_configure_config_set(const config_list *list, uint8_t count, uint32_t unconfig_bit) { #if GPS_MOVING_BASELINE active_config.list = list; active_config.count = count; active_config.done_mask = 0; active_config.unconfig_bit = unconfig_bit; uint8_t buf[sizeof(ubx_cfg_valget)+count*sizeof(ConfigKey)]; struct ubx_cfg_valget msg {}; if (port->txspace() < (uint16_t)(sizeof(struct ubx_header)+sizeof(buf)+2)) { return false; } msg.version = 0; msg.layers = 0; // ram memcpy(buf, &msg, sizeof(msg)); for (uint8_t i=0; iget_len(bytes); return len > 0; } #endif return false; } // clear previous RTCM3 packet void AP_GPS_UBLOX::clear_RTCMV3(void) { #if GPS_MOVING_BASELINE if (rtcm3_parser) { rtcm3_parser->clear_packet(); } #endif } // ublox specific healthy checks bool AP_GPS_UBLOX::is_healthy(void) const { #if CONFIG_HAL_BOARD == HAL_BOARD_SITL if (gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE) { // allow for fake ublox moving baseline return true; } #endif #if GPS_MOVING_BASELINE if ((role == AP_GPS::GPS_ROLE_MB_BASE || role == AP_GPS::GPS_ROLE_MB_ROVER) && !supports_F9_config()) { // need F9 or above for moving baseline return false; } if (role == AP_GPS::GPS_ROLE_MB_BASE && rtcm3_parser == nullptr && !mb_use_uart2()) { // we haven't initialised RTCMv3 parser return false; } #endif return true; } // return true if GPS is capable of F9 config bool AP_GPS_UBLOX::supports_F9_config(void) const { return _hardware_generation == UBLOX_F9 && _hardware_generation != UBLOX_UNKNOWN_HARDWARE_GENERATION; }