Merge pull request #2624 from iNavFlight/de_update_calibration_api

Update calibration API

src/main/fc/fc_msp.c

@@ -1043,6 +1043,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
 
     case MSP_CALIBRATION_DATA:
     #ifdef USE_ACC
+        sbufWriteU8(dst, accGetCalibrationAxisFlags());
         sbufWriteU16(dst, accelerometerConfig()->accZero.raw[X]);
         sbufWriteU16(dst, accelerometerConfig()->accZero.raw[Y]);
         sbufWriteU16(dst, accelerometerConfig()->accZero.raw[Z]);
@@ -1050,6 +1051,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         sbufWriteU16(dst, accelerometerConfig()->accGain.raw[Y]);
         sbufWriteU16(dst, accelerometerConfig()->accGain.raw[Z]);
     #else
+        sbufWriteU8(dst, 0);
         sbufWriteU16(dst, 0);
         sbufWriteU16(dst, 0);
         sbufWriteU16(dst, 0);

src/main/sensors/acceleration.c

@@ -349,7 +349,7 @@ uint8_t accGetCalibrationAxisFlags(void)
 {
     uint8_t flags = 0;
     for (int i = 0; i < 6; i++) {
-        if (accGetCalibrationAxisStatus(0)) {
+        if (accGetCalibrationAxisStatus(i)) {
             flags |= (1 << i);
         }
     }
@@ -359,6 +359,9 @@ uint8_t accGetCalibrationAxisFlags(void)
 
 int getPrimaryAxisIndex(int32_t sample[3])
 {
+    // Apply sensor alignment (for axis detection only)
+    applySensorAlignment(sample, acc.dev.accAlign);
+
     // Tolerate up to atan(1 / 1.5) = 33 deg tilt (in worst case 66 deg separation between points)
     if ((ABS(sample[Z]) / 1.5f) > ABS(sample[X]) && (ABS(sample[Z]) / 1.5f) > ABS(sample[Y])) {
         //Z-axis
@@ -366,11 +369,11 @@ int getPrimaryAxisIndex(int32_t sample[3])
     }
     else if ((ABS(sample[X]) / 1.5f) > ABS(sample[Y]) && (ABS(sample[X]) / 1.5f) > ABS(sample[Z])) {
         //X-axis
-        return (sample[X] > 0) ? 2 : 3;
+        return (sample[X] > 0) ? 3 : 5;
     }
     else if ((ABS(sample[Y]) / 1.5f) > ABS(sample[X]) && (ABS(sample[Y]) / 1.5f) > ABS(sample[Z])) {
         //Y-axis
-        return (sample[Y] > 0) ? 4 : 5;
+        return (sample[Y] > 0) ? 2 : 4;
     }
     else
         return -1;
@@ -508,7 +511,8 @@ void accUpdate(void)
     }
 
     applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain);
-    alignSensors(accADC, acc.dev.accAlign);
+    applySensorAlignment(accADC, acc.dev.accAlign);
+    applyBoardAlignment(accADC);
 
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
         acc.accADCf[axis] = (float)accADC[axis] / acc.dev.acc_1G;

src/main/sensors/boardalignment.c

@@ -69,8 +69,12 @@ void updateBoardAlignment(int16_t roll, int16_t pitch)
     initBoardAlignment();
 }
 
-static void alignBoard(int32_t *vec)
+void applyBoardAlignment(int32_t *vec)
 {
+    if (standardBoardAlignment) {
+        return;
+    }
+
     int32_t x = vec[X];
     int32_t y = vec[Y];
     int32_t z = vec[Z];
@@ -80,7 +84,7 @@ static void alignBoard(int32_t *vec)
     vec[Z] = lrintf(boardRotation[0][Z] * x + boardRotation[1][Z] * y + boardRotation[2][Z] * z);
 }
 
-void alignSensors(int32_t *dest, uint8_t rotation)
+void applySensorAlignment(int32_t *dest, uint8_t rotation)
 {
     const int32_t x = dest[X];
     const int32_t y = dest[Y];
@@ -129,7 +133,4 @@ void alignSensors(int32_t *dest, uint8_t rotation)
         dest[Z] = -z;
         break;
     }
-
-    if (!standardBoardAlignment)
-        alignBoard(dest);
 }

src/main/sensors/boardalignment.h

@@ -27,6 +27,7 @@ typedef struct boardAlignment_s {
 
 PG_DECLARE(boardAlignment_t, boardAlignment);
 
-void alignSensors(int32_t *dest, uint8_t rotation);
 void initBoardAlignment(void);
 void updateBoardAlignment(int16_t roll, int16_t pitch);
+void applySensorAlignment(int32_t *dest, uint8_t rotation);
+void applyBoardAlignment(int32_t *vec);

src/main/sensors/compass.c

@@ -366,7 +366,8 @@ void compassUpdate(timeUs_t currentTimeUs)
         }
     }
 
-    alignSensors(mag.magADC, mag.dev.magAlign);
+    applySensorAlignment(mag.magADC, mag.dev.magAlign);
+    applyBoardAlignment(mag.magADC);
 
     magUpdatedAtLeastOnce = 1;
 }

src/main/sensors/gyro.c

@@ -399,7 +399,8 @@ void gyroUpdate(timeDelta_t gyroUpdateDeltaUs)
             gyroADC[X] = (int32_t)gyroDev0.gyroADCRaw[X] - (int32_t)gyroDev0.gyroZero[X];
             gyroADC[Y] = (int32_t)gyroDev0.gyroADCRaw[Y] - (int32_t)gyroDev0.gyroZero[Y];
             gyroADC[Z] = (int32_t)gyroDev0.gyroADCRaw[Z] - (int32_t)gyroDev0.gyroZero[Z];
-            alignSensors(gyroADC, gyroDev0.gyroAlign);
+            applySensorAlignment(gyroADC, gyroDev0.gyroAlign);
+            applyBoardAlignment(gyroADC);
         } else {
             performGyroCalibration(&gyroDev0, &gyroCalibration, gyroConfig()->gyroMovementCalibrationThreshold);
             // Reset gyro values to zero to prevent other code from using uncalibrated data