Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rework the u-blox driver.

Browse Source 
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

276
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 -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/* //
GPS_UBLOX.cpp - Ublox GPS library for Arduino // u-blox UBX GPS driver for ArduPilot and ArduPilotMega.
Code by Jordi Muñoz 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 : 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)
*/
#include "AP_GPS_UBLOX.h" #include "AP_GPS_UBLOX.h"
#include "WProgram.h" #include "WProgram.h"
// Constructors //////////////////////////////////////////////////////////////// // Constructors ////////////////////////////////////////////////////////////////
AP_GPS_UBLOX::AP_GPS_UBLOX(Stream *s) : GPS(s) AP_GPS_UBLOX::AP_GPS_UBLOX(Stream *s) : GPS(s)
{ {
} }
// Public Methods //////////////////////////////////////////////////////////////
// Public Methods ////////////////////////////////////////////////////////////////////
void AP_GPS_UBLOX::init(void) 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 // 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 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) void AP_GPS_UBLOX::update(void)
{ {
byte data; byte data;
int numc; int numc;
numc = _port->available(); numc = _port->available();
for (int i = 0; i < numc; i++){ // Process bytes received
if (numc > 0){ // read the next byte
for (int i = 0;i < numc;i++){ // Process bytes received
data = _port->read(); data = _port->read();
switch(step){ switch(_step){
case 0:
if(data == 0xB5)
step++;
break;
// 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: case 1:
if(data == 0x62) if (PREAMBLE2 == data) {
step++; _step++;
else break;
step = 0; }
_step = 0;
// FALLTHROUGH
case 0:
if(PREAMBLE1 == data)
_step++;
break; 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: case 2:
msg_class = data; _step++;
checksum(msg_class); if (CLASS_NAV == data) {
step++; _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; break;
case 3: case 3:
id = data; _step++;
checksum(id); _ck_b += (_ck_a += data); // checksum byte
step++; _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; break;
case 4: case 4:
payload_length_hi = data; _step++;
checksum(payload_length_hi); _ck_b += (_ck_a += data); // checksum byte
step++; _payload_length = data; // payload length low byte
// 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; break;
case 5: case 5:
payload_length_lo = data; _step++;
checksum(payload_length_lo); _ck_b += (_ck_a += data); // checksum byte
step++; _payload_length += (uint16_t)data; // payload length high byte
payload_counter = 0; _payload_counter = 0; // prepare to receive payload
break; if (_payload_length != _expect) {
_error("payload %d expected %d\n", _payload_length, _expect);
case 6: // Payload data read... _gather = false;
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; 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: case 7:
GPS_ck_a = data; // First checksum byte _step++;
step++; if (_ck_a != data) {
_error("GPS_UBLOX: checksum error\n");
_step = 0;
}
break; break;
case 8: case 8:
GPS_ck_b = data; // Second checksum byte _step = 0;
// We end the GPS read... if (_ck_b != data) {
if((ck_a == GPS_ck_a) && (ck_b == GPS_ck_b)){ // Verify the received checksum with the generated checksum.. _error("GPS_UBLOX: checksum error\n");
parse_gps(); // Parse the new GPS packet
}else{
_error("ERR:GPS_CHK!!\n");
}
// Variable initialization
step = 0;
ck_a = 0;
ck_b = 0;
break; break;
} }
} // End for... if (_gather)
_parse_gps(); // Parse the new GPS packet
}
} }
} }
// Private Methods ////////////////////////////////////////////////////////////// // Private Methods /////////////////////////////////////////////////////////////
void 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.. switch (_msg_id) {
// In this case all the message im using are in msg_class 1, to know more about classes check PAGE 60 of DataSheet. case MSG_POSLLH:
if(msg_class == 0x01){ time = _buffer.posllh.time;
switch(id) {//Checking the UBX ID longitude = _buffer.posllh.longitude;
case 0x02: // ID NAV - POSLLH latitude = _buffer.posllh.latitude;
time = *(long *)&buffer[0]; // ms Time of week altitude = _buffer.posllh.altitude_msl / 10;
longitude = *(long *)&buffer[4]; // lon * 10,000,000 break;
latitude = *(long *)&buffer[8]; // lat * 10,000,000 case MSG_STATUS:
//altitude = *(long *)&buffer[12]; // elipsoid heigth mm fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D);
altitude = *(long *)&buffer[16] / 10; // MSL heigth mm break;
/* case MSG_SOL:
hacc = (float)*(long *)&buffer[20]; fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);
vacc = (float)*(long *)&buffer[24]; 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; 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;
} }

124
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 -*- // -*- 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 #ifndef AP_GPS_UBLOX_h
#define AP_GPS_UBLOX_h #define AP_GPS_UBLOX_h
#include <GPS.h> #include <GPS.h>
#define MAXPAYLOAD 60
class AP_GPS_UBLOX : public GPS class AP_GPS_UBLOX : public GPS
{ {
public: public:
// Methods // Methods
AP_GPS_UBLOX(Stream *s); AP_GPS_UBLOX(Stream *s);
void init(); void init();
void update(); void update();
private: private:
// Internal variables // u-blox UBX protocol essentials
uint8_t ck_a; // Packet checksum #pragma pack(1)
uint8_t ck_b; struct ubx_nav_posllh {
uint8_t GPS_ck_a; uint32_t time; // GPS msToW
uint8_t GPS_ck_b; 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; // Packet checksum accumulators
uint8_t msg_class; uint8_t _ck_a;
uint8_t id; uint8_t _ck_b;
uint8_t payload_length_hi;
uint8_t payload_length_lo; // State machine state
uint8_t payload_counter; uint8_t _step;
uint8_t buffer[MAXPAYLOAD]; uint8_t _msg_id;
long ecefVZ; bool _gather;
void parse_gps(); uint16_t _expect;
void checksum(unsigned char data); uint16_t _payload_length;
long join_4_bytes(unsigned char Buffer[]); 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 #endif