From e858a0fab7033e4c36daf3ec936c5bdfe55bccef Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 13 Apr 2022 23:04:21 +1000 Subject: [PATCH] AP_GPS: factor out a _detect_instance method This means we don't need the goto to handle the case of a detected GPS, and it also allows for restructure remove the "else" statements, which will allow compilinmg out the uBlox driver --- libraries/AP_GPS/AP_GPS.cpp | 98 ++++++++++++++++++------------------- libraries/AP_GPS/AP_GPS.h | 4 ++ 2 files changed, 53 insertions(+), 49 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 14e7ed7bb2..997f4404e6 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -620,24 +620,41 @@ void AP_GPS::send_blob_update(uint8_t instance) */ void AP_GPS::detect_instance(uint8_t instance) { - AP_GPS_Backend *new_gps = nullptr; - struct detect_state *dstate = &detect_state[instance]; const uint32_t now = AP_HAL::millis(); state[instance].status = NO_GPS; state[instance].hdop = GPS_UNKNOWN_DOP; state[instance].vdop = GPS_UNKNOWN_DOP; + AP_GPS_Backend *new_gps = _detect_instance(instance); + if (new_gps == nullptr) { + return; + } + + state[instance].status = NO_FIX; + drivers[instance] = new_gps; + timing[instance].last_message_time_ms = now; + timing[instance].delta_time_ms = GPS_TIMEOUT_MS; + + new_gps->broadcast_gps_type(); +} + +/* + run detection step for one GPS instance. If this finds a GPS then it + will return it - otherwise nullptr + */ +AP_GPS_Backend *AP_GPS::_detect_instance(uint8_t instance) +{ + struct detect_state *dstate = &detect_state[instance]; + switch (_type[instance]) { // user has to explicitly set the MAV type, do not use AUTO // do not try to detect the MAV type, assume it's there case GPS_TYPE_MAV: #if AP_GPS_MAV_ENABLED dstate->auto_detected_baud = false; // specified, not detected - new_gps = new AP_GPS_MAV(*this, state[instance], nullptr); - goto found_gps; + return new AP_GPS_MAV(*this, state[instance], nullptr); #endif //AP_GPS_MAV_ENABLED - break; // user has to explicitly set the UAVCAN type, do not use AUTO case GPS_TYPE_UAVCAN: @@ -645,21 +662,18 @@ void AP_GPS::detect_instance(uint8_t instance) case GPS_TYPE_UAVCAN_RTK_ROVER: #if HAL_ENABLE_LIBUAVCAN_DRIVERS dstate->auto_detected_baud = false; // specified, not detected - new_gps = AP_GPS_UAVCAN::probe(*this, state[instance]); - goto found_gps; + return AP_GPS_UAVCAN::probe(*this, state[instance]); #endif - return; // We don't do anything here if UAVCAN is not supported + return nullptr; // We don't do anything here if UAVCAN is not supported #if HAL_MSP_GPS_ENABLED case GPS_TYPE_MSP: dstate->auto_detected_baud = false; // specified, not detected - new_gps = new AP_GPS_MSP(*this, state[instance], nullptr); - goto found_gps; + return new AP_GPS_MSP(*this, state[instance], nullptr); #endif #if HAL_EXTERNAL_AHRS_ENABLED case GPS_TYPE_EXTERNAL_AHRS: dstate->auto_detected_baud = false; // specified, not detected - new_gps = new AP_GPS_ExternalAHRS(*this, state[instance], nullptr); - goto found_gps; + return new AP_GPS_ExternalAHRS(*this, state[instance], nullptr); #endif default: break; @@ -667,7 +681,7 @@ void AP_GPS::detect_instance(uint8_t instance) if (_port[instance] == nullptr) { // UART not available - return; + return nullptr; } // all remaining drivers automatically cycle through baud rates to detect @@ -678,30 +692,28 @@ void AP_GPS::detect_instance(uint8_t instance) #if AP_GPS_SBF_ENABLED // by default the sbf/trimble gps outputs no data on its port, until configured. case GPS_TYPE_SBF: - new_gps = new AP_GPS_SBF(*this, state[instance], _port[instance]); - break; + return new AP_GPS_SBF(*this, state[instance], _port[instance]); #endif //AP_GPS_SBF_ENABLED #if AP_GPS_GSOF_ENABLED case GPS_TYPE_GSOF: - new_gps = new AP_GPS_GSOF(*this, state[instance], _port[instance]); - break; + return new AP_GPS_GSOF(*this, state[instance], _port[instance]); #endif //AP_GPS_GSOF_ENABLED #if AP_GPS_NOVA_ENABLED case GPS_TYPE_NOVA: - new_gps = new AP_GPS_NOVA(*this, state[instance], _port[instance]); - break; + return new AP_GPS_NOVA(*this, state[instance], _port[instance]); #endif //AP_GPS_NOVA_ENABLED #if HAL_SIM_GPS_ENABLED case GPS_TYPE_SITL: - new_gps = new AP_GPS_SITL(*this, state[instance], _port[instance]); - break; + return new AP_GPS_SITL(*this, state[instance], _port[instance]); #endif // HAL_SIM_GPS_ENABLED default: break; } + const uint32_t now = AP_HAL::millis(); + if (now - dstate->last_baud_change_ms > GPS_BAUD_TIME_MS) { // try the next baud rate // incrementing like this will skip the first element in array of bauds @@ -715,17 +727,16 @@ void AP_GPS::detect_instance(uint8_t instance) _port[instance]->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); dstate->last_baud_change_ms = now; - if (_auto_config >= GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY && new_gps == nullptr) { + if (_auto_config >= GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY) { send_blob_start(instance); } } - if (_auto_config >= GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY && new_gps == nullptr) { + if (_auto_config >= GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY) { send_blob_update(instance); } - while (initblob_state[instance].remaining == 0 && _port[instance]->available() > 0 - && new_gps == nullptr) { + while (initblob_state[instance].remaining == 0 && _port[instance]->available() > 0) { uint8_t data = _port[instance]->read(); /* running a uBlox at less than 38400 will lead to packet @@ -741,7 +752,7 @@ void AP_GPS::detect_instance(uint8_t instance) (_baudrates[dstate->current_baud] >= 115200 && option_set(DriverOptions::UBX_Use115200)) || _baudrates[dstate->current_baud] == 230400) && AP_GPS_UBLOX::_detect(dstate->ublox_detect_state, data)) { - new_gps = new AP_GPS_UBLOX(*this, state[instance], _port[instance], GPS_ROLE_NORMAL); + return new AP_GPS_UBLOX(*this, state[instance], _port[instance], GPS_ROLE_NORMAL); } const uint32_t ublox_mb_required_baud = option_set(DriverOptions::UBX_MBUseUart2)?230400:460800; @@ -755,54 +766,43 @@ void AP_GPS::detect_instance(uint8_t instance) } else { role = GPS_ROLE_MB_ROVER; } - new_gps = new AP_GPS_UBLOX(*this, state[instance], _port[instance], role); + return new AP_GPS_UBLOX(*this, state[instance], _port[instance], role); } #if AP_GPS_SBP2_ENABLED - else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) && + if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) && AP_GPS_SBP2::_detect(dstate->sbp2_detect_state, data)) { - new_gps = new AP_GPS_SBP2(*this, state[instance], _port[instance]); + return new AP_GPS_SBP2(*this, state[instance], _port[instance]); } #endif //AP_GPS_SBP2_ENABLED #if AP_GPS_SBP_ENABLED - else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) && + if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) && AP_GPS_SBP::_detect(dstate->sbp_detect_state, data)) { - new_gps = new AP_GPS_SBP(*this, state[instance], _port[instance]); + return new AP_GPS_SBP(*this, state[instance], _port[instance]); } #endif //AP_GPS_SBP_ENABLED #if !HAL_MINIMIZE_FEATURES && AP_GPS_SIRF_ENABLED - else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SIRF) && + if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SIRF) && AP_GPS_SIRF::_detect(dstate->sirf_detect_state, data)) { - new_gps = new AP_GPS_SIRF(*this, state[instance], _port[instance]); + return new AP_GPS_SIRF(*this, state[instance], _port[instance]); } #endif #if AP_GPS_ERB_ENABLED - else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_ERB) && + if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_ERB) && AP_GPS_ERB::_detect(dstate->erb_detect_state, data)) { - new_gps = new AP_GPS_ERB(*this, state[instance], _port[instance]); + return new AP_GPS_ERB(*this, state[instance], _port[instance]); } #endif // AP_GPS_ERB_ENABLED #if AP_GPS_NMEA_ENABLED - else if ((_type[instance] == GPS_TYPE_NMEA || + if ((_type[instance] == GPS_TYPE_NMEA || _type[instance] == GPS_TYPE_HEMI || _type[instance] == GPS_TYPE_ALLYSTAR) && AP_GPS_NMEA::_detect(dstate->nmea_detect_state, data)) { - new_gps = new AP_GPS_NMEA(*this, state[instance], _port[instance]); + return new AP_GPS_NMEA(*this, state[instance], _port[instance]); } #endif //AP_GPS_NMEA_ENABLED - - if (new_gps) { - goto found_gps; - } } -found_gps: - if (new_gps != nullptr) { - state[instance].status = NO_FIX; - drivers[instance] = new_gps; - timing[instance].last_message_time_ms = now; - timing[instance].delta_time_ms = GPS_TIMEOUT_MS; - new_gps->broadcast_gps_type(); - } + return nullptr; } AP_GPS::GPS_Status AP_GPS::highest_supported_status(uint8_t instance) const diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 50f3da0208..9f16c7aa5a 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -665,6 +665,10 @@ private: static const char _initialisation_raw_blob[]; void detect_instance(uint8_t instance); + // run detection step for one GPS instance. If this finds a GPS then it + // will return it - otherwise nullptr + AP_GPS_Backend *_detect_instance(uint8_t instance); + void update_instance(uint8_t instance); /*