From 406cff296da2b4b4a2bbb71e9005a425ebbfa8d4 Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Sun, 18 Dec 2011 02:24:15 +0200 Subject: [PATCH] CameraStab: bypass LPF calculation if ResponseTime is zero --- flight/Modules/CameraStab/camerastab.c | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/flight/Modules/CameraStab/camerastab.c b/flight/Modules/CameraStab/camerastab.c index 87f07e079..20f4db03a 100644 --- a/flight/Modules/CameraStab/camerastab.c +++ b/flight/Modules/CameraStab/camerastab.c @@ -160,9 +160,14 @@ static void attitudeUpdated(UAVObjEvent* ev) default: PIOS_Assert(0); } + + // bypass LPF calculation if ResponseTime is zero float rt = (float)cameraStab.ResponseTime[i]; - csd->inputs_filtered[i] = (rt / (rt + csd->dT)) * csd->inputs_filtered[i] - + (csd->dT / (rt + csd->dT)) * csd->inputs[i]; + if (rt) + csd->inputs_filtered[i] = (rt / (rt + csd->dT)) * csd->inputs_filtered[i] + + (csd->dT / (rt + csd->dT)) * csd->inputs[i]; + else + csd->inputs_filtered[i] = csd->inputs[i]; } } }