Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-31 20:38:27 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

[imu] Enable IMU integration and fix small mistakes

Browse Source
This commit is contained in:
Freek van Tienen
2022-09-20 13:51:28 +02:00
parent affcc2c2d9
commit 3e473e72a6
11 changed files with 226 additions and 180 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/abi.xml
+2 -2
View File
@@ -231,13 +231,13 @@
<message name="IMU_GYRO_INT" id="34"> <message name="IMU_GYRO_INT" id="34">
<field name="stamp" type="uint32_t" unit="us"/> <field name="stamp" type="uint32_t" unit="us"/>
<field name="delta_gyro" type="struct FloatRates *"/> <field name="delta_gyro" type="struct FloatRates *"/>
<field name="delta_dt" type="uint16_t" unit="us"/> <field name="dt" type="uint16_t" unit="us"/>
</message> </message>
<message name="IMU_ACCEL_INT" id="35"> <message name="IMU_ACCEL_INT" id="35">
<field name="stamp" type="uint32_t" unit="us"/> <field name="stamp" type="uint32_t" unit="us"/>
<field name="delta_accel" type="struct FloatVect3 *"/> <field name="delta_accel" type="struct FloatVect3 *"/>
<field name="delta_dt" type="uint16_t" unit="us"/> <field name="dt" type="uint16_t" unit="us"/>
</message> </message>
</msg_class> </msg_class>
conf/airframes/examples/cube_orange.xml
+24 -45
View File
@@ -49,7 +49,9 @@
<define name="RADIO_FBW_MODE" value="RADIO_AUX3"/> <!-- Switch between AP and FBW control --> <define name="RADIO_FBW_MODE" value="RADIO_AUX3"/> <!-- Switch between AP and FBW control -->
<define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/> <define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/>
<!-- Use the external mag (not in NPS target as then it needs to listen to all) --> <!-- Choose which sensors to use in the EKF2 (not in NPS target as then it needs to listen to all) -->
<define name="INS_EKF2_ACCEL_ID" value="IMU_CUBE2_ID"/>
<define name="INS_EKF2_GYRO_ID" value="IMU_CUBE2_ID"/>
<define name="INS_EKF2_MAG_ID" value="MAG_LIS3MDL_SENDER_ID"/> <define name="INS_EKF2_MAG_ID" value="MAG_LIS3MDL_SENDER_ID"/>
</target> </target>
@@ -84,7 +86,7 @@
<module name="gps" type="ubx_ucenter"/> <module name="gps" type="ubx_ucenter"/>
<module name="stabilization" type="indi_simple"/> <module name="stabilization" type="indi_simple"/>
<module name="stabilization" type="rate_indi"/> <module name="stabilization" type="rate_indi"/>
<module name="ins" type="ekf2" /> <module name="ins" type="ekf2"/>
<module name="air_data"/> <module name="air_data"/>
@@ -128,7 +130,7 @@
<module name="motor_mixing"/> <module name="motor_mixing"/>
</firmware> </firmware>
<!-- CAN BUS 1 (Front Wing) --> <!-- CPWM outputs -->
<servos driver="Pwm"> <servos driver="Pwm">
<servo name="MOTOR_1" no="0" min="1000" neutral="1100" max="2000"/> <servo name="MOTOR_1" no="0" min="1000" neutral="1100" max="2000"/>
<servo name="MOTOR_2" no="1" min="1000" neutral="1100" max="2000"/> <servo name="MOTOR_2" no="1" min="1000" neutral="1100" max="2000"/>
@@ -139,7 +141,7 @@
<servo name="MOTOR_7" no="6" min="1000" neutral="1100" max="2000"/> <servo name="MOTOR_7" no="6" min="1000" neutral="1100" max="2000"/>
</servos> </servos>
<!-- CAN BUS 1 (Front Wing) --> <!-- CAN BUS 1 outputs -->
<servos driver="Uavcan1"> <servos driver="Uavcan1">
<!--servo name="MOTOR_1" no="0" min="-8191" neutral="1500" max="8191"/> <!--servo name="MOTOR_1" no="0" min="-8191" neutral="1500" max="8191"/>
<servo name="MOTOR_2" no="1" min="-8191" neutral="1500" max="8191"/> <servo name="MOTOR_2" no="1" min="-8191" neutral="1500" max="8191"/>
@@ -153,17 +155,17 @@
<servo name="AIL_2" no="9" min="-6000" neutral="0" max="6000"/> <servo name="AIL_2" no="9" min="-6000" neutral="0" max="6000"/>
</servos> </servos>
<!-- CAN BUS 2 outputs -->
<servos driver="Uavcan2"> <servos driver="Uavcan2">
<!--servo name="MOTOR_1" no="0" min="-8191" neutral="1500" max="8191"/> <servo name="MOTOR_8" no="0" min="-8191" neutral="1500" max="8191"/>
<servo name="MOTOR_2" no="1" min="-8191" neutral="1500" max="8191"/> <servo name="MOTOR_9" no="1" min="-8191" neutral="1500" max="8191"/>
<servo name="MOTOR_3" no="2" min="-8191" neutral="1500" max="8191"/> <servo name="MOTOR_10" no="2" min="-8191" neutral="1500" max="8191"/>
<servo name="MOTOR_4" no="3" min="-8191" neutral="1500" max="8191"/> <servo name="MOTOR_11" no="3" min="-8191" neutral="1500" max="8191"/>
<servo name="MOTOR_5" no="4" min="-8191" neutral="1500" max="8191"/> <servo name="MOTOR_12" no="4" min="-8191" neutral="1500" max="8191"/>
<servo name="MOTOR_6" no="5" min="-8191" neutral="1500" max="8191"/--> <servo name="AIL_3" no="6" min="6000" neutral="0" max="-6000"/>
<servo name="AIL1_1" no="6" min="6000" neutral="0" max="-6000"/> <servo name="FLAP_3" no="7" min="6000" neutral="0" max="-6000"/>
<servo name="FLAP1_1" no="7" min="6000" neutral="0" max="-6000"/> <servo name="FLAP_4" no="8" min="-6000" neutral="0" max="6000"/>
<servo name="FLAP1_2" no="8" min="-6000" neutral="0" max="6000"/> <servo name="AIL_4" no="9" min="-6000" neutral="0" max="6000"/>
<servo name="AIL1_2" no="9" min="-6000" neutral="0" max="6000"/>
</servos> </servos>
<commands> <commands>
@@ -202,6 +204,12 @@
<set servo="MOTOR_4" value="($th_hold? -9600 : motor_mixing.commands[3])"/> <set servo="MOTOR_4" value="($th_hold? -9600 : motor_mixing.commands[3])"/>
<set servo="MOTOR_5" value="($th_hold? -9600 : motor_mixing.commands[4])"/> <set servo="MOTOR_5" value="($th_hold? -9600 : motor_mixing.commands[4])"/>
<set servo="MOTOR_6" value="($th_hold? -9600 : motor_mixing.commands[5])"/> <set servo="MOTOR_6" value="($th_hold? -9600 : motor_mixing.commands[5])"/>
<set servo="MOTOR_7" value="($th_hold? -9600 : motor_mixing.commands[6])"/>
<set servo="MOTOR_8" value="($th_hold? -9600 : motor_mixing.commands[7])"/>
<set servo="MOTOR_9" value="($th_hold? -9600 : motor_mixing.commands[8])"/>
<set servo="MOTOR_10" value="($th_hold? -9600 : motor_mixing.commands[9])"/>
<set servo="MOTOR_11" value="($th_hold? -9600 : motor_mixing.commands[10])"/>
<set servo="MOTOR_12" value="($th_hold? -9600 : motor_mixing.commands[11])"/>
</command_laws> </command_laws>
<section name="MISC"> <section name="MISC">
@@ -232,37 +240,8 @@
</section> </section>
<section name="IMU" prefix="IMU_"> <section name="IMU" prefix="IMU_">
<!-- Rotate the IMU (for Pixhawk 4) --> <!-- Calibrations -->
<define name="MPU_CHAN_X" value="1"/> <define name="MAG_CALIB" value="{{.abi_id=3, .calibrated={.neutral=true, .scale=true}, .neutral={1696,-4095,3641}, .scale={{15263,26021,26309},{25440,40858,41277}}}}"/>
<define name="MPU_CHAN_Y" value="0"/>
<define name="MPU_CHAN_Z" value="2"/>
<define name="MPU_X_SIGN" value="1"/>
<define name="MPU_Y_SIGN" value="1"/>
<define name="MPU_Z_SIGN" value="-1"/>
<!-- Calibrated 09-03-2021 in MAVLab outside body -->
<define name="ACCEL_X_NEUTRAL" value="16"/>
<define name="ACCEL_Y_NEUTRAL" value="31"/>
<define name="ACCEL_Z_NEUTRAL" value="10"/>
<define name="ACCEL_X_SENS" value="4.793311542012962" integer="16"/>
<define name="ACCEL_Y_SENS" value="4.893892083504835" integer="16"/>
<define name="ACCEL_Z_SENS" value="4.791159603565934" integer="16"/>
<!-- NOT CALIBRATED (internal magnetometer) -->
<!--define name="MAG_X_NEUTRAL" value="-13"/>
<define name="MAG_Y_NEUTRAL" value="-15"/>
<define name="MAG_Z_NEUTRAL" value="12"/>
<define name="MAG_X_SENS" value="12.823826623678" integer="16"/>
<define name="MAG_Y_SENS" value="12.86305350064853" integer="16"/>
<define name="MAG_Z_SENS" value="12.512614596093622" integer="16"/-->
<!-- NOT CALIBRATED (external magnetometer) -->
<define name="MAG_X_NEUTRAL" value="2457"/>
<define name="MAG_Y_NEUTRAL" value="-1123"/>
<define name="MAG_Z_NEUTRAL" value="-1843"/>
<define name="MAG_X_SENS" value="0.590610116730755" integer="16"/>
<define name="MAG_Y_SENS" value="0.6411943049128617" integer="16"/>
<define name="MAG_Z_SENS" value="0.6279536066632421" integer="16"/>
<!-- Define axis in hover frame --> <!-- Define axis in hover frame -->
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/> <define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
conf/conf_tests.xml
+1 -1
View File
@@ -7,7 +7,7 @@
telemetry="telemetry/highspeed_rotorcraft.xml" telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic_geofence.xml" flight_plan="flight_plans/rotorcraft_basic_geofence.xml"
settings="settings/rotorcraft_basic.xml" settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.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/scheduling_indi_simple.xml modules/stabilization_indi_simple.xml" settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.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/scheduling_indi_simple.xml modules/stabilization_indi_simple.xml"
gui_color="blue" gui_color="blue"
/> />
<aircraft <aircraft
conf/modules/imu_cube.xml
+13 -8
View File
@@ -4,6 +4,9 @@
<doc> <doc>
<description> <description>
IMU driver for the sensors inside the cube autopilots IMU driver for the sensors inside the cube autopilots
- IMU1: ICM20948 (non-isolated)
- IMU2: ICM20602 (isolated)
- IMU3: ICM20649 (isolated)
</description> </description>
</doc> </doc>
<dep> <dep>
@@ -19,31 +22,33 @@
<periodic fun="imu_cube_periodic()"/> <periodic fun="imu_cube_periodic()"/>
<event fun="imu_cube_event()"/> <event fun="imu_cube_event()"/>
<makefile target="!sim|nps|fbw"> <makefile target="!sim|nps|fbw">
<!-- ICM20602 (isolated) --> <!-- ICM20948 (non-isolated) -->
<configure name="CUBE_IMU1_SPI_DEV" default="spi4" case="lower|upper"/> <configure name="CUBE_IMU1_SPI_DEV" default="spi1" case="lower|upper"/>
<configure name="CUBE_IMU1_SPI_SLAVE_IDX" default="SPI_SLAVE3"/> <configure name="CUBE_IMU1_SPI_SLAVE_IDX" default="SPI_SLAVE2"/>
<define name="CUBE_IMU1_SPI_DEV" value="$(CUBE_IMU1_SPI_DEV_LOWER)"/> <define name="CUBE_IMU1_SPI_DEV" value="$(CUBE_IMU1_SPI_DEV_LOWER)"/>
<define name="USE_$(CUBE_IMU1_SPI_DEV_UPPER)"/> <define name="USE_$(CUBE_IMU1_SPI_DEV_UPPER)"/>
<define name="CUBE_IMU1_SPI_SLAVE_IDX" value="$(CUBE_IMU1_SPI_SLAVE_IDX)"/> <define name="CUBE_IMU1_SPI_SLAVE_IDX" value="$(CUBE_IMU1_SPI_SLAVE_IDX)"/>
<define name="USE_$(CUBE_IMU1_SPI_SLAVE_IDX)"/> <define name="USE_$(CUBE_IMU1_SPI_SLAVE_IDX)"/>
<!-- ICM20649 (isolated) --> <!-- ICM20602 (isolated) -->
<configure name="CUBE_IMU2_SPI_DEV" default="spi4" case="lower|upper"/> <configure name="CUBE_IMU2_SPI_DEV" default="spi4" case="lower|upper"/>
<configure name="CUBE_IMU2_SPI_SLAVE_IDX" default="SPI_SLAVE8"/> <configure name="CUBE_IMU2_SPI_SLAVE_IDX" default="SPI_SLAVE3"/>
<define name="CUBE_IMU2_SPI_DEV" value="$(CUBE_IMU2_SPI_DEV_LOWER)"/> <define name="CUBE_IMU2_SPI_DEV" value="$(CUBE_IMU2_SPI_DEV_LOWER)"/>
<define name="USE_$(CUBE_IMU2_SPI_DEV_UPPER)"/> <define name="USE_$(CUBE_IMU2_SPI_DEV_UPPER)"/>
<define name="CUBE_IMU2_SPI_SLAVE_IDX" value="$(CUBE_IMU2_SPI_SLAVE_IDX)"/> <define name="CUBE_IMU2_SPI_SLAVE_IDX" value="$(CUBE_IMU2_SPI_SLAVE_IDX)"/>
<define name="USE_$(CUBE_IMU2_SPI_SLAVE_IDX)"/> <define name="USE_$(CUBE_IMU2_SPI_SLAVE_IDX)"/>
<!-- ICM20948 (non-isolated) --> <!-- ICM20649 (isolated) -->
<configure name="CUBE_IMU3_SPI_DEV" default="spi1" case="lower|upper"/> <configure name="CUBE_IMU3_SPI_DEV" default="spi4" case="lower|upper"/>
<configure name="CUBE_IMU3_SPI_SLAVE_IDX" default="SPI_SLAVE2"/> <configure name="CUBE_IMU3_SPI_SLAVE_IDX" default="SPI_SLAVE8"/>
<define name="CUBE_IMU3_SPI_DEV" value="$(CUBE_IMU3_SPI_DEV_LOWER)"/> <define name="CUBE_IMU3_SPI_DEV" value="$(CUBE_IMU3_SPI_DEV_LOWER)"/>
<define name="USE_$(CUBE_IMU3_SPI_DEV_UPPER)"/> <define name="USE_$(CUBE_IMU3_SPI_DEV_UPPER)"/>
<define name="CUBE_IMU3_SPI_SLAVE_IDX" value="$(CUBE_IMU3_SPI_SLAVE_IDX)"/> <define name="CUBE_IMU3_SPI_SLAVE_IDX" value="$(CUBE_IMU3_SPI_SLAVE_IDX)"/>
<define name="USE_$(CUBE_IMU3_SPI_SLAVE_IDX)"/> <define name="USE_$(CUBE_IMU3_SPI_SLAVE_IDX)"/>
<file name="invensense2.c" dir="peripherals"/> <file name="invensense2.c" dir="peripherals"/>
<file name="mpu60x0.c" dir="peripherals"/>
<file name="mpu60x0_spi.c" dir="peripherals"/>
<file name="imu_cube.c"/> <file name="imu_cube.c"/>
</makefile> </makefile>
</module> </module>
conf/modules/ins_ekf2.xml
+1 -1
View File
@@ -79,7 +79,7 @@
<include name="$(PAPARAZZI_SRC)/sw/ext/matrix/"/> <include name="$(PAPARAZZI_SRC)/sw/ext/matrix/"/>
<define name="__PAPARAZZI" value="TRUE"/> <define name="__PAPARAZZI" value="TRUE"/>
<define name="ECL_STANDALONE" value="TRUE"/> <define name="ECL_STANDALONE" value="TRUE"/>
<define name="USE_MAGNETOMETER" value="TRUE"/> <!-- Needed for IMU to get scaled version --> <define name="IMU_INTEGRATION" value="TRUE"/> <!-- Needed to enable IMU integration -->
<!-- Compile needed ecl files --> <!-- Compile needed ecl files -->
<file name="geo.cpp" dir="ecl/geo"/> <file name="geo.cpp" dir="ecl/geo"/>
conf/simulator/nps/nps_sensors_params_default.h
+6 -1
View File
@@ -25,10 +25,15 @@
#include "generated/airframe.h" #include "generated/airframe.h"
#include "modules/imu/imu.h" #include "modules/imu/imu.h"
#if !defined(IMU_BODY_TO_IMU_PHI) && !defined(IMU_BODY_TO_IMU_THETA) && !defined(IMU_BODY_TO_IMU_PSI)
#define NPS_BODY_TO_IMU_PHI 0
#define NPS_BODY_TO_IMU_THETA 0
#define NPS_BODY_TO_IMU_PSI 0
#else
#define NPS_BODY_TO_IMU_PHI IMU_BODY_TO_IMU_PHI #define NPS_BODY_TO_IMU_PHI IMU_BODY_TO_IMU_PHI
#define NPS_BODY_TO_IMU_THETA IMU_BODY_TO_IMU_THETA #define NPS_BODY_TO_IMU_THETA IMU_BODY_TO_IMU_THETA
#define NPS_BODY_TO_IMU_PSI IMU_BODY_TO_IMU_PSI #define NPS_BODY_TO_IMU_PSI IMU_BODY_TO_IMU_PSI
#endif
// try to determine propagate frequency // try to determine propagate frequency
#if defined AHRS_PROPAGATE_FREQUENCY #if defined AHRS_PROPAGATE_FREQUENCY
sw/airborne/modules/imu/imu.c
+49 -21
View File
@@ -470,25 +470,39 @@ static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates
// Only integrate if we have gotten a previous measurement and didn't overflow the timer // Only integrate if we have gotten a previous measurement and didn't overflow the timer
if(gyro->last_stamp > 0 && stamp > gyro->last_stamp) { if(gyro->last_stamp > 0 && stamp > gyro->last_stamp) {
struct FloatRates integrated; struct FloatRates integrated;
uint16_t delta_dt = stamp - gyro->last_stamp; uint16_t dt = stamp - gyro->last_stamp;
// Trapezoidal integration (TODO: coning correction) // Trapezoidal integration (TODO: coning correction)
integrated.p = RATE_FLOAT_OF_BFP(gyro->scaled.p + scaled_rot.p) * 0.5f; integrated.p = RATE_FLOAT_OF_BFP(gyro->scaled.p + scaled_rot.p) * 0.5f;
integrated.q = RATE_FLOAT_OF_BFP(gyro->scaled.q + scaled_rot.q) * 0.5f; integrated.q = RATE_FLOAT_OF_BFP(gyro->scaled.q + scaled_rot.q) * 0.5f;
integrated.r = RATE_FLOAT_OF_BFP(gyro->scaled.r + scaled_rot.r) * 0.5f; integrated.r = RATE_FLOAT_OF_BFP(gyro->scaled.r + scaled_rot.r) * 0.5f;
for(uint8_t i = 0; i < samples-1; i++) { // Only perform multiple integrations in sensor frame if needed
integrated.p += data[i].p; if(samples > 1) {
integrated.q += data[i].q; struct FloatRates integrated_sensor;
integrated.r += data[i].r; struct FloatRMat body_to_sensor;
// Rotate back to sensor frame
RMAT_FLOAT_OF_BFP(body_to_sensor, gyro->body_to_sensor);
float_rmat_ratemult(&integrated_sensor, &body_to_sensor, &integrated);
// Add all the other samples
for(uint8_t i = 0; i < samples-1; i++) {
integrated_sensor.p += RATE_FLOAT_OF_BFP((data[i].p - gyro->neutral.p) * gyro->scale[0].p / gyro->scale[1].p);
integrated_sensor.q += RATE_FLOAT_OF_BFP((data[i].q - gyro->neutral.q) * gyro->scale[0].q / gyro->scale[1].q);
integrated_sensor.r += RATE_FLOAT_OF_BFP((data[i].r - gyro->neutral.r) * gyro->scale[0].r / gyro->scale[1].r);
}
// Rotate to body frame
float_rmat_transp_ratemult(&integrated, &body_to_sensor, &integrated_sensor);
} }
integrated.p = integrated.p / samples * ((float)delta_dt * 1e-6f); // Divide by the amount of samples and multiply by the delta time
integrated.q = integrated.q / samples * ((float)delta_dt * 1e-6f); integrated.p = integrated.p / samples * ((float)dt * 1e-6f);
integrated.r = integrated.r / samples * ((float)delta_dt * 1e-6f); integrated.q = integrated.q / samples * ((float)dt * 1e-6f);
integrated.r = integrated.r / samples * ((float)dt * 1e-6f);
// Send the integrated values // Send the integrated values
AbiSendMsgIMU_GYRO_INT(sender_id, stamp, &integrated, delta_dt); AbiSendMsgIMU_GYRO_INT(sender_id, stamp, &integrated, dt);
} }
#endif #endif
@@ -521,25 +535,39 @@ static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect
// Only integrate if we have gotten a previous measurement and didn't overflow the timer // Only integrate if we have gotten a previous measurement and didn't overflow the timer
if(accel->last_stamp > 0 && stamp > accel->last_stamp) { if(accel->last_stamp > 0 && stamp > accel->last_stamp) {
struct FloatVect3 integrated; struct FloatVect3 integrated;
uint16_t delta_dt = stamp - accel->last_stamp; uint16_t dt = stamp - accel->last_stamp;
// Trapezoidal integration // Trapezoidal integration
integrated.x = RATE_FLOAT_OF_BFP(accel->scaled.x + scaled_rot.x) * 0.5f; integrated.x = ACCEL_FLOAT_OF_BFP(accel->scaled.x + scaled_rot.x) * 0.5f;
integrated.y = RATE_FLOAT_OF_BFP(accel->scaled.y + scaled_rot.y) * 0.5f; integrated.y = ACCEL_FLOAT_OF_BFP(accel->scaled.y + scaled_rot.y) * 0.5f;
integrated.z = RATE_FLOAT_OF_BFP(accel->scaled.z + scaled_rot.z) * 0.5f; integrated.z = ACCEL_FLOAT_OF_BFP(accel->scaled.z + scaled_rot.z) * 0.5f;
for(uint8_t i = 0; i < samples-1; i++) { // Only perform multiple integrations in sensor frame if needed
integrated.x += data[i].x; if(samples > 1) {
integrated.y += data[i].y; struct FloatVect3 integrated_sensor;
integrated.z += data[i].z; struct FloatRMat body_to_sensor;
// Rotate back to sensor frame
RMAT_FLOAT_OF_BFP(body_to_sensor, accel->body_to_sensor);
float_rmat_vmult(&integrated_sensor, &body_to_sensor, &integrated);
// Add all the other samples
for(uint8_t i = 0; i < samples-1; i++) {
integrated_sensor.x += ACCEL_FLOAT_OF_BFP((data[i].x - accel->neutral.x) * accel->scale[0].x / accel->scale[1].x);
integrated_sensor.y += ACCEL_FLOAT_OF_BFP((data[i].y - accel->neutral.y) * accel->scale[0].y / accel->scale[1].y);
integrated_sensor.z += ACCEL_FLOAT_OF_BFP((data[i].z - accel->neutral.z) * accel->scale[0].z / accel->scale[1].z);
}
// Rotate to body frame
float_rmat_transp_vmult(&integrated, &body_to_sensor, &integrated_sensor);
} }
integrated.x = integrated.x / samples * ((float)delta_dt * 1e-6f); // Divide by the amount of samples and multiply by the delta time
integrated.y = integrated.y / samples * ((float)delta_dt * 1e-6f); integrated.x = integrated.x / samples * ((float)dt * 1e-6f);
integrated.z = integrated.z / samples * ((float)delta_dt * 1e-6f); integrated.y = integrated.y / samples * ((float)dt * 1e-6f);
integrated.z = integrated.z / samples * ((float)dt * 1e-6f);
// Send the integrated values // Send the integrated values
AbiSendMsgIMU_ACCEL_INT(sender_id, stamp, &integrated, delta_dt); AbiSendMsgIMU_ACCEL_INT(sender_id, stamp, &integrated, dt);
} }
#endif #endif
sw/airborne/modules/imu/imu.h
+21 -21
View File
@@ -46,45 +46,45 @@ struct imu_calib_t {
struct imu_gyro_t { struct imu_gyro_t {
uint8_t abi_id; ///< ABI sensor ID uint8_t abi_id; ///< ABI sensor ID
uint32_t last_stamp; ///< Last measurement timestamp uint32_t last_stamp; ///< Last measurement timestamp for integration
struct imu_calib_t calibrated; ///< Calibration bitmask struct imu_calib_t calibrated; ///< Calibration bitmask
struct Int32Rates scaled; ///< Last scaled values in body frame struct Int32Rates scaled; ///< Last scaled values in body frame
struct Int32Rates unscaled; ///< Last unscaled values in sensor frame struct Int32Rates unscaled; ///< Last unscaled values in sensor frame
struct Int32Rates neutral; ///< Neutral values, compensation on unscaled->scaled struct Int32Rates neutral; ///< Neutral values, compensation on unscaled->scaled
struct Int32Rates scale[2]; ///< Scaling, first is numerator and second denominator struct Int32Rates scale[2]; ///< Scaling, first is numerator and second denominator
struct Int32RMat body_to_sensor; ///< Rotation from body to sensor frame struct Int32RMat body_to_sensor; ///< Rotation from body to sensor frame (body to imu combined with imu to sensor)
}; };
struct imu_accel_t { struct imu_accel_t {
uint8_t abi_id; uint8_t abi_id; ///< ABI sensor ID
uint32_t last_stamp; uint32_t last_stamp; ///< Last measurement timestamp for integration
struct imu_calib_t calibrated; ///< Calibration bitmask struct imu_calib_t calibrated; ///< Calibration bitmask
struct Int32Vect3 scaled; struct Int32Vect3 scaled; ///< Last scaled values in body frame
struct Int32Vect3 unscaled; struct Int32Vect3 unscaled; ///< Last unscaled values in sensor frame
struct Int32Vect3 neutral; struct Int32Vect3 neutral; ///< Neutral values, compensation on unscaled->scaled
struct Int32Vect3 scale[2]; struct Int32Vect3 scale[2]; ///< Scaling, first is numerator and second denominator
struct Int32RMat body_to_sensor; ///< Rotation from body to sensor frame struct Int32RMat body_to_sensor; ///< Rotation from body to sensor frame (body to imu combined with imu to sensor)
}; };
struct imu_mag_t { struct imu_mag_t {
uint8_t abi_id; uint8_t abi_id; ///< ABI sensor ID
struct imu_calib_t calibrated; ///< Calibration bitmask struct imu_calib_t calibrated; ///< Calibration bitmask
struct Int32Vect3 scaled; struct Int32Vect3 scaled; ///< Last scaled values in body frame
struct Int32Vect3 unscaled; struct Int32Vect3 unscaled; ///< Last unscaled values in sensor frame
struct Int32Vect3 neutral; struct Int32Vect3 neutral; ///< Neutral values, compensation on unscaled->scaled
struct Int32Vect3 scale[2]; struct Int32Vect3 scale[2]; ///< Scaling, first is numerator and second denominator
struct FloatVect3 current_scale; struct FloatVect3 current_scale; ///< Current scaling multiplying
struct Int32RMat body_to_sensor; ///< Rotation from body to sensor frame struct Int32RMat body_to_sensor; ///< Rotation from body to sensor frame (body to imu combined with imu to sensor)
}; };
/** abstract IMU interface providing fixed point interface */ /** abstract IMU interface providing fixed point interface */
struct Imu { struct Imu {
bool initialized; bool initialized; ///< Check if the IMU is initialized
struct imu_gyro_t gyros[IMU_MAX_SENSORS]; struct imu_gyro_t gyros[IMU_MAX_SENSORS]; ///< The gyro sensors
struct imu_accel_t accels[IMU_MAX_SENSORS]; struct imu_accel_t accels[IMU_MAX_SENSORS]; ///< The accelerometer sensors
struct imu_mag_t mags[IMU_MAX_SENSORS]; struct imu_mag_t mags[IMU_MAX_SENSORS]; ///< The magnetometer sensors
struct OrientationReps body_to_imu; ///< rotation from body to imu frame struct OrientationReps body_to_imu; ///< Rotation from body to imu (all sensors) frame
/** flag for adjusting body_to_imu via settings. /** flag for adjusting body_to_imu via settings.
* if FALSE, reset to airframe values, if TRUE set current roll/pitch * if FALSE, reset to airframe values, if TRUE set current roll/pitch
sw/airborne/modules/imu/imu_cube.c
+70 -26
View File
@@ -28,9 +28,11 @@
#include "modules/core/abi.h" #include "modules/core/abi.h"
#include "mcu_periph/spi.h" #include "mcu_periph/spi.h"
#include "peripherals/invensense2.h" #include "peripherals/invensense2.h"
#include "peripherals/mpu60x0_spi.h"
static struct invensense2_t imu2; static struct invensense2_t imu1;
static struct Mpu60x0_Spi imu2;
static struct invensense2_t imu3; static struct invensense2_t imu3;
void imu_cube_init(void) void imu_cube_init(void)
@@ -38,26 +40,43 @@ void imu_cube_init(void)
struct Int32RMat rmat; struct Int32RMat rmat;
struct Int32Eulers eulers; struct Int32Eulers eulers;
/* IMU 2 (ICM2094) */ /* IMU 1 (ICM20649 not isolated) */
imu2.abi_id = IMU_CUBE2_ID; imu1.abi_id = IMU_CUBE1_ID;
imu2.bus = INVENSENSE2_SPI; imu1.bus = INVENSENSE2_SPI;
imu2.spi.p = &CUBE_IMU2_SPI_DEV; imu1.spi.p = &CUBE_IMU1_SPI_DEV;
imu2.spi.slave_idx = CUBE_IMU2_SPI_SLAVE_IDX; imu1.spi.slave_idx = CUBE_IMU1_SPI_SLAVE_IDX;
imu2.gyro_dlpf = INVENSENSE2_GYRO_DLPF_229HZ; imu1.gyro_dlpf = INVENSENSE2_GYRO_DLPF_229HZ;
imu2.gyro_range = INVENSENSE2_GYRO_RANGE_4000DPS; imu1.gyro_range = INVENSENSE2_GYRO_RANGE_4000DPS;
imu2.accel_dlpf = INVENSENSE2_ACCEL_DLPF_265HZ; imu1.accel_dlpf = INVENSENSE2_ACCEL_DLPF_265HZ;
imu2.accel_range = INVENSENSE2_ACCEL_RANGE_30G; imu1.accel_range = INVENSENSE2_ACCEL_RANGE_30G;
invensense2_init(&imu2); invensense2_init(&imu1);
// Rotation
eulers.phi = ANGLE_BFP_OF_REAL(0),
eulers.theta = ANGLE_BFP_OF_REAL(RadOfDeg(180));
eulers.psi = ANGLE_BFP_OF_REAL(0);
int32_rmat_of_eulers(&rmat, &eulers);
imu_set_defaults_gyro(IMU_CUBE2_ID, &rmat, NULL, NULL);
imu_set_defaults_accel(IMU_CUBE2_ID, &rmat, NULL, NULL);
/* IMU 3 (ICM20649) */ // Rotation
eulers.phi = ANGLE_BFP_OF_REAL(0);
eulers.theta = ANGLE_BFP_OF_REAL(0);
eulers.psi = ANGLE_BFP_OF_REAL(RadOfDeg(270));
int32_rmat_of_eulers(&rmat, &eulers);
imu_set_defaults_gyro(IMU_CUBE1_ID, &rmat, NULL, NULL);
imu_set_defaults_accel(IMU_CUBE1_ID, &rmat, NULL, NULL);
/* IMU 2 (ICM20602 isolated) */
mpu60x0_spi_init(&imu2, &CUBE_IMU2_SPI_DEV, CUBE_IMU2_SPI_SLAVE_IDX);
// change the default configuration
imu2.config.smplrt_div = 3;
imu2.config.dlpf_cfg = MPU60X0_DLPF_256HZ;
imu2.config.dlpf_cfg_acc = MPU60X0_DLPF_ACC_218HZ; // only for ICM sensors
imu2.config.gyro_range = MPU60X0_GYRO_RANGE_2000;
imu2.config.accel_range = MPU60X0_ACCEL_RANGE_16G;
// Rotation
eulers.phi = ANGLE_BFP_OF_REAL(RadOfDeg(180)),
eulers.theta = ANGLE_BFP_OF_REAL(0);
eulers.psi = ANGLE_BFP_OF_REAL(RadOfDeg(270));
int32_rmat_of_eulers(&rmat, &eulers);
imu_set_defaults_gyro(IMU_CUBE2_ID, &rmat, NULL, MPU60X0_GYRO_SENS_FRAC[MPU60X0_GYRO_RANGE_2000]);
imu_set_defaults_accel(IMU_CUBE2_ID, &rmat, NULL, MPU60X0_ACCEL_SENS_FRAC[MPU60X0_ACCEL_RANGE_16G]);
/* IMU 3 (ICM2094 isolated) */
imu3.abi_id = IMU_CUBE3_ID; imu3.abi_id = IMU_CUBE3_ID;
imu3.bus = INVENSENSE2_SPI; imu3.bus = INVENSENSE2_SPI;
imu3.spi.p = &CUBE_IMU3_SPI_DEV; imu3.spi.p = &CUBE_IMU3_SPI_DEV;
@@ -67,11 +86,11 @@ void imu_cube_init(void)
imu3.accel_dlpf = INVENSENSE2_ACCEL_DLPF_265HZ; imu3.accel_dlpf = INVENSENSE2_ACCEL_DLPF_265HZ;
imu3.accel_range = INVENSENSE2_ACCEL_RANGE_30G; imu3.accel_range = INVENSENSE2_ACCEL_RANGE_30G;
invensense2_init(&imu3); invensense2_init(&imu3);
// Rotation // Rotation
eulers.phi = ANGLE_BFP_OF_REAL(0); eulers.phi = ANGLE_BFP_OF_REAL(0),
eulers.theta = ANGLE_BFP_OF_REAL(0); eulers.theta = ANGLE_BFP_OF_REAL(RadOfDeg(180));
eulers.psi = ANGLE_BFP_OF_REAL(RadOfDeg(270)); eulers.psi = ANGLE_BFP_OF_REAL(0);
int32_rmat_of_eulers(&rmat, &eulers); int32_rmat_of_eulers(&rmat, &eulers);
imu_set_defaults_gyro(IMU_CUBE3_ID, &rmat, NULL, NULL); imu_set_defaults_gyro(IMU_CUBE3_ID, &rmat, NULL, NULL);
imu_set_defaults_accel(IMU_CUBE3_ID, &rmat, NULL, NULL); imu_set_defaults_accel(IMU_CUBE3_ID, &rmat, NULL, NULL);
@@ -79,12 +98,37 @@ void imu_cube_init(void)
void imu_cube_periodic(void) void imu_cube_periodic(void)
{ {
invensense2_periodic(&imu2); invensense2_periodic(&imu1);
mpu60x0_spi_periodic(&imu2);
invensense2_periodic(&imu3); invensense2_periodic(&imu3);
} }
void imu_cube_event(void) void imu_cube_event(void)
{ {
invensense2_event(&imu2); invensense2_event(&imu1);
mpu60x0_spi_event(&imu2);
if (imu2.data_available) {
uint32_t now_ts = get_sys_time_usec();
// set channel order
struct Int32Vect3 accel = {
(int32_t)(imu2.data_accel.value[1]),
(int32_t)(imu2.data_accel.value[0]),
-(int32_t)(imu2.data_accel.value[2])
};
struct Int32Rates rates = {
(int32_t)(imu2.data_rates.value[1]),
(int32_t)(imu2.data_rates.value[0]),
-(int32_t)(imu2.data_rates.value[2])
};
imu2.data_available = false;
// Send the scaled values over ABI
AbiSendMsgIMU_GYRO_RAW(IMU_CUBE2_ID, now_ts, &rates, 1);
AbiSendMsgIMU_ACCEL_RAW(IMU_CUBE2_ID, now_ts, &accel, 1);
}
invensense2_event(&imu3); invensense2_event(&imu3);
} }
sw/airborne/modules/ins/ins_ekf2.cpp
+26 -38
View File
@@ -280,8 +280,8 @@ PRINT_CONFIG_VAR(INS_EKF2_BARO_NOISE)
static abi_event baro_ev; static abi_event baro_ev;
static abi_event temperature_ev; static abi_event temperature_ev;
static abi_event agl_ev; static abi_event agl_ev;
static abi_event gyro_ev; static abi_event gyro_int_ev;
static abi_event accel_ev; static abi_event accel_int_ev;
static abi_event mag_ev; static abi_event mag_ev;
static abi_event gps_ev; static abi_event gps_ev;
static abi_event optical_flow_ev; static abi_event optical_flow_ev;
@@ -290,8 +290,8 @@ static abi_event optical_flow_ev;
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure); static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure);
static void temperature_cb(uint8_t sender_id, float temp); static void temperature_cb(uint8_t sender_id, float temp);
static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance); static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance);
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro); static void gyro_int_cb(uint8_t sender_id, uint32_t stamp, struct FloatRates *delta_gyro, uint16_t dt);
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel); static void accel_int_cb(uint8_t sender_id, uint32_t stamp, struct FloatVect3 *delta_accel, uint16_t dt);
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag); static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag);
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s); static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
static void optical_flow_cb(uint8_t sender_id, uint32_t stamp, int32_t flow_x, int32_t flow_y, int32_t flow_der_x, static void optical_flow_cb(uint8_t sender_id, uint32_t stamp, int32_t flow_x, int32_t flow_y, int32_t flow_der_x,
@@ -509,8 +509,6 @@ void ins_ekf2_init(void)
/* Initialize struct */ /* Initialize struct */
ekf2.ltp_stamp = 0; ekf2.ltp_stamp = 0;
ekf2.accel_stamp = 0;
ekf2.gyro_stamp = 0;
ekf2.flow_stamp = 0; ekf2.flow_stamp = 0;
ekf2.gyro_valid = false; ekf2.gyro_valid = false;
ekf2.accel_valid = false; ekf2.accel_valid = false;
@@ -563,8 +561,8 @@ void ins_ekf2_init(void)
AbiBindMsgBARO_ABS(INS_EKF2_BARO_ID, &baro_ev, baro_cb); AbiBindMsgBARO_ABS(INS_EKF2_BARO_ID, &baro_ev, baro_cb);
AbiBindMsgTEMPERATURE(INS_EKF2_TEMPERATURE_ID, &temperature_ev, temperature_cb); AbiBindMsgTEMPERATURE(INS_EKF2_TEMPERATURE_ID, &temperature_ev, temperature_cb);
AbiBindMsgAGL(INS_EKF2_AGL_ID, &agl_ev, agl_cb); AbiBindMsgAGL(INS_EKF2_AGL_ID, &agl_ev, agl_cb);
AbiBindMsgIMU_GYRO(INS_EKF2_GYRO_ID, &gyro_ev, gyro_cb); AbiBindMsgIMU_GYRO_INT(INS_EKF2_GYRO_ID, &gyro_int_ev, gyro_int_cb);
AbiBindMsgIMU_ACCEL(INS_EKF2_ACCEL_ID, &accel_ev, accel_cb); AbiBindMsgIMU_ACCEL_INT(INS_EKF2_ACCEL_ID, &accel_int_ev, accel_int_cb);
AbiBindMsgIMU_MAG(INS_EKF2_MAG_ID, &mag_ev, mag_cb); AbiBindMsgIMU_MAG(INS_EKF2_MAG_ID, &mag_ev, mag_cb);
AbiBindMsgGPS(INS_EKF2_GPS_ID, &gps_ev, gps_cb); AbiBindMsgGPS(INS_EKF2_GPS_ID, &gps_ev, gps_cb);
AbiBindMsgOPTICAL_FLOW(INS_EKF2_OF_ID, &optical_flow_ev, optical_flow_cb); AbiBindMsgOPTICAL_FLOW(INS_EKF2_OF_ID, &optical_flow_ev, optical_flow_cb);
@@ -697,9 +695,9 @@ static void ins_ekf2_publish_attitude(uint32_t stamp)
imuSample imu_sample = {}; imuSample imu_sample = {};
imu_sample.time_us = stamp; imu_sample.time_us = stamp;
imu_sample.delta_ang_dt = ekf2.gyro_dt * 1.e-6f; imu_sample.delta_ang_dt = ekf2.gyro_dt * 1.e-6f;
imu_sample.delta_ang = Vector3f{ekf2.gyro.p, ekf2.gyro.q, ekf2.gyro.r} * imu_sample.delta_ang_dt; imu_sample.delta_ang = Vector3f{ekf2.delta_gyro.p, ekf2.delta_gyro.q, ekf2.delta_gyro.r};
imu_sample.delta_vel_dt = ekf2.accel_dt * 1.e-6f; imu_sample.delta_vel_dt = ekf2.accel_dt * 1.e-6f;
imu_sample.delta_vel = Vector3f{ekf2.accel.x, ekf2.accel.y, ekf2.accel.z} * imu_sample.delta_vel_dt; imu_sample.delta_vel = Vector3f{ekf2.delta_accel.x, ekf2.delta_accel.y, ekf2.delta_accel.z};
ekf.setIMUData(imu_sample); ekf.setIMUData(imu_sample);
if (ekf.attitude_valid()) { if (ekf.attitude_valid()) {
@@ -740,9 +738,9 @@ static void ins_ekf2_publish_attitude(uint32_t stamp)
/* Get in-run gyro bias */ /* Get in-run gyro bias */
struct FloatRates body_rates; struct FloatRates body_rates;
Vector3f gyro_bias{ekf.getGyroBias()}; Vector3f gyro_bias{ekf.getGyroBias()};
body_rates.p = ekf2.gyro.p - gyro_bias(0); body_rates.p = (ekf2.delta_gyro.p / (ekf2.gyro_dt * 1.e-6f)) - gyro_bias(0);
body_rates.q = ekf2.gyro.q - gyro_bias(1); body_rates.q = (ekf2.delta_gyro.q / (ekf2.gyro_dt * 1.e-6f)) - gyro_bias(1);
body_rates.r = ekf2.gyro.r - gyro_bias(2); body_rates.r = (ekf2.delta_gyro.r / (ekf2.gyro_dt * 1.e-6f)) - gyro_bias(2);
// Publish it to the state // Publish it to the state
stateSetBodyRates_f(&body_rates); stateSetBodyRates_f(&body_rates);
@@ -750,9 +748,9 @@ static void ins_ekf2_publish_attitude(uint32_t stamp)
/* Get the in-run acceleration bias */ /* Get the in-run acceleration bias */
struct Int32Vect3 accel; struct Int32Vect3 accel;
Vector3f accel_bias{ekf.getAccelBias()}; Vector3f accel_bias{ekf.getAccelBias()};
accel.x = ACCEL_BFP_OF_REAL(ekf2.accel.x - accel_bias(0)); accel.x = ACCEL_BFP_OF_REAL((ekf2.delta_accel.x / (ekf2.accel_dt * 1e-6f)) - accel_bias(0));
accel.y = ACCEL_BFP_OF_REAL(ekf2.accel.y - accel_bias(1)); accel.y = ACCEL_BFP_OF_REAL((ekf2.delta_accel.y / (ekf2.accel_dt * 1e-6f)) - accel_bias(1));
accel.z = ACCEL_BFP_OF_REAL(ekf2.accel.z - accel_bias(2)); accel.z = ACCEL_BFP_OF_REAL((ekf2.delta_accel.z / (ekf2.accel_dt * 1e-6f)) - accel_bias(2));
// Publish it to the state // Publish it to the state
stateSetAccelBody_i(&accel); stateSetAccelBody_i(&accel);
@@ -796,18 +794,13 @@ static void agl_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp, fl
} }
/* Update INS based on Gyro information */ /* Update INS based on Gyro information */
static void gyro_cb(uint8_t __attribute__((unused)) sender_id, static void gyro_int_cb(uint8_t __attribute__((unused)) sender_id,
uint32_t stamp, struct Int32Rates *gyro) uint32_t stamp, struct FloatRates *delta_gyro, uint16_t dt)
{ {
// Convert Gyro information to float // Copy and save the gyro data
RATES_FLOAT_OF_BFP(ekf2.gyro, *gyro); RATES_COPY(ekf2.delta_gyro, *delta_gyro);
ekf2.gyro_dt = dt;
// Calculate the Gyro interval ekf2.gyro_valid = true;
if (ekf2.gyro_stamp > 0) {
ekf2.gyro_dt = stamp - ekf2.gyro_stamp;
ekf2.gyro_valid = true;
}
ekf2.gyro_stamp = stamp;
/* When Gyro and accelerometer are valid enter it into the EKF */ /* When Gyro and accelerometer are valid enter it into the EKF */
if (ekf2.gyro_valid && ekf2.accel_valid) { if (ekf2.gyro_valid && ekf2.accel_valid) {
@@ -816,18 +809,13 @@ static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
} }
/* Update INS based on Accelerometer information */ /* Update INS based on Accelerometer information */
static void accel_cb(uint8_t sender_id __attribute__((unused)), static void accel_int_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp, struct Int32Vect3 *accel) uint32_t stamp, struct FloatVect3 *delta_accel, uint16_t dt)
{ {
// Convert Accelerometer information to float // Copy and save the gyro data
ACCELS_FLOAT_OF_BFP(ekf2.accel, *accel); VECT3_COPY(ekf2.delta_accel, *delta_accel);
ekf2.accel_dt = dt;
// Calculate the Accelerometer interval ekf2.accel_valid = true;
if (ekf2.accel_stamp > 0) {
ekf2.accel_dt = stamp - ekf2.accel_stamp;
ekf2.accel_valid = true;
}
ekf2.accel_stamp = stamp;
/* When Gyro and accelerometer are valid enter it into the EKF */ /* When Gyro and accelerometer are valid enter it into the EKF */
if (ekf2.gyro_valid && ekf2.accel_valid) { if (ekf2.gyro_valid && ekf2.accel_valid) {
sw/airborne/modules/ins/ins_ekf2.h
+13 -16
View File
@@ -38,23 +38,20 @@ extern "C" {
/* Main EKF2 structure for keeping track of the status and use cross messaging */ /* Main EKF2 structure for keeping track of the status and use cross messaging */
struct ekf2_t { struct ekf2_t {
uint32_t gyro_stamp; ///< Gyroscope last abi message timestamp struct FloatRates delta_gyro; ///< Last gyroscope measurements
uint32_t gyro_dt; ///< Gyroscope delta timestamp between abi messages struct FloatVect3 delta_accel; ///< Last accelerometer measurements
uint32_t accel_stamp; ///< Accelerometer last abi message timestamp uint32_t gyro_dt; ///< Gyroscope delta timestamp between abi messages (us)
uint32_t accel_dt; ///< Accelerometer delta timestamp between abi messages uint32_t accel_dt; ///< Accelerometer delta timestamp between abi messages (us)
uint32_t flow_stamp; ///< Optic flow last abi message timestamp bool gyro_valid; ///< If we received a gyroscope measurement
bool accel_valid; ///< If we received a acceleration measurement
uint32_t flow_stamp; ///< Optic flow last abi message timestamp
struct FloatRates gyro; ///< Last gyroscope measurements float temp; ///< Latest temperature measurement in degrees celcius
struct FloatVect3 accel; ///< Last accelerometer measurements float qnh; ///< QNH value in hPa
bool gyro_valid; ///< If we received a gyroscope measurement uint8_t quat_reset_counter; ///< Amount of quaternion resets from the EKF2
bool accel_valid; ///< If we received a acceleration measurement uint64_t ltp_stamp; ///< Last LTP change timestamp from the EKF2
struct LtpDef_i ltp_def; ///< Latest LTP definition from the quat_reset_counter EKF2
float temp; ///< Latest temperature measurement in degrees celcius bool got_imu_data; ///< If we received valid IMU data (any sensor)
float qnh; ///< QNH value in hPa
uint8_t quat_reset_counter; ///< Amount of quaternion resets from the EKF2
uint64_t ltp_stamp; ///< Last LTP change timestamp from the EKF2
struct LtpDef_i ltp_def; ///< Latest LTP definition from the quat_reset_counter EKF2
bool got_imu_data; ///< If we received valid IMU data (any sensor)
int32_t mag_fusion_type; int32_t mag_fusion_type;
int32_t fusion_mode; int32_t fusion_mode;