mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-01 05:50:36 +08:00
NaN check and better init in lowpass
This commit is contained in:
@@ -121,6 +121,9 @@ int blockLimitSymTest()
|
||||
|
||||
float BlockLowPass::update(float input)
|
||||
{
|
||||
if (!isfinite(getState())) {
|
||||
setState(input);
|
||||
}
|
||||
float b = 2 * float(M_PI) * getFCut() * getDt();
|
||||
float a = b / (1 + b);
|
||||
setState(a * input + (1 - a)*getState());
|
||||
|
||||
Reference in New Issue
Block a user