Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-07 09:13:32 +08:00
Code Issues Actions 34 Packages Projects Releases Wiki Activity

Added support for MTK revision 19, working condition but configuration of MTK is very slow and needs improvement

Browse Source
This commit is contained in:
Julian Oes
2013-02-06 22:58:52 -08:00
parent 0d54661ce9
commit b620136af4
2 changed files with 22 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/drivers/gps/mtk.cpp
+19 -6
View File
@@ -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
apps/drivers/gps/mtk.h
+3 -1
View File
@@ -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;