imu: optimize imuMahonyAHRSupdate() hot path (5 micro-opts)

Five cycle-saving changes to imuMahonyAHRSupdate(), which accounts for
~205 µs (30%) of the PID loop on RP2350.  Each change is safe on all
targets; the gains are proportionally smaller on F7/H7 where the function
already lives in ITCM RAM.

1. Replace quaternionRotateVector({0,0,1}) with rMat[2][*] reads.
   imuComputeRotationMatrix() is called at the end of every invocation and
   keeps rMat in sync with orientation.  Rotating the constant gravity
   vector EF→BF yields exactly the third row of rMat, so three float loads
   replace ~56 floating-point multiply/add operations.

2. Eliminate sqrt() from the Taylor-series threshold.
   Original: thetaMagnitudeSq < sqrt(24e-6).
   Squaring both sides (both non-negative) gives the equivalent condition
   thetaMagnitudeSq² < 24e-6 with no sqrt call.

3. First-order Newton quaternion renormalization replaces quaternionNormalize()
   (sqrt + 4 divides, ~35 cycles) with scale = (3 - normSq) * 0.5 (~14 cycles).
   At 1 kHz the per-step drift |ε| < 1e-6, making the O(ε²) error < 1e-12.
   imuCheckAndResetOrientationQuaternion() remains as the catastrophic-failure
   safety net.

4. Precompute the anti-windup i_limit in imuConfigure() (called only on
   settings save).  The hot path now reads a single float instead of
   performing an add, a multiply, and a divide every PID cycle.
   Adds dcm_i_limit to imuRuntimeConfig_t and imuConfigure().

5. Reduce prevOrientation snapshot frequency from every PID cycle to
   every 100 cycles (~100 ms at 1 kHz).  The snapshot is only used by
   the fault-recovery path which should never fire in normal flight;
   16 bytes of unnecessary SRAM write every ms is not justified.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

Author: sensei-hacker

src/main/flight/imu.c

@@ -171,6 +171,12 @@ void imuConfigure(void)
     imuRuntimeConfig.dcm_kp_mag = imuConfig()->dcm_kp_mag / 10000.0f;
     imuRuntimeConfig.dcm_ki_mag = imuConfig()->dcm_ki_mag / 10000.0f;
     imuRuntimeConfig.small_angle = imuConfig()->small_angle;
+    /* Precompute the Mahony anti-windup clamp.  The PID loop reads this value
+     * 1000× per second; the kP gains only change when the user saves settings,
+     * so computing it here (called once per save) avoids one add, one multiply,
+     * and one divide on every PID cycle. */
+    imuRuntimeConfig.dcm_i_limit = DEGREES_TO_RADIANS(2.0f)
+        * (imuRuntimeConfig.dcm_kp_acc + imuRuntimeConfig.dcm_kp_mag) * 0.5f;
 }
 
 void imuInit(void)
@@ -368,7 +374,19 @@ static float imuCalculateMcCogAccWeight(void)
 static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, const fpVector3_t * vCOG, const fpVector3_t * vCOGAcc, float accWScaler, float magWScaler)
 {
     STATIC_FASTRAM fpVector3_t vGyroDriftEstimate = { 0 };
-    fpQuaternion_t prevOrientation = orientation;
+
+    /* Opt 5: snapshot prevOrientation every 100 PID cycles instead of every cycle.
+     * The snapshot is only used by the fault-recovery path in
+     * imuCheckAndResetOrientationQuaternion(), which should never fire in normal
+     * flight.  Copying 4 floats 1000×/s just to support a near-zero-probability
+     * reset path is wasteful; 100 ms staleness is a safe recovery point. */
+    static uint8_t prevOrientationSnapshotCount = 0;
+    static fpQuaternion_t prevOrientation = { .q0 = 1.0f };  // identity quaternion safe default
+    if (++prevOrientationSnapshotCount >= 100) {
+        prevOrientationSnapshotCount = 0;
+        prevOrientation = orientation;
+    }
+
     fpVector3_t vRotation = *gyroBF;
 
     /* Calculate general spin rate (rad/s) */
@@ -501,11 +519,17 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
 
     /* Step 2: Roll and pitch correction -  use measured acceleration vector */
     if (accBF) {
-        static const fpVector3_t vGravity = { .v = { 0.0f, 0.0f, 1.0f } };
         fpVector3_t vEstGravity, vAcc, vErr;
 
-        // Calculate estimated gravity vector in body frame
-        quaternionRotateVector(&vEstGravity, &vGravity, &orientation);    // EF -> BF
+        /* Opt 1: imuComputeRotationMatrix() is called at the END of every
+         * imuMahonyAHRSupdate() and keeps rMat in sync with orientation.
+         * Rotating the constant unit-gravity vector {0,0,1} from EF to BF by
+         * the current orientation quaternion yields exactly the third row of rMat
+         * (rMat[2][0..2]).  Reading those three floats replaces a quaternionRotateVector()
+         * call that costs 2× quaternionMultiply = ~32 multiplies + 24 adds. */
+        vEstGravity.x = rMat[2][0];
+        vEstGravity.y = rMat[2][1];
+        vEstGravity.z = rMat[2][2];
 
         // Error is sum of cross product between estimated direction and measured direction of gravity
         vectorNormalize(&vAcc, accBF);
@@ -528,7 +552,9 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
         vectorAdd(&vRotation, &vRotation, &vErr);
     }
     // Anti wind-up
-    float i_limit = DEGREES_TO_RADIANS(2.0f) * (imuRuntimeConfig.dcm_kp_acc + imuRuntimeConfig.dcm_kp_mag) / 2.0f;
+    /* Opt 4: dcm_i_limit is computed once in imuConfigure() (called on settings save),
+     * not recomputed each PID cycle.  The kP gains do not change at runtime. */
+    const float i_limit = imuRuntimeConfig.dcm_i_limit;
     vGyroDriftEstimate.x = constrainf(vGyroDriftEstimate.x, -i_limit, i_limit);
     vGyroDriftEstimate.y = constrainf(vGyroDriftEstimate.y, -i_limit, i_limit);
     vGyroDriftEstimate.z = constrainf(vGyroDriftEstimate.z, -i_limit, i_limit);
@@ -551,7 +577,10 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
         // Proper quaternion from axis/angle involves computing sin/cos, but the formula becomes numerically unstable as Theta approaches zero.
         // For near-zero cases we use the first 3 terms of the Taylor series expansion for sin/cos. We check if fourth term is less than machine precision -
         // then we can safely use the "low angle" approximated version without loss of accuracy.
-        if (thetaMagnitudeSq < fast_fsqrtf(24.0f * 1e-6f)) {
+        /* Opt 2: original condition was "thetaMagnitudeSq < sqrt(24e-6)".
+         * Squaring both sides (both are non-negative) gives an equivalent
+         * condition without a sqrt() call: thetaMagnitudeSq² < 24e-6. */
+        if (thetaMagnitudeSq * thetaMagnitudeSq < 24.0e-6f) {
             quaternionScale(&deltaQ, &deltaQ, 1.0f - thetaMagnitudeSq / 6.0f);
             deltaQ.q0 = 1.0f - thetaMagnitudeSq / 2.0f;
         }
@@ -563,7 +592,20 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
 
         // Calculate final orientation and renormalize
         quaternionMultiply(&orientation, &orientation, &deltaQ);
-        quaternionNormalize(&orientation, &orientation);
+        /* Opt 3: first-order Newton renormalization avoids sqrt() and 4 divides.
+         * At 1 kHz the quaternion norm drifts by < 1e-6 per step, so normSq = 1 + ε
+         * with |ε| ≪ 1.  The identity 1/sqrt(x) ≈ (3-x)/2 is accurate to O(ε²) ≈ 1e-12
+         * — well within float precision.  imuCheckAndResetOrientationQuaternion() below
+         * catches any catastrophic norm deviation that this approximation cannot correct. */
+        {
+            const float normSq = orientation.q0 * orientation.q0 + orientation.q1 * orientation.q1
+                               + orientation.q2 * orientation.q2 + orientation.q3 * orientation.q3;
+            const float scale = (3.0f - normSq) * 0.5f;
+            orientation.q0 *= scale;
+            orientation.q1 *= scale;
+            orientation.q2 *= scale;
+            orientation.q3 *= scale;
+        }
     }
 
     // Check for invalid quaternion and reset to previous known good one

src/main/flight/imu.h

@@ -65,6 +65,11 @@ typedef struct imuRuntimeConfig_s {
     float dcm_ki_acc;
     float dcm_kp_mag;
     float dcm_ki_mag;
+    /* Precomputed anti-windup limit for imuMahonyAHRSupdate(): equals
+     * DEGREES_TO_RADIANS(2) * (dcm_kp_acc + dcm_kp_mag) / 2.
+     * Updated once by imuConfigure() whenever settings are saved, so the
+     * hot PID path reads a single float instead of doing arithmetic. */
+    float dcm_i_limit;
     uint8_t small_angle;
 } imuRuntimeConfig_t;