Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_Avoid: add "BACKZ_SPD" vertical backup speed limit parameter

This commit is contained in:
Iampete1 2024-04-30 19:50:41 +01:00 committed by Andrew Tridgell
parent 92ff18da1b
commit 13c5ddc4a3
2 changed files with 42 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
libraries/AC_Avoidance/AC_Avoid.cpp
View File

@ -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,13 +228,11 @@ 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".
@ -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
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;
}

3
libraries/AC_Avoidance/AC_Avoid.h
View File

@ -211,7 +211,8 @@ private:
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 _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