Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_GSOF: factor out common library for GSOF 

* Add tests too

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
This commit is contained in:
Ryan Friedman 2024-05-11 21:45:11 -06:00 committed by Andrew Tridgell
parent f8295cb52b
commit 921ef6cd36
5 changed files with 427 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

247
libraries/AP_GSOF/AP_GSOF.cpp Normal file
View File

@ -0,0 +1,247 @@
#define ALLOW_DOUBLE_MATH_FUNCTIONS
#include "AP_GSOF_config.h"
#include "AP_GSOF.h"
#if AP_GSOF_ENABLED
#include <AP_Logger/AP_Logger.h>
#include <AP_HAL/utility/sparse-endian.h>
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL& hal;
#define gsof_DEBUGGING 0
#if gsof_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
int
AP_GSOF::parse(const uint8_t temp, const uint8_t n_expected)
{
// https://receiverhelp.trimble.com/oem-gnss/index.html#API_DataCollectorFormatPacketStructure.html
switch (msg.state) {
default:
case Msg_Parser::State::STARTTX:
if (temp == STX) {
msg.state = Msg_Parser::State::STATUS;
msg.read = 0;
msg.checksumcalc = 0;
}
break;
case Msg_Parser::State::STATUS:
msg.status = temp;
msg.state = Msg_Parser::State::PACKETTYPE;
msg.checksumcalc += temp;
break;
case Msg_Parser::State::PACKETTYPE:
msg.packettype = temp;
msg.state = Msg_Parser::State::LENGTH;
msg.checksumcalc += temp;
break;
case Msg_Parser::State::LENGTH:
msg.length = temp;
msg.state = Msg_Parser::State::DATA;
msg.checksumcalc += temp;
break;
case Msg_Parser::State::DATA:
msg.data[msg.read] = temp;
msg.read++;
msg.checksumcalc += temp;
if (msg.read >= msg.length) {
msg.state = Msg_Parser::State::CHECKSUM;
}
break;
case Msg_Parser::State::CHECKSUM:
msg.checksum = temp;
msg.state = Msg_Parser::State::ENDTX;
if (msg.checksum == msg.checksumcalc) {
const auto n_processed = process_message();
if (n_processed != n_expected ) {
return UNEXPECTED_NUM_GSOF_PACKETS;
}
return n_processed;
}
break;
case Msg_Parser::State::ENDTX:
msg.endtx = temp;
msg.state = Msg_Parser::State::STARTTX;
break;
}
return NO_GSOF_DATA;
}
double
AP_GSOF::SwapDouble(const uint8_t* src, const uint32_t pos) const
{
union {
double d;
char bytes[sizeof(double)];
} doubleu;
doubleu.bytes[0] = src[pos + 7];
doubleu.bytes[1] = src[pos + 6];
doubleu.bytes[2] = src[pos + 5];
doubleu.bytes[3] = src[pos + 4];
doubleu.bytes[4] = src[pos + 3];
doubleu.bytes[5] = src[pos + 2];
doubleu.bytes[6] = src[pos + 1];
doubleu.bytes[7] = src[pos + 0];
return doubleu.d;
}
float
AP_GSOF::SwapFloat(const uint8_t* src, const uint32_t pos) const
{
union {
float f;
char bytes[sizeof(float)];
} floatu;
floatu.bytes[0] = src[pos + 3];
floatu.bytes[1] = src[pos + 2];
floatu.bytes[2] = src[pos + 1];
floatu.bytes[3] = src[pos + 0];
return floatu.f;
}
uint32_t
AP_GSOF::SwapUint32(const uint8_t* src, const uint32_t pos) const
{
uint32_t u;
memcpy(&u, &src[pos], sizeof(u));
return be32toh(u);
}
uint16_t
AP_GSOF::SwapUint16(const uint8_t* src, const uint32_t pos) const
{
uint16_t u;
memcpy(&u, &src[pos], sizeof(u));
return be16toh(u);
}
int
AP_GSOF::process_message(void)
{
if (msg.packettype == 0x40) { // GSOF
// https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_TIME.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____25
#if gsof_DEBUGGING
const uint8_t trans_number = msg.data[0];
const uint8_t pageidx = msg.data[1];
const uint8_t maxpageidx = msg.data[2];
Debug("GSOF page: %u of %u (trans_number=%u)",
pageidx, maxpageidx, trans_number);
#endif
// This counts up the number of received packets.
int valid = 0;
// want 1 2 8 9 12
for (uint32_t a = 3; a < msg.length; a++) {
const uint8_t output_type = msg.data[a];
a++;
const uint8_t output_length = msg.data[a];
a++;
// TODO handle corruption on output_length causing buffer overrun?
switch (output_type) {
case POS_TIME:
parse_pos_time(a);
valid++;
break;
case POS:
parse_pos(a);
valid++;
break;
case VEL:
parse_vel(a);
valid++;
break;
case DOP:
parse_dop(a);
valid++;
break;
case POS_SIGMA:
parse_pos_sigma(a);
valid++;
break;
default:
break;
}
a += output_length - 1u;
}
return valid;
}
// No GSOF packets.
return NO_GSOF_DATA;
}
void AP_GSOF::parse_pos_time(uint32_t a)
{
// https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_TIME.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____25
pos_time.time_week_ms = SwapUint32(msg.data, a);
pos_time.time_week = SwapUint16(msg.data, a + 4);
pos_time.num_sats = msg.data[a + 6];
pos_time.pos_flags1 = msg.data[a + 7];
pos_time.pos_flags2 = msg.data[a + 8];
}
void AP_GSOF::parse_pos(uint32_t a)
{
// This packet is not documented in Trimble's receiver help as of May 18, 2023
position.latitude_rad = SwapDouble(msg.data, a);
position.longitude_rad = SwapDouble(msg.data, a + 8);
position.altitude = SwapDouble(msg.data, a + 16);
}
void AP_GSOF::parse_vel(uint32_t a)
{
// https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_Velocity.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____32
vel.velocity_flags = msg.data[a];
constexpr uint8_t BIT_VELOCITY_VALID = 0;
if (BIT_IS_SET(vel.velocity_flags, BIT_VELOCITY_VALID)) {
vel.horizontal_velocity = SwapFloat(msg.data, a + 1);
vel.vertical_velocity = SwapFloat(msg.data, a + 9);
}
constexpr uint8_t BIT_HEADING_VALID = 2;
if (BIT_IS_SET(vel.velocity_flags, BIT_HEADING_VALID)) {
vel.heading = SwapFloat(msg.data, a + 5);
}
}
void AP_GSOF::parse_dop(uint32_t a)
{
// https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_PDOP.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____12
// Skip pdop.
dop.hdop = SwapFloat(msg.data, a + 4);
}
void AP_GSOF::parse_pos_sigma(uint32_t a)
{
// https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_SIGMA.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____24
// Skip pos_rms
pos_sigma.sigma_east = SwapFloat(msg.data, a + 4);
pos_sigma.sigma_north = SwapFloat(msg.data, a + 8);
pos_sigma.sigma_up = SwapFloat(msg.data, a + 16);
}
#endif // AP_GSOF_ENABLED

138
libraries/AP_GSOF/AP_GSOF.h Normal file
View File

@ -0,0 +1,138 @@
// Trimble GSOF protocol parser library.
#pragma once
#include "AP_GSOF_config.h"
#if AP_GSOF_ENABLED
class AP_GSOF
{
public:
static constexpr int NO_GSOF_DATA = 0;
static constexpr int UNEXPECTED_NUM_GSOF_PACKETS = -1;
// Parse a single byte into the buffer.
// When enough data has arrived, it returns the number of GSOF packets parsed in the GENOUT packet.
// If an unexpected number of GSOF packets were parsed, returns UNEXPECTED_NUM_GSOF_PACKETS.
// This means there is no fix.
// If it returns NO_GSOF_DATA, the parser just needs more data.
int parse(const uint8_t temp, const uint8_t n_expected) WARN_IF_UNUSED;
// GSOF packet numbers.
enum msg_idx_t {
POS_TIME = 1,
POS = 2,
VEL = 8,
DOP = 9,
POS_SIGMA = 12
};
// GSOF1
struct PACKED pos_time_t {
uint32_t time_week_ms;
uint16_t time_week;
uint16_t num_sats;
uint8_t pos_flags1;
uint8_t pos_flags2;
uint8_t initialized_number;
};
pos_time_t pos_time;
// GSOF2
struct PACKED pos_t {
// [rad]
double latitude_rad;
// [rad]
double longitude_rad;
// [m]
double altitude;
};
pos_t position;
// GSOF8
struct PACKED vel_t {
uint8_t velocity_flags;
float horizontal_velocity;
// True north, WGS-84 [rad]. Actually ground course, just mislabeled in the docs.
float heading;
// Stored in the direction from GSOF (up = positive).
float vertical_velocity;
// These bytes are included only if a local coordinate system is loaded.
float local_heading;
};
vel_t vel;
// GSOF9
struct PACKED dop_t {
float pdop;
float hdop;
float vdop;
float tdop;
};
dop_t dop;
// GSOF12
struct PACKED pos_sigma_t {
float pos_rms;
float sigma_east;
float sigma_north;
float cov_east_north;
float sigma_up;
float semi_major_axis;
float semi_minor_axis;
float orientation;
float unit_variance;
uint16_t num_epochs;
};
pos_sigma_t pos_sigma;
private:
// Parses current data and returns the number of parsed GSOF messages.
int process_message() WARN_IF_UNUSED;
double SwapDouble(const uint8_t* src, const uint32_t pos) const WARN_IF_UNUSED;
float SwapFloat(const uint8_t* src, const uint32_t pos) const WARN_IF_UNUSED;
uint32_t SwapUint32(const uint8_t* src, const uint32_t pos) const WARN_IF_UNUSED;
uint16_t SwapUint16(const uint8_t* src, const uint32_t pos) const WARN_IF_UNUSED;
void parse_pos_time(uint32_t a);
void parse_pos(uint32_t a);
void parse_vel(uint32_t a);
void parse_dop(uint32_t a);
void parse_pos_sigma(uint32_t a);
struct Msg_Parser {
enum class State {
STARTTX = 0,
STATUS,
PACKETTYPE,
LENGTH,
DATA,
CHECKSUM,
ENDTX
};
State state {State::STARTTX};
uint8_t status;
uint8_t packettype;
uint8_t length;
uint8_t data[256];
uint8_t checksum;
uint8_t endtx;
uint8_t read;
uint8_t checksumcalc;
} msg;
static const uint8_t STX = 0x02;
static const uint8_t ETX = 0x03;
uint8_t packetcount;
uint32_t gsofmsg_time;
};
#endif // AP_GSOF_ENABLED

9
libraries/AP_GSOF/AP_GSOF_config.h Normal file
View File

@ -0,0 +1,9 @@
#pragma once
#include <AP_HAL/AP_HAL_Boards.h>
#include <GCS_MAVLink/GCS_config.h>
#ifndef AP_GSOF_ENABLED
#define AP_GSOF_ENABLED 1
#endif

26
libraries/AP_GSOF/tests/test_gsof.cpp Normal file
View File

@ -0,0 +1,26 @@
// Tests for the GSOF parser.
// * ./waf tests
// * ./build/sitl/tests/test_gsof
#include <AP_gtest.h>
#include <AP_GSOF/AP_GSOF.h>
const AP_HAL::HAL &hal = AP_HAL::get_HAL();
TEST(AP_GSOF, incomplete_packet)
{
AP_GSOF gsof;
EXPECT_FALSE(gsof.parse(0));
}
TEST(AP_GSOF, packet1)
{
// 02084072580000010a1e02e0680909009400000218000000000000000000000000000000000000000000000000080d000000000000000000000000000910000000000000000000000000000000000c260000000000000000000000000000000000000000000000000000000000000000000000000000a503
AP_GSOF gsof;
EXPECT_FALSE(gsof.parse(0));
}
AP_GTEST_MAIN()

7
libraries/AP_GSOF/tests/wscript Normal file
View File

@ -0,0 +1,7 @@
#!/usr/bin/env python3
def build(bld):
bld.ap_find_tests(
use='ap',
)