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

SITL: Bump up read rate on SITL 

* This is needed to do active configuration quickly
* Read/Write split and exposed to ensure physics/write rate is still
  coupled to avoid impacting the jamming and delayed data
* Created a utility to allocate the SITL instance

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
This commit is contained in:
Ryan Friedman 2023-11-16 00:04:09 -07:00 committed by Peter Barker
parent 69bfe9b837
commit da0efa3323
2 changed files with 24 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/SITL/SIM_GPS.cpp
View File

@ -52,12 +52,6 @@ ssize_t GPS_Backend::read_from_autopilot(char *buffer, size_t size) const
return front.read_from_autopilot(buffer, size);
}
void GPS_Backend::update(const GPS_Data &d)
{
update_read(&d);
update_write(&d);
}
GPS::GPS(uint8_t _instance) :
SerialDevice(8192, 2048),
instance{_instance}
@ -183,7 +177,7 @@ static GPS_TOW gps_time()
/*
send a new set of GPS UBLOX packets
*/
void GPS_UBlox::update_write(const GPS_Data *d)
void GPS_UBlox::publish(const GPS_Data *d)
{
struct PACKED ubx_nav_posllh {
uint32_t time; // GPS msToW
@ -486,7 +480,7 @@ void GPS_NMEA::nmea_printf(const char *fmt, ...)
/*
send a new GPS NMEA packet
*/
void GPS_NMEA::update_write(const GPS_Data *d)
void GPS_NMEA::publish(const GPS_Data *d)
{
struct timeval tv;
struct tm *tm;
@ -599,7 +593,7 @@ void GPS_SBP_Common::sbp_send_message(uint16_t msg_type, uint16_t sender_id, uin
write_to_autopilot((char*)&crc, 2);
}
void GPS_SBP::update_write(const GPS_Data *d)
void GPS_SBP::publish(const GPS_Data *d)
{
struct sbp_heartbeat_t {
bool sys_error : 1;
@ -713,7 +707,7 @@ void GPS_SBP::update_write(const GPS_Data *d)
}
void GPS_SBP2::update_write(const GPS_Data *d)
void GPS_SBP2::publish(const GPS_Data *d)
{
struct sbp_heartbeat_t {
bool sys_error : 1;
@ -825,7 +819,7 @@ void GPS_SBP2::update_write(const GPS_Data *d)
do_every_count++;
}
void GPS_NOVA::update_write(const GPS_Data *d)
void GPS_NOVA::publish(const GPS_Data *d)
{
static struct PACKED nova_header
{
@ -991,7 +985,7 @@ uint32_t GPS_NOVA::CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_
return( crc );
}
void GPS_GSOF::update_write(const GPS_Data *d)
void GPS_GSOF::publish(const 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 };
@ -1286,7 +1280,7 @@ uint32_t GPS_GSOF::gsof_pack_float(const float& src)
/*
send MSP GPS data
*/
void GPS_MSP::update_write(const GPS_Data *d)
void GPS_MSP::publish(const GPS_Data *d)
{
struct PACKED {
// header
@ -1364,7 +1358,7 @@ void GPS_MSP::update_write(const GPS_Data *d)
read file data logged from AP_GPS_DEBUG_LOGGING_ENABLED
*/
#if AP_SIM_GPS_FILE_ENABLED
void GPS_FILE::update_write(const GPS_Data *d)
void GPS_FILE::publish(const GPS_Data *d)
{
static int fd[2] = {-1,-1};
static uint32_t base_time[2];
@ -1524,6 +1518,7 @@ void GPS::update()
// run at configured GPS rate (default 5Hz)
if ((now_ms - last_update) < (uint32_t)(1000/_sitl->gps_hertz[idx])) {
backend->update_read();
return;
}
@ -1593,10 +1588,10 @@ void GPS::update()
d.longitude += glitch_offsets.y;
d.altitude += glitch_offsets.z;
backend->update(d); // i.e. reading configuration etc from autopilot
backend->publish(&d);
}
void GPS_Backend::update_read(const GPS_Data *d)
void GPS_Backend::update_read()
{
// swallow any config bytes
char c;

30
libraries/SITL/SIM_GPS.h
View File

@ -69,14 +69,17 @@ public:
GPS_Backend(class GPS &front, uint8_t _instance);
virtual ~GPS_Backend() {}
void update(const GPS_Data &d);
// 0 baud means "unset" i.e. baud-rate checks should not apply
virtual uint32_t device_baud() const { return 0; }
ssize_t write_to_autopilot(const char *p, size_t size) const;
ssize_t read_from_autopilot(char *buffer, size_t size) const;
// read and process config from autopilot (e.g.)
virtual void update_read();
// writing fix information to autopilot (e.g.)
virtual void publish(const GPS_Data *d) = 0;
protected:
uint8_t instance;
@ -84,13 +87,6 @@ protected:
class SIM *_sitl;
private:
// read and process config from autopilot (e.g.)
virtual void update_read(const GPS_Data *d);
// writing fix information to autopilot (e.g.)
virtual void update_write(const GPS_Data *d) = 0;
};
class GPS_FILE : public GPS_Backend {
@ -99,7 +95,7 @@ public:
using GPS_Backend::GPS_Backend;
void update_write(const GPS_Data *d) override;
void publish(const GPS_Data *d) override;
};
class GPS_GSOF : public GPS_Backend {
@ -108,7 +104,7 @@ public:
using GPS_Backend::GPS_Backend;
void update_write(const GPS_Data *d) override;
void publish(const GPS_Data *d) override;
private:
void send_gsof(const uint8_t *buf, const uint16_t size);
@ -125,7 +121,7 @@ public:
using GPS_Backend::GPS_Backend;
void update_write(const GPS_Data *d) override;
void publish(const GPS_Data *d) override;
private:
@ -141,7 +137,7 @@ public:
using GPS_Backend::GPS_Backend;
void update_write(const GPS_Data *d) override;
void publish(const GPS_Data *d) override;
uint32_t device_baud() const override { return 19200; }
@ -158,7 +154,7 @@ public:
using GPS_Backend::GPS_Backend;
void update_write(const GPS_Data *d) override;
void publish(const GPS_Data *d) override;
};
class GPS_SBP_Common : public GPS_Backend {
@ -179,7 +175,7 @@ public:
using GPS_SBP_Common::GPS_SBP_Common;
void update_write(const GPS_Data *d) override;
void publish(const GPS_Data *d) override;
};
@ -189,7 +185,7 @@ public:
using GPS_SBP_Common::GPS_SBP_Common;
void update_write(const GPS_Data *d) override;
void publish(const GPS_Data *d) override;
};
@ -199,7 +195,7 @@ public:
using GPS_Backend::GPS_Backend;
void update_write(const GPS_Data *d) override;
void publish(const GPS_Data *d) override;
private:
void send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size);