diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index da0b41589d..0c02da247f 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -81,6 +81,11 @@ bool Simulator::getBaroSample(uint8_t *buf, int len) return _baro.copyData(buf, len); } +bool Simulator::getGPSSample(uint8_t *buf, int len) +{ + return _gps.copyData(buf, len); +} + void Simulator::write_MPU_data(void *buf) { _mpu.writeData(buf); } @@ -97,6 +102,10 @@ void Simulator::write_baro_data(void *buf) { _baro.writeData(buf); } +void Simulator::write_gps_data(void *buf) { + _gps.writeData(buf); +} + int Simulator::start(int argc, char *argv[]) { int ret = 0; diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 109c9f301c..39491d8c0d 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -98,6 +98,23 @@ struct RawBaroData { }; #pragma pack(pop) +#pragma pack(push, 1) +struct RawGPSData { + int32_t lat; + int32_t lon; + int32_t alt; + uint16_t eph; + uint16_t epv; + uint16_t vel; + int16_t vn; + int16_t ve; + int16_t vd; + uint16_t cog; + uint8_t fix_type; + uint8_t satellites_visible; +}; +#pragma pack(pop) + template class Report { public: Report(int readers) : @@ -177,11 +194,13 @@ public: bool getMagReport(uint8_t *buf, int len); bool getMPUReport(uint8_t *buf, int len); bool getBaroSample(uint8_t *buf, int len); + bool getGPSSample(uint8_t *buf, int len); void write_MPU_data(void *buf); void write_accel_data(void *buf); void write_mag_data(void *buf); void write_baro_data(void *buf); + void write_gps_data(void *buf); private: Simulator() : @@ -189,6 +208,7 @@ private: _mpu(1), _baro(1), _mag(1), + _gps(1), _sensor_combined_pub(nullptr) #ifndef __PX4_QURT , @@ -216,6 +236,7 @@ private: simulator::Report _mpu; simulator::Report _baro; simulator::Report _mag; + simulator::Report _gps; // uORB publisher handlers orb_advert_t _accel_pub; @@ -258,6 +279,7 @@ private: void pack_actuator_message(mavlink_hil_controls_t &actuator_msg); void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID); void update_sensors(struct sensor_combined_s *sensor, mavlink_hil_sensor_t *imu); + void update_gps(mavlink_hil_gps_t *gps_sim); static void *sending_trampoline(void *); void send(); #endif diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 9bcac4879c..befb249f4f 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -188,6 +188,25 @@ void Simulator::update_sensors(struct sensor_combined_s *sensor, mavlink_hil_sen write_baro_data((void *)&baro); } +void Simulator::update_gps(mavlink_hil_gps_t *gps_sim) { + RawGPSData gps; + gps.lat = gps_sim->lat; + gps.lon = gps_sim->lon; + gps.alt = gps_sim->alt; + gps.eph = gps_sim->eph; + gps.epv = gps_sim->epv; + gps.vel = gps_sim->vel; + gps.vn = gps_sim->vn; + gps.ve = gps_sim->ve; + gps.vd = gps_sim->vd; + gps.cog = gps_sim->cog; + gps.fix_type = gps_sim->fix_type; + gps.satellites_visible = gps_sim->satellites_visible; + + write_gps_data((void *)&gps); + +} + void Simulator::handle_message(mavlink_message_t *msg) { switch(msg->msgid) { case MAVLINK_MSG_ID_HIL_SENSOR: @@ -196,6 +215,12 @@ void Simulator::handle_message(mavlink_message_t *msg) { update_sensors(&_sensor, &imu); break; + case MAVLINK_MSG_ID_HIL_GPS: + mavlink_hil_gps_t gps_sim; + mavlink_msg_hil_gps_decode(msg, &gps_sim); + update_gps(&gps_sim); + break; + case MAVLINK_MSG_ID_RC_CHANNELS: mavlink_rc_channels_t rc_channels;