diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index f211d28dfa..1d32d412a0 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -242,6 +242,7 @@ public: virtual void send_scaled_pressure3(); // allow sub to override this void send_sensor_offsets(); virtual void send_simstate() const; + void send_sim_state() const; void send_ahrs(); void send_battery2(); void send_opticalflow(); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 9fb91362f7..3bd2b8f858 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -791,6 +791,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c { MAVLINK_MSG_ID_FENCE_STATUS, MSG_FENCE_STATUS}, { MAVLINK_MSG_ID_AHRS, MSG_AHRS}, { MAVLINK_MSG_ID_SIMSTATE, MSG_SIMSTATE}, + { MAVLINK_MSG_ID_SIM_STATE, MSG_SIM_STATE}, { MAVLINK_MSG_ID_AHRS2, MSG_AHRS2}, { MAVLINK_MSG_ID_HWSTATUS, MSG_HWSTATUS}, { MAVLINK_MSG_ID_WIND, MSG_WIND}, @@ -3524,6 +3525,17 @@ void GCS_MAVLINK::send_simstate() const #endif } +void GCS_MAVLINK::send_sim_state() const +{ +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + SITL::SITL *sitl = AP::sitl(); + if (sitl == nullptr) { + return; + } + sitl->sim_state_send(get_chan()); +#endif +} + MAV_RESULT GCS_MAVLINK::handle_command_flash_bootloader(const mavlink_command_long_t &packet) { if (uint32_t(packet.param5) != 290876) { @@ -4721,6 +4733,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) send_simstate(); break; + case MSG_SIM_STATE: + CHECK_PAYLOAD_SIZE(SIM_STATE); + send_sim_state(); + break; + case MSG_SYS_STATUS: CHECK_PAYLOAD_SIZE(SYS_STATUS); send_sys_status(); diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h index 8172d8e6f4..4af63d91de 100644 --- a/libraries/GCS_MAVLink/ap_message.h +++ b/libraries/GCS_MAVLink/ap_message.h @@ -40,6 +40,7 @@ enum ap_message : uint8_t { MSG_FENCE_STATUS, MSG_AHRS, MSG_SIMSTATE, + MSG_SIM_STATE, MSG_AHRS2, MSG_HWSTATUS, MSG_WIND,