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

SITL: break up GPS backends into separate files

This commit is contained in:
Peter Barker 2023-11-28 21:24:44 +11:00 committed by Andrew Tridgell
parent 0a4eb251c6
commit 568d633dcc
21 changed files with 2057 additions and 1778 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1562
libraries/SITL/SIM_GPS.cpp
View File

File diff suppressed because it is too large Load Diff

263
libraries/SITL/SIM_GPS.h
View File

@ -23,19 +23,10 @@ param set SERIAL5_PROTOCOL 5
#pragma once #pragma once
#include <AP_HAL/AP_HAL_Boards.h> #include "SIM_config.h"
#ifndef HAL_SIM_GPS_ENABLED
#define HAL_SIM_GPS_ENABLED AP_SIM_ENABLED
#endif
#if HAL_SIM_GPS_ENABLED #if HAL_SIM_GPS_ENABLED
#ifndef AP_SIM_GPS_FILE_ENABLED
// really need to use AP_FileSystem for this.
#define AP_SIM_GPS_FILE_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX)
#endif
#include "SIM_SerialDevice.h" #include "SIM_SerialDevice.h"
namespace SITL { namespace SITL {
@ -87,237 +78,15 @@ protected:
class SIM *_sitl; class SIM *_sitl;
}; struct GPS_TOW {
// Number of weeks since midnight 5-6 January 1980
class GPS_FILE : public GPS_Backend { uint16_t week;
public: // Time since start of the GPS week [mS]
CLASS_NO_COPY(GPS_FILE); uint32_t ms;
using GPS_Backend::GPS_Backend;
void publish(const GPS_Data *d) override;
};
class DCOL_Parser {
// The DCOL parser is used by Trimble GSOF devices.
// It's used for doing configuration.
// https://receiverhelp.trimble.com/oem-gnss/API_DataCollectorFormatPackets.html
public:
// Feed data in to the DCOL parser.
// If the data reaches a parse state that needs to write ACK/NACK back out,
// the function returns true with a populated data_out value.
// Otherwise, it returns false waiting for more data.
bool dcol_parse(const char data_in);
static constexpr uint8_t STX = 0x02;
static constexpr uint8_t ETX = 0x03;
// Receiver status code
enum class Status : uint8_t {
OK = 0x00,
}; };
// https://receiverhelp.trimble.com/oem-gnss/API_DataCollectorFormatPackets.html static GPS_TOW gps_time();
enum class Command_Response : uint8_t { static void simulation_timeval(struct timeval *tv);
ACK = 0x06,
NACK = 0x15,
};
// https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html#Frequenc
enum class Output_Rate : uint8_t {
OFF = 0,
FREQ_10_HZ = 1,
FREQ_50_HZ = 15,
FREQ_100_HZ = 16,
};
// https://receiverhelp.trimble.com/oem-gnss/ICD_ApplicationFilePackets.html?tocpath=API%20Documentation%7CCommand%20and%20report%20packets%7CApplication%20file%20packets%7C_____0
enum class Packet_Type : uint8_t {
COMMAND_APPFILE = 0x64,
};
// https://receiverhelp.trimble.com/oem-gnss/ICD_Pkt_Command64h_APPFILE.html
enum class Appfile_Record_Type : uint8_t {
SERIAL_PORT_BAUD_RATE_FORMAT = 0x02,
OUTPUT_MESSAGE = 0x07,
};
// https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html#Output
enum class Output_Msg_Msg_Type : uint8_t {
GSOF = 10,
};
// https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html#Output2
enum class Gsof_Msg_Record_Type : uint8_t {
POSITION_TIME = 1,
LLH = 2,
VELOCITY_DATA = 8,
PDOP_INFO = 9,
POSITION_SIGMA_INFO = 12,
};
protected:
// https://receiverhelp.trimble.com/oem-gnss/API_DataCollectorFormatPacketStructure.html
static constexpr uint8_t MAX_PAYLOAD_SIZE = 255;
// GSOF supports this many different packet types.
// Only a fraction are supported by the simulator.
// Waste some RAM and allocate arrays for the whole set.
// https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html#Output2
static constexpr uint8_t MAX_CHANNEL_NUM = 70;
// Rates of dynamically enabled channels.
// Assume factory behavior of no enabled channels.
// Each channel can send data out at its own rate.
Output_Rate channel_rates[MAX_CHANNEL_NUM] = {Output_Rate::OFF};
// Last publish time of dynamically enabled channels.
uint32_t last_publish_ms[MAX_CHANNEL_NUM];
static uint32_t RateToPeriodMs(const Output_Rate rate);
private:
// Internal parser implementation state
enum class Parse_State {
WAITING_ON_STX,
WAITING_ON_STATUS,
WAITING_ON_PACKET_TYPE,
WAITING_ON_LENGTH,
WAITING_ON_PACKET_DATA,
WAITING_ON_CSUM,
WAITING_ON_ETX,
};
bool valid_csum();
bool parse_payload();
// https://receiverhelp.trimble.com/oem-gnss/ICD_Pkt_Command64h_APPFILE.html
bool parse_cmd_appfile();
// states for currently parsing packet
Status status;
Parse_State parse_state = {Parse_State::WAITING_ON_STX};
Packet_Type packet_type;
// This is the length in the header.
uint8_t expected_payload_length;
// This is the increasing tally of bytes per packet.
uint8_t cur_payload_idx;
// This is the expected packet checksum in the trailer.
uint8_t expected_csum;
// The application file record transmission number
uint8_t appfile_trans_num;
uint8_t payload[MAX_PAYLOAD_SIZE];
// Clear all parser state/flags for handling a fresh packet.
void reset();
};
class GPS_GSOF : public GPS_Backend, public DCOL_Parser {
public:
CLASS_NO_COPY(GPS_GSOF);
using GPS_Backend::GPS_Backend;
// GPS_Backend overrides
void publish(const GPS_Data *d) override;
void update_read() override;
private:
void send_gsof(const uint8_t *buf, const uint16_t size);
// These packing utilities for GSOF perform a type-safe floating point byteswap.
// They return integer types because returning floating points would involve an extra copy.
uint64_t gsof_pack_double(const double& src) WARN_IF_UNUSED;
uint32_t gsof_pack_float(const float& src) WARN_IF_UNUSED;
};
class GPS_NMEA : public GPS_Backend {
public:
CLASS_NO_COPY(GPS_NMEA);
using GPS_Backend::GPS_Backend;
void publish(const GPS_Data *d) override;
private:
uint8_t nmea_checksum(const char *s);
void nmea_printf(const char *fmt, ...);
void update_nmea(const GPS_Data *d);
};
class GPS_NOVA : public GPS_Backend {
public:
CLASS_NO_COPY(GPS_NOVA);
using GPS_Backend::GPS_Backend;
void publish(const GPS_Data *d) override;
uint32_t device_baud() const override { return 19200; }
private:
void nova_send_message(uint8_t *header, uint8_t headerlength, uint8_t *payload, uint8_t payloadlen);
uint32_t CRC32Value(uint32_t icrc);
uint32_t CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc);
};
class GPS_MSP : public GPS_Backend {
public:
CLASS_NO_COPY(GPS_MSP);
using GPS_Backend::GPS_Backend;
void publish(const GPS_Data *d) override;
};
class GPS_SBP_Common : public GPS_Backend {
public:
CLASS_NO_COPY(GPS_SBP_Common);
using GPS_Backend::GPS_Backend;
protected:
void sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload);
};
class GPS_SBP : public GPS_SBP_Common {
public:
CLASS_NO_COPY(GPS_SBP);
using GPS_SBP_Common::GPS_SBP_Common;
void publish(const GPS_Data *d) override;
};
class GPS_SBP2 : public GPS_SBP_Common {
public:
CLASS_NO_COPY(GPS_SBP2);
using GPS_SBP_Common::GPS_SBP_Common;
void publish(const GPS_Data *d) override;
};
class GPS_UBlox : public GPS_Backend {
public:
CLASS_NO_COPY(GPS_UBlox);
using GPS_Backend::GPS_Backend;
void publish(const GPS_Data *d) override;
private:
void send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size);
}; };
class GPS : public SerialDevice { class GPS : public SerialDevice {
@ -327,16 +96,30 @@ public:
enum Type { enum Type {
NONE = 0, NONE = 0,
#if AP_SIM_GPS_UBLOX_ENABLED
UBLOX = 1, UBLOX = 1,
#endif
#if AP_SIM_GPS_NMEA_ENABLED
NMEA = 5, NMEA = 5,
#endif
#if AP_SIM_GPS_SBP_ENABLED
SBP = 6, SBP = 6,
#endif
#if AP_SIM_GPS_FILE_ENABLED #if AP_SIM_GPS_FILE_ENABLED
FILE = 7, FILE = 7,
#endif #endif
#if AP_SIM_GPS_NOVA_ENABLED
NOVA = 8, NOVA = 8,
#endif
#if AP_SIM_GPS_SBP2_ENABLED
SBP2 = 9, SBP2 = 9,
#endif
#if AP_SIM_GPS_GSOF_ENABLED
GSOF = 11, // matches GPS_TYPE GSOF = 11, // matches GPS_TYPE
#endif
#if AP_SIM_GPS_MSP_ENABLED
MSP = 19, MSP = 19,
#endif
}; };
GPS(uint8_t _instance); GPS(uint8_t _instance);
@ -352,8 +135,6 @@ private:
uint8_t instance; uint8_t instance;
int ext_fifo_fd;
// The last time GPS data was written [mS] // The last time GPS data was written [mS]
uint32_t last_write_update_ms; uint32_t last_write_update_ms;

67
libraries/SITL/SIM_GPS_FILE.cpp Normal file
View File

@ -0,0 +1,67 @@
#include "SIM_config.h"
#if AP_SIM_GPS_FILE_ENABLED
#include "SIM_GPS_FILE.h"
#include <AP_HAL/AP_HAL.h>
#include <SITL/SITL.h>
extern const AP_HAL::HAL& hal;
using namespace SITL;
/*
read file data logged from AP_GPS_DEBUG_LOGGING_ENABLED
*/
void GPS_FILE::publish(const GPS_Data *d)
{
static int fd[2] = {-1,-1};
static uint32_t base_time[2];
const uint16_t lognum = uint16_t(_sitl->gps_log_num.get());
if (instance > 1) {
return;
}
if (fd[instance] == -1) {
char fname[] = "gpsN_NNN.log";
hal.util->snprintf(fname, 13, "gps%u_%03u.log", instance+1, lognum);
fd[instance] = open(fname, O_RDONLY|O_CLOEXEC);
if (fd[instance] == -1) {
return;
}
}
const uint32_t magic = 0x7fe53b04;
struct {
uint32_t magic;
uint32_t time_ms;
uint32_t n;
} header;
uint8_t *buf = nullptr;
while (true) {
if (::read(fd[instance], (void *)&header, sizeof(header)) != sizeof(header) ||
header.magic != magic) {
goto rewind_file;
}
if (header.time_ms+base_time[instance] > AP_HAL::millis()) {
// not ready for this data yet
::lseek(fd[instance], -sizeof(header), SEEK_CUR);
return;
}
buf = new uint8_t[header.n];
if (buf != nullptr && ::read(fd[instance], buf, header.n) == ssize_t(header.n)) {
write_to_autopilot((const char *)buf, header.n);
delete[] buf;
buf = nullptr;
continue;
}
goto rewind_file;
}
rewind_file:
::printf("GPS[%u] rewind\n", unsigned(instance));
base_time[instance] = AP_HAL::millis();
::lseek(fd[instance], 0, SEEK_SET);
delete[] buf;
}
#endif // AP_SIM_GPS_FILE_ENABLED

20
libraries/SITL/SIM_GPS_FILE.h Normal file
View File

@ -0,0 +1,20 @@
#include "SIM_config.h"
#if AP_SIM_GPS_FILE_ENABLED
#include "SIM_GPS.h"
namespace SITL {
class GPS_FILE : public GPS_Backend {
public:
CLASS_NO_COPY(GPS_FILE);
using GPS_Backend::GPS_Backend;
void publish(const GPS_Data *d) override;
};
};
#endif // AP_SIM_GPS_FILE_ENABLED

569
libraries/SITL/SIM_GPS_GSOF.cpp Normal file
View File

@ -0,0 +1,569 @@
#include "SIM_config.h"
#if AP_SIM_GPS_GSOF_ENABLED
#include "SIM_GPS_GSOF.h"
#include <SITL/SITL.h>
#include <AP_HAL/utility/sparse-endian.h>
#include <assert.h>
using namespace SITL;
void GPS_GSOF::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);
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);
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);
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);
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);
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);
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_GSOF::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_GSOF::gsof_pack_double(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_GSOF::gsof_pack_float(const float& src)
{
uint32_t dst;
static_assert(sizeof(src) == sizeof(dst));
memcpy(&dst, &src, sizeof(dst));
dst = htobe32(dst);
return dst;
}
void GPS_GSOF::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_GSOF_ENABLED

149
libraries/SITL/SIM_GPS_GSOF.h Normal file
View File

@ -0,0 +1,149 @@
#pragma once
#include "SIM_config.h"
#if AP_SIM_GPS_GSOF_ENABLED
#include "SIM_GPS.h"
namespace SITL {
class DCOL_Parser {
// The DCOL parser is used by Trimble GSOF devices.
// It's used for doing configuration.
// https://receiverhelp.trimble.com/oem-gnss/API_DataCollectorFormatPackets.html
public:
// Feed data in to the DCOL parser.
// If the data reaches a parse state that needs to write ACK/NACK back out,
// the function returns true with a populated data_out value.
// Otherwise, it returns false waiting for more data.
bool dcol_parse(const char data_in);
static constexpr uint8_t STX = 0x02;
static constexpr uint8_t ETX = 0x03;
// Receiver status code
enum class Status : uint8_t {
OK = 0x00,
};
// https://receiverhelp.trimble.com/oem-gnss/API_DataCollectorFormatPackets.html
enum class Command_Response : uint8_t {
ACK = 0x06,
NACK = 0x15,
};
// https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html#Frequenc
enum class Output_Rate : uint8_t {
OFF = 0,
FREQ_10_HZ = 1,
FREQ_50_HZ = 15,
FREQ_100_HZ = 16,
};
// https://receiverhelp.trimble.com/oem-gnss/ICD_ApplicationFilePackets.html?tocpath=API%20Documentation%7CCommand%20and%20report%20packets%7CApplication%20file%20packets%7C_____0
enum class Packet_Type : uint8_t {
COMMAND_APPFILE = 0x64,
};
// https://receiverhelp.trimble.com/oem-gnss/ICD_Pkt_Command64h_APPFILE.html
enum class Appfile_Record_Type : uint8_t {
SERIAL_PORT_BAUD_RATE_FORMAT = 0x02,
OUTPUT_MESSAGE = 0x07,
};
// https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html#Output
enum class Output_Msg_Msg_Type : uint8_t {
GSOF = 10,
};
// https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html#Output2
enum class Gsof_Msg_Record_Type : uint8_t {
POSITION_TIME = 1,
LLH = 2,
VELOCITY_DATA = 8,
PDOP_INFO = 9,
POSITION_SIGMA_INFO = 12,
};
protected:
// https://receiverhelp.trimble.com/oem-gnss/API_DataCollectorFormatPacketStructure.html
static constexpr uint8_t MAX_PAYLOAD_SIZE = 255;
// GSOF supports this many different packet types.
// Only a fraction are supported by the simulator.
// Waste some RAM and allocate arrays for the whole set.
// https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html#Output2
static constexpr uint8_t MAX_CHANNEL_NUM = 70;
// Rates of dynamically enabled channels.
// Assume factory behavior of no enabled channels.
// Each channel can send data out at its own rate.
Output_Rate channel_rates[MAX_CHANNEL_NUM] = {Output_Rate::OFF};
// Last publish time of dynamically enabled channels.
uint32_t last_publish_ms[MAX_CHANNEL_NUM];
static uint32_t RateToPeriodMs(const Output_Rate rate);
private:
// Internal parser implementation state
enum class Parse_State {
WAITING_ON_STX,
WAITING_ON_STATUS,
WAITING_ON_PACKET_TYPE,
WAITING_ON_LENGTH,
WAITING_ON_PACKET_DATA,
WAITING_ON_CSUM,
WAITING_ON_ETX,
};
bool valid_csum();
bool parse_payload();
// https://receiverhelp.trimble.com/oem-gnss/ICD_Pkt_Command64h_APPFILE.html
bool parse_cmd_appfile();
// states for currently parsing packet
Status status;
Parse_State parse_state = {Parse_State::WAITING_ON_STX};
Packet_Type packet_type;
// This is the length in the header.
uint8_t expected_payload_length;
// This is the increasing tally of bytes per packet.
uint8_t cur_payload_idx;
// This is the expected packet checksum in the trailer.
uint8_t expected_csum;
// The application file record transmission number
uint8_t appfile_trans_num;
uint8_t payload[MAX_PAYLOAD_SIZE];
// Clear all parser state/flags for handling a fresh packet.
void reset();
};
class GPS_GSOF : public GPS_Backend, public DCOL_Parser {
public:
CLASS_NO_COPY(GPS_GSOF);
using GPS_Backend::GPS_Backend;
// GPS_Backend overrides
void publish(const GPS_Data *d) override;
void update_read() override;
private:
void send_gsof(const uint8_t *buf, const uint16_t size);
// These packing utilities for GSOF perform a type-safe floating point byteswap.
// They return integer types because returning floating points would involve an extra copy.
uint64_t gsof_pack_double(const double& src) WARN_IF_UNUSED;
uint32_t gsof_pack_float(const float& src) WARN_IF_UNUSED;
};
};
#endif // AP_SIM_GPS_GSOF_ENABLED

88
libraries/SITL/SIM_GPS_MSP.cpp Normal file
View File

@ -0,0 +1,88 @@
#include "SIM_config.h"
#if AP_SIM_GPS_MSP_ENABLED
#include "SIM_GPS_MSP.h"
#include <SITL/SITL.h>
using namespace SITL;
/*
send MSP GPS data
*/
void GPS_MSP::publish(const GPS_Data *d)
{
struct PACKED {
// header
struct PACKED {
uint8_t dollar = '$';
uint8_t magic = 'X';
uint8_t code = '<';
uint8_t flags;
uint16_t cmd = 0x1F03; // GPS
uint16_t size = 52;
} hdr;
uint8_t instance;
uint16_t gps_week;
uint32_t ms_tow;
uint8_t fix_type;
uint8_t satellites_in_view;
uint16_t horizontal_pos_accuracy; // [cm]
uint16_t vertical_pos_accuracy; // [cm]
uint16_t horizontal_vel_accuracy; // [cm/s]
uint16_t hdop;
int32_t longitude;
int32_t latitude;
int32_t msl_altitude; // cm
int32_t ned_vel_north; // cm/s
int32_t ned_vel_east;
int32_t ned_vel_down;
uint16_t ground_course; // deg * 100, 0..36000
uint16_t true_yaw; // deg * 100, values of 0..36000 are valid. 65535 = no data available
uint16_t year;
uint8_t month;
uint8_t day;
uint8_t hour;
uint8_t min;
uint8_t sec;
// footer CRC
uint8_t crc;
} msp_gps {};
auto t = gps_time();
struct timeval tv;
simulation_timeval(&tv);
auto *tm = gmtime(&tv.tv_sec);
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.horizontal_pos_accuracy = _sitl->gps_accuracy[instance]*100;
msp_gps.vertical_pos_accuracy = _sitl->gps_accuracy[instance]*100;
msp_gps.horizontal_vel_accuracy = 30;
msp_gps.hdop = 100;
msp_gps.longitude = d->longitude * 1.0e7;
msp_gps.latitude = d->latitude * 1.0e7;
msp_gps.msl_altitude = d->altitude * 100;
msp_gps.ned_vel_north = 100 * d->speedN;
msp_gps.ned_vel_east = 100 * d->speedE;
msp_gps.ned_vel_down = 100 * d->speedD;
msp_gps.ground_course = ToDeg(atan2f(d->speedE, d->speedN)) * 100;
msp_gps.true_yaw = wrap_360(d->yaw_deg)*100U; // can send 65535 for no yaw
msp_gps.year = tm->tm_year;
msp_gps.month = tm->tm_mon;
msp_gps.day = tm->tm_mday;
msp_gps.hour = tm->tm_hour;
msp_gps.min = tm->tm_min;
msp_gps.sec = tm->tm_sec;
// CRC is over packet without first 3 bytes and trailing CRC byte
msp_gps.crc = crc8_dvb_s2_update(0, (uint8_t *)&msp_gps.hdr.flags, sizeof(msp_gps)-4);
write_to_autopilot((const char *)&msp_gps, sizeof(msp_gps));
}
#endif // AP_SIM_GPS_MSP_ENABLED

20
libraries/SITL/SIM_GPS_MSP.h Normal file
View File

@ -0,0 +1,20 @@
#include "SIM_config.h"
#if AP_SIM_GPS_MSP_ENABLED
#include "SIM_GPS.h"
namespace SITL {
class GPS_MSP : public GPS_Backend {
public:
CLASS_NO_COPY(GPS_MSP);
using GPS_Backend::GPS_Backend;
void publish(const GPS_Data *d) override;
};
}
#endif // AP_SIM_GPS_MSP_ENABLED

125
libraries/SITL/SIM_GPS_NMEA.cpp Normal file
View File

@ -0,0 +1,125 @@
#include "SIM_config.h"
#if AP_SIM_GPS_NMEA_ENABLED
#include "SIM_GPS_NMEA.h"
#include <SITL/SITL.h>
#include <AP_Common/NMEA.h>
#include <AP_HAL/AP_HAL.h>
extern const AP_HAL::HAL& hal;
using namespace SITL;
/*
formatted print of NMEA message, with checksum appended
*/
void GPS_NMEA::nmea_printf(const char *fmt, ...)
{
va_list ap;
va_start(ap, fmt);
char *s = nmea_vaprintf(fmt, ap);
va_end(ap);
if (s != nullptr) {
write_to_autopilot((const char*)s, strlen(s));
free(s);
}
}
/*
send a new GPS NMEA packet
*/
void GPS_NMEA::publish(const GPS_Data *d)
{
struct timeval tv;
struct tm *tm;
char tstring[20];
char dstring[20];
char lat_string[20];
char lng_string[20];
simulation_timeval(&tv);
tm = gmtime(&tv.tv_sec);
// format time string
hal.util->snprintf(tstring, sizeof(tstring), "%02u%02u%06.3f", tm->tm_hour, tm->tm_min, tm->tm_sec + tv.tv_usec*1.0e-6);
// format date string
hal.util->snprintf(dstring, sizeof(dstring), "%02u%02u%02u", tm->tm_mday, tm->tm_mon+1, tm->tm_year % 100);
// format latitude
double deg = fabs(d->latitude);
hal.util->snprintf(lat_string, sizeof(lat_string), "%02u%08.5f,%c",
(unsigned)deg,
(deg - int(deg))*60,
d->latitude<0?'S':'N');
// format longitude
deg = fabs(d->longitude);
hal.util->snprintf(lng_string, sizeof(lng_string), "%03u%08.5f,%c",
(unsigned)deg,
(deg - int(deg))*60,
d->longitude<0?'W':'E');
nmea_printf("$GPGGA,%s,%s,%s,%01d,%02d,%04.1f,%07.2f,M,0.0,M,,",
tstring,
lat_string,
lng_string,
d->have_lock?1:0,
d->have_lock?_sitl->gps_numsats[instance]:3,
1.2,
d->altitude);
const float speed_mps = d->speed_2d();
const float speed_knots = speed_mps * M_PER_SEC_TO_KNOTS;
const auto heading_rad = d->heading();
//$GPVTG,133.18,T,120.79,M,0.11,N,0.20,K,A*24
nmea_printf("$GPVTG,%.2f,T,%.2f,M,%.2f,N,%.2f,K,A",
tstring,
heading_rad,
heading_rad,
speed_knots,
speed_knots * KNOTS_TO_METERS_PER_SECOND * 3.6);
nmea_printf("$GPRMC,%s,%c,%s,%s,%.2f,%.2f,%s,,",
tstring,
d->have_lock?'A':'V',
lat_string,
lng_string,
speed_knots,
heading_rad,
dstring);
if (_sitl->gps_hdg_enabled[instance] == SITL::SIM::GPS_HEADING_HDT) {
nmea_printf("$GPHDT,%.2f,T", d->yaw_deg);
}
else if (_sitl->gps_hdg_enabled[instance] == SITL::SIM::GPS_HEADING_THS) {
nmea_printf("$GPTHS,%.2f,%c,T", d->yaw_deg, d->have_lock ? 'A' : 'V');
} else if (_sitl->gps_hdg_enabled[instance] == SITL::SIM::GPS_HEADING_KSXT) {
// Unicore support
// $KSXT,20211016083433.00,116.31296102,39.95817066,49.4911,223.57,-11.32,330.19,0.024,,1,3,28,27,,,,-0.012,0.021,0.020,,*2D
nmea_printf("$KSXT,%04u%02u%02u%02u%02u%02u.%02u,%.8f,%.8f,%.4f,%.2f,%.2f,%.2f,%.2f,%.3f,%u,%u,%u,%u,,,,%.3f,%.3f,%.3f,,",
tm->tm_year+1900, tm->tm_mon+1, tm->tm_mday, tm->tm_hour, tm->tm_min, tm->tm_sec, unsigned(tv.tv_usec*1.e-4),
d->longitude, d->latitude,
d->altitude,
wrap_360(d->yaw_deg),
d->pitch_deg,
heading_rad,
speed_mps,
d->roll_deg,
d->have_lock?1:0, // 2=rtkfloat 3=rtkfixed,
3, // fixed rtk yaw solution,
d->have_lock?_sitl->gps_numsats[instance]:3,
d->have_lock?_sitl->gps_numsats[instance]:3,
d->speedE * 3.6,
d->speedN * 3.6,
-d->speedD * 3.6);
}
}
#endif // AP_SIM_GPS_NMEA_ENABLED

29
libraries/SITL/SIM_GPS_NMEA.h Normal file
View File

@ -0,0 +1,29 @@
#pragma once
#include "SIM_config.h"
#if AP_SIM_GPS_NMEA_ENABLED
#include "SIM_GPS.h"
namespace SITL {
class GPS_NMEA : public GPS_Backend {
public:
CLASS_NO_COPY(GPS_NMEA);
using GPS_Backend::GPS_Backend;
void publish(const GPS_Data *d) override;
private:
uint8_t nmea_checksum(const char *s);
void nmea_printf(const char *fmt, ...);
void update_nmea(const GPS_Data *d);
};
};
#endif // AP_SIM_GPS_NMEA_ENABLED

177
libraries/SITL/SIM_GPS_NOVA.cpp Normal file
View File

@ -0,0 +1,177 @@
#include "SIM_config.h"
#if AP_SIM_GPS_NOVA_ENABLED
#include "SIM_GPS_NOVA.h"
#include <SITL/SITL.h>
using namespace SITL;
void GPS_NOVA::publish(const GPS_Data *d)
{
static struct PACKED nova_header
{
// 0
uint8_t preamble[3];
// 3
uint8_t headerlength;
// 4
uint16_t messageid;
// 6
uint8_t messagetype;
//7
uint8_t portaddr;
//8
uint16_t messagelength;
//10
uint16_t sequence;
//12
uint8_t idletime;
//13
uint8_t timestatus;
//14
uint16_t week;
//16
uint32_t tow;
//20
uint32_t recvstatus;
// 24
uint16_t resv;
//26
uint16_t recvswver;
} header;
struct PACKED psrdop
{
float gdop;
float pdop;
float hdop;
float htdop;
float tdop;
float cutoff;
uint32_t svcount;
// extra data for individual prns
} psrdop {};
struct PACKED bestpos
{
uint32_t solstat;
uint32_t postype;
double lat;
double lng;
double hgt;
float undulation;
uint32_t datumid;
float latsdev;
float lngsdev;
float hgtsdev;
// 4 bytes
uint8_t stnid[4];
float diffage;
float sol_age;
uint8_t svstracked;
uint8_t svsused;
uint8_t svsl1;
uint8_t svsmultfreq;
uint8_t resv;
uint8_t extsolstat;
uint8_t galbeisigmask;
uint8_t gpsglosigmask;
} bestpos {};
struct PACKED bestvel
{
uint32_t solstat;
uint32_t veltype;
float latency;
float age;
double horspd;
double trkgnd;
// + up
double vertspd;
float resv;
} bestvel {};
const auto gps_tow = gps_time();
header.preamble[0] = 0xaa;
header.preamble[1] = 0x44;
header.preamble[2] = 0x12;
header.headerlength = sizeof(header);
header.week = gps_tow.week;
header.tow = gps_tow.ms;
header.messageid = 174;
header.messagelength = sizeof(psrdop);
header.sequence += 1;
psrdop.hdop = 1.20;
psrdop.htdop = 1.20;
nova_send_message((uint8_t*)&header,sizeof(header),(uint8_t*)&psrdop, sizeof(psrdop));
header.messageid = 99;
header.messagelength = sizeof(bestvel);
header.sequence += 1;
bestvel.horspd = norm(d->speedN, d->speedE);
bestvel.trkgnd = ToDeg(atan2f(d->speedE, d->speedN));
bestvel.vertspd = -d->speedD;
nova_send_message((uint8_t*)&header,sizeof(header),(uint8_t*)&bestvel, sizeof(bestvel));
header.messageid = 42;
header.messagelength = sizeof(bestpos);
header.sequence += 1;
bestpos.lat = d->latitude;
bestpos.lng = d->longitude;
bestpos.hgt = d->altitude;
bestpos.svsused = d->have_lock ? _sitl->gps_numsats[instance] : 3;
bestpos.latsdev=0.2;
bestpos.lngsdev=0.2;
bestpos.hgtsdev=0.2;
bestpos.solstat=0;
bestpos.postype=32;
nova_send_message((uint8_t*)&header,sizeof(header),(uint8_t*)&bestpos, sizeof(bestpos));
}
void GPS_NOVA::nova_send_message(uint8_t *header, uint8_t headerlength, uint8_t *payload, uint8_t payloadlen)
{
write_to_autopilot((char*)header, headerlength);
write_to_autopilot((char*)payload, payloadlen);
uint32_t crc = CalculateBlockCRC32(headerlength, header, (uint32_t)0);
crc = CalculateBlockCRC32(payloadlen, payload, crc);
write_to_autopilot((char*)&crc, 4);
}
#define CRC32_POLYNOMIAL 0xEDB88320L
uint32_t GPS_NOVA::CRC32Value(uint32_t icrc)
{
int i;
uint32_t crc = icrc;
for ( i = 8 ; i > 0; i-- )
{
if ( crc & 1 )
crc = ( crc >> 1 ) ^ CRC32_POLYNOMIAL;
else
crc >>= 1;
}
return crc;
}
uint32_t GPS_NOVA::CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc)
{
while ( length-- != 0 )
{
crc = ((crc >> 8) & 0x00FFFFFFL) ^ (CRC32Value(((uint32_t) crc ^ *buffer++) & 0xff));
}
return( crc );
}
#endif // AP_SIM_GPS_NOVA_ENABLED

30
libraries/SITL/SIM_GPS_NOVA.h Normal file
View File

@ -0,0 +1,30 @@
#pragma once
#include "SIM_config.h"
#if AP_SIM_GPS_NOVA_ENABLED
#include "SIM_GPS.h"
namespace SITL {
class GPS_NOVA : public GPS_Backend {
public:
CLASS_NO_COPY(GPS_NOVA);
using GPS_Backend::GPS_Backend;
void publish(const GPS_Data *d) override;
uint32_t device_baud() const override { return 19200; }
private:
void nova_send_message(uint8_t *header, uint8_t headerlength, uint8_t *payload, uint8_t payloadlen);
uint32_t CRC32Value(uint32_t icrc);
uint32_t CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc);
};
};
#endif // AP_SIM_GPS_NOVA_ENABLED

122
libraries/SITL/SIM_GPS_SBP.cpp Normal file
View File

@ -0,0 +1,122 @@
#include "SIM_GPS_SBP.h"
#if AP_SIM_GPS_SBP_ENABLED
#include <SITL/SITL.h>
using namespace SITL;
void GPS_SBP::publish(const GPS_Data *d)
{
struct sbp_heartbeat_t {
bool sys_error : 1;
bool io_error : 1;
bool nap_error : 1;
uint8_t res : 5;
uint8_t protocol_minor : 8;
uint8_t protocol_major : 8;
uint8_t res2 : 7;
bool ext_antenna : 1;
} hb; // 4 bytes
struct PACKED sbp_gps_time_t {
uint16_t wn; //< GPS week number
uint32_t tow; //< GPS Time of Week rounded to the nearest ms
int32_t ns; //< Nanosecond remainder of rounded tow
uint8_t flags; //< Status flags (reserved)
} t;
struct PACKED sbp_pos_llh_t {
uint32_t tow; //< GPS Time of Week
double lat; //< Latitude
double lon; //< Longitude
double height; //< Height
uint16_t h_accuracy; //< Horizontal position accuracy estimate
uint16_t v_accuracy; //< Vertical position accuracy estimate
uint8_t n_sats; //< Number of satellites used in solution
uint8_t flags; //< Status flags
} pos;
struct PACKED sbp_vel_ned_t {
uint32_t tow; //< GPS Time of Week
int32_t n; //< Velocity North coordinate
int32_t e; //< Velocity East coordinate
int32_t d; //< Velocity Down coordinate
uint16_t h_accuracy; //< Horizontal velocity accuracy estimate
uint16_t v_accuracy; //< Vertical velocity accuracy estimate
uint8_t n_sats; //< Number of satellites used in solution
uint8_t flags; //< Status flags (reserved)
} velned;
struct PACKED sbp_dops_t {
uint32_t tow; //< GPS Time of Week
uint16_t gdop; //< Geometric Dilution of Precision
uint16_t pdop; //< Position Dilution of Precision
uint16_t tdop; //< Time Dilution of Precision
uint16_t hdop; //< Horizontal Dilution of Precision
uint16_t vdop; //< Vertical Dilution of Precision
uint8_t flags; //< Status flags (reserved)
} dops;
static const uint16_t SBP_HEARTBEAT_MSGTYPE = 0xFFFF;
static const uint16_t SBP_GPS_TIME_MSGTYPE = 0x0100;
static const uint16_t SBP_DOPS_MSGTYPE = 0x0206;
static const uint16_t SBP_POS_LLH_MSGTYPE = 0x0201;
static const uint16_t SBP_VEL_NED_MSGTYPE = 0x0205;
const auto gps_tow = gps_time();
t.wn = gps_tow.week;
t.tow = gps_tow.ms;
t.ns = 0;
t.flags = 0;
sbp_send_message(SBP_GPS_TIME_MSGTYPE, 0x2222, sizeof(t), (uint8_t*)&t);
if (!d->have_lock) {
return;
}
pos.tow = gps_tow.ms;
pos.lon = d->longitude;
pos.lat= d->latitude;
pos.height = d->altitude;
pos.h_accuracy = _sitl->gps_accuracy[instance]*1000;
pos.v_accuracy = _sitl->gps_accuracy[instance]*1000;
pos.n_sats = d->have_lock ? _sitl->gps_numsats[instance] : 3;
// Send single point position solution
pos.flags = 0;
sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos);
// Send "pseudo-absolute" RTK position solution
pos.flags = 1;
sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos);
velned.tow = gps_tow.ms;
velned.n = 1e3 * d->speedN;
velned.e = 1e3 * d->speedE;
velned.d = 1e3 * d->speedD;
velned.h_accuracy = 5e3;
velned.v_accuracy = 5e3;
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);
static uint32_t do_every_count = 0;
if (do_every_count % 5 == 0) {
dops.tow = gps_tow.ms;
dops.gdop = 1;
dops.pdop = 1;
dops.tdop = 1;
dops.hdop = 100;
dops.vdop = 1;
dops.flags = 1;
sbp_send_message(SBP_DOPS_MSGTYPE, 0x2222, sizeof(dops),
(uint8_t*)&dops);
hb.protocol_major = 0; //Sends protocol version 0
sbp_send_message(SBP_HEARTBEAT_MSGTYPE, 0x2222, sizeof(hb),
(uint8_t*)&hb);
}
do_every_count++;
}
#endif // AP_SIM_GPS_SBP_ENABLED

23
libraries/SITL/SIM_GPS_SBP.h Normal file
View File

@ -0,0 +1,23 @@
#pragma once
#include "SIM_config.h"
#if AP_SIM_GPS_SBP_ENABLED
#include "SIM_GPS_SBP_Common.h"
namespace SITL {
class GPS_SBP : public GPS_SBP_Common {
public:
CLASS_NO_COPY(GPS_SBP);
using GPS_SBP_Common::GPS_SBP_Common;
void publish(const GPS_Data *d) override;
};
};
#endif // AP_SIM_GPS_SBP_ENABLED

121
libraries/SITL/SIM_GPS_SBP2.cpp Normal file
View File

@ -0,0 +1,121 @@
#include "SIM_GPS_SBP2.h"
#if AP_SIM_GPS_SBP2_ENABLED
#include <SITL/SITL.h>
using namespace SITL;
void GPS_SBP2::publish(const GPS_Data *d)
{
struct sbp_heartbeat_t {
bool sys_error : 1;
bool io_error : 1;
bool nap_error : 1;
uint8_t res : 5;
uint8_t protocol_minor : 8;
uint8_t protocol_major : 8;
uint8_t res2 : 7;
bool ext_antenna : 1;
} hb; // 4 bytes
struct PACKED sbp_gps_time_t {
uint16_t wn; //< GPS week number
uint32_t tow; //< GPS Time of Week rounded to the nearest ms
int32_t ns; //< Nanosecond remainder of rounded tow
uint8_t flags; //< Status flags (reserved)
} t;
struct PACKED sbp_pos_llh_t {
uint32_t tow; //< GPS Time of Week
double lat; //< Latitude
double lon; //< Longitude
double height; //< Height
uint16_t h_accuracy; //< Horizontal position accuracy estimate
uint16_t v_accuracy; //< Vertical position accuracy estimate
uint8_t n_sats; //< Number of satellites used in solution
uint8_t flags; //< Status flags
} pos;
struct PACKED sbp_vel_ned_t {
uint32_t tow; //< GPS Time of Week
int32_t n; //< Velocity North coordinate
int32_t e; //< Velocity East coordinate
int32_t d; //< Velocity Down coordinate
uint16_t h_accuracy; //< Horizontal velocity accuracy estimate
uint16_t v_accuracy; //< Vertical velocity accuracy estimate
uint8_t n_sats; //< Number of satellites used in solution
uint8_t flags; //< Status flags (reserved)
} velned;
struct PACKED sbp_dops_t {
uint32_t tow; //< GPS Time of Week
uint16_t gdop; //< Geometric Dilution of Precision
uint16_t pdop; //< Position Dilution of Precision
uint16_t tdop; //< Time Dilution of Precision
uint16_t hdop; //< Horizontal Dilution of Precision
uint16_t vdop; //< Vertical Dilution of Precision
uint8_t flags; //< Status flags (reserved)
} dops;
static const uint16_t SBP_HEARTBEAT_MSGTYPE = 0xFFFF;
static const uint16_t SBP_GPS_TIME_MSGTYPE = 0x0102;
static const uint16_t SBP_DOPS_MSGTYPE = 0x0208;
static const uint16_t SBP_POS_LLH_MSGTYPE = 0x020A;
static const uint16_t SBP_VEL_NED_MSGTYPE = 0x020E;
const auto gps_tow = gps_time();
t.wn = gps_tow.week;
t.tow = gps_tow.ms;
t.ns = 0;
t.flags = 1;
sbp_send_message(SBP_GPS_TIME_MSGTYPE, 0x2222, sizeof(t), (uint8_t*)&t);
if (!d->have_lock) {
return;
}
pos.tow = gps_tow.ms;
pos.lon = d->longitude;
pos.lat= d->latitude;
pos.height = d->altitude;
pos.h_accuracy = _sitl->gps_accuracy[instance]*1000;
pos.v_accuracy = _sitl->gps_accuracy[instance]*1000;
pos.n_sats = d->have_lock ? _sitl->gps_numsats[instance] : 3;
// Send single point position solution
pos.flags = 1;
sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos);
// Send "pseudo-absolute" RTK position solution
pos.flags = 4;
sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos);
velned.tow = gps_tow.ms;
velned.n = 1e3 * d->speedN;
velned.e = 1e3 * d->speedE;
velned.d = 1e3 * d->speedD;
velned.h_accuracy = 5e3;
velned.v_accuracy = 5e3;
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);
static uint32_t do_every_count = 0;
if (do_every_count % 5 == 0) {
dops.tow = gps_tow.ms;
dops.gdop = 1;
dops.pdop = 1;
dops.tdop = 1;
dops.hdop = 100;
dops.vdop = 1;
dops.flags = 1;
sbp_send_message(SBP_DOPS_MSGTYPE, 0x2222, sizeof(dops),
(uint8_t*)&dops);
hb.protocol_major = 2; //Sends protocol version 2.0
sbp_send_message(SBP_HEARTBEAT_MSGTYPE, 0x2222, sizeof(hb),
(uint8_t*)&hb);
}
do_every_count++;
}
#endif // AP_SIM_GPS_SBP2_ENABLED

23
libraries/SITL/SIM_GPS_SBP2.h Normal file
View File

@ -0,0 +1,23 @@
#pragma once
#include "SIM_config.h"
#if AP_SIM_GPS_SBP2_ENABLED
#include "SIM_GPS_SBP_Common.h"
namespace SITL {
class GPS_SBP2 : public GPS_SBP_Common {
public:
CLASS_NO_COPY(GPS_SBP2);
using GPS_SBP_Common::GPS_SBP_Common;
void publish(const GPS_Data *d) override;
};
};
#endif // AP_SIM_GPS_SBP2_ENABLED

34
libraries/SITL/SIM_GPS_SBP_Common.cpp Normal file
View File

@ -0,0 +1,34 @@
#include "SIM_config.h"
#if HAL_SIM_GPS_ENABLED
#include "SIM_GPS_SBP_Common.h"
#include <AP_Math/crc.h>
using namespace SITL;
void GPS_SBP_Common::sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload)
{
if (len != 0 && payload == 0) {
return; //SBP_NULL_ERROR;
}
uint8_t preamble = 0x55;
write_to_autopilot((char*)&preamble, 1);
write_to_autopilot((char*)&msg_type, 2);
write_to_autopilot((char*)&sender_id, 2);
write_to_autopilot((char*)&len, 1);
if (len > 0) {
write_to_autopilot((char*)payload, len);
}
uint16_t crc;
crc = crc16_ccitt((uint8_t*)&(msg_type), 2, 0);
crc = crc16_ccitt((uint8_t*)&(sender_id), 2, crc);
crc = crc16_ccitt(&(len), 1, crc);
crc = crc16_ccitt(payload, len, crc);
write_to_autopilot((char*)&crc, 2);
}
#endif

25
libraries/SITL/SIM_GPS_SBP_Common.h Normal file
View File

@ -0,0 +1,25 @@
#pragma once
#include "SIM_config.h"
#if HAL_SIM_GPS_ENABLED
#include "SIM_GPS.h"
namespace SITL {
class GPS_SBP_Common : public GPS_Backend {
public:
CLASS_NO_COPY(GPS_SBP_Common);
using GPS_Backend::GPS_Backend;
protected:
void sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload);
};
};
#endif

324
libraries/SITL/SIM_GPS_UBLOX.cpp Normal file
View File

@ -0,0 +1,324 @@
#include "SIM_config.h"
#if AP_SIM_GPS_UBLOX_ENABLED
#include "SIM_GPS_UBLOX.h"
#include <SITL/SITL.h>
using namespace SITL;
/*
send a UBLOX GPS message
*/
void GPS_UBlox::send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size)
{
const uint8_t PREAMBLE1 = 0xb5;
const uint8_t PREAMBLE2 = 0x62;
const uint8_t CLASS_NAV = 0x1;
uint8_t hdr[6], chk[2];
hdr[0] = PREAMBLE1;
hdr[1] = PREAMBLE2;
hdr[2] = CLASS_NAV;
hdr[3] = msgid;
hdr[4] = size & 0xFF;
hdr[5] = size >> 8;
chk[0] = chk[1] = hdr[2];
chk[1] += (chk[0] += hdr[3]);
chk[1] += (chk[0] += hdr[4]);
chk[1] += (chk[0] += hdr[5]);
for (uint16_t i=0; i<size; i++) {
chk[1] += (chk[0] += buf[i]);
}
write_to_autopilot((char*)hdr, sizeof(hdr));
write_to_autopilot((char*)buf, size);
write_to_autopilot((char*)chk, sizeof(chk));
}
/*
send a new set of GPS UBLOX packets
*/
void GPS_UBlox::publish(const GPS_Data *d)
{
struct PACKED ubx_nav_posllh {
uint32_t time; // GPS msToW
int32_t longitude;
int32_t latitude;
int32_t altitude_ellipsoid;
int32_t altitude_msl;
uint32_t horizontal_accuracy;
uint32_t vertical_accuracy;
} pos {};
struct PACKED ubx_nav_status {
uint32_t time; // GPS msToW
uint8_t fix_type;
uint8_t fix_status;
uint8_t differential_status;
uint8_t res;
uint32_t time_to_first_fix;
uint32_t uptime; // milliseconds
} status {};
struct PACKED ubx_nav_velned {
uint32_t time; // GPS msToW
int32_t ned_north;
int32_t ned_east;
int32_t ned_down;
uint32_t speed_3d;
uint32_t speed_2d;
int32_t heading_2d;
uint32_t speed_accuracy;
uint32_t heading_accuracy;
} velned {};
struct PACKED ubx_nav_solution {
uint32_t time;
int32_t time_nsec;
int16_t week;
uint8_t fix_type;
uint8_t fix_status;
int32_t ecef_x;
int32_t ecef_y;
int32_t ecef_z;
uint32_t position_accuracy_3d;
int32_t ecef_x_velocity;
int32_t ecef_y_velocity;
int32_t ecef_z_velocity;
uint32_t speed_accuracy;
uint16_t position_DOP;
uint8_t res;
uint8_t satellites;
uint32_t res2;
} sol {};
struct PACKED ubx_nav_dop {
uint32_t time; // GPS msToW
uint16_t gDOP;
uint16_t pDOP;
uint16_t tDOP;
uint16_t vDOP;
uint16_t hDOP;
uint16_t nDOP;
uint16_t eDOP;
} dop {};
struct PACKED ubx_nav_pvt {
uint32_t itow;
uint16_t year;
uint8_t month, day, hour, min, sec;
uint8_t valid;
uint32_t t_acc;
int32_t nano;
uint8_t fix_type;
uint8_t flags;
uint8_t flags2;
uint8_t num_sv;
int32_t lon, lat;
int32_t height, h_msl;
uint32_t h_acc, v_acc;
int32_t velN, velE, velD, gspeed;
int32_t head_mot;
uint32_t s_acc;
uint32_t head_acc;
uint16_t p_dop;
uint8_t reserved1[6];
uint32_t headVeh;
uint8_t reserved2[4];
} pvt {};
const uint8_t SV_COUNT = 10;
struct PACKED ubx_nav_svinfo {
uint32_t itow;
uint8_t numCh;
uint8_t globalFlags;
uint8_t reserved1[2];
// repeated block
struct PACKED svinfo_sv {
uint8_t chn;
uint8_t svid;
uint8_t flags;
uint8_t quality;
uint8_t cno;
int8_t elev;
int16_t azim;
int32_t prRes;
} sv[SV_COUNT];
} svinfo {};
enum RELPOSNED {
gnssFixOK = 1U << 0,
diffSoln = 1U << 1,
relPosValid = 1U << 2,
carrSolnFloat = 1U << 3,
carrSolnFixed = 1U << 4,
isMoving = 1U << 5,
refPosMiss = 1U << 6,
refObsMiss = 1U << 7,
relPosHeadingValid = 1U << 8,
relPosNormalized = 1U << 9
};
struct PACKED ubx_nav_relposned {
uint8_t version;
uint8_t reserved1;
uint16_t refStationId;
uint32_t iTOW;
int32_t relPosN;
int32_t relPosE;
int32_t relPosD;
int32_t relPosLength;
int32_t relPosHeading;
uint8_t reserved2[4];
int8_t relPosHPN;
int8_t relPosHPE;
int8_t relPosHPD;
int8_t relPosHPLength;
uint32_t accN;
uint32_t accE;
uint32_t accD;
uint32_t accLength;
uint32_t accHeading;
uint8_t reserved3[4];
uint32_t flags;
} relposned {};
const uint8_t MSG_POSLLH = 0x2;
const uint8_t MSG_STATUS = 0x3;
const uint8_t MSG_DOP = 0x4;
const uint8_t MSG_VELNED = 0x12;
const uint8_t MSG_SOL = 0x6;
const uint8_t MSG_PVT = 0x7;
const uint8_t MSG_SVINFO = 0x30;
const uint8_t MSG_RELPOSNED = 0x3c;
uint32_t _next_nav_sv_info_time = 0;
const auto gps_tow = gps_time();
pos.time = gps_tow.ms;
pos.longitude = d->longitude * 1.0e7;
pos.latitude = d->latitude * 1.0e7;
pos.altitude_ellipsoid = d->altitude * 1000.0f;
pos.altitude_msl = d->altitude * 1000.0f;
pos.horizontal_accuracy = _sitl->gps_accuracy[instance]*1000;
pos.vertical_accuracy = _sitl->gps_accuracy[instance]*1000;
status.time = gps_tow.ms;
status.fix_type = d->have_lock?3:0;
status.fix_status = d->have_lock?1:0;
status.differential_status = 0;
status.res = 0;
status.time_to_first_fix = 0;
status.uptime = AP_HAL::millis();
velned.time = gps_tow.ms;
velned.ned_north = 100.0f * d->speedN;
velned.ned_east = 100.0f * d->speedE;
velned.ned_down = 100.0f * d->speedD;
velned.speed_2d = norm(d->speedN, d->speedE) * 100;
velned.speed_3d = norm(d->speedN, d->speedE, d->speedD) * 100;
velned.heading_2d = ToDeg(atan2f(d->speedE, d->speedN)) * 100000.0f;
if (velned.heading_2d < 0.0f) {
velned.heading_2d += 360.0f * 100000.0f;
}
velned.speed_accuracy = 40;
velned.heading_accuracy = 4;
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.time = gps_tow.ms;
sol.week = gps_tow.week;
dop.time = gps_tow.ms;
dop.gDOP = 65535;
dop.pDOP = 65535;
dop.tDOP = 65535;
dop.vDOP = 200;
dop.hDOP = 121;
dop.nDOP = 65535;
dop.eDOP = 65535;
pvt.itow = gps_tow.ms;
pvt.year = 0;
pvt.month = 0;
pvt.day = 0;
pvt.hour = 0;
pvt.min = 0;
pvt.sec = 0;
pvt.valid = 0; // invalid utc date
pvt.t_acc = 0;
pvt.nano = 0;
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.lon = d->longitude * 1.0e7;
pvt.lat = d->latitude * 1.0e7;
pvt.height = d->altitude * 1000.0f;
pvt.h_msl = d->altitude * 1000.0f;
pvt.h_acc = _sitl->gps_accuracy[instance] * 1000;
pvt.v_acc = _sitl->gps_accuracy[instance] * 1000;
pvt.velN = 1000.0f * d->speedN;
pvt.velE = 1000.0f * d->speedE;
pvt.velD = 1000.0f * d->speedD;
pvt.gspeed = norm(d->speedN, d->speedE) * 1000;
pvt.head_mot = ToDeg(atan2f(d->speedE, d->speedN)) * 1.0e5;
pvt.s_acc = 40;
pvt.head_acc = 38 * 1.0e5;
pvt.p_dop = 65535;
memset(pvt.reserved1, '\0', ARRAY_SIZE(pvt.reserved1));
pvt.headVeh = 0;
memset(pvt.reserved2, '\0', ARRAY_SIZE(pvt.reserved2));
if (_sitl->gps_hdg_enabled[instance] > SITL::SIM::GPS_HEADING_NONE) {
const Vector3f ant1_pos = _sitl->gps_pos_offset[instance^1].get();
const Vector3f ant2_pos = _sitl->gps_pos_offset[instance].get();
Vector3f rel_antenna_pos = ant2_pos - ant1_pos;
Matrix3f rot;
// project attitude back using gyros to get antenna orientation at time of GPS sample
Vector3f gyro(radians(_sitl->state.rollRate),
radians(_sitl->state.pitchRate),
radians(_sitl->state.yawRate));
rot.from_euler(radians(_sitl->state.rollDeg), radians(_sitl->state.pitchDeg), radians(d->yaw_deg));
const float lag = _sitl->gps_delay_ms[instance] * 0.001;
rot.rotate(gyro * (-lag));
rel_antenna_pos = rot * rel_antenna_pos;
relposned.version = 1;
relposned.iTOW = gps_tow.ms;
relposned.relPosN = rel_antenna_pos.x * 100;
relposned.relPosE = rel_antenna_pos.y * 100;
relposned.relPosD = rel_antenna_pos.z * 100;
relposned.relPosLength = rel_antenna_pos.length() * 100;
relposned.relPosHeading = degrees(Vector2f(rel_antenna_pos.x, rel_antenna_pos.y).angle()) * 1.0e5;
relposned.flags = gnssFixOK | diffSoln | carrSolnFixed | isMoving | relPosValid | relPosHeadingValid;
}
send_ubx(MSG_POSLLH, (uint8_t*)&pos, sizeof(pos));
send_ubx(MSG_STATUS, (uint8_t*)&status, sizeof(status));
send_ubx(MSG_VELNED, (uint8_t*)&velned, sizeof(velned));
send_ubx(MSG_SOL, (uint8_t*)&sol, sizeof(sol));
send_ubx(MSG_DOP, (uint8_t*)&dop, sizeof(dop));
send_ubx(MSG_PVT, (uint8_t*)&pvt, sizeof(pvt));
if (_sitl->gps_hdg_enabled[instance] > SITL::SIM::GPS_HEADING_NONE) {
send_ubx(MSG_RELPOSNED, (uint8_t*)&relposned, sizeof(relposned));
}
if (gps_tow.ms > _next_nav_sv_info_time) {
svinfo.itow = gps_tow.ms;
svinfo.numCh = 32;
svinfo.globalFlags = 4; // u-blox 8/M8
// fill in the SV's with some data even though firmware does not currently use it
// note that this is not using num_sats as we aren't dynamically creating this to match
for (uint8_t i = 0; i < SV_COUNT; i++) {
svinfo.sv[i].chn = i;
svinfo.sv[i].svid = i;
svinfo.sv[i].flags = (i < _sitl->gps_numsats[instance]) ? 0x7 : 0x6; // sv used, diff correction data, orbit information
svinfo.sv[i].quality = 7; // code and carrier lock and time synchronized
svinfo.sv[i].cno = MAX(20, 30 - i);
svinfo.sv[i].elev = MAX(30, 90 - i);
svinfo.sv[i].azim = i;
// not bothering to fill in prRes
}
send_ubx(MSG_SVINFO, (uint8_t*)&svinfo, sizeof(svinfo));
_next_nav_sv_info_time = gps_tow.ms + 10000; // 10 second delay
}
}
#endif // AP_SIM_GPS_UBLOX_ENABLED

23
libraries/SITL/SIM_GPS_UBLOX.h Normal file
View File

@ -0,0 +1,23 @@
#include "SIM_config.h"
#if AP_SIM_GPS_UBLOX_ENABLED
#include "SIM_GPS.h"
namespace SITL {
class GPS_UBlox : public GPS_Backend {
public:
CLASS_NO_COPY(GPS_UBlox);
using GPS_Backend::GPS_Backend;
void publish(const GPS_Data *d) override;
private:
void send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size);
};
};
#endif // AP_SIM_GPS_UBLOX_ENABLED

41
libraries/SITL/SIM_config.h
View File

@ -37,3 +37,44 @@
#ifndef AP_SIM_SERIALDEVICE_CORRUPTION_ENABLED #ifndef AP_SIM_SERIALDEVICE_CORRUPTION_ENABLED
#define AP_SIM_SERIALDEVICE_CORRUPTION_ENABLED 0 #define AP_SIM_SERIALDEVICE_CORRUPTION_ENABLED 0
#endif #endif
#ifndef HAL_SIM_GPS_ENABLED
#define HAL_SIM_GPS_ENABLED AP_SIM_ENABLED
#endif
#ifndef AP_SIM_GPS_BACKEND_DEFAULT_ENABLED
#define AP_SIM_GPS_BACKEND_DEFAULT_ENABLED AP_SIM_ENABLED
#endif
#ifndef AP_SIM_GPS_FILE_ENABLED
// really need to use AP_FileSystem for this.
#define AP_SIM_GPS_FILE_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX)
#endif
#ifndef AP_SIM_GPS_GSOF_ENABLED
#define AP_SIM_GPS_GSOF_ENABLED AP_SIM_GPS_BACKEND_DEFAULT_ENABLED
#endif
#ifndef AP_SIM_GPS_MSP_ENABLED
#define AP_SIM_GPS_MSP_ENABLED AP_SIM_GPS_BACKEND_DEFAULT_ENABLED
#endif
#ifndef AP_SIM_GPS_NMEA_ENABLED
#define AP_SIM_GPS_NMEA_ENABLED AP_SIM_GPS_BACKEND_DEFAULT_ENABLED
#endif
#ifndef AP_SIM_GPS_NOVA_ENABLED
#define AP_SIM_GPS_NOVA_ENABLED AP_SIM_GPS_BACKEND_DEFAULT_ENABLED
#endif
#ifndef AP_SIM_GPS_SBP2_ENABLED
#define AP_SIM_GPS_SBP2_ENABLED AP_SIM_GPS_BACKEND_DEFAULT_ENABLED
#endif
#ifndef AP_SIM_GPS_SBP_ENABLED
#define AP_SIM_GPS_SBP_ENABLED AP_SIM_GPS_BACKEND_DEFAULT_ENABLED
#endif
#ifndef AP_SIM_GPS_UBLOX_ENABLED
#define AP_SIM_GPS_UBLOX_ENABLED AP_SIM_GPS_BACKEND_DEFAULT_ENABLED
#endif