fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

PositionControl: add failsafe

Browse Source
This commit is contained in:
Dennis Mannhart 2018-04-29 09:05:02 +02:00 committed by Lorenz Meier
parent 2017e394b3
commit 2519d97aa2
1 changed files with 13 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/modules/mc_pos_control/PositionControl.cpp
View File

@ -100,6 +100,8 @@ void PositionControl::generateThrustYawSetpoint(const float &dt)
void PositionControl::_interfaceMapping()
{
// if noting is valid, then apply failsafe landing
bool failsafe = false;
// Respects FlightTask interface, where NAN-set-points are of no interest
// and do not require control. A valide position and velocity setpoint will
// be mapped to a desired position setpoint with a feed-forward term.
@ -137,6 +139,8 @@ void PositionControl::_interfaceMapping()
_vel_dot(i) = 0.0f;
} else {
// nothing is valid. do failsafe
failsafe = true;
PX4_WARN("TODO: add safety");
}
}
@ -152,6 +156,15 @@ void PositionControl::_interfaceMapping()
// agrees with FlightTask-interface definition.
_yaw_sp = _yaw;
}
// check failsafe
if (failsafe) {
// point the thrust upwards
_thr_sp(0) = _thr_sp(1) = 0.0f;
// throttle down such that vehicle goes down with
// 30% of throttle range between min and hover
_thr_sp(2) = MPC_THR_MIN.get() + (MPC_THR_HOVER.get() - MPC_THR_MIN.get()) * 0.3f;
}
}
void PositionControl::_positionController()