dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mc_pos_control: ignore a NaN/inf position setpoint (#7186) 

* mc_pos_control: ignore a NaN/inf position setpoint

This is a hotfix that prevents the position controller from trying to
do velocity control with a position setpoint that isn't valid in the
first place.

This is only a workaround, ideally the controls later should not scale
down throttle to the minimum just because the position setpoint is far
away if they still have valid setpoint in z.

* mc_pos_control: use PX4_ISFINITE and not isfinite
This commit is contained in:
Julian Oes 2017-05-04 22:55:53 +02:00 committed by Lorenz Meier
parent 1bb56e775e
commit bcd66f1408
1 changed files with 10 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -1693,8 +1693,16 @@ MulticopterPositionControl::calculate_velocity_setpoint(float dt)
{
/* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */
if (_run_pos_control) {
_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0);
_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
// If for any reason, we get a NaN position setpoint, we better just stay where we are.
if (PX4_ISFINITE(_pos_sp(0)) && PX4_ISFINITE(_pos_sp(1))) {
_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0);
_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
} else {
_vel_sp(0) = 0.0f;
_vel_sp(1) = 0.0f;
}
}
limit_altitude();