diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index a81f3d5a38cb7b..310562bb983ab3 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -68,6 +68,8 @@ #include "MissionItemProtocol_Rally.h" #include "MissionItemProtocol_Fence.h" +#include + #include #if HAL_RCINPUT_WITH_AP_RADIO @@ -652,7 +654,11 @@ void GCS_MAVLINK::send_mission_current(const class AP_Mission &mission, uint16_t num_commands -= 1; } +#if AP_VEHICLE_ENABLED const uint8_t mission_mode = AP::vehicle()->current_mode_requires_mission() ? 1 : 0; +#else + const uint8_t mission_mode = 0; +#endif mavlink_msg_mission_current_send( chan, @@ -2587,6 +2593,7 @@ void GCS_MAVLINK::handle_set_mode(const mavlink_message_t &msg) MAV_RESULT GCS_MAVLINK::_set_mode_common(const MAV_MODE _base_mode, const uint32_t _custom_mode) { // only accept custom modes because there is no easy mapping from Mavlink flight modes to AC flight modes +#if AP_VEHICLE_ENABLED if (uint32_t(_base_mode) & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { if (!AP::vehicle()->set_mode(_custom_mode, ModeReason::GCS_COMMAND)) { // often we should be returning DENIED rather than FAILED @@ -2596,6 +2603,7 @@ MAV_RESULT GCS_MAVLINK::_set_mode_common(const MAV_MODE _base_mode, const uint32 } return MAV_RESULT_ACCEPTED; } +#endif if (_base_mode == (MAV_MODE)MAV_MODE_FLAG_DECODE_POSITION_SAFETY) { // set the safety switch position. Must be in a command by itself @@ -3269,7 +3277,11 @@ MAV_RESULT GCS_MAVLINK::handle_preflight_reboot(const mavlink_command_int_t &pac // when packet.param1 == 3 we reboot to hold in bootloader const bool hold_in_bootloader = is_equal(packet.param1, 3.0f); +#if AP_VEHICLE_ENABLED AP::vehicle()->reboot(hold_in_bootloader); // not expected to return +#else + hal.scheduler->reboot(hold_in_bootloader); +#endif return MAV_RESULT_FAILED; } @@ -5613,10 +5625,12 @@ void GCS_MAVLINK::send_autopilot_state_for_gimbal_device() const // get vehicle earth-frame rotation rate targets Vector3f rate_ef_targets; +#if AP_VEHICLE_ENABLED const AP_Vehicle *vehicle = AP::vehicle(); if (vehicle != nullptr) { vehicle->get_rate_ef_targets(rate_ef_targets); } +#endif // get estimator flags uint16_t est_status_flags = 0;