Skip to content

Commit

Permalink
GCS_MAVLink: correct compilation when AP_Vehicle disabled
Browse files Browse the repository at this point in the history
  • Loading branch information
peterbarker committed Dec 12, 2023
1 parent 4cf082f commit 5ba6af7
Showing 1 changed file with 14 additions and 0 deletions.
14 changes: 14 additions & 0 deletions libraries/GCS_MAVLink/GCS_Common.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -68,6 +68,8 @@
#include "MissionItemProtocol_Rally.h"
#include "MissionItemProtocol_Fence.h"

#include <AP_Vehicle/AP_Vehicle_config.h>

#include <stdio.h>

#if HAL_RCINPUT_WITH_AP_RADIO
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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;
Expand Down

0 comments on commit 5ba6af7

Please sign in to comment.