mirror of https://github.com/ArduPilot/ardupilot
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
parent
a83f663942
commit
c981b7d0b0
|
|
@ -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();
|
||||
|
||||
if (numc > 0){
|
||||
for (int i = 0; i < numc; i++){ // Process bytes received
|
||||
|
||||
// read the next byte
|
||||
data = _port->read();
|
||||
|
||||
switch(step){
|
||||
case 0:
|
||||
if(data == 0xB5)
|
||||
step++;
|
||||
break;
|
||||
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(data == 0x62)
|
||||
step++;
|
||||
else
|
||||
step = 0;
|
||||
if (PREAMBLE2 == data) {
|
||||
_step++;
|
||||
break;
|
||||
}
|
||||
_step = 0;
|
||||
// FALLTHROUGH
|
||||
case 0:
|
||||
if(PREAMBLE1 == data)
|
||||
_step++;
|
||||
break;
|
||||
|
||||
// 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:
|
||||
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
|
||||
_step++;
|
||||
if (CLASS_NAV == data) {
|
||||
_gather = true; // class is interesting, maybe gather
|
||||
_ck_b = _ck_a = data; // reset the checksum accumulators
|
||||
} else {
|
||||
_error("ERR:GPS_CHK!!\n");
|
||||
_error("ignoring class 0x%x\n", (int)data);
|
||||
_gather = false; // class is not interesting, discard
|
||||
}
|
||||
// Variable initialization
|
||||
step = 0;
|
||||
ck_a = 0;
|
||||
ck_b = 0;
|
||||
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;
|
||||
|
||||
// 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;
|
||||
|
||||
// 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;
|
||||
}
|
||||
} // End for...
|
||||
if (_gather)
|
||||
_parse_gps(); // Parse the new GPS packet
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Private Methods //////////////////////////////////////////////////////////////
|
||||
// Private Methods /////////////////////////////////////////////////////////////
|
||||
|
||||
void
|
||||
AP_GPS_UBLOX::parse_gps(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];
|
||||
*/
|
||||
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;
|
||||
break;
|
||||
|
||||
case 0x03: //ID NAV - STATUS
|
||||
fix = ((buffer[4] >= 0x03) && (buffer[5] & 0x01));
|
||||
break;
|
||||
|
||||
case 0x06: //ID NAV - SOL
|
||||
fix = ((buffer[10] >= 0x03) && (buffer[11] & 0x01));
|
||||
num_sats = buffer[47]; // Number of sats...
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Ublox checksum algorithm
|
||||
void AP_GPS_UBLOX::checksum(byte data)
|
||||
{
|
||||
ck_a += data;
|
||||
ck_b += ck_a;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,9 +1,17 @@
|
|||
// -*- 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
|
||||
{
|
||||
|
|
@ -14,23 +22,101 @@ class AP_GPS_UBLOX : public GPS
|
|||
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;
|
||||
// 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
|
||||
|
|
|
|||
Loading…
Reference in New Issue