diff --git a/apps/drivers/gps/mtk.cpp b/apps/drivers/gps/mtk.cpp index 555fb7a515..bed388c49b 100644 --- a/apps/drivers/gps/mtk.cpp +++ b/apps/drivers/gps/mtk.cpp @@ -48,7 +48,8 @@ #include "mtk.h" -MTK::MTK() +MTK::MTK() : +_mtk_revision(0) { decodeInit(); } @@ -102,13 +103,18 @@ MTK::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ { if (_decode_state == MTK_DECODE_UNINIT) { - if (b == 0xd0) { + if (b == MTK_SYNC1_V16) { _decode_state = MTK_DECODE_GOT_CK_A; config_needed = false; + _mtk_revision = 16; + } else if (b == MTK_SYNC1_V19) { + _decode_state = MTK_DECODE_GOT_CK_A; + config_needed = false; + _mtk_revision = 19; } } else if (_decode_state == MTK_DECODE_GOT_CK_A) { - if (b == 0xdd) { + if (b == MTK_SYNC2) { _decode_state = MTK_DECODE_GOT_CK_B; } else { @@ -131,11 +137,18 @@ MTK::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ /* Check if checksum is valid */ if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { - gps_position->lat = packet->latitude * 10; // from degrees*1e6 to degrees*1e7 - gps_position->lon = packet->longitude * 10; // from degrees*1e6 to degrees*1e7 + if (_mtk_revision == 16) { + gps_position->lat = packet->latitude * 10; // from degrees*1e6 to degrees*1e7 + gps_position->lon = packet->longitude * 10; // from degrees*1e6 to degrees*1e7 + } else if (_mtk_revision == 19) { + gps_position->lat = packet->latitude; // both degrees*1e7 + gps_position->lon = packet->longitude; // both degrees*1e7 + } else { + warnx("mtk: unknown revision"); + } gps_position->alt = (int32_t)(packet->msl_altitude * 10); // from cm to mm gps_position->fix_type = packet->fix_type; - gps_position->eph_m = packet->hdop; + gps_position->eph_m = packet->hdop; // XXX: Check this because eph_m is in m and hdop is without unit gps_position->epv_m = 0.0; //unknown in mtk custom mode gps_position->vel_m_s = ((float)packet->ground_speed)*1e-2f; // from cm/s to m/s gps_position->cog_rad = ((float)packet->heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad diff --git a/apps/drivers/gps/mtk.h b/apps/drivers/gps/mtk.h index 8929eb041c..7a4b4f197c 100644 --- a/apps/drivers/gps/mtk.h +++ b/apps/drivers/gps/mtk.h @@ -40,7 +40,8 @@ #include "gps_helper.h" -#define MTK_SYNC1 0xd0 +#define MTK_SYNC1_V16 0xd0 +#define MTK_SYNC1_V19 0xd1 #define MTK_SYNC2 0xdd #define MTK_OUTPUT_5HZ "$PMTK220,200*2C\r\n" @@ -99,6 +100,7 @@ private: void addByteToChecksum(uint8_t); mtk_decode_state_t _decode_state; + uint8_t _mtk_revision; uint8_t _rx_buffer[MTK_RECV_BUFFER_SIZE]; unsigned _rx_count; uint8_t _rx_ck_a;