diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index e5cc818a4b..42fd80bd26 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -263,12 +263,12 @@ static mavlink_hil_state_t last_hil_state; #endif // report simulator state -void Plane::send_simstate(mavlink_channel_t chan) +void GCS_MAVLINK_Plane::send_simstate() const { #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - sitl.simstate_send(chan); + GCS_MAVLINK::send_simstate(); #elif HIL_SUPPORT - if (g.hil_mode == 1) { + if (plane.g.hil_mode == 1) { mavlink_msg_simstate_send(chan, last_hil_state.roll, last_hil_state.pitch, @@ -430,13 +430,6 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id) #endif break; - case MSG_SIMSTATE: - CHECK_PAYLOAD_SIZE(SIMSTATE); - plane.send_simstate(chan); - CHECK_PAYLOAD_SIZE2(AHRS2); - send_ahrs2(); - break; - case MSG_TERRAIN: #if AP_TERRAIN_AVAILABLE CHECK_PAYLOAD_SIZE(TERRAIN_REQUEST); diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index 0f8dc95575..c989c315d2 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -37,6 +37,7 @@ protected: virtual bool in_hil_mode() const override; void send_attitude() const override; + void send_simstate() const override; private: diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 9490168e06..cd96179b67 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -774,7 +774,6 @@ private: void send_nav_controller_output(mavlink_channel_t chan); void send_servo_out(mavlink_channel_t chan); void send_vfr_hud(mavlink_channel_t chan); - void send_simstate(mavlink_channel_t chan); void send_wind(mavlink_channel_t chan); void send_pid_info(const mavlink_channel_t chan, const DataFlash_Class::PID_Info *pid_info, const uint8_t axis, const float achieved); void send_pid_tuning(mavlink_channel_t chan);