From da73b419abd1d42ccfe0b61c77394926edd8722e Mon Sep 17 00:00:00 2001 From: Tatsuya Yamaguchi Date: Fri, 3 Jun 2022 22:30:50 +0900 Subject: [PATCH] Copter: fix compilation when GUIDED, AUTO and CIRCLE modes are disabled --- ArduCopter/Copter.cpp | 6 ++++++ ArduCopter/Copter.h | 6 ++++++ ArduCopter/GCS_Mavlink.cpp | 6 ++++++ 3 files changed, 18 insertions(+) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 94ae6a0c33..63252e3ca4 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -268,6 +268,7 @@ void Copter::get_scheduler_tasks(const AP_Scheduler::Task *&tasks, constexpr int8_t Copter::_failsafe_priorities[7]; #if AP_SCRIPTING_ENABLED +#if MODE_GUIDED_ENABLED == ENABLED // start takeoff to given altitude (for use by scripting) bool Copter::start_takeoff(float alt) { @@ -378,7 +379,9 @@ bool Copter::set_target_angle_and_climbrate(float roll_deg, float pitch_deg, flo mode_guided.set_angle(q, Vector3f{}, climb_rate_ms*100, false); return true; } +#endif +#if MODE_CIRCLE_ENABLED == ENABLED // circle mode controls bool Copter::get_circle_radius(float &radius_m) { @@ -391,6 +394,7 @@ bool Copter::set_circle_rate(float rate_dps) circle_nav->set_rate(rate_dps); return true; } +#endif // set desired speed (m/s). Used for scripting. bool Copter::set_desired_speed(float speed) @@ -404,6 +408,7 @@ bool Copter::set_desired_speed(float speed) return true; } +#if MODE_AUTO_ENABLED == ENABLED // returns true if mode supports NAV_SCRIPT_TIME mission commands bool Copter::nav_scripting_enable(uint8_t mode) { @@ -429,6 +434,7 @@ void Copter::nav_script_time_done(uint16_t id) return mode_auto.nav_script_time_done(id); } +#endif // returns true if the EKF failsafe has triggered. Only used by Lua scripts bool Copter::has_ekf_failsafed() const diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index fb665fdf65..51f114ad3c 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -651,6 +651,7 @@ private: uint8_t &task_count, uint32_t &log_bit) override; #if AP_SCRIPTING_ENABLED +#if MODE_GUIDED_ENABLED == ENABLED bool start_takeoff(float alt) override; bool set_target_location(const Location& target_loc) override; bool set_target_pos_NED(const Vector3f& target_pos, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative, bool terrain_alt) override; @@ -659,12 +660,17 @@ private: bool set_target_velocity_NED(const Vector3f& vel_ned) override; bool set_target_velaccel_NED(const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool relative_yaw) override; bool set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs) override; +#endif +#if MODE_CIRCLE_ENABLED == ENABLED bool get_circle_radius(float &radius_m) override; bool set_circle_rate(float rate_dps) override; +#endif bool set_desired_speed(float speed) override; +#if MODE_AUTO_ENABLED == ENABLED bool nav_scripting_enable(uint8_t mode) override; bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4) override; void nav_script_time_done(uint16_t id) override; +#endif // lua scripts use this to retrieve EKF failsafe state // returns true if the EKF failsafe has triggered bool has_ekf_failsafed() const override; diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 91d47aa481..c88d528986 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -672,6 +672,7 @@ bool GCS_MAVLINK_Copter::set_home(const Location& loc, bool _lock) { MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_do_reposition(const mavlink_command_int_t &packet) { +#if MODE_GUIDED_ENABLED == ENABLED const bool change_modes = ((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) == MAV_DO_REPOSITION_FLAGS_CHANGE_MODE; if (!copter.flightmode->in_guided_mode() && !change_modes) { return MAV_RESULT_DENIED; @@ -708,6 +709,9 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_do_reposition(const mavlink_co } return MAV_RESULT_ACCEPTED; +#else + return MAV_RESULT_UNSUPPORTED; +#endif } MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_int_t &packet) @@ -1073,6 +1077,7 @@ void GCS_MAVLINK_Copter::handle_manual_control_axes(const mavlink_manual_control void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) { +#if MODE_GUIDED_ENABLED == ENABLED // for mavlink SET_POSITION_TARGET messages constexpr uint32_t MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE = POSITION_TARGET_TYPEMASK_X_IGNORE | @@ -1095,6 +1100,7 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; constexpr uint32_t MAVLINK_SET_POS_TYPE_MASK_FORCE_SET = POSITION_TARGET_TYPEMASK_FORCE_SET; +#endif switch (msg.msgid) {