From 2519d97aa2471ea2162cf8e409aec9231482fc9e Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Sun, 29 Apr 2018 09:05:02 +0200 Subject: [PATCH] PositionControl: add failsafe --- src/modules/mc_pos_control/PositionControl.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/modules/mc_pos_control/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl.cpp index 93cf7fc57e..2df6404431 100644 --- a/src/modules/mc_pos_control/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl.cpp @@ -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()