From 9cc94fcb2dde1a591c20e008ca59d1f876c2070a Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Mon, 26 Jan 2015 13:51:34 +0100 Subject: mc_pos_control: Protect against NaN and Inf setpoints --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 3b631e2ce..962dc6d4a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -726,11 +726,18 @@ MulticopterPositionControl::control_auto(float dt) reset_alt_sp(); } + //Poll position setpoint bool updated; orb_check(_pos_sp_triplet_sub, &updated); - if (updated) { orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); + + //Make sure that the position setpoint is valid + if (!isfinite(_pos_sp_triplet.current.lat) || + !isfinite(_pos_sp_triplet.current.lon) || + !isfinite(_pos_sp_triplet.current.alt)) { + _pos_sp_triplet.current.valid = false; + } } if (_pos_sp_triplet.current.valid) { -- cgit v1.2.3