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

AC_AttitudeControl: AC_PosControl: Non functional clean up

Browse Source
This commit is contained in:
Leonard Hall 2021-08-24 09:37:02 +09:30 committed by Randy Mackay
parent a1975d7535
commit f9452ec17a
1 changed files with 15 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -1021,21 +1021,6 @@ void AC_PosControl::update_z_controller()
/// Accessors /// Accessors
/// ///
/// get_stopping_point_z_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration
void AC_PosControl::get_stopping_point_z_cm(postype_t &stopping_point) const
{
const float curr_pos_z = _inav.get_position().z;
float curr_vel_z = _inav.get_velocity().z;
// avoid divide by zero by using current position if kP is very low or acceleration is zero
if (!is_positive(_p_pos_z.kP()) || !is_positive(_accel_max_z_cmss)) {
stopping_point = curr_pos_z;
return;
}
stopping_point = curr_pos_z + constrain_float(stopping_distance(curr_vel_z, _p_pos_z.kP(), _accel_max_z_cmss), - POSCONTROL_STOPPING_DIST_DOWN_MAX, POSCONTROL_STOPPING_DIST_UP_MAX);
}
/// get_lean_angle_max_cd - returns the maximum lean angle the autopilot may request /// get_lean_angle_max_cd - returns the maximum lean angle the autopilot may request
float AC_PosControl::get_lean_angle_max_cd() const float AC_PosControl::get_lean_angle_max_cd() const
{ {
@ -1118,6 +1103,21 @@ void AC_PosControl::get_stopping_point_xy_cm(Vector2p &stopping_point) const
stopping_point.y += t * curr_vel.y; stopping_point.y += t * curr_vel.y;
} }
/// get_stopping_point_z_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration
void AC_PosControl::get_stopping_point_z_cm(postype_t &stopping_point) const
{
const float curr_pos_z = _inav.get_position().z;
float curr_vel_z = _inav.get_velocity().z;
// avoid divide by zero by using current position if kP is very low or acceleration is zero
if (!is_positive(_p_pos_z.kP()) || !is_positive(_accel_max_z_cmss)) {
stopping_point = curr_pos_z;
return;
}
stopping_point = curr_pos_z + constrain_float(stopping_distance(curr_vel_z, _p_pos_z.kP(), _accel_max_z_cmss), - POSCONTROL_STOPPING_DIST_DOWN_MAX, POSCONTROL_STOPPING_DIST_UP_MAX);
}
/// get_bearing_to_target_cd - get bearing to target position in centi-degrees /// get_bearing_to_target_cd - get bearing to target position in centi-degrees
int32_t AC_PosControl::get_bearing_to_target_cd() const int32_t AC_PosControl::get_bearing_to_target_cd() const
{ {