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

571 lines
24 KiB
C++
Raw Blame History

#include "SIM_config.h"
#if AP_SIM_GPS_TRIMBLE_ENABLED
#include "SIM_GPS_Trimble.h"
#include <SITL/SITL.h>
#include <AP_HAL/utility/sparse-endian.h>
#include <AP_InternalError/AP_InternalError.h>
#include <assert.h>
using namespace SITL;
void GPS_Trimble::publish(const GPS_Data *d)
{
// This logic is to populate output buffer only with enabled channels.
// It also only sends each channel at the configured rate.
const uint64_t now = AP_HAL::millis();
uint8_t buf[MAX_PAYLOAD_SIZE] = {};
uint8_t payload_sz = 0;
uint8_t offset = 0;
if (channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_TIME)] != Output_Rate::OFF){
const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::POSITION_TIME)];
const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_TIME)];
if (now - last_time > RateToPeriodMs(desired_rate)) {
// https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_TIME.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____25
constexpr uint8_t GSOF_POS_TIME_LEN { 0x0A };
// TODO magic number until SITL supports GPS bootcount based on GPSN_ENABLE
const uint8_t bootcount = 17;
// https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%201
enum class POS_FLAGS_1 : uint8_t {
NEW_POSITION = 1U << 0,
CLOCK_FIX_CALULATED = 1U << 1,
HORIZ_FROM_THIS_POS = 1U << 2,
HEIGHT_FROM_THIS_POS = 1U << 3,
RESERVED_4 = 1U << 4,
LEAST_SQ_POSITION = 1U << 5,
RESERVED_6 = 1U << 6,
POSITION_L1_PSEUDORANGES = 1U << 7
};
const uint8_t pos_flags_1 {
uint8_t(POS_FLAGS_1::NEW_POSITION) |
uint8_t(POS_FLAGS_1::CLOCK_FIX_CALULATED) |
uint8_t(POS_FLAGS_1::HORIZ_FROM_THIS_POS) |
uint8_t(POS_FLAGS_1::HEIGHT_FROM_THIS_POS) |
uint8_t(POS_FLAGS_1::RESERVED_4) |
uint8_t(POS_FLAGS_1::LEAST_SQ_POSITION) |
uint8_t(POS_FLAGS_1::POSITION_L1_PSEUDORANGES)
};
// https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%202
enum class POS_FLAGS_2 : uint8_t {
DIFFERENTIAL_POS = 1U << 0,
DIFFERENTIAL_POS_PHASE_RTK = 1U << 1,
POSITION_METHOD_FIXED_PHASE = 1U << 2,
OMNISTAR_ACTIVE = 1U << 3,
DETERMINED_WITH_STATIC_CONSTRAINT = 1U << 4,
NETWORK_RTK = 1U << 5,
DITHERED_RTK = 1U << 6,
BEACON_DGNSS = 1U << 7,
};
// Simulate a GPS without RTK in SIM since there is no RTK SIM params.
// This means these flags are unset:
// NETWORK_RTK, DITHERED_RTK, BEACON_DGNSS
uint8_t pos_flags_2 {0};
if(d->have_lock) {
pos_flags_2 |= uint8_t(POS_FLAGS_2::DIFFERENTIAL_POS);
pos_flags_2 |= uint8_t(POS_FLAGS_2::DIFFERENTIAL_POS_PHASE_RTK);
pos_flags_2 |= uint8_t(POS_FLAGS_2::POSITION_METHOD_FIXED_PHASE);
pos_flags_2 |= uint8_t(POS_FLAGS_2::OMNISTAR_ACTIVE);
pos_flags_2 |= uint8_t(POS_FLAGS_2::DETERMINED_WITH_STATIC_CONSTRAINT);
}
const auto gps_tow = gps_time();
const struct PACKED gsof_pos_time {
const uint8_t OUTPUT_RECORD_TYPE;
const uint8_t RECORD_LEN;
uint32_t time_week_ms;
uint16_t time_week;
uint8_t num_sats;
// https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%201
uint8_t pos_flags_1;
// https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%202
uint8_t pos_flags_2;
uint8_t initialized_num;
} pos_time {
uint8_t(Gsof_Msg_Record_Type::POSITION_TIME),
GSOF_POS_TIME_LEN,
htobe32(gps_tow.ms),
htobe16(gps_tow.week),
d->have_lock ? _sitl->gps_numsats[instance] : uint8_t(3),
pos_flags_1,
pos_flags_2,
bootcount
};
static_assert(sizeof(gsof_pos_time) - (sizeof(gsof_pos_time::OUTPUT_RECORD_TYPE) + sizeof(gsof_pos_time::RECORD_LEN)) == GSOF_POS_TIME_LEN, "Trimble size check failed");
payload_sz += sizeof(pos_time);
memcpy(&buf[offset], &pos_time, sizeof(pos_time));
offset += sizeof(pos_time);
}
}
if (channel_rates[uint8_t(Gsof_Msg_Record_Type::LLH)] != Output_Rate::OFF){
const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::LLH)];
const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::LLH)];
if (now - last_time > RateToPeriodMs(desired_rate)) {
// https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_LLH.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____20
constexpr uint8_t GSOF_POS_LEN = 0x18;
const struct PACKED gsof_pos {
const uint8_t OUTPUT_RECORD_TYPE;
const uint8_t RECORD_LEN;
uint64_t lat;
uint64_t lng;
uint64_t alt;
} pos {
uint8_t(Gsof_Msg_Record_Type::LLH),
GSOF_POS_LEN,
gsof_pack_double(d->latitude * DEG_TO_RAD_DOUBLE),
gsof_pack_double(d->longitude * DEG_TO_RAD_DOUBLE),
gsof_pack_double(static_cast<double>(d->altitude))
};
static_assert(sizeof(gsof_pos) - (sizeof(gsof_pos::OUTPUT_RECORD_TYPE) + sizeof(gsof_pos::RECORD_LEN)) == GSOF_POS_LEN, "Trimble size check failed");
payload_sz += sizeof(pos);
memcpy(&buf[offset], &pos, sizeof(pos));
offset += sizeof(pos);
}
}
if (channel_rates[uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA)] != Output_Rate::OFF){
const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA)];
const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA)];
if (now - last_time > RateToPeriodMs(desired_rate)) {
// https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Velocity.html
// use the smaller packet by ignoring local coordinate system
constexpr uint8_t GSOF_VEL_LEN = 0x0D;
// https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Velocity%20flags
enum class VEL_FIELDS : uint8_t {
VALID = 1U << 0,
CONSECUTIVE_MEASUREMENTS = 1U << 1,
HEADING_VALID = 1U << 2,
RESERVED_3 = 1U << 3,
RESERVED_4 = 1U << 4,
RESERVED_5 = 1U << 5,
RESERVED_6 = 1U << 6,
RESERVED_7 = 1U << 7,
};
uint8_t vel_flags {0};
if(d->have_lock) {
vel_flags |= uint8_t(VEL_FIELDS::VALID);
vel_flags |= uint8_t(VEL_FIELDS::CONSECUTIVE_MEASUREMENTS);
vel_flags |= uint8_t(VEL_FIELDS::HEADING_VALID);
}
const struct PACKED gsof_vel {
const uint8_t OUTPUT_RECORD_TYPE;
const uint8_t RECORD_LEN;
// https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Velocity%20flags
uint8_t flags;
uint32_t horiz_m_p_s;
uint32_t heading_rad;
uint32_t vertical_m_p_s;
} vel {
uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA),
GSOF_VEL_LEN,
vel_flags,
gsof_pack_float(d->speed_2d()),
gsof_pack_float(d->heading()),
// Trimble API has ambiguous direction here.
// Intentionally narrow from double.
gsof_pack_float(static_cast<float>(d->speedD))
};
static_assert(sizeof(gsof_vel) - (sizeof(gsof_vel::OUTPUT_RECORD_TYPE) + sizeof(gsof_vel::RECORD_LEN)) == GSOF_VEL_LEN, "Trimble size check failed");
payload_sz += sizeof(vel);
memcpy(&buf[offset], &vel, sizeof(vel));
offset += sizeof(vel);
}
}
if (channel_rates[uint8_t(Gsof_Msg_Record_Type::PDOP_INFO)] != Output_Rate::OFF){
const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::PDOP_INFO)];
const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::PDOP_INFO)];
if (now - last_time > RateToPeriodMs(desired_rate)) {
// https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_PDOP.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____12
constexpr uint8_t GSOF_DOP_LEN = 0x10;
const struct PACKED gsof_dop {
const uint8_t OUTPUT_RECORD_TYPE { uint8_t(Gsof_Msg_Record_Type::PDOP_INFO) };
const uint8_t RECORD_LEN { GSOF_DOP_LEN };
uint32_t pdop = htobe32(1);
uint32_t hdop = htobe32(1);
uint32_t vdop = htobe32(1);
uint32_t tdop = htobe32(1);
} dop {};
// Check the payload size calculation in the compiler
constexpr auto dop_size = sizeof(gsof_dop);
static_assert(dop_size == 18, "gsof_dop must be 8 bytes");
constexpr auto dop_record_type_size = sizeof(gsof_dop::OUTPUT_RECORD_TYPE);
static_assert(dop_record_type_size == 1, "gsof_dop::OUTPUT_RECORD_TYPE must be 1 byte");
constexpr auto len_size = sizeof(gsof_dop::RECORD_LEN);
static_assert(len_size == 1, "gsof_dop::RECORD_LEN must be 1 bytes");
constexpr auto dop_payload_size = dop_size - (dop_record_type_size + len_size);
static_assert(dop_payload_size == GSOF_DOP_LEN, "dop_payload_size must be GSOF_DOP_LEN bytes");
payload_sz += sizeof(dop);
memcpy(&buf[offset], &dop, sizeof(dop));
offset += sizeof(dop);
}
}
if (channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO)] != Output_Rate::OFF){
const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO)];
const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO)];
if (now - last_time > RateToPeriodMs(desired_rate)) {
// https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_SIGMA.html
constexpr uint8_t GSOF_POS_SIGMA_LEN = 0x26;
const struct PACKED gsof_pos_sigma {
const uint8_t OUTPUT_RECORD_TYPE { uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO) };
const uint8_t RECORD_LEN { GSOF_POS_SIGMA_LEN };
uint32_t pos_rms = htobe32(0);
uint32_t sigma_e = htobe32(0);
uint32_t sigma_n = htobe32(0);
uint32_t cov_en = htobe32(0);
uint32_t sigma_up = htobe32(0);
uint32_t semi_major_axis = htobe32(0);
uint32_t semi_minor_axis = htobe32(0);
uint32_t orientation = htobe32(0);
uint32_t unit_variance = htobe32(0);
uint16_t n_epocs = htobe32(1); // Always 1 for kinematic.
} pos_sigma {};
static_assert(sizeof(gsof_pos_sigma) - (sizeof(gsof_pos_sigma::OUTPUT_RECORD_TYPE) + sizeof(gsof_pos_sigma::RECORD_LEN)) == GSOF_POS_SIGMA_LEN, "Trimble size check failed");
payload_sz += sizeof(pos_sigma);
memcpy(&buf[offset], &pos_sigma, sizeof(pos_sigma));
offset += sizeof(pos_sigma);
}
}
assert(offset == payload_sz);
// Don't send empy frames when all channels are disabled;
if (payload_sz > 0) {
send_gsof(buf, payload_sz);
}
}
bool DCOL_Parser::dcol_parse(const char data_in) {
bool ret = false;
switch (parse_state) {
case Parse_State::WAITING_ON_STX:
if (data_in == STX) {
reset();
parse_state = Parse_State::WAITING_ON_STATUS;
}
break;
case Parse_State::WAITING_ON_STATUS:
if (data_in != (uint8_t)Status::OK) {
// Invalid, status should always be OK.
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
} else {
status = static_cast<Status>(data_in);
parse_state = Parse_State::WAITING_ON_PACKET_TYPE;
}
break;
case Parse_State::WAITING_ON_PACKET_TYPE:
if (data_in == (uint8_t)Packet_Type::COMMAND_APPFILE) {
packet_type = Packet_Type::COMMAND_APPFILE;
} else {
// Unsupported command in this simulator.
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
parse_state = Parse_State::WAITING_ON_LENGTH;
break;
case Parse_State::WAITING_ON_LENGTH:
expected_payload_length = data_in;
parse_state = Parse_State::WAITING_ON_PACKET_DATA;
break;
case Parse_State::WAITING_ON_PACKET_DATA:
payload[cur_payload_idx] = data_in;
if (++cur_payload_idx == expected_payload_length) {
parse_state = Parse_State::WAITING_ON_CSUM;
}
break;
case Parse_State::WAITING_ON_CSUM:
expected_csum = data_in;
parse_state = Parse_State::WAITING_ON_ETX;
break;
case Parse_State::WAITING_ON_ETX:
if (data_in != ETX) {
reset();
}
if (!valid_csum()) {
// GSOF driver sent a packet with invalid CSUM.
// In real GSOF driver, the packet is simply ignored with no reply.
// In the SIM, treat this as a coding error.
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
} else {
ret = parse_payload();
}
reset();
break;
}
return ret;
}
uint32_t DCOL_Parser::RateToPeriodMs(const Output_Rate rate) {
uint32_t min_period_ms = 0;
switch (rate) {
case Output_Rate::OFF:
min_period_ms = 0;
break;
case Output_Rate::FREQ_10_HZ:
min_period_ms = 100;
break;
case Output_Rate::FREQ_50_HZ:
min_period_ms = 20;
break;
case Output_Rate::FREQ_100_HZ:
min_period_ms = 10;
break;
}
return min_period_ms;
}
bool DCOL_Parser::valid_csum() {
uint8_t sum = (uint8_t)status;
sum += (uint8_t)packet_type;
sum += expected_payload_length;
sum += crc_sum_of_bytes(payload, expected_payload_length);
return sum == expected_csum;
}
bool DCOL_Parser::parse_payload() {
bool success = false;
if (packet_type == Packet_Type::COMMAND_APPFILE) {
success = parse_cmd_appfile();
}
return success;
}
bool DCOL_Parser::parse_cmd_appfile() {
// A file control info block contains:
// * version
// * device type
// * start application file flag
// * factory settings flag
constexpr uint8_t file_control_info_block_sz = 4;
// An appfile header contains:
// * transmisison number
// * page index
// * max page index
constexpr uint8_t appfile_header_sz = 3;
constexpr uint8_t min_cmd_appfile_sz = file_control_info_block_sz + appfile_header_sz;
if (expected_payload_length < min_cmd_appfile_sz) {
return false;
}
// For now, ignore appfile_trans_num, return success regardless.
// If the driver doesn't send the right value, it's not clear what the behavior is supposed to be.
// Also would need to handle re-sync.
// For now, just store it for debugging.
appfile_trans_num = payload[0];
// File control information block parsing:
// https://receiverhelp.trimble.com/oem-gnss/ICD_Subtype_Command64h_FileControlInfo.html
constexpr uint8_t expected_app_file_spec_version = 0x03;
constexpr uint8_t file_ctrl_idx = appfile_header_sz;
if (payload[file_ctrl_idx] != expected_app_file_spec_version) {
// Only version 3 is supported at this time.
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
constexpr uint8_t expected_dev_type = 0x00;
if (payload[file_ctrl_idx+1] != expected_dev_type) {
// Only "all" device type is supported.
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
constexpr uint8_t expected_start_flag = 0x01;
if (payload[file_ctrl_idx+2] != expected_start_flag) {
// Only "immediate" start type is supported.
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
constexpr uint8_t expected_factory_settings_flag = 0x00;
if (payload[file_ctrl_idx+3] != expected_factory_settings_flag) {
// Factory settings restore before appfile is not supported.
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
constexpr uint8_t app_file_records_idx = appfile_header_sz + file_control_info_block_sz;
const uint8_t record_type = payload[app_file_records_idx];
if (record_type == (uint8_t)Appfile_Record_Type::SERIAL_PORT_BAUD_RATE_FORMAT) {
// Serial port baud/format
// https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_SerialPort.html
// Ignore serial port index (COM Port) since there's only one in SITL.
// Ignore baud rate because you can't change baud yet in SITL.
// Ignore parity because it can't be changed in SITL.
// Ignore flow control because it's not yet in SITL.
} else if (record_type == (uint8_t)Appfile_Record_Type::OUTPUT_MESSAGE){
// Output Message
// https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html
// Ignore record length to save flash.
// Ignore port index since SITL is only one port.
if (payload[app_file_records_idx + 2] == (uint8_t)(Output_Msg_Msg_Type::GSOF)) {
const auto gsof_submessage_type = payload[app_file_records_idx + 6];
const auto rate = payload[app_file_records_idx + 4];
if (rate == (uint8_t)Output_Rate::OFF) {
channel_rates[gsof_submessage_type] = static_cast<Output_Rate>(rate);
} else if (rate == (uint8_t)Output_Rate::FREQ_10_HZ) {
channel_rates[gsof_submessage_type] = static_cast<Output_Rate>(rate);
} else if (rate == (uint8_t)Output_Rate::FREQ_50_HZ) {
channel_rates[gsof_submessage_type] = static_cast<Output_Rate>(rate);
} else if (rate == (uint8_t)Output_Rate::FREQ_100_HZ) {
channel_rates[gsof_submessage_type] = static_cast<Output_Rate>(rate);
} else {
// Unsupported GSOF rate in SITL.
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
} else {
// Only some data output protocols are supported in SITL.
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
} else {
// Other application file packets are not yet supported.
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
return true;
}
void DCOL_Parser::reset() {
cur_payload_idx = 0;
expected_payload_length = 0;
parse_state = Parse_State::WAITING_ON_STX;
// To be pedantic, one could zero the bytes in the payload,
// but this is skipped because it's extra CPU.
// Note - appfile_trans_number is intended to persist over parser resets.
}
void GPS_Trimble::send_gsof(const uint8_t *buf, const uint16_t size)
{
// All Trimble "Data Collector" packets, including GSOF, are comprised of three fields:
// * A fixed-length packet header (dcol_header)
// * A variable-length data frame (buf)
// * A fixed-length packet trailer (dcol_trailer)
// Reference: // https://receiverhelp.trimble.com/oem-gnss/index.html#API_DataCollectorFormatPacketStructure.html?TocPath=API%2520Documentation%257CData%2520collector%2520format%2520packets%257CData%2520collector%2520format%253A%2520packet%2520structure%257C_____0
// status bitfield
// https://receiverhelp.trimble.com/oem-gnss/index.html#API_ReceiverStatusByte.html?TocPath=API%2520Documentation%257CData%2520collector%2520format%2520packets%257CData%2520collector%2520format%253A%2520packet%2520structure%257C_____1
const uint8_t STATUS = 0xa8;
const uint8_t PACKET_TYPE = 0x40; // Report Packet 40h (GENOUT)
// Before writing the GSOF data buffer, the GSOF header needs added between the DCOL header and the payload data frame.
// https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_GSOF.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____2
static uint8_t TRANSMISSION_NUMBER = 0; // Functionally, this is a sequence number
// Most messages, even GSOF49, only take one page. For SIM, assume it.
assert(size < 0xFA); // GPS SIM doesn't yet support paging
constexpr uint8_t PAGE_INDEX = 0;
constexpr uint8_t MAX_PAGE_INDEX = 0;
const uint8_t gsof_header[3] = {
TRANSMISSION_NUMBER,
PAGE_INDEX,
MAX_PAGE_INDEX,
};
++TRANSMISSION_NUMBER;
// A captured GSOF49 packet from BD940 has LENGTH field set to 0x6d = 109 bytes.
// A captured GSOF49 packet from BD940 has total bytes of 115 bytes.
// Thus, the following 5 bytes are not counted.
// 1) STX
// 2) STATUS
// 3) PACKET TYPE
// 4) LENGTH
// 5) CHECKSUM
// 6) ETX
// This aligns with manual's idea of data bytes:
// "Each message begins with a 4-byte header, followed by the bytes of data in each packet. The packet ends with a 2-byte trailer."
// Thus, for this implementation with single-page single-record per DCOL packet,
// the length is simply the sum of data packet size, the gsof_header size.
const uint8_t length = size + sizeof(gsof_header);
const uint8_t dcol_header[4] {
STX,
STATUS,
PACKET_TYPE,
length
};
// Sum bytes (status + type + length + data bytes) and modulo 256 the summation
// Because it's a uint8, use natural overflow
uint8_t csum = STATUS + PACKET_TYPE + length;
for (size_t i = 0; i < ARRAY_SIZE(gsof_header); i++) {
csum += gsof_header[i];
}
for (size_t i = 0; i < size; i++) {
csum += buf[i];
}
const uint8_t dcol_trailer[2] = {
csum,
ETX
};
write_to_autopilot((char*)dcol_header, sizeof(dcol_header));
write_to_autopilot((char*)gsof_header, sizeof(gsof_header));
write_to_autopilot((char*)buf, size);
write_to_autopilot((char*)dcol_trailer, sizeof(dcol_trailer));
const uint8_t total_size = sizeof(dcol_header) + sizeof(gsof_header) + size + sizeof(dcol_trailer);
// Validate length based on everything but DCOL h
if(dcol_header[3] != total_size - (sizeof(dcol_header) + sizeof(dcol_trailer))) {
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
}
uint64_t GPS_Trimble::gsof_pack_double(const double& src)
{
uint64_t dst;
static_assert(sizeof(src) == sizeof(dst), "src and dst must have equal size");
memcpy(&dst, &src, sizeof(dst));
dst = htobe64(dst);
return dst;
}
uint32_t GPS_Trimble::gsof_pack_float(const float& src)
{
uint32_t dst;
static_assert(sizeof(src) == sizeof(dst), "src and dst must have equal size");
memcpy(&dst, &src, sizeof(dst));
dst = htobe32(dst);
return dst;
}
void GPS_Trimble::update_read() {
// Technically, the max command is slightly larger.
// This will only slightly slow the response for packets that big.
char c[MAX_PAYLOAD_SIZE];
const auto n_read = read_from_autopilot(c, MAX_PAYLOAD_SIZE);
if (n_read > 0) {
for (uint8_t i = 0; i < n_read; i++) {
if (dcol_parse(c[i])) {
constexpr uint8_t response[1] = {(uint8_t)Command_Response::ACK};
write_to_autopilot((char*)response, sizeof(response));
}
// TODO handle NACK for failure
}
}
}
#endif // AP_SIM_GPS_TRIMBLE_ENABLED
Reference in New Issue View Git Blame Copy Permalink