From fc6e1d8fe8598ceb34f3da3a2f5b3d3afeea514f Mon Sep 17 00:00:00 2001 From: rishabsingh3003 Date: Wed, 4 Mar 2026 14:21:49 +0900 Subject: [PATCH] AC_Avoid: disable AltHold based avoidace by default Co-authored-by: Randy Mackay --- libraries/AC_Avoidance/AC_Avoid.cpp | 10 +++++++++- libraries/AC_Avoidance/AC_Avoid.h | 6 ++++++ libraries/AC_Avoidance/AC_Avoidance_config.h | 6 ++++-- 3 files changed, 19 insertions(+), 3 deletions(-) diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index 60d5a226fc0..4557cb2a632 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -112,14 +112,16 @@ const AP_Param::GroupInfo AC_Avoid::var_info[] = { // @User: Standard AP_GROUPINFO("BACKZ_SPD", 10, AC_Avoid, _backup_speed_max_u_ms, 0.75), +#if AP_AVOIDANCE_ALTHOLD_ENABLED // @Param{Copter}: ANG_MAX // @DisplayName: Avoidance max lean angle in non-GPS flight modes - // @Description: Max lean angle used to avoid obstacles while in non-GPS modes + // @Description: Max lean angle used to avoid obstacles while in non-GPS modes. Set to zero to disable lean-based avoidance // @Units: deg // @Increment: 0.1 // @Range: 0 45 // @User: Standard AP_GROUPINFO_FRAME("ANG_MAX", 11, AC_Avoid, _angle_max_deg, 10.0, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_TRICOPTER), +#endif AP_GROUPEND }; @@ -135,6 +137,7 @@ AC_Avoid::AC_Avoid() // convert parameters void AC_Avoid::convert_params() { +#if AP_AVOIDANCE_ALTHOLD_ENABLED // PARAMETER_CONVERSION - Added: Feb 2026 ahead of ardupilot-4.7 // exit immediately if ANG_MAX has already been configured @@ -147,6 +150,7 @@ void AC_Avoid::convert_params() { 95, 2, AP_PARAM_INT16, "AVOID_ANG_MAX" }, // AVOID_ANGLE_MAX moved to AVOID_ANG_MAX }; AP_Param::convert_old_parameters_scaled(conversion_info, ARRAY_SIZE(conversion_info), 0.01, 0); +#endif } /* @@ -512,6 +516,7 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c #endif } +#if AP_AVOIDANCE_ALTHOLD_ENABLED // adjust roll-pitch to push vehicle away from objects // roll and pitch value are in radians // veh_angle_max_rad is the user defined maximum lean angle for the vehicle in radians @@ -560,6 +565,7 @@ void AC_Avoid::adjust_roll_pitch_rad(float &roll_rad, float &pitch_rad, float ve roll_rad = rp_out_rad.x; pitch_rad = rp_out_rad.y; } +#endif // AP_AVOIDANCE_ALTHOLD_ENABLED /* * Note: This method is used to limit velocity horizontally only @@ -1501,6 +1507,7 @@ float AC_Avoid::get_stopping_distance(float kP, float accel_cmss, float speed_cm } } +#if AP_AVOIDANCE_ALTHOLD_ENABLED // convert distance (in meters) to a lean percentage (in 0~1 range) for use in manual flight modes float AC_Avoid::distance_m_to_lean_norm(float dist_m) const { @@ -1554,6 +1561,7 @@ void AC_Avoid::get_proximity_roll_pitch_norm(float &roll_positive_norm, float &r } #endif // HAL_PROXIMITY_ENABLED } +#endif // AP_AVOIDANCE_ALTHOLD_ENABLED // singleton instance AC_Avoid *AC_Avoid::_singleton; diff --git a/libraries/AC_Avoidance/AC_Avoid.h b/libraries/AC_Avoidance/AC_Avoid.h index b9aa2d0bdcc..079c5921719 100644 --- a/libraries/AC_Avoidance/AC_Avoid.h +++ b/libraries/AC_Avoidance/AC_Avoid.h @@ -79,10 +79,12 @@ public: void adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms, float& backup_speed_cms, float dt); void adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms, float dt); +#if AP_AVOIDANCE_ALTHOLD_ENABLED // adjust roll-pitch to push vehicle away from objects // roll and pitch value are in radians // veh_angle_max_rad is the user defined maximum lean angle for the vehicle in radians void adjust_roll_pitch_rad(float &roll_rad, float &pitch_rad, float veh_angle_max_rad) const; +#endif // enable/disable proximity based avoidance void proximity_avoidance_enable(bool on_off) { _proximity_enabled = on_off; } @@ -199,18 +201,22 @@ private: * methods for avoidance in non-GPS flight modes */ +#if AP_AVOIDANCE_ALTHOLD_ENABLED // convert distance (in meters) to a lean percentage (in 0~1 range) for use in manual flight modes float distance_m_to_lean_norm(float dist_m) const; // returns the maximum positive and negative roll and pitch percentages (in -1 ~ +1 range) based on the proximity sensor void get_proximity_roll_pitch_norm(float &roll_positive, float &roll_negative, float &pitch_positive, float &pitch_negative) const; +#endif // Logging function void Write_SimpleAvoidance(const uint8_t state, const Vector3f& desired_vel, const Vector3f& modified_vel, const bool back_up) const; // parameters AP_Int8 _enabled; +#if AP_AVOIDANCE_ALTHOLD_ENABLED AP_Float _angle_max_deg; // maximum lean angle in degrees to avoid obstacles (only used in non-GPS flight modes) +#endif AP_Float _dist_max_m; // distance (in meters) from object at which obstacle avoidance will begin in non-GPS modes AP_Float _margin_m; // vehicle will attempt to stay this distance (in meters) from objects while in GPS modes AP_Int8 _behavior; // avoidance behaviour (slide or stop) diff --git a/libraries/AC_Avoidance/AC_Avoidance_config.h b/libraries/AC_Avoidance/AC_Avoidance_config.h index 602d06829ca..c36c254919a 100644 --- a/libraries/AC_Avoidance/AC_Avoidance_config.h +++ b/libraries/AC_Avoidance/AC_Avoidance_config.h @@ -23,8 +23,10 @@ #define AP_OAPATHPLANNER_DIJKSTRA_ENABLED AP_OAPATHPLANNER_BACKEND_DEFAULT_ENABLED #endif - - #ifndef AP_OADATABASE_ENABLED #define AP_OADATABASE_ENABLED AP_OAPATHPLANNER_ENABLED #endif + +#ifndef AP_AVOIDANCE_ALTHOLD_ENABLED +#define AP_AVOIDANCE_ALTHOLD_ENABLED 0 +#endif