diff --git a/sw/airborne/subsystems/imu/imu_aspirin2.h b/sw/airborne/subsystems/imu/imu_aspirin2.h index 9a5e53eba1..14b64de6f9 100644 --- a/sw/airborne/subsystems/imu/imu_aspirin2.h +++ b/sw/airborne/subsystems/imu/imu_aspirin2.h @@ -50,6 +50,51 @@ #define IMU_ACCEL_Z_SIGN 1 #endif +/** default gyro sensitivy and neutral from the datasheet + * MPU60X0 has 16.4 LSB/(deg/s) at 2000deg/s range + * sens = 1/16.4 * pi/180 * 2^INT32_RATE_FRAC + * sens = 1/16.4 * pi/180 * 4096 = 4.359066229 + */ +#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS +#define IMU_GYRO_P_SENS 4.359 +#define IMU_GYRO_P_SENS_NUM 4359 +#define IMU_GYRO_P_SENS_DEN 1000 +#define IMU_GYRO_Q_SENS 4.359 +#define IMU_GYRO_Q_SENS_NUM 4359 +#define IMU_GYRO_Q_SENS_DEN 1000 +#define IMU_GYRO_R_SENS 4.359 +#define IMU_GYRO_R_SENS_NUM 4359 +#define IMU_GYRO_R_SENS_DEN 1000 +#endif +#if !defined IMU_GYRO_P_NEUTRAL & !defined IMU_GYRO_Q_NEUTRAL & !defined IMU_GYRO_R_NEUTRAL +#define IMU_GYRO_P_NEUTRAL 0 +#define IMU_GYRO_Q_NEUTRAL 0 +#define IMU_GYRO_R_NEUTRAL 0 +#endif + +/** default accel sensitivy from the datasheet + * MPU60X0 has 2048 LSB/g + * fixed point sens: 9.81 [m/s^2] / 2048 [LSB/g] * 2^INT32_ACCEL_FRAC + * sens = 9.81 / 2048 * 1024 = 4.905 + */ +#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS +#define IMU_ACCEL_X_SENS 4.905 +#define IMU_ACCEL_X_SENS_NUM 4905 +#define IMU_ACCEL_X_SENS_DEN 1000 +#define IMU_ACCEL_Y_SENS 4.905 +#define IMU_ACCEL_Y_SENS_NUM 4905 +#define IMU_ACCEL_Y_SENS_DEN 1000 +#define IMU_ACCEL_Z_SENS 4.905 +#define IMU_ACCEL_Z_SENS_NUM 4905 +#define IMU_ACCEL_Z_SENS_DEN 1000 +#endif +#if !defined IMU_ACCEL_X_NEUTRAL & !defined IMU_ACCEL_Y_NEUTRAL & !defined IMU_ACCEL_Z_NEUTRAL +#define IMU_ACCEL_X_NEUTRAL 0 +#define IMU_ACCEL_Y_NEUTRAL 0 +#define IMU_ACCEL_Z_NEUTRAL 0 +#endif + + enum Aspirin2Status { Aspirin2StatusUninit, Aspirin2StatusIdle,