Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

GPS: fixed state machine logic errors in MTK19 driver

This commit is contained in:
Andrew Tridgell 2013-01-02 10:12:55 +11:00
parent 17a63dc76a
commit c980b32319
1 changed files with 18 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/AP_GPS/AP_GPS_MTK19.cpp
View File

@ -101,16 +101,17 @@ restart:
_step++; _step++;
_mtk_type_step2 = MTK_GPS_REVISION_V16; _mtk_type_step2 = MTK_GPS_REVISION_V16;
break; break;
} } else if ((_mtk_type_step1 == MTK_GPS_REVISION_V19) && (data == PREAMBLE2_V19)){
if ((_mtk_type_step1 == MTK_GPS_REVISION_V19) && (data == PREAMBLE2_V19)){
_step++; _step++;
_mtk_type_step2 = MTK_GPS_REVISION_V19; _mtk_type_step2 = MTK_GPS_REVISION_V19;
break; break;
} } else {
_mtk_type_step1 = 0; _mtk_type_step1 = 0;
_mtk_type_step2 = 0; _mtk_type_step2 = 0;
_step = 0; _step = 0;
//goto restart; goto restart;
}
break;
case 2: case 2:
if (sizeof(_buffer) == data) { if (sizeof(_buffer) == data) {
_step++; _step++;
@ -118,7 +119,7 @@ restart:
_payload_counter = 0; _payload_counter = 0;
} else { } else {
_step = 0; // reset and wait for a message of the right class _step = 0; // reset and wait for a message of the right class
//goto restart; goto restart;
} }
break; break;
@ -206,17 +207,17 @@ AP_GPS_MTK19::_detect(uint8_t data)
{ {
static uint8_t payload_counter; static uint8_t payload_counter;
static uint8_t step; static uint8_t step;
static uint8_t mtk_type_step1, mtk_type_step2; static uint8_t mtk_type_step1;
static uint8_t ck_a, ck_b; static uint8_t ck_a, ck_b;
restart:
switch (step) { switch (step) {
case 0: case 0:
ck_b = ck_a = payload_counter = 0; ck_b = ck_a = payload_counter = 0;
if (data == PREAMBLE1_V16) { if (data == PREAMBLE1_V16) {
mtk_type_step1 = MTK_GPS_REVISION_V16; mtk_type_step1 = MTK_GPS_REVISION_V16;
step++; step++;
} } else if (data == PREAMBLE1_V19) {
if (data == PREAMBLE1_V19) {
mtk_type_step1 = MTK_GPS_REVISION_V19; mtk_type_step1 = MTK_GPS_REVISION_V19;
step++; step++;
} }
@ -224,17 +225,13 @@ AP_GPS_MTK19::_detect(uint8_t data)
case 1: case 1:
if ((mtk_type_step1 == MTK_GPS_REVISION_V16) && (PREAMBLE2_V16 == data)){ if ((mtk_type_step1 == MTK_GPS_REVISION_V16) && (PREAMBLE2_V16 == data)){
step++; step++;
mtk_type_step2 = MTK_GPS_REVISION_V16; } else if ((mtk_type_step1 == MTK_GPS_REVISION_V19) && (PREAMBLE2_V19 == data)){
break;
}
if ((mtk_type_step1 == MTK_GPS_REVISION_V19) && (PREAMBLE2_V19 == data)){
step++; step++;
mtk_type_step2 = MTK_GPS_REVISION_V19; } else {
break;
}
mtk_type_step1 = 0;
mtk_type_step2 = 0;
step = 0; step = 0;
goto restart;
}
break;
case 2: case 2:
if (data == sizeof(struct diyd_mtk_msg)) { if (data == sizeof(struct diyd_mtk_msg)) {
step++; step++;