aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Improved parsing in auto detect

Browse Source
This commit is contained in:
Craig@3DR 2012-12-21 16:04:02 -08:00 committed by Andrew Tridgell
parent 0ed25cf719
commit 2de676306e
1 changed files with 17 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
libraries/AP_GPS/AP_GPS_MTK19.cpp
View File

@ -31,22 +31,22 @@ AP_GPS_MTK19::init(enum GPS_Engine_Setting nav_setting)
{ {
_port->flush(); _port->flush();
// set WAAS on
_port->print(WAAS_ON);
// set SBAS on
_port->print(SBAS_ON);
// Set Nav Threshold to 0 m/s
_port->print(MTK_NAVTHRES_OFF);
// set 5Hz update rate
_port->print(MTK_OUTPUT_5HZ);
// initialize serial port for binary protocol use // initialize serial port for binary protocol use
// XXX should assume binary, let GPS_AUTO handle dynamic config? // XXX should assume binary, let GPS_AUTO handle dynamic config?
_port->print(MTK_SET_BINARY); _port->print(MTK_SET_BINARY);
// set 5Hz update rate
_port->print(MTK_OUTPUT_5HZ);
// set SBAS on
_port->print(SBAS_ON);
// set WAAS on
_port->print(WAAS_ON);
// Set Nav Threshold to 0 m/s
_port->print(MTK_NAVTHRES_OFF);
// set initial epoch code // set initial epoch code
_epoch = TIME_OF_DAY; _epoch = TIME_OF_DAY;
_time_offset = 0; _time_offset = 0;
@ -114,15 +114,15 @@ restart:
_mtk_type_step1 = 0; _mtk_type_step1 = 0;
_mtk_type_step2 = 0; _mtk_type_step2 = 0;
_step = 0; _step = 0;
goto restart; //goto restart;
case 2: case 2:
if (sizeof(_buffer) == data) { if (sizeof(_buffer) == data) {
_step++; _step++;
_ck_b = _ck_a = data; // reset the checksum accumulators _ck_b = _ck_a = data; // reset the checksum accumulators
_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;
@ -172,7 +172,6 @@ restart:
temp = (time_utc/100000); temp = (time_utc/100000);
time_utc -= temp*100000; time_utc -= temp*100000;
time += temp * 60000 + time_utc; time += temp * 60000 + time_utc;
parsed = true; parsed = true;
#ifdef FAKE_GPS_LOCK_TIME #ifdef FAKE_GPS_LOCK_TIME
@ -204,7 +203,7 @@ restart:
/* /*
detect a MTK16 GPS detect a MTK16 or MTK19 GPS
*/ */
bool bool
AP_GPS_MTK19::_detect(uint8_t data) AP_GPS_MTK19::_detect(uint8_t data)
@ -214,7 +213,7 @@ AP_GPS_MTK19::_detect(uint8_t data)
static uint8_t mtk_type_step1, mtk_type_step2; static uint8_t mtk_type_step1, mtk_type_step2;
static uint8_t ck_a, ck_b; static uint8_t ck_a, ck_b;
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) {
@ -225,7 +224,6 @@ AP_GPS_MTK19::_detect(uint8_t data)
mtk_type_step1 = MTK_GPS_REVISION_V19; mtk_type_step1 = MTK_GPS_REVISION_V19;
step++; step++;
} }
mtk_type_step1 = 0;
break; break;
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)){