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

AC_WPNav: Add units to the AC_AttitudeControl Library

This commit is contained in:
Leonard Hall 2021-09-06 19:15:59 +09:30 committed by Randy Mackay
parent 90b5bc6a2d
commit 8c51271b45
2 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AC_WPNav/AC_Loiter.cpp
View File

@ -186,7 +186,7 @@ void AC_Loiter::get_stopping_point_xy(Vector2f& stopping_point) const
float AC_Loiter::get_angle_max_cd() const float AC_Loiter::get_angle_max_cd() const
{ {
if (is_zero(_angle_max)) { if (is_zero(_angle_max)) {
return MIN(_attitude_control.lean_angle_max(), _pos_control.get_lean_angle_max_cd()) * (2.0f/3.0f); return MIN(_attitude_control.lean_angle_max_cd(), _pos_control.get_lean_angle_max_cd()) * (2.0f/3.0f);
} }
return MIN(_angle_max*100.0f, _pos_control.get_lean_angle_max_cd()); return MIN(_angle_max*100.0f, _pos_control.get_lean_angle_max_cd());
} }
@ -202,7 +202,7 @@ void AC_Loiter::update(bool avoidance_on)
void AC_Loiter::sanity_check_params() void AC_Loiter::sanity_check_params()
{ {
_speed_cms = MAX(_speed_cms, LOITER_SPEED_MIN); _speed_cms = MAX(_speed_cms, LOITER_SPEED_MIN);
_accel_cmss = MIN(_accel_cmss, GRAVITY_MSS * 100.0f * tanf(ToRad(_attitude_control.lean_angle_max() * 0.01f))); _accel_cmss = MIN(_accel_cmss, GRAVITY_MSS * 100.0f * tanf(ToRad(_attitude_control.lean_angle_max_cd() * 0.01f)));
} }
/// calc_desired_velocity - updates desired velocity (i.e. feed forward) with pilot requested acceleration and fake wind resistance /// calc_desired_velocity - updates desired velocity (i.e. feed forward) with pilot requested acceleration and fake wind resistance

2
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -150,7 +150,7 @@ void AC_WPNav::wp_and_spline_init(float speed_cms)
// sanity check parameters // sanity check parameters
// check _wp_accel_cmss is reasonable // check _wp_accel_cmss is reasonable
const float wp_accel_cmss = MIN(_wp_accel_cmss, GRAVITY_MSS * 100.0f * tanf(ToRad(_attitude_control.lean_angle_max() * 0.01f))); const float wp_accel_cmss = MIN(_wp_accel_cmss, GRAVITY_MSS * 100.0f * tanf(ToRad(_attitude_control.lean_angle_max_cd() * 0.01f)));
_wp_accel_cmss.set_and_save_ifchanged((_wp_accel_cmss <= 0) ? WPNAV_ACCELERATION : wp_accel_cmss); _wp_accel_cmss.set_and_save_ifchanged((_wp_accel_cmss <= 0) ? WPNAV_ACCELERATION : wp_accel_cmss);
// check _wp_radius_cm is reasonable // check _wp_radius_cm is reasonable