diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index 75f0aeb0fc..b4484648e2 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -76,12 +76,12 @@ const AP_Param::GroupInfo AC_Avoid::var_info[] = { AP_GROUPINFO_FRAME("BEHAVE", 5, AC_Avoid, _behavior, AP_AVOID_BEHAVE_DEFAULT, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_TRICOPTER | AP_PARAM_FRAME_ROVER), // @Param: BACKUP_SPD - // @DisplayName: Avoidance maximum backup speed - // @Description: Maximum speed that will be used to back away from obstacles in GPS modes (m/s). Set zero to disable + // @DisplayName: Avoidance maximum horizontal backup speed + // @Description: Maximum speed that will be used to back away from obstacles horizontally in position control modes (m/s). Set zero to disable horizontal backup. // @Units: m/s // @Range: 0 2 // @User: Standard - AP_GROUPINFO("BACKUP_SPD", 6, AC_Avoid, _backup_speed_max, 0.75f), + AP_GROUPINFO("BACKUP_SPD", 6, AC_Avoid, _backup_speed_xy_max, 0.75f), // @Param{Copter}: ALT_MIN // @DisplayName: Avoidance minimum altitude @@ -107,6 +107,14 @@ const AP_Param::GroupInfo AC_Avoid::var_info[] = { // @User: Standard AP_GROUPINFO("BACKUP_DZ", 9, AC_Avoid, _backup_deadzone, 0.10f), + // @Param: BACKZ_SPD + // @DisplayName: Avoidance maximum vertical backup speed + // @Description: Maximum speed that will be used to back away from obstacles vertically in height control modes (m/s). Set zero to disable vertical backup. + // @Units: m/s + // @Range: 0 2 + // @User: Standard + AP_GROUPINFO("BACKZ_SPD", 10, AC_Avoid, _backup_speed_z_max, 0.75), + AP_GROUPEND }; @@ -220,14 +228,12 @@ void AC_Avoid::adjust_velocity(Vector3f &desired_vel_cms, bool &backing_up, floa const float desired_backup_vel_z = back_vel_down + back_vel_up; Vector3f desired_backup_vel{desired_backup_vel_xy.x, desired_backup_vel_xy.y, desired_backup_vel_z}; - const float max_back_spd_cms = _backup_speed_max * 100.0f; - if (!desired_backup_vel.is_zero() && is_positive(max_back_spd_cms)) { + const float max_back_spd_xy_cms = _backup_speed_xy_max * 100.0; + if (!desired_backup_vel.xy().is_zero() && is_positive(max_back_spd_xy_cms)) { backing_up = true; - // Constrain backing away speed - if (desired_backup_vel.length() > max_back_spd_cms) { - desired_backup_vel = desired_backup_vel.normalized() * max_back_spd_cms; - } - + // Constrain horizontal backing away speed + desired_backup_vel.xy().limit_length(max_back_spd_xy_cms); + // let user take control if they are backing away at a greater speed than what we have calculated // this has to be done for x,y,z separately. For eg, user is doing fine in "x" direction but might need backing up in "y". if (!is_zero(desired_backup_vel.x)) { @@ -244,6 +250,15 @@ void AC_Avoid::adjust_velocity(Vector3f &desired_vel_cms, bool &backing_up, floa desired_vel_cms.y = MIN(desired_vel_cms.y, desired_backup_vel.y); } } + } + + const float max_back_spd_z_cms = _backup_speed_z_max * 100.0; + if (!is_zero(desired_backup_vel.z) && is_positive(max_back_spd_z_cms)) { + backing_up = true; + + // Constrain vertical backing away speed + desired_backup_vel.z = constrain_float(desired_backup_vel.z, -max_back_spd_z_cms, max_back_spd_z_cms); + if (!is_zero(desired_backup_vel.z)) { if (is_positive(desired_backup_vel.z)) { desired_vel_cms.z = MAX(desired_vel_cms.z, desired_backup_vel.z); @@ -252,6 +267,7 @@ void AC_Avoid::adjust_velocity(Vector3f &desired_vel_cms, bool &backing_up, floa } } } + // limit acceleration limit_accel(desired_vel_cms_original, desired_vel_cms, dt); @@ -418,7 +434,13 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c if (alt_diff <= 0.0f) { climb_rate_cms = MIN(climb_rate_cms, 0.0f); // also calculate backup speed that will get us back to safe altitude - backup_speed = -1*(get_max_speed(kP, accel_cmss_limited, -alt_diff*100.0f, dt)); + const float max_back_spd_cms = _backup_speed_z_max * 100.0; + if (is_positive(max_back_spd_cms)) { + backup_speed = -1*(get_max_speed(kP, accel_cmss_limited, -alt_diff*100.0f, dt)); + + // Constrain to max backup speed + backup_speed = MAX(backup_speed, -max_back_spd_cms); + } return; } diff --git a/libraries/AC_Avoidance/AC_Avoid.h b/libraries/AC_Avoidance/AC_Avoid.h index 6bccfb846d..29dca71752 100644 --- a/libraries/AC_Avoidance/AC_Avoid.h +++ b/libraries/AC_Avoidance/AC_Avoid.h @@ -207,14 +207,15 @@ private: // parameters AP_Int8 _enabled; - AP_Int16 _angle_max; // maximum lean angle to avoid obstacles (only used in non-GPS flight modes) - AP_Float _dist_max; // distance (in meters) from object at which obstacle avoidance will begin in non-GPS modes - AP_Float _margin; // vehicle will attempt to stay this distance (in meters) from objects while in GPS modes - AP_Int8 _behavior; // avoidance behaviour (slide or stop) - AP_Float _backup_speed_max; // Maximum speed that will be used to back away (in m/s) - AP_Float _alt_min; // alt below which Proximity based avoidance is turned off - AP_Float _accel_max; // maximum acceleration while simple avoidance is active - AP_Float _backup_deadzone; // distance beyond AVOID_MARGIN parameter, after which vehicle will backaway from obstacles + AP_Int16 _angle_max; // maximum lean angle to avoid obstacles (only used in non-GPS flight modes) + AP_Float _dist_max; // distance (in meters) from object at which obstacle avoidance will begin in non-GPS modes + AP_Float _margin; // vehicle will attempt to stay this distance (in meters) from objects while in GPS modes + AP_Int8 _behavior; // avoidance behaviour (slide or stop) + AP_Float _backup_speed_xy_max; // Maximum speed that will be used to back away horizontally (in m/s) + AP_Float _backup_speed_z_max; // Maximum speed that will be used to back away verticality (in m/s) + AP_Float _alt_min; // alt below which Proximity based avoidance is turned off + AP_Float _accel_max; // maximum acceleration while simple avoidance is active + AP_Float _backup_deadzone; // distance beyond AVOID_MARGIN parameter, after which vehicle will backaway from obstacles bool _proximity_enabled = true; // true if proximity sensor based avoidance is enabled (used to allow pilot to enable/disable) bool _proximity_alt_enabled = true; // true if proximity sensor based avoidance is enabled based on altitude