mirror of https://github.com/ArduPilot/ardupilot
294 lines
8.9 KiB
C++
294 lines
8.9 KiB
C++
// -*- 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;
|
|
}
|