diff --git a/libraries/AP_GSOF/AP_GSOF.cpp b/libraries/AP_GSOF/AP_GSOF.cpp new file mode 100644 index 0000000000..92353e6502 --- /dev/null +++ b/libraries/AP_GSOF/AP_GSOF.cpp @@ -0,0 +1,247 @@ +#define ALLOW_DOUBLE_MATH_FUNCTIONS + +#include "AP_GSOF_config.h" +#include "AP_GSOF.h" + +#if AP_GSOF_ENABLED + + + +#include +#include +#include + +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 + diff --git a/libraries/AP_GSOF/AP_GSOF.h b/libraries/AP_GSOF/AP_GSOF.h new file mode 100644 index 0000000000..3573d270ef --- /dev/null +++ b/libraries/AP_GSOF/AP_GSOF.h @@ -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 diff --git a/libraries/AP_GSOF/AP_GSOF_config.h b/libraries/AP_GSOF/AP_GSOF_config.h new file mode 100644 index 0000000000..a44e7ff407 --- /dev/null +++ b/libraries/AP_GSOF/AP_GSOF_config.h @@ -0,0 +1,9 @@ +#pragma once + +#include +#include + +#ifndef AP_GSOF_ENABLED +#define AP_GSOF_ENABLED 1 +#endif + diff --git a/libraries/AP_GSOF/tests/test_gsof.cpp b/libraries/AP_GSOF/tests/test_gsof.cpp new file mode 100644 index 0000000000..7e23d9c3a6 --- /dev/null +++ b/libraries/AP_GSOF/tests/test_gsof.cpp @@ -0,0 +1,26 @@ +// Tests for the GSOF parser. +// * ./waf tests +// * ./build/sitl/tests/test_gsof + + +#include + +#include + +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() diff --git a/libraries/AP_GSOF/tests/wscript b/libraries/AP_GSOF/tests/wscript new file mode 100644 index 0000000000..a11f1fe060 --- /dev/null +++ b/libraries/AP_GSOF/tests/wscript @@ -0,0 +1,7 @@ +#!/usr/bin/env python3 + +def build(bld): + bld.ap_find_tests( + use='ap', + ) +