Skip to content

Commit

Permalink
AP_GPS: rewrite uBlox and RTCMv3 parser to allow re-use buffer
Browse files Browse the repository at this point in the history
  • Loading branch information
@peterbarker
peterbarker committed Feb 26, 2024
1 parent 9799832 commit 443b5f0
Show file tree
Hide file tree
Showing 6 changed files with 222 additions and 269 deletions.
39 changes: 25 additions & 14 deletions libraries/AP_GPS/AP_GPS_DroneCAN.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,8 +80,8 @@ AP_GPS_DroneCAN::~AP_GPS_DroneCAN()
_detected_modules[_detected_module].driver = nullptr;

#if GPS_MOVING_BASELINE
if (rtcm3_parser != nullptr) {
delete rtcm3_parser;
if (rtcm3.packet != nullptr) {
delete rtcm3.packet;
}
#endif
}
Expand Down Expand Up @@ -211,8 +211,8 @@ AP_GPS_Backend* AP_GPS_DroneCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state)
}
#if GPS_MOVING_BASELINE
if (backend->role == AP_GPS::GPS_ROLE_MB_BASE) {
backend->rtcm3_parser = new RTCM3_Parser;
if (backend->rtcm3_parser == nullptr) {
backend->rtcm3.packet = new uint8_t[backend->rtcm3.packet_max_length];
if (backend->rtcm3.packet == nullptr) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "DroneCAN%u-%u: failed RTCMv3 parser allocation", _detected_modules[found_match].ap_dronecan->get_driver_index()+1, _detected_modules[found_match].node_id);
}
}
Expand Down Expand Up @@ -548,11 +548,22 @@ void AP_GPS_DroneCAN::handle_moving_baseline_msg(const ardupilot_gnss_MovingBase
return;
}

if (rtcm3_parser == nullptr) {
if (rtcm3.packet == nullptr) {
return;
}
for (int i=0; i < msg.data.len; i++) {
rtcm3_parser->read(msg.data.data[i]);

if (msg.data.len > rtcm3.packet_max_length) {
// couldn't store it anyway
return;
}

// < 0 is "discard bytes" - but we're only expecting complete packets
// == 0 is "might be a packet - but we're only expecting complete packets
// > 0 is found-a-packet
auto parser_result = rtcm3.parser.find_packet(msg.data.data, msg.data.len);
if (parser_result > 0) {
memcpy(rtcm3.packet, msg.data.data, parser_result);
rtcm3.packet_length = parser_result;
}
}

Expand Down Expand Up @@ -581,20 +592,20 @@ void AP_GPS_DroneCAN::handle_relposheading_msg(const ardupilot_gnss_RelPosHeadin
bool AP_GPS_DroneCAN::get_RTCMV3(const uint8_t *&bytes, uint16_t &len)
{
WITH_SEMAPHORE(sem);
if (rtcm3_parser != nullptr) {
len = rtcm3_parser->get_len(bytes);
return len > 0;
if (rtcm3.packet_length == 0) {
return false;
}
return false;
bytes = rtcm3.packet;
len = rtcm3.packet_length;

return true;
}

// clear previous RTCM3 packet
void AP_GPS_DroneCAN::clear_RTCMV3(void)
{
WITH_SEMAPHORE(sem);
if (rtcm3_parser != nullptr) {
rtcm3_parser->clear_packet();
}
rtcm3.packet_length = 0;
}

#endif // GPS_MOVING_BASELINE
Expand Down
7 changes: 6 additions & 1 deletion libraries/AP_GPS/AP_GPS_DroneCAN.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -130,7 +130,12 @@ class AP_GPS_DroneCAN : public AP_GPS_Backend {

#if GPS_MOVING_BASELINE
// RTCM3 parser for when in moving baseline base mode
RTCM3_Parser *rtcm3_parser;
struct {
RTCM3_Parser parser;
uint8_t *packet;
uint16_t packet_length;
const uint16_t packet_max_length{300};
} rtcm3;
#endif
// the role set from GPS_TYPE
AP_GPS::GPS_Role role;
Expand Down
252 changes: 125 additions & 127 deletions libraries/AP_GPS/AP_GPS_UBLOX.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -594,6 +594,68 @@ AP_GPS_UBLOX::_request_port(void)
_send_message(CLASS_CFG, MSG_CFG_PRT, nullptr, 0);
}

int16_t AP_GPS_UBLOX::discard_byte_return(const uint8_t *buffer, uint16_t len)
{
auto preamble_offset = offset_of_byte_in_buffer(PREAMBLE1, &buffer[1], len-1);
if (preamble_offset == -1) {
return -len;
}
return -preamble_offset;
}

int16_t AP_GPS_UBLOX::find_ublox(const uint8_t *buffer, uint16_t buffer_len)
{
if (buffer_len < 6) {
// don't bother unless we have enough data to make all this worth-while:
return 0;
}

auto preamble_offset = offset_of_byte_in_buffer(PREAMBLE1, buffer, buffer_len);
if (preamble_offset == -1) {
return -buffer_len;
}
if (preamble_offset > 0) {
return -preamble_offset;
}
if (buffer[1] != PREAMBLE2) {
return discard_byte_return(buffer, buffer_len);
}
const struct UBLOXBuffer *ublox_buffer = (UBLOXBuffer*)buffer;

// 2 header
// 1 class
// 1 id
// 2 length
// variable payload
// 2 checksum
const uint16_t required_buffer_len = ublox_buffer->payload_length + 8;

if (required_buffer_len > 300) {
// probably corrupt
return discard_byte_return(buffer, buffer_len);
}

if (buffer_len < required_buffer_len) {
// not enough data yet....
return 0;
}

// buffer is long enough; calculate checksum of received data:
uint8_t _ck_a = 0;
uint8_t _ck_b = 0;
for (uint8_t i=2; i<required_buffer_len-2; i++) {
_ck_b += (_ck_a += buffer[i]);
}

// check calculated vs received checksum:
if (_ck_a != buffer[required_buffer_len-2] ||
_ck_b != buffer[required_buffer_len-1]) {
return discard_byte_return(buffer, buffer_len);
}

return required_buffer_len;
}

// Ensure there is enough space for the largest possible outgoing message
// Process bytes available from the stream
//
Expand All @@ -607,7 +669,6 @@ AP_GPS_UBLOX::_request_port(void)
bool
AP_GPS_UBLOX::read(void)
{
bool parsed = false;
uint32_t millis_now = AP_HAL::millis();

// walk through the gps configuration at 1 message per second
Expand Down Expand Up @@ -635,141 +696,78 @@ AP_GPS_UBLOX::read(void)
}
}

const uint16_t numc = MIN(port->available(), 8192U);
for (uint16_t i = 0; i < numc; i++) { // Process bytes received

// read the next byte
uint8_t data;
if (!port->read(data)) {
break;
// consider discarding the buffer; this can happen on corruption
// or on a completed packet:
uint16_t num_bytes_to_discard = ublox_discard;
#if GPS_MOVING_BASELINE
if (rtcm3_parser) {
if (message_parsed) {
num_bytes_to_discard = MAX(ublox_discard, rtcm_discard);
} else {
num_bytes_to_discard = MIN(ublox_discard, rtcm_discard);
}
#if AP_GPS_DEBUG_LOGGING_ENABLED
log_data(&data, 1);
}
#endif

uint8_t *buf_ptr = (uint8_t*)&_outer_buffer;
if (num_bytes_to_discard > 0) {
memmove(
buf_ptr,
&buf_ptr[num_bytes_to_discard],
_buffer_ofs-num_bytes_to_discard);
_buffer_ofs -= num_bytes_to_discard;
#if GPS_MOVING_BASELINE
if (rtcm3_parser) {
if (rtcm3_parser->read(data)) {
// we've found a RTCMv3 packet. We stop parsing at
// this point and reset u-blox parse state. We need to
// stop parsing to give the higher level driver a
// chance to send the RTCMv3 packet to another (rover)
// GPS
_step = 0;
break;
}
}
rtcm_discard = 0;
#endif
ublox_discard = 0;
message_parsed = false;
}

reset:
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;
}
_step = 0;
Debug("reset %u", __LINE__);
FALLTHROUGH;
case 0:
if(PREAMBLE1 == data)
_step++;
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:
_step++;
_class = data;
_ck_b = _ck_a = data; // reset the checksum accumulators
break;
case 3:
_step++;
_ck_b += (_ck_a += data); // checksum byte
_msg_id = data;
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<<8);
if (_payload_length > sizeof(_buffer)) {
Debug("large payload %u", (unsigned)_payload_length);
// assume any payload bigger then what we know about is noise
_payload_length = 0;
_step = 0;
goto reset;
}
_payload_counter = 0; // prepare to receive payload
if (_payload_length == 0) {
// bypass payload and go straight to checksum
_step++;
}
break;

// Receive message data
//
case 6:
_ck_b += (_ck_a += data); // checksum byte
if (_payload_counter < sizeof(_buffer)) {
_buffer[_payload_counter] = data;
}
if (++_payload_counter == _payload_length)
_step++;
break;

// Checksum and message processing
//
case 7:
_step++;
if (_ck_a != data) {
Debug("bad cka %x should be %x", data, _ck_a);
_step = 0;
goto reset;
}
break;
case 8:
_step = 0;
if (_ck_b != data) {
Debug("bad ckb %x should be %x", data, _ck_b);
break; // bad checksum
}
// populate the read buffer:
const uint16_t numc = port->read(&buf_ptr[_buffer_ofs], sizeof(_outer_buffer) - _buffer_ofs);
if (numc == 0) {
return false;
}
#if AP_GPS_DEBUG_LOGGING_ENABLED
log_data(&_buffer[_buffer_ofs], numc);
#endif
_buffer_ofs += numc;

// does RTCM see a packet?
#if GPS_MOVING_BASELINE
if (rtcm3_parser) {
// this is a uBlox packet, discard any partial RTCMv3 state
rtcm3_parser->reset();
}
if (rtcm3_parser && rtcm_discard == 0) {
const int16_t result = rtcm3_parser->find_packet(buf_ptr, _buffer_ofs);
if (result > 0) {
// RTCM found a packet!
memcpy(rtcm3_packet, buf_ptr, MIN(ARRAY_SIZE(rtcm3_packet), uint16_t(result)));
message_parsed = true;
rtcm_discard = result;
} else if (result < 0) {
// RTCM thinks we should discard bytes
rtcm_discard = -result;
} else {
// result is zero; rtcm3 *may* have a packet forming....
}
}
#endif
if (_parse_gps()) {
parsed = true;
}
break;

// is there a uBlox packet there?
if (ublox_discard == 0) {
const int16_t result = find_ublox(buf_ptr, _buffer_ofs);
if (result > 0) {
ublox_discard = result;
_class = _outer_buffer.message_class;
_msg_id = _outer_buffer.id;
return _parse_gps();
} else if (result < 0) {
// ublox thinks we should discard bytes
ublox_discard = -result;
} else {
// result is zero; ublox *may* have a packet forming....
}
}
return parsed;

return false;
}

// Private Methods /////////////////////////////////////////////////////////////
Expand Down Expand Up @@ -2136,7 +2134,7 @@ bool AP_GPS_UBLOX::get_RTCMV3(const uint8_t *&bytes, uint16_t &len)
{
#if GPS_MOVING_BASELINE
if (rtcm3_parser) {
len = rtcm3_parser->get_len(bytes);
// len = rtcm3_parser->get_len(bytes);
return len > 0;
}
#endif
Expand All @@ -2148,7 +2146,7 @@ void AP_GPS_UBLOX::clear_RTCMV3(void)
{
#if GPS_MOVING_BASELINE
if (rtcm3_parser) {
rtcm3_parser->clear_packet();
// rtcm3_parser->clear_packet();
}
#endif
}
Expand Down
Loading

0 comments on commit 443b5f0

Please sign in to comment.