diff --git a/conf/airframes/example_twog_analogimu.xml b/conf/airframes/example_twog_analogimu.xml
index 391bd21495..e780a3ad43 100644
--- a/conf/airframes/example_twog_analogimu.xml
+++ b/conf/airframes/example_twog_analogimu.xml
@@ -74,12 +74,6 @@
-
-
-
-
-
-
@@ -199,7 +193,14 @@
-
+
+
+
+
+
+
+
+
diff --git a/conf/autopilot/subsystems/fixedwing/attitude_analogimu.makefile b/conf/autopilot/subsystems/fixedwing/attitude_analogimu.makefile
index 157310e76e..09a0e4f3ac 100644
--- a/conf/autopilot/subsystems/fixedwing/attitude_analogimu.makefile
+++ b/conf/autopilot/subsystems/fixedwing/attitude_analogimu.makefile
@@ -2,28 +2,32 @@
ifeq ($(ARCH), lpc21)
-ap.CFLAGS += -DANALOG_IMU -DADC -DUSE_ADC_0 -DUSE_ADC_1 -DUSE_ADC_2 -DUSE_ADC_3 -DUSE_ADC_4 -DUSE_ADC_5 -DUSE_ADC_6 -DUSE_ADC_7
+ap.CFLAGS += -DANALOG_IMU
+ap.CFLAGS += -DADC -DUSE_$(GYRO_P) -DUSE_$(GYRO_Q) -DUSE_ADC_$(GYRO_R)
+ap.CFLAGS += -DUSE_ADC_3 -DUSE_$(ACCEL_X) -DUSE_$(ACCEL_y) -DUSE_$(ACCEL_Z)
+
+ap.CFLAGS += -DADC_CHANNEL_GYRO_P=$(GYRO_P) -DADC_CHANNEL_GYRO_Q=$(GYRO_Q) -DADC_CHANNEL_GYRO_R=$(GYRO_R)
+ap.CFLAGS += -DADC_CHANNEL_ACCEL_X=$(ACCEL_X) -DADC_CHANNEL_ACCEL_Y=$(ACCEL_Y) -DADC_CHANNEL_ACCEL_Z=$(ACCEL_Z)
+
+ap.srcs += $(SRC_SUBSYSTEMS)/imu/imu_analog.c
ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/dcm/dcm.c
ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/dcm/analogimu.c
-ap.srcs += $(SRC_SUBSYSTEMS)/imu/imu_analog.c
ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/dcm/analogimu_util.c
endif
-# since there is currently no SITL sim for the Analog IMU, we use the infrared sim
+# since there is currently no SITL sim for the Analog IMU, we use the infrared sim
ifeq ($(TARGET), sim)
sim.CFLAGS += -DIR_ROLL_NEUTRAL_DEFAULT=0
-
sim.CFLAGS += -DIR_PITCH_NEUTRAL_DEFAULT=0
$(TARGET).CFLAGS += -DUSE_INFRARED
$(TARGET).srcs += subsystems/sensors/infrared.c
sim.srcs += $(SRC_ARCH)/sim_ir.c
-
sim.srcs += $(SRC_ARCH)/sim_analogimu.c
endif
diff --git a/sw/airborne/subsystems/ahrs/dcm/analogimu.c b/sw/airborne/subsystems/ahrs/dcm/analogimu.c
index a8e7964cf7..56779d822f 100644
--- a/sw/airborne/subsystems/ahrs/dcm/analogimu.c
+++ b/sw/airborne/subsystems/ahrs/dcm/analogimu.c
@@ -154,11 +154,11 @@ void estimator_update_state_analog_imu( void ) {
Euler_angles();
// return euler angles to phi and theta
- estimator_phi = euler[EULER_ROLL]-imu_roll_neutral;
+ estimator_phi = euler.phi-imu_roll_neutral;
//estimator_phi = angle[ANG_ROLL];
- estimator_theta = euler[EULER_PITCH]-imu_pitch_neutral;
+ estimator_theta = euler.theta-imu_pitch_neutral;
//estimator_theta = angle[ANG_PITCH];
- estimator_psi = euler[EULER_YAW];
+ estimator_psi = euler.psi;
}
#endif
diff --git a/sw/airborne/subsystems/ahrs/dcm/dcm.c b/sw/airborne/subsystems/ahrs/dcm/dcm.c
index dc9740715b..61dabf2025 100644
--- a/sw/airborne/subsystems/ahrs/dcm/dcm.c
+++ b/sw/airborne/subsystems/ahrs/dcm/dcm.c
@@ -24,7 +24,9 @@ float DCM_Matrix[3][3] = {{1,0,0},{0,1,0},{0,0,1}};
float Update_Matrix[3][3] = {{0,1,2},{3,4,5},{6,7,8}}; //Gyros here
float Temporary_Matrix[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
-float euler[EULER_LAST] = {0.};
+struct FloatEulers euler= {0, 0, 0};
+float speed_3d = 0;
+
/**************************************************/
@@ -235,9 +237,6 @@ float MAG_Heading;
#endif
-// Stored for Accelerometer Tab
-float speed_3d = 0;
-
void Drift_correction(void)
{
//Compensation the Roll, Pitch and Yaw drift.
@@ -378,17 +377,17 @@ void Matrix_update(void)
void Euler_angles(void)
{
#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
- euler[EULER_ROLL] = atan2(Accel_Vector[1],Accel_Vector[2]); // atan2(acc_y,acc_z)
- euler[EULER_PITCH] = -asin((Accel_Vector[0])/GRAVITY); // asin(acc_x)
- euler[EULER_YAW] = 0;
+ euler.phi = atan2(Accel_Vector[1],Accel_Vector[2]); // atan2(acc_y,acc_z)
+ euler.theta = -asin((Accel_Vector[0])/GRAVITY); // asin(acc_x)
+ euler.psi = 0;
#else
//pitch = -asin(DCM_Matrix[2][0]);
//roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
//yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
- euler[EULER_PITCH] = -asin(DCM_Matrix[2][0]);
- euler[EULER_ROLL] = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
- euler[EULER_YAW] = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
- euler[EULER_YAW] += M_PI; // Rotating the angle 180deg to fit for PPRZ
+ euler.phi = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
+ euler.theta = -asin(DCM_Matrix[2][0]);
+ euler.psi = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
+ euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ
#endif
}
diff --git a/sw/airborne/subsystems/ahrs/dcm/dcm.h b/sw/airborne/subsystems/ahrs/dcm/dcm.h
index 19468c6d12..d5a92b781b 100644
--- a/sw/airborne/subsystems/ahrs/dcm/dcm.h
+++ b/sw/airborne/subsystems/ahrs/dcm/dcm.h
@@ -1,4 +1,6 @@
+#include "math/pprz_algebra_float.h"
+
// Inputs for DCM
extern float Gyro_Vector[3];
extern float Accel_Vector[3];
@@ -14,8 +16,7 @@ void Drift_correction(void);
// Get outputs
void Euler_angles(void);
-enum euler_idx_t { EULER_ROLL, EULER_PITCH, EULER_YAW, EULER_LAST };
-extern float euler[3];
+extern struct FloatEulers euler;
// DCM Parameters