diff --git a/conf/abi.xml b/conf/abi.xml
index f219e58f3a..223f97b1dc 100644
--- a/conf/abi.xml
+++ b/conf/abi.xml
@@ -231,13 +231,13 @@
-
+
-
+
diff --git a/conf/airframes/examples/cube_orange.xml b/conf/airframes/examples/cube_orange.xml
index cd508699a3..c4aecd8b08 100644
--- a/conf/airframes/examples/cube_orange.xml
+++ b/conf/airframes/examples/cube_orange.xml
@@ -49,7 +49,9 @@
-
+
+
+
@@ -84,7 +86,7 @@
-
+
@@ -128,7 +130,7 @@
-
+
@@ -139,7 +141,7 @@
-
+
-
-
-
-
-
+
+
+
+
+
+
+
+
+
@@ -202,6 +204,12 @@
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
diff --git a/conf/conf_tests.xml b/conf/conf_tests.xml
index 8a6e35a5c8..b1edd90266 100644
--- a/conf/conf_tests.xml
+++ b/conf/conf_tests.xml
@@ -7,7 +7,7 @@
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic_geofence.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"
/>
IMU driver for the sensors inside the cube autopilots
+ - IMU1: ICM20948 (non-isolated)
+ - IMU2: ICM20602 (isolated)
+ - IMU3: ICM20649 (isolated)
@@ -19,31 +22,33 @@
-
-
-
+
+
+
-
+
-
+
-
-
-
+
+
+
+
+
diff --git a/conf/modules/ins_ekf2.xml b/conf/modules/ins_ekf2.xml
index da39dd8930..1520534b55 100644
--- a/conf/modules/ins_ekf2.xml
+++ b/conf/modules/ins_ekf2.xml
@@ -79,7 +79,7 @@
-
+
diff --git a/conf/simulator/nps/nps_sensors_params_default.h b/conf/simulator/nps/nps_sensors_params_default.h
index feaf3e3606..69471f1653 100644
--- a/conf/simulator/nps/nps_sensors_params_default.h
+++ b/conf/simulator/nps/nps_sensors_params_default.h
@@ -25,10 +25,15 @@
#include "generated/airframe.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_THETA IMU_BODY_TO_IMU_THETA
#define NPS_BODY_TO_IMU_PSI IMU_BODY_TO_IMU_PSI
+#endif
// try to determine propagate frequency
#if defined AHRS_PROPAGATE_FREQUENCY
diff --git a/sw/airborne/modules/imu/imu.c b/sw/airborne/modules/imu/imu.c
index 61cd8381c7..7218a57076 100644
--- a/sw/airborne/modules/imu/imu.c
+++ b/sw/airborne/modules/imu/imu.c
@@ -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
if(gyro->last_stamp > 0 && stamp > gyro->last_stamp) {
struct FloatRates integrated;
- uint16_t delta_dt = stamp - gyro->last_stamp;
+ uint16_t dt = stamp - gyro->last_stamp;
// Trapezoidal integration (TODO: coning correction)
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.r = RATE_FLOAT_OF_BFP(gyro->scaled.r + scaled_rot.r) * 0.5f;
- for(uint8_t i = 0; i < samples-1; i++) {
- integrated.p += data[i].p;
- integrated.q += data[i].q;
- integrated.r += data[i].r;
+ // Only perform multiple integrations in sensor frame if needed
+ if(samples > 1) {
+ struct FloatRates integrated_sensor;
+ 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);
- integrated.q = integrated.q / samples * ((float)delta_dt * 1e-6f);
- integrated.r = integrated.r / samples * ((float)delta_dt * 1e-6f);
+ // Divide by the amount of samples and multiply by the delta time
+ integrated.p = integrated.p / samples * ((float)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
- AbiSendMsgIMU_GYRO_INT(sender_id, stamp, &integrated, delta_dt);
+ AbiSendMsgIMU_GYRO_INT(sender_id, stamp, &integrated, dt);
}
#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
if(accel->last_stamp > 0 && stamp > accel->last_stamp) {
struct FloatVect3 integrated;
- uint16_t delta_dt = stamp - accel->last_stamp;
+ uint16_t dt = stamp - accel->last_stamp;
// Trapezoidal integration
- integrated.x = RATE_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.z = RATE_FLOAT_OF_BFP(accel->scaled.z + scaled_rot.z) * 0.5f;
+ integrated.x = ACCEL_FLOAT_OF_BFP(accel->scaled.x + scaled_rot.x) * 0.5f;
+ integrated.y = ACCEL_FLOAT_OF_BFP(accel->scaled.y + scaled_rot.y) * 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++) {
- integrated.x += data[i].x;
- integrated.y += data[i].y;
- integrated.z += data[i].z;
+ // Only perform multiple integrations in sensor frame if needed
+ if(samples > 1) {
+ struct FloatVect3 integrated_sensor;
+ 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);
- integrated.y = integrated.y / samples * ((float)delta_dt * 1e-6f);
- integrated.z = integrated.z / samples * ((float)delta_dt * 1e-6f);
+ // Divide by the amount of samples and multiply by the delta time
+ integrated.x = integrated.x / samples * ((float)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
- AbiSendMsgIMU_ACCEL_INT(sender_id, stamp, &integrated, delta_dt);
+ AbiSendMsgIMU_ACCEL_INT(sender_id, stamp, &integrated, dt);
}
#endif
diff --git a/sw/airborne/modules/imu/imu.h b/sw/airborne/modules/imu/imu.h
index 87d15276a3..09f037ddde 100644
--- a/sw/airborne/modules/imu/imu.h
+++ b/sw/airborne/modules/imu/imu.h
@@ -46,45 +46,45 @@ struct imu_calib_t {
struct imu_gyro_t {
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 Int32Rates scaled; ///< Last scaled values in body frame
struct Int32Rates unscaled; ///< Last unscaled values in sensor frame
struct Int32Rates neutral; ///< Neutral values, compensation on unscaled->scaled
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 {
- uint8_t abi_id;
- uint32_t last_stamp;
+ uint8_t abi_id; ///< ABI sensor ID
+ uint32_t last_stamp; ///< Last measurement timestamp for integration
struct imu_calib_t calibrated; ///< Calibration bitmask
- struct Int32Vect3 scaled;
- struct Int32Vect3 unscaled;
- struct Int32Vect3 neutral;
- struct Int32Vect3 scale[2];
- struct Int32RMat body_to_sensor; ///< Rotation from body to sensor frame
+ struct Int32Vect3 scaled; ///< Last scaled values in body frame
+ struct Int32Vect3 unscaled; ///< Last unscaled values in sensor frame
+ struct Int32Vect3 neutral; ///< Neutral values, compensation on unscaled->scaled
+ struct Int32Vect3 scale[2]; ///< Scaling, first is numerator and second denominator
+ struct Int32RMat body_to_sensor; ///< Rotation from body to sensor frame (body to imu combined with imu to sensor)
};
struct imu_mag_t {
- uint8_t abi_id;
+ uint8_t abi_id; ///< ABI sensor ID
struct imu_calib_t calibrated; ///< Calibration bitmask
- struct Int32Vect3 scaled;
- struct Int32Vect3 unscaled;
- struct Int32Vect3 neutral;
- struct Int32Vect3 scale[2];
- struct FloatVect3 current_scale;
- struct Int32RMat body_to_sensor; ///< Rotation from body to sensor frame
+ struct Int32Vect3 scaled; ///< Last scaled values in body frame
+ struct Int32Vect3 unscaled; ///< Last unscaled values in sensor frame
+ struct Int32Vect3 neutral; ///< Neutral values, compensation on unscaled->scaled
+ struct Int32Vect3 scale[2]; ///< Scaling, first is numerator and second denominator
+ struct FloatVect3 current_scale; ///< Current scaling multiplying
+ 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 */
struct Imu {
- bool initialized;
- struct imu_gyro_t gyros[IMU_MAX_SENSORS];
- struct imu_accel_t accels[IMU_MAX_SENSORS];
- struct imu_mag_t mags[IMU_MAX_SENSORS];
- struct OrientationReps body_to_imu; ///< rotation from body to imu frame
+ bool initialized; ///< Check if the IMU is initialized
+ struct imu_gyro_t gyros[IMU_MAX_SENSORS]; ///< The gyro sensors
+ struct imu_accel_t accels[IMU_MAX_SENSORS]; ///< The accelerometer sensors
+ struct imu_mag_t mags[IMU_MAX_SENSORS]; ///< The magnetometer sensors
+ struct OrientationReps body_to_imu; ///< Rotation from body to imu (all sensors) frame
/** flag for adjusting body_to_imu via settings.
* if FALSE, reset to airframe values, if TRUE set current roll/pitch
diff --git a/sw/airborne/modules/imu/imu_cube.c b/sw/airborne/modules/imu/imu_cube.c
index 062b91ce2d..6c4cf6b49c 100644
--- a/sw/airborne/modules/imu/imu_cube.c
+++ b/sw/airborne/modules/imu/imu_cube.c
@@ -28,9 +28,11 @@
#include "modules/core/abi.h"
#include "mcu_periph/spi.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;
void imu_cube_init(void)
@@ -38,26 +40,43 @@ void imu_cube_init(void)
struct Int32RMat rmat;
struct Int32Eulers eulers;
- /* IMU 2 (ICM2094) */
- imu2.abi_id = IMU_CUBE2_ID;
- imu2.bus = INVENSENSE2_SPI;
- imu2.spi.p = &CUBE_IMU2_SPI_DEV;
- imu2.spi.slave_idx = CUBE_IMU2_SPI_SLAVE_IDX;
- imu2.gyro_dlpf = INVENSENSE2_GYRO_DLPF_229HZ;
- imu2.gyro_range = INVENSENSE2_GYRO_RANGE_4000DPS;
- imu2.accel_dlpf = INVENSENSE2_ACCEL_DLPF_265HZ;
- imu2.accel_range = INVENSENSE2_ACCEL_RANGE_30G;
- invensense2_init(&imu2);
-
- // 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 1 (ICM20649 not isolated) */
+ imu1.abi_id = IMU_CUBE1_ID;
+ imu1.bus = INVENSENSE2_SPI;
+ imu1.spi.p = &CUBE_IMU1_SPI_DEV;
+ imu1.spi.slave_idx = CUBE_IMU1_SPI_SLAVE_IDX;
+ imu1.gyro_dlpf = INVENSENSE2_GYRO_DLPF_229HZ;
+ imu1.gyro_range = INVENSENSE2_GYRO_RANGE_4000DPS;
+ imu1.accel_dlpf = INVENSENSE2_ACCEL_DLPF_265HZ;
+ imu1.accel_range = INVENSENSE2_ACCEL_RANGE_30G;
+ invensense2_init(&imu1);
- /* 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.bus = INVENSENSE2_SPI;
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_range = INVENSENSE2_ACCEL_RANGE_30G;
invensense2_init(&imu3);
-
+
// Rotation
- eulers.phi = ANGLE_BFP_OF_REAL(0);
- eulers.theta = ANGLE_BFP_OF_REAL(0);
- eulers.psi = ANGLE_BFP_OF_REAL(RadOfDeg(270));
+ 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_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)
{
- invensense2_periodic(&imu2);
+ invensense2_periodic(&imu1);
+ mpu60x0_spi_periodic(&imu2);
invensense2_periodic(&imu3);
}
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);
}
diff --git a/sw/airborne/modules/ins/ins_ekf2.cpp b/sw/airborne/modules/ins/ins_ekf2.cpp
index 7cba61bb56..86677c38e5 100644
--- a/sw/airborne/modules/ins/ins_ekf2.cpp
+++ b/sw/airborne/modules/ins/ins_ekf2.cpp
@@ -280,8 +280,8 @@ PRINT_CONFIG_VAR(INS_EKF2_BARO_NOISE)
static abi_event baro_ev;
static abi_event temperature_ev;
static abi_event agl_ev;
-static abi_event gyro_ev;
-static abi_event accel_ev;
+static abi_event gyro_int_ev;
+static abi_event accel_int_ev;
static abi_event mag_ev;
static abi_event gps_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 temperature_cb(uint8_t sender_id, float temp);
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 accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel);
+static void gyro_int_cb(uint8_t sender_id, uint32_t stamp, struct FloatRates *delta_gyro, uint16_t dt);
+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 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,
@@ -509,8 +509,6 @@ void ins_ekf2_init(void)
/* Initialize struct */
ekf2.ltp_stamp = 0;
- ekf2.accel_stamp = 0;
- ekf2.gyro_stamp = 0;
ekf2.flow_stamp = 0;
ekf2.gyro_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);
AbiBindMsgTEMPERATURE(INS_EKF2_TEMPERATURE_ID, &temperature_ev, temperature_cb);
AbiBindMsgAGL(INS_EKF2_AGL_ID, &agl_ev, agl_cb);
- AbiBindMsgIMU_GYRO(INS_EKF2_GYRO_ID, &gyro_ev, gyro_cb);
- AbiBindMsgIMU_ACCEL(INS_EKF2_ACCEL_ID, &accel_ev, accel_cb);
+ AbiBindMsgIMU_GYRO_INT(INS_EKF2_GYRO_ID, &gyro_int_ev, gyro_int_cb);
+ AbiBindMsgIMU_ACCEL_INT(INS_EKF2_ACCEL_ID, &accel_int_ev, accel_int_cb);
AbiBindMsgIMU_MAG(INS_EKF2_MAG_ID, &mag_ev, mag_cb);
AbiBindMsgGPS(INS_EKF2_GPS_ID, &gps_ev, gps_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 = {};
imu_sample.time_us = stamp;
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 = 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);
if (ekf.attitude_valid()) {
@@ -740,9 +738,9 @@ static void ins_ekf2_publish_attitude(uint32_t stamp)
/* Get in-run gyro bias */
struct FloatRates body_rates;
Vector3f gyro_bias{ekf.getGyroBias()};
- body_rates.p = ekf2.gyro.p - gyro_bias(0);
- body_rates.q = ekf2.gyro.q - gyro_bias(1);
- body_rates.r = ekf2.gyro.r - gyro_bias(2);
+ body_rates.p = (ekf2.delta_gyro.p / (ekf2.gyro_dt * 1.e-6f)) - gyro_bias(0);
+ body_rates.q = (ekf2.delta_gyro.q / (ekf2.gyro_dt * 1.e-6f)) - gyro_bias(1);
+ body_rates.r = (ekf2.delta_gyro.r / (ekf2.gyro_dt * 1.e-6f)) - gyro_bias(2);
// Publish it to the state
stateSetBodyRates_f(&body_rates);
@@ -750,9 +748,9 @@ static void ins_ekf2_publish_attitude(uint32_t stamp)
/* Get the in-run acceleration bias */
struct Int32Vect3 accel;
Vector3f accel_bias{ekf.getAccelBias()};
- accel.x = ACCEL_BFP_OF_REAL(ekf2.accel.x - accel_bias(0));
- accel.y = ACCEL_BFP_OF_REAL(ekf2.accel.y - accel_bias(1));
- accel.z = ACCEL_BFP_OF_REAL(ekf2.accel.z - accel_bias(2));
+ 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.delta_accel.y / (ekf2.accel_dt * 1e-6f)) - accel_bias(1));
+ accel.z = ACCEL_BFP_OF_REAL((ekf2.delta_accel.z / (ekf2.accel_dt * 1e-6f)) - accel_bias(2));
// Publish it to the state
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 */
-static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
- uint32_t stamp, struct Int32Rates *gyro)
+static void gyro_int_cb(uint8_t __attribute__((unused)) sender_id,
+ uint32_t stamp, struct FloatRates *delta_gyro, uint16_t dt)
{
- // Convert Gyro information to float
- RATES_FLOAT_OF_BFP(ekf2.gyro, *gyro);
-
- // Calculate the Gyro interval
- if (ekf2.gyro_stamp > 0) {
- ekf2.gyro_dt = stamp - ekf2.gyro_stamp;
- ekf2.gyro_valid = true;
- }
- ekf2.gyro_stamp = stamp;
+ // Copy and save the gyro data
+ RATES_COPY(ekf2.delta_gyro, *delta_gyro);
+ ekf2.gyro_dt = dt;
+ ekf2.gyro_valid = true;
/* When Gyro and accelerometer are valid enter it into the EKF */
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 */
-static void accel_cb(uint8_t sender_id __attribute__((unused)),
- uint32_t stamp, struct Int32Vect3 *accel)
+static void accel_int_cb(uint8_t sender_id __attribute__((unused)),
+ uint32_t stamp, struct FloatVect3 *delta_accel, uint16_t dt)
{
- // Convert Accelerometer information to float
- ACCELS_FLOAT_OF_BFP(ekf2.accel, *accel);
-
- // Calculate the Accelerometer interval
- if (ekf2.accel_stamp > 0) {
- ekf2.accel_dt = stamp - ekf2.accel_stamp;
- ekf2.accel_valid = true;
- }
- ekf2.accel_stamp = stamp;
+ // Copy and save the gyro data
+ VECT3_COPY(ekf2.delta_accel, *delta_accel);
+ ekf2.accel_dt = dt;
+ ekf2.accel_valid = true;
/* When Gyro and accelerometer are valid enter it into the EKF */
if (ekf2.gyro_valid && ekf2.accel_valid) {
diff --git a/sw/airborne/modules/ins/ins_ekf2.h b/sw/airborne/modules/ins/ins_ekf2.h
index 9398cfb05f..4a817b6ec0 100644
--- a/sw/airborne/modules/ins/ins_ekf2.h
+++ b/sw/airborne/modules/ins/ins_ekf2.h
@@ -38,23 +38,20 @@ extern "C" {
/* Main EKF2 structure for keeping track of the status and use cross messaging */
struct ekf2_t {
- uint32_t gyro_stamp; ///< Gyroscope last abi message timestamp
- uint32_t gyro_dt; ///< Gyroscope delta timestamp between abi messages
- uint32_t accel_stamp; ///< Accelerometer last abi message timestamp
- uint32_t accel_dt; ///< Accelerometer delta timestamp between abi messages
- uint32_t flow_stamp; ///< Optic flow last abi message timestamp
+ struct FloatRates delta_gyro; ///< Last gyroscope measurements
+ struct FloatVect3 delta_accel; ///< Last accelerometer measurements
+ uint32_t gyro_dt; ///< Gyroscope delta timestamp between abi messages (us)
+ uint32_t accel_dt; ///< Accelerometer delta timestamp between abi messages (us)
+ 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
- struct FloatVect3 accel; ///< Last accelerometer measurements
- bool gyro_valid; ///< If we received a gyroscope measurement
- bool accel_valid; ///< If we received a acceleration measurement
-
- float temp; ///< Latest temperature measurement in degrees celcius
- 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)
+ float temp; ///< Latest temperature measurement in degrees celcius
+ 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 fusion_mode;