Plane: move sending of simstate up

This commit is contained in:
Peter Barker 2018-01-29 10:02:58 +11:00 committed by Peter Barker
parent 81e9edd80a
commit ef77bea4eb
3 changed files with 4 additions and 11 deletions

View File

@ -263,12 +263,12 @@ static mavlink_hil_state_t last_hil_state;
#endif #endif
// report simulator state // 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 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
sitl.simstate_send(chan); GCS_MAVLINK::send_simstate();
#elif HIL_SUPPORT #elif HIL_SUPPORT
if (g.hil_mode == 1) { if (plane.g.hil_mode == 1) {
mavlink_msg_simstate_send(chan, mavlink_msg_simstate_send(chan,
last_hil_state.roll, last_hil_state.roll,
last_hil_state.pitch, last_hil_state.pitch,
@ -430,13 +430,6 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id)
#endif #endif
break; break;
case MSG_SIMSTATE:
CHECK_PAYLOAD_SIZE(SIMSTATE);
plane.send_simstate(chan);
CHECK_PAYLOAD_SIZE2(AHRS2);
send_ahrs2();
break;
case MSG_TERRAIN: case MSG_TERRAIN:
#if AP_TERRAIN_AVAILABLE #if AP_TERRAIN_AVAILABLE
CHECK_PAYLOAD_SIZE(TERRAIN_REQUEST); CHECK_PAYLOAD_SIZE(TERRAIN_REQUEST);

View File

@ -37,6 +37,7 @@ protected:
virtual bool in_hil_mode() const override; virtual bool in_hil_mode() const override;
void send_attitude() const override; void send_attitude() const override;
void send_simstate() const override;
private: private:

View File

@ -774,7 +774,6 @@ private:
void send_nav_controller_output(mavlink_channel_t chan); void send_nav_controller_output(mavlink_channel_t chan);
void send_servo_out(mavlink_channel_t chan); void send_servo_out(mavlink_channel_t chan);
void send_vfr_hud(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_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_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); void send_pid_tuning(mavlink_channel_t chan);