Browse Source
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 basemission-4.1.18
Alexey Bulatov
9 years ago
committed by
Tom Pittenger
2 changed files with 432 additions and 0 deletions
@ -0,0 +1,293 @@
@@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/ |
||||
|
||||
//
|
||||
// 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; |
||||
} |
||||
@ -0,0 +1,139 @@
@@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/ |
||||
|
||||
//
|
||||
// Emlid Reach Binary (ERB) GPS driver for ArduPilot.
|
||||
// ERB protocol: http://files.emlid.com/ERB.pdf
|
||||
|
||||
#pragma once |
||||
|
||||
#include <AP_HAL/AP_HAL.h> |
||||
#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; |
||||
}; |
||||
Loading…
Reference in new issue