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

AP_GPS: Allow switching primary GPS instance with 1 sat difference

Browse Source 
Rapid switching between GPS receivers can cause real problems.
Switch if:
1) secondary GPS has 1 more satellite for at least 20 seconds
OR
2) secondary GPS has 2 more satellites for at least 5 seconds

Fixes https://github.com/diydrones/ardupilot/pull/2320
This commit is contained in:
Tom Pittenger 2015-05-21 14:07:59 -07:00 committed by Andrew Tridgell
parent 58505d8242
commit 3544549cf4
2 changed files with 15 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_GPS/AP_GPS.cpp
View File

@ -120,6 +120,7 @@ void AP_GPS::init(DataFlash_Class *dataflash, const AP_SerialManager& serial_man
#if GPS_MAX_INSTANCES > 1 #if GPS_MAX_INSTANCES > 1
_port[1] = serial_manager.find_serial(AP_SerialManager::SerialProtocol_GPS, 1); _port[1] = serial_manager.find_serial(AP_SerialManager::SerialProtocol_GPS, 1);
_last_instance_swap_ms = 0;
#endif #endif
} }
@ -380,14 +381,24 @@ AP_GPS::update(void)
primary_instance = i; primary_instance = i;
continue; continue;
} }
if (state[i].status == state[primary_instance].status &&
state[i].num_sats >= state[primary_instance].num_sats + 2) { bool another_gps_has_1_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 1);
// this GPS has at least 2 more satellites than the
if (state[i].status == state[primary_instance].status && another_gps_has_1_or_more_sats) {
uint32_t now = hal.scheduler->millis();
bool another_gps_has_2_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 2);
if ( (another_gps_has_1_or_more_sats && (now - _last_instance_swap_ms) >= 20000) ||
(another_gps_has_2_or_more_sats && (now - _last_instance_swap_ms) >= 5000 ) ) {
// this GPS has more satellites than the
// current primary, switch primary. Once we switch we will // current primary, switch primary. Once we switch we will
// then tend to stick to the new GPS as primary. We don't // then tend to stick to the new GPS as primary. We don't
// want to switch too often as it will look like a // want to switch too often as it will look like a
// position shift to the controllers. // position shift to the controllers.
primary_instance = i; primary_instance = i;
_last_instance_swap_ms = now;
}
} }
} else { } else {
primary_instance = 0; primary_instance = 0;

1
libraries/AP_GPS/AP_GPS.h
View File

@ -331,6 +331,7 @@ public:
AP_Int8 _min_dgps; AP_Int8 _min_dgps;
AP_Int16 _sbp_logmask; AP_Int16 _sbp_logmask;
AP_Int8 _inject_to; AP_Int8 _inject_to;
uint32_t _last_instance_swap_ms;
#endif #endif
AP_Int8 _sbas_mode; AP_Int8 _sbas_mode;
AP_Int8 _min_elevation; AP_Int8 _min_elevation;