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

215
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 -*-
/*
GPS_MTK.cpp - Ublox GPS library for Arduino
Code by 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
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 : 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.
*/
//
// 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"
//
#include "AP_GPS_MTK.h"
#include "WProgram.h"
@ -36,7 +19,6 @@ AP_GPS_MTK::AP_GPS_MTK(Stream *s) : GPS(s)
{
}
// Public Methods //////////////////////////////////////////////////////////////
void AP_GPS_MTK::init(void)
{
@ -45,106 +27,119 @@ void AP_GPS_MTK::init(void)
_port->print(MTK_OUTPUT_4HZ);
}
// optimization : This code doesn't wait for data, only proccess the data available
// 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.
// Process bytes available from the stream
//
// 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)
{
byte data;
int numc;
numc = _port->available();
if (numc > 0)
for (int i = 0; i < numc; i++){ // Process bytes received
data = _port->read();
for (int i = 0; i < numc; i++){ // Process bytes received
// read the next byte
data = _port->read();
restart:
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);
// Message preamble, class, ID detection
//
// If we fail to match any of the expected bytes, we
// reset the state machine and re-consider the failed
// 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++;
break;
case 3:
id = data;
step = 4;
payload_length_hi = 26;
payload_length_lo = 0;
}
step = 0;
goto restart;
case 2:
if (MESSAGE_CLASS == data) {
step++;
ck_b = ck_a = data; // reset the checksum accumulators
} else {
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;
checksum(id);
break;
case 4:
if (payload_counter < payload_length_hi){ // We stay in this state until we reach the payload_length
buffer[payload_counter] = data;
checksum(data);
payload_counter++;
if (payload_counter == payload_length_hi)
step++;
}
break;
case 5:
GPS_ck_a = data; // First checksum byte
step++;
break;
case 6:
GPS_ck_b = data; // Second checksum byte
// We end the GPS read...
if((ck_a == GPS_ck_a) && (ck_b == GPS_ck_b)){ // Verify the received checksum with the generated checksum..
parse_gps(); // Parse the new GPS packet
}else {
_error("ERR:GPS_CHK!!\n");
}
// Variable initialization
} else {
step = 0;
ck_a = 0;
ck_b = 0;
goto restart;
}
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;
} // End switch
} // End for
}
_parse_gps(); // Parse the new GPS packet
}
}
}
// Private Methods
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..
//In this case all the message im using are in class 1, to know more about classes check PAGE 60 of DataSheet.
if(msg_class == 0x01) {
switch(id){
//Checking the UBX ID
case 0x05: // ID Custom
latitude = _swapl(&buffer[0]) * 10;
longitude = _swapl(&buffer[4]) * 10;
altitude = _swapl(&buffer[8]);
speed_3d = ground_speed = _swapl(&buffer[12]);
ground_course = _swapl(&buffer[16]) / 10000;
num_sats = buffer[20];
fix = buffer[21] == 3;
time = _swapl(&buffer[22]);
new_data = true;
break;
}
}
}
if (FIX_3D != buffer.msg.fix_type) {
fix = false;
} else {
fix = true;
latitude = _swapl(&buffer.msg.latitude) * 10;
longitude = _swapl(&buffer.msg.longitude) * 10;
altitude = _swapl(&buffer.msg.altitude);
ground_speed = _swapl(&buffer.msg.ground_speed);
ground_course = _swapl(&buffer.msg.ground_course) / 10000;
num_sats = buffer.msg.satellites;
// checksum algorithm
void
AP_GPS_MTK::checksum(byte data)
{
ck_a += data;
ck_b += ck_a;
// XXX docs say this is UTC, but our clients expect msToW
time = _swapl(&buffer.msg.utc_time);
}
new_data = true;
}

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 -*-
//
// 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
#define AP_GPS_MTK_h
@ -22,30 +33,55 @@
#define WAAS_ON "$PSRF151,1*3F\r\n"
#define WAAS_OFF "$PSRF151,0*3E\r\n"
class AP_GPS_MTK : public GPS
{
public:
// Methods
class AP_GPS_MTK : public GPS {
public:
AP_GPS_MTK(Stream *s);
void init();
void update();
void init();
void update();
private:
// Packet checksums
uint8_t ck_a;
uint8_t ck_b;
uint8_t GPS_ck_a;
uint8_t GPS_ck_b;
private:
#pragma pack(1)
struct diyd_mtk_msg {
int32_t latitude;
int32_t longitude;
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;
uint8_t msg_class;
uint8_t id;
uint8_t payload_length_hi;
uint8_t payload_length_lo;
uint8_t payload_counter;
uint8_t buffer[MAXPAYLOAD];
enum diyd_mtk_protocol_bytes {
PREAMBLE1 = 0xb5,
PREAMBLE2 = 0x62,
MESSAGE_CLASS = 1,
MESSAGE_ID = 5
};
void parse_gps();
void checksum(unsigned char data);
// Packet checksum accumulators
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
/// @returns endian-swapped value
///
long _swapl(const uint8_t *bytes);
long _swapl(const void *bytes);
/// perform an endian swap on an int
///
/// @param bytes pointer to a buffer containing bytes representing an
/// int in the wrong byte order
/// @returns endian-swapped value
int _swapi(const uint8_t *bytes);
int _swapi(const void *bytes);
/// emit an error message
///
@ -84,31 +84,33 @@ protected:
};
inline long
GPS::_swapl(const uint8_t *bytes)
GPS::_swapl(const void *bytes)
{
const uint8_t *b = (const uint8_t *)bytes;
union {
long v;
uint8_t b[4];
} u;
u.b[0] = bytes[3];
u.b[1] = bytes[2];
u.b[2] = bytes[1];
u.b[3] = bytes[0];
u.b[0] = b[3];
u.b[1] = b[2];
u.b[2] = b[1];
u.b[3] = b[0];
return(u.v);
}
inline int16_t
GPS::_swapi(const uint8_t *bytes)
GPS::_swapi(const void *bytes)
{
const uint8_t *b = (const uint8_t *)bytes;
union {
int16_t v;
uint8_t b[2];
} u;
u.b[0] = bytes[1];
u.b[1] = bytes[0];
u.b[0] = b[1];
u.b[1] = b[0];
return(u.v);
}