diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2014-08-01 01:04:11 +0200 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2014-08-01 01:04:11 +0200 |
| commit | f761a79bde71f6ba437c98ea5a92c2d93150c3f3 (patch) | |
| tree | a239fab6f169a12ab2265917269c4c989ce6bf9c | |
| parent | c8da9545714714ea425f98818e5c98db83ff9a64 (diff) | |
| download | px4-firmware-f761a79bde71f6ba437c98ea5a92c2d93150c3f3.tar.gz px4-firmware-f761a79bde71f6ba437c98ea5a92c2d93150c3f3.tar.bz2 px4-firmware-f761a79bde71f6ba437c98ea5a92c2d93150c3f3.zip | |
mc_pos_control: target velocity LPF reset implemented on NaNFORK
| -rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 8 |
1 files changed, 6 insertions, 2 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 0a4aaafa6..f67e25905 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -672,8 +672,12 @@ MulticopterPositionControl::update_target_pos() if (isfinite(tvel_current(0)) && isfinite(tvel_current(1)) && isfinite(tvel_current(2))) { _tvel = tvel_current; - } else if (!(isfinite(_tvel(0)) && isfinite(_tvel(1)) && isfinite(_tvel(2)))) { - _tvel.zero(); + } else { + /* NaN on output, use previous value if possible and reset LPF */ + if (!(isfinite(_tvel(0)) && isfinite(_tvel(1)) && isfinite(_tvel(2)))) { + _tvel.zero(); + } + _tvel_lpf.reset(_tvel); } /* update target position predictor */ |