Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: Refactor first_unconfigured_gps to return bool

Browse Source
This commit is contained in:
Michael du Breuil 2019-07-22 23:20:47 -07:00 committed by Andrew Tridgell
parent 11b9737b34
commit d33006a2e7
2 changed files with 15 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_GPS/AP_GPS.cpp
View File

@ -984,23 +984,26 @@ void AP_GPS::send_mavlink_gps_rtk(mavlink_channel_t chan, uint8_t inst)
} }
} }
uint8_t AP_GPS::first_unconfigured_gps(void) const bool AP_GPS::first_unconfigured_gps(uint8_t &instance) const
{ {
for (int i = 0; i < GPS_MAX_RECEIVERS; i++) { for (int i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (_type[i] != GPS_TYPE_NONE && (drivers[i] == nullptr || !drivers[i]->is_configured())) { if (_type[i] != GPS_TYPE_NONE && (drivers[i] == nullptr || !drivers[i]->is_configured())) {
return i; instance = i;
return true;
} }
} }
return GPS_ALL_CONFIGURED; return false;
} }
void AP_GPS::broadcast_first_configuration_failure_reason(void) const void AP_GPS::broadcast_first_configuration_failure_reason(void) const
{ {
const uint8_t unconfigured = first_unconfigured_gps(); uint8_t unconfigured;
if (drivers[unconfigured] == nullptr) { if (first_unconfigured_gps(unconfigured)) {
gcs().send_text(MAV_SEVERITY_INFO, "GPS %d: was not found", unconfigured + 1); if (drivers[unconfigured] == nullptr) {
} else { gcs().send_text(MAV_SEVERITY_INFO, "GPS %d: was not found", unconfigured + 1);
drivers[unconfigured]->broadcast_configuration_failure_reason(); } else {
drivers[unconfigured]->broadcast_configuration_failure_reason();
}
} }
} }
@ -1123,7 +1126,8 @@ bool AP_GPS::get_lag(uint8_t instance, float &lag_sec) const
if (instance == GPS_BLENDED_INSTANCE) { if (instance == GPS_BLENDED_INSTANCE) {
lag_sec = _blended_lag_sec; lag_sec = _blended_lag_sec;
// auto switching uses all GPS receivers, so all must be configured // auto switching uses all GPS receivers, so all must be configured
return all_configured(); uint8_t inst; // we don't actually care what the number is, but must pass it
return first_unconfigured_gps(inst);
} }
if (_delay_ms[instance] > 0) { if (_delay_ms[instance] > 0) {

9
libraries/AP_GPS/AP_GPS.h
View File

@ -384,15 +384,10 @@ public:
void send_mavlink_gps_rtk(mavlink_channel_t chan, uint8_t inst); void send_mavlink_gps_rtk(mavlink_channel_t chan, uint8_t inst);
// Returns the index of the first unconfigured GPS (returns GPS_ALL_CONFIGURED if all instances report as being configured) // Returns true if there is an unconfigured GPS, and provides the instance number of the first non configured GPS
uint8_t first_unconfigured_gps(void) const; bool first_unconfigured_gps(uint8_t &instance) const WARN_IF_UNUSED;
void broadcast_first_configuration_failure_reason(void) const; void broadcast_first_configuration_failure_reason(void) const;
// return true if all GPS instances have finished configuration
bool all_configured(void) const {
return first_unconfigured_gps() == GPS_ALL_CONFIGURED;
}
// pre-arm check that all GPSs are close to each other. farthest distance between GPSs (in meters) is returned // pre-arm check that all GPSs are close to each other. farthest distance between GPSs (in meters) is returned
bool all_consistent(float &distance) const; bool all_consistent(float &distance) const;