From 97190c89d19908d0423f1b9a798a58b7b8fa6cea Mon Sep 17 00:00:00 2001 From: Alexey Bulatov Date: Mon, 18 Jan 2016 19:06:15 +0300 Subject: [PATCH] AP_GPS: ERB GPS driver ERB - Emlid Reach Binary protocol. That driver designed for communication between Reach and ArduPilot. Provided opportunities: - Detection of the driver - Parsing of input messages: status of transmitter and navigation information. - Inject GPS messages from base --- libraries/AP_GPS/AP_GPS_ERB.cpp | 293 ++++++++++++++++++++++++++++++++ libraries/AP_GPS/AP_GPS_ERB.h | 139 +++++++++++++++ 2 files changed, 432 insertions(+) create mode 100644 libraries/AP_GPS/AP_GPS_ERB.cpp create mode 100644 libraries/AP_GPS/AP_GPS_ERB.h diff --git a/libraries/AP_GPS/AP_GPS_ERB.cpp b/libraries/AP_GPS/AP_GPS_ERB.cpp new file mode 100644 index 0000000000..322890e3c8 --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_ERB.cpp @@ -0,0 +1,293 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + + // + // Emlid Reach Binary (ERB) GPS driver for ArduPilot. + // ERB protocol: http://files.emlid.com/ERB.pdf + +#include "AP_GPS.h" +#include "AP_GPS_ERB.h" + +#define ERB_DEBUGGING 0 + +#define STAT_FIX_VALID 0x01 + +extern const AP_HAL::HAL& hal; + +#if ERB_DEBUGGING + # define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0) +#else + # define Debug(fmt, args ...) +#endif + +AP_GPS_ERB::AP_GPS_ERB(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) : + AP_GPS_Backend(_gps, _state, _port), + _step(0), + _msg_id(0), + _payload_length(0), + _payload_counter(0), + _fix_count(0), + _new_position(0), + _new_speed(0), + next_fix(AP_GPS::NO_FIX) +{ +} + +// 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. +// +bool +AP_GPS_ERB::read(void) +{ + uint8_t data; + int16_t numc; + bool parsed = false; + + numc = port->available(); + for (int16_t i = 0; i < numc; i++) { // Process bytes received + + // read the next byte + data = port->read(); + + reset: + switch(_step) { + + // Message preamble detection + // + case 1: + if (PREAMBLE2 == data) { + _step++; + break; + } + _step = 0; + Debug("reset %u", __LINE__); + /* no break */ + case 0: + if(PREAMBLE1 == data) + _step++; + break; + + // Message header processing + // + case 2: + _step++; + _msg_id = data; + _ck_b = _ck_a = data; // reset the checksum accumulators + break; + case 3: + _step++; + _ck_b += (_ck_a += data); // checksum byte + _payload_length = data; // payload length low byte + break; + case 4: + _step++; + _ck_b += (_ck_a += data); // checksum byte + _payload_length += (uint16_t)(data<<8); + _payload_counter = 0; // prepare to receive payload + break; + + // Receive message data + // + case 5: + _ck_b += (_ck_a += data); // checksum byte + if (_payload_counter < sizeof(_buffer)) { + _buffer.bytes[_payload_counter] = data; + } + if (++_payload_counter == _payload_length) + _step++; + break; + + // Checksum and message processing + // + case 6: + _step++; + if (_ck_a != data) { + Debug("bad cka %x should be %x", data, _ck_a); + _step = 0; + goto reset; + } + break; + case 7: + _step = 0; + if (_ck_b != data) { + Debug("bad ckb %x should be %x", data, _ck_b); + break; // bad checksum + } + + if (_parse_gps()) { + parsed = true; + } + break; + } + } + return parsed; +} + +bool +AP_GPS_ERB::_parse_gps(void) +{ + switch (_msg_id) { + case MSG_VER: + Debug("Version of ERB protocol %u.%u.%u", + _buffer.ver.ver_high, + _buffer.ver.ver_medium, + _buffer.ver.ver_low); + break; + case MSG_POS: + Debug("Message POS"); + _last_pos_time = _buffer.pos.time; + state.location.lng = (int32_t)(_buffer.pos.longitude * 1e7); + state.location.lat = (int32_t)(_buffer.pos.latitude * 1e7); + state.location.alt = (int32_t)(_buffer.pos.altitude_msl * 1e2); + state.status = next_fix; + _new_position = true; + state.horizontal_accuracy = _buffer.pos.horizontal_accuracy * 1.0e-3f; + state.vertical_accuracy = _buffer.pos.vertical_accuracy * 1.0e-3f; + state.have_horizontal_accuracy = true; + state.have_vertical_accuracy = true; + break; + case MSG_STAT: + Debug("Message STAT fix_status=%u fix_type=%u", + _buffer.stat.fix_status, + _buffer.stat.fix_type); + if (_buffer.stat.fix_status & STAT_FIX_VALID) { + if (_buffer.stat.fix_type == AP_GPS_ERB::FIX_FIX) { + next_fix = AP_GPS::GPS_OK_FIX_3D_RTK; + } else if (_buffer.stat.fix_type == AP_GPS_ERB::FIX_FLOAT) { + next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS; + } else if (_buffer.stat.fix_type == AP_GPS_ERB::FIX_SINGLE) { + next_fix = AP_GPS::GPS_OK_FIX_3D; + } else { + next_fix = AP_GPS::NO_FIX; + state.status = AP_GPS::NO_FIX; + } + } else { + next_fix = AP_GPS::NO_FIX; + state.status = AP_GPS::NO_FIX; + } + state.num_sats = _buffer.stat.satellites; + if (next_fix >= AP_GPS::GPS_OK_FIX_3D) { + state.last_gps_time_ms = AP_HAL::millis(); + state.time_week_ms = _buffer.stat.time; + state.time_week = _buffer.stat.week; + } + break; + case MSG_DOPS: + Debug("Message DOPS"); + state.hdop = _buffer.dops.hDOP; + state.vdop = _buffer.dops.vDOP; + break; + case MSG_VEL: + Debug("Message VEL"); + _last_vel_time = _buffer.vel.time; + state.ground_speed = _buffer.vel.speed_2d * 0.01f; // m/s + // Heading 2D deg * 100000 rescaled to deg * 100 + state.ground_course_cd = wrap_360_cd(_buffer.vel.heading_2d / 1000); + state.have_vertical_velocity = true; + state.velocity.x = _buffer.vel.vel_north * 0.01f; + state.velocity.y = _buffer.vel.vel_east * 0.01f; + state.velocity.z = _buffer.vel.vel_down * 0.01f; + state.have_speed_accuracy = true; + state.speed_accuracy = _buffer.vel.speed_accuracy * 0.01f; + _new_speed = true; + break; + default: + Debug("Unexpected message 0x%02x", (unsigned)_msg_id); + return false; + } + // we only return true when we get new position and speed data + // this ensures we don't use stale data + if (_new_position && _new_speed && _last_vel_time == _last_pos_time) { + _new_speed = _new_position = false; + _fix_count++; + return true; + } + return false; +} + +void +AP_GPS_ERB::inject_data(uint8_t *data, uint8_t len) +{ + + if (port->txspace() > len) { + port->write(data, len); + } else { + Debug("ERB: Not enough TXSPACE"); + } +} + +/* + detect a ERB GPS. Adds one byte, and returns true if the stream + matches a ERB + */ +bool +AP_GPS_ERB::_detect(struct ERB_detect_state &state, uint8_t data) +{ +reset: + switch (state.step) { + case 1: + if (PREAMBLE2 == data) { + state.step++; + break; + } + state.step = 0; + /* no break */ + case 0: + if (PREAMBLE1 == data) + state.step++; + break; + case 2: + state.step++; + state.ck_b = state.ck_a = data; + break; + case 3: + state.step++; + state.ck_b += (state.ck_a += data); + state.payload_length = data; + break; + case 4: + state.step++; + state.ck_b += (state.ck_a += data); + state.payload_counter = 0; + break; + case 5: + state.ck_b += (state.ck_a += data); + if (++state.payload_counter == state.payload_length) + state.step++; + break; + case 6: + state.step++; + if (state.ck_a != data) { + state.step = 0; + goto reset; + } + break; + case 7: + state.step = 0; + if (state.ck_b == data) { + return true; + } else { + goto reset; + } + } + return false; +} diff --git a/libraries/AP_GPS/AP_GPS_ERB.h b/libraries/AP_GPS/AP_GPS_ERB.h new file mode 100644 index 0000000000..51e6fc0020 --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_ERB.h @@ -0,0 +1,139 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +// +// Emlid Reach Binary (ERB) GPS driver for ArduPilot. +// ERB protocol: http://files.emlid.com/ERB.pdf + +#pragma once + +#include +#include "AP_GPS.h" + +class AP_GPS_ERB : public AP_GPS_Backend +{ +public: + AP_GPS_ERB(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port); + + // Methods + bool read(); + + AP_GPS::GPS_Status highest_supported_status(void) { return AP_GPS::GPS_OK_FIX_3D_RTK; } + + static bool _detect(struct ERB_detect_state &state, uint8_t data); + +private: + struct PACKED erb_header { + uint8_t preamble1; + uint8_t preamble2; + uint8_t msg_id; + uint16_t length; + }; + struct PACKED erb_ver { + uint32_t time; + uint8_t ver_high; + uint8_t ver_medium; + uint8_t ver_low; + }; + struct PACKED erb_pos { + uint32_t time; + double longitude; + double latitude; + double altitude_ellipsoid; + double altitude_msl; + uint32_t horizontal_accuracy; + uint32_t vertical_accuracy; + }; + struct PACKED erb_stat { + uint32_t time; + uint16_t week; + uint8_t fix_type; + uint8_t fix_status; + uint8_t satellites; + }; + struct PACKED erb_dops { + uint32_t time; + uint16_t gDOP; + uint16_t pDOP; + uint16_t vDOP; + uint16_t hDOP; + }; + struct PACKED erb_vel { + uint32_t time; + int32_t vel_north; + int32_t vel_east; + int32_t vel_down; + uint32_t speed_2d; + int32_t heading_2d; + uint32_t speed_accuracy; + }; + + // Receive buffer + union PACKED { + erb_ver ver; + erb_pos pos; + erb_stat stat; + erb_dops dops; + erb_vel vel; + uint8_t bytes[]; + } _buffer; + + enum erb_protocol_bytes { + PREAMBLE1 = 0x45, + PREAMBLE2 = 0x52, + MSG_VER = 0x01, + MSG_POS = 0x02, + MSG_STAT = 0x03, + MSG_DOPS = 0x04, + MSG_VEL = 0x05, + }; + + enum erb_fix_type { + FIX_NONE = 0x00, + FIX_SINGLE = 0x01, + FIX_FLOAT = 0x02, + FIX_FIX = 0x03, + }; + + // Packet checksum accumulators + uint8_t _ck_a; + uint8_t _ck_b; + + // State machine state + uint8_t _step; + uint8_t _msg_id; + uint16_t _payload_length; + uint16_t _payload_counter; + + // 8 bit count of fix messages processed, used for periodic processing + uint8_t _fix_count; + + uint32_t _last_pos_time; + uint32_t _last_vel_time; + + // do we have new position information? + bool _new_position:1; + // do we have new speed information? + bool _new_speed:1; + + // Buffer parse & GPS state update + bool _parse_gps(); + + void inject_data(uint8_t *data, uint8_t len); + + // used to update fix between status and position packets + AP_GPS::GPS_Status next_fix; +};