diff --git a/conf/airframes/ardrone2_raw.xml b/conf/airframes/ardrone2_raw.xml index f72c47161c..1a4f10df16 100644 --- a/conf/airframes/ardrone2_raw.xml +++ b/conf/airframes/ardrone2_raw.xml @@ -72,35 +72,11 @@ - - - - - - - - - - - - - - - - - - - - - - - + + - - - diff --git a/sw/airborne/subsystems/imu/imu_ardrone2_raw.h b/sw/airborne/subsystems/imu/imu_ardrone2_raw.h index 99a9f39d52..18b1b1bbad 100644 --- a/sw/airborne/subsystems/imu/imu_ardrone2_raw.h +++ b/sw/airborne/subsystems/imu/imu_ardrone2_raw.h @@ -31,6 +31,83 @@ #include "generated/airframe.h" #include "navdata.h" +#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 +#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 +#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 + +/** default gyro sensitivy and neutral from the datasheet + * MPU with 2000 deg/s + */ +#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 + * 512 LSB/g + */ +#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS +#define IMU_ACCEL_X_SENS 19.5 +#define IMU_ACCEL_X_SENS_NUM 195 +#define IMU_ACCEL_X_SENS_DEN 10 +#define IMU_ACCEL_Y_SENS 19.5 +#define IMU_ACCEL_Y_SENS_NUM 195 +#define IMU_ACCEL_Y_SENS_DEN 10 +#define IMU_ACCEL_Z_SENS 19.5 +#define IMU_ACCEL_Z_SENS_NUM 195 +#define IMU_ACCEL_Z_SENS_DEN 10 +#endif + +#if !defined IMU_ACCEL_X_NEUTRAL & !defined IMU_ACCEL_Y_NEUTRAL & !defined IMU_ACCEL_Z_NEUTRAL +#define IMU_ACCEL_X_NEUTRAL 2048 +#define IMU_ACCEL_Y_NEUTRAL 2048 +#define IMU_ACCEL_Z_NEUTRAL 2048 +#endif + +#if !defined IMU_MAG_X_SENS & !defined IMU_MAG_Y_SENS & !defined IMU_MAG_Z_SENS +#define IMU_MAG_X_SENS 16.0 +#define IMU_MAG_X_SENS_NUM 16 +#define IMU_MAG_X_SENS_DEN 1 +#define IMU_MAG_Y_SENS 16.0 +#define IMU_MAG_Y_SENS_NUM 16 +#define IMU_MAG_Y_SENS_DEN 1 +#define IMU_MAG_Z_SENS 16.0 +#define IMU_MAG_Z_SENS_NUM 16 +#define IMU_MAG_Z_SENS_DEN 1 +#endif + +#if !defined IMU_MAG_X_NEUTRAL & !defined IMU_MAG_Y_NEUTRAL & !defined IMU_MAG_Z_NEUTRAL +#define IMU_MAG_X_NEUTRAL 0 +#define IMU_MAG_Y_NEUTRAL 0 +#define IMU_MAG_Z_NEUTRAL 0 +#endif + + + void navdata_event(void); static inline void imu_ardrone2_event ( void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void))