Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Peter Barker 2022-04-13 23:04:21 +10:00 committed by Andrew Tridgell
parent 0b9c5d6dc1
commit e858a0fab7
2 changed files with 53 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

98
libraries/AP_GPS/AP_GPS.cpp
View File

@ -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

4
libraries/AP_GPS/AP_GPS.h
View File

@ -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);
/*