Merge pull request #9116 from iNavFlight/dzikuvx-dsp-acc-zero

Acc processing with DSP
iNavFlight · Jun 14, 2023 · 710891f · 710891f
2 parents 12e13da + 815685d
commit 710891f
Show file tree

Hide file tree

Showing 3 changed files with 41 additions and 8 deletions.
diff --git a/src/main/common/maths.c b/src/main/common/maths.c
@@ -577,4 +577,18 @@ void arm_scale_f32(
     }
 }
 
+/**
+ * @brief Floating-point vector multiplication, equivalent of CMSIS arm_mult_f32.
+*/
+void arm_mult_f32(
+  float * pSrcA,
+  float * pSrcB,
+  float * pDst,
+  uint32_t blockSize)
+{
+    for (uint32_t i = 0; i < blockSize; i++) {
+        pDst[i] = pSrcA[i] * pSrcB[i];
+    }
+}
+
 #endif
diff --git a/src/main/common/maths.h b/src/main/common/maths.h
@@ -206,4 +206,5 @@ float calc_length_pythagorean_3D(const float firstElement, const float secondEle
 #ifdef SITL_BUILD
 void arm_sub_f32(float * pSrcA, float * pSrcB, float * pDst, uint32_t blockSize);
 void arm_scale_f32(float * pSrc, float scale, float * pDst, uint32_t blockSize);
+void arm_mult_f32(float * pSrcA, float * pSrcB, float * pDst, uint32_t blockSize);
 #endif
diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c
@@ -82,6 +82,9 @@ static EXTENDED_FASTRAM pt1Filter_t accVibeFilter[XYZ_AXIS_COUNT];
 static EXTENDED_FASTRAM filterApplyFnPtr accNotchFilterApplyFn;
 static EXTENDED_FASTRAM void *accNotchFilter[XYZ_AXIS_COUNT];
 
+static EXTENDED_FASTRAM float fAccZero[XYZ_AXIS_COUNT];
+static EXTENDED_FASTRAM float fAccGain[XYZ_AXIS_COUNT];
+
 PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 5);
 
 void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance)
@@ -105,6 +108,17 @@ void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance)
     );
 }
 
+static void updateAccCoefficients(void) {
+
+    for (uint8_t i = 0; i < XYZ_AXIS_COUNT; i++) {
+        //Float zero
+        fAccZero[i] = (float)accelerometerConfig()->accZero.raw[i];
+        //Float gain
+        fAccGain[i] = (float)accelerometerConfig()->accGain.raw[i] / 4096.0f;
+    }
+
+}
+
 static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
 {
     accelerationSensor_e accHardware = ACC_NONE;
@@ -280,6 +294,7 @@ bool accInit(uint32_t targetLooptime)
     acc.accTargetLooptime = targetLooptime;
     acc.accClipCount = 0;
     accInitFilters();
+    updateAccCoefficients();
 
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
         acc.extremes[axis].min = 100;
@@ -477,25 +492,28 @@ static void performAcclerationCalibration(void)
             // saveConfigAndNotify will trigger eepromREAD and in turn call back the accelerometer gain validation
             // that will set ENABLE_STATE(ACCELEROMETER_CALIBRATED) if all is good
             saveConfigAndNotify();
+            //Recompute all coeffs
+            updateAccCoefficients();
         }
     }
 }
 
-static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const flightDynamicsTrims_t * accGain)
+static void applyAccelerationZero(void)
 {
-    accADC[X] = (accADC[X] - accZero->raw[X]) * accGain->raw[X] / 4096;
-    accADC[Y] = (accADC[Y] - accZero->raw[Y]) * accGain->raw[Y] / 4096;
-    accADC[Z] = (accADC[Z] - accZero->raw[Z]) * accGain->raw[Z] / 4096;
+    float tmp[XYZ_AXIS_COUNT];
+
+    //Apply zero
+    arm_sub_f32(accADC, fAccZero, tmp, XYZ_AXIS_COUNT);
+    //Apply gain
+    arm_mult_f32(tmp, fAccGain, accADC, XYZ_AXIS_COUNT);
 }
 
 /*
  * Calculate measured acceleration in body frame in m/s^2
  */
 void accGetMeasuredAcceleration(fpVector3_t *measuredAcc)
 {
-    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        measuredAcc->v[axis] = acc.accADCf[axis] * GRAVITY_CMSS;
-    }
+    arm_scale_f32(acc.accADCf, GRAVITY_CMSS, measuredAcc->v, XYZ_AXIS_COUNT);
 }
 
 /*
@@ -530,7 +548,7 @@ void accUpdate(void)
 
     if (!ARMING_FLAG(SIMULATOR_MODE_SITL)) {
         performAcclerationCalibration();
-        applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain);  
+        applyAccelerationZero();  
     } 
 
     applySensorAlignment(accADC, accADC, acc.dev.accAlign);