Skip to content

Commit

Permalink
SITL: Bump up read rate on SITL
Browse files Browse the repository at this point in the history 
* 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 <[email protected]>
  • Loading branch information
@Ryanf55 @peterbarker
Ryanf55 authored and peterbarker committed Nov 21, 2023
1 parent 69bfe9b commit da0efa3
Show file tree
Hide file tree
Showing 2 changed files with 24 additions and 33 deletions.
27 changes: 11 additions & 16 deletions libraries/SITL/SIM_GPS.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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}
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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
{
Expand Down Expand Up @@ -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 };
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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];
Expand Down Expand Up @@ -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;
}

Expand Down Expand Up @@ -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;
Expand Down
30 changes: 13 additions & 17 deletions libraries/SITL/SIM_GPS.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,28 +69,24 @@ class GPS_Backend {
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;
GPS &front;

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 {
Expand All @@ -99,7 +95,7 @@ class GPS_FILE : public GPS_Backend {

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 {
Expand All @@ -108,7 +104,7 @@ class GPS_GSOF : public GPS_Backend {

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);
Expand All @@ -125,7 +121,7 @@ class GPS_NMEA : public GPS_Backend {

using GPS_Backend::GPS_Backend;

void update_write(const GPS_Data *d) override;
void publish(const GPS_Data *d) override;

private:

Expand All @@ -141,7 +137,7 @@ class GPS_NOVA : public GPS_Backend {

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; }

Expand All @@ -158,7 +154,7 @@ class GPS_MSP : public GPS_Backend {

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 {
Expand All @@ -179,7 +175,7 @@ class GPS_SBP : public GPS_SBP_Common {

using GPS_SBP_Common::GPS_SBP_Common;

void update_write(const GPS_Data *d) override;
void publish(const GPS_Data *d) override;

};

Expand All @@ -189,7 +185,7 @@ class GPS_SBP2 : public GPS_SBP_Common {

using GPS_SBP_Common::GPS_SBP_Common;

void update_write(const GPS_Data *d) override;
void publish(const GPS_Data *d) override;

};

Expand All @@ -199,7 +195,7 @@ class GPS_UBlox : public GPS_Backend {

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);
Expand Down

0 comments on commit da0efa3

Please sign in to comment.