Merge remote-tracking branch 'origin/master' into mmosca-gps-glonass

Author: mmosca

.github/workflows/ci.yml

@@ -79,7 +79,6 @@ jobs:
       - uses: actions/checkout@v3
       - name: Install dependencies
         run: |
-          #brew update
           brew install cmake ninja ruby
 
       - name: Setup environment

cmake/at32.cmake

@@ -15,6 +15,8 @@ set(CMSIS_DSP_DIR "${MAIN_LIB_DIR}/main/CMSIS/DSP")
 set(CMSIS_DSP_INCLUDE_DIR "${CMSIS_DSP_DIR}/Include")
 
 set(CMSIS_DSP_SRC
+    BasicMathFunctions/arm_scale_f32.c
+    BasicMathFunctions/arm_sub_f32.c
     BasicMathFunctions/arm_mult_f32.c
     TransformFunctions/arm_rfft_fast_f32.c
     TransformFunctions/arm_cfft_f32.c

cmake/stm32.cmake

@@ -16,6 +16,8 @@ set(CMSIS_DSP_DIR "${MAIN_LIB_DIR}/main/CMSIS/DSP")
 set(CMSIS_DSP_INCLUDE_DIR "${CMSIS_DSP_DIR}/Include")
 
 set(CMSIS_DSP_SRC
+    BasicMathFunctions/arm_scale_f32.c
+    BasicMathFunctions/arm_sub_f32.c
     BasicMathFunctions/arm_mult_f32.c
     TransformFunctions/arm_rfft_fast_f32.c
     TransformFunctions/arm_cfft_f32.c

src/main/common/maths.c

@@ -538,11 +538,57 @@ float fast_fsqrtf(const float value) {
 // function to calculate the normalization (pythagoras) of a 2-dimensional vector
 float NOINLINE calc_length_pythagorean_2D(const float firstElement, const float secondElement)
 {
-  return fast_fsqrtf(sq(firstElement) + sq(secondElement));
+    return fast_fsqrtf(sq(firstElement) + sq(secondElement));
 }
 
 // function to calculate the normalization (pythagoras) of a 3-dimensional vector
 float NOINLINE calc_length_pythagorean_3D(const float firstElement, const float secondElement, const float thirdElement)
 {
-  return fast_fsqrtf(sq(firstElement) + sq(secondElement) + sq(thirdElement));
-}
+    return fast_fsqrtf(sq(firstElement) + sq(secondElement) + sq(thirdElement));
+}
+
+#ifdef SITL_BUILD
+
+/**
+ * @brief Floating-point vector subtraction, equivalent of CMSIS arm_sub_f32.
+*/
+void arm_sub_f32(
+  float * pSrcA,
+  float * pSrcB,
+  float * pDst,
+  uint32_t blockSize)
+{
+    for (uint32_t i = 0; i < blockSize; i++) {
+        pDst[i] = pSrcA[i] - pSrcB[i];
+    }
+}
+
+/**
+ * @brief Floating-point vector scaling, equivalent of CMSIS arm_scale_f32.
+*/
+void arm_scale_f32(
+  float * pSrc,
+  float scale,
+  float * pDst,
+  uint32_t blockSize)
+{
+    for (uint32_t i = 0; i < blockSize; i++) {
+        pDst[i] = pSrc[i] * scale;
+    }
+}
+
+/**
+ * @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

src/main/common/maths.h

@@ -201,4 +201,10 @@ float calc_length_pythagorean_3D(const float firstElement, const float secondEle
  * The most significat byte is placed at the highest address
  * in other words, the most significant byte is "last", on odd indexes
  */
-#define int16_val_little_endian(v, idx) ((int16_t)(((uint8_t)v[2 * idx + 1] << 8) | v[2 * idx]))
+#define int16_val_little_endian(v, idx) ((int16_t)(((uint8_t)v[2 * idx + 1] << 8) | v[2 * idx]))
+
+#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

src/main/drivers/accgyro/accgyro_icm42605.c

@@ -232,8 +232,8 @@ static bool icm42605GyroRead(gyroDev_t *gyro)
     }
 
     gyro->gyroADCRaw[X] = (float) int16_val_big_endian(data, 0);
-    gyro->gyroADCRaw[Y] = (float) int16_val_big_endian(data, 0);
-    gyro->gyroADCRaw[Z] = (float) int16_val_big_endian(data, 0);
+    gyro->gyroADCRaw[Y] = (float) int16_val_big_endian(data, 1);
+    gyro->gyroADCRaw[Z] = (float) int16_val_big_endian(data, 2);
 
     return true;
 }

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);

src/main/sensors/gyro.c

@@ -411,19 +411,15 @@ static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroC
         if (zeroCalibrationIsCompleteV(gyroCal)) {
             float gyroADCtmp[XYZ_AXIS_COUNT];
 
-            // Copy gyro value into int32_t (to prevent overflow) and then apply calibration and alignment
-            gyroADCtmp[X] = gyroDev->gyroADCRaw[X] - (int32_t)gyroDev->gyroZero[X];
-            gyroADCtmp[Y] = gyroDev->gyroADCRaw[Y] - (int32_t)gyroDev->gyroZero[Y];
-            gyroADCtmp[Z] = gyroDev->gyroADCRaw[Z] - (int32_t)gyroDev->gyroZero[Z];
+            //Apply zero calibration with CMSIS DSP
+            arm_sub_f32(gyroDev->gyroADCRaw, gyroDev->gyroZero, gyroADCtmp, 3);
 
             // Apply sensor alignment
             applySensorAlignment(gyroADCtmp, gyroADCtmp, gyroDev->gyroAlign);
             applyBoardAlignment(gyroADCtmp);
 
             // Convert to deg/s and store in unified data
-            gyroADCf[X] = (float)gyroADCtmp[X] * gyroDev->scale;
-            gyroADCf[Y] = (float)gyroADCtmp[Y] * gyroDev->scale;
-            gyroADCf[Z] = (float)gyroADCtmp[Z] * gyroDev->scale;
+            arm_scale_f32(gyroADCtmp, gyroDev->scale, gyroADCf, 3);
 
             return true;
         } else {

src/main/sensors/gyro.h

@@ -25,6 +25,11 @@
 #include "drivers/sensor.h"
 #include "flight/dynamic_gyro_notch.h"
 #include "flight/secondary_dynamic_gyro_notch.h"
+#if !defined(SITL_BUILD)
+#include "arm_math.h"
+#else
+#include <math.h>
+#endif
 
 typedef enum {
     GYRO_NONE = 0,

src/test/unit/target.h

@@ -17,6 +17,7 @@
 
 #pragma once
 
+#define SITL_BUILD
 #define USE_MAG
 #define USE_BARO
 #define USE_GPS