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

AC_WPNav: use multiply instead of divide 

non-functional change
This commit is contained in:
Randy Mackay 2018-02-08 11:24:12 +09:00
parent 69cfea4057
commit 5be1020578
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -281,7 +281,7 @@ void AC_WPNav::get_loiter_stopping_point_xy(Vector3f& stopping_point) const
float AC_WPNav::get_loiter_angle_max_cd() const float AC_WPNav::get_loiter_angle_max_cd() const
{ {
if (is_zero(_loiter_angle_max)){ if (is_zero(_loiter_angle_max)){
return MIN(_attitude_control.lean_angle_max()*2.0f/3.0f, _pos_control.get_lean_angle_max_cd()*2.0f/3.0f); return MIN(_attitude_control.lean_angle_max(), _pos_control.get_lean_angle_max_cd()) * (2.0f/3.0f);
} }
return MIN(_loiter_angle_max*100.0f, _pos_control.get_lean_angle_max_cd()); return MIN(_loiter_angle_max*100.0f, _pos_control.get_lean_angle_max_cd());
} }
@ -331,7 +331,7 @@ void AC_WPNav::calc_loiter_desired_velocity(float nav_dt, float ekfGndSpdLimit)
float loiter_brake_accel = 0.0f; float loiter_brake_accel = 0.0f;
if (_loiter_desired_accel.is_zero()) { if (_loiter_desired_accel.is_zero()) {
if((AP_HAL::millis()-_brake_timer) > _loiter_brake_delay * 1000.0f){ if((AP_HAL::millis()-_brake_timer) > _loiter_brake_delay * 1000.0f){
float brake_gain = _pos_control.get_vel_xy_pid().kP()/2.0f; float brake_gain = _pos_control.get_vel_xy_pid().kP() * 0.5f;
loiter_brake_accel = constrain_float(AC_AttitudeControl::sqrt_controller(desired_speed, brake_gain, _loiter_brake_jerk_max_cmsss, nav_dt), 0.0f, _loiter_brake_accel_cmss); loiter_brake_accel = constrain_float(AC_AttitudeControl::sqrt_controller(desired_speed, brake_gain, _loiter_brake_jerk_max_cmsss, nav_dt), 0.0f, _loiter_brake_accel_cmss);
} }
} else { } else {