Browse Source

Sub: move sending of simstate up

mission-4.1.18
Peter Barker 7 years ago committed by Peter Barker
parent
commit
420b9869f7
  1. 17
      ArduSub/GCS_Mavlink.cpp
  2. 1
      ArduSub/Sub.h

17
ArduSub/GCS_Mavlink.cpp

@ -251,14 +251,6 @@ void NOINLINE Sub::send_nav_controller_output(mavlink_channel_t chan)
0); 0);
} }
// report simulator state
void NOINLINE Sub::send_simstate(mavlink_channel_t chan)
{
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
sitl.simstate_send(chan);
#endif
}
void NOINLINE Sub::send_vfr_hud(mavlink_channel_t chan) void NOINLINE Sub::send_vfr_hud(mavlink_channel_t chan)
{ {
mavlink_msg_vfr_hud_send( mavlink_msg_vfr_hud_send(
@ -461,15 +453,6 @@ bool GCS_MAVLINK_Sub::try_send_message(enum ap_message id)
#endif #endif
break; break;
case MSG_SIMSTATE:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
CHECK_PAYLOAD_SIZE(SIMSTATE);
sub.send_simstate(chan);
#endif
CHECK_PAYLOAD_SIZE(AHRS2);
send_ahrs2();
break;
case MSG_MOUNT_STATUS: case MSG_MOUNT_STATUS:
#if MOUNT == ENABLED #if MOUNT == ENABLED
CHECK_PAYLOAD_SIZE(MOUNT_STATUS); CHECK_PAYLOAD_SIZE(MOUNT_STATUS);

1
ArduSub/Sub.h

@ -476,7 +476,6 @@ private:
void send_heartbeat(mavlink_channel_t chan); void send_heartbeat(mavlink_channel_t chan);
void send_extended_status1(mavlink_channel_t chan); void send_extended_status1(mavlink_channel_t chan);
void send_nav_controller_output(mavlink_channel_t chan); void send_nav_controller_output(mavlink_channel_t chan);
void send_simstate(mavlink_channel_t chan);
void send_vfr_hud(mavlink_channel_t chan); void send_vfr_hud(mavlink_channel_t chan);
#if RPM_ENABLED == ENABLED #if RPM_ENABLED == ENABLED
void send_rpm(mavlink_channel_t chan); void send_rpm(mavlink_channel_t chan);

Loading…
Cancel
Save