From 343049d653f7880d34e7576e94ffd63b653eba90 Mon Sep 17 00:00:00 2001 From: s0up Date: Thu, 14 Jun 2018 14:36:31 -0700 Subject: [PATCH] fix the calculation of acceleration magnitude average to run at gps update freq vs PID looptime freq to save CPU resources --- src/main/flight/gps_rescue.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index 5d6fa1e037..2c14226c6e 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -213,12 +213,12 @@ void sensorUpdate() rescueState.sensor.zVelocity = (rescueState.sensor.currentAltitude - previousAltitude) * 1000000.0f / dTime; rescueState.sensor.zVelocityAvg = 0.8f * rescueState.sensor.zVelocityAvg + rescueState.sensor.zVelocity * 0.2f; + rescueState.sensor.accMagnitude = (float) sqrt(sq(acc.accADC[Z]) + sq(acc.accADC[X]) + sq(acc.accADC[Y]) / sq(acc.dev.acc_1G)); + rescueState.sensor.accMagnitudeAvg = (rescueState.sensor.accMagnitudeAvg * 0.8f) + (rescueState.sensor.accMagnitude * 0.2f); + previousAltitude = rescueState.sensor.currentAltitude; previousTimeUs = currentTimeUs; } - - rescueState.sensor.accMagnitude = (float) sqrt(sq(acc.accADC[Z]) + sq(acc.accADC[X]) + sq(acc.accADC[Y]) / sq(acc.dev.acc_1G)); - rescueState.sensor.accMagnitudeAvg = (rescueState.sensor.accMagnitudeAvg * 0.998f) + (rescueState.sensor.accMagnitude * 0.002f); } void performSanityChecks()