From 98e27828518e50d018a3bb64ad3fad2574224c70 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 7 Apr 2023 18:25:34 +1000 Subject: [PATCH] Plane: correct compilation when AP_Rally disabled --- ArduPlane/Parameters.cpp | 2 ++ ArduPlane/Plane.h | 6 ++++++ ArduPlane/commands_logic.cpp | 13 ++++++++++++- ArduPlane/fence.cpp | 2 +- ArduPlane/mode_qrtl.cpp | 5 ++--- 5 files changed, 23 insertions(+), 5 deletions(-) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 83188fe945..83c1924e42 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -943,9 +943,11 @@ const AP_Param::Info Plane::var_info[] = { // @Path: ../libraries/AP_Mission/AP_Mission.cpp GOBJECT(mission, "MIS_", AP_Mission), +#if HAL_RALLY_ENABLED // @Group: RALLY_ // @Path: ../libraries/AP_Rally/AP_Rally.cpp GOBJECT(rally, "RALLY_", AP_Rally), +#endif #if HAL_NAVEKF2_AVAILABLE // @Group: EK2_ diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 8d5a08a189..087e86cd41 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -244,8 +244,14 @@ private: AP_OpticalFlow optflow; #endif +#if HAL_RALLY_ENABLED // Rally Ponints AP_Rally rally; +#endif + + // returns a Location for a rally point or home; if + // HAL_RALLY_ENABLED is false, just home. + Location calc_best_rally_or_home_location(const Location ¤t_loc, float rtl_home_alt_amsl_cm) const; #if OSD_ENABLED || OSD_PARAM_ENABLED AP_OSD osd; diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 299f5e004c..b6d017a53a 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -344,7 +344,7 @@ void Plane::do_RTL(int32_t rtl_altitude_AMSL_cm) auto_state.next_wp_crosstrack = false; auto_state.crosstrack = false; prev_WP_loc = current_loc; - next_WP_loc = rally.calc_best_rally_or_home_location(current_loc, rtl_altitude_AMSL_cm); + next_WP_loc = calc_best_rally_or_home_location(current_loc, rtl_altitude_AMSL_cm); setup_terrain_target_alt(next_WP_loc); set_target_altitude_location(next_WP_loc); @@ -360,6 +360,17 @@ void Plane::do_RTL(int32_t rtl_altitude_AMSL_cm) logger.Write_Mode(control_mode->mode_number(), control_mode_reason); } +Location Plane::calc_best_rally_or_home_location(const Location &_current_loc, float rtl_home_alt_amsl_cm) const +{ +#if HAL_RALLY_ENABLED + return plane.rally.calc_best_rally_or_home_location(_current_loc, rtl_home_alt_amsl_cm); +#else + Location destination = plane.home; + destination.set_alt_cm(rtl_home_alt_amsl_cm, Location::AltFrame::ABSOLUTE); + return destination; +#endif +} + /* start a NAV_TAKEOFF command */ diff --git a/ArduPlane/fence.cpp b/ArduPlane/fence.cpp index 28f4abdca6..640ad65869 100644 --- a/ArduPlane/fence.cpp +++ b/ArduPlane/fence.cpp @@ -75,7 +75,7 @@ void Plane::fence_check() Location loc; if (fence.get_return_rally() != 0 || fence_act == AC_FENCE_ACTION_RTL_AND_LAND) { - loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude_cm()); + loc = calc_best_rally_or_home_location(current_loc, get_RTL_altitude_cm()); } else { //return to fence return point, not a rally point if (fence.get_return_altitude() > 0) { diff --git a/ArduPlane/mode_qrtl.cpp b/ArduPlane/mode_qrtl.cpp index 98bfe3a283..75d82b4822 100644 --- a/ArduPlane/mode_qrtl.cpp +++ b/ArduPlane/mode_qrtl.cpp @@ -17,8 +17,7 @@ bool ModeQRTL::_enter() int32_t RTL_alt_abs_cm = plane.home.alt + quadplane.qrtl_alt*100UL; if (quadplane.motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) { // VTOL motors are active, either in VTOL flight or assisted flight - Location destination = plane.rally.calc_best_rally_or_home_location(plane.current_loc, RTL_alt_abs_cm); - + Location destination = plane.calc_best_rally_or_home_location(plane.current_loc, RTL_alt_abs_cm); const float dist = plane.current_loc.get_distance(destination); const float radius = get_VTOL_return_radius(); @@ -122,7 +121,7 @@ void ModeQRTL::run() plane.prev_WP_loc = plane.current_loc; int32_t RTL_alt_abs_cm = plane.home.alt + quadplane.qrtl_alt*100UL; - Location destination = plane.rally.calc_best_rally_or_home_location(plane.current_loc, RTL_alt_abs_cm); + Location destination = plane.calc_best_rally_or_home_location(plane.current_loc, RTL_alt_abs_cm); const float dist = plane.current_loc.get_distance(destination); const float radius = get_VTOL_return_radius(); if (dist < radius) {