zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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
apm_2208
Peter Barker 3 years ago committed by Andrew Tridgell
parent
0b9c5d6dc1
commit
e858a0fab7
2 changed files with 53 additions and 49 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 98
      libraries/AP_GPS/AP_GPS.cpp
  2. 4
      libraries/AP_GPS/AP_GPS.h

98
libraries/AP_GPS/AP_GPS.cpp
Unescape View File

@ -620,24 +620,41 @@ void AP_GPS::send_blob_update(uint8_t instance) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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
Unescape View File

@ -665,6 +665,10 @@ private: @@ -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);
/*

Write Preview
Loading…
Cancel
Save