Update Mahony_AHRS.h

bareboat-necessities · Aug 31, 2024 · 8b2e5fe · 8b2e5fe
1 parent 127c01e
commit 8b2e5fe
Showing 1 changed file with 8 additions and 0 deletions.
diff --git a/bbn_wave_freq_m5atomS3/Mahony_AHRS.h b/bbn_wave_freq_m5atomS3/Mahony_AHRS.h
@@ -25,11 +25,18 @@ typedef struct mahony_AHRS_vars {
   float integralFBz = 0.0f;  // integral error terms scaled by Ki
 } Mahony_AHRS_Vars;
 
+void mahony_AHRS_init(Mahony_AHRS_Vars* m, float twoKp, float twoKi);
+
 void mahony_AHRS_update(Mahony_AHRS_Vars* m,
                         float gx, float gy, float gz, float ax, float ay, float az, 
                         float *pitch, float *roll, float *yaw, float delta_t_sec);
 float invSqrt(float x);
 
+void mahony_AHRS_init(Mahony_AHRS_Vars* m, float twoKp, float twoKi) {
+  m->twoKp = twoKp;
+  m->twoKi = twoKi;
+}
+
 // IMU algorithm update (without magnetometer)
 void mahony_AHRS_update(Mahony_AHRS_Vars* m,
                         float gx, float gy, float gz, float ax, float ay, float az,
@@ -125,3 +132,4 @@ float invSqrt(float x) {
 #pragma GCC diagnostic pop
 
 #endif
+