diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index fb4ce1a699..77d305f432 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -177,6 +177,7 @@ public: virtual void send_scaled_pressure3(); // allow sub to override this void send_scaled_pressure(); void send_sensor_offsets(); + virtual void send_simstate() const; void send_ahrs(); void send_battery2(); #if AP_AHRS_NAVEKF_AVAILABLE diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 967ac4a747..aac1ec9766 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -36,6 +36,10 @@ #include #endif +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#include +#endif + extern const AP_HAL::HAL& hal; uint32_t GCS_MAVLINK::last_radio_status_remrssi_ms; @@ -2352,6 +2356,17 @@ void GCS_MAVLINK::send_banner() } +void GCS_MAVLINK::send_simstate() const +{ +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + SITL::SITL *sitl = AP::sitl(); + if (sitl == nullptr) { + return; + } + sitl->simstate_send(get_chan()); +#endif +} + MAV_RESULT GCS_MAVLINK::handle_command_preflight_set_sensor_offsets(const mavlink_command_long_t &packet) { Compass *compass = get_compass(); @@ -2873,6 +2888,13 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) send_servo_output_raw(); break; + case MSG_SIMSTATE: + CHECK_PAYLOAD_SIZE(SIMSTATE); + send_simstate(); + CHECK_PAYLOAD_SIZE(AHRS2); + send_ahrs2(); + break; + case MSG_AHRS: CHECK_PAYLOAD_SIZE(AHRS); send_ahrs();