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

AC_Avoid: eliminate get_position and get_alt_above_home

Browse Source
This commit is contained in:
Peter Barker 2017-12-07 15:03:36 +11:00 committed by Randy Mackay
parent 54010451bf
commit 0fe45ef6b6
2 changed files with 11 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
libraries/AC_Avoidance/AC_Avoid.cpp
View File

@ -102,8 +102,10 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c
// calculate distance below fence // calculate distance below fence
if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0 && (_fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) > 0) { if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0 && (_fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) > 0) {
// calculate distance from vehicle to safe altitude // calculate distance from vehicle to safe altitude
float veh_alt = get_alt_above_home(); float veh_alt;
alt_diff_cm = _fence.get_safe_alt_max() * 100.0f - veh_alt; _ahrs.get_relative_position_D_home(veh_alt);
// _fence.get_safe_alt_max() is UP, veh_alt is DOWN:
alt_diff_cm = (_fence.get_safe_alt_max() + veh_alt) * 100.0f;
limit_alt = true; limit_alt = true;
} }
@ -208,8 +210,13 @@ void AC_Avoid::adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f
return; return;
} }
// get position as a 2D offset in cm from ahrs home // get position as a 2D offset from ahrs home
const Vector2f position_xy = get_position(); Vector2f position_xy;
if (!_ahrs.get_relative_position_NE_home(position_xy)) {
// we have no idea where we are....
return;
}
position_xy *= 100.0f; // m -> cm
float speed = desired_vel.length(); float speed = desired_vel.length();
// get the fence radius in cm // get the fence radius in cm
@ -398,26 +405,6 @@ void AC_Avoid::limit_velocity(float kP, float accel_cmss, Vector2f &desired_vel,
} }
} }
/*
* Gets the current xy-position, relative to home (not relative to EKF origin)
*/
Vector2f AC_Avoid::get_position() const
{
Vector2f position_xy;
_ahrs.get_relative_position_NE_home(position_xy);
return position_xy * 100.0f;
}
/*
* Gets the altitude above home in cm
*/
float AC_Avoid::get_alt_above_home() const
{
float alt;
_ahrs.get_relative_position_D_home(alt);
return (alt * -100.0f);
}
/* /*
* Computes the speed such that the stopping distance * Computes the speed such that the stopping distance
* of the vehicle will be exactly the input distance. * of the vehicle will be exactly the input distance.

6
libraries/AC_Avoidance/AC_Avoid.h
View File

@ -102,12 +102,6 @@ private:
*/ */
void limit_velocity(float kP, float accel_cmss, Vector2f &desired_vel, const Vector2f& limit_direction, float limit_distance) const; void limit_velocity(float kP, float accel_cmss, Vector2f &desired_vel, const Vector2f& limit_direction, float limit_distance) const;
/*
* 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 * Computes the speed such that the stopping distance
* of the vehicle will be exactly the input distance. * of the vehicle will be exactly the input distance.