Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-26 18:48:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Avoidance: add adjust_velocity_z

Browse Source 
This allows reducing the climb rate to slow the vehicle's climb so it does not breach the altitude fence
This commit is contained in:
Randy Mackay 2017-01-05 15:21:55 +09:00
parent 12dd6e11fb
commit f73efd5249
2 changed files with 49 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
libraries/AC_Avoidance/AC_Avoid.cpp
View File

@ -67,6 +67,41 @@ void AC_Avoid::adjust_velocity(float kP, float accel_cmss, Vector3f &desired_vel
desired_vel.y = des_vel_xy.y;
}
// adjust vertical climb rate so vehicle does not break the vertical fence
void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms)
{
// exit immediately if disabled
if (_enabled == AC_AVOID_DISABLED) {
return;
}
// do not adjust climb_rate if level or descending
if (climb_rate_cms <= 0.0f) {
return;
}
// limit acceleration
float accel_cmss_limited = MIN(accel_cmss, AC_AVOID_ACCEL_CMSS_MAX);
// stop before breaching fence altitude
if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0) {
// calculate distance from vehicle to safe altitude
float veh_alt = get_alt_above_home();
float alt_diff_cm = _fence.get_safe_alt() * 100.0f - veh_alt;
// do not allow climbing if we've breached the safe altitude
if (alt_diff_cm <= 0.0f) {
climb_rate_cms = MIN(climb_rate_cms, 0.0f);
return;
}
// limit climb rate
const float max_speed = get_max_speed(kP, accel_cmss_limited, alt_diff_cm);
climb_rate_cms = MIN(max_speed, climb_rate_cms);
}
}
// adjust roll-pitch to push vehicle away from objects
// roll and pitch value are in centi-degrees
void AC_Avoid::adjust_roll_pitch(float &roll, float &pitch, float veh_angle_max)
@ -297,6 +332,15 @@ Vector2f AC_Avoid::get_position() const
return position_xy - diff;
}
/*
* Gets the altitude above home in cm
*/
float AC_Avoid::get_alt_above_home() const
{
// vehicle's alt above ekf origin + ekf origin's alt above sea level - home's alt above sea level
return _inav.get_altitude() + _inav.get_origin().alt - _ahrs.get_home().alt;
}
/*
* Computes the speed such that the stopping distance
* of the vehicle will be exactly the input distance.

6
libraries/AC_Avoidance/AC_Avoid.h
View File

@ -39,6 +39,9 @@ public:
void adjust_velocity(float kP, float accel_cmss, Vector2f &desired_vel);
void adjust_velocity(float kP, float accel_cmss, Vector3f &desired_vel);
// adjust vertical climb rate so vehicle does not break the vertical fence
void adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms);
// adjust roll-pitch to push vehicle away from objects
// roll and pitch value are in centi-degrees
// angle_max is the user defined maximum lean angle for the vehicle in centi-degrees
@ -83,9 +86,10 @@ private:
void limit_velocity(float kP, float accel_cmss, Vector2f &desired_vel, const Vector2f& limit_direction, float limit_distance) const;
/*
* Gets the current position, relative to home (not relative to EKF origin)
* Gets the current position or altitude, relative to home (not relative to EKF origin) in cm
*/
Vector2f get_position() const;
float get_alt_above_home() const;
/*
* Computes the speed such that the stopping distance