diff --git a/conf/airframes/TU_Delft/MicrojetCDW.xml b/conf/airframes/TU_Delft/MicrojetCDW.xml
index 92866b13e9..506537f8f1 100644
--- a/conf/airframes/TU_Delft/MicrojetCDW.xml
+++ b/conf/airframes/TU_Delft/MicrojetCDW.xml
@@ -52,37 +52,39 @@
-
-
-
+
+
+
-
-
-
+
+
+
+
-
-
+
+
-
-
-
+
+
+
-
-
-
+
+
+
+
-
+
-
+
-
-
-
+
+
+
-
-
-
+
+
+
@@ -146,14 +148,14 @@
-
+
-
+
@@ -181,8 +183,10 @@
-
-
+
+
+
+
diff --git a/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile b/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile
index 9a344ab635..606a43bec4 100644
--- a/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile
+++ b/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile
@@ -5,11 +5,16 @@ $(TARGET).CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm.h\"
ifeq ($(ARCH), lpc21)
-ap.CFLAGS += -DUSE_IMU
+ap.CFLAGS += -DUSE_AHRS
ap.srcs += $(SRC_SUBSYSTEMS)/ahrs.c
ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_dcm.c
+ap.CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
+
+ ifdef CPU_LED
+ ap.CFLAGS += -DAHRS_CPU_LED=$(CPU_LED)
+ endif
endif
diff --git a/conf/autopilot/subsystems/fixedwing/imu_analog.makefile b/conf/autopilot/subsystems/fixedwing/imu_analog.makefile
index 499706aa75..b2b3e8ca83 100644
--- a/conf/autopilot/subsystems/fixedwing/imu_analog.makefile
+++ b/conf/autopilot/subsystems/fixedwing/imu_analog.makefile
@@ -44,7 +44,7 @@
ifeq ($(ARCH), lpc21)
-imu_CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_analog.h\"
+imu_CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_analog.h\" -DUSE_IMU
imu_CFLAGS += -DADC
imu_CFLAGS += -DUSE_$(GYRO_P) -DUSE_$(GYRO_Q) -DUSE_$(GYRO_R)
diff --git a/conf/autopilot/subsystems/fixedwing/imu_booz.makefile b/conf/autopilot/subsystems/fixedwing/imu_booz.makefile
index 5de6f0bf69..e4d397d37a 100644
--- a/conf/autopilot/subsystems/fixedwing/imu_booz.makefile
+++ b/conf/autopilot/subsystems/fixedwing/imu_booz.makefile
@@ -41,7 +41,7 @@
# imu Booz2 v1.0, v1.1, v1.2, YAI v1.0
-imu_CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_b2.h\"
+imu_CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_b2.h\" -DUSE_IMU
imu_srcs += $(SRC_SUBSYSTEMS)/imu.c
imu_srcs += $(SRC_SUBSYSTEMS)/imu/imu_b2.c
imu_srcs += $(SRC_ARCH)/subsystems/imu/imu_b2_arch.c
diff --git a/conf/settings/tuning_ins.xml b/conf/settings/tuning_ins.xml
index fb7ac75c5d..bc080d1c9b 100644
--- a/conf/settings/tuning_ins.xml
+++ b/conf/settings/tuning_ins.xml
@@ -41,7 +41,7 @@
-
+
diff --git a/conf/telemetry/default_fixedwing_imu.xml b/conf/telemetry/default_fixedwing_imu.xml
index fcf88d9dfb..c106da4e29 100644
--- a/conf/telemetry/default_fixedwing_imu.xml
+++ b/conf/telemetry/default_fixedwing_imu.xml
@@ -7,7 +7,7 @@
-
+
@@ -26,8 +26,8 @@
-
-
+
+
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 037196de75..456c7bff65 100644
--- a/sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.c
+++ b/sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.c
@@ -84,7 +84,8 @@ void imu_b2_arch_init(void) {
void imu_periodic(void) {
// check ssp idle
- // ASSERT((imu_status == IMU_STA_IDLE), DEBUG_IMU, IMU_ERR_OVERUN);
+ if (imu_ssp_status != IMU_SSP_STA_IDLE)
+ return; //, DEBUG_IMU, IMU_ERR_OVERUN);
// setup 16 bits
ImuSetSSP16bits();
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index 4024353f06..3eaa229848 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -65,8 +65,10 @@
#endif
-#ifdef USE_ANALOG_IMU
+#ifdef USE_IMU
#include "subsystems/imu.h"
+#endif
+#ifdef USE_AHRS
#include "subsystems/ahrs.h"
#include "subsystems/ahrs/ahrs_aligner.h"
#include "subsystems/ahrs/ahrs_float_dcm.h"
@@ -377,6 +379,21 @@ void periodic_task_ap( void ) {
static uint8_t _4Hz = 0;
static uint8_t _1Hz = 0;
+#ifdef USE_IMU
+ // Run at PERIODIC_FREQUENCY (60Hz if not defined)
+
+#ifdef AHRS_CPU_LED
+ LED_ON(AHRS_CPU_LED);
+#endif
+
+ imu_periodic();
+#ifdef AHRS_CPU_LED
+ LED_OFF(AHRS_CPU_LED);
+#endif
+
+#endif // USE_IMU
+
+
#ifdef PERIODIC_FREQUENCY
#warning Using HighSpeed Periodic: Manually check to make sure PERIODIC_FREQUENCY is a multiple of 60.
static uint8_t _60Hz = 0;
@@ -392,13 +409,8 @@ void periodic_task_ap( void ) {
#endif
-#ifdef USE_IMU
-// if (!_20Hz) {
- imu_periodic();
-// }
-#endif // USE_IMU
-
+ // Rest of the periodic function still runs at 60Hz like always
_20Hz++;
if (_20Hz>=3) _20Hz=0;
_10Hz++;
@@ -518,8 +530,10 @@ void init_ap( void ) {
GpioInit();
#endif
-#ifdef USE_ANALOG_IMU
+#ifdef USE_IMU
imu_init();
+#endif
+#ifdef USE_AHRS
ahrs_aligner_init();
ahrs_init();
#endif
@@ -578,9 +592,9 @@ void init_ap( void ) {
/*********** EVENT ***********************************************************/
void event_task_ap( void ) {
-#ifdef USE_ANALOG_IMU
+#ifdef USE_AHRS
ImuEvent(on_gyro_accel_event, on_mag_event);
-#endif // USE_ANALOG_IMU
+#endif // USE_AHRS
#ifdef USE_GPS
#if !(defined HITL) && !(defined UBX_EXTERNAL) /** else comes through the datalink */
@@ -653,10 +667,14 @@ void event_task_ap( void ) {
modules_event_task();
} /* event_task_ap() */
-#ifdef USE_ANALOG_IMU
+#ifdef USE_AHRS
static inline void on_gyro_accel_event( void ) {
- static uint8_t _60Hz = 0;
- LED_ON(2);
+ static uint8_t _reduced_rate = 0;
+
+#ifdef AHRS_CPU_LED
+ LED_ON(AHRS_CPU_LED);
+#endif
+
ImuScaleGyro(imu);
ImuScaleAccel(imu);
@@ -667,16 +685,26 @@ static inline void on_gyro_accel_event( void ) {
ahrs_align();
}
else {
- ahrs_propagate();
- if (_60Hz >= 8)
+ //if (_reduced_rate % 4)
{
- _60Hz = 0;
+ ahrs_propagate();
+ }
+
+
+ _reduced_rate++;
+ if (_reduced_rate >= (PERIODIC_FREQUENCY / 60))
+ {
+ _reduced_rate = 0;
ahrs_update_accel();
ahrs_update_fw_estimator();
}
+
}
- LED_OFF(2);
+
+#ifdef AHRS_CPU_LED
+ LED_OFF(AHRS_CPU_LED);
+#endif
}
static inline void on_mag_event(void) {
@@ -688,4 +716,4 @@ static inline void on_mag_event(void) {
}
*/
}
-#endif // USE_ANALOG_IMU
+#endif // USE_AHRS
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
index f764d8a4af..819019e74f 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
@@ -73,14 +73,23 @@ float MAG_Heading;
static inline void compute_body_orientation_and_rates(void);
void Normalize(void);
void Drift_correction(void);
-void Euler_angles(void);
void Matrix_update(void);
/**************************************************/
void ahrs_update_fw_estimator( void )
{
- Euler_angles();
+#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
+ ahrs_float.ltp_to_imu_euler.phi = atan2(accel_float.y,accel_float.z); // atan2(acc_y,acc_z)
+ ahrs_float.ltp_to_imu_euler.theta = -asin((accel_float.x)/GRAVITY); // asin(acc_x)
+ ahrs_float.ltp_to_imu_euler.psi = 0;
+#else
+ ahrs_float.ltp_to_imu_euler.phi = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
+ ahrs_float.ltp_to_imu_euler.theta = -asin(DCM_Matrix[2][0]);
+ ahrs_float.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
+ ahrs_float.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ
+#endif
+
//warning, only eulers written to ahrs struct so far
//compute_body_orientation_and_rates();
@@ -89,6 +98,8 @@ void ahrs_update_fw_estimator( void )
estimator_phi = ahrs_float.ltp_to_imu_euler.phi - ins_roll_neutral;
estimator_theta = ahrs_float.ltp_to_imu_euler.theta - ins_pitch_neutral;
estimator_psi = ahrs_float.ltp_to_imu_euler.psi;
+
+// estimator_p =
}
@@ -147,13 +158,18 @@ 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);
+
+
Matrix_update();
- Normalize();
- //INFO, ahrs struct only updated in ahrs_update_fw_estimator
+ // INFO, ahrs struct only updated in ahrs_update_fw_estimator
+
+ // Normalize();
}
void ahrs_update_accel(void)
{
+
ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
#ifdef USE_GPS
@@ -379,20 +395,6 @@ void Matrix_update(void)
}
}
-void Euler_angles(void)
-{
-#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
- ahrs_float.ltp_to_imu_euler.phi = atan2(accel_float.y,accel_float.z); // atan2(acc_y,acc_z)
- ahrs_float.ltp_to_imu_euler.theta = -asin((accel_float.x)/GRAVITY); // asin(acc_x)
- ahrs_float.ltp_to_imu_euler.psi = 0;
-#else
- ahrs_float.ltp_to_imu_euler.phi = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
- ahrs_float.ltp_to_imu_euler.theta = -asin(DCM_Matrix[2][0]);
- ahrs_float.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
- ahrs_float.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ
-#endif
-}
-
/*
* Compute body orientation and rates from imu orientation and rates
*/
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h
index 6bf2551539..cb9c3d718a 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h
@@ -47,7 +47,7 @@ void ahrs_update_fw_estimator(void);
//#define Kp_ROLLPITCH 0.2
#define Kp_ROLLPITCH 0.015
#define Ki_ROLLPITCH 0.000010
-#define Kp_YAW 1.2 //High yaw drift correction gain - use with caution!
+#define Kp_YAW 0.9 //High yaw drift correction gain - use with caution!
#define Ki_YAW 0.00005
#define GRAVITY 9.81