diff --git a/conf/airframes/example_twog_analogimu.xml b/conf/airframes/example_twog_analogimu.xml
index 2705ce0ed4..6d7a26e89f 100644
--- a/conf/airframes/example_twog_analogimu.xml
+++ b/conf/airframes/example_twog_analogimu.xml
@@ -91,12 +91,6 @@
-
-
-
-
-
-
@@ -216,8 +210,15 @@
-
-
+
+
+
+
+
+
+
+
+
diff --git a/conf/autopilot/subsystems/fixedwing/attitude_analogimu.makefile b/conf/autopilot/subsystems/fixedwing/attitude_analogimu.makefile
index 1503e3d091..a0f1d4ff90 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 += -DUSE_ANALOG_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 += -DUSE_ANALOG_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)/ahrs/dcm/dcm.c
ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/dcm/analogimu.c
ap.srcs += $(SRC_SUBSYSTEMS)/imu/imu_analog.c
ap.srcs += $(SRC_SUBSYSTEMS)/imu.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 ff02fed074..116d9a6ef8 100644
--- a/sw/airborne/subsystems/ahrs/dcm/analogimu.c
+++ b/sw/airborne/subsystems/ahrs/dcm/analogimu.c
@@ -126,11 +126,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..26a038f46e 100644
--- a/sw/airborne/subsystems/ahrs/dcm/dcm.c
+++ b/sw/airborne/subsystems/ahrs/dcm/dcm.c
@@ -24,19 +24,19 @@ 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;
+
/**************************************************/
// Algebra
-//Computes the dot product of two vectors
static inline float Vector_Dot_Product(float vector1[3],float vector2[3])
{
return vector1[0]*vector2[0] + vector1[1]*vector2[1] + vector1[2]*vector2[2];
}
-//Computes the cross product of two vectors
static inline void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3])
{
vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
@@ -44,7 +44,6 @@ static inline void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2
vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
}
-//Multiply the vector by a scalar.
static inline void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2)
{
vectorOut[0]=vectorIn[0]*scale2;
@@ -99,117 +98,65 @@ void Normalize(void)
float temporary[3][3];
float renorm=0;
boolean problem=FALSE;
-
+
error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
- Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
- Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
-
- Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
- Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19
-
+ Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
+ Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
+
+ Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]); //eq.19
+ Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]); //eq.19
+
Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
-
- renorm= Vector_Dot_Product(&temporary[0][0],&temporary[0][0]);
+
+ renorm= Vector_Dot_Product(&temporary[0][0],&temporary[0][0]);
if (renorm < 1.5625f && renorm > 0.64f) {
- renorm= .5 * (3-renorm); //eq.21
+ renorm= .5 * (3-renorm); //eq.21
} else if (renorm < 100.0f && renorm > 0.01f) {
renorm= 1. / sqrt(renorm);
-#if PERFORMANCE_REPORTING == 1
+#if PERFORMANCE_REPORTING == 1
renorm_sqrt_count++;
-#endif
-#if PRINT_DEBUG != 0
- Serial.print("???SQT:1,RNM:");
- Serial.print (renorm);
- Serial.print (",ERR:");
- Serial.print (error);
- Serial.print (",TOW:");
- Serial.print (iTOW);
- Serial.println("***");
#endif
} else {
problem = TRUE;
#if PERFORMANCE_REPORTING == 1
renorm_blowup_count++;
-#endif
- #if PRINT_DEBUG != 0
- Serial.print("???PRB:1,RNM:");
- Serial.print (renorm);
- Serial.print (",ERR:");
- Serial.print (error);
- Serial.print (",TOW:");
- Serial.print (iTOW);
- Serial.println("***");
#endif
}
Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
-
- renorm= Vector_Dot_Product(&temporary[1][0],&temporary[1][0]);
+
+ renorm= Vector_Dot_Product(&temporary[1][0],&temporary[1][0]);
if (renorm < 1.5625f && renorm > 0.64f) {
renorm= .5 * (3-renorm); //eq.21
} else if (renorm < 100.0f && renorm > 0.01f) {
- renorm= 1. / sqrt(renorm);
-#if PERFORMANCE_REPORTING == 1
+ renorm= 1. / sqrt(renorm);
+#if PERFORMANCE_REPORTING == 1
renorm_sqrt_count++;
-#endif
-#if PRINT_DEBUG != 0
- Serial.print("???SQT:2,RNM:");
- Serial.print (renorm);
- Serial.print (",ERR:");
- Serial.print (error);
- Serial.print (",TOW:");
- Serial.print (iTOW);
- Serial.println("***");
#endif
} else {
problem = TRUE;
#if PERFORMANCE_REPORTING == 1
renorm_blowup_count++;
-#endif
-#if PRINT_DEBUG != 0
- Serial.print("???PRB:2,RNM:");
- Serial.print (renorm);
- Serial.print (",ERR:");
- Serial.print (error);
- Serial.print (",TOW:");
- Serial.print (iTOW);
- Serial.println("***");
#endif
}
Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
-
- renorm= Vector_Dot_Product(&temporary[2][0],&temporary[2][0]);
+
+ renorm= Vector_Dot_Product(&temporary[2][0],&temporary[2][0]);
if (renorm < 1.5625f && renorm > 0.64f) {
renorm= .5 * (3-renorm); //eq.21
} else if (renorm < 100.0f && renorm > 0.01f) {
- renorm= 1. / sqrt(renorm);
-#if PERFORMANCE_REPORTING == 1
+ renorm= 1. / sqrt(renorm);
+#if PERFORMANCE_REPORTING == 1
renorm_sqrt_count++;
-#endif
-#if PRINT_DEBUG != 0
- Serial.print("???SQT:3,RNM:");
- Serial.print (renorm);
- Serial.print (",ERR:");
- Serial.print (error);
- Serial.print (",TOW:");
- Serial.print (iTOW);
- Serial.println("***");
#endif
} else {
- problem = TRUE;
+ problem = TRUE;
#if PERFORMANCE_REPORTING == 1
renorm_blowup_count++;
-#endif
-#if PRINT_DEBUG != 0
- Serial.print("???PRB:3,RNM:");
- Serial.print (renorm);
- Serial.print (",TOW:");
- Serial.print (iTOW);
- Serial.println("***");
#endif
}
Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
-
+
if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down!
DCM_Matrix[0][0]= 1.0f;
DCM_Matrix[0][1]= 0.0f;
@@ -220,7 +167,7 @@ void Normalize(void)
DCM_Matrix[2][0]= 0.0f;
DCM_Matrix[2][1]= 0.0f;
DCM_Matrix[2][2]= 1.0f;
- problem = FALSE;
+ problem = FALSE;
}
}
@@ -235,12 +182,9 @@ float MAG_Heading;
#endif
-// Stored for Accelerometer Tab
-float speed_3d = 0;
-
void Drift_correction(void)
{
- //Compensation the Roll, Pitch and Yaw drift.
+ //Compensation the Roll, Pitch and Yaw drift.
static float Scaled_Omega_P[3];
static float Scaled_Omega_I[3];
float Accel_magnitude;
@@ -268,34 +212,37 @@ void Drift_correction(void)
Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
// Dynamic weighting of accelerometer info (reliability filter)
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
- Accel_weight = Chop(1 - 2*fabs(1 - Accel_magnitude),0,1); //
-
+ Accel_weight = Chop(1 - 2*fabs(1 - Accel_magnitude),0,1); //
+
#if PERFORMANCE_REPORTING == 1
- tempfloat = ((Accel_weight - 0.5) * 256.0f); //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction
+ {
+
+ float tempfloat = ((Accel_weight - 0.5) * 256.0f); //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction
imu_health += tempfloat;
Bound(imu_health,129,65405);
+ }
#endif
-
+
Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
-
+
Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
- Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
-
+ Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
+
//*****YAW***************
-
- #if USE_MAGNETOMETER==1
+
+ #if USE_MAGNETOMETER==1
// We make the gyro YAW drift correction based on compass magnetic heading
mag_heading_x = cos(MAG_Heading);
mag_heading_y = sin(MAG_Heading);
errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x); //Calculating YAW error
Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
-
+
Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
-
+
Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
- Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
+ Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
#else // Use GPS Ground course to correct yaw gyro drift
if(gps_mode==3 && ground_speed>= 0.5) //hwarm
{
@@ -304,29 +251,21 @@ void Drift_correction(void)
COGY = sin(RadOfDeg(ground_course));
errorCourse=(DCM_Matrix[0][0]*COGY) - (DCM_Matrix[1][0]*COGX); //Calculating YAW error
Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
-
+
Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
-
+
Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
- Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
+ Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
}
#endif
// Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I,Omega_I));
if (Integrator_magnitude > DegOfRad(300)) {
Vector_Scale(Omega_I,Omega_I,0.5f*DegOfRad(300)/Integrator_magnitude);
-#if PRINT_DEBUG != 0
- Serial.print("!!!INT:1,MAG:");
- Serial.print (ToDeg(Integrator_magnitude));
-
- Serial.print (",TOW:");
- Serial.print (iTOW);
- Serial.println("***");
-#endif
}
-
-
+
+
}
/**************************************************/
@@ -338,11 +277,11 @@ void Matrix_update(void)
if (gps_mode==3) //Remove centrifugal acceleration.
{
Accel_Vector[1] += speed_3d*Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ
- Accel_Vector[2] -= speed_3d*Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY
+ Accel_Vector[2] -= speed_3d*Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY
}
-
- #if OUTPUTMODE==1 // With corrected data (drift correction)
+
+ #if OUTPUTMODE==1 // With corrected data (drift correction)
Update_Matrix[0][0]=0;
Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
@@ -371,24 +310,21 @@ void Matrix_update(void)
for(int y=0; y<3; y++)
{
DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
- }
+ }
}
}
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..0cb1dda865 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
@@ -33,5 +34,15 @@ extern float euler[3];
// Mode 1 = DCM integration with Kp and Ki
// Mode 2 = direct accelerometer -> euler
+#define MAGNETOMETER 1
+extern float MAG_Heading;
+
+#define PERFORMANCE_REPORTING 1
+#if PERFORMANCE_REPORTING == 1
+extern int renorm_sqrt_count;
+extern int renorm_blowup_count;
+extern float imu_health;
+#endif
+