Optimised alignSensors function

src/main/sensors/acceleration.c

@@ -203,7 +203,7 @@ void updateAccelerationReadings(void)
 
     applyAccelerationZero(accZero, accGain);
 
-    alignSensors(accADC, accADC, accAlign);
+    alignSensors(accADC, accAlign);
 }
 
 void setAccelerationZero(flightDynamicsTrims_t * accZeroToUse)

src/main/sensors/boardalignment.c

@@ -75,52 +75,53 @@ static void alignBoard(int32_t *vec)
     vec[Z] = lrintf(boardRotation[0][Z] * x + boardRotation[1][Z] * y + boardRotation[2][Z] * z);
 }
 
-void alignSensors(const int32_t *src, int32_t *dest, uint8_t rotation)
+void alignSensors(int32_t *dest, uint8_t rotation)
 {
-    static uint32_t swap[3];
+    const int32_t x = dest[X];
-    memcpy(swap, src, sizeof(swap));
+    const int32_t y = dest[Y];
+    const int32_t z = dest[Z];
 
     switch (rotation) {
     default:
     case CW0_DEG:
-            dest[X] = swap[X];
+        dest[X] = x;
-            dest[Y] = swap[Y];
+        dest[Y] = y;
-            dest[Z] = swap[Z];
+        dest[Z] = z;
         break;
     case CW90_DEG:
-            dest[X] = swap[Y];
+        dest[X] = y;
-            dest[Y] = -swap[X];
+        dest[Y] = -x;
-            dest[Z] = swap[Z];
+        dest[Z] = z;
         break;
     case CW180_DEG:
-            dest[X] = -swap[X];
+        dest[X] = -x;
-            dest[Y] = -swap[Y];
+        dest[Y] = -y;
-            dest[Z] = swap[Z];
+        dest[Z] = z;
         break;
     case CW270_DEG:
-            dest[X] = -swap[Y];
+        dest[X] = -y;
-            dest[Y] = swap[X];
+        dest[Y] = x;
-            dest[Z] = swap[Z];
+        dest[Z] = z;
         break;
     case CW0_DEG_FLIP:
-            dest[X] = -swap[X];
+        dest[X] = -x;
-            dest[Y] = swap[Y];
+        dest[Y] = y;
-            dest[Z] = -swap[Z];
+        dest[Z] = -z;
         break;
     case CW90_DEG_FLIP:
-            dest[X] = swap[Y];
+        dest[X] = y;
-            dest[Y] = swap[X];
+        dest[Y] = x;
-            dest[Z] = -swap[Z];
+        dest[Z] = -z;
         break;
     case CW180_DEG_FLIP:
-            dest[X] = swap[X];
+        dest[X] = x;
-            dest[Y] = -swap[Y];
+        dest[Y] = -y;
-            dest[Z] = -swap[Z];
+        dest[Z] = -z;
         break;
     case CW270_DEG_FLIP:
-            dest[X] = -swap[Y];
+        dest[X] = -y;
-            dest[Y] = -swap[X];
+        dest[Y] = -x;
-            dest[Z] = -swap[Z];
+        dest[Z] = -z;
         break;
     }
 

src/main/sensors/boardalignment.h

@@ -23,6 +23,6 @@ typedef struct boardAlignment_s {
     int16_t yawDeciDegrees;
 } boardAlignment_t;
 
-void alignSensors(const int32_t *src, int32_t *dest, uint8_t rotation);
+void alignSensors(int32_t *dest, uint8_t rotation);
 void initBoardAlignment(const boardAlignment_t *boardAlignment);
 void updateBoardAlignment(boardAlignment_t *boardAlignment, int16_t roll, int16_t pitch);

src/main/sensors/compass.c

@@ -140,7 +140,7 @@ void compassUpdate(uint32_t currentTime, flightDynamicsTrims_t *magZero)
         }
     }
 
-    alignSensors(magADC, magADC, magAlign);
+    alignSensors(magADC, magAlign);
 
     magUpdatedAtLeastOnce = 1;
 }

src/main/sensors/gyro.c

@@ -156,5 +156,5 @@ void gyroUpdate(void)
 
     applyGyroZero();
 
-    alignSensors(gyroADC, gyroADC, gyroAlign);
+    alignSensors(gyroADC, gyroAlign);
 }