[imu] imu_scale_x functions

paparazzi · Oct 9, 2014 · e226166 · e226166
1 parent a54359c
commit e226166
Show file tree

Hide file tree

Showing 28 changed files with 219 additions and 349 deletions.
diff --git a/sw/airborne/firmwares/beth/main_overo.c b/sw/airborne/firmwares/beth/main_overo.c
@@ -118,7 +118,7 @@ static void main_periodic(int my_sig_num) {
 
   main_talk_with_stm32();
 
-  ImuScaleGyro(imu);
+  imu_scale_gyro(&imu);
 
   RunOnceEvery(50, {DOWNLINK_SEND_BETH(gcs_com.udp_transport,
       &msg_in.payload.msg_up.bench_sensor.x,&msg_in.payload.msg_up.bench_sensor.y,

diff --git a/sw/airborne/firmwares/beth/main_stm32.c b/sw/airborne/firmwares/beth/main_stm32.c
@@ -154,8 +154,8 @@ static inline void on_accel_event(void) {
 }
 
 static inline void on_gyro_accel_event(void) {
-  ImuScaleGyro(imu);
-  ImuScaleAccel(imu);
+  imu_scale_gyro(&imu);
+  imu_scale_accel(&imu);
 
   //LED_TOGGLE(2);
   static uint8_t cnt;
@@ -188,7 +188,7 @@ static inline void on_gyro_accel_event(void) {
 
 
 static inline void on_mag_event(void) {
-  ImuScaleMag(imu);
+  imu_scale_mag(&imu);
   static uint8_t cnt;
   cnt++;
   if (cnt > 1) cnt = 0;

diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -737,7 +737,7 @@ PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
   const float dt = 1./AHRS_CORRECT_FREQUENCY;
 #endif
 
-  ImuScaleAccel(imu);
+  imu_scale_accel(&imu);
   if (ahrs.status != AHRS_UNINIT) {
     ahrs_update_accel(dt);
   }
@@ -761,7 +761,7 @@ PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
 
   ahrs_timeout_counter = 0;
 
-  ImuScaleGyro(imu);
+  imu_scale_gyro(&imu);
 
 #if USE_AHRS_ALIGNER
   // Run aligner on raw data as it also makes averages.
@@ -803,7 +803,7 @@ PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
   const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY);
 #endif
 
-  ImuScaleMag(imu);
+  imu_scale_mag(&imu);
   if (ahrs.status == AHRS_RUNNING) {
     ahrs_update_mag(dt);
   }

diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c
@@ -322,7 +322,7 @@ PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
   const float dt = 1. / (AHRS_CORRECT_FREQUENCY);
 #endif
 
-  ImuScaleAccel(imu);
+  imu_scale_accel(&imu);
 
   if (ahrs.status != AHRS_UNINIT) {
     ahrs_update_accel(dt);
@@ -345,7 +345,7 @@ PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
   const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
 #endif
 
-  ImuScaleGyro(imu);
+  imu_scale_gyro(&imu);
 
   if (ahrs.status == AHRS_UNINIT) {
     ahrs_aligner_run();
@@ -374,7 +374,7 @@ static inline void on_gps_event(void) {
 }
 
 static inline void on_mag_event(void) {
-  ImuScaleMag(imu);
+  imu_scale_mag(&imu);
 
 #if USE_MAGNETOMETER
 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY)

diff --git a/sw/airborne/modules/sensors/mag_hmc58xx.c b/sw/airborne/modules/sensors/mag_hmc58xx.c
@@ -80,7 +80,7 @@ PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
     // unscaled vector
     VECT3_COPY(imu.mag_unscaled, mag);
     // scale vector
-    ImuScaleMag(imu);
+    imu_scale_mag(&imu);
     // update ahrs
     if (ahrs.status == AHRS_RUNNING) {
 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY)

diff --git a/sw/airborne/subsystems/ahrs/ahrs_gx3.c b/sw/airborne/subsystems/ahrs/ahrs_gx3.c
@@ -335,3 +335,7 @@ void ahrs_aligner_run(void) {
 void ahrs_aligner_init(void) {
 }
 
+/* no scaling */
+void imu_scale_gyro(struct Imu* _imu __attribute__((unused))) {}
+void imu_scale_accel(struct Imu* _imu __attribute__((unused))) {}
+void imu_scale_mag(struct Imu* _imu __attribute__((unused))) {}
diff --git a/sw/airborne/subsystems/ahrs/ahrs_gx3.h b/sw/airborne/subsystems/ahrs/ahrs_gx3.h
@@ -43,21 +43,6 @@
 #include "state.h"
 #include "led.h"
 
-#ifdef ImuScaleGyro
-#undef ImuScaleGyro
-#endif
-#define ImuScaleGyro(_imu) {}
-
-#ifdef ImuScaleAccel
-#undef ImuScaleAccel
-#endif
-#define ImuScaleAccel(_imu) {}
-
-#ifdef ImuScaleMag
-#undef ImuScaleMag
-#endif
-#define ImuScaleMag(_imu) {}
-
 #define GX3_MAX_PAYLOAD 128
 #define GX3_MSG_LEN 67
 #define GX3_HEADER 0xC8

diff --git a/sw/airborne/subsystems/imu.c b/sw/airborne/subsystems/imu.c
@@ -222,3 +222,58 @@ void imu_SetBodyToImuCurrent(float set) {
 #endif
   }
 }
+
+
+#define WEAK __attribute__((weak))
+// weak functions, used if not explicitly provided by implementation
+
+void WEAK imu_scale_gyro(struct Imu* _imu)
+{
+  RATES_COPY(_imu->gyro_prev, _imu->gyro);
+  _imu->gyro.p = ((_imu->gyro_unscaled.p - _imu->gyro_neutral.p) * IMU_GYRO_P_SIGN *
+                  IMU_GYRO_P_SENS_NUM) / IMU_GYRO_P_SENS_DEN;
+  _imu->gyro.q = ((_imu->gyro_unscaled.q - _imu->gyro_neutral.q) * IMU_GYRO_Q_SIGN *
+                  IMU_GYRO_Q_SENS_NUM) / IMU_GYRO_Q_SENS_DEN;
+  _imu->gyro.r = ((_imu->gyro_unscaled.r - _imu->gyro_neutral.r) * IMU_GYRO_R_SIGN *
+                  IMU_GYRO_R_SENS_NUM) / IMU_GYRO_R_SENS_DEN;
+}
+
+void WEAK imu_scale_accel(struct Imu* _imu)
+{
+  VECT3_COPY(_imu->accel_prev, _imu->accel);
+  _imu->accel.x = ((_imu->accel_unscaled.x - _imu->accel_neutral.x) * IMU_ACCEL_X_SIGN *
+                   IMU_ACCEL_X_SENS_NUM) / IMU_ACCEL_X_SENS_DEN;
+  _imu->accel.y = ((_imu->accel_unscaled.y - _imu->accel_neutral.y) * IMU_ACCEL_Y_SIGN *
+                   IMU_ACCEL_Y_SENS_NUM) / IMU_ACCEL_Y_SENS_DEN;
+  _imu->accel.z = ((_imu->accel_unscaled.z - _imu->accel_neutral.z) * IMU_ACCEL_Z_SIGN *
+                   IMU_ACCEL_Z_SENS_NUM) / IMU_ACCEL_Z_SENS_DEN;
+}
+
+#if defined IMU_MAG_X_CURRENT_COEF && defined IMU_MAG_Y_CURRENT_COEF && defined IMU_MAG_Z_CURRENT_COEF
+#include "subsystems/electrical.h"
+void WEAK imu_scale_mag(struct Imu* _imu)
+{
+  struct Int32Vect3 mag_correction;
+  mag_correction.x = (int32_t)(IMU_MAG_X_CURRENT_COEF * (float) electrical.current);
+  mag_correction.y = (int32_t)(IMU_MAG_Y_CURRENT_COEF * (float) electrical.current);
+  mag_correction.z = (int32_t)(IMU_MAG_Z_CURRENT_COEF * (float) electrical.current);
+  _imu->mag.x = (((_imu->mag_unscaled.x - mag_correction.x) - _imu->mag_neutral.x) *
+                 IMU_MAG_X_SIGN * IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN;
+  _imu->mag.y = (((_imu->mag_unscaled.y - mag_correction.y) - _imu->mag_neutral.y) *
+                 IMU_MAG_Y_SIGN * IMU_MAG_Y_SENS_NUM) / IMU_MAG_Y_SENS_DEN;
+  _imu->mag.z = (((_imu->mag_unscaled.z - mag_correction.z) - _imu->mag_neutral.z) *
+                 IMU_MAG_Z_SIGN * IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN;
+}
+#elif USE_MAGNETOMETER
+void WEAK imu_scale_mag(struct Imu* _imu)
+{
+  _imu->mag.x = ((_imu->mag_unscaled.x - _imu->mag_neutral.x) * IMU_MAG_X_SIGN *
+                 IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN;
+  _imu->mag.y = ((_imu->mag_unscaled.y - _imu->mag_neutral.y) * IMU_MAG_Y_SIGN *
+                 IMU_MAG_Y_SENS_NUM) / IMU_MAG_Y_SENS_DEN;
+  _imu->mag.z = ((_imu->mag_unscaled.z - _imu->mag_neutral.z) * IMU_MAG_Z_SIGN *
+                 IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN;
+}
+#else
+void WEAK imu_scale_mag(struct Imu* _imu __attribute__((unused))) {}
+#endif /* MAG_x_CURRENT_COEF */
diff --git a/sw/airborne/subsystems/imu.h b/sw/airborne/subsystems/imu.h
@@ -85,6 +85,11 @@ extern void imu_SetBodyToImuPsi(float psi);
 extern void imu_SetBodyToImuCurrent(float set);
 extern void imu_ResetBodyToImu(float reset);
 
+/* can be provided implementation */
+extern void imu_scale_gyro(struct Imu* _imu);
+extern void imu_scale_accel(struct Imu* _imu);
+extern void imu_scale_mag(struct Imu* _imu);
+
 #if !defined IMU_BODY_TO_IMU_PHI && !defined IMU_BODY_TO_IMU_THETA && !defined IMU_BODY_TO_IMU_PSI
 #define IMU_BODY_TO_IMU_PHI   0
 #define IMU_BODY_TO_IMU_THETA 0
@@ -103,53 +108,21 @@ extern void imu_ResetBodyToImu(float reset);
 #define IMU_ACCEL_Z_NEUTRAL 0
 #endif
 
-
-#ifndef ImuScaleGyro
-#define ImuScaleGyro(_imu) {					\
-    RATES_COPY(_imu.gyro_prev, _imu.gyro);				\
-    _imu.gyro.p = ((_imu.gyro_unscaled.p - _imu.gyro_neutral.p)*IMU_GYRO_P_SIGN*IMU_GYRO_P_SENS_NUM)/IMU_GYRO_P_SENS_DEN; \
-    _imu.gyro.q = ((_imu.gyro_unscaled.q - _imu.gyro_neutral.q)*IMU_GYRO_Q_SIGN*IMU_GYRO_Q_SENS_NUM)/IMU_GYRO_Q_SENS_DEN; \
-    _imu.gyro.r = ((_imu.gyro_unscaled.r - _imu.gyro_neutral.r)*IMU_GYRO_R_SIGN*IMU_GYRO_R_SENS_NUM)/IMU_GYRO_R_SENS_DEN; \
-  }
+#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
+#define IMU_GYRO_P_SIGN   1
+#define IMU_GYRO_Q_SIGN   1
+#define IMU_GYRO_R_SIGN   1
 #endif
-
-
-#ifndef ImuScaleAccel
-#define ImuScaleAccel(_imu) {					\
-    VECT3_COPY(_imu.accel_prev, _imu.accel);				\
-    _imu.accel.x = ((_imu.accel_unscaled.x - _imu.accel_neutral.x)*IMU_ACCEL_X_SIGN*IMU_ACCEL_X_SENS_NUM)/IMU_ACCEL_X_SENS_DEN; \
-    _imu.accel.y = ((_imu.accel_unscaled.y - _imu.accel_neutral.y)*IMU_ACCEL_Y_SIGN*IMU_ACCEL_Y_SENS_NUM)/IMU_ACCEL_Y_SENS_DEN; \
-    _imu.accel.z = ((_imu.accel_unscaled.z - _imu.accel_neutral.z)*IMU_ACCEL_Z_SIGN*IMU_ACCEL_Z_SENS_NUM)/IMU_ACCEL_Z_SENS_DEN; \
-  }
+#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
+#define IMU_ACCEL_X_SIGN  1
+#define IMU_ACCEL_Y_SIGN  1
+#define IMU_ACCEL_Z_SIGN  1
+#endif
+#if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
+#define IMU_MAG_X_SIGN 1
+#define IMU_MAG_Y_SIGN 1
+#define IMU_MAG_Z_SIGN 1
 #endif
-
-#ifndef ImuScaleMag
-#if defined IMU_MAG_45_HACK
-#define ImuScaleMag(_imu) {						\
-    int32_t msx = ((_imu.mag_unscaled.x - _imu.mag_neutral.x) * IMU_MAG_X_SIGN * IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN; \
-    int32_t msy = ((_imu.mag_unscaled.y - _imu.mag_neutral.y) * IMU_MAG_Y_SIGN * IMU_MAG_Y_SENS_NUM) / IMU_MAG_Y_SENS_DEN; \
-    _imu.mag.x = msx - msy;						\
-    _imu.mag.y = msx + msy;						\
-    _imu.mag.z = ((_imu.mag_unscaled.z - _imu.mag_neutral.z) * IMU_MAG_Z_SIGN * IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN; \
-  }
-#elif defined IMU_MAG_X_CURRENT_COEF && defined IMU_MAG_Y_CURRENT_COEF && defined IMU_MAG_Z_CURRENT_COEF
-#define ImuScaleMag(_imu) {						\
-    struct Int32Vect3 _mag_correction;                                  \
-    _mag_correction.x = (int32_t) (IMU_MAG_X_CURRENT_COEF * (float) electrical.current); \
-    _mag_correction.y = (int32_t) (IMU_MAG_Y_CURRENT_COEF * (float) electrical.current); \
-    _mag_correction.z = (int32_t) (IMU_MAG_Z_CURRENT_COEF * (float) electrical.current); \
-    _imu.mag.x = (((_imu.mag_unscaled.x - _mag_correction.x) - _imu.mag_neutral.x) * IMU_MAG_X_SIGN * IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN; \
-    _imu.mag.y = (((_imu.mag_unscaled.y - _mag_correction.y) - _imu.mag_neutral.y) * IMU_MAG_Y_SIGN * IMU_MAG_Y_SENS_NUM) / IMU_MAG_Y_SENS_DEN; \
-    _imu.mag.z = (((_imu.mag_unscaled.z - _mag_correction.z) - _imu.mag_neutral.z) * IMU_MAG_Z_SIGN * IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN; \
- }
-#else
-#define ImuScaleMag(_imu) {                                             \
-    _imu.mag.x = ((_imu.mag_unscaled.x - _imu.mag_neutral.x) * IMU_MAG_X_SIGN * IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN; \
-    _imu.mag.y = ((_imu.mag_unscaled.y - _imu.mag_neutral.y) * IMU_MAG_Y_SIGN * IMU_MAG_Y_SENS_NUM) / IMU_MAG_Y_SENS_DEN; \
-    _imu.mag.z = ((_imu.mag_unscaled.z - _imu.mag_neutral.z) * IMU_MAG_Z_SIGN * IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN; \
-  }
-#endif //IMU_MAG_45_HACK
-#endif //ImuScaleMag
 
 
 #endif /* IMU_H */
diff --git a/sw/airborne/subsystems/imu/imu_analog.c b/sw/airborne/subsystems/imu/imu_analog.c
@@ -84,3 +84,26 @@ void imu_periodic(void) {
 
   analog_imu_available = TRUE;
 }
+
+// if not all gyros are used, override the imu_scale_gyro handler
+#if defined ADC_CHANNEL_GYRO_P && defined ADC_CHANNEL_GYRO_Q && ! defined ADC_CHANNEL_GYRO_R
+
+void imu_scale_gyro(struct Imu* _imu)
+{
+  _imu->gyro.p = ((_imu->gyro_unscaled.p - _imu->gyro_neutral.p)*IMU_GYRO_P_SIGN*IMU_GYRO_P_SENS_NUM)/IMU_GYRO_P_SENS_DEN;
+  _imu->gyro.q = ((_imu->gyro_unscaled.q - _imu->gyro_neutral.q)*IMU_GYRO_Q_SIGN*IMU_GYRO_Q_SENS_NUM)/IMU_GYRO_Q_SENS_DEN;
+}
+
+#elif defined ADC_CHANNEL_GYRO_P && ! defined ADC_CHANNEL_GYRO_Q && ! defined ADC_CHANNEL_GYRO_R
+
+void imu_scale_gyro(struct Imu* _imu)
+{
+  _imu->gyro.p = ((_imu->gyro_unscaled.p - _imu->gyro_neutral.p)*IMU_GYRO_P_SIGN*IMU_GYRO_P_SENS_NUM)/IMU_GYRO_P_SENS_DEN;
+}
+
+#endif
+
+// if we don't have any accelerometers, set an empty imu_scale_accel handler
+#if ! defined ADC_CHANNEL_ACCEL_X && ! defined ADC_CHANNEL_ACCEL_Z && ! defined ADC_CHANNEL_ACCEL_Z
+void imu_scale_accel(struct Imu* _imu __attribute__((unused))) {}
+#endif
diff --git a/sw/airborne/subsystems/imu/imu_analog.h b/sw/airborne/subsystems/imu/imu_analog.h
@@ -19,61 +19,68 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/**
+ * @file subsystems/imu/imu_analog.h
+ * Inertial Measurement Unit using onboard ADCs.
+ */
+
 #ifndef IMU_ANALOG_H
 #define IMU_ANALOG_H
 
 
 #define NB_ANALOG_IMU_ADC 6
 
-// if not all gyros are used, override the ImuScaleGyro handler
+// if not all gyros are used, override the imu_scale_gyro handler
 #if defined ADC_CHANNEL_GYRO_P && defined ADC_CHANNEL_GYRO_Q && ! defined ADC_CHANNEL_GYRO_R
 
+#define IMU_GYRO_R_SIGN 1
 #define IMU_GYRO_R_NEUTRAL 0
-#define ImuScaleGyro(_imu) {                                            \
-    _imu.gyro.p = ((_imu.gyro_unscaled.p - _imu.gyro_neutral.p)*IMU_GYRO_P_SIGN*IMU_GYRO_P_SENS_NUM)/IMU_GYRO_P_SENS_DEN; \
-    _imu.gyro.q = ((_imu.gyro_unscaled.q - _imu.gyro_neutral.q)*IMU_GYRO_Q_SIGN*IMU_GYRO_Q_SENS_NUM)/IMU_GYRO_Q_SENS_DEN; \
-  }
+#define IMU_GYRO_R_SENS_NUM 1
+#define IMU_GYRO_R_SENS_DEN 1
 
 #elif defined ADC_CHANNEL_GYRO_P && ! defined ADC_CHANNEL_GYRO_Q && ! defined ADC_CHANNEL_GYRO_R
 
+#define IMU_GYRO_Q_SIGN 1
 #define IMU_GYRO_Q_NEUTRAL 0
+#define IMU_GYRO_Q_SENS_NUM 1
+#define IMU_GYRO_Q_SENS_DEN 1
+#define IMU_GYRO_R_SIGN 1
 #define IMU_GYRO_R_NEUTRAL 0
-#define ImuScaleGyro(_imu) {                                            \
-    _imu.gyro.p = ((_imu.gyro_unscaled.p - _imu.gyro_neutral.p)*IMU_GYRO_P_SIGN*IMU_GYRO_P_SENS_NUM)/IMU_GYRO_P_SENS_DEN; \
-  }
+#define IMU_GYRO_R_SENS_NUM 1
+#define IMU_GYRO_R_SENS_DEN 1
 
 #endif
 
-// if we don't have any accelerometers, set an empty ImuScaleAccel handler
 #if ! defined ADC_CHANNEL_ACCEL_X && ! defined ADC_CHANNEL_ACCEL_Z && ! defined ADC_CHANNEL_ACCEL_Z
-#define ImuScaleAccel(_imu) {}
+
+#define IMU_ACCEL_X_SENS_NUM 1
+#define IMU_ACCEL_X_SENS_DEN 1
+#define IMU_ACCEL_Y_SENS_NUM 1
+#define IMU_ACCEL_Y_SENS_DEN 1
+#define IMU_ACCEL_Z_SENS_NUM 1
+#define IMU_ACCEL_Z_SENS_DEN 1
+
 #endif
 
+
 /*
- * we include imh.h after the definitions of ImuScale so we can override the default handlers
+ * we include imh.h after the definitions of the neutrals
  */
 #include "subsystems/imu.h"
 
 
 extern volatile bool_t analog_imu_available;
 extern int imu_overrun;
 
-#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) {   \
-    if (analog_imu_available) {                         \
-      analog_imu_available = FALSE;                     \
-      _gyro_handler();                            \
-      _accel_handler();                            \
-    }                                                   \
-    ImuMagEvent(_mag_handler);                          \
-  }
-
-#define ImuMagEvent(_mag_handler) {             \
-    if (0) {                                    \
-      _mag_handler();                           \
-    }                                           \
+static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void),
+                            void (* _mag_handler)(void) __attribute__((unused)))
+{
+  if (analog_imu_available) {
+    analog_imu_available = FALSE;
+    _gyro_handler();
+    _accel_handler();
   }
-
-
+}
 
 
 #endif /* IMU_ANALOG_H */