Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-21 16:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: Implement GSOF SIM

Browse Source 
Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
This commit is contained in:
Ryan Friedman 2023-05-18 23:09:44 -06:00 committed by Peter Barker
parent f26528edba
commit 6a2a852450
2 changed files with 321 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

321
libraries/SITL/SIM_GPS.cpp
View File

@ -14,10 +14,12 @@
#define ALLOW_DOUBLE_MATH_FUNCTIONS
#include <assert.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_HAL/AP_HAL.h>
#include <SITL/SITL.h>
#include <AP_Common/NMEA.h>
#include <AP_HAL/utility/sparse-endian.h>
// simulated CAN GPS devices get fed from our SITL estimates:
#if HAL_SIM_GPS_EXTERNAL_FIFO_ENABLED
@ -81,6 +83,7 @@ uint32_t GPS::device_baud() const
#if AP_SIM_GPS_FILE_ENABLED
case Type::FILE:
#endif
case Type::GSOF:
return 0; // 0 meaning unset
}
return 0; // 0 meaning unset
@ -187,7 +190,7 @@ void GPS::send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size)
}
/*
return GPS time of week in milliseconds
return GPS time of week
*/
static GPS_TOW gps_time()
{
@ -390,7 +393,7 @@ void GPS::update_ubx(const struct gps_data *d)
memset(&sol, 0, sizeof(sol));
sol.fix_type = d->have_lock?3:0;
sol.fix_status = 221;
sol.satellites = d->have_lock?_sitl->gps_numsats[instance]:3;
sol.satellites = d->have_lock ? _sitl->gps_numsats[instance] : 3;
sol.time = gps_tow.ms;
sol.week = gps_tow.week;
@ -416,7 +419,7 @@ void GPS::update_ubx(const struct gps_data *d)
pvt.fix_type = d->have_lock? 0x3 : 0;
pvt.flags = 0b10000011; // carrsoln=fixed, psm = na, diffsoln and fixok
pvt.flags2 =0;
pvt.num_sv = d->have_lock?_sitl->gps_numsats[instance]:3;
pvt.num_sv = d->have_lock ? _sitl->gps_numsats[instance] : 3;
pvt.lon = d->longitude * 1.0e7;
pvt.lat = d->latitude * 1.0e7;
pvt.height = d->altitude * 1000.0f;
@ -695,7 +698,7 @@ void GPS::update_sbp(const struct gps_data *d)
pos.height = d->altitude;
pos.h_accuracy = _sitl->gps_accuracy[instance]*1000;
pos.v_accuracy = _sitl->gps_accuracy[instance]*1000;
pos.n_sats = _sitl->gps_numsats[instance];
pos.n_sats = d->have_lock ? _sitl->gps_numsats[instance] : 3;
// Send single point position solution
pos.flags = 0;
@ -710,7 +713,7 @@ void GPS::update_sbp(const struct gps_data *d)
velned.d = 1e3 * d->speedD;
velned.h_accuracy = 5e3;
velned.v_accuracy = 5e3;
velned.n_sats = _sitl->gps_numsats[instance];
velned.n_sats = d->have_lock ? _sitl->gps_numsats[instance] : 3;
velned.flags = 0;
sbp_send_message(SBP_VEL_NED_MSGTYPE, 0x2222, sizeof(velned), (uint8_t*)&velned);
@ -809,7 +812,7 @@ void GPS::update_sbp2(const struct gps_data *d)
pos.height = d->altitude;
pos.h_accuracy = _sitl->gps_accuracy[instance]*1000;
pos.v_accuracy = _sitl->gps_accuracy[instance]*1000;
pos.n_sats = _sitl->gps_numsats[instance];
pos.n_sats = d->have_lock ? _sitl->gps_numsats[instance] : 3;
// Send single point position solution
pos.flags = 1;
@ -824,7 +827,7 @@ void GPS::update_sbp2(const struct gps_data *d)
velned.d = 1e3 * d->speedD;
velned.h_accuracy = 5e3;
velned.v_accuracy = 5e3;
velned.n_sats = _sitl->gps_numsats[instance];
velned.n_sats = d->have_lock ? _sitl->gps_numsats[instance] : 3;
velned.flags = 1;
sbp_send_message(SBP_VEL_NED_MSGTYPE, 0x2222, sizeof(velned), (uint8_t*)&velned);
@ -969,7 +972,7 @@ void GPS::update_nova(const struct gps_data *d)
bestpos.lat = d->latitude;
bestpos.lng = d->longitude;
bestpos.hgt = d->altitude;
bestpos.svsused = _sitl->gps_numsats[instance];
bestpos.svsused = d->have_lock ? _sitl->gps_numsats[instance] : 3;
bestpos.latsdev=0.2;
bestpos.lngsdev=0.2;
bestpos.hgtsdev=0.2;
@ -1014,6 +1017,300 @@ uint32_t GPS::CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc
return( crc );
}
void GPS::update_gsof(const struct gps_data *d)
{
// https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_TIME.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____25
constexpr uint8_t GSOF_POS_TIME_TYPE { 0x01 };
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 {
GSOF_POS_TIME_TYPE,
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);
constexpr uint8_t GSOF_POS_TYPE = 0x02;
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 {
GSOF_POS_TYPE,
GSOF_POS_LEN,
pack_double_into_gsof_packet(d->latitude * DEG_TO_RAD_DOUBLE),
pack_double_into_gsof_packet(d->longitude * DEG_TO_RAD_DOUBLE),
pack_double_into_gsof_packet(static_cast<double>(d->altitude))
};
static_assert(sizeof(gsof_pos) - (sizeof(gsof_pos::OUTPUT_RECORD_TYPE) + sizeof(gsof_pos::RECORD_LEN)) == GSOF_POS_LEN);
// https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Velocity.html
constexpr uint8_t GSOF_VEL_TYPE = 0x08;
// 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 {
GSOF_VEL_TYPE,
GSOF_VEL_LEN,
vel_flags,
pack_float_into_gsof_packet(d->speed_2d()),
pack_float_into_gsof_packet(d->heading()),
// Trimble API has ambiguous direction here.
// Intentionally narrow from double.
pack_float_into_gsof_packet(static_cast<float>(d->speedD))
};
static_assert(sizeof(gsof_vel) - (sizeof(gsof_vel::OUTPUT_RECORD_TYPE) + sizeof(gsof_vel::RECORD_LEN)) == GSOF_VEL_LEN);
// https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_PDOP.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____12
constexpr uint8_t GSOF_DOP_TYPE = 0x09;
constexpr uint8_t GSOF_DOP_LEN = 0x10;
const struct PACKED gsof_dop {
const uint8_t OUTPUT_RECORD_TYPE { GSOF_DOP_TYPE };
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);
constexpr auto dop_record_type_size = sizeof(gsof_dop::OUTPUT_RECORD_TYPE);
static_assert(dop_record_type_size == 1);
constexpr auto len_size = sizeof(gsof_dop::RECORD_LEN);
static_assert(len_size == 1);
constexpr auto dop_payload_size = dop_size - (dop_record_type_size + len_size);
static_assert(dop_payload_size == GSOF_DOP_LEN);
constexpr uint8_t GSOF_POS_SIGMA_TYPE = 0x0C;
constexpr uint8_t GSOF_POS_SIGMA_LEN = 0x26;
const struct PACKED gsof_pos_sigma {
const uint8_t OUTPUT_RECORD_TYPE { GSOF_POS_SIGMA_TYPE };
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);
// TODO add GSOF49
const uint8_t payload_sz = sizeof(pos_time) + sizeof(pos) + sizeof(vel) + sizeof(dop) + sizeof(pos_sigma);
uint8_t buf[payload_sz] = {};
uint8_t offset = 0;
memcpy(&buf[offset], &pos_time, sizeof(pos_time));
offset += sizeof(pos_time);
memcpy(&buf[offset], &pos, sizeof(pos));
offset += sizeof(pos);
memcpy(&buf[offset], &vel, sizeof(vel));
offset += sizeof(vel);
memcpy(&buf[offset], &dop, sizeof(dop));
offset += sizeof(dop);
memcpy(&buf[offset], &pos_sigma, sizeof(pos_sigma));
offset += sizeof(pos_sigma);
assert(offset == payload_sz);
send_gsof(buf, sizeof(buf));
}
void GPS::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
const uint8_t STX = 0x02;
// 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];
}
constexpr uint8_t ETX = 0x03;
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::pack_double_into_gsof_packet(const double& src)
{
uint64_t dst;
static_assert(sizeof(src) == sizeof(dst));
memcpy(&dst, &src, sizeof(dst));
dst = htobe64(dst);
return dst;
}
uint32_t GPS::pack_float_into_gsof_packet(const float& src)
{
uint32_t dst;
static_assert(sizeof(src) == sizeof(dst));
memcpy(&dst, &src, sizeof(dst));
dst = htobe32(dst);
return dst;
}
/*
send MSP GPS data
*/
@ -1065,7 +1362,7 @@ void GPS::update_msp(const struct gps_data *d)
msp_gps.gps_week = t.week;
msp_gps.ms_tow = t.ms;
msp_gps.fix_type = d->have_lock?3:0;
msp_gps.satellites_in_view = d->have_lock?_sitl->gps_numsats[instance]:3;
msp_gps.satellites_in_view = d->have_lock ? _sitl->gps_numsats[instance] : 3;
msp_gps.horizontal_pos_accuracy = _sitl->gps_accuracy[instance]*100;
msp_gps.vertical_pos_accuracy = _sitl->gps_accuracy[instance]*100;
msp_gps.horizontal_vel_accuracy = 30;
@ -1186,7 +1483,7 @@ void GPS::update()
const uint8_t idx = instance; // alias to avoid code churn
struct gps_data d;
struct gps_data d {};
// simulate delayed lock times
bool have_lock = (!_sitl->gps_disable[idx] && now_ms >= _sitl->gps_lock_time[idx]*1000UL);
@ -1297,6 +1594,10 @@ void GPS::update()
update_msp(&d);
break;
case Type::GSOF:
update_gsof(&d);
break;
#if AP_SIM_GPS_FILE_ENABLED
case Type::FILE:
update_file();

11
libraries/SITL/SIM_GPS.h
View File

@ -59,7 +59,8 @@ public:
#endif
NOVA = 8,
SBP2 = 9,
MSP = 19,
GSOF = 11, // matches GPS_TYPE
MSP = 19,
};
GPS(uint8_t _instance);
@ -136,6 +137,14 @@ private:
uint32_t CRC32Value(uint32_t icrc);
uint32_t CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc);
// If gsof gets data in, handle it.
// Simply, it should respond to this: https://receiverhelp.trimble.com/oem-gnss/index.html#API_TestingComms.html
void on_data_gsof();
void update_gsof(const struct gps_data *d);
void send_gsof(const uint8_t *buf, const uint16_t size);
uint64_t pack_double_into_gsof_packet(const double& src) WARN_IF_UNUSED;
uint32_t pack_float_into_gsof_packet(const float& src) WARN_IF_UNUSED;
// get delayed data
gps_data interpolate_data(const gps_data &d, uint32_t delay_ms);
};