diff --git a/conf/airframes/tudelft/nederdrone4.xml b/conf/airframes/tudelft/nederdrone4.xml index ad7e066ffe..9af11e4934 100644 --- a/conf/airframes/tudelft/nederdrone4.xml +++ b/conf/airframes/tudelft/nederdrone4.xml @@ -49,7 +49,7 @@ - + @@ -88,17 +88,17 @@ - + - + - - - - - - + + + + + + + - - - - - - + + + + + + + + @@ -338,7 +341,6 @@
-
diff --git a/conf/modules/imu_apogee.xml b/conf/modules/imu_apogee.xml index 0d5274b7f8..d641ab8d85 100644 --- a/conf/modules/imu_apogee.xml +++ b/conf/modules/imu_apogee.xml @@ -26,8 +26,6 @@ - - diff --git a/conf/modules/imu_ardrone2.xml b/conf/modules/imu_ardrone2.xml index 748727917e..28f2fd972c 100644 --- a/conf/modules/imu_ardrone2.xml +++ b/conf/modules/imu_ardrone2.xml @@ -26,8 +26,6 @@ - - diff --git a/conf/modules/imu_aspirin_common.xml b/conf/modules/imu_aspirin_common.xml index 3541ea7592..8d18b1dc15 100644 --- a/conf/modules/imu_aspirin_common.xml +++ b/conf/modules/imu_aspirin_common.xml @@ -45,7 +45,6 @@ - diff --git a/conf/modules/imu_aspirin_i2c_common.xml b/conf/modules/imu_aspirin_i2c_common.xml index 1436f9cebf..314d40565e 100644 --- a/conf/modules/imu_aspirin_i2c_common.xml +++ b/conf/modules/imu_aspirin_i2c_common.xml @@ -37,8 +37,6 @@ - - diff --git a/conf/modules/imu_aspirin_v2_common.xml b/conf/modules/imu_aspirin_v2_common.xml index 763599ec75..3091c9b750 100644 --- a/conf/modules/imu_aspirin_v2_common.xml +++ b/conf/modules/imu_aspirin_v2_common.xml @@ -41,8 +41,6 @@ - - diff --git a/conf/modules/imu_bebop.xml b/conf/modules/imu_bebop.xml index b409f7321a..b73be62beb 100644 --- a/conf/modules/imu_bebop.xml +++ b/conf/modules/imu_bebop.xml @@ -36,8 +36,6 @@ - - @@ -47,7 +45,6 @@ - diff --git a/conf/modules/imu_bmi088_i2c.xml b/conf/modules/imu_bmi088_i2c.xml index 19c885bb9a..dcf3746c29 100644 --- a/conf/modules/imu_bmi088_i2c.xml +++ b/conf/modules/imu_bmi088_i2c.xml @@ -36,13 +36,10 @@ - - - diff --git a/conf/modules/imu_common.xml b/conf/modules/imu_common.xml index d73250e262..7c021da1e6 100644 --- a/conf/modules/imu_common.xml +++ b/conf/modules/imu_common.xml @@ -4,6 +4,8 @@ Common part for all IMUs. + + This takes the IMU_GYRO_RAW, IMU_ACCEL_RAW and IMU_MAG_RAW ABI messages as input. diff --git a/conf/modules/imu_cube.xml b/conf/modules/imu_cube.xml index 4a77c6b821..75fcde4995 100644 --- a/conf/modules/imu_cube.xml +++ b/conf/modules/imu_cube.xml @@ -19,8 +19,6 @@ - - diff --git a/conf/modules/imu_disco.xml b/conf/modules/imu_disco.xml index 132b3ca25b..6c864a1c7a 100644 --- a/conf/modules/imu_disco.xml +++ b/conf/modules/imu_disco.xml @@ -33,13 +33,11 @@ - - diff --git a/conf/modules/imu_mpu6000.xml b/conf/modules/imu_mpu6000.xml index e35ace8d22..2c34a9781f 100644 --- a/conf/modules/imu_mpu6000.xml +++ b/conf/modules/imu_mpu6000.xml @@ -34,13 +34,10 @@ - - - diff --git a/conf/modules/imu_mpu6000_hmc5883.xml b/conf/modules/imu_mpu6000_hmc5883.xml index c7615fb763..686f882631 100644 --- a/conf/modules/imu_mpu6000_hmc5883.xml +++ b/conf/modules/imu_mpu6000_hmc5883.xml @@ -56,14 +56,11 @@ - - - diff --git a/conf/modules/imu_mpu60x0_i2c.xml b/conf/modules/imu_mpu60x0_i2c.xml index 5f7feeb1d7..5eb49f8d53 100644 --- a/conf/modules/imu_mpu60x0_i2c.xml +++ b/conf/modules/imu_mpu60x0_i2c.xml @@ -32,8 +32,6 @@ - - diff --git a/conf/modules/imu_mpu9250_i2c.xml b/conf/modules/imu_mpu9250_i2c.xml index 9538ff027b..2f5759bfa1 100644 --- a/conf/modules/imu_mpu9250_i2c.xml +++ b/conf/modules/imu_mpu9250_i2c.xml @@ -37,14 +37,11 @@ - - - diff --git a/conf/modules/imu_mpu9250_spi.xml b/conf/modules/imu_mpu9250_spi.xml index fdc4c1685b..8c6c3b864f 100644 --- a/conf/modules/imu_mpu9250_spi.xml +++ b/conf/modules/imu_mpu9250_spi.xml @@ -44,13 +44,10 @@ - - - diff --git a/conf/modules/imu_nps.xml b/conf/modules/imu_nps.xml index d92aeceb2c..280f15bffc 100644 --- a/conf/modules/imu_nps.xml +++ b/conf/modules/imu_nps.xml @@ -17,7 +17,6 @@ - diff --git a/conf/modules/imu_openpilot_revo.xml b/conf/modules/imu_openpilot_revo.xml index 7526298997..8ae7148aeb 100644 --- a/conf/modules/imu_openpilot_revo.xml +++ b/conf/modules/imu_openpilot_revo.xml @@ -57,8 +57,6 @@ - - diff --git a/conf/modules/imu_openpilot_revo_nano.xml b/conf/modules/imu_openpilot_revo_nano.xml index 0fbb8fecdb..a12abc5ee2 100644 --- a/conf/modules/imu_openpilot_revo_nano.xml +++ b/conf/modules/imu_openpilot_revo_nano.xml @@ -42,8 +42,6 @@ - - diff --git a/conf/modules/imu_px4fmu_v1.7.xml b/conf/modules/imu_px4fmu_v1.7.xml index d55ed19947..b7d258f6d8 100644 --- a/conf/modules/imu_px4fmu_v1.7.xml +++ b/conf/modules/imu_px4fmu_v1.7.xml @@ -29,14 +29,11 @@ - - - diff --git a/conf/modules/imu_px4fmu_v2.4.xml b/conf/modules/imu_px4fmu_v2.4.xml index f1e7824e16..c02bfe4d96 100644 --- a/conf/modules/imu_px4fmu_v2.4.xml +++ b/conf/modules/imu_px4fmu_v2.4.xml @@ -37,14 +37,11 @@ - - - diff --git a/conf/modules/imu_vectornav.xml b/conf/modules/imu_vectornav.xml index 7471a1163a..627f5fb739 100644 --- a/conf/modules/imu_vectornav.xml +++ b/conf/modules/imu_vectornav.xml @@ -30,8 +30,6 @@ - - - + diff --git a/conf/modules/imu_xsens.xml b/conf/modules/imu_xsens.xml index bb8fbc137d..f530275123 100644 --- a/conf/modules/imu_xsens.xml +++ b/conf/modules/imu_xsens.xml @@ -33,9 +33,7 @@ - - diff --git a/conf/modules/ins_nps.xml b/conf/modules/ins_nps.xml index f8a9aaded0..092e56344b 100644 --- a/conf/modules/ins_nps.xml +++ b/conf/modules/ins_nps.xml @@ -19,7 +19,6 @@ - diff --git a/conf/simulator/nps/nps_sensors_params_default.h b/conf/simulator/nps/nps_sensors_params_default.h index eb105a757c..f447ee0391 100644 --- a/conf/simulator/nps/nps_sensors_params_default.h +++ b/conf/simulator/nps/nps_sensors_params_default.h @@ -53,13 +53,16 @@ #endif /* ms-2 */ /* aka 2^10/ACCEL_X_SENS */ -#define NPS_ACCEL_SENSITIVITY_XX (IMU_ACCEL_X_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_X_SENS)) -#define NPS_ACCEL_SENSITIVITY_YY (IMU_ACCEL_Y_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Y_SENS)) -#define NPS_ACCEL_SENSITIVITY_ZZ (IMU_ACCEL_Z_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Z_SENS)) +#define NPS_ACCEL_SENSITIVITY_NUM 981 +#define NPS_ACCEL_SENSITIVITY_DEN 200 +#define NPS_ACCEL_SENSITIVITY ((float)NPS_ACCEL_SENSITIVITY_NUM / (float)NPS_ACCEL_SENSITIVITY_DEN) +#define NPS_ACCEL_SENSITIVITY_XX ACCEL_BFP_OF_REAL(1./NPS_ACCEL_SENSITIVITY) +#define NPS_ACCEL_SENSITIVITY_YY ACCEL_BFP_OF_REAL(1./NPS_ACCEL_SENSITIVITY) +#define NPS_ACCEL_SENSITIVITY_ZZ ACCEL_BFP_OF_REAL(1./NPS_ACCEL_SENSITIVITY) -#define NPS_ACCEL_NEUTRAL_X IMU_ACCEL_X_NEUTRAL -#define NPS_ACCEL_NEUTRAL_Y IMU_ACCEL_Y_NEUTRAL -#define NPS_ACCEL_NEUTRAL_Z IMU_ACCEL_Z_NEUTRAL +#define NPS_ACCEL_NEUTRAL_X 0 +#define NPS_ACCEL_NEUTRAL_Y 0 +#define NPS_ACCEL_NEUTRAL_Z 0 /* m2s-4 */ #define NPS_ACCEL_NOISE_STD_DEV_X 5.e-2 #define NPS_ACCEL_NOISE_STD_DEV_Y 5.e-2 @@ -87,21 +90,24 @@ #endif /* 2^12/GYRO_X_SENS */ -#define NPS_GYRO_SENSITIVITY_PP (IMU_GYRO_P_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_P_SENS)) -#define NPS_GYRO_SENSITIVITY_QQ (IMU_GYRO_Q_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_Q_SENS)) -#define NPS_GYRO_SENSITIVITY_RR (IMU_GYRO_R_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_R_SENS)) +#define NPS_GYRO_SENSITIVITY_NUM 36542 +#define NPS_GYRO_SENSITIVITY_DEN 8383 +#define NPS_GYRO_SENSITIVITY ((float)NPS_GYRO_SENSITIVITY_NUM / (float)NPS_GYRO_SENSITIVITY_DEN) +#define NPS_GYRO_SENSITIVITY_PP RATE_BFP_OF_REAL(1./NPS_GYRO_SENSITIVITY) +#define NPS_GYRO_SENSITIVITY_QQ RATE_BFP_OF_REAL(1./NPS_GYRO_SENSITIVITY) +#define NPS_GYRO_SENSITIVITY_RR RATE_BFP_OF_REAL(1./NPS_GYRO_SENSITIVITY) -#define NPS_GYRO_NEUTRAL_P IMU_GYRO_P_NEUTRAL -#define NPS_GYRO_NEUTRAL_Q IMU_GYRO_Q_NEUTRAL -#define NPS_GYRO_NEUTRAL_R IMU_GYRO_R_NEUTRAL +#define NPS_GYRO_NEUTRAL_P 0 +#define NPS_GYRO_NEUTRAL_Q 0 +#define NPS_GYRO_NEUTRAL_R 0 #define NPS_GYRO_NOISE_STD_DEV_P RadOfDeg(0.) #define NPS_GYRO_NOISE_STD_DEV_Q RadOfDeg(0.) #define NPS_GYRO_NOISE_STD_DEV_R RadOfDeg(0.) -#define NPS_GYRO_BIAS_INITIAL_P RadOfDeg( 0.0) -#define NPS_GYRO_BIAS_INITIAL_Q RadOfDeg( 0.0) -#define NPS_GYRO_BIAS_INITIAL_R RadOfDeg( 0.0) +#define NPS_GYRO_BIAS_INITIAL_P RadOfDeg(0.0) +#define NPS_GYRO_BIAS_INITIAL_Q RadOfDeg(0.0) +#define NPS_GYRO_BIAS_INITIAL_R RadOfDeg(0.0) #define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(0.5) #define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(0.5) @@ -128,13 +134,16 @@ #define NPS_MAG_IMU_TO_SENSOR_THETA 0. #define NPS_MAG_IMU_TO_SENSOR_PSI 0. -#define NPS_MAG_SENSITIVITY_XX (IMU_MAG_X_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_X_SENS)) -#define NPS_MAG_SENSITIVITY_YY (IMU_MAG_Y_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_Y_SENS)) -#define NPS_MAG_SENSITIVITY_ZZ (IMU_MAG_Z_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_Z_SENS)) +#define NPS_MAG_SENSITIVITY_NUM 1 +#define NPS_MAG_SENSITIVITY_DEN 1 +#define NPS_MAG_SENSITIVITY ((float)NPS_MAG_SENSITIVITY_NUM / (float)NPS_MAG_SENSITIVITY_DEN) +#define NPS_MAG_SENSITIVITY_XX MAG_BFP_OF_REAL(1./NPS_MAG_SENSITIVITY) +#define NPS_MAG_SENSITIVITY_YY MAG_BFP_OF_REAL(1./NPS_MAG_SENSITIVITY) +#define NPS_MAG_SENSITIVITY_ZZ MAG_BFP_OF_REAL(1./NPS_MAG_SENSITIVITY) -#define NPS_MAG_NEUTRAL_X IMU_MAG_X_NEUTRAL -#define NPS_MAG_NEUTRAL_Y IMU_MAG_Y_NEUTRAL -#define NPS_MAG_NEUTRAL_Z IMU_MAG_Z_NEUTRAL +#define NPS_MAG_NEUTRAL_X 0 +#define NPS_MAG_NEUTRAL_Y 0 +#define NPS_MAG_NEUTRAL_Z 0 #define NPS_MAG_NOISE_STD_DEV_X 2e-3 #define NPS_MAG_NOISE_STD_DEV_Y 2e-3 diff --git a/conf/simulator/nps/nps_sensors_params_noisy.h b/conf/simulator/nps/nps_sensors_params_noisy.h index 121969874b..920bfea360 100644 --- a/conf/simulator/nps/nps_sensors_params_noisy.h +++ b/conf/simulator/nps/nps_sensors_params_noisy.h @@ -53,13 +53,16 @@ #endif /* ms-2 */ /* aka 2^10/ACCEL_X_SENS */ -#define NPS_ACCEL_SENSITIVITY_XX (IMU_ACCEL_X_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_X_SENS)) -#define NPS_ACCEL_SENSITIVITY_YY (IMU_ACCEL_Y_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Y_SENS)) -#define NPS_ACCEL_SENSITIVITY_ZZ (IMU_ACCEL_Z_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Z_SENS)) +#define NPS_ACCEL_SENSITIVITY_NUM 981 +#define NPS_ACCEL_SENSITIVITY_DEN 200 +#define NPS_ACCEL_SENSITIVITY ((float)NPS_ACCEL_SENSITIVITY_NUM / (float)NPS_ACCEL_SENSITIVITY_DEN) +#define NPS_ACCEL_SENSITIVITY_XX ACCEL_BFP_OF_REAL(1./NPS_ACCEL_SENSITIVITY) +#define NPS_ACCEL_SENSITIVITY_YY ACCEL_BFP_OF_REAL(1./NPS_ACCEL_SENSITIVITY) +#define NPS_ACCEL_SENSITIVITY_ZZ ACCEL_BFP_OF_REAL(1./NPS_ACCEL_SENSITIVITY) -#define NPS_ACCEL_NEUTRAL_X IMU_ACCEL_X_NEUTRAL -#define NPS_ACCEL_NEUTRAL_Y IMU_ACCEL_Y_NEUTRAL -#define NPS_ACCEL_NEUTRAL_Z IMU_ACCEL_Z_NEUTRAL +#define NPS_ACCEL_NEUTRAL_X 0 +#define NPS_ACCEL_NEUTRAL_Y 0 +#define NPS_ACCEL_NEUTRAL_Z 0 /* m2s-4 */ #define NPS_ACCEL_NOISE_STD_DEV_X .5 #define NPS_ACCEL_NOISE_STD_DEV_Y .5 @@ -86,22 +89,25 @@ #define NPS_GYRO_MAX 65536 #endif -/* 2^12/GYRO_X_SENS */ -#define NPS_GYRO_SENSITIVITY_PP (IMU_GYRO_P_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_P_SENS)) -#define NPS_GYRO_SENSITIVITY_QQ (IMU_GYRO_Q_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_Q_SENS)) -#define NPS_GYRO_SENSITIVITY_RR (IMU_GYRO_R_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_R_SENS)) +/* 2^12/GYRO_X_SENS */ +#define NPS_GYRO_SENSITIVITY_NUM 36542 +#define NPS_GYRO_SENSITIVITY_DEN 8383 +#define NPS_GYRO_SENSITIVITY ((float)NPS_GYRO_SENSITIVITY_NUM / (float)NPS_GYRO_SENSITIVITY_DEN) +#define NPS_GYRO_SENSITIVITY_PP RATE_BFP_OF_REAL(1./NPS_GYRO_SENSITIVITY) +#define NPS_GYRO_SENSITIVITY_QQ RATE_BFP_OF_REAL(1./NPS_GYRO_SENSITIVITY) +#define NPS_GYRO_SENSITIVITY_RR RATE_BFP_OF_REAL(1./NPS_GYRO_SENSITIVITY) -#define NPS_GYRO_NEUTRAL_P IMU_GYRO_P_NEUTRAL -#define NPS_GYRO_NEUTRAL_Q IMU_GYRO_Q_NEUTRAL -#define NPS_GYRO_NEUTRAL_R IMU_GYRO_R_NEUTRAL +#define NPS_GYRO_NEUTRAL_P 0 +#define NPS_GYRO_NEUTRAL_Q 0 +#define NPS_GYRO_NEUTRAL_R 0 #define NPS_GYRO_NOISE_STD_DEV_P RadOfDeg(9.) #define NPS_GYRO_NOISE_STD_DEV_Q RadOfDeg(9.) #define NPS_GYRO_NOISE_STD_DEV_R RadOfDeg(9.) -#define NPS_GYRO_BIAS_INITIAL_P RadOfDeg( 0.0) -#define NPS_GYRO_BIAS_INITIAL_Q RadOfDeg( 0.0) -#define NPS_GYRO_BIAS_INITIAL_R RadOfDeg( 0.0) +#define NPS_GYRO_BIAS_INITIAL_P RadOfDeg(0.0) +#define NPS_GYRO_BIAS_INITIAL_Q RadOfDeg(0.0) +#define NPS_GYRO_BIAS_INITIAL_R RadOfDeg(0.0) #define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(0.5) #define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(0.5) @@ -128,13 +134,16 @@ #define NPS_MAG_IMU_TO_SENSOR_THETA 0. #define NPS_MAG_IMU_TO_SENSOR_PSI 0. -#define NPS_MAG_SENSITIVITY_XX (IMU_MAG_X_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_X_SENS)) -#define NPS_MAG_SENSITIVITY_YY (IMU_MAG_Y_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_Y_SENS)) -#define NPS_MAG_SENSITIVITY_ZZ (IMU_MAG_Z_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_Z_SENS)) +#define NPS_MAG_SENSITIVITY_NUM 1 +#define NPS_MAG_SENSITIVITY_DEN 1 +#define NPS_MAG_SENSITIVITY ((float)NPS_MAG_SENSITIVITY_NUM / (float)NPS_MAG_SENSITIVITY_DEN) +#define NPS_MAG_SENSITIVITY_XX MAG_BFP_OF_REAL(1./NPS_MAG_SENSITIVITY) +#define NPS_MAG_SENSITIVITY_YY MAG_BFP_OF_REAL(1./NPS_MAG_SENSITIVITY) +#define NPS_MAG_SENSITIVITY_ZZ MAG_BFP_OF_REAL(1./NPS_MAG_SENSITIVITY) -#define NPS_MAG_NEUTRAL_X IMU_MAG_X_NEUTRAL -#define NPS_MAG_NEUTRAL_Y IMU_MAG_Y_NEUTRAL -#define NPS_MAG_NEUTRAL_Z IMU_MAG_Z_NEUTRAL +#define NPS_MAG_NEUTRAL_X 0 +#define NPS_MAG_NEUTRAL_Y 0 +#define NPS_MAG_NEUTRAL_Z 0 #define NPS_MAG_NOISE_STD_DEV_X 2e-1 #define NPS_MAG_NOISE_STD_DEV_Y 2e-1 diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml index a523170af2..da1ca13c6c 100644 --- a/conf/userconf/tudelft/conf.xml +++ b/conf/userconf/tudelft/conf.xml @@ -84,7 +84,7 @@ telemetry="telemetry/default_rotorcraft.xml" flight_plan="flight_plans/tudelft/rotorcraft_optitrack_path.xml" settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_speed.xml" - settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/geo_mag.xml modules/gps.xml modules/guidance_indi.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi.xml" + settings_modules="modules/air_data.xml modules/geo_mag.xml modules/gps.xml modules/guidance_indi.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi.xml" gui_color="#ffffcccaccca" /> scaled.p, + &gyro->scaled.q, + &gyro->scaled.r, + &ahrs_aligner.noise, + &ahrs_aligner.low_noise_cnt, + &ahrs_aligner.status); + } } #endif @@ -83,7 +86,7 @@ void ahrs_aligner_init(void) ahrs_aligner_restart(); // for now: only bind to gyro message and still read from global imu struct - AbiBindMsgIMU_GYRO_INT32(AHRS_ALIGNER_IMU_ID, &gyro_ev, gyro_cb); + AbiBindMsgIMU_GYRO(AHRS_ALIGNER_IMU_ID, &gyro_ev, gyro_cb); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_FILTER_ALIGNER, send_aligner); @@ -111,12 +114,19 @@ void ahrs_aligner_restart(void) void ahrs_aligner_run(void) { + struct imu_gyro_t *gyro = imu_get_gyro(AHRS_ALIGNER_IMU_ID, false); + struct imu_accel_t *accel = imu_get_accel(AHRS_ALIGNER_IMU_ID, false); + struct imu_mag_t *mag = imu_get_mag(AHRS_ALIGNER_IMU_ID, false); - RATES_ADD(gyro_sum, imu.gyro); - VECT3_ADD(accel_sum, imu.accel); - VECT3_ADD(mag_sum, imu.mag); + // Could not find all sensors + if(gyro == NULL || accel == NULL || mag == NULL) + return; + + RATES_ADD(gyro_sum, gyro->scaled); + VECT3_ADD(accel_sum, accel->scaled); + VECT3_ADD(mag_sum, mag->scaled); - ref_sensor_samples[samples_idx] = imu.accel.z; + ref_sensor_samples[samples_idx] = accel->scaled.z; samples_idx++; #ifdef AHRS_ALIGNER_LED diff --git a/sw/airborne/modules/ahrs/ahrs_float_cmpl_wrapper.c b/sw/airborne/modules/ahrs/ahrs_float_cmpl_wrapper.c index 7418e55d20..7fc5782cf3 100644 --- a/sw/airborne/modules/ahrs/ahrs_float_cmpl_wrapper.c +++ b/sw/airborne/modules/ahrs/ahrs_float_cmpl_wrapper.c @@ -289,9 +289,9 @@ void ahrs_fc_register(void) /* * Subscribe to scaled IMU measurements and attach callbacks */ - AbiBindMsgIMU_GYRO_INT32(AHRS_FC_IMU_ID, &gyro_ev, gyro_cb); - AbiBindMsgIMU_ACCEL_INT32(AHRS_FC_IMU_ID, &accel_ev, accel_cb); - AbiBindMsgIMU_MAG_INT32(AHRS_FC_MAG_ID, &mag_ev, mag_cb); + AbiBindMsgIMU_GYRO(AHRS_FC_IMU_ID, &gyro_ev, gyro_cb); + AbiBindMsgIMU_ACCEL(AHRS_FC_IMU_ID, &accel_ev, accel_cb); + AbiBindMsgIMU_MAG(AHRS_FC_MAG_ID, &mag_ev, mag_cb); AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb); AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb); AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb); diff --git a/sw/airborne/modules/ahrs/ahrs_float_dcm_wrapper.c b/sw/airborne/modules/ahrs/ahrs_float_dcm_wrapper.c index 37e6d0a28c..c1c3a36db5 100644 --- a/sw/airborne/modules/ahrs/ahrs_float_dcm_wrapper.c +++ b/sw/airborne/modules/ahrs/ahrs_float_dcm_wrapper.c @@ -204,9 +204,9 @@ void ahrs_dcm_register(void) /* * Subscribe to scaled IMU measurements and attach callbacks */ - AbiBindMsgIMU_GYRO_INT32(AHRS_DCM_IMU_ID, &gyro_ev, gyro_cb); - AbiBindMsgIMU_ACCEL_INT32(AHRS_DCM_IMU_ID, &accel_ev, accel_cb); - AbiBindMsgIMU_MAG_INT32(AHRS_DCM_MAG_ID, &mag_ev, mag_cb); + AbiBindMsgIMU_GYRO(AHRS_DCM_IMU_ID, &gyro_ev, gyro_cb); + AbiBindMsgIMU_ACCEL(AHRS_DCM_IMU_ID, &accel_ev, accel_cb); + AbiBindMsgIMU_MAG(AHRS_DCM_MAG_ID, &mag_ev, mag_cb); AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb); AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb); AbiBindMsgGPS(AHRS_DCM_GPS_ID, &gps_ev, gps_cb); diff --git a/sw/airborne/modules/ahrs/ahrs_float_invariant_wrapper.c b/sw/airborne/modules/ahrs/ahrs_float_invariant_wrapper.c index eb3b7bdde7..c781428266 100644 --- a/sw/airborne/modules/ahrs/ahrs_float_invariant_wrapper.c +++ b/sw/airborne/modules/ahrs/ahrs_float_invariant_wrapper.c @@ -243,9 +243,9 @@ void ahrs_float_invariant_register(void) /* * Subscribe to scaled IMU measurements and attach callbacks */ - AbiBindMsgIMU_MAG_INT32(AHRS_FINV_MAG_ID, &mag_ev, mag_cb); - AbiBindMsgIMU_GYRO_INT32(AHRS_FINV_IMU_ID, &gyro_ev, gyro_cb); - AbiBindMsgIMU_ACCEL_INT32(AHRS_FINV_IMU_ID, &accel_ev, accel_cb); + AbiBindMsgIMU_MAG(AHRS_FINV_MAG_ID, &mag_ev, mag_cb); + AbiBindMsgIMU_GYRO(AHRS_FINV_IMU_ID, &gyro_ev, gyro_cb); + AbiBindMsgIMU_ACCEL(AHRS_FINV_IMU_ID, &accel_ev, accel_cb); AbiBindMsgIMU_LOWPASSED(AHRS_FINV_IMU_ID, &aligner_ev, aligner_cb); AbiBindMsgBODY_TO_IMU_QUAT(AHRS_FINV_IMU_ID, &body_to_imu_ev, body_to_imu_cb); AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb); diff --git a/sw/airborne/modules/ahrs/ahrs_float_mlkf_wrapper.c b/sw/airborne/modules/ahrs/ahrs_float_mlkf_wrapper.c index e212e98ad9..07c57b0aba 100644 --- a/sw/airborne/modules/ahrs/ahrs_float_mlkf_wrapper.c +++ b/sw/airborne/modules/ahrs/ahrs_float_mlkf_wrapper.c @@ -228,9 +228,9 @@ void ahrs_mlkf_register(void) /* * Subscribe to scaled IMU measurements and attach callbacks */ - AbiBindMsgIMU_GYRO_INT32(AHRS_MLKF_IMU_ID, &gyro_ev, gyro_cb); - AbiBindMsgIMU_ACCEL_INT32(AHRS_MLKF_IMU_ID, &accel_ev, accel_cb); - AbiBindMsgIMU_MAG_INT32(AHRS_MLKF_MAG_ID, &mag_ev, mag_cb); + AbiBindMsgIMU_GYRO(AHRS_MLKF_IMU_ID, &gyro_ev, gyro_cb); + AbiBindMsgIMU_ACCEL(AHRS_MLKF_IMU_ID, &accel_ev, accel_cb); + AbiBindMsgIMU_MAG(AHRS_MLKF_MAG_ID, &mag_ev, mag_cb); AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb); AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb); AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb); diff --git a/sw/airborne/modules/ahrs/ahrs_int_cmpl_quat_wrapper.c b/sw/airborne/modules/ahrs/ahrs_int_cmpl_quat_wrapper.c index 9c8d85af8e..6157374e8a 100644 --- a/sw/airborne/modules/ahrs/ahrs_int_cmpl_quat_wrapper.c +++ b/sw/airborne/modules/ahrs/ahrs_int_cmpl_quat_wrapper.c @@ -277,9 +277,9 @@ void ahrs_icq_register(void) /* * Subscribe to scaled IMU measurements and attach callbacks */ - AbiBindMsgIMU_GYRO_INT32(AHRS_ICQ_IMU_ID, &gyro_ev, gyro_cb); - AbiBindMsgIMU_ACCEL_INT32(AHRS_ICQ_IMU_ID, &accel_ev, accel_cb); - AbiBindMsgIMU_MAG_INT32(AHRS_ICQ_MAG_ID, &mag_ev, mag_cb); + AbiBindMsgIMU_GYRO(AHRS_ICQ_IMU_ID, &gyro_ev, gyro_cb); + AbiBindMsgIMU_ACCEL(AHRS_ICQ_IMU_ID, &accel_ev, accel_cb); + AbiBindMsgIMU_MAG(AHRS_ICQ_MAG_ID, &mag_ev, mag_cb); AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb); AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb); AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb); diff --git a/sw/airborne/modules/ahrs/ahrs_madgwick_wrapper.c b/sw/airborne/modules/ahrs/ahrs_madgwick_wrapper.c index 765dee159d..831edb5879 100644 --- a/sw/airborne/modules/ahrs/ahrs_madgwick_wrapper.c +++ b/sw/airborne/modules/ahrs/ahrs_madgwick_wrapper.c @@ -209,8 +209,8 @@ void ahrs_madgwick_register(void) /* * Subscribe to scaled IMU measurements and attach callbacks */ - AbiBindMsgIMU_GYRO_INT32(AHRS_MADGWICK_IMU_ID, &gyro_ev, gyro_cb); - AbiBindMsgIMU_ACCEL_INT32(AHRS_MADGWICK_IMU_ID, &accel_ev, accel_cb); + AbiBindMsgIMU_GYRO(AHRS_MADGWICK_IMU_ID, &gyro_ev, gyro_cb); + AbiBindMsgIMU_ACCEL(AHRS_MADGWICK_IMU_ID, &accel_ev, accel_cb); AbiBindMsgIMU_LOWPASSED(AHRS_MADGWICK_IMU_ID, &aligner_ev, aligner_cb); AbiBindMsgBODY_TO_IMU_QUAT(AHRS_MADGWICK_IMU_ID, &body_to_imu_ev, body_to_imu_cb); diff --git a/sw/airborne/modules/ahrs/ahrs_vectornav.c b/sw/airborne/modules/ahrs/ahrs_vectornav.c index 6efba0669c..06e3f34f35 100644 --- a/sw/airborne/modules/ahrs/ahrs_vectornav.c +++ b/sw/airborne/modules/ahrs/ahrs_vectornav.c @@ -83,9 +83,9 @@ void ahrs_vectornav_event(void) uint32_t now_ts = get_sys_time_usec(); // in fixed point for sending as ABI and telemetry msgs RATES_BFP_OF_REAL(ahrs_vn.gyro_i, ahrs_vn.vn_data.gyro); - AbiSendMsgIMU_GYRO_INT32(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.gyro_i); + AbiSendMsgIMU_GYRO_RAW(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.gyro_i, 1); ACCELS_BFP_OF_REAL(ahrs_vn.accel_i, ahrs_vn.vn_data.accel); - AbiSendMsgIMU_ACCEL_INT32(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.accel_i); + AbiSendMsgIMU_ACCEL_RAW(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.accel_i, 1); } } diff --git a/sw/airborne/modules/calibration/mag_calib_ukf.c b/sw/airborne/modules/calibration/mag_calib_ukf.c index d629be6a91..e0bea922fc 100644 --- a/sw/airborne/modules/calibration/mag_calib_ukf.c +++ b/sw/airborne/modules/calibration/mag_calib_ukf.c @@ -141,7 +141,7 @@ void mag_calib_ukf_init(void) float initial_state[12] = MAG_CALIB_UKF_INITIAL_STATE; memcpy(&mag_calib.state, &initial_state, 12 * sizeof(float)); #endif - AbiBindMsgIMU_MAG_INT32(MAG_CALIB_UKF_ABI_BIND_ID, &mag_ev, mag_calib_ukf_run); + AbiBindMsgIMU_MAG(MAG_CALIB_UKF_ABI_BIND_ID, &mag_ev, mag_calib_ukf_run); AbiBindMsgGEO_MAG(ABI_BROADCAST, &h_ev, mag_calib_update_field); ///< GEO_MAG_SENDER_ID is defined in geo_mag.c so unknown } @@ -189,7 +189,7 @@ void mag_calib_ukf_run(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag VERBOSE_PRINT("expected measurement (x: %4.2f y: %4.2f z: %4.2f) norm: %4.2f\n", expected_mag_field[0], expected_mag_field[1], expected_mag_field[2], hypot(hypot(expected_mag_field[0], expected_mag_field[1]), expected_mag_field[2])); VERBOSE_PRINT("calibrated measurement (x: %4.2f y: %4.2f z: %4.2f) norm: %4.2f\n\n", calibrated_measurement[0], calibrated_measurement[1], calibrated_measurement[2], hypot(hypot(calibrated_measurement[0], calibrated_measurement[1]), calibrated_measurement[2])); /** Forward calibrated data */ - AbiSendMsgIMU_MAG_INT32(MAG_CALIB_UKF_ID, stamp, &calibrated_mag); + AbiSendMsgIMU_MAG_RAW(MAG_CALIB_UKF_ID, stamp, &calibrated_mag); } } } diff --git a/sw/airborne/modules/calibration/send_imu_mag_current.c b/sw/airborne/modules/calibration/send_imu_mag_current.c index 4ecfff4299..880773e03e 100644 --- a/sw/airborne/modules/calibration/send_imu_mag_current.c +++ b/sw/airborne/modules/calibration/send_imu_mag_current.c @@ -25,6 +25,7 @@ */ #include "modules/imu/imu.h" +#include "modules/core/abi.h" #include "modules/energy/electrical.h" #include "pprzlink/messages.h" @@ -33,10 +34,14 @@ void send_imu_mag_current(void) { + static uint8_t id = 0; DOWNLINK_SEND_IMU_MAG_CURRENT_CALIBRATION(DefaultChannel, DefaultDevice, - &imu.mag_unscaled.x, - &imu.mag_unscaled.y, - &imu.mag_unscaled.z, + &imu.mags[id].unscaled.x, + &imu.mags[id].unscaled.y, + &imu.mags[id].unscaled.z, &electrical.current); + id++; + if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE) + id = 0; } diff --git a/sw/airborne/modules/core/abi_sender_ids.h b/sw/airborne/modules/core/abi_sender_ids.h index cf31eea861..f7ce184a86 100644 --- a/sw/airborne/modules/core/abi_sender_ids.h +++ b/sw/airborne/modules/core/abi_sender_ids.h @@ -375,6 +375,10 @@ #define IMU_CUBE3_ID 22 #endif +#ifndef IMU_NPS_ID +#define IMU_NPS_ID 23 +#endif + // prefiltering with OneEuro filter #ifndef IMU_F1E_ID #define IMU_F1E_ID 30 diff --git a/sw/airborne/modules/dragspeed/dragspeed.c b/sw/airborne/modules/dragspeed/dragspeed.c index 12c77d2475..8cd07033b6 100644 --- a/sw/airborne/modules/dragspeed/dragspeed.c +++ b/sw/airborne/modules/dragspeed/dragspeed.c @@ -99,7 +99,7 @@ void dragspeed_init(void) // Register callbacks register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_DRAGSPEED, send_dragspeed); - AbiBindMsgIMU_ACCEL_INT32(DRAGSPEED_ACCEL_ID, &accel_ev, accel_cb); + AbiBindMsgIMU_ACCEL(DRAGSPEED_ACCEL_ID, &accel_ev, accel_cb); } bool dragspeed_calibrate_coeff(void) diff --git a/sw/airborne/modules/imu/filter_1euro_imu.c b/sw/airborne/modules/imu/filter_1euro_imu.c index 0d3c156e07..bc8b5ed89e 100644 --- a/sw/airborne/modules/imu/filter_1euro_imu.c +++ b/sw/airborne/modules/imu/filter_1euro_imu.c @@ -111,15 +111,15 @@ static abi_event gyro_ev; static abi_event accel_ev; static abi_event mag_ev; // only passthrough -static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro) +static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro, uint8_t samples) { if (sender_id == IMU_F1E_ID) { return; // don't process own data } - if (filter_1e_imu.enabled) { + if (filter_1e_imu.enabled && samples > 0) { struct FloatRates gyro_f; - RATES_FLOAT_OF_BFP(gyro_f, *gyro); + RATES_FLOAT_OF_BFP(gyro_f, gyro[samples-1]); // For now only filter last value // compute filters #ifdef FILTER_1EURO_FREQ gyro_f.p = update_1e_filter(&gyro_1e[0], gyro_f.p); @@ -134,19 +134,19 @@ static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro) // send filtered data struct Int32Rates gyro_i; RATES_BFP_OF_REAL(gyro_i, gyro_f); - AbiSendMsgIMU_GYRO_INT32(IMU_F1E_ID, stamp, &gyro_i); + AbiSendMsgIMU_GYRO_RAW(IMU_F1E_ID, stamp, &gyro_i, 1); } else { - AbiSendMsgIMU_GYRO_INT32(IMU_F1E_ID, stamp, gyro); + AbiSendMsgIMU_GYRO_RAW(IMU_F1E_ID, stamp, gyro, samples); } } -static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel) +static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel, uint8_t samples) { if (sender_id == IMU_F1E_ID) { return; // don't process own data } - if (filter_1e_imu.enabled) { + if (filter_1e_imu.enabled && samples > 0) { struct FloatVect3 accel_f; ACCELS_FLOAT_OF_BFP(accel_f, *accel); // compute filters @@ -163,9 +163,9 @@ static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel // send filtered data struct Int32Vect3 accel_i; ACCELS_BFP_OF_REAL(accel_i, accel_f); - AbiSendMsgIMU_ACCEL_INT32(IMU_F1E_ID, stamp, &accel_i); + AbiSendMsgIMU_ACCEL_RAW(IMU_F1E_ID, stamp, &accel_i, 1); } else { - AbiSendMsgIMU_ACCEL_INT32(IMU_F1E_ID, stamp, accel); + AbiSendMsgIMU_ACCEL_RAW(IMU_F1E_ID, stamp, accel, samples); } } @@ -176,7 +176,7 @@ static void mag_cb(uint8_t sender_id __attribute__((unused)), return; // don't process own data } - AbiSendMsgIMU_MAG_INT32(IMU_F1E_ID, stamp, mag); + AbiSendMsgIMU_MAG_RAW(IMU_F1E_ID, stamp, mag); } /** @@ -205,9 +205,9 @@ void filter_1euro_imu_init(void) filter_1e_imu.accel_dcutoff); } - AbiBindMsgIMU_GYRO_INT32(IMU_F1E_BIND_ID, &gyro_ev, gyro_cb); - AbiBindMsgIMU_ACCEL_INT32(IMU_F1E_BIND_ID, &accel_ev, accel_cb); - AbiBindMsgIMU_MAG_INT32(IMU_F1E_BIND_ID, &mag_ev, mag_cb); + AbiBindMsgIMU_GYRO_RAW(IMU_F1E_BIND_ID, &gyro_ev, gyro_cb); + AbiBindMsgIMU_ACCEL_RAW(IMU_F1E_BIND_ID, &accel_ev, accel_cb); + AbiBindMsgIMU_MAG_RAW(IMU_F1E_BIND_ID, &mag_ev, mag_cb); } /** @@ -270,4 +270,3 @@ void filter_1euro_imu_update_accel_dcutoff(float dcutoff) accel_1e[i].dcutoff = dcutoff; } } - diff --git a/sw/airborne/modules/imu/imu.c b/sw/airborne/modules/imu/imu.c index c575ffd3a6..8a307cb636 100644 --- a/sw/airborne/modules/imu/imu.c +++ b/sw/airborne/modules/imu/imu.c @@ -37,6 +37,18 @@ #define IMU_INTEGRATION true #endif +#ifndef IMU_GYRO_CALIB +#define IMU_GYRO_CALIB {} +#endif + +#ifndef IMU_ACCEL_CALIB +#define IMU_ACCEL_CALIB {} +#endif + +#ifndef IMU_MAG_CALIB +#define IMU_MAG_CALIB {} +#endif + #if PERIODIC_TELEMETRY #include "modules/datalink/telemetry.h" @@ -56,7 +68,7 @@ static void send_accel_scaled(struct transport_tx *trans, struct link_device *de pprz_msg_send_IMU_ACCEL_SCALED(trans, dev, AC_ID, &imu.accels[id].abi_id, &imu.accels[id].scaled.x, &imu.accels[id].scaled.y, &imu.accels[id].scaled.z); id++; - if(id >= IMU_MAX_SENSORS) + if(id >= IMU_MAX_SENSORS || imu.accels[id].abi_id == ABI_DISABLE) id = 0; } @@ -143,9 +155,6 @@ static abi_event imu_gyro_raw_ev, imu_accel_raw_ev, imu_mag_raw_ev; static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *data, uint8_t samples); static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data, uint8_t samples); static void imu_mag_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data); -static struct imu_gyro_t *imu_get_gyro(uint8_t sender_id); -static struct imu_accel_t *imu_get_accel(uint8_t sender_id); -static struct imu_mag_t *imu_get_mag(uint8_t sender_id); void imu_init(void) { @@ -158,31 +167,60 @@ void imu_init(void) {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI}; orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers); - // Initialize the non-initialized sensors to default values + // Set the calibrated sensors + const struct imu_gyro_t gyro_calib[] = IMU_GYRO_CALIB; + const struct imu_accel_t accel_calib[] = IMU_ACCEL_CALIB; + const struct imu_mag_t mag_calib[] = IMU_MAG_CALIB; + const uint8_t gyro_calib_len = sizeof(gyro_calib) / sizeof(struct imu_gyro_t); + const uint8_t accel_calib_len = sizeof(accel_calib) / sizeof(struct imu_accel_t); + const uint8_t mag_calib_len = sizeof(mag_calib) / sizeof(struct imu_mag_t); + + // Initialize the sensors to default values for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) { - if(imu.gyros[i].abi_id == ABI_DISABLE) { - imu.gyros[i].last_stamp = 0; + // Gyro initialization calibrated/non-calibrated + if(i >= gyro_calib_len) { + imu.gyros[i].abi_id = ABI_DISABLE; + imu.gyros[i].calibrated = false; INT_RATES_ZERO(imu.gyros[i].neutral); RATES_ASSIGN(imu.gyros[i].scale[0], 1, 1, 1); RATES_ASSIGN(imu.gyros[i].scale[1], 1, 1, 1); - int32_rmat_identity(&imu.gyros[i].imu_to_sensor); } + else { + imu.gyros[i] = gyro_calib[i]; + imu.gyros[i].calibrated = true; + } + imu.gyros[i].last_stamp = 0; + int32_rmat_identity(&imu.gyros[i].imu_to_sensor); - if(imu.accels[i].abi_id == ABI_DISABLE) { - imu.accels[i].last_stamp = 0; + // Accel initialization calibrated/non-calibrated + if(i >= accel_calib_len) { + imu.accels[i].abi_id = ABI_DISABLE; + imu.accels[i].calibrated = false; INT_VECT3_ZERO(imu.accels[i].neutral); VECT3_ASSIGN(imu.accels[i].scale[0], 1, 1, 1); VECT3_ASSIGN(imu.accels[i].scale[1], 1, 1, 1); - int32_rmat_identity(&imu.accels[i].imu_to_sensor); } + else { + imu.accels[i] = accel_calib[i]; + imu.accels[i].calibrated = true; + } + imu.accels[i].last_stamp = 0; + int32_rmat_identity(&imu.accels[i].imu_to_sensor); - if(imu.mags[i].abi_id == ABI_DISABLE) { + // Mag initialization calibrated/non-calibrated + if(i >= mag_calib_len) { + imu.mags[i].abi_id = ABI_DISABLE; + imu.mags[i].calibrated = false; INT_VECT3_ZERO(imu.mags[i].neutral); VECT3_ASSIGN(imu.mags[i].scale[0], 1, 1, 1); VECT3_ASSIGN(imu.mags[i].scale[1], 1, 1, 1); INT_VECT3_ZERO(imu.mags[i].current_scale); - int32_rmat_identity(&imu.mags[i].imu_to_sensor); } + else { + imu.mags[i] = mag_calib[i]; + imu.mags[i].calibrated = true; + } + int32_rmat_identity(&imu.mags[i].imu_to_sensor); } // Bind to raw measurements @@ -206,47 +244,96 @@ void imu_init(void) } /** - * @brief Set the sensor rotation - * - * @param abi_id - * @param imu_to_sensor + * @brief Set the defaults for a gyro sensor + * WARNING: Should be called before sensor is publishing messages to ensure correct values + * @param abi_id The ABI sender id to set the defaults for + * @param imu_to_sensor Imu to sensor rotation matrix + * @param neutral Neutral values + * @param scale Scale values, 0 index is multiply and 1 index is divide */ -void imu_set_gyro_rmat(uint8_t abi_id, struct Int32RMat *imu_to_sensor) +void imu_set_defaults_gyro(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Rates *neutral, const struct Int32Rates *scale) { // Could be that we are not initialized imu_init(); // Find the correct gyro - struct imu_gyro_t *gyro = imu_get_gyro(abi_id); + struct imu_gyro_t *gyro = imu_get_gyro(abi_id, true); if(gyro == NULL) return; - RMAT_COPY(gyro->imu_to_sensor, *imu_to_sensor); + // Copy the defaults + if(imu_to_sensor != NULL) + RMAT_COPY(gyro->imu_to_sensor, *imu_to_sensor); + if(neutral != NULL && !gyro->calibrated) + RATES_COPY(gyro->neutral, *neutral); + if(scale != NULL && !gyro->calibrated) { + RATES_COPY(gyro->scale[0], scale[0]); + RATES_COPY(gyro->scale[1], scale[1]); + } } /** - * @brief Set the sensor rotation - * - * @param abi_id - * @param imu_to_sensor + * @brief Set the defaults for a accel sensor + * WARNING: Should be called before sensor is publishing messages to ensure correct values + * @param abi_id The ABI sender id to set the defaults for + * @param imu_to_sensor Imu to sensor rotation matrix + * @param neutral Neutral values + * @param scale Scale values, 0 index is multiply and 1 index is divide */ -void imu_set_accel_rmat(uint8_t abi_id, struct Int32RMat *imu_to_sensor) +void imu_set_defaults_accel(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct Int32Vect3 *scale) { // Could be that we are not initialized imu_init(); // Find the correct accel - struct imu_accel_t *accel = imu_get_accel(abi_id); + struct imu_accel_t *accel = imu_get_accel(abi_id, true); if(accel == NULL) return; - RMAT_COPY(accel->imu_to_sensor, *imu_to_sensor); + // Copy the defaults + if(imu_to_sensor != NULL) + RMAT_COPY(accel->imu_to_sensor, *imu_to_sensor); + if(neutral != NULL && !accel->calibrated) + VECT3_COPY(accel->neutral, *neutral); + if(scale != NULL && !accel->calibrated) { + VECT3_COPY(accel->scale[0], scale[0]); + VECT3_COPY(accel->scale[1], scale[1]); + } +} + +/** + * @brief Set the defaults for a mag sensor + * WARNING: Should be called before sensor is publishing messages to ensure correct values + * @param abi_id The ABI sender id to set the defaults for + * @param imu_to_sensor Imu to sensor rotation matrix + * @param neutral Neutral values + * @param scale Scale values, 0 index is multiply and 1 index is divide + */ +void imu_set_defaults_mag(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct Int32Vect3 *scale) +{ + // Could be that we are not initialized + imu_init(); + + // Find the correct mag + struct imu_mag_t *mag = imu_get_mag(abi_id, true); + if(mag == NULL) + return; + + // Copy the defaults + if(imu_to_sensor != NULL) + RMAT_COPY(mag->imu_to_sensor, *imu_to_sensor); + if(neutral != NULL && !mag->calibrated) + VECT3_COPY(mag->neutral, *neutral); + if(scale != NULL && !mag->calibrated) { + VECT3_COPY(mag->scale[0], scale[0]); + VECT3_COPY(mag->scale[1], scale[1]); + } } static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *data, uint8_t samples) { // Find the correct gyro - struct imu_gyro_t *gyro = imu_get_gyro(sender_id); + struct imu_gyro_t *gyro = imu_get_gyro(sender_id, true); if(gyro == NULL || samples < 1) return; @@ -297,7 +384,7 @@ static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data, uint8_t samples) { // Find the correct accel - struct imu_accel_t *accel = imu_get_accel(sender_id); + struct imu_accel_t *accel = imu_get_accel(sender_id, true); if(accel == NULL || samples < 1) return; @@ -348,7 +435,7 @@ static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect static void imu_mag_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data) { // Find the correct mag - struct imu_mag_t *mag = imu_get_mag(sender_id); + struct imu_mag_t *mag = imu_get_mag(sender_id, true); if(mag == NULL) return; @@ -376,16 +463,19 @@ static void imu_mag_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 * @brief Find or create the gyro in the imu structure * * @param sender_id The ABI sender id to search for + * @param create Create a new index if not found * @return struct imu_gyro_t* The gyro structure if found/created else NULL */ -static struct imu_gyro_t *imu_get_gyro(uint8_t sender_id) { +struct imu_gyro_t *imu_get_gyro(uint8_t sender_id, bool create) { // Find the correct gyro or create index struct imu_gyro_t *gyro = NULL; for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) { - if(imu.gyros[i].abi_id == sender_id || imu.gyros[i].abi_id == ABI_DISABLE) { + if(imu.gyros[i].abi_id == sender_id || (create && imu.gyros[i].abi_id == ABI_DISABLE)) { gyro = &imu.gyros[i]; gyro->abi_id = sender_id; break; + } else if(sender_id == ABI_BROADCAST && imu.gyros[i].abi_id != ABI_DISABLE) { + gyro = &imu.gyros[i]; } } @@ -396,16 +486,19 @@ static struct imu_gyro_t *imu_get_gyro(uint8_t sender_id) { * @brief Find or create the accel in the imu structure * * @param sender_id The ABI sender id to search for + * @param create Create a new index if not found * @return struct imu_accel_t* The accel structure if found/created else NULL */ -static struct imu_accel_t *imu_get_accel(uint8_t sender_id) { +struct imu_accel_t *imu_get_accel(uint8_t sender_id, bool create) { // Find the correct accel struct imu_accel_t *accel = NULL; for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) { - if(imu.accels[i].abi_id == sender_id || imu.accels[i].abi_id == ABI_DISABLE) { + if(imu.accels[i].abi_id == sender_id || (create && imu.accels[i].abi_id == ABI_DISABLE)) { accel = &imu.accels[i]; accel->abi_id = sender_id; break; + } else if(sender_id == ABI_BROADCAST && imu.accels[i].abi_id != ABI_DISABLE) { + accel = &imu.accels[i]; } } @@ -416,16 +509,19 @@ static struct imu_accel_t *imu_get_accel(uint8_t sender_id) { * @brief Find or create the mag in the imu structure * * @param sender_id The ABI sender id to search for + * @param create Create a new index if not found * @return struct imu_mag_t* The mag structure if found/created else NULL */ -static struct imu_mag_t *imu_get_mag(uint8_t sender_id) { +struct imu_mag_t *imu_get_mag(uint8_t sender_id, bool create) { // Find the correct mag struct imu_mag_t *mag = NULL; for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) { - if(imu.mags[i].abi_id == sender_id || imu.mags[i].abi_id == ABI_DISABLE) { + if(imu.mags[i].abi_id == sender_id || (create && imu.mags[i].abi_id == ABI_DISABLE)) { mag = &imu.mags[i]; mag->abi_id = sender_id; break; + } else if(sender_id == ABI_BROADCAST && imu.mags[i].abi_id != ABI_DISABLE) { + mag = &imu.mags[i]; } } diff --git a/sw/airborne/modules/imu/imu.h b/sw/airborne/modules/imu/imu.h index 5edb9b3cbb..0337b99dba 100644 --- a/sw/airborne/modules/imu/imu.h +++ b/sw/airborne/modules/imu/imu.h @@ -39,6 +39,7 @@ struct imu_gyro_t { uint8_t abi_id; uint32_t last_stamp; + bool calibrated; struct Int32Rates scaled; struct Int32Rates unscaled; struct Int32Rates neutral; @@ -49,6 +50,7 @@ struct imu_gyro_t { struct imu_accel_t { uint8_t abi_id; uint32_t last_stamp; + bool calibrated; struct Int32Vect3 scaled; struct Int32Vect3 unscaled; struct Int32Vect3 neutral; @@ -58,6 +60,7 @@ struct imu_accel_t { struct imu_mag_t { uint8_t abi_id; + bool calibrated; struct Int32Vect3 scaled; struct Int32Vect3 unscaled; struct Int32Vect3 neutral; @@ -86,8 +89,13 @@ extern struct Imu imu; /** External functions */ extern void imu_init(void); -extern void imu_set_gyro_rmat(uint8_t abi_id, struct Int32RMat *imu_to_sensor); -extern void imu_set_accel_rmat(uint8_t abi_id, struct Int32RMat *imu_to_sensor); +extern void imu_set_defaults_gyro(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Rates *neutral, const struct Int32Rates *scale); +extern void imu_set_defaults_accel(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct Int32Vect3 *scale); +extern void imu_set_defaults_mag(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct Int32Vect3 *scale); + +extern struct imu_gyro_t *imu_get_gyro(uint8_t sender_id, bool create); +extern struct imu_accel_t *imu_get_accel(uint8_t sender_id, bool create); +extern struct imu_mag_t *imu_get_mag(uint8_t sender_id, bool create); extern void imu_SetBodyToImuPhi(float phi); extern void imu_SetBodyToImuTheta(float theta); diff --git a/sw/airborne/modules/imu/imu_ardrone2.c b/sw/airborne/modules/imu/imu_ardrone2.c index 0c3683f522..8d8f464119 100644 --- a/sw/airborne/modules/imu/imu_ardrone2.c +++ b/sw/airborne/modules/imu/imu_ardrone2.c @@ -24,10 +24,8 @@ * IMU implementation for ardrone2. */ -#include "modules/imu/imu.h" #include "navdata.h" #include "imu_ardrone2.h" -#include "mcu_periph/uart.h" void imu_ardrone2_init(void) { diff --git a/sw/airborne/modules/imu/imu_ardrone2.h b/sw/airborne/modules/imu/imu_ardrone2.h index bc46ebf3d0..dfd31934f1 100644 --- a/sw/airborne/modules/imu/imu_ardrone2.h +++ b/sw/airborne/modules/imu/imu_ardrone2.h @@ -27,63 +27,9 @@ #ifndef IMU_ARDRONE2_H_ #define IMU_ARDRONE2_H_ -#include "generated/airframe.h" +#include "std.h" #include "boards/ardrone/navdata.h" - -/** 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 - -/** 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 - -/* - * we include imh.h after the definitions of the neutrals - */ -#include "modules/imu/imu.h" - extern void imu_ardrone2_init(void); #endif /* IMU_ARDRONE2_H_ */ diff --git a/sw/airborne/modules/imu/imu_aspirin.c b/sw/airborne/modules/imu/imu_aspirin.c index f30a4dc1dc..3031db0218 100644 --- a/sw/airborne/modules/imu/imu_aspirin.c +++ b/sw/airborne/modules/imu/imu_aspirin.c @@ -25,6 +25,7 @@ * Driver for the Aspirin v1.x IMU using SPI for the accelerometer. */ +#include "modules/imu/imu_aspirin.h" #include "modules/imu/imu.h" #include "modules/core/abi.h" #include "mcu_periph/i2c.h" @@ -67,10 +68,6 @@ struct ImuAspirin imu_aspirin; void imu_aspirin_init(void) { - imu_aspirin.accel_valid = false; - imu_aspirin.gyro_valid = false; - imu_aspirin.mag_valid = false; - /* Set accel configuration */ adxl345_spi_init(&imu_aspirin.acc_adxl, &(ASPIRIN_SPI_DEV), ASPIRIN_SPI_SLAVE_IDX); // set the data rate @@ -90,6 +87,27 @@ void imu_aspirin_init(void) /* interrupt on data ready, idle high, latch until read any register */ //itg_conf.int_cfg = (0x01 | (0x1<<4) | (0x1<<5) | 0x01<<7); + // Default scaling values +#ifdef IMU_ASPIRIN_VERSION_1_5 + const struct Int32Rates gyro_scale[2] = { + {4359, 4359, 4359}, + {1000, 1000, 1000} + }; +#else + const struct Int32Rates gyro_scale[2] = { + {4973, 4973, 4973}, + {1000, 1000, 1000} + }; +#endif + const struct Int32Vect3 accel_scale[2] = { + {3791, 3791, 3791}, + {100, 100, 100} + }; + + // Set the default scaling + imu_set_defaults_gyro(IMU_ASPIRIN_ID, NULL, NULL, gyro_scale); + imu_set_defaults_accel(IMU_ASPIRIN_ID, NULL, NULL, accel_scale); + /* initialize mag and set default options */ hmc58xx_init(&imu_aspirin.mag_hmc, &(ASPIRIN_I2C_DEV), HMC58XX_ADDR); #ifdef IMU_ASPIRIN_VERSION_1_0 @@ -121,46 +139,29 @@ void imu_aspirin_event(void) adxl345_spi_event(&imu_aspirin.acc_adxl); if (imu_aspirin.acc_adxl.data_available) { - VECT3_COPY(imu.accel_unscaled, imu_aspirin.acc_adxl.data.vect); + AbiSendMsgIMU_ACCEL_RAW(IMU_ASPIRIN_ID, now_ts, &imu_aspirin.acc_adxl.data.vect, 1); imu_aspirin.acc_adxl.data_available = false; - imu_aspirin.accel_valid = true; } /* If the itg3200 I2C transaction has succeeded: convert the data */ itg3200_event(&imu_aspirin.gyro_itg); if (imu_aspirin.gyro_itg.data_available) { - RATES_COPY(imu.gyro_unscaled, imu_aspirin.gyro_itg.data.rates); + AbiSendMsgIMU_GYRO_RAW(IMU_ASPIRIN_ID, now_ts, &imu_aspirin.gyro_itg.data.rates, 1); imu_aspirin.gyro_itg.data_available = false; - imu_aspirin.gyro_valid = true; } /* HMC58XX event task */ hmc58xx_event(&imu_aspirin.mag_hmc); if (imu_aspirin.mag_hmc.data_available) { + struct Int32Vect3 mag; #ifdef IMU_ASPIRIN_VERSION_1_0 - VECT3_COPY(imu.mag_unscaled, imu_aspirin.mag_hmc.data.vect); + VECT3_COPY(mag, imu_aspirin.mag_hmc.data.vect); #else // aspirin 1.5 with hmc5883 - imu.mag_unscaled.x = imu_aspirin.mag_hmc.data.vect.y; - imu.mag_unscaled.y = -imu_aspirin.mag_hmc.data.vect.x; - imu.mag_unscaled.z = imu_aspirin.mag_hmc.data.vect.z; + mag.x = imu_aspirin.mag_hmc.data.vect.y; + mag.y = -imu_aspirin.mag_hmc.data.vect.x; + mag.z = imu_aspirin.mag_hmc.data.vect.z; #endif imu_aspirin.mag_hmc.data_available = false; - imu_aspirin.mag_valid = true; - } - - if (imu_aspirin.gyro_valid) { - imu_aspirin.gyro_valid = false; - imu_scale_gyro(&imu); - AbiSendMsgIMU_GYRO_INT32(IMU_ASPIRIN_ID, now_ts, &imu.gyro); - } - if (imu_aspirin.accel_valid) { - imu_aspirin.accel_valid = false; - imu_scale_accel(&imu); - AbiSendMsgIMU_ACCEL_INT32(IMU_ASPIRIN_ID, now_ts, &imu.accel); - } - if (imu_aspirin.mag_valid) { - imu_aspirin.mag_valid = false; - imu_scale_mag(&imu); - AbiSendMsgIMU_MAG_INT32(IMU_ASPIRIN_ID, now_ts, &imu.mag); + AbiSendMsgIMU_MAG_RAW(IMU_ASPIRIN_ID, now_ts, &mag); } } diff --git a/sw/airborne/modules/imu/imu_aspirin.h b/sw/airborne/modules/imu/imu_aspirin.h index d05dea0476..8dfb6fe5c3 100644 --- a/sw/airborne/modules/imu/imu_aspirin.h +++ b/sw/airborne/modules/imu/imu_aspirin.h @@ -36,13 +36,8 @@ #include "peripherals/hmc58xx.h" #include "peripherals/adxl345_spi.h" -/* include default aspirin sensitivity definitions */ -#include "modules/imu/imu_aspirin_defaults.h" struct ImuAspirin { - volatile uint8_t accel_valid; - volatile uint8_t gyro_valid; - volatile uint8_t mag_valid; struct Adxl345_Spi acc_adxl; struct Itg3200 gyro_itg; struct Hmc58xx mag_hmc; diff --git a/sw/airborne/modules/imu/imu_aspirin_2_spi.c b/sw/airborne/modules/imu/imu_aspirin_2_spi.c index 73032875dd..606b037ef3 100644 --- a/sw/airborne/modules/imu/imu_aspirin_2_spi.c +++ b/sw/airborne/modules/imu/imu_aspirin_2_spi.c @@ -24,6 +24,7 @@ * Driver for the Aspirin v2.x IMU using SPI for the MPU6000. */ +#include "modules/imu/imu_aspirin_2_spi.h" #include "modules/imu/imu.h" #include "modules/core/abi.h" #include "mcu_periph/sys_time.h" @@ -105,6 +106,10 @@ void imu_aspirin2_init(void) imu_aspirin2.mpu.config.gyro_range = ASPIRIN_2_GYRO_RANGE; imu_aspirin2.mpu.config.accel_range = ASPIRIN_2_ACCEL_RANGE; + // Set the default scaling + imu_set_defaults_gyro(IMU_ASPIRIN2_ID, NULL, NULL, MPU60X0_GYRO_SENS_FRAC[ASPIRIN_2_GYRO_RANGE]); + imu_set_defaults_accel(IMU_ASPIRIN2_ID, NULL, NULL, MPU60X0_ACCEL_SENS_FRAC[ASPIRIN_2_ACCEL_RANGE]); + /* read 15 bytes for status, accel, gyro + 6 bytes for mag slave */ imu_aspirin2.mpu.config.nb_bytes = 21; @@ -159,31 +164,33 @@ void imu_aspirin2_event(void) if (imu_aspirin2.mpu.data_available) { #if !ASPIRIN_2_DISABLE_MAG /* HMC5883 has xzy order of axes in returned data */ - struct Int32Vect3 mag; + struct Int32Vect3 mag, mag_rot; mag.x = Int16FromBuf(imu_aspirin2.mpu.data_ext, 0); mag.z = Int16FromBuf(imu_aspirin2.mpu.data_ext, 2); mag.y = Int16FromBuf(imu_aspirin2.mpu.data_ext, 4); #endif /* Handle axis assignement for Lisa/S integrated Aspirin like IMU. */ + struct Int32Rates gyro; + struct Int32Vect3 accel; #ifdef LISA_S #ifdef LISA_S_UPSIDE_DOWN - RATES_ASSIGN(imu.gyro_unscaled, + RATES_ASSIGN(gyro, imu_aspirin2.mpu.data_rates.rates.p, -imu_aspirin2.mpu.data_rates.rates.q, -imu_aspirin2.mpu.data_rates.rates.r); - VECT3_ASSIGN(imu.accel_unscaled, + VECT3_ASSIGN(accel, imu_aspirin2.mpu.data_accel.vect.x, -imu_aspirin2.mpu.data_accel.vect.y, -imu_aspirin2.mpu.data_accel.vect.z); #if !ASPIRIN_2_DISABLE_MAG - VECT3_ASSIGN(imu.mag_unscaled, mag.x, -mag.y, -mag.z); + VECT3_ASSIGN(mag_rot, mag.x, -mag.y, -mag.z); #endif #else - RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates); - VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect); + RATES_COPY(gyro, imu_aspirin2.mpu.data_rates.rates); + VECT3_COPY(accel, imu_aspirin2.mpu.data_accel.vect); #if !ASPIRIN_2_DISABLE_MAG - VECT3_COPY(imu.mag_unscaled, mag); + VECT3_COPY(mag_rot, mag); #endif #endif #else @@ -192,37 +199,37 @@ void imu_aspirin2_event(void) * IMU. */ #ifdef LISA_M_OR_MX_21 - RATES_ASSIGN(imu.gyro_unscaled, + RATES_ASSIGN(gyro, -imu_aspirin2.mpu.data_rates.rates.q, imu_aspirin2.mpu.data_rates.rates.p, imu_aspirin2.mpu.data_rates.rates.r); - VECT3_ASSIGN(imu.accel_unscaled, + VECT3_ASSIGN(accel, -imu_aspirin2.mpu.data_accel.vect.y, imu_aspirin2.mpu.data_accel.vect.x, imu_aspirin2.mpu.data_accel.vect.z); #if !ASPIRIN_2_DISABLE_MAG - VECT3_ASSIGN(imu.mag_unscaled, -mag.y, mag.x, mag.z); + VECT3_ASSIGN(mag_rot, -mag.y, mag.x, mag.z); #endif #else /* Handle real Aspirin IMU axis assignement. */ #ifdef LISA_M_LONGITUDINAL_X - RATES_ASSIGN(imu.gyro_unscaled, + RATES_ASSIGN(gyro, imu_aspirin2.mpu.data_rates.rates.q, -imu_aspirin2.mpu.data_rates.rates.p, imu_aspirin2.mpu.data_rates.rates.r); - VECT3_ASSIGN(imu.accel_unscaled, + VECT3_ASSIGN(accel, imu_aspirin2.mpu.data_accel.vect.y, -imu_aspirin2.mpu.data_accel.vect.x, imu_aspirin2.mpu.data_accel.vect.z); #if !ASPIRIN_2_DISABLE_MAG - VECT3_ASSIGN(imu.mag_unscaled, -mag.x, -mag.y, mag.z); + VECT3_ASSIGN(mag_rot, -mag.x, -mag.y, mag.z); #endif #else - RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates); - VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect); + RATES_COPY(gyro, imu_aspirin2.mpu.data_rates.rates); + VECT3_COPY(accel, imu_aspirin2.mpu.data_accel.vect); #if !ASPIRIN_2_DISABLE_MAG - VECT3_ASSIGN(imu.mag_unscaled, mag.y, -mag.x, mag.z) + VECT3_ASSIGN(mag_rot, mag.y, -mag.x, mag.z) #endif #endif #endif @@ -230,13 +237,10 @@ void imu_aspirin2_event(void) imu_aspirin2.mpu.data_available = false; - imu_scale_gyro(&imu); - imu_scale_accel(&imu); - AbiSendMsgIMU_GYRO_INT32(IMU_ASPIRIN2_ID, now_ts, &imu.gyro); - AbiSendMsgIMU_ACCEL_INT32(IMU_ASPIRIN2_ID, now_ts, &imu.accel); + AbiSendMsgIMU_GYRO_RAW(IMU_ASPIRIN2_ID, now_ts, &gyro, 1); + AbiSendMsgIMU_ACCEL_RAW(IMU_ASPIRIN2_ID, now_ts, &accel, 1); #if !ASPIRIN_2_DISABLE_MAG - imu_scale_mag(&imu); - AbiSendMsgIMU_MAG_INT32(IMU_ASPIRIN2_ID, now_ts, &imu.mag); + AbiSendMsgIMU_MAG_RAW(IMU_ASPIRIN2_ID, now_ts, &mag); #endif } } diff --git a/sw/airborne/modules/imu/imu_aspirin_2_spi.h b/sw/airborne/modules/imu/imu_aspirin_2_spi.h index 7c4fb21432..f77d445f79 100644 --- a/sw/airborne/modules/imu/imu_aspirin_2_spi.h +++ b/sw/airborne/modules/imu/imu_aspirin_2_spi.h @@ -41,32 +41,6 @@ #define ASPIRIN_2_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G #endif -// Set default sensitivity based on range if needed -#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS -#define IMU_GYRO_P_SENS MPU60X0_GYRO_SENS[ASPIRIN_2_GYRO_RANGE] -#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[ASPIRIN_2_GYRO_RANGE][0] -#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[ASPIRIN_2_GYRO_RANGE][1] -#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[ASPIRIN_2_GYRO_RANGE] -#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[ASPIRIN_2_GYRO_RANGE][0] -#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[ASPIRIN_2_GYRO_RANGE][1] -#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[ASPIRIN_2_GYRO_RANGE] -#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[ASPIRIN_2_GYRO_RANGE][0] -#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[ASPIRIN_2_GYRO_RANGE][1] -#endif - -// Set default sensitivity based on range if needed -#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS -#define IMU_ACCEL_X_SENS MPU60X0_ACCEL_SENS[ASPIRIN_2_ACCEL_RANGE] -#define IMU_ACCEL_X_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[ASPIRIN_2_ACCEL_RANGE][0] -#define IMU_ACCEL_X_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[ASPIRIN_2_ACCEL_RANGE][1] -#define IMU_ACCEL_Y_SENS MPU60X0_ACCEL_SENS[ASPIRIN_2_ACCEL_RANGE] -#define IMU_ACCEL_Y_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[ASPIRIN_2_ACCEL_RANGE][0] -#define IMU_ACCEL_Y_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[ASPIRIN_2_ACCEL_RANGE][1] -#define IMU_ACCEL_Z_SENS MPU60X0_ACCEL_SENS[ASPIRIN_2_ACCEL_RANGE] -#define IMU_ACCEL_Z_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[ASPIRIN_2_ACCEL_RANGE][0] -#define IMU_ACCEL_Z_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[ASPIRIN_2_ACCEL_RANGE][1] -#endif - struct ImuAspirin2Spi { struct Mpu60x0_Spi mpu; diff --git a/sw/airborne/modules/imu/imu_aspirin_defaults.h b/sw/airborne/modules/imu/imu_aspirin_defaults.h deleted file mode 100644 index bb47a630c4..0000000000 --- a/sw/airborne/modules/imu/imu_aspirin_defaults.h +++ /dev/null @@ -1,91 +0,0 @@ -/* - * Copyright (C) 2010-2013 The Paparazzi Team - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -/** - * @file modules/imu/imu_aspirin_defaults.h - * Default sensitivity definitions for IMU Aspirin 1x. - */ - - -#ifndef IMU_ASPIRIN_DEFAULTS_H -#define IMU_ASPIRIN_DEFAULTS_H - -#include "generated/airframe.h" - -#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS -#ifdef IMU_ASPIRIN_VERSION_1_5 -/** default gyro sensitivy and neutral from the datasheet - * IMU-3000 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 - */ -#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 -#else -/** default gyro sensitivy and neutral from the datasheet - * ITG3200 has 14.375 LSB/(deg/s) - * sens = 1/14.375 * pi/180 * 2^INT32_RATE_FRAC - * sens = 1/14.375 * pi/180 * 4096 = 4.973126 - */ -#define IMU_GYRO_P_SENS 4.973 -#define IMU_GYRO_P_SENS_NUM 4973 -#define IMU_GYRO_P_SENS_DEN 1000 -#define IMU_GYRO_Q_SENS 4.973 -#define IMU_GYRO_Q_SENS_NUM 4973 -#define IMU_GYRO_Q_SENS_DEN 1000 -#define IMU_GYRO_R_SENS 4.973 -#define IMU_GYRO_R_SENS_NUM 4973 -#define IMU_GYRO_R_SENS_DEN 1000 -#endif // IMU_ASPIRIN_VERSION_1_5 -#endif - - -/** default accel sensitivy from the ADXL345 datasheet - * sensitivity of x & y axes depends on supply voltage: - * - 256 LSB/g @ 2.5V - * - 265 LSB/g @ 3.3V - * z sensitivity stays at 256 LSB/g - * fixed point sens: 9.81 [m/s^2] / 256 [LSB/g] * 2^INT32_ACCEL_FRAC - * x/y sens = 9.81 / 265 * 1024 = 37.91 - * z sens = 9.81 / 256 * 1024 = 39.24 - * - * what about the offset at 3.3V? - */ -#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS -#define IMU_ACCEL_X_SENS 37.91 -#define IMU_ACCEL_X_SENS_NUM 3791 -#define IMU_ACCEL_X_SENS_DEN 100 -#define IMU_ACCEL_Y_SENS 37.91 -#define IMU_ACCEL_Y_SENS_NUM 3791 -#define IMU_ACCEL_Y_SENS_DEN 100 -#define IMU_ACCEL_Z_SENS 39.24 -#define IMU_ACCEL_Z_SENS_NUM 3924 -#define IMU_ACCEL_Z_SENS_DEN 100 -#endif - -#endif /* IMU_ASPIRIN_DEFAULTS_H */ diff --git a/sw/airborne/modules/imu/imu_aspirin_i2c.c b/sw/airborne/modules/imu/imu_aspirin_i2c.c index 1e7d0fa98c..513ca9cd12 100644 --- a/sw/airborne/modules/imu/imu_aspirin_i2c.c +++ b/sw/airborne/modules/imu/imu_aspirin_i2c.c @@ -21,10 +21,11 @@ */ /** - * @file modules/imu/imu_aspirin.c + * @file modules/imu/imu_aspirin_i2c.c * Driver for the Aspirin v1.x IMU using I2C for the accelerometer. */ +#include "modules/imu/imu_aspirin_i2c.h" #include "modules/imu/imu.h" #include "modules/core/abi.h" #include "mcu_periph/i2c.h" @@ -107,6 +108,27 @@ void imu_aspirin_i2c_init(void) /* interrupt on data ready, idle high, latch until read any register */ //itg_conf.int_cfg = (0x01 | (0x1<<4) | (0x1<<5) | 0x01<<7); + // Default scaling values +#ifdef IMU_ASPIRIN_VERSION_1_5 + const struct Int32Rates gyro_scale[2] = { + {4359, 4359, 4359}, + {1000, 1000, 1000} + }; +#else + const struct Int32Rates gyro_scale[2] = { + {4973, 4973, 4973}, + {1000, 1000, 1000} + }; +#endif + const struct Int32Vect3 accel_scale[2] = { + {3791, 3791, 3791}, + {100, 100, 100} + }; + + // Set the default scaling + imu_set_defaults_gyro(IMU_ASPIRIN_ID, NULL, NULL, gyro_scale); + imu_set_defaults_accel(IMU_ASPIRIN_ID, NULL, NULL, accel_scale); + /* initialize mag and set default options */ hmc58xx_init(&imu_aspirin.mag_hmc, &(ASPIRIN_I2C_DEV), HMC58XX_ADDR); #ifdef IMU_ASPIRIN_VERSION_1_0 @@ -132,33 +154,29 @@ void imu_aspirin_i2c_event(void) adxl345_i2c_event(&imu_aspirin.acc_adxl); if (imu_aspirin.acc_adxl.data_available) { - VECT3_COPY(imu.accel_unscaled, imu_aspirin.acc_adxl.data.vect); + AbiSendMsgIMU_ACCEL_RAW(IMU_ASPIRIN_ID, now_ts, &imu_aspirin.acc_adxl.data.vect, 1); imu_aspirin.acc_adxl.data_available = false; - imu_scale_accel(&imu); - AbiSendMsgIMU_ACCEL_INT32(IMU_ASPIRIN_ID, now_ts, &imu.accel); } /* If the itg3200 I2C transaction has succeeded: convert the data */ itg3200_event(&imu_aspirin.gyro_itg); if (imu_aspirin.gyro_itg.data_available) { - RATES_COPY(imu.gyro_unscaled, imu_aspirin.gyro_itg.data.rates); + AbiSendMsgIMU_GYRO_RAW(IMU_ASPIRIN_ID, now_ts, &imu_aspirin.gyro_itg.data.rates, 1); imu_aspirin.gyro_itg.data_available = false; - imu_scale_gyro(&imu); - AbiSendMsgIMU_GYRO_INT32(IMU_ASPIRIN_ID, now_ts, &imu.gyro); } /* HMC58XX event task */ hmc58xx_event(&imu_aspirin.mag_hmc); if (imu_aspirin.mag_hmc.data_available) { + struct Int32Vect3 mag; #ifdef IMU_ASPIRIN_VERSION_1_0 - VECT3_COPY(imu.mag_unscaled, imu_aspirin.mag_hmc.data.vect); + VECT3_COPY(mag, imu_aspirin.mag_hmc.data.vect); #else // aspirin 1.5 with hmc5883 - imu.mag_unscaled.x = imu_aspirin.mag_hmc.data.vect.y; - imu.mag_unscaled.y = -imu_aspirin.mag_hmc.data.vect.x; - imu.mag_unscaled.z = imu_aspirin.mag_hmc.data.vect.z; + mag.x = imu_aspirin.mag_hmc.data.vect.y; + mag.y = -imu_aspirin.mag_hmc.data.vect.x; + mag.z = imu_aspirin.mag_hmc.data.vect.z; #endif imu_aspirin.mag_hmc.data_available = false; - imu_scale_mag(&imu); - AbiSendMsgIMU_MAG_INT32(IMU_ASPIRIN_ID, now_ts, &imu.mag); + AbiSendMsgIMU_MAG_RAW(IMU_ASPIRIN_ID, now_ts, &mag); } } diff --git a/sw/airborne/modules/imu/imu_aspirin_i2c.h b/sw/airborne/modules/imu/imu_aspirin_i2c.h index 203c524cc9..8481cf8505 100644 --- a/sw/airborne/modules/imu/imu_aspirin_i2c.h +++ b/sw/airborne/modules/imu/imu_aspirin_i2c.h @@ -21,7 +21,7 @@ */ /** - * @file modules/imu/imu_aspirin.h + * @file modules/imu/imu_aspirin_i2c.h * Interface for the Aspirin v1.x IMU using I2C for the accelerometer. */ @@ -36,9 +36,6 @@ #include "peripherals/hmc58xx.h" #include "peripherals/adxl345_i2c.h" -/* include default aspirin sensitivity/channel definitions */ -#include "modules/imu/imu_aspirin_defaults.h" - struct ImuAspirinI2c { struct Adxl345_I2c acc_adxl; diff --git a/sw/airborne/modules/imu/imu_bebop.c b/sw/airborne/modules/imu/imu_bebop.c index 410e732fe3..61e4f1053c 100644 --- a/sw/airborne/modules/imu/imu_bebop.c +++ b/sw/airborne/modules/imu/imu_bebop.c @@ -24,6 +24,7 @@ * Driver for the Bebop (2) magnetometer, accelerometer and gyroscope */ +#include "modules/imu/imu_bebop.h" #include "modules/imu/imu.h" #include "modules/core/abi.h" #include "mcu_periph/i2c.h" @@ -80,6 +81,10 @@ void imu_bebop_init(void) imu_bebop.mpu.config.gyro_range = BEBOP_GYRO_RANGE; imu_bebop.mpu.config.accel_range = BEBOP_ACCEL_RANGE; + // Set the default scaling + imu_set_defaults_gyro(IMU_BOARD_ID, NULL, NULL, MPU60X0_GYRO_SENS_FRAC[BEBOP_GYRO_RANGE]); + imu_set_defaults_accel(IMU_BOARD_ID, NULL, NULL, MPU60X0_ACCEL_SENS_FRAC[BEBOP_ACCEL_RANGE]); + /* AKM8963 */ ak8963_init(&imu_bebop.ak, &(BEBOP_MAG_I2C_DEV), AK8963_ADDR); @@ -118,35 +123,35 @@ void imu_bebop_event(void) if (imu_bebop.mpu.data_available) { /* default orientation of the MPU is upside down sor corrigate this here */ - RATES_ASSIGN(imu.gyro_unscaled, imu_bebop.mpu.data_rates.rates.p, -imu_bebop.mpu.data_rates.rates.q, + struct Int32Rates gyro; + struct Int32Vect3 accel; + RATES_ASSIGN(gyro, imu_bebop.mpu.data_rates.rates.p, -imu_bebop.mpu.data_rates.rates.q, -imu_bebop.mpu.data_rates.rates.r); - VECT3_ASSIGN(imu.accel_unscaled, imu_bebop.mpu.data_accel.vect.x, -imu_bebop.mpu.data_accel.vect.y, + VECT3_ASSIGN(accel, imu_bebop.mpu.data_accel.vect.x, -imu_bebop.mpu.data_accel.vect.y, -imu_bebop.mpu.data_accel.vect.z); imu_bebop.mpu.data_available = false; - imu_scale_gyro(&imu); - imu_scale_accel(&imu); - AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro); - AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel); + AbiSendMsgIMU_GYRO_RAW(IMU_BOARD_ID, now_ts, &gyro, 1); + AbiSendMsgIMU_ACCEL_RAW(IMU_BOARD_ID, now_ts, &accel, 1); } /* AKM8963 event task */ ak8963_event(&imu_bebop.ak); if (imu_bebop.ak.data_available) { + struct Int32Vect3 mag; #if BEBOP_VERSION2 struct Int32Vect3 mag_temp; // In the second bebop version the magneto is turned 90 degrees VECT3_ASSIGN(mag_temp, imu_bebop.ak.data.vect.x, imu_bebop.ak.data.vect.y, imu_bebop.ak.data.vect.z); // Rotate the magneto struct Int32RMat *imu_to_mag_rmat = orientationGetRMat_i(&imu_to_mag_bebop); - int32_rmat_vmult(&imu.mag_unscaled, imu_to_mag_rmat, &mag_temp); + int32_rmat_vmult(&mag, imu_to_mag_rmat, &mag_temp); #else //BEBOP regular first verion - VECT3_ASSIGN(imu.mag_unscaled, -imu_bebop.ak.data.vect.y, imu_bebop.ak.data.vect.x, imu_bebop.ak.data.vect.z); + VECT3_ASSIGN(mag, -imu_bebop.ak.data.vect.y, imu_bebop.ak.data.vect.x, imu_bebop.ak.data.vect.z); #endif imu_bebop.ak.data_available = false; - imu_scale_mag(&imu); - AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag); + AbiSendMsgIMU_MAG_RAW(IMU_BOARD_ID, now_ts, &mag); } } diff --git a/sw/airborne/modules/imu/imu_bebop.h b/sw/airborne/modules/imu/imu_bebop.h index 2a97146839..b2e9346d00 100644 --- a/sw/airborne/modules/imu/imu_bebop.h +++ b/sw/airborne/modules/imu/imu_bebop.h @@ -42,33 +42,6 @@ #define BEBOP_ACCEL_RANGE MPU60X0_ACCEL_RANGE_8G #endif -// Set default sensitivity based on range if needed -#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS -#define IMU_GYRO_P_SENS MPU60X0_GYRO_SENS[BEBOP_GYRO_RANGE] -#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[BEBOP_GYRO_RANGE][0] -#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[BEBOP_GYRO_RANGE][1] -#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[BEBOP_GYRO_RANGE] -#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[BEBOP_GYRO_RANGE][0] -#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[BEBOP_GYRO_RANGE][1] -#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[BEBOP_GYRO_RANGE] -#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[BEBOP_GYRO_RANGE][0] -#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[BEBOP_GYRO_RANGE][1] -#endif - -// Set default sensitivity based on range if needed -#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS -#define IMU_ACCEL_X_SENS MPU60X0_ACCEL_SENS[BEBOP_ACCEL_RANGE] -#define IMU_ACCEL_X_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[BEBOP_ACCEL_RANGE][0] -#define IMU_ACCEL_X_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[BEBOP_ACCEL_RANGE][1] -#define IMU_ACCEL_Y_SENS MPU60X0_ACCEL_SENS[BEBOP_ACCEL_RANGE] -#define IMU_ACCEL_Y_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[BEBOP_ACCEL_RANGE][0] -#define IMU_ACCEL_Y_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[BEBOP_ACCEL_RANGE][1] -#define IMU_ACCEL_Z_SENS MPU60X0_ACCEL_SENS[BEBOP_ACCEL_RANGE] -#define IMU_ACCEL_Z_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[BEBOP_ACCEL_RANGE][0] -#define IMU_ACCEL_Z_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[BEBOP_ACCEL_RANGE][1] -#endif - - /** Everything that is in the bebop IMU */ struct ImuBebop { struct Mpu60x0_I2c mpu; ///< The MPU gyro/accel device diff --git a/sw/airborne/modules/imu/imu_bmi088_i2c.c b/sw/airborne/modules/imu/imu_bmi088_i2c.c index 958835a440..ee1f7312bb 100644 --- a/sw/airborne/modules/imu/imu_bmi088_i2c.c +++ b/sw/airborne/modules/imu/imu_bmi088_i2c.c @@ -25,6 +25,7 @@ * */ +#include "modules/imu/imu_bmi088_i2c.h" #include "modules/imu/imu.h" #include "mcu_periph/i2c.h" #include "mcu_periph/sys_time.h" @@ -98,6 +99,10 @@ void imu_bmi088_init(void) imu_bmi088.bmi.config.accel_range = IMU_BMI088_ACCEL_RANGE; imu_bmi088.bmi.config.accel_odr = IMU_BMI088_ACCEL_ODR; imu_bmi088.bmi.config.accel_bw = IMU_BMI088_ACCEL_BW; + + // Set the default scaling + imu_set_defaults_gyro(IMU_BMI088_ID, NULL, NULL, BMI088_GYRO_SENS_FRAC[IMU_BMI088_GYRO_RANGE]); + imu_set_defaults_accel(IMU_BMI088_ID, NULL, NULL, MI088_ACCEL_SENS_FRAC[IMU_BMI088_ACCEL_RANGE]); } void imu_bmi088_periodic(void) @@ -118,13 +123,9 @@ void imu_bmi088_event(void) IMU_BMI088_Y_SIGN *(int32_t)(imu_bmi088.bmi.data_rates.value[IMU_BMI088_CHAN_Y]), IMU_BMI088_Z_SIGN *(int32_t)(imu_bmi088.bmi.data_rates.value[IMU_BMI088_CHAN_Z]) }; - // unscaled vector - RATES_COPY(imu.gyro_unscaled, rates); imu_bmi088.bmi.gyro_available = false; - - imu_scale_gyro(&imu); - AbiSendMsgIMU_GYRO_INT32(IMU_BMI088_ID, now_ts, &imu.gyro); + AbiSendMsgIMU_GYRO_RAW(IMU_BMI088_ID, now_ts, &rates, 1); } if (imu_bmi088.bmi.accel_available) { // set channel order @@ -133,13 +134,9 @@ void imu_bmi088_event(void) IMU_BMI088_Y_SIGN *(int32_t)(imu_bmi088.bmi.data_accel.value[IMU_BMI088_CHAN_Y]), IMU_BMI088_Z_SIGN *(int32_t)(imu_bmi088.bmi.data_accel.value[IMU_BMI088_CHAN_Z]) }; - // unscaled vector - VECT3_COPY(imu.accel_unscaled, accel); imu_bmi088.bmi.accel_available = false; - - imu_scale_accel(&imu); - AbiSendMsgIMU_ACCEL_INT32(IMU_BMI088_ID, now_ts, &imu.accel); + AbiSendMsgIMU_ACCEL_RAW(IMU_BMI088_ID, now_ts, &accel, 1); } } diff --git a/sw/airborne/modules/imu/imu_bmi088_i2c.h b/sw/airborne/modules/imu/imu_bmi088_i2c.h index ac0d504245..384ff8ec85 100644 --- a/sw/airborne/modules/imu/imu_bmi088_i2c.h +++ b/sw/airborne/modules/imu/imu_bmi088_i2c.h @@ -43,33 +43,6 @@ #define IMU_BMI088_ACCEL_RANGE BMI088_ACCEL_RANGE_6G #endif -// Set default sensitivity based on range if needed -#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS -#define IMU_GYRO_P_SENS BMI088_GYRO_SENS[IMU_BMI088_GYRO_RANGE] -#define IMU_GYRO_P_SENS_NUM BMI088_GYRO_SENS_FRAC[IMU_BMI088_GYRO_RANGE][0] -#define IMU_GYRO_P_SENS_DEN BMI088_GYRO_SENS_FRAC[IMU_BMI088_GYRO_RANGE][1] -#define IMU_GYRO_Q_SENS BMI088_GYRO_SENS[IMU_BMI088_GYRO_RANGE] -#define IMU_GYRO_Q_SENS_NUM BMI088_GYRO_SENS_FRAC[IMU_BMI088_GYRO_RANGE][0] -#define IMU_GYRO_Q_SENS_DEN BMI088_GYRO_SENS_FRAC[IMU_BMI088_GYRO_RANGE][1] -#define IMU_GYRO_R_SENS BMI088_GYRO_SENS[IMU_BMI088_GYRO_RANGE] -#define IMU_GYRO_R_SENS_NUM BMI088_GYRO_SENS_FRAC[IMU_BMI088_GYRO_RANGE][0] -#define IMU_GYRO_R_SENS_DEN BMI088_GYRO_SENS_FRAC[IMU_BMI088_GYRO_RANGE][1] -#endif - -// Set default sensitivity based on range if needed -#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS -#define IMU_ACCEL_X_SENS BMI088_ACCEL_SENS[IMU_BMI088_ACCEL_RANGE] -#define IMU_ACCEL_X_SENS_NUM BMI088_ACCEL_SENS_FRAC[IMU_BMI088_ACCEL_RANGE][0] -#define IMU_ACCEL_X_SENS_DEN BMI088_ACCEL_SENS_FRAC[IMU_BMI088_ACCEL_RANGE][1] -#define IMU_ACCEL_Y_SENS BMI088_ACCEL_SENS[IMU_BMI088_ACCEL_RANGE] -#define IMU_ACCEL_Y_SENS_NUM BMI088_ACCEL_SENS_FRAC[IMU_BMI088_ACCEL_RANGE][0] -#define IMU_ACCEL_Y_SENS_DEN BMI088_ACCEL_SENS_FRAC[IMU_BMI088_ACCEL_RANGE][1] -#define IMU_ACCEL_Z_SENS BMI088_ACCEL_SENS[IMU_BMI088_ACCEL_RANGE] -#define IMU_ACCEL_Z_SENS_NUM BMI088_ACCEL_SENS_FRAC[IMU_BMI088_ACCEL_RANGE][0] -#define IMU_ACCEL_Z_SENS_DEN BMI088_ACCEL_SENS_FRAC[IMU_BMI088_ACCEL_RANGE][1] -#endif - - struct ImuBmi088 { struct Bmi088_I2c bmi; }; diff --git a/sw/airborne/modules/imu/imu_cube.h b/sw/airborne/modules/imu/imu_cube.h index 9d1272f36c..bfdb9ad311 100644 --- a/sw/airborne/modules/imu/imu_cube.h +++ b/sw/airborne/modules/imu/imu_cube.h @@ -29,32 +29,6 @@ #include "std.h" -// Set default sensitivity based on range if needed -#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS -#define IMU_GYRO_P_SENS 1 -#define IMU_GYRO_P_SENS_NUM 1 -#define IMU_GYRO_P_SENS_DEN 1 -#define IMU_GYRO_Q_SENS 1 -#define IMU_GYRO_Q_SENS_NUM 1 -#define IMU_GYRO_Q_SENS_DEN 1 -#define IMU_GYRO_R_SENS 1 -#define IMU_GYRO_R_SENS_NUM 1 -#define IMU_GYRO_R_SENS_DEN 1 -#endif - -// Set default sensitivity based on range if needed -#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS -#define IMU_ACCEL_X_SENS 1 -#define IMU_ACCEL_X_SENS_NUM 1 -#define IMU_ACCEL_X_SENS_DEN 1 -#define IMU_ACCEL_Y_SENS 1 -#define IMU_ACCEL_Y_SENS_NUM 1 -#define IMU_ACCEL_Y_SENS_DEN 1 -#define IMU_ACCEL_Z_SENS 1 -#define IMU_ACCEL_Z_SENS_NUM 1 -#define IMU_ACCEL_Z_SENS_DEN 1 -#endif - extern void imu_cube_init(void); extern void imu_cube_periodic(void); extern void imu_cube_event(void); diff --git a/sw/airborne/modules/imu/imu_disco.c b/sw/airborne/modules/imu/imu_disco.c index c6529720cd..dbac98d558 100644 --- a/sw/airborne/modules/imu/imu_disco.c +++ b/sw/airborne/modules/imu/imu_disco.c @@ -24,6 +24,7 @@ * Driver for the Disco magnetometer, accelerometer and gyroscope */ +#include "modules/imu/imu_disco.h" #include "modules/imu/imu.h" #include "modules/core/abi.h" #include "mcu_periph/i2c.h" @@ -78,6 +79,10 @@ void imu_disco_init(void) imu_disco.mpu.config.gyro_range = DISCO_GYRO_RANGE; imu_disco.mpu.config.accel_range = DISCO_ACCEL_RANGE; + // Set the default scaling + imu_set_defaults_gyro(IMU_BOARD_ID, NULL, NULL, MPU60X0_GYRO_SENS_FRAC[DISCO_GYRO_RANGE]); + imu_set_defaults_accel(IMU_BOARD_ID, NULL, NULL, MPU60X0_ACCEL_SENS_FRAC[DISCO_ACCEL_RANGE]); + /* AKM8963 */ ak8963_init(&imu_disco.ak, &(DISCO_MAG_I2C_DEV), AK8963_ADDR); } @@ -114,33 +119,27 @@ void imu_disco_event(void) if (imu_disco.mpu.data_available) { /* set correct orientation here */ - RATES_ASSIGN(imu.gyro_unscaled, + struct Int32Rates gyro; + struct Int32Vect3 accel; + RATES_ASSIGN(gyro, -imu_disco.mpu.data_rates.rates.p, -imu_disco.mpu.data_rates.rates.q, imu_disco.mpu.data_rates.rates.r); - VECT3_ASSIGN(imu.accel_unscaled, + VECT3_ASSIGN(accel, -imu_disco.mpu.data_accel.vect.x, -imu_disco.mpu.data_accel.vect.y, imu_disco.mpu.data_accel.vect.z); imu_disco.mpu.data_available = false; - imu_scale_gyro(&imu); - imu_scale_accel(&imu); - AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro); - AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel); + AbiSendMsgIMU_GYRO_RAW(IMU_BOARD_ID, now_ts, &gyro, 1); + AbiSendMsgIMU_ACCEL_RAW(IMU_BOARD_ID, now_ts, &accel, 1); } /* AKM8963 event task */ ak8963_event(&imu_disco.ak); if (imu_disco.ak.data_available) { - VECT3_ASSIGN(imu.mag_unscaled, - imu_disco.ak.data.vect.x, - imu_disco.ak.data.vect.y, - imu_disco.ak.data.vect.z); - - imu_disco.ak.data_available = false; - imu_scale_mag(&imu); - AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag); + AbiSendMsgIMU_MAG_RAW(IMU_BOARD_ID, now_ts, &imu_disco.ak.data.vect); + imu_disco.ak.data_available = false; } } diff --git a/sw/airborne/modules/imu/imu_disco.h b/sw/airborne/modules/imu/imu_disco.h index 18eaf0a1ad..ba5103ce95 100644 --- a/sw/airborne/modules/imu/imu_disco.h +++ b/sw/airborne/modules/imu/imu_disco.h @@ -42,32 +42,6 @@ #define DISCO_ACCEL_RANGE MPU60X0_ACCEL_RANGE_8G #endif -// Set default sensitivity based on range if needed -#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS -#define IMU_GYRO_P_SENS MPU60X0_GYRO_SENS[DISCO_GYRO_RANGE] -#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[DISCO_GYRO_RANGE][0] -#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[DISCO_GYRO_RANGE][1] -#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[DISCO_GYRO_RANGE] -#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[DISCO_GYRO_RANGE][0] -#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[DISCO_GYRO_RANGE][1] -#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[DISCO_GYRO_RANGE] -#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[DISCO_GYRO_RANGE][0] -#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[DISCO_GYRO_RANGE][1] -#endif - -// Set default sensitivity based on range if needed -#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS -#define IMU_ACCEL_X_SENS MPU60X0_ACCEL_SENS[DISCO_ACCEL_RANGE] -#define IMU_ACCEL_X_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[DISCO_ACCEL_RANGE][0] -#define IMU_ACCEL_X_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[DISCO_ACCEL_RANGE][1] -#define IMU_ACCEL_Y_SENS MPU60X0_ACCEL_SENS[DISCO_ACCEL_RANGE] -#define IMU_ACCEL_Y_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[DISCO_ACCEL_RANGE][0] -#define IMU_ACCEL_Y_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[DISCO_ACCEL_RANGE][1] -#define IMU_ACCEL_Z_SENS MPU60X0_ACCEL_SENS[DISCO_ACCEL_RANGE] -#define IMU_ACCEL_Z_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[DISCO_ACCEL_RANGE][0] -#define IMU_ACCEL_Z_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[DISCO_ACCEL_RANGE][1] -#endif - /** Everything that is in the disco IMU */ struct ImuDisco { struct Mpu60x0_I2c mpu; ///< The MPU gyro/accel device diff --git a/sw/airborne/modules/imu/imu_mpu6000.c b/sw/airborne/modules/imu/imu_mpu6000.c index dcf2e1b332..098063b9dd 100644 --- a/sw/airborne/modules/imu/imu_mpu6000.c +++ b/sw/airborne/modules/imu/imu_mpu6000.c @@ -25,6 +25,7 @@ * Driver for IMU with only MPU6000 via SPI. */ +#include "modules/imu/imu_mpu6000.h" #include "modules/imu/imu.h" #include "modules/core/abi.h" #include "mcu_periph/spi.h" @@ -126,6 +127,10 @@ void imu_mpu_spi_init(void) imu_mpu_spi.mpu.config.dlpf_cfg_acc = IMU_MPU_ACCEL_LOWPASS_FILTER; // only for ICM sensors imu_mpu_spi.mpu.config.gyro_range = IMU_MPU_GYRO_RANGE; imu_mpu_spi.mpu.config.accel_range = IMU_MPU_ACCEL_RANGE; + + // Set the default scaling + imu_set_defaults_gyro(IMU_MPU6000_ID, NULL, NULL, MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE]); + imu_set_defaults_accel(IMU_MPU6000_ID, NULL, NULL, MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE]); } void imu_mpu_spi_periodic(void) @@ -156,18 +161,11 @@ void imu_mpu_spi_event(void) UpdateMedianFilterVect3Int(medianfilter_accel, accel); UpdateMedianFilterRatesInt(medianfilter_rates, rates); #endif - // unscaled vector - VECT3_COPY(imu.accel_unscaled, accel); - RATES_COPY(imu.gyro_unscaled, rates); imu_mpu_spi.mpu.data_available = false; - // Scale the gyro and accelerometer - imu_scale_gyro(&imu); - imu_scale_accel(&imu); - // Send the scaled values over ABI - AbiSendMsgIMU_GYRO_INT32(IMU_MPU6000_ID, now_ts, &imu.gyro); - AbiSendMsgIMU_ACCEL_INT32(IMU_MPU6000_ID, now_ts, &imu.accel); + AbiSendMsgIMU_GYRO_RAW(IMU_MPU6000_ID, now_ts, &rates, 1); + AbiSendMsgIMU_ACCEL_RAW(IMU_MPU6000_ID, now_ts, &accel, 1); } } diff --git a/sw/airborne/modules/imu/imu_mpu6000.h b/sw/airborne/modules/imu/imu_mpu6000.h index a753133bdd..919157550f 100644 --- a/sw/airborne/modules/imu/imu_mpu6000.h +++ b/sw/airborne/modules/imu/imu_mpu6000.h @@ -33,6 +33,7 @@ #include "peripherals/mpu60x0_spi.h" +/* Default range configurations */ #ifndef IMU_MPU_GYRO_RANGE #define IMU_MPU_GYRO_RANGE MPU60X0_GYRO_RANGE_2000 #endif @@ -41,33 +42,6 @@ #define IMU_MPU_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G #endif -// Set default sensitivity based on range if needed -#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS -#define IMU_GYRO_P_SENS MPU60X0_GYRO_SENS[IMU_MPU_GYRO_RANGE] -#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][0] -#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][1] -#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[IMU_MPU_GYRO_RANGE] -#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][0] -#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][1] -#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[IMU_MPU_GYRO_RANGE] -#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][0] -#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][1] -#endif - -// Set default sensitivity based on range if needed -#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS -#define IMU_ACCEL_X_SENS MPU60X0_ACCEL_SENS[IMU_MPU_ACCEL_RANGE] -#define IMU_ACCEL_X_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][0] -#define IMU_ACCEL_X_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][1] -#define IMU_ACCEL_Y_SENS MPU60X0_ACCEL_SENS[IMU_MPU_ACCEL_RANGE] -#define IMU_ACCEL_Y_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][0] -#define IMU_ACCEL_Y_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][1] -#define IMU_ACCEL_Z_SENS MPU60X0_ACCEL_SENS[IMU_MPU_ACCEL_RANGE] -#define IMU_ACCEL_Z_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][0] -#define IMU_ACCEL_Z_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][1] -#endif - - struct ImuMpu6000 { struct Mpu60x0_Spi mpu; }; diff --git a/sw/airborne/modules/imu/imu_mpu6000_hmc5883.c b/sw/airborne/modules/imu/imu_mpu6000_hmc5883.c index 77fbcd5e6f..1f24628468 100644 --- a/sw/airborne/modules/imu/imu_mpu6000_hmc5883.c +++ b/sw/airborne/modules/imu/imu_mpu6000_hmc5883.c @@ -24,6 +24,7 @@ * Driver for IMU with MPU6000 via SPI and HMC5883 via I2c. */ +#include "modules/imu/imu_mpu6000_hmc5883.h" #include "modules/imu/imu.h" #include "modules/core/abi.h" #include "mcu_periph/spi.h" @@ -128,6 +129,10 @@ void imu_mpu_hmc_init(void) imu_mpu_hmc.mpu.config.gyro_range = IMU_MPU_GYRO_RANGE; imu_mpu_hmc.mpu.config.accel_range = IMU_MPU_ACCEL_RANGE; + // Set the default scaling + imu_set_defaults_gyro(IMU_MPU6000_HMC_ID, NULL, NULL, MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE]); + imu_set_defaults_accel(IMU_MPU6000_HMC_ID, NULL, NULL, MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE]); + /* initialize mag and set default options */ hmc58xx_init(&imu_mpu_hmc.hmc, &IMU_HMC_I2C_DEV, HMC58XX_ADDR); } @@ -160,26 +165,22 @@ void imu_mpu_hmc_event(void) IMU_MPU_Y_SIGN * (int32_t)(imu_mpu_hmc.mpu.data_rates.value[IMU_MPU_CHAN_Y]), IMU_MPU_Z_SIGN * (int32_t)(imu_mpu_hmc.mpu.data_rates.value[IMU_MPU_CHAN_Z]) }; - // unscaled vector - VECT3_COPY(imu.accel_unscaled, accel); - RATES_COPY(imu.gyro_unscaled, rates); imu_mpu_hmc.mpu.data_available = false; - imu_scale_gyro(&imu); - imu_scale_accel(&imu); - AbiSendMsgIMU_GYRO_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.gyro); - AbiSendMsgIMU_ACCEL_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.accel); + AbiSendMsgIMU_GYRO_RAW(IMU_MPU6000_HMC_ID, now_ts, &gyro, 1); + AbiSendMsgIMU_ACCEL_RAW(IMU_MPU6000_HMC_ID, now_ts, &accel, 1); } /* HMC58XX event task */ hmc58xx_event(&imu_mpu_hmc.hmc); if (imu_mpu_hmc.hmc.data_available) { /* mag by default rotated by 90deg around z axis relative to MPU */ - imu.mag_unscaled.x = IMU_HMC_X_SIGN * imu_mpu_hmc.hmc.data.value[IMU_HMC_CHAN_X]; - imu.mag_unscaled.y = IMU_HMC_Y_SIGN * imu_mpu_hmc.hmc.data.value[IMU_HMC_CHAN_Y]; - imu.mag_unscaled.z = IMU_HMC_Z_SIGN * imu_mpu_hmc.hmc.data.value[IMU_HMC_CHAN_Z]; + struct Int32Vect3 mag = { + IMU_HMC_X_SIGN * imu_mpu_hmc.hmc.data.value[IMU_HMC_CHAN_X], + IMU_HMC_Y_SIGN * imu_mpu_hmc.hmc.data.value[IMU_HMC_CHAN_Y], + IMU_HMC_Z_SIGN * imu_mpu_hmc.hmc.data.value[IMU_HMC_CHAN_Z] + }; imu_mpu_hmc.hmc.data_available = false; - imu_scale_mag(&imu); - AbiSendMsgIMU_MAG_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.mag); + AbiSendMsgIMU_MAG_RAW(IMU_MPU6000_HMC_ID, now_ts, &mag); } } diff --git a/sw/airborne/modules/imu/imu_mpu6000_hmc5883.h b/sw/airborne/modules/imu/imu_mpu6000_hmc5883.h index 7a669512ca..277cd37301 100644 --- a/sw/airborne/modules/imu/imu_mpu6000_hmc5883.h +++ b/sw/airborne/modules/imu/imu_mpu6000_hmc5883.h @@ -43,33 +43,6 @@ #define IMU_MPU_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G #endif -// Set default sensitivity based on range if needed -#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS -#define IMU_GYRO_P_SENS MPU60X0_GYRO_SENS[IMU_MPU_GYRO_RANGE] -#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][0] -#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][1] -#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[IMU_MPU_GYRO_RANGE] -#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][0] -#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][1] -#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[IMU_MPU_GYRO_RANGE] -#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][0] -#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][1] -#endif - -// Set default sensitivity based on range if needed -#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS -#define IMU_ACCEL_X_SENS MPU60X0_ACCEL_SENS[IMU_MPU_ACCEL_RANGE] -#define IMU_ACCEL_X_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][0] -#define IMU_ACCEL_X_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][1] -#define IMU_ACCEL_Y_SENS MPU60X0_ACCEL_SENS[IMU_MPU_ACCEL_RANGE] -#define IMU_ACCEL_Y_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][0] -#define IMU_ACCEL_Y_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][1] -#define IMU_ACCEL_Z_SENS MPU60X0_ACCEL_SENS[IMU_MPU_ACCEL_RANGE] -#define IMU_ACCEL_Z_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][0] -#define IMU_ACCEL_Z_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][1] -#endif - - struct ImuMpu6000Hmc5883 { struct Mpu60x0_Spi mpu; struct Hmc58xx hmc; diff --git a/sw/airborne/modules/imu/imu_mpu60x0_i2c.c b/sw/airborne/modules/imu/imu_mpu60x0_i2c.c index 3fe377be36..cce76c47f3 100644 --- a/sw/airborne/modules/imu/imu_mpu60x0_i2c.c +++ b/sw/airborne/modules/imu/imu_mpu60x0_i2c.c @@ -25,6 +25,7 @@ */ #include +#include "modules/imu/imu_mpu60x0_i2c.h" #include "modules/imu/imu.h" #include "modules/core/abi.h" #include "mcu_periph/i2c.h" @@ -70,6 +71,10 @@ void imu_mpu_i2c_init(void) imu_mpu_i2c.mpu.config.dlpf_cfg = IMU_MPU60X0_LOWPASS_FILTER; imu_mpu_i2c.mpu.config.gyro_range = IMU_MPU60X0_GYRO_RANGE; imu_mpu_i2c.mpu.config.accel_range = IMU_MPU60X0_ACCEL_RANGE; + + // Set the default scaling + imu_set_defaults_gyro(IMU_MPU60X0_ID, NULL, NULL, MPU60X0_GYRO_SENS_FRAC[IMU_MPU60X0_GYRO_RANGE]); + imu_set_defaults_accel(IMU_MPU60X0_ID, NULL, NULL, MPU60X0_ACCEL_SENS_FRAC[IMU_MPU60X0_ACCEL_RANGE]); } void imu_mpu_i2c_periodic(void) @@ -84,12 +89,8 @@ void imu_mpu_i2c_event(void) // If the MPU60X0 I2C transaction has succeeded: convert the data mpu60x0_i2c_event(&imu_mpu_i2c.mpu); if (imu_mpu_i2c.mpu.data_available) { - RATES_COPY(imu.gyro_unscaled, imu_mpu_i2c.mpu.data_rates.rates); - VECT3_COPY(imu.accel_unscaled, imu_mpu_i2c.mpu.data_accel.vect); + AbiSendMsgIMU_GYRO_RAW(IMU_MPU60X0_ID, now_ts, &imu_mpu_i2c.mpu.data_rates.rates, 1); + AbiSendMsgIMU_ACCEL_RAW(IMU_MPU60X0_ID, now_ts, &imu_mpu_i2c.mpu.data_accel.vect, 1); imu_mpu_i2c.mpu.data_available = false; - imu_scale_gyro(&imu); - imu_scale_accel(&imu); - AbiSendMsgIMU_GYRO_INT32(IMU_MPU60X0_ID, now_ts, &imu.gyro); - AbiSendMsgIMU_ACCEL_INT32(IMU_MPU60X0_ID, now_ts, &imu.accel); } } diff --git a/sw/airborne/modules/imu/imu_mpu60x0_i2c.h b/sw/airborne/modules/imu/imu_mpu60x0_i2c.h index 8a8eb213e1..cfae20c564 100644 --- a/sw/airborne/modules/imu/imu_mpu60x0_i2c.h +++ b/sw/airborne/modules/imu/imu_mpu60x0_i2c.h @@ -41,32 +41,6 @@ #define IMU_MPU60X0_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G #endif -// Set default sensitivity based on range if needed -#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS -#define IMU_GYRO_P_SENS MPU60X0_GYRO_SENS[IMU_MPU60X0_GYRO_RANGE] -#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[IMU_MPU60X0_GYRO_RANGE][0] -#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[IMU_MPU60X0_GYRO_RANGE][1] -#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[IMU_MPU60X0_GYRO_RANGE] -#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[IMU_MPU60X0_GYRO_RANGE][0] -#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[IMU_MPU60X0_GYRO_RANGE][1] -#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[IMU_MPU60X0_GYRO_RANGE] -#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[IMU_MPU60X0_GYRO_RANGE][0] -#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[IMU_MPU60X0_GYRO_RANGE][1] -#endif - -// Set default sensitivity based on range if needed -#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS -#define IMU_ACCEL_X_SENS MPU60X0_ACCEL_SENS[IMU_MPU60X0_ACCEL_RANGE] -#define IMU_ACCEL_X_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[IMU_MPU60X0_ACCEL_RANGE][0] -#define IMU_ACCEL_X_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[IMU_MPU60X0_ACCEL_RANGE][1] -#define IMU_ACCEL_Y_SENS MPU60X0_ACCEL_SENS[IMU_MPU60X0_ACCEL_RANGE] -#define IMU_ACCEL_Y_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[IMU_MPU60X0_ACCEL_RANGE][0] -#define IMU_ACCEL_Y_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[IMU_MPU60X0_ACCEL_RANGE][1] -#define IMU_ACCEL_Z_SENS MPU60X0_ACCEL_SENS[IMU_MPU60X0_ACCEL_RANGE] -#define IMU_ACCEL_Z_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[IMU_MPU60X0_ACCEL_RANGE][0] -#define IMU_ACCEL_Z_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[IMU_MPU60X0_ACCEL_RANGE][1] -#endif - struct ImuMpu60x0 { struct Mpu60x0_I2c mpu; }; diff --git a/sw/airborne/modules/imu/imu_mpu9250_i2c.c b/sw/airborne/modules/imu/imu_mpu9250_i2c.c index 46e3b25fde..bbbc4eb032 100644 --- a/sw/airborne/modules/imu/imu_mpu9250_i2c.c +++ b/sw/airborne/modules/imu/imu_mpu9250_i2c.c @@ -25,6 +25,7 @@ * */ +#include "modules/imu/imu_mpu9250_i2c.h" #include "modules/imu/imu.h" #include "mcu_periph/i2c.h" #include "mcu_periph/sys_time.h" @@ -109,6 +110,10 @@ void imu_mpu9250_init(void) imu_mpu9250.mpu.config.dlpf_accel_cfg = IMU_MPU9250_ACCEL_LOWPASS_FILTER; imu_mpu9250.mpu.config.gyro_range = IMU_MPU9250_GYRO_RANGE; imu_mpu9250.mpu.config.accel_range = IMU_MPU9250_ACCEL_RANGE; + + // Set the default scaling + imu_set_defaults_gyro(IMU_MPU9250_ID, NULL, NULL, MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE]); + imu_set_defaults_accel(IMU_MPU9250_ID, NULL, NULL, MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE]); } void imu_mpu9250_periodic(void) @@ -135,16 +140,10 @@ void imu_mpu9250_event(void) IMU_MPU9250_Y_SIGN *(int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Y]), IMU_MPU9250_Z_SIGN *(int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Z]) }; - // unscaled vector - VECT3_COPY(imu.accel_unscaled, accel); - RATES_COPY(imu.gyro_unscaled, rates); imu_mpu9250.mpu.data_available = false; - - imu_scale_gyro(&imu); - imu_scale_accel(&imu); - AbiSendMsgIMU_GYRO_INT32(IMU_MPU9250_ID, now_ts, &imu.gyro); - AbiSendMsgIMU_ACCEL_INT32(IMU_MPU9250_ID, now_ts, &imu.accel); + AbiSendMsgIMU_GYRO_RAW(IMU_MPU9250_ID, now_ts, &gyro, 1); + AbiSendMsgIMU_ACCEL_INT32(IMU_MPU9250_ID, now_ts, &accel, 1); } #if IMU_MPU9250_READ_MAG // Test if mag data are updated @@ -154,10 +153,8 @@ void imu_mpu9250_event(void) IMU_MPU9250_Y_SIGN *(int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_X]), -IMU_MPU9250_Z_SIGN *(int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_Z]) }; - VECT3_COPY(imu.mag_unscaled, mag); imu_mpu9250.mpu.akm.data_available = false; - imu_scale_mag(&imu); - AbiSendMsgIMU_MAG_INT32(IMU_MPU9250_ID, now_ts, &imu.mag); + AbiSendMsgIMU_MAG_RAW(IMU_MPU9250_ID, now_ts, &mag); } #endif diff --git a/sw/airborne/modules/imu/imu_mpu9250_i2c.h b/sw/airborne/modules/imu/imu_mpu9250_i2c.h index 746a0e474b..f044f138a3 100644 --- a/sw/airborne/modules/imu/imu_mpu9250_i2c.h +++ b/sw/airborne/modules/imu/imu_mpu9250_i2c.h @@ -43,33 +43,6 @@ #define IMU_MPU9250_ACCEL_RANGE MPU9250_ACCEL_RANGE_8G #endif -// Set default sensitivity based on range if needed -#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS -#define IMU_GYRO_P_SENS MPU9250_GYRO_SENS[IMU_MPU9250_GYRO_RANGE] -#define IMU_GYRO_P_SENS_NUM MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][0] -#define IMU_GYRO_P_SENS_DEN MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][1] -#define IMU_GYRO_Q_SENS MPU9250_GYRO_SENS[IMU_MPU9250_GYRO_RANGE] -#define IMU_GYRO_Q_SENS_NUM MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][0] -#define IMU_GYRO_Q_SENS_DEN MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][1] -#define IMU_GYRO_R_SENS MPU9250_GYRO_SENS[IMU_MPU9250_GYRO_RANGE] -#define IMU_GYRO_R_SENS_NUM MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][0] -#define IMU_GYRO_R_SENS_DEN MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][1] -#endif - -// Set default sensitivity based on range if needed -#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS -#define IMU_ACCEL_X_SENS MPU9250_ACCEL_SENS[IMU_MPU9250_ACCEL_RANGE] -#define IMU_ACCEL_X_SENS_NUM MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][0] -#define IMU_ACCEL_X_SENS_DEN MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][1] -#define IMU_ACCEL_Y_SENS MPU9250_ACCEL_SENS[IMU_MPU9250_ACCEL_RANGE] -#define IMU_ACCEL_Y_SENS_NUM MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][0] -#define IMU_ACCEL_Y_SENS_DEN MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][1] -#define IMU_ACCEL_Z_SENS MPU9250_ACCEL_SENS[IMU_MPU9250_ACCEL_RANGE] -#define IMU_ACCEL_Z_SENS_NUM MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][0] -#define IMU_ACCEL_Z_SENS_DEN MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][1] -#endif - - struct ImuMpu9250 { struct Mpu9250_I2c mpu; }; diff --git a/sw/airborne/modules/imu/imu_mpu9250_spi.c b/sw/airborne/modules/imu/imu_mpu9250_spi.c index 469dce6a0e..133c789a7a 100644 --- a/sw/airborne/modules/imu/imu_mpu9250_spi.c +++ b/sw/airborne/modules/imu/imu_mpu9250_spi.c @@ -25,6 +25,7 @@ * */ +#include "modules/imu/imu_mpu9250_spi.h" #include "modules/imu/imu.h" #include "modules/core/abi.h" #include "mcu_periph/sys_time.h" @@ -122,6 +123,9 @@ void imu_mpu9250_init(void) imu_mpu9250.mpu.config.gyro_range = IMU_MPU9250_GYRO_RANGE; imu_mpu9250.mpu.config.accel_range = IMU_MPU9250_ACCEL_RANGE; + // Set the default scaling + imu_set_defaults_gyro(IMU_MPU9250_ID, NULL, NULL, MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE]); + imu_set_defaults_accel(IMU_MPU9250_ID, NULL, NULL, MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE]); /* "internal" ak8963 magnetometer as I2C slave */ #if IMU_MPU9250_READ_MAG @@ -183,9 +187,6 @@ void imu_mpu9250_event(void) IMU_MPU9250_Y_SIGN * (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Y]), IMU_MPU9250_Z_SIGN * (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Z]) }; - // unscaled vector - VECT3_COPY(imu.accel_unscaled, accel); - RATES_COPY(imu.gyro_unscaled, rates); #if IMU_MPU9250_READ_MAG if (!bit_is_set(imu_mpu9250.mpu.data_ext[6], 3)) { //mag valid just HOFL == 0 @@ -194,18 +195,13 @@ void imu_mpu9250_event(void) mag.x = (IMU_MPU9250_X_SIGN) * Int16FromBuf(imu_mpu9250.mpu.data_ext, 2 * IMU_MPU9250_CHAN_Y); mag.y = (IMU_MPU9250_Y_SIGN) * Int16FromBuf(imu_mpu9250.mpu.data_ext, 2 * IMU_MPU9250_CHAN_X); mag.z = -(IMU_MPU9250_Z_SIGN) * Int16FromBuf(imu_mpu9250.mpu.data_ext, 2 * IMU_MPU9250_CHAN_Z); - VECT3_COPY(imu.mag_unscaled, mag); - imu_scale_mag(&imu); - AbiSendMsgIMU_MAG_INT32(IMU_MPU9250_ID, now_ts, &imu.mag); + AbiSendMsgIMU_MAG_RAW(IMU_MPU9250_ID, now_ts, &mag); } #endif imu_mpu9250.mpu.data_available = false; - - imu_scale_gyro(&imu); - imu_scale_accel(&imu); - AbiSendMsgIMU_GYRO_INT32(IMU_MPU9250_ID, now_ts, &imu.gyro); - AbiSendMsgIMU_ACCEL_INT32(IMU_MPU9250_ID, now_ts, &imu.accel); + AbiSendMsgIMU_GYRO_RAW(IMU_MPU9250_ID, now_ts, &rates, 1); + AbiSendMsgIMU_ACCEL_RAW(IMU_MPU9250_ID, now_ts, &accel, 1); } } diff --git a/sw/airborne/modules/imu/imu_mpu9250_spi.h b/sw/airborne/modules/imu/imu_mpu9250_spi.h index 7527427e15..836bfb0511 100644 --- a/sw/airborne/modules/imu/imu_mpu9250_spi.h +++ b/sw/airborne/modules/imu/imu_mpu9250_spi.h @@ -43,33 +43,6 @@ #define IMU_MPU9250_ACCEL_RANGE MPU9250_ACCEL_RANGE_8G #endif -// Set default sensitivity based on range if needed -#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS -#define IMU_GYRO_P_SENS MPU9250_GYRO_SENS[IMU_MPU9250_GYRO_RANGE] -#define IMU_GYRO_P_SENS_NUM MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][0] -#define IMU_GYRO_P_SENS_DEN MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][1] -#define IMU_GYRO_Q_SENS MPU9250_GYRO_SENS[IMU_MPU9250_GYRO_RANGE] -#define IMU_GYRO_Q_SENS_NUM MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][0] -#define IMU_GYRO_Q_SENS_DEN MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][1] -#define IMU_GYRO_R_SENS MPU9250_GYRO_SENS[IMU_MPU9250_GYRO_RANGE] -#define IMU_GYRO_R_SENS_NUM MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][0] -#define IMU_GYRO_R_SENS_DEN MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][1] -#endif - -// Set default sensitivity based on range if needed -#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS -#define IMU_ACCEL_X_SENS MPU9250_ACCEL_SENS[IMU_MPU9250_ACCEL_RANGE] -#define IMU_ACCEL_X_SENS_NUM MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][0] -#define IMU_ACCEL_X_SENS_DEN MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][1] -#define IMU_ACCEL_Y_SENS MPU9250_ACCEL_SENS[IMU_MPU9250_ACCEL_RANGE] -#define IMU_ACCEL_Y_SENS_NUM MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][0] -#define IMU_ACCEL_Y_SENS_DEN MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][1] -#define IMU_ACCEL_Z_SENS MPU9250_ACCEL_SENS[IMU_MPU9250_ACCEL_RANGE] -#define IMU_ACCEL_Z_SENS_NUM MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][0] -#define IMU_ACCEL_Z_SENS_DEN MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][1] -#endif - - struct ImuMpu9250 { struct Mpu9250_Spi mpu; diff --git a/sw/airborne/modules/imu/imu_nps.c b/sw/airborne/modules/imu/imu_nps.c index 28480c22d7..a3eb863f06 100644 --- a/sw/airborne/modules/imu/imu_nps.c +++ b/sw/airborne/modules/imu/imu_nps.c @@ -19,6 +19,7 @@ * Boston, MA 02111-1307, USA. */ +#include "modules/imu/imu_nps.h" #include "modules/imu/imu.h" #include "modules/core/abi.h" #include "generated/airframe.h" @@ -34,14 +35,39 @@ void imu_nps_init(void) imu_nps.mag_available = false; imu_nps.accel_available = false; + // Set the default scaling + const struct Int32Rates gyro_scale[2] = { + {NPS_GYRO_SENSITIVITY_NUM, NPS_GYRO_SENSITIVITY_NUM, NPS_GYRO_SENSITIVITY_NUM}, + {NPS_GYRO_SENSITIVITY_DEN, NPS_GYRO_SENSITIVITY_DEN, NPS_GYRO_SENSITIVITY_DEN} + }; + const struct Int32Rates gyro_neutral = { + NPS_GYRO_NEUTRAL_P, NPS_GYRO_NEUTRAL_Q, NPS_GYRO_NEUTRAL_R + }; + const struct Int32Vect3 accel_scale[2] = { + {NPS_ACCEL_SENSITIVITY_NUM, NPS_ACCEL_SENSITIVITY_NUM, NPS_ACCEL_SENSITIVITY_NUM}, + {NPS_ACCEL_SENSITIVITY_DEN, NPS_ACCEL_SENSITIVITY_DEN, NPS_ACCEL_SENSITIVITY_DEN} + }; + const struct Int32Vect3 accel_neutral = { + NPS_ACCEL_NEUTRAL_X, NPS_ACCEL_NEUTRAL_Y, NPS_ACCEL_NEUTRAL_Z + }; + const struct Int32Vect3 mag_scale[2] = { + {NPS_MAG_SENSITIVITY_NUM, NPS_MAG_SENSITIVITY_NUM, NPS_MAG_SENSITIVITY_NUM}, + {NPS_MAG_SENSITIVITY_DEN, NPS_MAG_SENSITIVITY_DEN, NPS_MAG_SENSITIVITY_DEN} + }; + const struct Int32Vect3 mag_neutral = { + NPS_MAG_NEUTRAL_X, NPS_MAG_NEUTRAL_Y, NPS_MAG_NEUTRAL_Z + }; + imu_set_defaults_gyro(IMU_NPS_ID, NULL, &gyro_neutral, gyro_scale); + imu_set_defaults_accel(IMU_NPS_ID, NULL, &accel_neutral, accel_scale); + imu_set_defaults_mag(IMU_NPS_ID, NULL, &mag_neutral, mag_scale); } void imu_feed_gyro_accel(void) { - RATES_ASSIGN(imu.gyro_unscaled, sensors.gyro.value.x, sensors.gyro.value.y, sensors.gyro.value.z); - VECT3_ASSIGN(imu.accel_unscaled, sensors.accel.value.x, sensors.accel.value.y, sensors.accel.value.z); + RATES_ASSIGN(imu_nps.gyro, sensors.gyro.value.x, sensors.gyro.value.y, sensors.gyro.value.z); + VECT3_ASSIGN(imu_nps.accel, sensors.accel.value.x, sensors.accel.value.y, sensors.accel.value.z); // set availability flags... imu_nps.accel_available = true; @@ -53,7 +79,7 @@ void imu_feed_gyro_accel(void) void imu_feed_mag(void) { - VECT3_ASSIGN(imu.mag_unscaled, sensors.mag.value.x, sensors.mag.value.y, sensors.mag.value.z); + VECT3_ASSIGN(imu_nps.mag, sensors.mag.value.x, sensors.mag.value.y, sensors.mag.value.z); imu_nps.mag_available = true; } @@ -62,18 +88,15 @@ void imu_nps_event(void) { uint32_t now_ts = get_sys_time_usec(); if (imu_nps.gyro_available) { + AbiSendMsgIMU_GYRO_RAW(IMU_NPS_ID, now_ts, &imu_nps.gyro, 1); imu_nps.gyro_available = false; - imu_scale_gyro(&imu); - AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro); } if (imu_nps.accel_available) { + AbiSendMsgIMU_ACCEL_RAW(IMU_NPS_ID, now_ts, &imu_nps.accel, 1); imu_nps.accel_available = false; - imu_scale_accel(&imu); - AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel); } if (imu_nps.mag_available) { + AbiSendMsgIMU_MAG_RAW(IMU_NPS_ID, now_ts, &imu_nps.mag); imu_nps.mag_available = false; - imu_scale_mag(&imu); - AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag); } } diff --git a/sw/airborne/modules/imu/imu_nps.h b/sw/airborne/modules/imu/imu_nps.h index 8155b7ef37..72edec50fa 100644 --- a/sw/airborne/modules/imu/imu_nps.h +++ b/sw/airborne/modules/imu/imu_nps.h @@ -26,53 +26,14 @@ #include "generated/airframe.h" -/** we just define some defaults for aspirin v1.5 for now - */ -#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 - - -/** we just define some defaults for aspirin v1.5 for now - */ -#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS -#define IMU_ACCEL_X_SENS 37.91 -#define IMU_ACCEL_X_SENS_NUM 3791 -#define IMU_ACCEL_X_SENS_DEN 100 -#define IMU_ACCEL_Y_SENS 37.91 -#define IMU_ACCEL_Y_SENS_NUM 3791 -#define IMU_ACCEL_Y_SENS_DEN 100 -#define IMU_ACCEL_Z_SENS 39.24 -#define IMU_ACCEL_Z_SENS_NUM 3924 -#define IMU_ACCEL_Z_SENS_DEN 100 -#endif - - -#if !defined IMU_MAG_X_SENS & !defined IMU_MAG_Y_SENS & !defined IMU_MAG_Z_SENS -#define IMU_MAG_X_SENS 3.5 -#define IMU_MAG_X_SENS_NUM 7 -#define IMU_MAG_X_SENS_DEN 2 -#define IMU_MAG_Y_SENS 3.5 -#define IMU_MAG_Y_SENS_NUM 7 -#define IMU_MAG_Y_SENS_DEN 2 -#define IMU_MAG_Z_SENS 3.5 -#define IMU_MAG_Z_SENS_NUM 7 -#define IMU_MAG_Z_SENS_DEN 2 -#endif - - struct ImuNps { uint8_t mag_available; uint8_t accel_available; uint8_t gyro_available; + + struct Int32Rates gyro; + struct Int32Vect3 accel; + struct Int32Vect3 mag; }; extern struct ImuNps imu_nps; diff --git a/sw/airborne/modules/imu/imu_px4_defaults.h b/sw/airborne/modules/imu/imu_px4_defaults.h deleted file mode 100644 index 11bc0702b1..0000000000 --- a/sw/airborne/modules/imu/imu_px4_defaults.h +++ /dev/null @@ -1,64 +0,0 @@ -/* - * Copyright (C) 2013 Felix Ruess - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - */ - -/** - * @file modules/imu/imu_px4_defaults.h - * Default sensitivity definitions for the Pixhawk IMU using the l3d20 gyro and lsm303dlc acc. - */ - -#ifndef IMU_PX4_DEFAULTS_H -#define IMU_PX4_DEFAULTS_H - -#include "generated/airframe.h" - -/** default gyro sensitivy and neutral from the datasheet - * L3GD20 has 70e-3 LSB/(deg/s) at 2000deg/s range - * sens = 70e-3 * pi/180 * 2^INT32_RATE_FRAC - * sens = (70e-3 / 180.0f) * pi * 4096 - */ -#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS -#define IMU_GYRO_P_SENS_NUM 5004 -#define IMU_GYRO_P_SENS_DEN 1000 -#define IMU_GYRO_Q_SENS_NUM 5004 -#define IMU_GYRO_Q_SENS_DEN 1000 -#define IMU_GYRO_R_SENS_NUM 5004 -#define IMU_GYRO_R_SENS_DEN 1000 -#endif - -/** default accel sensitivy from the datasheet - * LSM303DLHC has 732 LSB/g - * fixed point sens: 9.81 [m/s^2] / 732 [LSB/g] * 2^INT32_ACCEL_FRAC - * sens = 9.81 / 732 * 1024 = 13.72 - */ -#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS - -#define IMU_ACCEL_X_SENS 13.723 -#define IMU_ACCEL_X_SENS_NUM 13723 -#define IMU_ACCEL_X_SENS_DEN 1000 -#define IMU_ACCEL_Y_SENS 13.723 -#define IMU_ACCEL_Y_SENS_NUM 13723 -#define IMU_ACCEL_Y_SENS_DEN 1000 -#define IMU_ACCEL_Z_SENS 13.723 -#define IMU_ACCEL_Z_SENS_NUM 13723 -#define IMU_ACCEL_Z_SENS_DEN 1000 -#endif - -#endif /* IMU_PX4_DEFAULTS_H */ diff --git a/sw/airborne/modules/imu/imu_px4fmu.c b/sw/airborne/modules/imu/imu_px4fmu.c index a0d9554cc3..6d9feb0486 100644 --- a/sw/airborne/modules/imu/imu_px4fmu.c +++ b/sw/airborne/modules/imu/imu_px4fmu.c @@ -24,6 +24,7 @@ * Driver for the PX4FMU SPI1 for the MPU6000 and I2C2 for the HMC5883. */ +#include "modules/imu/imu_px4fmu.h" #include "modules/imu/imu.h" #include "modules/core/abi.h" #include "mcu_periph/spi.h" @@ -68,6 +69,10 @@ void imu_px4fmu_init(void) imu_px4fmu.mpu.config.gyro_range = PX4FMU_GYRO_RANGE; imu_px4fmu.mpu.config.accel_range = PX4FMU_ACCEL_RANGE; + // Set the default scaling + imu_set_defaults_gyro(IMU_BOARD_ID, NULL, NULL, MPU60X0_GYRO_SENS_FRAC[PX4FMU_GYRO_RANGE]); + imu_set_defaults_accel(IMU_BOARD_ID, NULL, NULL, MPU60X0_ACCEL_SENS_FRAC[PX4FMU_ACCEL_RANGE]); + /* initialize mag on i2c2 and set default options */ hmc58xx_init(&imu_px4fmu.hmc, &i2c2, HMC58XX_ADDR); } @@ -87,30 +92,31 @@ void imu_px4fmu_event(void) mpu60x0_spi_event(&imu_px4fmu.mpu); if (imu_px4fmu.mpu.data_available) { - RATES_ASSIGN(imu.gyro_unscaled, - imu_px4fmu.mpu.data_rates.rates.q, - imu_px4fmu.mpu.data_rates.rates.p, - -imu_px4fmu.mpu.data_rates.rates.r); - VECT3_ASSIGN(imu.accel_unscaled, - imu_px4fmu.mpu.data_accel.vect.y, - imu_px4fmu.mpu.data_accel.vect.x, - -imu_px4fmu.mpu.data_accel.vect.z); + struct Int32Rates gyro = { + imu_px4fmu.mpu.data_rates.rates.q, + imu_px4fmu.mpu.data_rates.rates.p, + -imu_px4fmu.mpu.data_rates.rates.r + }; + struct Int32Vect3 accel = { + imu_px4fmu.mpu.data_accel.vect.y, + imu_px4fmu.mpu.data_accel.vect.x, + -imu_px4fmu.mpu.data_accel.vect.z + }; imu_px4fmu.mpu.data_available = false; - imu_scale_gyro(&imu); - imu_scale_accel(&imu); - AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro); - AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel); + AbiSendMsgIMU_GYRO_RAW(IMU_BOARD_ID, now_ts, &gyro, 1); + AbiSendMsgIMU_ACCEL_RAW(IMU_BOARD_ID, now_ts, &accel, 1); } /* HMC58XX event task */ hmc58xx_event(&imu_px4fmu.hmc); if (imu_px4fmu.hmc.data_available) { - imu.mag_unscaled.x = imu_px4fmu.hmc.data.vect.y; - imu.mag_unscaled.y = imu_px4fmu.hmc.data.vect.x; - imu.mag_unscaled.z = -imu_px4fmu.hmc.data.vect.z; + struct Int32Vect3 mag = { + imu_px4fmu.hmc.data.vect.y, + imu_px4fmu.hmc.data.vect.x, + -imu_px4fmu.hmc.data.vect.z + }; imu_px4fmu.hmc.data_available = false; - imu_scale_mag(&imu); - AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag); + AbiSendMsgIMU_MAG_RAW(IMU_BOARD_ID, now_ts, &mag); } } diff --git a/sw/airborne/modules/imu/imu_px4fmu.h b/sw/airborne/modules/imu/imu_px4fmu.h index 4993aec414..08748849cf 100644 --- a/sw/airborne/modules/imu/imu_px4fmu.h +++ b/sw/airborne/modules/imu/imu_px4fmu.h @@ -37,38 +37,11 @@ #ifndef PX4FMU_GYRO_RANGE #define PX4FMU_GYRO_RANGE MPU60X0_GYRO_RANGE_2000 #endif -PRINT_CONFIG_VAR(PX4FMU_GYRO_RANGE) #ifndef PX4FMU_ACCEL_RANGE #define PX4FMU_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G #endif -// Set default sensitivity based on range if needed -#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS -#define IMU_GYRO_P_SENS MPU60X0_GYRO_SENS[PX4FMU_GYRO_RANGE] -#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[PX4FMU_GYRO_RANGE][0] -#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[PX4FMU_GYRO_RANGE][1] -#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[PX4FMU_GYRO_RANGE] -#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[PX4FMU_GYRO_RANGE][0] -#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[PX4FMU_GYRO_RANGE][1] -#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[PX4FMU_GYRO_RANGE] -#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[PX4FMU_GYRO_RANGE][0] -#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[PX4FMU_GYRO_RANGE][1] -#endif - -// Set default sensitivity based on range if needed -#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS -#define IMU_ACCEL_X_SENS MPU60X0_ACCEL_SENS[PX4FMU_ACCEL_RANGE] -#define IMU_ACCEL_X_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[PX4FMU_ACCEL_RANGE][0] -#define IMU_ACCEL_X_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[PX4FMU_ACCEL_RANGE][1] -#define IMU_ACCEL_Y_SENS MPU60X0_ACCEL_SENS[PX4FMU_ACCEL_RANGE] -#define IMU_ACCEL_Y_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[PX4FMU_ACCEL_RANGE][0] -#define IMU_ACCEL_Y_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[PX4FMU_ACCEL_RANGE][1] -#define IMU_ACCEL_Z_SENS MPU60X0_ACCEL_SENS[PX4FMU_ACCEL_RANGE] -#define IMU_ACCEL_Z_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[PX4FMU_ACCEL_RANGE][0] -#define IMU_ACCEL_Z_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[PX4FMU_ACCEL_RANGE][1] -#endif - struct ImuPx4fmu { struct Mpu60x0_Spi mpu; struct Hmc58xx hmc; diff --git a/sw/airborne/modules/imu/imu_px4fmu_v2.4.c b/sw/airborne/modules/imu/imu_px4fmu_v2.4.c index 7a40b8c678..fbcf75dd52 100644 --- a/sw/airborne/modules/imu/imu_px4fmu_v2.4.c +++ b/sw/airborne/modules/imu/imu_px4fmu_v2.4.c @@ -25,12 +25,10 @@ * Driver for pixhawk IMU's. * L3GD20H + LSM303D (both on spi) */ +#include "modules/imu/imu_px4fmu_v2.4.h" #include "modules/imu/imu.h" #include "modules/core/abi.h" #include "mcu_periph/spi.h" -#include "peripherals/l3gd20_regs.h" -#include "peripherals/lsm303d_regs.h" -//#include "peripherals/lsm303d_spi.h" /* SPI defaults set in subsystem makefile, can be configured from airframe file */ PRINT_CONFIG_VAR(IMU_LSM_SPI_SLAVE_IDX) @@ -44,9 +42,23 @@ void imu_px4_init(void) { /* initialize gyro and set default options */ l3gd20_spi_init(&imu_px4.l3g, &IMU_PX4FMU_SPI_DEV, IMU_L3G_SPI_SLAVE_IDX); - /* LSM303d acc + magneto init */ + /* LSM303d acc init */ lsm303d_spi_init(&imu_px4.lsm_acc, &IMU_PX4FMU_SPI_DEV, IMU_LSM_SPI_SLAVE_IDX, LSM303D_TARGET_ACC); + + // Set the default scaling + const struct Int32Rates gyro_scale[2] = { + {L3GD20_SENS_2000_NUM, L3GD20_SENS_2000_NUM, L3GD20_SENS_2000_NUM}, + {L3GD20_SENS_2000_DEN, L3GD20_SENS_2000_DEN, L3GD20_SENS_2000_DEN} + }; + const struct Int32Vect3 accel_scale[2] = { + {LSM303D_ACCEL_SENS_16G_NUM, LSM303D_ACCEL_SENS_16G_NUM, LSM303D_ACCEL_SENS_16G_NUM}, + {LSM303D_ACCEL_SENS_16G_DEN, LSM303D_ACCEL_SENS_16G_DEN, LSM303D_ACCEL_SENS_16G_DEN} + }; + imu_set_defaults_gyro(IMU_PX4_ID, NULL, NULL, gyro_scale); + imu_set_defaults_accel(IMU_PX4_ID, NULL, NULL, accel_scale); + #if !IMU_PX4_DISABLE_MAG + /* LSM303d mag init */ lsm303d_spi_init(&imu_px4.lsm_mag, &IMU_PX4FMU_SPI_DEV, IMU_LSM_SPI_SLAVE_IDX, LSM303D_TARGET_MAG); #endif @@ -72,29 +84,28 @@ void imu_px4_event(void) { l3gd20_spi_event(&imu_px4.l3g); if (imu_px4.l3g.data_available) { //the p and q seem to be swapped on the Pixhawk board compared to the acc - imu.gyro_unscaled.p = imu_px4.l3g.data_rates.rates.q; - imu.gyro_unscaled.q = -imu_px4.l3g.data_rates.rates.p; - imu.gyro_unscaled.r = imu_px4.l3g.data_rates.rates.r; + struct Int32Rates gyro = { + imu_px4.l3g.data_rates.rates.q, + -imu_px4.l3g.data_rates.rates.p, + imu_px4.l3g.data_rates.rates.r + }; imu_px4.l3g.data_available = FALSE; - imu_scale_gyro(&imu); - AbiSendMsgIMU_GYRO_INT32(IMU_PX4_ID, now_ts, &imu.gyro); + AbiSendMsgIMU_GYRO_RAW(IMU_PX4_ID, now_ts, &gyro, 1); } /* LSM303d event task */ lsm303d_spi_event(&imu_px4.lsm_acc); if (imu_px4.lsm_acc.data_available_acc) { - VECT3_COPY(imu.accel_unscaled, imu_px4.lsm_acc.data_accel.vect); + struct Int32Vect3 accel; + VECT3_COPY(accel, imu_px4.lsm_acc.data_accel.vect); + AbiSendMsgIMU_ACCEL_RAW(IMU_PX4_ID, now_ts, &accel, 1); imu_px4.lsm_acc.data_available_acc = FALSE; - imu_scale_accel(&imu); - AbiSendMsgIMU_ACCEL_INT32(IMU_PX4_ID, now_ts, &imu.accel); } #if !IMU_PX4_DISABLE_MAG lsm303d_spi_event(&imu_px4.lsm_mag); if (imu_px4.lsm_mag.data_available_mag) { - VECT3_COPY(imu.mag_unscaled, imu_px4.lsm_mag.data_mag.vect); + AbiSendMsgIMU_MAG_RAW(IMU_PX4_ID, now_ts, &imu_px4.lsm_mag.data_mag.vect); imu_px4.lsm_mag.data_available_mag = FALSE; - imu_scale_mag(&imu); - AbiSendMsgIMU_MAG_INT32(IMU_PX4_ID, now_ts, &imu.mag); } #endif diff --git a/sw/airborne/modules/imu/imu_px4fmu_v2.4.h b/sw/airborne/modules/imu/imu_px4fmu_v2.4.h index e091816b8d..32a997b8e7 100644 --- a/sw/airborne/modules/imu/imu_px4fmu_v2.4.h +++ b/sw/airborne/modules/imu/imu_px4fmu_v2.4.h @@ -32,7 +32,6 @@ #include "generated/airframe.h" #include "modules/imu/imu.h" -#include "modules/imu/imu_px4_defaults.h" #include "peripherals/l3gd20_spi.h" #include "peripherals/lsm303d_spi.h" diff --git a/sw/airborne/modules/imu/imu_vectornav.c b/sw/airborne/modules/imu/imu_vectornav.c index a3103ef171..0d62f7e927 100644 --- a/sw/airborne/modules/imu/imu_vectornav.c +++ b/sw/airborne/modules/imu/imu_vectornav.c @@ -39,10 +39,6 @@ struct ImuVectornav imu_vn; -/* no scaling */ -void imu_scale_gyro(struct Imu *_imu __attribute__((unused))) {} -void imu_scale_accel(struct Imu *_imu __attribute__((unused))) {} - #if PERIODIC_TELEMETRY #include "modules/datalink/telemetry.h" @@ -151,12 +147,8 @@ void imu_vectornav_propagate(void) uint64_t tmp = imu_vn.vn_data.nanostamp / 1000; uint32_t now_ts = (uint32_t) tmp; - // Rates and accel - RATES_BFP_OF_REAL(imu.gyro, imu_vn.vn_data.gyro); - ACCELS_BFP_OF_REAL(imu.accel, imu_vn.vn_data.accel); - // Send accel and gyro data separate for other AHRS algorithms - AbiSendMsgIMU_GYRO_INT32(IMU_VECTORNAV_ID, now_ts, &imu.gyro); - AbiSendMsgIMU_ACCEL_INT32(IMU_VECTORNAV_ID, now_ts, &imu.accel); + AbiSendMsgIMU_GYRO_RAW(IMU_VECTORNAV_ID, now_ts, &imu_vn.vn_data.gyro, 1); + AbiSendMsgIMU_ACCEL_RAW(IMU_VECTORNAV_ID, now_ts, &imu_vn.vn_data.accel, 1); } diff --git a/sw/airborne/modules/imu/imu_vectornav.h b/sw/airborne/modules/imu/imu_vectornav.h index e95c87a514..fbc3b80b01 100644 --- a/sw/airborne/modules/imu/imu_vectornav.h +++ b/sw/airborne/modules/imu/imu_vectornav.h @@ -52,29 +52,4 @@ void imu_vectornav_event(void); void imu_vectornav_periodic(void); void imu_vectornav_propagate(void); -/* no scaling (the WEAK attribute has no effect */ -#ifndef IMU_GYRO_P_SENS_NUM -#define IMU_GYRO_P_SENS_NUM 1 -#endif - -#ifndef IMU_GYRO_P_SENS_DEN -#define IMU_GYRO_P_SENS_DEN 1 -#endif - -#ifndef IMU_GYRO_Q_SENS_NUM -#define IMU_GYRO_Q_SENS_NUM 1 -#endif - -#ifndef IMU_GYRO_Q_SENS_DEN -#define IMU_GYRO_Q_SENS_DEN 1 -#endif - -#ifndef IMU_GYRO_R_SENS_NUM -#define IMU_GYRO_R_SENS_NUM 1 -#endif - -#ifndef IMU_GYRO_R_SENS_DEN -#define IMU_GYRO_R_SENS_DEN 1 -#endif - #endif /* IMU_VECTORNAV_H */ diff --git a/sw/airborne/modules/ins/imu_xsens.c b/sw/airborne/modules/ins/imu_xsens.c index 07c62f9680..52980ec8b3 100644 --- a/sw/airborne/modules/ins/imu_xsens.c +++ b/sw/airborne/modules/ins/imu_xsens.c @@ -55,41 +55,44 @@ static void handle_ins_msg(void) uint32_t now_ts = get_sys_time_usec(); #ifdef XSENS_BACKWARDS if (xsens.gyro_available) { - RATES_ASSIGN(imu.gyro_unscaled, -RATE_BFP_OF_REAL(xsens.gyro.p), -RATE_BFP_OF_REAL(xsens.gyro.q), RATE_BFP_OF_REAL(xsens.gyro.r)); - xsens.gyro_available = FALSE; - imu_scale_gyro(&imu); - AbiSendMsgIMU_GYRO_INT32(IMU_XSENS_ID, now_ts, &imu.gyro); + struct Int32Rates gyro = { + -RATE_BFP_OF_REAL(xsens.gyro.p), + -RATE_BFP_OF_REAL(xsens.gyro.q), + RATE_BFP_OF_REAL(xsens.gyro.r) + }; + xsens.gyro_available = FALSE + AbiSendMsgIMU_GYRO_RAW(IMU_XSENS_ID, now_ts, &gyro, 1); } if (xsens.accel_available) { - VECT3_ASSIGN(imu.accel_unscaled, -ACCEL_BFP_OF_REAL(xsens.accel.ax), -ACCEL_BFP_OF_REAL(xsens.accel.ay), ACCEL_BFP_OF_REAL(xsens.accel.az)); + struct Int32Vect3 accel = { + -ACCEL_BFP_OF_REAL(xsens.accel.ax), + -ACCEL_BFP_OF_REAL(xsens.accel.ay), + ACCEL_BFP_OF_REAL(xsens.accel.az) + }; xsens.accel_available = FALSE; - imu_scale_accel(&imu); - AbiSendMsgIMU_ACCEL_INT32(IMU_XSENS_ID, now_ts, &imu.accel); + AbiSendMsgIMU_ACCEL_RAW(IMU_XSENS_ID, now_ts, &accel, 1); } if (xsens.mag_available) { - VECT3_ASSIGN(imu.mag_unscaled, -MAG_BFP_OF_REAL(xsens.mag.mx), -MAG_BFP_OF_REAL(xsens.mag.my), MAG_BFP_OF_REAL(xsens.mag.mz)); + struct Int32Vect3 mag = { + -MAG_BFP_OF_REAL(xsens.mag.mx), + -MAG_BFP_OF_REAL(xsens.mag.my), + MAG_BFP_OF_REAL(xsens.mag.mz) + }; xsens.mag_available = FALSE; - imu_scale_mag(&imu); - AbiSendMsgIMU_MAG_INT32(IMU_XSENS_ID, now_ts, &imu.mag); + AbiSendMsgIMU_MAG_RAW(IMU_XSENS_ID, now_ts, &mag); } #else if (xsens.gyro_available) { - RATES_BFP_OF_REAL(imu.gyro_unscaled, xsens.gyro); + AbiSendMsgIMU_GYRO_RAW(IMU_XSENS_ID, now_ts, &xsens.gyro, 1); xsens.gyro_available = FALSE; - imu_scale_gyro(&imu); - AbiSendMsgIMU_GYRO_INT32(IMU_XSENS_ID, now_ts, &imu.gyro); } if (xsens.accel_available) { - ACCELS_BFP_OF_REAL(imu.accel_unscaled, xsens.accel); + AbiSendMsgIMU_ACCEL_RAW(IMU_XSENS_ID, now_ts, &xsens.accel, 1); xsens.accel_available = FALSE; - imu_scale_accel(&imu); - AbiSendMsgIMU_ACCEL_INT32(IMU_XSENS_ID, now_ts, &imu.accel); } if (xsens.mag_available) { - MAGS_BFP_OF_REAL(imu.mag_unscaled, xsens.mag); + AbiSendMsgIMU_MAG_RAW(IMU_XSENS_ID, now_ts, &xsens.mag); xsens.mag_available = FALSE; - imu_scale_mag(&imu); - AbiSendMsgIMU_MAG_INT32(IMU_XSENS_ID, now_ts, &imu.mag); } #endif /* XSENS_BACKWARDS */ } diff --git a/sw/airborne/modules/ins/ins_alt_float.c b/sw/airborne/modules/ins/ins_alt_float.c index 83555d45e8..9a8fcd5b24 100644 --- a/sw/airborne/modules/ins/ins_alt_float.c +++ b/sw/airborne/modules/ins/ins_alt_float.c @@ -131,7 +131,7 @@ void ins_alt_float_init(void) AbiBindMsgBARO_ABS(INS_ALT_BARO_ID, &baro_ev, baro_cb); #endif AbiBindMsgGPS(INS_ALT_GPS_ID, &gps_ev, gps_cb); - AbiBindMsgIMU_ACCEL_INT32(INS_ALT_IMU_ID, &accel_ev, accel_cb); + AbiBindMsgIMU_ACCEL(INS_ALT_IMU_ID, &accel_ev, accel_cb); AbiBindMsgBODY_TO_IMU_QUAT(INS_ALT_IMU_ID, &body_to_imu_ev, body_to_imu_cb); } diff --git a/sw/airborne/modules/ins/ins_float_invariant_wrapper.c b/sw/airborne/modules/ins/ins_float_invariant_wrapper.c index aa09aebcfa..38cb94939a 100644 --- a/sw/airborne/modules/ins/ins_float_invariant_wrapper.c +++ b/sw/airborne/modules/ins/ins_float_invariant_wrapper.c @@ -229,12 +229,12 @@ void ins_float_invariant_wrapper_init(void) // Bind to ABI messages AbiBindMsgBARO_ABS(INS_FINV_BARO_ID, &baro_ev, baro_cb); - AbiBindMsgIMU_GYRO_INT32(INS_FINV_IMU_ID, &gyro_ev, gyro_cb); - AbiBindMsgIMU_ACCEL_INT32(INS_FINV_IMU_ID, &accel_ev, accel_cb); + AbiBindMsgIMU_GYRO(INS_FINV_IMU_ID, &gyro_ev, gyro_cb); + AbiBindMsgIMU_ACCEL(INS_FINV_IMU_ID, &accel_ev, accel_cb); AbiBindMsgIMU_LOWPASSED(INS_FINV_IMU_ID, &aligner_ev, aligner_cb); AbiBindMsgBODY_TO_IMU_QUAT(INS_FINV_IMU_ID, &body_to_imu_ev, body_to_imu_cb); #if USE_MAGNETOMETER - AbiBindMsgIMU_MAG_INT32(INS_FINV_MAG_ID, &mag_ev, mag_cb); + AbiBindMsgIMU_MAG(INS_FINV_MAG_ID, &mag_ev, mag_cb); AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb); #endif AbiBindMsgGPS(INS_FINV_GPS_ID, &gps_ev, gps_cb); diff --git a/sw/airborne/modules/ins/ins_gps_passthrough.c b/sw/airborne/modules/ins/ins_gps_passthrough.c index 43627a4ad4..9e9ceb21ac 100644 --- a/sw/airborne/modules/ins/ins_gps_passthrough.c +++ b/sw/airborne/modules/ins/ins_gps_passthrough.c @@ -170,7 +170,7 @@ void ins_gps_passthrough_init(void) #endif AbiBindMsgGPS(INS_PT_GPS_ID, &gps_ev, gps_cb); - AbiBindMsgIMU_ACCEL_INT32(INS_PT_IMU_ID, &accel_ev, accel_cb); + AbiBindMsgIMU_ACCEL(INS_PT_IMU_ID, &accel_ev, accel_cb); AbiBindMsgBODY_TO_IMU_QUAT(INS_PT_IMU_ID, &body_to_imu_ev, body_to_imu_cb); } diff --git a/sw/airborne/modules/ins/ins_int.c b/sw/airborne/modules/ins/ins_int.c index d5a62ee57f..bfe4489a32 100644 --- a/sw/airborne/modules/ins/ins_int.c +++ b/sw/airborne/modules/ins/ins_int.c @@ -234,7 +234,7 @@ void ins_int_init(void) /* * Subscribe to scaled IMU measurements and attach callbacks */ - AbiBindMsgIMU_ACCEL_INT32(INS_INT_IMU_ID, &accel_ev, accel_cb); + AbiBindMsgIMU_ACCEL(INS_INT_IMU_ID, &accel_ev, accel_cb); AbiBindMsgGPS(INS_INT_GPS_ID, &gps_ev, gps_cb); AbiBindMsgVELOCITY_ESTIMATE(INS_INT_VEL_ID, &vel_est_ev, vel_est_cb); AbiBindMsgPOSITION_ESTIMATE(INS_INT_POS_ID, &pos_est_ev, pos_est_cb); diff --git a/sw/airborne/modules/ins/ins_mekf_wind_wrapper.c b/sw/airborne/modules/ins/ins_mekf_wind_wrapper.c index f866fc2ccc..3735504e88 100644 --- a/sw/airborne/modules/ins/ins_mekf_wind_wrapper.c +++ b/sw/airborne/modules/ins/ins_mekf_wind_wrapper.c @@ -589,9 +589,9 @@ void ins_mekf_wind_wrapper_init(void) AbiBindMsgBARO_DIFF(INS_MEKF_WIND_AIRSPEED_ID, &pressure_diff_ev, pressure_diff_cb); AbiBindMsgAIRSPEED(INS_MEKF_WIND_AIRSPEED_ID, &airspeed_ev, airspeed_cb); AbiBindMsgINCIDENCE(INS_MEKF_WIND_INCIDENCE_ID, &incidence_ev, incidence_cb); - AbiBindMsgIMU_MAG_INT32(INS_MEKF_WIND_MAG_ID, &mag_ev, mag_cb); - AbiBindMsgIMU_GYRO_INT32(INS_MEKF_WIND_IMU_ID, &gyro_ev, gyro_cb); - AbiBindMsgIMU_ACCEL_INT32(INS_MEKF_WIND_IMU_ID, &accel_ev, accel_cb); + AbiBindMsgIMU_MAG(INS_MEKF_WIND_MAG_ID, &mag_ev, mag_cb); + AbiBindMsgIMU_GYRO(INS_MEKF_WIND_IMU_ID, &gyro_ev, gyro_cb); + AbiBindMsgIMU_ACCEL(INS_MEKF_WIND_IMU_ID, &accel_ev, accel_cb); AbiBindMsgIMU_LOWPASSED(INS_MEKF_WIND_IMU_ID, &aligner_ev, aligner_cb); AbiBindMsgBODY_TO_IMU_QUAT(INS_MEKF_WIND_IMU_ID, &body_to_imu_ev, body_to_imu_cb); AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb); diff --git a/sw/airborne/modules/ins/ins_skeleton.c b/sw/airborne/modules/ins/ins_skeleton.c index 049e7fef13..7664da7542 100644 --- a/sw/airborne/modules/ins/ins_skeleton.c +++ b/sw/airborne/modules/ins/ins_skeleton.c @@ -237,7 +237,7 @@ void ins_module_wrapper_init(void) // Bind to ABI messages AbiBindMsgBARO_ABS(INS_MODULE_BARO_ID, &baro_ev, baro_cb); - AbiBindMsgIMU_ACCEL_INT32(INS_MODULE_IMU_ID, &accel_ev, accel_cb); + AbiBindMsgIMU_ACCEL(INS_MODULE_IMU_ID, &accel_ev, accel_cb); AbiBindMsgGPS(INS_MODULE_GPS_ID, &gps_ev, gps_cb); AbiBindMsgBODY_TO_IMU_QUAT(INS_MODULE_IMU_ID, &body_to_imu_ev, body_to_imu_cb); } diff --git a/sw/airborne/modules/meteo/wind_estimator.c b/sw/airborne/modules/meteo/wind_estimator.c index 23fadbbd41..aa4c1df104 100644 --- a/sw/airborne/modules/meteo/wind_estimator.c +++ b/sw/airborne/modules/meteo/wind_estimator.c @@ -314,11 +314,10 @@ void wind_estimator_periodic(void) // float_rmat_vmult(&accel_body, ned_to_body, &accel_ned); ///// IMU test - struct FloatVect3 accel_body = { - .x = ACCEL_FLOAT_OF_BFP(imu.accel.x), - .y = ACCEL_FLOAT_OF_BFP(imu.accel.y), - .z = ACCEL_FLOAT_OF_BFP(imu.accel.z) - }; + struct Int32Vect3 *accel_body_i = stateGetAccelBody_i(); + struct FloatVect3 accel_body; + ACCELS_FLOAT_OF_BFP(accel_body, *accel_body_i); + struct FloatVect3 tmp = accel_body; //struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&body_to_imu); //int32_rmat_transp_vmult(&accel_body, body_to_imu_rmat, accel); diff --git a/sw/airborne/modules/sensors/mag_hmc58xx.c b/sw/airborne/modules/sensors/mag_hmc58xx.c index 28c1d1dbf2..71a7607eff 100644 --- a/sw/airborne/modules/sensors/mag_hmc58xx.c +++ b/sw/airborne/modules/sensors/mag_hmc58xx.c @@ -104,15 +104,10 @@ void mag_hmc58xx_module_event(void) // rotate data from mag frame to imu frame int32_rmat_vmult(&imu_mag, &mag_to_imu, &mag); // unscaled vector - VECT3_COPY(imu.mag_unscaled, imu_mag); -#else - // unscaled vector - VECT3_COPY(imu.mag_unscaled, mag); + VECT3_COPY(mag, imu_mag); #endif - // scale vector - imu_scale_mag(&imu); - - AbiSendMsgIMU_MAG_INT32(MAG_HMC58XX_SENDER_ID, now_ts, &imu.mag); + + AbiSendMsgIMU_MAG_RAW(MAG_HMC58XX_SENDER_ID, now_ts, &mag); #endif #if MODULE_HMC58XX_SYNC_SEND mag_hmc58xx_report(); @@ -125,10 +120,11 @@ void mag_hmc58xx_module_event(void) void mag_hmc58xx_report(void) { + uint8_t id = MAG_HMC58XX_SENDER_ID; struct Int32Vect3 mag = { HMC58XX_CHAN_X_SIGN(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_X]), HMC58XX_CHAN_Y_SIGN(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Y]), HMC58XX_CHAN_Z_SIGN(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Z]) }; - DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &mag.x, &mag.y, &mag.z); + DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &id, &mag.x, &mag.y, &mag.z); } diff --git a/sw/airborne/modules/sensors/mag_ist8310.c b/sw/airborne/modules/sensors/mag_ist8310.c index 23e1b90d2b..8ad78ff844 100644 --- a/sw/airborne/modules/sensors/mag_ist8310.c +++ b/sw/airborne/modules/sensors/mag_ist8310.c @@ -52,13 +52,6 @@ #if MODULE_IST8310_UPDATE_AHRS #include "modules/imu/imu.h" #include "modules/core/abi.h" - -#if defined IST8310_MAG_TO_IMU_PHI && defined IST8310_MAG_TO_IMU_THETA && defined IST8310_MAG_TO_IMU_PSI -#define USE_MAG_TO_IMU 1 -static struct Int32RMat mag_to_imu; ///< rotation from mag to imu frame -#else -#define USE_MAG_TO_IMU 0 -#endif #endif struct IST8310 mag_ist8310; @@ -67,13 +60,16 @@ void mag_ist8310_module_init(void) { ist8310_init(&mag_ist8310, &(MAG_IST8310_I2C_DEV), IST8310_ADDR); -#if MODULE_IST8310_UPDATE_AHRS && USE_MAG_TO_IMU +#if MODULE_IST8310_UPDATE_AHRS && defined(IST8310_MAG_TO_IMU_PHI) && defined(IST8310_MAG_TO_IMU_THETA) && defined(IST8310_MAG_TO_IMU_PSI) + struct Int32RMat mag_to_imu; struct Int32Eulers mag_to_imu_eulers = { ANGLE_BFP_OF_REAL(IST8310_MAG_TO_IMU_PHI), ANGLE_BFP_OF_REAL(IST8310_MAG_TO_IMU_THETA), ANGLE_BFP_OF_REAL(IST8310_MAG_TO_IMU_PSI) }; int32_rmat_of_eulers(&mag_to_imu, &mag_to_imu_eulers); + + imu_set_defaults_mag(MAG_IST8310_SENDER_ID, &mag_to_imu, NULL, NULL) #endif } @@ -97,21 +93,8 @@ void mag_ist8310_module_event(void) IST8310_CHAN_Y_SIGN(int32_t)(mag_ist8310.data.value[IST8310_CHAN_Y]), IST8310_CHAN_Z_SIGN(int32_t)(mag_ist8310.data.value[IST8310_CHAN_Z]) }; - // only rotate if needed -#if USE_MAG_TO_IMU - struct Int32Vect3 imu_mag; - // rotate data from mag frame to imu frame - int32_rmat_vmult(&imu_mag, &mag_to_imu, &mag); - // unscaled vector - VECT3_COPY(imu.mag_unscaled, imu_mag); -#else - // unscaled vector - VECT3_COPY(imu.mag_unscaled, mag); -#endif - // scale vector - imu_scale_mag(&imu); - AbiSendMsgIMU_MAG_INT32(MAG_IST8310_SENDER_ID, now_ts, &imu.mag); + AbiSendMsgIMU_MAG_RAW(MAG_IST8310_SENDER_ID, now_ts, &mag); #endif #if MODULE_IST8310_SYNC_SEND mag_ist8310_report(); @@ -124,10 +107,11 @@ void mag_ist8310_module_event(void) void mag_ist8310_report(void) { + uint8_t id = MAG_IST8310_SENDER_ID; struct Int32Vect3 mag = { IST8310_CHAN_X_SIGN(int32_t)(mag_ist8310.data.value[IST8310_CHAN_X]), IST8310_CHAN_Y_SIGN(int32_t)(mag_ist8310.data.value[IST8310_CHAN_Y]), IST8310_CHAN_Z_SIGN(int32_t)(mag_ist8310.data.value[IST8310_CHAN_Z]) }; - DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &mag.x, &mag.y, &mag.z); + DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &id, &mag.x, &mag.y, &mag.z); } diff --git a/sw/airborne/modules/sensors/mag_pitot_uart.c b/sw/airborne/modules/sensors/mag_pitot_uart.c index f333e01b52..7fe1932bc1 100644 --- a/sw/airborne/modules/sensors/mag_pitot_uart.c +++ b/sw/airborne/modules/sensors/mag_pitot_uart.c @@ -40,17 +40,6 @@ static struct mag_pitot_t mag_pitot = { }; static uint8_t mp_msg_buf[128] __attribute__((aligned)); ///< The message buffer for the Magneto and pitot - -#if PERIODIC_TELEMETRY -#include "modules/datalink/telemetry.h" - -static void mag_pitot_raw_downlink(struct transport_tx *trans, struct link_device *dev) -{ - pprz_msg_send_IMU_MAG_RAW(trans, dev, AC_ID, &imu.mag_unscaled.x, &imu.mag_unscaled.y, - &imu.mag_unscaled.z); -} -#endif - /* Initialize the magneto and pitot */ void mag_pitot_init() { // Initialize transport protocol @@ -60,10 +49,6 @@ void mag_pitot_init() { struct FloatEulers imu_to_mag_eulers = {IMU_TO_MAG_PHI, IMU_TO_MAG_THETA, IMU_TO_MAG_PSI}; orientationSetEulers_f(&mag_pitot.imu_to_mag, &imu_to_mag_eulers); - -#if PERIODIC_TELEMETRY - register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG_RAW, mag_pitot_raw_downlink); -#endif } /* Parse the InterMCU message */ @@ -81,7 +66,7 @@ static inline void mag_pitot_parse_msg(void) switch (msg_id) { /* Got a magneto message */ case DL_IMCU_REMOTE_MAG: { - struct Int32Vect3 raw_mag; + struct Int32Vect3 raw_mag, mag; // Read the raw magneto information raw_mag.x = DL_IMCU_REMOTE_MAG_mag_x(mp_msg_buf); @@ -90,11 +75,10 @@ static inline void mag_pitot_parse_msg(void) // Rotate the magneto struct Int32RMat *imu_to_mag_rmat = orientationGetRMat_i(&mag_pitot.imu_to_mag); - int32_rmat_vmult(&imu.mag_unscaled, imu_to_mag_rmat, &raw_mag); + int32_rmat_vmult(&mag, imu_to_mag_rmat, &raw_mag); // Send and set the magneto IMU data - imu_scale_mag(&imu); - AbiSendMsgIMU_MAG_INT32(IMU_MAG_PITOT_ID, now_ts, &imu.mag); + AbiSendMsgIMU_MAG_RAW(IMU_MAG_PITOT_ID, now_ts, &mag); break; } diff --git a/sw/airborne/modules/sensors/mag_rm3100.c b/sw/airborne/modules/sensors/mag_rm3100.c index 507a828a2b..0f7e43dcb1 100644 --- a/sw/airborne/modules/sensors/mag_rm3100.c +++ b/sw/airborne/modules/sensors/mag_rm3100.c @@ -111,15 +111,10 @@ void mag_rm3100_module_event(void) // rotate data from mag frame to imu frame int32_rmat_vmult(&imu_mag, &mag_to_imu, &mag); // unscaled vector - VECT3_COPY(imu.mag_unscaled, imu_mag); -#else - // unscaled vector - VECT3_COPY(imu.mag_unscaled, mag); + VECT3_COPY(mag, imu_mag); #endif - // scale vector - imu_scale_mag(&imu); - AbiSendMsgIMU_MAG_INT32(MAG_RM3100_SENDER_ID, now_ts, &imu.mag); + AbiSendMsgIMU_MAG_RAW(MAG_RM3100_SENDER_ID, now_ts, &mag); #endif #if MODULE_RM3100_SYNC_SEND mag_rm3100_report(); @@ -132,10 +127,11 @@ void mag_rm3100_module_event(void) void mag_rm3100_report(void) { + uint8_t id = MAG_RM3100_SENDER_ID; struct Int32Vect3 mag = { RM3100_CHAN_X_SIGN(int32_t)(mag_rm3100.data.value[RM3100_CHAN_X]), RM3100_CHAN_Y_SIGN(int32_t)(mag_rm3100.data.value[RM3100_CHAN_Y]), RM3100_CHAN_Z_SIGN(int32_t)(mag_rm3100.data.value[RM3100_CHAN_Z]) }; - DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &mag.x, &mag.y, &mag.z); + DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &id, &mag.x, &mag.y, &mag.z); } diff --git a/sw/airborne/peripherals/bmi088.c b/sw/airborne/peripherals/bmi088.c index ee14de28de..3bd9915683 100644 --- a/sw/airborne/peripherals/bmi088.c +++ b/sw/airborne/peripherals/bmi088.c @@ -36,12 +36,17 @@ const float BMI088_GYRO_SENS[5] = { BMI088_GYRO_SENS_125, }; -const int32_t BMI088_GYRO_SENS_FRAC[5][2] = { - { BMI088_GYRO_SENS_2000_NUM, BMI088_GYRO_SENS_2000_DEN }, - { BMI088_GYRO_SENS_1000_NUM, BMI088_GYRO_SENS_1000_DEN }, - { BMI088_GYRO_SENS_500_NUM, BMI088_GYRO_SENS_500_DEN }, - { BMI088_GYRO_SENS_250_NUM, BMI088_GYRO_SENS_250_DEN }, - { BMI088_GYRO_SENS_125_NUM, BMI088_GYRO_SENS_125_DEN }, +const struct Int32Rates BMI088_GYRO_SENS_FRAC[5][2] = { + { {BMI088_GYRO_SENS_2000_NUM, BMI088_GYRO_SENS_2000_NUM, BMI088_GYRO_SENS_2000_NUM}, + {BMI088_GYRO_SENS_2000_DEN, BMI088_GYRO_SENS_2000_DEN, BMI088_GYRO_SENS_2000_DEN} }, + { {BMI088_GYRO_SENS_1000_NUM, BMI088_GYRO_SENS_1000_NUM, BMI088_GYRO_SENS_1000_NUM}, + {BMI088_GYRO_SENS_1000_DEN, BMI088_GYRO_SENS_1000_DEN, BMI088_GYRO_SENS_1000_DEN} }, + { {BMI088_GYRO_SENS_500_NUM, BMI088_GYRO_SENS_500_NUM, BMI088_GYRO_SENS_500_NUM}, + {BMI088_GYRO_SENS_500_DEN, BMI088_GYRO_SENS_500_DEN, BMI088_GYRO_SENS_500_DEN} }, + { {BMI088_GYRO_SENS_250_NUM, BMI088_GYRO_SENS_250_NUM, BMI088_GYRO_SENS_250_NUM}, + {BMI088_GYRO_SENS_250_DEN, BMI088_GYRO_SENS_250_DEN, BMI088_GYRO_SENS_250_DEN} }, + { {BMI088_GYRO_SENS_125_NUM, BMI088_GYRO_SENS_125_NUM, BMI088_GYRO_SENS_125_NUM}, + {BMI088_GYRO_SENS_125_DEN, BMI088_GYRO_SENS_125_DEN, BMI088_GYRO_SENS_125_DEN} } }; const float BMI088_ACCEL_SENS[4] = { @@ -51,11 +56,15 @@ const float BMI088_ACCEL_SENS[4] = { BMI088_ACCEL_SENS_24G }; -const int32_t BMI088_ACCEL_SENS_FRAC[4][2] = { - { BMI088_ACCEL_SENS_3G_NUM, BMI088_ACCEL_SENS_3G_DEN }, - { BMI088_ACCEL_SENS_6G_NUM, BMI088_ACCEL_SENS_6G_DEN }, - { BMI088_ACCEL_SENS_12G_NUM, BMI088_ACCEL_SENS_12G_DEN }, - { BMI088_ACCEL_SENS_24G_NUM, BMI088_ACCEL_SENS_24G_DEN } +const struct Int32Vect3 BMI088_ACCEL_SENS_FRAC[4][2] = { + { {BMI088_ACCEL_SENS_3G_NUM, BMI088_ACCEL_SENS_3G_NUM, BMI088_ACCEL_SENS_3G_NUM}, + {BMI088_ACCEL_SENS_3G_DEN, BMI088_ACCEL_SENS_3G_DEN, BMI088_ACCEL_SENS_3G_DEN} }, + { {BMI088_ACCEL_SENS_6G_NUM, BMI088_ACCEL_SENS_6G_NUM, BMI088_ACCEL_SENS_6G_NUM}, + {BMI088_ACCEL_SENS_6G_DEN, BMI088_ACCEL_SENS_6G_DEN, BMI088_ACCEL_SENS_6G_DEN} }, + { {BMI088_ACCEL_SENS_12G_NUM, BMI088_ACCEL_SENS_12G_NUM, BMI088_ACCEL_SENS_12G_NUM}, + {BMI088_ACCEL_SENS_12G_DEN, BMI088_ACCEL_SENS_12G_DEN, BMI088_ACCEL_SENS_12G_DEN} }, + { {BMI088_ACCEL_SENS_24G_NUM, BMI088_ACCEL_SENS_24G_NUM, BMI088_ACCEL_SENS_24G_NUM}, + {BMI088_ACCEL_SENS_24G_DEN, BMI088_ACCEL_SENS_24G_DEN, BMI088_ACCEL_SENS_24G_DEN} } }; void bmi088_set_default_config(struct Bmi088Config *c) diff --git a/sw/airborne/peripherals/bmi088.h b/sw/airborne/peripherals/bmi088.h index 17fb183d2c..0dafa09470 100644 --- a/sw/airborne/peripherals/bmi088.h +++ b/sw/airborne/peripherals/bmi088.h @@ -67,7 +67,7 @@ // Get default sensitivity from a table extern const float BMI088_GYRO_SENS[5]; // Get default sensitivity numerator and denominator from a table -extern const int32_t BMI088_GYRO_SENS_FRAC[5][2]; +extern const struct Int32Rates BMI088_GYRO_SENS_FRAC[5][2]; /** default accel sensitivy from the datasheet * sens = 9.81 [m/s^2] / [LSB/g] * 2^INT32_ACCEL_FRAC @@ -91,7 +91,7 @@ extern const int32_t BMI088_GYRO_SENS_FRAC[5][2]; // Get default sensitivity from a table extern const float BMI088_ACCEL_SENS[4]; // Get default sensitivity numerator and denominator from a table -extern const int32_t BMI088_ACCEL_SENS_FRAC[4][2]; +extern const struct Int32Vect3 BMI088_ACCEL_SENS_FRAC[4][2]; enum Bmi088ConfStatus { BMI088_CONF_UNINIT, diff --git a/sw/airborne/peripherals/l3gd20.h b/sw/airborne/peripherals/l3gd20.h index 93e5afe475..98dfe6848f 100644 --- a/sw/airborne/peripherals/l3gd20.h +++ b/sw/airborne/peripherals/l3gd20.h @@ -31,6 +31,15 @@ /* Include address and register definition */ #include "peripherals/l3gd20_regs.h" + +/** default gyro sensitivy and neutral from the datasheet + * L3GD20 has 70e-3 LSB/(deg/s) at 2000deg/s range + * sens = 70e-3 * pi/180 * 2^INT32_RATE_FRAC + * sens = (70e-3 / 180.0f) * pi * 4096 + */ +#define L3GD20_SENS_2000_NUM 5004 +#define L3GD20_SENS_2000_DEN 1000 + enum L3gd20ConfStatus { L3G_CONF_UNINIT = 0, L3G_CONF_WHO_AM_I = 1, diff --git a/sw/airborne/peripherals/lsm303d.h b/sw/airborne/peripherals/lsm303d.h index 165306cc79..af25f1c1e6 100644 --- a/sw/airborne/peripherals/lsm303d.h +++ b/sw/airborne/peripherals/lsm303d.h @@ -53,6 +53,14 @@ #define LSM303D_DEFAULT_MD (LSM303D_MAG_MODE_CONTINOUS_CONVERSION << 0) // Magneto continious conversion mode #endif +/** default accel sensitivy from the datasheet + * LSM303DLHC has 732 LSB/g + * fixed point sens: 9.81 [m/s^2] / 732 [LSB/g] * 2^INT32_ACCEL_FRAC + * sens = 9.81 / 732 * 1024 = 13.72 + */ +#define LSM303D_ACCEL_SENS_16G_NUM 13723 +#define LSM303D_ACCEL_SENS_16G_DEN 1000 + struct Lsm303dConfig { uint8_t acc_rate; ///< Data Output Rate (Hz) uint8_t acc_scale; ///< full scale selection (m/s²) diff --git a/sw/airborne/peripherals/mpu60x0.c b/sw/airborne/peripherals/mpu60x0.c index e094243fe0..bae2a3f928 100644 --- a/sw/airborne/peripherals/mpu60x0.c +++ b/sw/airborne/peripherals/mpu60x0.c @@ -36,11 +36,15 @@ const float MPU60X0_GYRO_SENS[4] = { MPU60X0_GYRO_SENS_2000 }; -const int32_t MPU60X0_GYRO_SENS_FRAC[4][2] = { - { MPU60X0_GYRO_SENS_250_NUM, MPU60X0_GYRO_SENS_250_DEN }, - { MPU60X0_GYRO_SENS_500_NUM, MPU60X0_GYRO_SENS_500_DEN }, - { MPU60X0_GYRO_SENS_1000_NUM, MPU60X0_GYRO_SENS_1000_DEN }, - { MPU60X0_GYRO_SENS_2000_NUM, MPU60X0_GYRO_SENS_2000_DEN } +const struct Int32Rates MPU60X0_GYRO_SENS_FRAC[4][2] = { + { {MPU60X0_GYRO_SENS_250_NUM, MPU60X0_GYRO_SENS_250_NUM, MPU60X0_GYRO_SENS_250_NUM}, + {MPU60X0_GYRO_SENS_250_DEN, MPU60X0_GYRO_SENS_250_DEN, MPU60X0_GYRO_SENS_250_DEN} }, + { {MPU60X0_GYRO_SENS_500_NUM, MPU60X0_GYRO_SENS_500_NUM, MPU60X0_GYRO_SENS_500_NUM}, + {MPU60X0_GYRO_SENS_500_DEN, MPU60X0_GYRO_SENS_500_DEN, MPU60X0_GYRO_SENS_500_DEN} }, + { {MPU60X0_GYRO_SENS_1000_NUM, MPU60X0_GYRO_SENS_1000_NUM, MPU60X0_GYRO_SENS_1000_NUM}, + {MPU60X0_GYRO_SENS_1000_DEN, MPU60X0_GYRO_SENS_1000_DEN, MPU60X0_GYRO_SENS_1000_DEN} }, + { {MPU60X0_GYRO_SENS_2000_NUM, MPU60X0_GYRO_SENS_2000_NUM, MPU60X0_GYRO_SENS_2000_NUM}, + {MPU60X0_GYRO_SENS_2000_DEN, MPU60X0_GYRO_SENS_2000_DEN, MPU60X0_GYRO_SENS_2000_DEN} } }; const float MPU60X0_ACCEL_SENS[4] = { @@ -50,11 +54,15 @@ const float MPU60X0_ACCEL_SENS[4] = { MPU60X0_ACCEL_SENS_16G }; -const int32_t MPU60X0_ACCEL_SENS_FRAC[4][2] = { - { MPU60X0_ACCEL_SENS_2G_NUM, MPU60X0_ACCEL_SENS_2G_DEN }, - { MPU60X0_ACCEL_SENS_4G_NUM, MPU60X0_ACCEL_SENS_4G_DEN }, - { MPU60X0_ACCEL_SENS_8G_NUM, MPU60X0_ACCEL_SENS_8G_DEN }, - { MPU60X0_ACCEL_SENS_16G_NUM, MPU60X0_ACCEL_SENS_16G_DEN } +const struct Int32Vect3 MPU60X0_ACCEL_SENS_FRAC[4][2] = { + { {MPU60X0_ACCEL_SENS_2G_NUM, MPU60X0_ACCEL_SENS_2G_NUM, MPU60X0_ACCEL_SENS_2G_NUM}, + {MPU60X0_ACCEL_SENS_2G_DEN, MPU60X0_ACCEL_SENS_2G_DEN, MPU60X0_ACCEL_SENS_2G_DEN} }, + { {MPU60X0_ACCEL_SENS_4G_NUM, MPU60X0_ACCEL_SENS_4G_NUM, MPU60X0_ACCEL_SENS_4G_NUM}, + {MPU60X0_ACCEL_SENS_4G_DEN, MPU60X0_ACCEL_SENS_4G_DEN, MPU60X0_ACCEL_SENS_4G_DEN} }, + { {MPU60X0_ACCEL_SENS_8G_NUM, MPU60X0_ACCEL_SENS_8G_NUM, MPU60X0_ACCEL_SENS_8G_NUM}, + {MPU60X0_ACCEL_SENS_8G_DEN, MPU60X0_ACCEL_SENS_8G_DEN, MPU60X0_ACCEL_SENS_8G_DEN} }, + { {MPU60X0_ACCEL_SENS_16G_NUM, MPU60X0_ACCEL_SENS_16G_NUM, MPU60X0_ACCEL_SENS_16G_NUM}, + {MPU60X0_ACCEL_SENS_16G_DEN, MPU60X0_ACCEL_SENS_16G_DEN, MPU60X0_ACCEL_SENS_16G_DEN} } }; void mpu60x0_set_default_config(struct Mpu60x0Config *c) diff --git a/sw/airborne/peripherals/mpu60x0.h b/sw/airborne/peripherals/mpu60x0.h index fe7032f220..d25b851255 100644 --- a/sw/airborne/peripherals/mpu60x0.h +++ b/sw/airborne/peripherals/mpu60x0.h @@ -29,6 +29,7 @@ #define MPU60X0_H #include "std.h" +#include "math/pprz_algebra_int.h" /* Include address and register definition */ #include "peripherals/mpu60x0_regs.h" @@ -74,7 +75,7 @@ // Get default sensitivity from a table extern const float MPU60X0_GYRO_SENS[4]; // Get default sensitivity numerator and denominator from a table -extern const int32_t MPU60X0_GYRO_SENS_FRAC[4][2]; +extern const struct Int32Rates MPU60X0_GYRO_SENS_FRAC[4][2]; /** default accel sensitivy from the datasheet * sens = 9.81 [m/s^2] / [LSB/g] * 2^INT32_ACCEL_FRAC @@ -97,7 +98,7 @@ extern const int32_t MPU60X0_GYRO_SENS_FRAC[4][2]; // Get default sensitivity from a table extern const float MPU60X0_ACCEL_SENS[4]; // Get default sensitivity numerator and denominator from a table -extern const int32_t MPU60X0_ACCEL_SENS_FRAC[4][2]; +extern const struct Int32Vect3 MPU60X0_ACCEL_SENS_FRAC[4][2]; /** MPU60x0 sensor type */ diff --git a/sw/airborne/peripherals/mpu9250.c b/sw/airborne/peripherals/mpu9250.c index c79ea9e224..7177bb36c9 100644 --- a/sw/airborne/peripherals/mpu9250.c +++ b/sw/airborne/peripherals/mpu9250.c @@ -36,11 +36,15 @@ const float MPU9250_GYRO_SENS[4] = { MPU9250_GYRO_SENS_2000 }; -const int32_t MPU9250_GYRO_SENS_FRAC[4][2] = { - { MPU9250_GYRO_SENS_250_NUM, MPU9250_GYRO_SENS_250_DEN }, - { MPU9250_GYRO_SENS_500_NUM, MPU9250_GYRO_SENS_500_DEN }, - { MPU9250_GYRO_SENS_1000_NUM, MPU9250_GYRO_SENS_1000_DEN }, - { MPU9250_GYRO_SENS_2000_NUM, MPU9250_GYRO_SENS_2000_DEN } +const struct Int32Rates MPU9250_GYRO_SENS_FRAC[4][2] = { + { {MPU9250_GYRO_SENS_250_NUM, MPU9250_GYRO_SENS_250_NUM, MPU9250_GYRO_SENS_250_NUM}, + {MPU9250_GYRO_SENS_250_DEN, MPU9250_GYRO_SENS_250_DEN, MPU9250_GYRO_SENS_250_DEN} }, + { {MPU9250_GYRO_SENS_500_NUM, MPU9250_GYRO_SENS_500_NUM, MPU9250_GYRO_SENS_500_NUM}, + {MPU9250_GYRO_SENS_500_DEN, MPU9250_GYRO_SENS_500_DEN, MPU9250_GYRO_SENS_500_DEN} }, + { {MPU9250_GYRO_SENS_1000_NUM, MPU9250_GYRO_SENS_1000_NUM, MPU9250_GYRO_SENS_1000_NUM}, + {MPU9250_GYRO_SENS_1000_DEN, MPU9250_GYRO_SENS_1000_DEN, MPU9250_GYRO_SENS_1000_DEN} }, + { {MPU9250_GYRO_SENS_2000_NUM, MPU9250_GYRO_SENS_2000_NUM, MPU9250_GYRO_SENS_2000_NUM}, + {MPU9250_GYRO_SENS_2000_DEN, MPU9250_GYRO_SENS_2000_DEN, MPU9250_GYRO_SENS_2000_DEN} } }; const float MPU9250_ACCEL_SENS[4] = { @@ -50,11 +54,15 @@ const float MPU9250_ACCEL_SENS[4] = { MPU9250_ACCEL_SENS_16G }; -const int32_t MPU9250_ACCEL_SENS_FRAC[4][2] = { - { MPU9250_ACCEL_SENS_2G_NUM, MPU9250_ACCEL_SENS_2G_DEN }, - { MPU9250_ACCEL_SENS_4G_NUM, MPU9250_ACCEL_SENS_4G_DEN }, - { MPU9250_ACCEL_SENS_8G_NUM, MPU9250_ACCEL_SENS_8G_DEN }, - { MPU9250_ACCEL_SENS_16G_NUM, MPU9250_ACCEL_SENS_16G_DEN } +const struct Int32Vect3 MPU9250_ACCEL_SENS_FRAC[4][2] = { + { {MPU9250_ACCEL_SENS_2G_NUM, MPU9250_ACCEL_SENS_2G_NUM, MPU9250_ACCEL_SENS_2G_NUM}, + {MPU9250_ACCEL_SENS_2G_DEN, MPU9250_ACCEL_SENS_2G_DEN, MPU9250_ACCEL_SENS_2G_DEN} }, + { {MPU9250_ACCEL_SENS_4G_NUM, MPU9250_ACCEL_SENS_4G_NUM, MPU9250_ACCEL_SENS_4G_NUM}, + {MPU9250_ACCEL_SENS_4G_DEN, MPU9250_ACCEL_SENS_4G_DEN, MPU9250_ACCEL_SENS_4G_DEN} }, + { {MPU9250_ACCEL_SENS_8G_NUM, MPU9250_ACCEL_SENS_8G_NUM, MPU9250_ACCEL_SENS_8G_NUM}, + {MPU9250_ACCEL_SENS_8G_DEN, MPU9250_ACCEL_SENS_8G_DEN, MPU9250_ACCEL_SENS_8G_DEN} }, + { {MPU9250_ACCEL_SENS_16G_NUM, MPU9250_ACCEL_SENS_16G_NUM, MPU9250_ACCEL_SENS_16G_NUM}, + {MPU9250_ACCEL_SENS_16G_DEN, MPU9250_ACCEL_SENS_16G_DEN, MPU9250_ACCEL_SENS_16G_DEN} } }; void mpu9250_set_default_config(struct Mpu9250Config *c) diff --git a/sw/airborne/peripherals/mpu9250.h b/sw/airborne/peripherals/mpu9250.h index db19d9d061..18bcf8afaf 100644 --- a/sw/airborne/peripherals/mpu9250.h +++ b/sw/airborne/peripherals/mpu9250.h @@ -28,6 +28,7 @@ #define MPU9250_H #include "std.h" +#include "math/pprz_algebra_int.h" /* Include address and register definition */ #include "peripherals/mpu9250_regs.h" @@ -73,7 +74,7 @@ // Get default sensitivity from a table extern const float MPU9250_GYRO_SENS[4]; // Get default sensitivity numerator and denominator from a table -extern const int32_t MPU9250_GYRO_SENS_FRAC[4][2]; +extern const struct Int32Rates MPU9250_GYRO_SENS_FRAC[4][2]; /** default accel sensitivy from the datasheet * sens = 9.81 [m/s^2] / [LSB/g] * 2^INT32_ACCEL_FRAC @@ -96,7 +97,7 @@ extern const int32_t MPU9250_GYRO_SENS_FRAC[4][2]; // Get default sensitivity from a table extern const float MPU9250_ACCEL_SENS[4]; // Get default sensitivity numerator and denominator from a table -extern const int32_t MPU9250_ACCEL_SENS_FRAC[4][2]; +extern const struct Int32Vect3 MPU9250_ACCEL_SENS_FRAC[4][2]; enum Mpu9250ConfStatus { MPU9250_CONF_UNINIT, diff --git a/sw/airborne/test/modules/test_imu.c b/sw/airborne/test/modules/test_imu.c index 69ceb2e8da..b61b5379e4 100644 --- a/sw/airborne/test/modules/test_imu.c +++ b/sw/airborne/test/modules/test_imu.c @@ -84,9 +84,9 @@ static inline void main_init(void) downlink_init(); pprz_dl_init(); - AbiBindMsgIMU_GYRO_INT32(ABI_BROADCAST, &gyro_ev, gyro_cb); - AbiBindMsgIMU_ACCEL_INT32(ABI_BROADCAST, &accel_ev, accel_cb); - AbiBindMsgIMU_MAG_INT32(ABI_BROADCAST, &mag_ev, mag_cb); + AbiBindMsgIMU_GYRO(ABI_BROADCAST, &gyro_ev, gyro_cb); + AbiBindMsgIMU_ACCEL(ABI_BROADCAST, &accel_ev, accel_cb); + AbiBindMsgIMU_MAG(ABI_BROADCAST, &mag_ev, mag_cb); } static inline void led_toggle(void) @@ -180,20 +180,10 @@ static void accel_cb(uint8_t sender_id __attribute__((unused)), #if USE_LED_3 RunOnceEvery(50, LED_TOGGLE(3)); #endif - static uint8_t cnt; - cnt++; - if (cnt > 15) { cnt = 0; } - if (cnt == 0) { - DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, - &imu.accel_unscaled.x, - &imu.accel_unscaled.y, - &imu.accel_unscaled.z); - } else if (cnt == 7) { - DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, DefaultDevice, - &accel->x, - &accel->y, - &accel->z); - } + DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, DefaultDevice, + &accel->x, + &accel->y, + &accel->z); } static void gyro_cb(uint8_t sender_id __attribute__((unused)), @@ -203,21 +193,10 @@ static void gyro_cb(uint8_t sender_id __attribute__((unused)), #if USE_LED_2 RunOnceEvery(50, LED_TOGGLE(2)); #endif - static uint8_t cnt; - cnt++; - if (cnt > 15) { cnt = 0; } - - if (cnt == 0) { - DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, - &imu.gyro_unscaled.p, - &imu.gyro_unscaled.q, - &imu.gyro_unscaled.r); - } else if (cnt == 7) { - DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, DefaultDevice, - &gyro->p, - &gyro->q, - &gyro->r); - } + DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, DefaultDevice, + &gyro->p, + &gyro->q, + &gyro->r); } @@ -225,21 +204,10 @@ static void mag_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp __attribute__((unused)), struct Int32Vect3 *mag) { - static uint8_t cnt; - cnt++; - if (cnt > 10) { cnt = 0; } - - if (cnt == 0) { - DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, DefaultDevice, - &mag->x, - &mag->y, - &mag->z); - } else if (cnt == 5) { - DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, - &imu.mag_unscaled.x, - &imu.mag_unscaled.y, - &imu.mag_unscaled.z); - } + DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, DefaultDevice, + &mag->x, + &mag->y, + &mag->z); } void dl_parse_msg(struct link_device *dev __attribute__((unused)), diff --git a/sw/simulator/nps/nps_ivy.c b/sw/simulator/nps/nps_ivy.c index 6de8c4212b..eafddf7067 100644 --- a/sw/simulator/nps/nps_ivy.c +++ b/sw/simulator/nps/nps_ivy.c @@ -26,8 +26,6 @@ #include "modules/gps/gps.h" #endif -#include NPS_SENSORS_PARAMS - pthread_t th_ivy_main; // runs main Ivy loop static MsgRcvPtr ivyPtr = NULL; static int seq = 1; diff --git a/sw/simulator/nps/nps_sensor_accel.c b/sw/simulator/nps/nps_sensor_accel.c index d75427962d..d96fcd600d 100644 --- a/sw/simulator/nps/nps_sensor_accel.c +++ b/sw/simulator/nps/nps_sensor_accel.c @@ -1,10 +1,8 @@ #include "nps_sensor_accel.h" -#include "generated/airframe.h" /* to get NPS_SENSORS_PARAMS */ - #include "nps_fdm.h" #include "nps_random.h" -#include NPS_SENSORS_PARAMS +#include "nps_sensors.h" #include "math/pprz_algebra_int.h" void nps_sensor_accel_init(struct NpsSensorAccel *accel, double time) diff --git a/sw/simulator/nps/nps_sensor_airspeed.c b/sw/simulator/nps/nps_sensor_airspeed.c index bf3ebfcb82..6d28aa0546 100644 --- a/sw/simulator/nps/nps_sensor_airspeed.c +++ b/sw/simulator/nps/nps_sensor_airspeed.c @@ -33,7 +33,7 @@ #include "std.h" #include "nps_fdm.h" #include "nps_random.h" -#include NPS_SENSORS_PARAMS +#include "nps_sensors.h" /// 10Hz default #ifndef NPS_AIRSPEED_DT diff --git a/sw/simulator/nps/nps_sensor_aoa.c b/sw/simulator/nps/nps_sensor_aoa.c index 5524cb69e6..a8894d940a 100644 --- a/sw/simulator/nps/nps_sensor_aoa.c +++ b/sw/simulator/nps/nps_sensor_aoa.c @@ -32,7 +32,7 @@ #include "std.h" #include "nps_fdm.h" #include "nps_random.h" -#include NPS_SENSORS_PARAMS +#include "nps_sensors.h" /// 10Hz default #ifndef NPS_AOA_DT diff --git a/sw/simulator/nps/nps_sensor_baro.c b/sw/simulator/nps/nps_sensor_baro.c index a56f6ae3ce..1d612c4561 100644 --- a/sw/simulator/nps/nps_sensor_baro.c +++ b/sw/simulator/nps/nps_sensor_baro.c @@ -5,7 +5,7 @@ #include "std.h" #include "nps_fdm.h" #include "nps_random.h" -#include NPS_SENSORS_PARAMS +#include "nps_sensors.h" #ifndef NPS_BARO_NOISE_STD_DEV #define NPS_BARO_NOISE_STD_DEV 0. diff --git a/sw/simulator/nps/nps_sensor_gps.c b/sw/simulator/nps/nps_sensor_gps.c index 9465c09df4..d7ec26689a 100644 --- a/sw/simulator/nps/nps_sensor_gps.c +++ b/sw/simulator/nps/nps_sensor_gps.c @@ -5,7 +5,7 @@ #include "nps_fdm.h" #include "nps_random.h" #include "nps_sensors_utils.h" -#include NPS_SENSORS_PARAMS +#include "nps_sensors.h" void nps_sensor_gps_init(struct NpsSensorGps *gps, double time) { diff --git a/sw/simulator/nps/nps_sensor_gyro.c b/sw/simulator/nps/nps_sensor_gyro.c index 1bf66296f7..6362a7d89c 100644 --- a/sw/simulator/nps/nps_sensor_gyro.c +++ b/sw/simulator/nps/nps_sensor_gyro.c @@ -2,7 +2,7 @@ #include "generated/airframe.h" #include "nps_fdm.h" -#include NPS_SENSORS_PARAMS +#include "nps_sensors.h" #include "math/pprz_algebra_int.h" #include "nps_random.h" diff --git a/sw/simulator/nps/nps_sensor_mag.c b/sw/simulator/nps/nps_sensor_mag.c index 1612c776bb..be0a1114ff 100644 --- a/sw/simulator/nps/nps_sensor_mag.c +++ b/sw/simulator/nps/nps_sensor_mag.c @@ -2,7 +2,7 @@ #include "generated/airframe.h" #include "nps_fdm.h" -#include NPS_SENSORS_PARAMS +#include "nps_sensors.h" #include "math/pprz_algebra_int.h" void nps_sensor_mag_init(struct NpsSensorMag *mag, double time) diff --git a/sw/simulator/nps/nps_sensor_sideslip.c b/sw/simulator/nps/nps_sensor_sideslip.c index f2fbab3c43..5e38af752d 100644 --- a/sw/simulator/nps/nps_sensor_sideslip.c +++ b/sw/simulator/nps/nps_sensor_sideslip.c @@ -33,7 +33,7 @@ #include "std.h" #include "nps_fdm.h" #include "nps_random.h" -#include NPS_SENSORS_PARAMS +#include "nps_sensors.h" /// 10Hz default #ifndef NPS_SIDESLIP_DT diff --git a/sw/simulator/nps/nps_sensor_sonar.c b/sw/simulator/nps/nps_sensor_sonar.c index ea33050888..d7b2e32cc0 100644 --- a/sw/simulator/nps/nps_sensor_sonar.c +++ b/sw/simulator/nps/nps_sensor_sonar.c @@ -33,7 +33,7 @@ #include "std.h" #include "nps_fdm.h" #include "nps_random.h" -#include NPS_SENSORS_PARAMS +#include "nps_sensors.h" /// 10Hz default #ifndef NPS_SONAR_DT diff --git a/sw/simulator/nps/nps_sensor_temperature.c b/sw/simulator/nps/nps_sensor_temperature.c index 1dd2791913..8af96bbcd1 100644 --- a/sw/simulator/nps/nps_sensor_temperature.c +++ b/sw/simulator/nps/nps_sensor_temperature.c @@ -5,7 +5,7 @@ #include "std.h" #include "nps_fdm.h" #include "nps_random.h" -#include NPS_SENSORS_PARAMS +#include "nps_sensors.h" /// 10Hz default #ifndef NPS_TEMPERATURE_DT diff --git a/sw/simulator/nps/nps_sensors.c b/sw/simulator/nps/nps_sensors.c index 11744c8e7d..d59c615795 100644 --- a/sw/simulator/nps/nps_sensors.c +++ b/sw/simulator/nps/nps_sensors.c @@ -1,8 +1,5 @@ #include "nps_sensors.h" -#include "generated/airframe.h" -#include NPS_SENSORS_PARAMS - struct NpsSensors sensors; void nps_sensors_init(double time) diff --git a/sw/simulator/nps/nps_sensors.h b/sw/simulator/nps/nps_sensors.h index 19f6d1bc18..93e66b72f9 100644 --- a/sw/simulator/nps/nps_sensors.h +++ b/sw/simulator/nps/nps_sensors.h @@ -2,6 +2,12 @@ #define NPS_SENSORS_H #include "math/pprz_algebra.h" + +#ifndef NPS_SENSORS_PARAMS +#include "nps_sensors_params_default.h" +#else +#include NPS_SENSORS_PARAMS +#endif /* NPS_SENSORS_PARAMS */ #include "nps_sensor_gyro.h" #include "nps_sensor_accel.h" #include "nps_sensor_mag.h" diff --git a/sw/tools/calibration/calibrate.py b/sw/tools/calibration/calibrate.py index 295d2d702f..d33a958929 100755 --- a/sw/tools/calibration/calibrate.py +++ b/sw/tools/calibration/calibrate.py @@ -78,7 +78,7 @@ def main(): if options.verbose: print("Using aircraft id "+options.ac_id) - sensor_ids = calibration_utils.get_ids_in_log(options.ac_id, filename, options.sensor) + sensor_ids = calibration_utils.get_sensor_ids(options.ac_id, filename, options.sensor) if options.sensor_id is None: if len(sensor_ids) == 1: options.sensor_id = sensor_ids[0] @@ -167,7 +167,7 @@ def main(): print("optimized guess : avg "+str(np1.mean())+" std "+str(np1.std())) if not optimze_failed: - calibration_utils.print_xml(p1, options.sensor, sensor_res) + calibration_utils.print_xml(p1, options.sensor, options.sensor_id, sensor_res) if options.plot: # if we are calibrating a mag, just draw first plot (non-blocking), then show the second diff --git a/sw/tools/calibration/calibration_utils.py b/sw/tools/calibration/calibration_utils.py index 121d93c14f..583f7dd89a 100644 --- a/sw/tools/calibration/calibration_utils.py +++ b/sw/tools/calibration/calibration_utils.py @@ -22,9 +22,11 @@ from __future__ import print_function, division import re +from telnetlib import theNULL import numpy as np from numpy import sin, cos from scipy import linalg, stats +from fractions import Fraction import matplotlib import matplotlib.pyplot as plt @@ -155,9 +157,43 @@ def estimate_mag_current_relation(meas): offset.append(intercept) return coefficient, offset +def continious_frac(v): + max_val = 2**16 + if v > 0: + s = 1 + else: + v = -v + s = -1 + return _continious_frac(v, max_val, int(v), v, (1, int(v)), (0,1), s) -def print_xml(p, sensor, res): +def _continious_frac(v, max_val, a, x, num, den, s): + x1 = 1 / (x - a) + a1 = int(x1) + (num1, num2) = num + num3 = a1 * num2 + num1 + (den1, den2) = den + den3 = a1 * den2 + den1 + if num3 > max_val or den3 > max_val: + return (num2, s*den2) + elif (num3 / den3) == v: + return (num3, s*den3) + else: + return _continious_frac(v, max_val, a1, x1, (num2, num3), (den2, den3), s) + +def print_xml(p, sensor, sensor_id, res): """Print xml for airframe file.""" + x_sens = continious_frac(p[3]*2**res) + y_sens = continious_frac(p[4]*2**res) + z_sens = continious_frac(p[5]*2**res) + + struct = "{{.abi_id="+sensor_id+", " + struct += ".neutral={"+str(int(round(p[0])))+","+str(int(round(p[1])))+","+str(int(round(p[2])))+"}, " + struct += ".scale={{"+str(x_sens[0])+","+str(y_sens[0])+","+str(z_sens[0])+"}," + struct += "{"+str(x_sens[1])+","+str(y_sens[1])+","+str(z_sens[1])+"}}" + struct += "}}" + + print("") + print("") print("") print("") print("") @@ -165,7 +201,6 @@ def print_xml(p, sensor, res): print("") print("") print("") - print("") def print_imu_scaled(sensor, measurements, attrs): diff --git a/sw/tools/px4/set_target.py b/sw/tools/px4/set_target.py index ef57bf4379..18f221be71 100755 --- a/sw/tools/px4/set_target.py +++ b/sw/tools/px4/set_target.py @@ -74,7 +74,7 @@ if mode == -1: # no pprz cdc was found, look for PX4 if mode == -1: print("No original PX4 CDC firmware found either.") print("Error: no compatible usb device found...") - sys.exit(1) + #sys.exit(1) if target == "fbw": print("Error: original firmware cannot be used to upload the fbw code. Wait for the PX4 bootloader to exit (takes 5 seconds), or in case this is the first upload; first upload the Paparazzi ap target.") diff --git a/tests/modules/generated/airframe.h b/tests/modules/generated/airframe.h index 65b06a5da2..2b11ce1b88 100644 --- a/tests/modules/generated/airframe.h +++ b/tests/modules/generated/airframe.h @@ -37,39 +37,6 @@ #define FAILSAFE_HOME_RADIUS 100 #define SECTION_IMU 1 -#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 -#define IMU_ACCEL_X_NEUTRAL -85 -#define IMU_ACCEL_Y_NEUTRAL 51 -#define IMU_ACCEL_Z_NEUTRAL -76 -#define IMU_ACCEL_X_SENS 2.44493965424 -#define IMU_ACCEL_X_SENS_NUM 61989 -#define IMU_ACCEL_X_SENS_DEN 25354 -#define IMU_ACCEL_Y_SENS 2.43424121497 -#define IMU_ACCEL_Y_SENS_NUM 61579 -#define IMU_ACCEL_Y_SENS_DEN 25297 -#define IMU_ACCEL_Z_SENS 2.44715908645 -#define IMU_ACCEL_Z_SENS_NUM 4307 -#define IMU_ACCEL_Z_SENS_DEN 1760 -#define IMU_MAG_X_NEUTRAL 0 -#define IMU_MAG_Y_NEUTRAL 0 -#define IMU_MAG_Z_NEUTRAL 0 -#define IMU_MAG_X_SENS 7.28514789391 -#define IMU_MAG_X_SENS_NUM 23888 -#define IMU_MAG_X_SENS_DEN 3279 -#define IMU_MAG_Y_SENS 7.33022132691 -#define IMU_MAG_Y_SENS_NUM 21199 -#define IMU_MAG_Y_SENS_DEN 2892 -#define IMU_MAG_Z_SENS 7.57102035692 -#define IMU_MAG_Z_SENS_NUM 63589 -#define IMU_MAG_Z_SENS_DEN 8399 #define IMU_BODY_TO_IMU_PHI 0. #define IMU_BODY_TO_IMU_THETA 0. #define IMU_BODY_TO_IMU_PSI 0