mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 18:08:30 -04:00
add gps character count.
this problem happens when the mtk ends up in nmea mode at 10hz. there is no 50ms window of no activity
This commit is contained in:
parent
3d85f99546
commit
306e2e3f7d
@ -99,6 +99,7 @@ AP_GPS_Auto::_detect(void)
|
||||
unsigned long then;
|
||||
int fingerprint[4];
|
||||
int tries;
|
||||
int charcount;
|
||||
GPS *gps;
|
||||
|
||||
//
|
||||
@ -114,14 +115,16 @@ AP_GPS_Auto::_detect(void)
|
||||
// XXX We can detect babble by counting incoming characters, but
|
||||
// what would we do about it?
|
||||
//
|
||||
charcount = 0;
|
||||
_port->flush();
|
||||
then = millis();
|
||||
do {
|
||||
if (_port->available()) {
|
||||
then = millis();
|
||||
_port->read();
|
||||
charcount++;
|
||||
}
|
||||
} while ((millis() - then) < 50);
|
||||
} while ((millis() - then) < 50 && charcount < 5000);
|
||||
|
||||
//
|
||||
// Collect four characters to fingerprint a device
|
||||
|
Loading…
Reference in New Issue
Block a user