mirror of https://github.com/ArduPilot/ardupilot
GPS: fixed state machine logic errors in MTK19 driver
This commit is contained in:
parent
17a63dc76a
commit
c980b32319
|
@ -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;
|
step = 0;
|
||||||
}
|
goto restart;
|
||||||
mtk_type_step1 = 0;
|
}
|
||||||
mtk_type_step2 = 0;
|
break;
|
||||||
step = 0;
|
|
||||||
case 2:
|
case 2:
|
||||||
if (data == sizeof(struct diyd_mtk_msg)) {
|
if (data == sizeof(struct diyd_mtk_msg)) {
|
||||||
step++;
|
step++;
|
||||||
|
|
Loading…
Reference in New Issue