Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

[imu] Fix new imu structure

Browse Source
This commit is contained in:
Freek van Tienen
2022-05-18 13:21:22 +02:00
parent 7c7cfbc395
commit ff37e63af4
120 changed files with 826 additions and 1302 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/tudelft/nederdrone4.xml
+20 -18
View File
@@ -49,7 +49,7 @@
<define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/>
<!-- Use the external mag (not in NPS target as then it needs to listen to all) -->
<define name="INS_EKF2_MAG_ID" value="MAG_LIS3MDL_SENDER_ID"/>
<!--define name="INS_EKF2_MAG_ID" value="MAG_LIS3MDL_SENDER_ID"/-->
</target>
<target name="nps" board="pc">
@@ -88,17 +88,17 @@
<module name="send_imu_mag_current"/>
<!-- Internal MAG -->
<!--module name="mag_ist8310">
<module name="mag_ist8310">
<define name="MODULE_IST8310_UPDATE_AHRS" value="TRUE"/>
<configure name="MAG_IST8310_I2C_DEV" value="I2C3"/>
</module-->
</module>
<!-- External MAG on GPS -->
<module name="mag_lis3mdl">
<!--module name="mag_lis3mdl">
<define name="MODULE_LIS3MDL_UPDATE_AHRS" value="TRUE"/>
<configure name="MAG_LIS3MDL_I2C_DEV" value="I2C1"/>
<define name="LIS3MDL_CHAN_X_SIGN" value="-"/>
<define name="LIS3MDL_CHAN_Y_SIGN" value="-"/>
</module>
</module-->
<!--module name="lidar" type="tfmini">
<configure name="TFMINI_PORT" value="UART4"/>
<configure name="USE_TFMINI_AGL" value="FALSE"/>
@@ -246,20 +246,23 @@
<define name="MPU_Z_SIGN" value="-1"/>
<!-- Calibrated in the MAVLab 14-05-2020 -->
<define name="ACCEL_X_NEUTRAL" value="-337"/>
<define name="ACCEL_Y_NEUTRAL" value="64"/>
<define name="ACCEL_Z_NEUTRAL" value="-25"/>
<define name="ACCEL_X_SENS" value="4.670307671109528" integer="16"/>
<define name="ACCEL_Y_SENS" value="4.9016250738902425" integer="16"/>
<define name="ACCEL_Z_SENS" value="4.846689188075245" integer="16"/>
<define name="ACCEL_CALIB" value="{{.abi_id=9, .neutral={-423,18,-28}, .scale={{28111,6328,61706},{6151,1291,12775}}}}"/>
<define name="ACCEL_X_NEUTRAL" value="-423"/>
<define name="ACCEL_Y_NEUTRAL" value="18"/>
<define name="ACCEL_Z_NEUTRAL" value="-28"/>
<define name="ACCEL_X_SENS" value="4.570151198384357" integer="16"/>
<define name="ACCEL_Y_SENS" value="4.901626625187466" integer="16"/>
<define name="ACCEL_Z_SENS" value="4.830215267421878" integer="16"/>
<!-- Calibrated at valkenburg 20-05-2020 (external magnetometer) -->
<define name="MAG_X_NEUTRAL" value="866"/>
<define name="MAG_Y_NEUTRAL" value="-1530"/>
<define name="MAG_Z_NEUTRAL" value="-3313"/>
<define name="MAG_X_SENS" value="0.6067461130451115" integer="16"/>
<define name="MAG_Y_SENS" value="0.6544292255627779" integer="16"/>
<define name="MAG_Z_SENS" value="0.6352539557433349" integer="16"/>
<define name="MAG_CALIB" value="{{.abi_id=4, .neutral={12,113,49}, .scale={{42598,47669,16336},{3039,3365,1153}}}}"/>
<define name="MAG_X_NEUTRAL" value="12"/>
<define name="MAG_Y_NEUTRAL" value="113"/>
<define name="MAG_Z_NEUTRAL" value="49"/>
<define name="MAG_X_SENS" value="14.017110902869321" integer="16"/>
<define name="MAG_Y_SENS" value="14.166121879366417" integer="16"/>
<define name="MAG_Z_SENS" value="14.168256847606495" integer="16"/>
<!-- Define axis in hover frame -->
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
@@ -338,7 +341,6 @@
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="m1,m2,m3,m4,m5,m6,m7,m8,m9,m10,m11,m12,ail1,ail2,ail3,ail4,flap1,flap2,flap3,flap4" type="string[]"/>
<define name="JSBSIM_MODEL" value="nederdrone" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<!-- mode switch on joystick channel 5 (axis numbering starting at zero) -->
<define name="JS_AXIS_MODE" value="4"/>
</section>
conf/modules/imu_apogee.xml
-2
View File
@@ -26,8 +26,6 @@
<define name="IMU_APOGEE_I2C_DEV" value="i2c1"/>
<define name="USE_I2C1"/>
<define name="IMU_TYPE_H" value="boards/apogee/imu_apogee.h" type="string"/>
<file name="mpu60x0.c" dir="peripherals"/>
<file name="mpu60x0_i2c.c" dir="peripherals"/>
<file name="imu_apogee.c" dir="boards/apogee"/>
conf/modules/imu_ardrone2.xml
-2
View File
@@ -26,8 +26,6 @@
<configure name="AHRS_CORRECT_FREQUENCY" default="200"/>
<define name="AHRS_CORRECT_FREQUENCY" value="$(AHRS_CORRECT_FREQUENCY)"/>
<define name="IMU_TYPE_H" value="modules/imu/imu_ardrone2.h" type="string"/>
<file name="imu_ardrone2.c"/>
<file name="navdata.c" dir="boards/ardrone"/>
<test>
conf/modules/imu_aspirin_common.xml
-1
View File
@@ -45,7 +45,6 @@
<define name="ASPIRIN_SPI_SLAVE_IDX" value="$(ASPIRIN_SPI_SLAVE_IDX)"/>
<define name="USE_$(ASPIRIN_SPI_SLAVE_IDX)"/>
<define name="IMU_TYPE_H" value="imu/imu_aspirin.h" type="string"/>
<define name="ASPIRIN_ARCH_INDEP"/>
<file name="adxl345_spi.c" dir="peripherals"/>
conf/modules/imu_aspirin_i2c_common.xml
-2
View File
@@ -37,8 +37,6 @@
<define name="ASPIRIN_I2C_DEV" value="$(ASPIRIN_I2C_DEV_LOWER)"/>
<define name="USE_$(ASPIRIN_I2C_DEV_UPPER)"/>
<define name="IMU_TYPE_H" value="imu/imu_aspirin_i2c.h" type="string"/>
<file name="adxl345_i2c.c" dir="peripherals"/>
<file name="itg3200.c" dir="peripherals"/>
<file name="hmc58xx.c" dir="peripherals"/>
conf/modules/imu_aspirin_v2_common.xml
-2
View File
@@ -41,8 +41,6 @@
<define name="ASPIRIN_2_SPI_SLAVE_IDX" value="$(ASPIRIN_2_SPI_SLAVE_IDX)"/>
<define name="USE_$(ASPIRIN_2_SPI_SLAVE_IDX)"/>
<define name="IMU_TYPE_H" value="modules/imu/imu_aspirin_2_spi.h" type="string"/>
<file name="mpu60x0.c" dir="peripherals"/>
<file name="mpu60x0_spi.c" dir="peripherals"/>
<file name="imu_aspirin_2_spi.c"/>
conf/modules/imu_bebop.xml
-3
View File
@@ -36,8 +36,6 @@
<define name="BEBOP_MPU_I2C_DEV" value="i2c2"/>
<define name="USE_I2C2"/>
<define name="IMU_TYPE_H" value="modules/imu/imu_bebop.h" type="string"/>
<file name="mpu60x0.c" dir="peripherals"/>
<file name="mpu60x0_i2c.c" dir="peripherals"/>
<file name="ak8963.c" dir="peripherals"/>
@@ -47,7 +45,6 @@
<define name="USE_I2C1"/>
<define name="BEBOP_MPU_I2C_DEV" value="i2c2"/>
<define name="USE_I2C2"/>
<define name="IMU_TYPE_H" value="modules/imu/imu_bebop.h" type="string"/>
<define name="PERIODIC_FREQUENCY" value="512"/>
</test>
</makefile>
conf/modules/imu_bmi088_i2c.xml
-3
View File
@@ -36,13 +36,10 @@
<define name="IMU_BMI088_I2C_DEV" value="$(IMU_BMI088_I2C_DEV_LOWER)"/>
<define name="USE_$(IMU_BMI088_I2C_DEV_UPPER)"/>
<define name="IMU_TYPE_H" value="modules/imu/imu_bmi088_i2c.h" type="string"/>
<file name="bmi088.c" dir="peripherals"/>
<file name="bmi088_i2c.c" dir="peripherals"/>
<file name="imu_bmi088_i2c.c"/>
<test>
<define name="IMU_TYPE_H" value="modules/imu/imu_bmi088_i2c.h" type="string"/>
<define name="USE_I2C1"/>
<define name="IMU_BMI088_I2C_DEV" value="i2c1"/>
<define name="DOWNLINK_TRANSPORT" value="pprz_tp"/>
conf/modules/imu_common.xml
+2
View File
@@ -4,6 +4,8 @@
<doc>
<description>
Common part for all IMUs.
This takes the IMU_GYRO_RAW, IMU_ACCEL_RAW and IMU_MAG_RAW ABI messages as input.
</description>
</doc>
<settings>
conf/modules/imu_cube.xml
-2
View File
@@ -19,8 +19,6 @@
<periodic fun="imu_cube_periodic()"/>
<event fun="imu_cube_event()"/>
<makefile target="!sim|nps|fbw">
<define name="IMU_TYPE_H" value="modules/imu/imu_cube.h" type="string"/>
<!-- ICM20602 (isolated) -->
<configure name="CUBE_IMU1_SPI_DEV" default="spi4" case="lower|upper"/>
<configure name="CUBE_IMU1_SPI_SLAVE_IDX" default="SPI_SLAVE3"/>
conf/modules/imu_disco.xml
-2
View File
@@ -33,13 +33,11 @@
<makefile target="!sim|nps|fbw">
<define name="USE_I2C1"/>
<define name="USE_I2C2"/>
<define name="IMU_TYPE_H" value="modules/imu/imu_disco.h" type="string"/>
<file name="mpu60x0.c" dir="peripherals"/>
<file name="mpu60x0_i2c.c" dir="peripherals"/>
<file name="ak8963.c" dir="peripherals"/>
<file name="imu_disco.c"/>
<test>
<define name="IMU_TYPE_H" value="modules/imu/imu_disco.h" type="string"/>
<define name="USE_I2C2"/>
<define name="USE_I2C1"/>
<define name="PERIODIC_FREQUENCY" value="512"/>
conf/modules/imu_mpu6000.xml
-3
View File
@@ -34,13 +34,10 @@
<define name="IMU_MPU_SPI_SLAVE_IDX" value="$(IMU_MPU_SPI_SLAVE_IDX)"/>
<define name="USE_$(IMU_MPU_SPI_SLAVE_IDX)"/>
<define name="IMU_TYPE_H" value="modules/imu/imu_mpu6000.h" type="string"/>
<file name="mpu60x0.c" dir="peripherals"/>
<file name="mpu60x0_spi.c" dir="peripherals"/>
<file name="imu_mpu6000.c"/>
<test>
<define name="IMU_TYPE_H" value="modules/imu/imu_mpu6000.h" type="string"/>
<define name="SPI_MASTER"/>
<define name="IMU_MPU_SPI_DEV" value="spi1"/>
<define name="USE_SPI1"/>
conf/modules/imu_mpu6000_hmc5883.xml
-3
View File
@@ -56,14 +56,11 @@
<define name="IMU_HMC_I2C_DEV" value="$(IMU_HMC_I2C_DEV_LOWER)"/>
<define name="USE_$(IMU_HMC_I2C_DEV_UPPER)"/>
<define name="IMU_TYPE_H" value="modules/imu/imu_mpu6000_hmc5883.h" type="string"/>
<file name="mpu60x0.c" dir="peripherals"/>
<file name="mpu60x0_spi.c" dir="peripherals"/>
<file name="hmc58xx.c" dir="peripherals"/>
<file name="imu_mpu6000_hmc5883.c"/>
<test>
<define name="IMU_TYPE_H" value="modules/imu/imu_mpu6000_hmc5883.h" type="string"/>
<define name="SPI_MASTER"/>
<define name="IMU_MPU_SPI_DEV" value="spi1"/>
<define name="USE_SPI1"/>
conf/modules/imu_mpu60x0_i2c.xml
-2
View File
@@ -32,8 +32,6 @@
<define name="IMU_MPU60X0_I2C_DEV" value="$(IMU_MPU60X0_I2C_DEV_LOWER)"/>
<define name="USE_$(IMU_MPU60X0_I2C_DEV_UPPER)"/>
<define name="IMU_TYPE_H" value="modules/imu/imu_mpu60x0_i2c.h" type="string"/>
<file name="mpu60x0.c" dir="peripherals"/>
<file name="mpu60x0_i2c.c" dir="peripherals"/>
<file name="imu_mpu60x0_i2c.c"/>
conf/modules/imu_mpu9250_i2c.xml
-3
View File
@@ -37,14 +37,11 @@
<define name="IMU_MPU9250_I2C_DEV" value="$(IMU_MPU9250_I2C_DEV_LOWER)"/>
<define name="USE_$(IMU_MPU9250_I2C_DEV_UPPER)"/>
<define name="IMU_TYPE_H" value="modules/imu/imu_mpu9250_i2c.h" type="string"/>
<file name="mpu9250.c" dir="peripherals"/>
<file name="mpu9250_i2c.c" dir="peripherals"/>
<file name="ak8963.c" dir="peripherals"/>
<file name="imu_mpu9250_i2c.c"/>
<test>
<define name="IMU_TYPE_H" value="modules/imu/imu_mpu9250_i2c.h" type="string"/>
<define name="IMU_MPU9250_I2C_DEV" value="i2c1"/>
<define name="USE_I2C1"/>
</test>
conf/modules/imu_mpu9250_spi.xml
-3
View File
@@ -44,13 +44,10 @@
<define name="IMU_MPU9250_SPI_SLAVE_IDX" value="$(IMU_MPU9250_SPI_SLAVE_IDX)"/>
<define name="USE_$(IMU_MPU9250_SPI_SLAVE_IDX)"/>
<define name="IMU_TYPE_H" value="imu/imu_mpu9250_spi.h" type="string"/>
<file name="mpu9250.c" dir="peripherals"/>
<file name="mpu9250_spi.c" dir="peripherals"/>
<file name="imu_mpu9250_spi.c"/>
<test>
<define name="IMU_TYPE_H" value="imu/imu_mpu9250_spi.h" type="string"/>
<define name="SPI_MASTER"/>
<define name="IMU_MPU9250_SPI_DEV" value="spi1"/>
<define name="USE_SPI1"/>
conf/modules/imu_nps.xml
-1
View File
@@ -17,7 +17,6 @@
<event fun="imu_nps_event()"/>
<makefile target="nps|hitl">
<define name="IMU_TYPE_H" value="modules/imu/imu_nps.h" type="string"/>
<file name="imu_nps.c"/>
</makefile>
</module>
conf/modules/imu_openpilot_revo.xml
-2
View File
@@ -57,8 +57,6 @@
<define name="IMU_HMC_Y_SIGN" value="1"/>
<define name="IMU_HMC_Z_SIGN" value="-1"/>
<define name="IMU_TYPE_H" value="modules/imu/imu_mpu6000_hmc5883.h" type="string"/>
<file name="mpu60x0.c" dir="peripherals"/>
<file name="mpu60x0_spi.c" dir="peripherals"/>
<file name="hmc58xx.c" dir="peripherals"/>
conf/modules/imu_openpilot_revo_nano.xml
-2
View File
@@ -42,8 +42,6 @@
<define name="IMU_MPU9250_SPI_SLAVE_IDX" value="SPI_SLAVE0"/>
<define name="USE_SPI_SLAVE0"/>
<define name="IMU_TYPE_H" value="imu/imu_mpu9250_spi.h" type="string"/>
<file name="mpu9250.c" dir="peripherals"/>
<file name="mpu9250_spi.c" dir="peripherals"/>
<file name="imu_mpu9250_spi.c"/>
conf/modules/imu_px4fmu_v1.7.xml
-3
View File
@@ -29,14 +29,11 @@
<define name="USE_SPI_SLAVE2"/>
<define name="USE_I2C2"/>
<define name="IMU_TYPE_H" value="imu/imu_px4fmu.h" type="string"/>
<file name="mpu60x0.c" dir="peripherals"/>
<file name="mpu60x0_spi.c" dir="peripherals"/>
<file name="hmc58xx.c" dir="peripherals"/>
<file name="imu_px4fmu.c"/>
<test>
<define name="IMU_TYPE_H" value="imu/imu_px4fmu.h" type="string"/>
<define name="SPI_MASTER"/>
<define name="USE_SPI1"/>
<define name="USE_SPI_SLAVE0"/>
conf/modules/imu_px4fmu_v2.4.xml
-3
View File
@@ -37,14 +37,11 @@
<define name="USE_$(IMU_LSM_SPI_SLAVE_IDX)"/>
<define name="IMU_LSM_SPI_SLAVE_IDX" value="$(IMU_LSM_SPI_SLAVE_IDX)"/>
<define name="IMU_TYPE_H" value="imu/imu_px4fmu_v2.4.h" type="string"/>
<file name="l3gd20_spi.c" dir="peripherals"/>
<file name="lsm303d_spi.c" dir="peripherals"/>
<file name="hmc58xx.c" dir="peripherals"/>
<file name="imu_px4fmu_v2.4.c"/>
<test>
<define name="IMU_TYPE_H" value="imu/imu_px4fmu_v2.4.h" type="string"/>
<define name="IMU_PX4FMU_SPI_DEV" value="spi1"/>
<define name="IMU_L3G_SPI_SLAVE_IDX" value="SPI_SLAVE0"/>
<define name="IMU_LSM_SPI_SLAVE_IDX" value="SPI_SLAVE1"/>
conf/modules/imu_vectornav.xml
+1 -3
View File
@@ -30,8 +30,6 @@
<define name="$(VN_PORT_UPPER)_BAUD" value="$(VN_BAUD)"/>
<file name="vn200_serial.c" dir="peripherals"/>
<file name="imu_vectornav.c"/>
<define name="IMU_TYPE_H" value="modules/imu/imu_vectornav.h" type="string"/>
<file name="imu_vectornav.c"/
</makefile>
</module>
conf/modules/imu_xsens.xml
-2
View File
@@ -33,9 +33,7 @@
<define name="$(XSENS_PORT_UPPER)_BAUD" value="$(XSENS_BAUD)"/>
<!-- TODO: check output mode -->
<define name="XSENS_OUTPUT_MODE" value="0x1836"/>
<define name="IMU_TYPE_H" value="modules/ins/imu_xsens.h" type="string"/>
<test>
<define name="IMU_TYPE_H" value="modules/ins/imu_xsens.h" type="string"/>
<define name="XSENS_LINK" value="uart1"/>
<define name="USE_UART1"/>
<define name="DOWNLINK_TRANSPORT" value="pprz_tp"/>
conf/modules/ins_nps.xml
-1
View File
@@ -19,7 +19,6 @@
<file name="imu.c" dir="modules/imu"/>
<file name="imu_nps.c" dir="modules/imu"/>
<define name="USE_IMU"/>
<define name="IMU_TYPE_H" value="modules/imu/imu_nps.h" type="string"/>
<!-- use attitude from nps sim -->
<define name="NPS_BYPASS_AHRS" value="TRUE"/>
conf/simulator/nps/nps_sensors_params_default.h
+30 -21
View File
@@ -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
conf/simulator/nps/nps_sensors_params_noisy.h
+31 -22
View File
@@ -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
conf/userconf/tudelft/conf.xml
+4 -4
View File
@@ -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"
/>
<aircraft
@@ -282,7 +282,7 @@
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/nederdrone_valkenburg.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/nav_basic_rotorcraft.xml modules/nav_survey_rectangle_rotorcraft.xml modules/scheduling_indi_simple.xml modules/stabilization_indi_simple.xml"
settings_modules="modules/air_data.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_basic_rotorcraft.xml modules/nav_survey_rectangle_rotorcraft.xml modules/scheduling_indi_simple.xml modules/stabilization_indi_simple.xml"
gui_color="blue"
/>
<aircraft
@@ -497,12 +497,12 @@
<aircraft
name="bebop_opticflow_indoor"
ac_id="97"
airframe="airframes/tudelft/bebop_opticflow_indoor_2x_30hz.xml"
airframe="airframes/tudelft/bebop_opticflow_indoor.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/tudelft/rotorcraft_cyberzoo_no_gps.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/bebop_ae_awb.xml modules/bebop_cam.xml modules/cv_opticflow.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_hff_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi.xml modules/video_capture.xml modules/video_rtp_stream.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/bebop_cam.xml modules/cv_opticflow.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_hff_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi.xml modules/video_capture.xml modules/video_rtp_stream.xml"
gui_color="blue"
/>
<aircraft
sw/airborne/boards/apogee/imu_apogee.c
+8 -21
View File
@@ -105,6 +105,10 @@ void imu_apogee_init(void)
imu_apogee.mpu.config.nb_slaves = 1;
imu_apogee.mpu.config.slaves[0].configure = &configure_baro_slave;
imu_apogee.mpu.config.i2c_bypass = true;
// set the default values
imu_set_defaults_gyro(IMU_BOARD_ID, NULL, NULL, MPU60X0_GYRO_SENS_FRAC[APOGEE_GYRO_RANGE]);
imu_set_defaults_accel(IMU_BOARD_ID, NULL, NULL, MPU60X0_ACCEL_SENS_FRAC[APOGEE_ACCEL_RANGE]);
#if APOGEE_USE_MPU9150
// if using MPU9150, internal mag needs to be configured
ak8975_init(&imu_apogee.ak, &(IMU_APOGEE_I2C_DEV), AK8975_I2C_SLV_ADDR);
@@ -122,19 +126,8 @@ void imu_apogee_periodic(void)
// Start reading internal mag if available
RunOnceEvery(MAG_PRESCALER, ak8975_periodic(&imu_apogee.ak));
#endif
//RunOnceEvery(10,imu_apogee_downlink_raw());
}
void imu_apogee_downlink_raw(void)
{
DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, &imu.gyro_unscaled.p, &imu.gyro_unscaled.q,
&imu.gyro_unscaled.r);
DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, &imu.accel_unscaled.x, &imu.accel_unscaled.y,
&imu.accel_unscaled.z);
}
void imu_apogee_event(void)
{
uint32_t now_ts = get_sys_time_usec();
@@ -147,18 +140,14 @@ void imu_apogee_event(void)
(int32_t)(-imu_apogee.mpu.data_rates.value[IMU_APOGEE_CHAN_Y]),
(int32_t)(-imu_apogee.mpu.data_rates.value[IMU_APOGEE_CHAN_Z])
};
RATES_COPY(imu.gyro_unscaled, rates);
AbiSendMsgIMU_GYRO_RAW(IMU_BOARD_ID, now_ts, &rates, 1);
struct Int32Vect3 accel = {
(int32_t)( imu_apogee.mpu.data_accel.value[IMU_APOGEE_CHAN_X]),
(int32_t)(-imu_apogee.mpu.data_accel.value[IMU_APOGEE_CHAN_Y]),
(int32_t)(-imu_apogee.mpu.data_accel.value[IMU_APOGEE_CHAN_Z])
};
VECT3_COPY(imu.accel_unscaled, accel);
AbiSendMsgIMU_ACCEL_RAW(IMU_BOARD_ID, now_ts, &accel, 1);
imu_apogee.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);
}
#if APOGEE_USE_MPU9150
@@ -169,10 +158,8 @@ void imu_apogee_event(void)
(int32_t)(-imu_apogee.ak.data.value[IMU_APOGEE_CHAN_X]),
(int32_t)( imu_apogee.ak.data.value[IMU_APOGEE_CHAN_Z])
};
VECT3_COPY(imu.mag_unscaled, mag);
imu_apogee.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);
imu_apogee.ak.data_available = false;
}
#endif
}
sw/airborne/boards/apogee/imu_apogee.h
+1 -50
View File
@@ -39,18 +39,7 @@
#include "peripherals/mpu60x0_i2c.h"
// Default configuration
#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
#define IMU_GYRO_P_SIGN 1
#define IMU_GYRO_Q_SIGN 1
#define IMU_GYRO_R_SIGN 1
#endif
#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
#define IMU_ACCEL_X_SIGN 1
#define IMU_ACCEL_Y_SIGN 1
#define IMU_ACCEL_Z_SIGN 1
#endif
/* Default range configurations */
#ifndef APOGEE_GYRO_RANGE
#define APOGEE_GYRO_RANGE MPU60X0_GYRO_RANGE_1000
#endif
@@ -58,43 +47,6 @@
#define APOGEE_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[APOGEE_GYRO_RANGE]
#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[APOGEE_GYRO_RANGE][0]
#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[APOGEE_GYRO_RANGE][1]
#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[APOGEE_GYRO_RANGE]
#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[APOGEE_GYRO_RANGE][0]
#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[APOGEE_GYRO_RANGE][1]
#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[APOGEE_GYRO_RANGE]
#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[APOGEE_GYRO_RANGE][0]
#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[APOGEE_GYRO_RANGE][1]
#endif
#if !defined IMU_GYRO_P_NEUTRAL & !defined IMU_GYRO_Q_NEUTRAL & !defined IMU_GYRO_R_NEUTRAL
#define IMU_GYRO_P_NEUTRAL 0
#define IMU_GYRO_Q_NEUTRAL 0
#define IMU_GYRO_R_NEUTRAL 0
#endif
// 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[APOGEE_ACCEL_RANGE]
#define IMU_ACCEL_X_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[APOGEE_ACCEL_RANGE][0]
#define IMU_ACCEL_X_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[APOGEE_ACCEL_RANGE][1]
#define IMU_ACCEL_Y_SENS MPU60X0_ACCEL_SENS[APOGEE_ACCEL_RANGE]
#define IMU_ACCEL_Y_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[APOGEE_ACCEL_RANGE][0]
#define IMU_ACCEL_Y_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[APOGEE_ACCEL_RANGE][1]
#define IMU_ACCEL_Z_SENS MPU60X0_ACCEL_SENS[APOGEE_ACCEL_RANGE]
#define IMU_ACCEL_Z_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[APOGEE_ACCEL_RANGE][0]
#define IMU_ACCEL_Z_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[APOGEE_ACCEL_RANGE][1]
#endif
#if !defined IMU_ACCEL_X_NEUTRAL & !defined IMU_ACCEL_Y_NEUTRAL & !defined IMU_ACCEL_Z_NEUTRAL
#define IMU_ACCEL_X_NEUTRAL 0
#define IMU_ACCEL_Y_NEUTRAL 0
#define IMU_ACCEL_Z_NEUTRAL 0
#endif
struct ImuApogee {
struct Mpu60x0_I2c mpu;
#if APOGEE_USE_MPU9150
@@ -107,6 +59,5 @@ extern struct ImuApogee imu_apogee;
extern void imu_apogee_init(void);
extern void imu_apogee_periodic(void);
extern void imu_apogee_event(void);
extern void imu_apogee_downlink_raw(void);
#endif // IMU_APOGEE_H
sw/airborne/boards/ardrone/navdata.c
+52 -9
View File
@@ -86,6 +86,33 @@ static pthread_cond_t navdata_cond = PTHREAD_COND_INITIALIZER;
#define SONAR_SCALE 0.00047
#endif
/**
* Default gyro scale
*/
static const struct Int32Rates gyro_scale[2] = {
{4359, 4359, 4359},
{1000, 1000, 1000}
};
/**
* Default accel scale/neutral
*/
static const struct Int32Vect3 accel_scale[2] = {
{195, 195, 195},
{10, 10, 10}
};
static const struct Int32Vect3 accel_neutral = {
2048, 2048, 2048
};
/**
* Default mag scale
*/
static const struct Int32Vect3 mag_scale[2] = {
{16, 16, 16},
{1, 1, 1}
};
/**
* Write to fd even while being interrupted
*/
@@ -227,6 +254,11 @@ bool navdata_init()
}
navdata.baro_calibrated = true;
/* Set the default scalings/neutrals */
imu_set_defaults_gyro(IMU_BOARD_ID, NULL, NULL, gyro_scale);
imu_set_defaults_accel(IMU_BOARD_ID, NULL, &accel_neutral, accel_scale);
imu_set_defaults_mag(IMU_BOARD_ID, NULL, NULL, mag_scale);
/* Start acquisition */
navdata_cmd_send(NAVDATA_CMD_START);
@@ -329,16 +361,27 @@ static void *navdata_read(void *data __attribute__((unused)))
#include "modules/imu/imu.h"
static void navdata_publish_imu(void)
{
RATES_ASSIGN(imu.gyro_unscaled, navdata.measure.vx, -navdata.measure.vy, -navdata.measure.vz);
VECT3_ASSIGN(imu.accel_unscaled, navdata.measure.ax, 4096 - navdata.measure.ay, 4096 - navdata.measure.az);
VECT3_ASSIGN(imu.mag_unscaled, -navdata.measure.mx, -navdata.measure.my, -navdata.measure.mz);
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
imu_scale_mag(&imu);
uint32_t now_ts = get_sys_time_usec();
AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag);
struct Int32Rates gyro = {
navdata.measure.vx,
-navdata.measure.vy,
-navdata.measure.vz
};
AbiSendMsgIMU_GYRO_RAW(IMU_BOARD_ID, now_ts, &gyro, 1);
struct Int32Vect3 accel = {
navdata.measure.ax,
4096 - navdata.measure.ay,
4096 - navdata.measure.az
};
AbiSendMsgIMU_ACCEL_RAW(IMU_BOARD_ID, now_ts, &accel, 1);
struct Int32Vect3 mag = {
-navdata.measure.mx,
-navdata.measure.my,
-navdata.measure.mz
};
AbiSendMsgIMU_MAG_RAW(IMU_BOARD_ID, now_ts, &mag);
}
/**
sw/airborne/firmwares/fixedwing/autopilot_static.c
+1 -1
View File
@@ -85,7 +85,7 @@ void autopilot_static_init(void)
///@todo: properly implement/fix a triggered attitude loop
#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
AbiBindMsgIMU_GYRO_INT32(ABI_BROADCAST, &new_att_ev, &new_att_cb);
AbiBindMsgIMU_GYRO(ABI_BROADCAST, &new_att_ev, &new_att_cb);
#endif
}
sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c
+1 -1
View File
@@ -283,7 +283,7 @@ void v_ctl_init(void)
float_quat_identity(&imu_to_body_quat);
AbiBindMsgIMU_ACCEL_INT32(V_CTL_ENERGY_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgIMU_ACCEL(V_CTL_ENERGY_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgBODY_TO_IMU_QUAT(V_CTL_ENERGY_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
}
sw/airborne/modules/ahrs/ahrs_aligner.c
+25 -15
View File
@@ -65,16 +65,19 @@ static void gyro_cb(uint8_t sender_id __attribute__((unused)),
static void send_aligner(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_FILTER_ALIGNER(trans, dev, AC_ID,
&ahrs_aligner.lp_gyro.p,
&ahrs_aligner.lp_gyro.q,
&ahrs_aligner.lp_gyro.r,
&imu.gyro.p,
&imu.gyro.q,
&imu.gyro.r,
&ahrs_aligner.noise,
&ahrs_aligner.low_noise_cnt,
&ahrs_aligner.status);
struct imu_gyro_t *gyro = imu_get_gyro(AHRS_ALIGNER_IMU_ID, false);
if(gyro != NULL) {
pprz_msg_send_FILTER_ALIGNER(trans, dev, AC_ID,
&ahrs_aligner.lp_gyro.p,
&ahrs_aligner.lp_gyro.q,
&ahrs_aligner.lp_gyro.r,
&gyro->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
sw/airborne/modules/ahrs/ahrs_float_cmpl_wrapper.c
+3 -3
View File
@@ -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);
sw/airborne/modules/ahrs/ahrs_float_dcm_wrapper.c
+3 -3
View File
@@ -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);
sw/airborne/modules/ahrs/ahrs_float_invariant_wrapper.c
+3 -3
View File
@@ -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);
sw/airborne/modules/ahrs/ahrs_float_mlkf_wrapper.c
+3 -3
View File
@@ -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);
sw/airborne/modules/ahrs/ahrs_int_cmpl_quat_wrapper.c
+3 -3
View File
@@ -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);
sw/airborne/modules/ahrs/ahrs_madgwick_wrapper.c
+2 -2
View File
@@ -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);
sw/airborne/modules/ahrs/ahrs_vectornav.c
+2 -2
View File
@@ -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);
}
}
sw/airborne/modules/calibration/mag_calib_ukf.c
+2 -2
View File
@@ -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);
}
}
}
sw/airborne/modules/calibration/send_imu_mag_current.c
+8 -3
View File
@@ -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;
}
sw/airborne/modules/core/abi_sender_ids.h
+4
View File
@@ -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
sw/airborne/modules/dragspeed/dragspeed.c
+1 -1
View File
@@ -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)
sw/airborne/modules/imu/filter_1euro_imu.c
+13 -14
View File
@@ -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;
}
}
sw/airborne/modules/imu/imu.c
+132 -36
View File
@@ -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];
}
}
sw/airborne/modules/imu/imu.h
+10 -2
View File
@@ -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);
sw/airborne/modules/imu/imu_ardrone2.c
-2
View File
@@ -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)
{
sw/airborne/modules/imu/imu_ardrone2.h
+1 -55
View File
@@ -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_ */
sw/airborne/modules/imu/imu_aspirin.c
+30 -29
View File
@@ -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);
}
}

Some files were not shown because too many files have changed in this diff Show More