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

AP_GPS: make UBLOX driver a bit more robust

Browse Source
This commit is contained in:
Andrew Tridgell 2013-01-05 16:32:42 +11:00
parent 3b0398dc14
commit f7e5f88199
2 changed files with 17 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -73,6 +73,7 @@ AP_GPS_UBLOX::read(void)
// read the next byte
data = _port->read();
reset:
switch(_step) {
// Message preamble detection
@ -131,6 +132,7 @@ AP_GPS_UBLOX::read(void)
// assume very large payloads are line noise
_payload_length = 0;
_step = 0;
goto reset;
}
_payload_counter = 0; // prepare to receive payload
break;
@ -152,7 +154,8 @@ AP_GPS_UBLOX::read(void)
_step++;
if (_ck_a != data) {
Debug("bad cka %x should be %x", data, _ck_a);
_step = 0; // bad checksum
_step = 0;
goto reset;
}
break;
case 8:
@ -378,6 +381,7 @@ AP_GPS_UBLOX::_detect(uint8_t data)
static uint8_t step;
static uint8_t ck_a, ck_b;
reset:
switch (step) {
case 1:
if (PREAMBLE2 == data) {
@ -416,6 +420,7 @@ AP_GPS_UBLOX::_detect(uint8_t data)
step++;
if (ck_a != data) {
step = 0;
goto reset;
}
break;
case 8:
@ -423,6 +428,8 @@ AP_GPS_UBLOX::_detect(uint8_t data)
if (ck_b == data) {
// a valid UBlox packet
return true;
} else {
goto reset;
}
}
return false;

19
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -124,6 +124,15 @@ private:
uint32_t speed_accuracy;
uint32_t heading_accuracy;
};
// Receive buffer
union {
ubx_nav_posllh posllh;
ubx_nav_status status;
ubx_nav_solution solution;
ubx_nav_velned velned;
ubx_cfg_nav_settings nav_settings;
uint8_t bytes[];
} _buffer;
#pragma pack(pop)
enum ubs_protocol_bytes {
@ -179,16 +188,6 @@ private:
uint8_t _disable_counter;
// Receive buffer
union {
ubx_nav_posllh posllh;
ubx_nav_status status;
ubx_nav_solution solution;
ubx_nav_velned velned;
ubx_cfg_nav_settings nav_settings;
uint8_t bytes[];
} _buffer;
// Buffer parse & GPS state update
bool _parse_gps();