mirror of https://github.com/ArduPilot/ardupilot
Refactor the EM-406 support into a 406-specific init routine and a generic SiRF parser.
git-svn-id: https://arducopter.googlecode.com/svn/trunk@423 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
DrZiplok@gmail.com
parent
c981b7d0b0
commit
c6ceb85c21
|
|
@ -1,156 +1,38 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
/*
|
||||
GPS_406.cpp - 406 GPS library for Arduino
|
||||
Code by Jason Short, Jordi Muñoz and Jose Julio. DIYDrones.com
|
||||
This code works with boards based on ATMega168/328 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 : 406 protocol
|
||||
Baud rate : 57600
|
||||
|
||||
Methods:
|
||||
init() : GPS initialization
|
||||
update() : Call this funcion as often as you want to ensure you read the incomming gps data
|
||||
|
||||
Properties:
|
||||
Latitude : Latitude * 10,000,000 (long value)
|
||||
Longitude : Longitude * 10,000,000 (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 : 1: GPS FIX, 0: No fix (normal logic)
|
||||
|
||||
*/
|
||||
//
|
||||
// GPS_406.cpp - 406 GPS library for Arduino
|
||||
// Code by Michael Smith, Jason Short, Jordi Muñoz and Jose Julio. DIYDrones.com
|
||||
// This code works with boards based on ATMega168/328 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.
|
||||
|
||||
#include "../FastSerial/FastSerial.h" // because we need to change baud rates... ugh.
|
||||
#include "AP_GPS_406.h"
|
||||
#include "WProgram.h"
|
||||
#include <Stream.h>
|
||||
|
||||
uint8_t AP_GPS_406::buffer[MAXPAYLOAD] = {0x24,0x50,0x53,0x52,0x46,0x31,0x30,0x30,0x2C,0x30,0x2C,0x35,0x37,0x36,0x30,0x30,0x2C,0x38,0x2C,0x31,0x2C,0x30,0x2A,0x33,0x37,0x0D,0x0A};
|
||||
|
||||
#define PAYLOAD_LENGTH 92
|
||||
#define BAUD_RATE 57600
|
||||
static const char init_str[] = "$PSRF100,0,57600,8,1,0*37";
|
||||
|
||||
// Constructors ////////////////////////////////////////////////////////////////
|
||||
AP_GPS_406::AP_GPS_406(Stream *s) : GPS(s)
|
||||
AP_GPS_406::AP_GPS_406(Stream *s) : AP_GPS_SIRF(s)
|
||||
{
|
||||
}
|
||||
|
||||
// Public Methods ////////////////////////////////////////////////////////////////////
|
||||
void AP_GPS_406::init(void)
|
||||
{
|
||||
change_to_sirf_protocol(); // Changes to SIRF protocol and sets baud rate
|
||||
delay(100); // Waits fot the GPS to start_UP
|
||||
configure_gps(); // Function to configure GPS, to output only the desired msg's
|
||||
}
|
||||
_change_to_sirf_protocol(); // Changes to SIRF protocol and sets baud rate
|
||||
_configure_gps(); // Function to configure GPS, to output only the desired msg's
|
||||
|
||||
// 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.
|
||||
void AP_GPS_406::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();
|
||||
|
||||
switch(step){
|
||||
case 0:
|
||||
if(data == 0xA0)
|
||||
step++;
|
||||
break;
|
||||
|
||||
case 1:
|
||||
if(data == 0xA2)
|
||||
step++;
|
||||
else
|
||||
step = 0;
|
||||
break;
|
||||
|
||||
/* case 2:
|
||||
if(data == 0xA2)
|
||||
step++;
|
||||
else
|
||||
step = 0;
|
||||
break;
|
||||
*/
|
||||
case 2:
|
||||
if(data == 0x00)
|
||||
step++;
|
||||
else
|
||||
step = 0;
|
||||
break;
|
||||
|
||||
case 3:
|
||||
if(data == 0x5B)
|
||||
step++;
|
||||
else
|
||||
step = 0;
|
||||
break;
|
||||
|
||||
case 4:
|
||||
if(data == 0x29){
|
||||
payload_counter = 0;
|
||||
step++;
|
||||
}else
|
||||
step = 0;
|
||||
break;
|
||||
|
||||
case 5: // Payload data read...
|
||||
if (payload_counter <= PAYLOAD_LENGTH){ // We stay in this state until we reach the payload_length
|
||||
buffer[payload_counter] = data;
|
||||
payload_counter++;
|
||||
if (payload_counter == PAYLOAD_LENGTH){
|
||||
parse_gps();
|
||||
step = 0;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
} // End for...
|
||||
}
|
||||
AP_GPS_SIRF::init(); // let the superclass do anything it might need here
|
||||
}
|
||||
|
||||
// Private Methods //////////////////////////////////////////////////////////////
|
||||
void
|
||||
AP_GPS_406::parse_gps(void)
|
||||
{
|
||||
uint8_t j;
|
||||
|
||||
fix = buffer[1] > 0;
|
||||
|
||||
latitude = _swapl(&buffer[22]); // lat * 10, 000, 000
|
||||
longitude = _swapl(&buffer[26]); // lon * 10, 000, 000
|
||||
altitude = _swapl(&buffer[34]); // alt in meters * 100
|
||||
ground_speed = _swapi(&buffer[39]); // meters / second * 100
|
||||
|
||||
if (ground_speed >= 50) { // Only updates data if we are really moving...
|
||||
ground_course = (unsigned int)_swapi(&buffer[41]); // meters / second * 100
|
||||
}
|
||||
|
||||
//climb_rate = _swapi(&buffer[45]); // meters / second * 100
|
||||
|
||||
if (latitude == 0) {
|
||||
new_data = false;
|
||||
fix = false;
|
||||
} else {
|
||||
new_data = true;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
AP_GPS_406::configure_gps(void)
|
||||
AP_GPS_406::_configure_gps(void)
|
||||
{
|
||||
const uint8_t gps_header[] = {0xA0, 0xA2, 0x00, 0x08, 0xA6, 0x00};
|
||||
const uint8_t gps_payload[] = {0x02, 0x04, 0x07, 0x09, 0x1B};
|
||||
|
|
@ -170,23 +52,22 @@ AP_GPS_406::configure_gps(void)
|
|||
}
|
||||
}
|
||||
|
||||
// The EM406 defalts to NMEA at 4800bps. We want to switch it to SiRF binary
|
||||
// mode at a higher rate.
|
||||
//
|
||||
// The change is sticky, but only for as long as the internal supercap holds
|
||||
// settings (usually less than a week).
|
||||
//
|
||||
void
|
||||
AP_GPS_406::change_to_sirf_protocol(void)
|
||||
AP_GPS_406::_change_to_sirf_protocol(void)
|
||||
{
|
||||
FastSerial *fs = (FastSerial *)_port; // this is a bit grody...
|
||||
|
||||
fs->begin(4800); // First try at 4800bps
|
||||
fs->begin(4800);
|
||||
delay(300);
|
||||
_port->write(buffer, 28); // Sending special bytes declared at the beginning
|
||||
_port->print(init_str);
|
||||
delay(300);
|
||||
|
||||
fs->begin(9600); // Then try at 9600bps
|
||||
delay(300);
|
||||
_port->write(buffer, 28);
|
||||
delay(300);
|
||||
|
||||
fs->begin(BAUD_RATE); // Finally try at the configured bit rate (57600?)
|
||||
delay(300);
|
||||
_port->write(buffer, 28);
|
||||
|
||||
fs->begin(GPS_406_BITRATE);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,28 +1,31 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
//
|
||||
// EM406 GPS driver for ArduPilot and ArduPilotMega.
|
||||
// Code by Michael Smith, Jason Short, Jordi Muñoz 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.
|
||||
//
|
||||
|
||||
#ifndef AP_GPS_406_h
|
||||
#define AP_GPS_406_h
|
||||
|
||||
#include <GPS.h>
|
||||
#define MAXPAYLOAD 100
|
||||
#include "AP_GPS_SIRF.h"
|
||||
|
||||
class AP_GPS_406 : public GPS
|
||||
#define GPS_406_BITRATE 57600
|
||||
|
||||
class AP_GPS_406 : public AP_GPS_SIRF
|
||||
{
|
||||
public:
|
||||
// Methods
|
||||
AP_GPS_406(Stream *port);
|
||||
void init();
|
||||
void update();
|
||||
void init();
|
||||
|
||||
private:
|
||||
// Internal variables
|
||||
uint8_t step;
|
||||
uint8_t payload_counter;
|
||||
static uint8_t buffer[MAXPAYLOAD];
|
||||
|
||||
void parse_gps();
|
||||
void change_to_sirf_protocol(void);
|
||||
void configure_gps(void);
|
||||
void _change_to_sirf_protocol(void);
|
||||
void _configure_gps(void);
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -0,0 +1,186 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
//
|
||||
// SiRF Binary GPS driver for ArduPilot and ArduPilotMega.
|
||||
// Code by Michael Smith.
|
||||
//
|
||||
// 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.
|
||||
//
|
||||
|
||||
#include "AP_GPS_SIRF.h"
|
||||
#include "WProgram.h"
|
||||
|
||||
// Initialisation messages
|
||||
//
|
||||
// Turn off all messages except for 0x29.
|
||||
//
|
||||
// XXX the bytes show up on the wire, but at least my test unit (EM-411) seems to ignore them.
|
||||
//
|
||||
static uint8_t init_messages[] = {
|
||||
0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xa8, 0xb0, 0xb3,
|
||||
0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x00, 0x29, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd0, 0xb0, 0xb3
|
||||
};
|
||||
|
||||
// Constructors ////////////////////////////////////////////////////////////////
|
||||
AP_GPS_SIRF::AP_GPS_SIRF(Stream *s) : GPS(s)
|
||||
{
|
||||
}
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
void AP_GPS_SIRF::init(void)
|
||||
{
|
||||
// For modules that default to something other than SiRF binary,
|
||||
// the module-specific subclass should take care of switching to binary mode.
|
||||
|
||||
_port->write(init_messages, sizeof(init_messages));
|
||||
}
|
||||
|
||||
// Process bytes available from the stream
|
||||
//
|
||||
// The stream is assumed to contain only messages we recognise. If it
|
||||
// contains other messages, and those messages contain the preamble
|
||||
// bytes, it is possible for this code to fail to synchronise to the
|
||||
// stream immediately. Without buffering the entire message and
|
||||
// re-processing it from the top, this is unavoidable. The parser
|
||||
// attempts to avoid this when possible.
|
||||
//
|
||||
void AP_GPS_SIRF::update(void)
|
||||
{
|
||||
byte data;
|
||||
int numc;
|
||||
|
||||
numc = _port->available();
|
||||
while(numc--) {
|
||||
|
||||
// read the next byte
|
||||
data = _port->read();
|
||||
|
||||
switch(_step){
|
||||
|
||||
// Message preamble 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 1:
|
||||
if (PREAMBLE2 == data) {
|
||||
_step++;
|
||||
break;
|
||||
}
|
||||
_step = 0;
|
||||
// FALLTHROUGH
|
||||
case 0:
|
||||
if(PREAMBLE1 == data)
|
||||
_step++;
|
||||
break;
|
||||
|
||||
// Message length
|
||||
//
|
||||
// We always collect the length so that we can avoid being
|
||||
// fooled by preamble bytes in messages.
|
||||
//
|
||||
case 2:
|
||||
_step++;
|
||||
_payload_length = (uint16_t)data << 8;
|
||||
break;
|
||||
case 3:
|
||||
_step++;
|
||||
_payload_length |= data;
|
||||
_payload_counter = 0;
|
||||
_checksum = 0;
|
||||
break;
|
||||
|
||||
// Message header processing
|
||||
//
|
||||
// We sniff the message ID to determine whether we are going
|
||||
// to gather the message bytes or just discard them.
|
||||
//
|
||||
case 4:
|
||||
_step++;
|
||||
_accumulate(data);
|
||||
_payload_length--;
|
||||
_gather = false;
|
||||
switch(data) {
|
||||
case MSG_GEONAV:
|
||||
if (_payload_length == sizeof(sirf_geonav)) {
|
||||
_gather = true;
|
||||
_msg_id = data;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
_error("ignoring message %d\n", (int)data);
|
||||
}
|
||||
break;
|
||||
|
||||
// Receive message data
|
||||
//
|
||||
// Note that we are effectively guaranteed by the protocol
|
||||
// that the checksum and postamble cannot be mistaken for
|
||||
// the preamble, so if we are discarding bytes in this
|
||||
// message when the payload is done we return directly
|
||||
// to the preamble detector rather than bothering with
|
||||
// the checksum logic.
|
||||
//
|
||||
case 5:
|
||||
if (_gather) { // gather data if requested
|
||||
_accumulate(data);
|
||||
_buffer.bytes[_payload_counter] = data;
|
||||
if (++_payload_counter == _payload_length)
|
||||
_step++;
|
||||
} else {
|
||||
if (++_payload_counter == _payload_length)
|
||||
_step = 0;
|
||||
}
|
||||
break;
|
||||
|
||||
// Checksum and message processing
|
||||
//
|
||||
case 6:
|
||||
_step++;
|
||||
if ((_checksum >> 8) != data) {
|
||||
_error("GPS_SIRF: checksum error\n");
|
||||
_step = 0;
|
||||
}
|
||||
break;
|
||||
case 7:
|
||||
_step = 0;
|
||||
if ((_checksum & 0xff) != data) {
|
||||
_error("GPS_SIRF: checksum error\n");
|
||||
break;
|
||||
}
|
||||
if (_gather) {
|
||||
_parse_gps(); // Parse the new GPS packet
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
AP_GPS_SIRF::_parse_gps(void)
|
||||
{
|
||||
switch(_msg_id) {
|
||||
case MSG_GEONAV:
|
||||
time = _swapl(&_buffer.nav.time);
|
||||
fix = (0 == _buffer.nav.fix_invalid) && (FIX_3D == (_buffer.nav.fix_type & FIX_MASK));
|
||||
latitude = _swapl(&_buffer.nav.latitude);
|
||||
longitude = _swapl(&_buffer.nav.longitude);
|
||||
altitude = _swapl(&_buffer.nav.altitude_msl);
|
||||
ground_speed = _swapi(&_buffer.nav.ground_speed);
|
||||
ground_course = _swapi(&_buffer.nav.ground_course);
|
||||
num_sats = _buffer.nav.satellites;
|
||||
break;
|
||||
}
|
||||
new_data = true;
|
||||
}
|
||||
|
||||
void
|
||||
AP_GPS_SIRF::_accumulate(uint8_t val)
|
||||
{
|
||||
_checksum = (_checksum + val) & 0x7fff;
|
||||
}
|
||||
|
|
@ -0,0 +1,94 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
//
|
||||
// SiRF Binary GPS driver for ArduPilot and ArduPilotMega.
|
||||
// Code by Michael Smith.
|
||||
//
|
||||
// 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.
|
||||
//
|
||||
#ifndef AP_GPS_SIRF_h
|
||||
#define AP_GPS_SIRF_h
|
||||
|
||||
#include <GPS.h>
|
||||
|
||||
class AP_GPS_SIRF : public GPS {
|
||||
public:
|
||||
AP_GPS_SIRF(Stream *s);
|
||||
|
||||
virtual void init();
|
||||
virtual void update();
|
||||
|
||||
private:
|
||||
#pragma pack(1)
|
||||
struct sirf_geonav {
|
||||
uint16_t fix_invalid;
|
||||
uint16_t fix_type;
|
||||
uint16_t week;
|
||||
uint32_t time;
|
||||
uint16_t year;
|
||||
uint8_t month;
|
||||
uint8_t day;
|
||||
uint8_t hour;
|
||||
uint8_t minute;
|
||||
uint16_t second;
|
||||
uint32_t satellites_used;
|
||||
int32_t latitude;
|
||||
int32_t longitude;
|
||||
int32_t altitude_ellipsoid;
|
||||
int32_t altitude_msl;
|
||||
int8_t map_datum;
|
||||
int16_t ground_speed;
|
||||
int16_t ground_course;
|
||||
int16_t res1;
|
||||
int16_t climb_rate;
|
||||
uint16_t heading_rate;
|
||||
uint32_t horizontal_position_error;
|
||||
uint32_t vertical_position_error;
|
||||
uint32_t time_error;
|
||||
int16_t horizontal_velocity_error;
|
||||
int32_t clock_bias;
|
||||
uint32_t clock_bias_error;
|
||||
int32_t clock_drift;
|
||||
uint32_t clock_drift_error;
|
||||
uint32_t distance;
|
||||
uint16_t distance_error;
|
||||
uint16_t heading_error;
|
||||
uint8_t satellites;
|
||||
uint8_t hdop;
|
||||
uint8_t mode_info;
|
||||
};
|
||||
#pragma pack(pop)
|
||||
enum sirf_protocol_bytes {
|
||||
PREAMBLE1 = 0xa0,
|
||||
PREAMBLE2 = 0xa2,
|
||||
POSTAMBLE1 = 0xb0,
|
||||
POSTAMBLE2 = 0xb3,
|
||||
MSG_GEONAV = 0x29
|
||||
};
|
||||
enum sirf_fix_type {
|
||||
FIX_3D = 0x6,
|
||||
FIX_MASK = 0x7
|
||||
};
|
||||
|
||||
|
||||
// State machine state
|
||||
uint8_t _step;
|
||||
uint16_t _checksum;
|
||||
bool _gather;
|
||||
uint16_t _payload_length;
|
||||
uint16_t _payload_counter;
|
||||
uint8_t _msg_id;
|
||||
|
||||
// Message buffer
|
||||
union {
|
||||
sirf_geonav nav;
|
||||
uint8_t bytes[];
|
||||
} _buffer;
|
||||
|
||||
void _parse_gps(void);
|
||||
void _accumulate(uint8_t val);
|
||||
};
|
||||
|
||||
#endif // AP_GPS_SIRF_h
|
||||
Loading…
Reference in New Issue