diff --git a/conf/airframes/TU_Delft/MicrojetCDW.xml b/conf/airframes/TU_Delft/MicrojetCDW.xml
index 0dca776a29..3f33ab4ba3 100644
--- a/conf/airframes/TU_Delft/MicrojetCDW.xml
+++ b/conf/airframes/TU_Delft/MicrojetCDW.xml
@@ -4,13 +4,13 @@
-
-
-
+
+
+
-
+
@@ -22,8 +22,8 @@
@@ -61,6 +61,13 @@
+
+
+
+
+
+
+
@@ -145,18 +152,18 @@
-
-
+
+
-
-
+
+
-
+
-
-
+
+
diff --git a/conf/settings/tuning_ins.xml b/conf/settings/tuning_ins.xml
index b41c76a9fa..08250f98b9 100644
--- a/conf/settings/tuning_ins.xml
+++ b/conf/settings/tuning_ins.xml
@@ -34,7 +34,7 @@
-
+
diff --git a/sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.c b/sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.c
index 9d056e3adf..858581dca1 100644
--- a/sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.c
+++ b/sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.c
@@ -23,7 +23,7 @@
#include "subsystems/imu.h"
-int imu_overrun_error = 0;
+int imu_overrun = 0;
volatile uint8_t imu_ssp_status;
static void SSP_ISR(void) __attribute__((naked));
#if 0
@@ -86,7 +86,10 @@ void imu_b2_arch_init(void) {
void imu_periodic(void) {
// check ssp idle
if (imu_ssp_status != IMU_SSP_STA_IDLE)
+ {
+ imu_overrun++;
return; //, DEBUG_IMU, IMU_ERR_OVERUN);
+ }
// setup 16 bits
ImuSetSSP16bits();
diff --git a/sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.h b/sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.h
index 76e7804d31..dce131ff6e 100644
--- a/sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.h
+++ b/sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.h
@@ -45,7 +45,7 @@
#define IMU_SSP_STA_BUSY_MAX1168 1
#define IMU_SSP_STA_BUSY_MS2100 2
extern volatile uint8_t imu_ssp_status;
-extern int imu_overrun_error;
+extern int imu_overrun;
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
index a74caa4730..03c2a72107 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
@@ -129,7 +129,6 @@ void ahrs_update_fw_estimator( void )
));
-// estimator_p =
}
@@ -188,8 +187,20 @@ void ahrs_propagate(void)
/* unbias rate measurement */
RATES_DIFF(ahrs_float.imu_rate, gyro_float, ahrs_impl.gyro_bias);
- ahrs_float.imu_rate.p += (ahrs_float.imu_rate.r / 75);
+ /* Uncouple Motions */
+#ifdef GYRO_P_Q
+ float dp=0,dq=0,dr=0;
+ dp += ahrs_float.imu_rate.q * GYRO_P_Q;
+ dp += ahrs_float.imu_rate.r * GYRO_P_R;
+ dq += ahrs_float.imu_rate.p * GYRO_Q_P;
+ dq += ahrs_float.imu_rate.r * GYRO_Q_R;
+ dr += ahrs_float.imu_rate.p * GYRO_R_P;
+ dr += ahrs_float.imu_rate.q * GYRO_R_Q;
+ ahrs_float.imu_rate.p += dp;
+ ahrs_float.imu_rate.q += dq;
+ ahrs_float.imu_rate.r += dr;
+#endif
Matrix_update();
// INFO, ahrs struct only updated in ahrs_update_fw_estimator
diff --git a/sw/airborne/subsystems/imu/imu_analog.c b/sw/airborne/subsystems/imu/imu_analog.c
index 91727c687f..7901d0e86f 100644
--- a/sw/airborne/subsystems/imu/imu_analog.c
+++ b/sw/airborne/subsystems/imu/imu_analog.c
@@ -24,12 +24,14 @@
#include "mcu_periph/uart.h"
volatile bool_t analog_imu_available;
+int imu_overrun;
static struct adc_buf analog_imu_adc_buf[NB_ANALOG_IMU_ADC];
void imu_impl_init(void) {
analog_imu_available = FALSE;
+ imu_overrun = 0;
adc_buf_channel(ADC_CHANNEL_GYRO_P, &analog_imu_adc_buf[0], ADC_CHANNEL_GYRO_NB_SAMPLES);
adc_buf_channel(ADC_CHANNEL_GYRO_Q, &analog_imu_adc_buf[1], ADC_CHANNEL_GYRO_NB_SAMPLES);
@@ -41,6 +43,15 @@ void imu_impl_init(void) {
}
void imu_periodic(void) {
+ // Actual Nr of ADC measurements per channel per periodic loop
+ static int last_head = 0;
+
+ imu_overrun = analog_imu_adc_buf[0].head - last_head;
+ if (imu_overrun < 0)
+ imu_overrun += ADC_CHANNEL_GYRO_NB_SAMPLES;
+ last_head = analog_imu_adc_buf[0].head;
+
+ // Read All Measurements
imu.gyro_unscaled.p = analog_imu_adc_buf[0].sum / ADC_CHANNEL_GYRO_NB_SAMPLES;
imu.gyro_unscaled.q = analog_imu_adc_buf[1].sum / ADC_CHANNEL_GYRO_NB_SAMPLES;
imu.gyro_unscaled.r = analog_imu_adc_buf[2].sum / ADC_CHANNEL_GYRO_NB_SAMPLES;
diff --git a/sw/airborne/subsystems/imu/imu_analog.h b/sw/airborne/subsystems/imu/imu_analog.h
index c0ef110251..2337aa7d6f 100644
--- a/sw/airborne/subsystems/imu/imu_analog.h
+++ b/sw/airborne/subsystems/imu/imu_analog.h
@@ -27,6 +27,7 @@
#define NB_ANALOG_IMU_ADC 6
extern volatile bool_t analog_imu_available;
+extern int imu_overrun;
#define ImuEvent(_gyro_accel_handler, _mag_handler) { \
if (analog_imu_available) { \