diff options
| author | Johan Jansen <jnsn.johan@gmail.com> | 2015-01-26 13:51:34 +0100 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-01-26 13:58:19 +0100 |
| commit | 9cc94fcb2dde1a591c20e008ca59d1f876c2070a (patch) | |
| tree | 4f9d9deda851dc0f1ef676aa947a67113ddc831b | |
| parent | 18d756dd599f93abcd5dc89f323a5df77384ceac (diff) | |
| download | px4-firmware-9cc94fcb2dde1a591c20e008ca59d1f876c2070a.tar.gz px4-firmware-9cc94fcb2dde1a591c20e008ca59d1f876c2070a.tar.bz2 px4-firmware-9cc94fcb2dde1a591c20e008ca59d1f876c2070a.zip | |
mc_pos_control: Protect against NaN and Inf setpoints
| -rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 9 |
1 files changed, 8 insertions, 1 deletions
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) { |