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

Rework the u-blox driver. 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@422 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
DrZiplok@gmail.com 2010-09-06 22:31:18 +00:00
parent a83f663942
commit c981b7d0b0
2 changed files with 260 additions and 172 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

304
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -1,186 +1,188 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/*
GPS_UBLOX.cpp - Ublox GPS library for Arduino
Code by Jordi Muñoz 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 : Ublox protocol
Baud rate : 38400
Active messages :
NAV - POSLLH Geodetic Position Solution, PAGE 66 of datasheet
NAV - VELNED Velocity Solution in NED, PAGE 71 of datasheet
NAV - STATUS Receiver Navigation Status
or
NAV - SOL Navigation Solution Information
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 : 1: GPS FIX, 0: No fix (normal logic)
*/
//
// u-blox UBX 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.
//
#include "AP_GPS_UBLOX.h"
#include "WProgram.h"
// Constructors ////////////////////////////////////////////////////////////////
AP_GPS_UBLOX::AP_GPS_UBLOX(Stream *s) : GPS(s)
{
}
// Public Methods //////////////////////////////////////////////////////////////
// Public Methods ////////////////////////////////////////////////////////////////////
void AP_GPS_UBLOX::init(void)
{
// XXX it might make sense to send some CFG_MSG,CFG_NMEA messages to get the
// right reporting configuration.
}
// 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 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_UBLOX::update(void)
{
byte data;
int numc;
numc = _port->available();
for (int i = 0; i < numc; i++){ // Process bytes received
if (numc > 0){
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;
checksum(id);
step++;
break;
case 4:
payload_length_hi = data;
checksum(payload_length_hi);
step++;
// We check if the payload lenght is valid...
if (payload_length_hi >= MAXPAYLOAD){
_error("ERR:GPS_BAD_PAYLOAD_LENGTH!!\n");
step = 0; // Bad data, so restart to step zero and try again.
ck_a = 0;
ck_b = 0;
}
break;
case 5:
payload_length_lo = data;
checksum(payload_length_lo);
step++;
payload_counter = 0;
break;
case 6: // Payload data read...
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 7:
GPS_ck_a = data; // First checksum byte
step++;
break;
case 8:
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
step = 0;
ck_a = 0;
ck_b = 0;
// 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;
}
} // End for...
}
}
// Private Methods //////////////////////////////////////////////////////////////
void
AP_GPS_UBLOX::parse_gps(void)
{
//Verifing if we are in msg_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 msg_class 1, to know more about classes check PAGE 60 of DataSheet.
if(msg_class == 0x01){
switch(id) {//Checking the UBX ID
case 0x02: // ID NAV - POSLLH
time = *(long *)&buffer[0]; // ms Time of week
longitude = *(long *)&buffer[4]; // lon * 10,000,000
latitude = *(long *)&buffer[8]; // lat * 10,000,000
//altitude = *(long *)&buffer[12]; // elipsoid heigth mm
altitude = *(long *)&buffer[16] / 10; // MSL heigth mm
/*
hacc = (float)*(long *)&buffer[20];
vacc = (float)*(long *)&buffer[24];
*/
new_data = true;
_step = 0;
// FALLTHROUGH
case 0:
if(PREAMBLE1 == data)
_step++;
break;
case 0x03: //ID NAV - STATUS
fix = ((buffer[4] >= 0x03) && (buffer[5] & 0x01));
// Message header processing
//
// We sniff the class and message ID to decide whether we
// are going to gather the message bytes or just discard
// them.
//
// We always collect the length so that we can avoid being
// fooled by preamble bytes in messages.
//
case 2:
_step++;
if (CLASS_NAV == data) {
_gather = true; // class is interesting, maybe gather
_ck_b = _ck_a = data; // reset the checksum accumulators
} else {
_error("ignoring class 0x%x\n", (int)data);
_gather = false; // class is not interesting, discard
}
break;
case 3:
_step++;
_ck_b += (_ck_a += data); // checksum byte
_msg_id = data;
if (_gather) { // if class was interesting
switch(data) {
case MSG_POSLLH: // message is interesting
_expect = sizeof(ubx_nav_posllh);
break;
case MSG_STATUS:
_expect = sizeof(ubx_nav_status);
break;
case MSG_SOL:
_expect = sizeof(ubx_nav_solution);
break;
case MSG_VELNED:
_expect = sizeof(ubx_nav_velned);
break;
default:
_error("ignoring message 0x%x\n", (int)data);
_gather = false; // message is not interesting
}
}
break;
case 4:
_step++;
_ck_b += (_ck_a += data); // checksum byte
_payload_length = data; // payload length low byte
break;
case 5:
_step++;
_ck_b += (_ck_a += data); // checksum byte
_payload_length += (uint16_t)data; // payload length high byte
_payload_counter = 0; // prepare to receive payload
if (_payload_length != _expect) {
_error("payload %d expected %d\n", _payload_length, _expect);
_gather = false;
}
break;
case 0x06: //ID NAV - SOL
fix = ((buffer[10] >= 0x03) && (buffer[11] & 0x01));
num_sats = buffer[47]; // Number of sats...
// Receive message data
//
case 6:
_ck_b += (_ck_a += data); // checksum byte
if (_gather) // gather data if requested
_buffer.bytes[_payload_counter] = data;
if (++_payload_counter == _payload_length)
_step++;
break;
case 0x12: // ID NAV - VELNED
speed_3d = *(long *)&buffer[16]; // cm / s
ground_speed = *(long *)&buffer[20]; // Ground speed 2D cm / s
ground_course = *(long *)&buffer[24] / 1000; // Heading 2D deg * 100000 rescaled to deg * 100
break;
// Checksum and message processing
//
case 7:
_step++;
if (_ck_a != data) {
_error("GPS_UBLOX: checksum error\n");
_step = 0;
}
break;
case 8:
_step = 0;
if (_ck_b != data) {
_error("GPS_UBLOX: checksum error\n");
break;
}
if (_gather)
_parse_gps(); // Parse the new GPS packet
}
}
}
}
// Ublox checksum algorithm
void AP_GPS_UBLOX::checksum(byte data)
// Private Methods /////////////////////////////////////////////////////////////
void
AP_GPS_UBLOX::_parse_gps(void)
{
ck_a += data;
ck_b += ck_a;
switch (_msg_id) {
case MSG_POSLLH:
time = _buffer.posllh.time;
longitude = _buffer.posllh.longitude;
latitude = _buffer.posllh.latitude;
altitude = _buffer.posllh.altitude_msl / 10;
break;
case MSG_STATUS:
fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D);
break;
case MSG_SOL:
fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);
num_sats = _buffer.solution.satellites;
break;
case MSG_VELNED:
speed_3d = _buffer.velned.speed_3d; // cm/s
ground_speed = _buffer.velned.speed_2d; // cm/s
ground_course = _buffer.velned.heading_2d / 1000; // Heading 2D deg * 100000 rescaled to deg * 100
break;
}
new_data = true;
}

128
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -1,36 +1,122 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
//
// u-blox UBX 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.
//
#ifndef AP_GPS_UBLOX_h
#define AP_GPS_UBLOX_h
#include <GPS.h>
#define MAXPAYLOAD 60
class AP_GPS_UBLOX : public GPS
{
public:
public:
// Methods
AP_GPS_UBLOX(Stream *s);
void init();
void update();
void init();
void update();
private:
// Internal variables
uint8_t ck_a; // Packet checksum
uint8_t ck_b;
uint8_t GPS_ck_a;
uint8_t GPS_ck_b;
private:
// u-blox UBX protocol essentials
#pragma pack(1)
struct ubx_nav_posllh {
uint32_t time; // GPS msToW
int32_t longitude;
int32_t latitude;
int32_t altitude_ellipsoid;
int32_t altitude_msl;
uint32_t horizontal_accuracy;
uint32_t vertical_accuracy;
};
struct ubx_nav_status {
uint32_t time; // GPS msToW
uint8_t fix_type;
uint8_t fix_status;
uint8_t differential_status;
uint8_t res;
uint32_t time_to_first_fix;
uint32_t uptime; // milliseconds
};
struct ubx_nav_solution {
uint32_t time;
int32_t time_nsec;
int16_t week;
uint8_t fix_type;
uint8_t fix_status;
int32_t ecef_x;
int32_t ecef_y;
int32_t ecef_z;
uint32_t position_accuracy_3d;
int32_t ecef_x_velocity;
int32_t ecef_y_velocity;
int32_t ecef_z_velocity;
uint32_t speed_accuracy;
uint16_t position_DOP;
uint8_t res;
uint8_t satellites;
uint32_t res2;
};
struct ubx_nav_velned {
uint32_t time; // GPS msToW
int32_t ned_north;
int32_t ned_east;
int32_t ned_down;
uint32_t speed_3d;
uint32_t speed_2d;
int32_t heading_2d;
uint32_t speed_accuracy;
uint32_t heading_accuracy;
};
#pragma pack(pop)
enum ubs_protocol_bytes {
PREAMBLE1 = 0xb5,
PREAMBLE2 = 0x62,
CLASS_NAV = 0x1,
MSG_POSLLH = 0x2,
MSG_STATUS = 0x3,
MSG_SOL = 0x6,
MSG_VELNED = 0x12
};
enum ubs_nav_fix_type {
FIX_NONE = 0,
FIX_DEAD_RECKONING = 1,
FIX_2D = 2,
FIX_3D = 3,
FIX_GPS_DEAD_RECKONING = 4,
FIX_TIME = 5
};
enum ubx_nav_status_bits {
NAV_STATUS_FIX_VALID = 1
};
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];
long ecefVZ;
void parse_gps();
void checksum(unsigned char data);
long join_4_bytes(unsigned char Buffer[]);
// Packet checksum accumulators
uint8_t _ck_a;
uint8_t _ck_b;
// State machine state
uint8_t _step;
uint8_t _msg_id;
bool _gather;
uint16_t _expect;
uint16_t _payload_length;
uint16_t _payload_counter;
// Receive buffer
union {
ubx_nav_posllh posllh;
ubx_nav_status status;
ubx_nav_solution solution;
ubx_nav_velned velned;
uint8_t bytes[];
} _buffer;
// Buffer parse & GPS state update
void _parse_gps();
};
#endif