mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-10-06 19:00:27 +08:00
AC_Avoid: disable AltHold based avoidace by default
Co-authored-by: Randy Mackay <rmackay9@yahoo.com>
This commit is contained in:
committed by
Randy Mackay
co-authored by
Randy Mackay
parent
33add14f28
commit
fc6e1d8fe8
@@ -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;
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user