Migrate IMU and PID to fastram

src/main/flight/imu.c

@@ -75,22 +75,22 @@
 #define MAX_ACC_SQ_NEARNESS         25      // 25% or G^2, accepted acceleration of (0.87 - 1.12G)
 #define MAX_GPS_HEADING_ERROR_DEG   60      // Amount of error between GPS CoG and estimated Yaw at witch we stop trusting GPS and fallback to MAG
 
-t_fp_vector imuMeasuredAccelBF;
+FASTRAM t_fp_vector imuMeasuredAccelBF;
-t_fp_vector imuMeasuredRotationBF;
+FASTRAM t_fp_vector imuMeasuredRotationBF;
-static float smallAngleCosZ = 0;
+STATIC_FASTRAM float smallAngleCosZ;
 
-static bool isAccelUpdatedAtLeastOnce = false;
+STATIC_FASTRAM bool isAccelUpdatedAtLeastOnce;
 
-STATIC_UNIT_TESTED float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;    // quaternion of sensor frame relative to earth frame
+STATIC_FASTRAM_UNIT_TESTED float q0, q1, q2, q3;    // quaternion of sensor frame relative to earth frame
-STATIC_UNIT_TESTED float rMat[3][3];
+STATIC_FASTRAM_UNIT_TESTED float rMat[3][3];
 
-attitudeEulerAngles_t attitude = { { 0, 0, 0 } };     // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
+FASTRAM attitudeEulerAngles_t attitude;             // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
 
-static imuRuntimeConfig_t imuRuntimeConfig;
+STATIC_FASTRAM imuRuntimeConfig_t imuRuntimeConfig;
 
-static const float gyroScale = (M_PIf / 180.0f);  // gyro output scaled to rad per second
+static const float gyroScale = (M_PIf / 180.0f);    // gyro output scaled to rad per second
 
-static bool gpsHeadingInitialized = false;
+STATIC_FASTRAM bool gpsHeadingInitialized;
 
 PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0);
 
@@ -100,15 +100,14 @@ PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
     .dcm_kp_mag = 10000,            // 1.00 * 10000
     .dcm_ki_mag = 0,                // 0.00 * 10000
     .small_angle = 25
-
 );
 
 #ifdef ASYNC_GYRO_PROCESSING
 /* Asynchronous update accumulators */
-static float imuAccumulatedRate[XYZ_AXIS_COUNT];
+STATIC_FASTRAM float imuAccumulatedRate[XYZ_AXIS_COUNT];
-static timeUs_t imuAccumulatedRateTimeUs;
+STATIC_FASTRAM timeUs_t imuAccumulatedRateTimeUs;
-static float imuAccumulatedAcc[XYZ_AXIS_COUNT];
+STATIC_FASTRAM float imuAccumulatedAcc[XYZ_AXIS_COUNT];
-static int   imuAccumulatedAccCount;
+STATIC_FASTRAM int   imuAccumulatedAccCount;
 #endif
 
 #ifdef ASYNC_GYRO_PROCESSING
@@ -168,6 +167,14 @@ void imuInit(void)
         imuMeasuredAccelBF.A[axis] = 0;
     }
 
+    // Explicitly initialize FASTRAM statics
+    isAccelUpdatedAtLeastOnce = false;
+    gpsHeadingInitialized = false;
+    q0 = 1.0f;
+    q1 = 0.0f;
+    q2 = 0.0f;
+    q3 = 0.0f;
+
     imuComputeRotationMatrix();
 }
 
@@ -284,8 +291,8 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
                                 bool useMag, float mx, float my, float mz,
                                 bool useCOG, float courseOverGround)
 {
-    static float integralAccX = 0.0f,  integralAccY = 0.0f, integralAccZ = 0.0f;    // integral error terms scaled by Ki
+    STATIC_FASTRAM float integralAccX = 0.0f,  integralAccY = 0.0f, integralAccZ = 0.0f;    // integral error terms scaled by Ki
-    static float integralMagX = 0.0f,  integralMagY = 0.0f, integralMagZ = 0.0f;    // integral error terms scaled by Ki
+    STATIC_FASTRAM float integralMagX = 0.0f,  integralMagY = 0.0f, integralMagZ = 0.0f;    // integral error terms scaled by Ki
     float ex, ey, ez;
 
     /* Calculate general spin rate (rad/s) */

src/main/flight/pid.c

@@ -87,24 +87,24 @@ typedef struct {
 } pidState_t;
 
 #ifdef USE_DTERM_NOTCH
-    static filterApplyFnPtr notchFilterApplyFn;
+STATIC_FASTRAM filterApplyFnPtr notchFilterApplyFn;
 #endif
 
 extern float dT;
 
-float headingHoldCosZLimit;
+FASTRAM float headingHoldCosZLimit;
-int16_t headingHoldTarget;
+FASTRAM int16_t headingHoldTarget;
-static pt1Filter_t headingHoldRateFilter;
+STATIC_FASTRAM pt1Filter_t headingHoldRateFilter;
 
 // Thrust PID Attenuation factor. 0.0f means fully attenuated, 1.0f no attenuation is applied
-static bool pidGainsUpdateRequired = false;
+STATIC_FASTRAM bool pidGainsUpdateRequired;
-int16_t axisPID[FLIGHT_DYNAMICS_INDEX_COUNT];
+FASTRAM int16_t axisPID[FLIGHT_DYNAMICS_INDEX_COUNT];
 
 #ifdef BLACKBOX
 int32_t axisPID_P[FLIGHT_DYNAMICS_INDEX_COUNT], axisPID_I[FLIGHT_DYNAMICS_INDEX_COUNT], axisPID_D[FLIGHT_DYNAMICS_INDEX_COUNT], axisPID_Setpoint[FLIGHT_DYNAMICS_INDEX_COUNT];
 #endif
 
-static pidState_t pidState[FLIGHT_DYNAMICS_INDEX_COUNT];
+STATIC_FASTRAM pidState_t pidState[FLIGHT_DYNAMICS_INDEX_COUNT];
 
 PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 3);
 
@@ -206,6 +206,8 @@ void pidInit(void)
     // Calculate max overall tilt (max pitch + max roll combined) as a limit to heading hold
     headingHoldCosZLimit = cos_approx(DECIDEGREES_TO_RADIANS(pidProfile()->max_angle_inclination[FD_ROLL])) *
                            cos_approx(DECIDEGREES_TO_RADIANS(pidProfile()->max_angle_inclination[FD_PITCH]));
+
+    pidGainsUpdateRequired = false;
 }
 
 #ifdef USE_DTERM_NOTCH
@@ -313,7 +315,7 @@ void schedulePidGainsUpdate(void)
 
 void updatePIDCoefficients(void)
 {
-    static uint16_t prevThrottle = 0;
+    STATIC_FASTRAM uint16_t prevThrottle = 0;
 
     // Check if throttle changed
     if (rcCommand[THROTTLE] != prevThrottle) {