diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 15de6f791a..71cfc2cf8c 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -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; } diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index 76dc528db6..dfbce36ca7 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -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 -#define MAXPAYLOAD 60 class AP_GPS_UBLOX : public GPS { - public: +public: // Methods AP_GPS_UBLOX(Stream *s); - 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; - - 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[]); + void init(); + void update(); + +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 + }; + + // 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