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

Rework the MTK protocol decoder for robustness and code size. 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@417 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
DrZiplok@gmail.com 2010-09-06 20:00:57 +00:00
parent 1dd4bc9080
commit add89239f3
3 changed files with 178 additions and 145 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

221
libraries/AP_GPS/AP_GPS_MTK.cpp
View File

@ -1,32 +1,15 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/* //
GPS_MTK.cpp - Ublox GPS library for Arduino // DIYDrones Custom Mediatek GPS driver for ArduPilot and ArduPilotMega.
Code by Jordi Munoz and Jose Julio. DIYDrones.com // Code by Michael Smith, Jordi Munoz and Jose Julio, DIYDrones.com
This code works with boards based on ATMega168 / 328 and ATMega1280 (Serial port 1) //
// This library is free software; you can redistribute it and / or
This library is free software; you can redistribute it and / or // modify it under the terms of the GNU Lesser General Public
modify it under the terms of the GNU Lesser General Public // License as published by the Free Software Foundation; either
License as published by the Free Software Foundation; either // version 2.1 of the License, or (at your option) any later version.
version 2.1 of the License, or (at your option) any later version. //
// GPS configuration : Custom protocol per "DIYDrones Custom Binary Sentence Specification V1.1"
GPS configuration : Custum protocol //
Baud rate : 38400
Methods:
init() : GPS initialization
update() : Call this funcion as often as you want to ensure you read the incomming gps data
Properties:
lattitude : lattitude * 10000000 (long value)
longitude : longitude * 10000000 (long value)
altitude : altitude * 100 (meters) (long value)
ground_speed : Speed (m/s) * 100 (long value)
ground_course : Course (degrees) * 100 (long value)
new_data : 1 when a new data is received.
You need to write a 0 to new_data when you read the data
Fix : 0: GPS NO FIX or 2D FIX, 1: 3D FIX.
*/
#include "AP_GPS_MTK.h" #include "AP_GPS_MTK.h"
#include "WProgram.h" #include "WProgram.h"
@ -36,7 +19,6 @@ AP_GPS_MTK::AP_GPS_MTK(Stream *s) : GPS(s)
{ {
} }
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
void AP_GPS_MTK::init(void) void AP_GPS_MTK::init(void)
{ {
@ -45,106 +27,119 @@ void AP_GPS_MTK::init(void)
_port->print(MTK_OUTPUT_4HZ); _port->print(MTK_OUTPUT_4HZ);
} }
// optimization : This code doesn't wait for data, only proccess the data available // Process bytes available from the stream
// We can call this function on the main loop (50Hz loop) //
// If we get a complete packet this function calls parse_gps() to parse and update the GPS info. // The stream is assumed to contain only our custom message. If it
// contains other messages, and those messages contain the preamble bytes,
// it is possible for this code to become de-synchronised. Without
// buffering the entire message and re-processing it from the top,
// this is unavoidable.
//
// The lack of a standard header length field makes it impossible to skip
// unrecognised messages.
//
void AP_GPS_MTK::update(void) void AP_GPS_MTK::update(void)
{ {
byte data; byte data;
int numc; int numc;
numc = _port->available(); numc = _port->available();
if (numc > 0) for (int i = 0; i < numc; i++){ // Process bytes received
for (int i = 0; i < numc; i++){ // Process bytes received
data = _port->read();
switch(step){
case 0:
if(data == 0xB5)
step++;
break;
case 1:
if(data == 0x62)
step++;
else
step = 0;
break;
case 2:
msg_class = data;
checksum(msg_class);
step++;
break;
case 3:
id = data;
step = 4;
payload_length_hi = 26;
payload_length_lo = 0;
payload_counter = 0;
checksum(id);
break;
case 4: // read the next byte
if (payload_counter < payload_length_hi){ // We stay in this state until we reach the payload_length data = _port->read();
buffer[payload_counter] = data;
checksum(data); restart:
payload_counter++; switch(step){
if (payload_counter == payload_length_hi)
step++; // Message preamble, class, ID detection
} //
break; // If we fail to match any of the expected bytes, we
case 5: // reset the state machine and re-consider the failed
GPS_ck_a = data; // First checksum byte // byte as the first byte of the preamble. This
// improves our chances of recovering from a mismatch
// and makes it less likely that we will be fooled by
// the preamble appearing as data in some other message.
//
case 0:
if(PREAMBLE1 == data)
step++;
break;
case 1:
if (PREAMBLE2 == data) {
step++; step++;
break; break;
case 6: }
GPS_ck_b = data; // Second checksum byte step = 0;
// We end the GPS read... goto restart;
if((ck_a == GPS_ck_a) && (ck_b == GPS_ck_b)){ // Verify the received checksum with the generated checksum.. case 2:
parse_gps(); // Parse the new GPS packet if (MESSAGE_CLASS == data) {
}else { step++;
_error("ERR:GPS_CHK!!\n"); ck_b = ck_a = data; // reset the checksum accumulators
} } else {
// Variable initialization step = 0; // reset and wait for a message of the right class
goto restart;
}
break;
case 3:
if (MESSAGE_ID == data) {
step++;
ck_b += (ck_a += data);
payload_length = sizeof(buffer); // prepare to receive our message
payload_counter = 0;
} else {
step = 0; step = 0;
ck_a = 0; goto restart;
ck_b = 0; }
break;
// Receive message data
//
case 4:
buffer.bytes[payload_counter++] = data;
ck_b += (ck_a += data);
if (payload_counter == payload_length)
step++;
break;
// Checksum and message processing
//
case 5:
step++;
if (ck_a != data) {
_error("GPS_MTK: checksum error\n");
step = 0;
}
break;
case 6:
step = 0;
if (ck_b != data) {
_error("GPS_MTK: checksum error\n");
break; break;
} // End switch }
} // End for _parse_gps(); // Parse the new GPS packet
}
}
} }
// Private Methods // Private Methods
void void
AP_GPS_MTK::parse_gps(void) AP_GPS_MTK::_parse_gps(void)
{ {
//Verifing if we are in class 1, you can change this "IF" for a "Switch" in case you want to use other UBX classes.. if (FIX_3D != buffer.msg.fix_type) {
//In this case all the message im using are in class 1, to know more about classes check PAGE 60 of DataSheet. fix = false;
if(msg_class == 0x01) { } else {
switch(id){ fix = true;
//Checking the UBX ID latitude = _swapl(&buffer.msg.latitude) * 10;
case 0x05: // ID Custom longitude = _swapl(&buffer.msg.longitude) * 10;
latitude = _swapl(&buffer[0]) * 10; altitude = _swapl(&buffer.msg.altitude);
longitude = _swapl(&buffer[4]) * 10; ground_speed = _swapl(&buffer.msg.ground_speed);
altitude = _swapl(&buffer[8]); ground_course = _swapl(&buffer.msg.ground_course) / 10000;
speed_3d = ground_speed = _swapl(&buffer[12]); num_sats = buffer.msg.satellites;
ground_course = _swapl(&buffer[16]) / 10000;
num_sats = buffer[20]; // XXX docs say this is UTC, but our clients expect msToW
fix = buffer[21] == 3; time = _swapl(&buffer.msg.utc_time);
time = _swapl(&buffer[22]);
new_data = true;
break;
}
} }
} new_data = true;
// checksum algorithm
void
AP_GPS_MTK::checksum(byte data)
{
ck_a += data;
ck_b += ck_a;
} }

80
libraries/AP_GPS/AP_GPS_MTK.h
View File

@ -1,4 +1,15 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
//
// DIYDrones Custom Mediatek GPS driver for ArduPilot and ArduPilotMega.
// Code by Michael Smith, Jordi Munoz and Jose Julio, DIYDrones.com
//
// This library is free software; you can redistribute it and / or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
//
// GPS configuration : Custom protocol per "DIYDrones Custom Binary Sentence Specification V1.1"
//
#ifndef AP_GPS_MTK_h #ifndef AP_GPS_MTK_h
#define AP_GPS_MTK_h #define AP_GPS_MTK_h
@ -22,30 +33,55 @@
#define WAAS_ON "$PSRF151,1*3F\r\n" #define WAAS_ON "$PSRF151,1*3F\r\n"
#define WAAS_OFF "$PSRF151,0*3E\r\n" #define WAAS_OFF "$PSRF151,0*3E\r\n"
class AP_GPS_MTK : public GPS class AP_GPS_MTK : public GPS {
{ public:
public:
// Methods
AP_GPS_MTK(Stream *s); AP_GPS_MTK(Stream *s);
void init(); void init();
void update(); void update();
private: private:
// Packet checksums #pragma pack(1)
uint8_t ck_a; struct diyd_mtk_msg {
uint8_t ck_b; int32_t latitude;
uint8_t GPS_ck_a; int32_t longitude;
uint8_t GPS_ck_b; int32_t altitude;
int32_t ground_speed;
int32_t ground_course;
uint8_t satellites;
uint8_t fix_type;
uint32_t utc_time;
};
#pragma pack(pop)
enum diyd_mtk_fix_type {
FIX_NONE = 1,
FIX_2D = 2,
FIX_3D = 3
};
uint8_t step; enum diyd_mtk_protocol_bytes {
uint8_t msg_class; PREAMBLE1 = 0xb5,
uint8_t id; PREAMBLE2 = 0x62,
uint8_t payload_length_hi; MESSAGE_CLASS = 1,
uint8_t payload_length_lo; MESSAGE_ID = 5
uint8_t payload_counter; };
uint8_t buffer[MAXPAYLOAD];
void parse_gps(); // Packet checksum accumulators
void checksum(unsigned char data); uint8_t ck_a;
uint8_t ck_b;
// State machine state
uint8_t step;
uint8_t payload_length;
uint8_t payload_counter;
// Receive buffer
union {
diyd_mtk_msg msg;
uint8_t bytes[];
} buffer;
// Buffer parse & GPS state update
void _parse_gps();
}; };
#endif
#endif // AP_GPS_MTK_H

22
libraries/AP_GPS/GPS.h
View File

@ -64,14 +64,14 @@ protected:
/// long in the wrong byte order /// long in the wrong byte order
/// @returns endian-swapped value /// @returns endian-swapped value
/// ///
long _swapl(const uint8_t *bytes); long _swapl(const void *bytes);
/// perform an endian swap on an int /// perform an endian swap on an int
/// ///
/// @param bytes pointer to a buffer containing bytes representing an /// @param bytes pointer to a buffer containing bytes representing an
/// int in the wrong byte order /// int in the wrong byte order
/// @returns endian-swapped value /// @returns endian-swapped value
int _swapi(const uint8_t *bytes); int _swapi(const void *bytes);
/// emit an error message /// emit an error message
/// ///
@ -84,31 +84,33 @@ protected:
}; };
inline long inline long
GPS::_swapl(const uint8_t *bytes) GPS::_swapl(const void *bytes)
{ {
const uint8_t *b = (const uint8_t *)bytes;
union { union {
long v; long v;
uint8_t b[4]; uint8_t b[4];
} u; } u;
u.b[0] = bytes[3]; u.b[0] = b[3];
u.b[1] = bytes[2]; u.b[1] = b[2];
u.b[2] = bytes[1]; u.b[2] = b[1];
u.b[3] = bytes[0]; u.b[3] = b[0];
return(u.v); return(u.v);
} }
inline int16_t inline int16_t
GPS::_swapi(const uint8_t *bytes) GPS::_swapi(const void *bytes)
{ {
const uint8_t *b = (const uint8_t *)bytes;
union { union {
int16_t v; int16_t v;
uint8_t b[2]; uint8_t b[2];
} u; } u;
u.b[0] = bytes[1]; u.b[0] = b[1];
u.b[1] = bytes[0]; u.b[1] = b[0];
return(u.v); return(u.v);
} }