Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-13 18:14:19 -03:00
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl: protect against POS_Z_P, ACCEL_Z_P divide-by-zero

Browse Source
This commit is contained in:
Randy Mackay 2017-03-14 20:50:32 +09:00
parent 40cceea8b8
commit c2a290b2c3
1 changed files with 6 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -288,6 +288,12 @@ void AC_PosControl::get_stopping_point_z(Vector3f& stopping_point) const
} }
} }
// avoid divide by zero by using current position if kP is very low or acceleration is zero
if (_p_pos_z.kP() <= 0.0f || _accel_z_cms <= 0.0f) {
stopping_point.z = curr_pos_z;
return;
}
// calculate the velocity at which we switch from calculating the stopping point using a linear function to a sqrt function // calculate the velocity at which we switch from calculating the stopping point using a linear function to a sqrt function
linear_velocity = _accel_z_cms/_p_pos_z.kP(); linear_velocity = _accel_z_cms/_p_pos_z.kP();