diff --git a/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile b/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile
index 0af7123028..cdc8ee2bb7 100644
--- a/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile
+++ b/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile
@@ -4,8 +4,9 @@
ifeq ($(ARCH), lpc21)
ap.CFLAGS += -DUSE_ANALOG_IMU
-ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/dcm/dcm.c
-ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/dcm/analogimu.c
+ap.srcs += $(SRC_SUBSYSTEMS)/ahrs.c
+ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
+ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_dcm.c
endif
diff --git a/conf/settings/tuning_analog_imu.xml b/conf/settings/tuning_analog_imu.xml
index 86a4e8ed1b..df7bfafe91 100644
--- a/conf/settings/tuning_analog_imu.xml
+++ b/conf/settings/tuning_analog_imu.xml
@@ -46,9 +46,9 @@
-->
-
-
-
+
+
+
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index 1762dd1fc0..faf12e3fe6 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -66,8 +66,10 @@
#ifdef USE_ANALOG_IMU
+#include "subsystems/ahrs.h"
+#include "subsystems/ahrs/ahrs_aligner.h"
+#include "subsystems/ahrs/ahrs_float_dcm.h"
#include "subsystems/imu/imu_analog.h"
-#include "subsystems/ahrs/dcm/analogimu.h"
static inline void on_gyro_accel_event( void );
static inline void on_mag_event( void );
#endif
@@ -499,6 +501,8 @@ void init_ap( void ) {
#ifdef USE_ANALOG_IMU
imu_init();
+ ahrs_aligner_init();
+ ahrs_init();
#endif
/************* Links initialization ***************/
@@ -634,10 +638,25 @@ void event_task_ap( void ) {
static inline void on_gyro_accel_event( void ) {
ImuScaleGyro(imu);
ImuScaleAccel(imu);
- estimator_update_state_analog_imu();
+ if (ahrs.status == AHRS_UNINIT) {
+ ahrs_aligner_run();
+ if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
+ ahrs_align();
+ }
+ else {
+ ahrs_propagate();
+ ahrs_update_accel();
+ ahrs_update_fw_estimator();
+ }
}
static inline void on_mag_event(void) {
- //ImuScaleMag(imu);
+ /*
+ ImuScaleMag(imu);
+ if (ahrs.status == AHRS_RUNNING) {
+ ahrs_update_mag();
+ ahrs_update_fw_estimator();
+ }
+ */
}
#endif // USE_ANALOG_IMU
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
new file mode 100644
index 0000000000..7905ca6a1d
--- /dev/null
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
@@ -0,0 +1,419 @@
+/*
+ * Copyright (C) 2010 The Paparazzi Team
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** \file ahrs_float_dcm.c
+ * \brief Attitude estimation for fixedwings based on the DCM
+ *
+ */
+
+#include "std.h"
+
+#include "subsystems/ahrs.h"
+#include "subsystems/ahrs/ahrs_float_dcm.h"
+#include "subsystems/ahrs/ahrs_float_utils.h"
+#include "subsystems/ahrs/ahrs_aligner.h"
+#include "subsystems/imu.h"
+#include "math/pprz_algebra_float.h"
+#include "math/pprz_algebra_int.h"
+
+#include "subsystems/ahrs/ahrs_float_dcm_algebra.h"
+
+#include
+
+//FIXME this is still needed for fixedwing integration
+#include "estimator.h"
+
+struct AhrsFloatDCM ahrs_impl;
+
+// remotely settable
+float imu_roll_neutral = RadOfDeg(IMU_ROLL_NEUTRAL_DEFAULT);
+float imu_pitch_neutral = RadOfDeg(IMU_PITCH_NEUTRAL_DEFAULT);
+
+// Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down.
+// Positive pitch : nose up
+// Positive roll : right wing down
+// Positive yaw : clockwise
+
+// DCM Working variables
+float G_Dt=0.05;
+
+struct FloatRates gyro_float = {0,0,0};
+struct FloatVect3 accel_float = {0,0,0};
+
+float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data
+float Omega_P[3]= {0,0,0}; //Omega Proportional correction
+float Omega_I[3]= {0,0,0}; //Omega Integrator
+float Omega[3]= {0,0,0};
+
+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 speed_3d = 0;
+
+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();
+
+ //warning, only eulers written to ahrs struct so far
+ //compute_body_orientation_and_rates();
+
+ // export results to estimator
+ estimator_phi = ahrs_float.ltp_to_imu_euler.phi - imu_roll_neutral;
+ estimator_theta = ahrs_float.ltp_to_imu_euler.theta - imu_pitch_neutral;
+ estimator_psi = ahrs_float.ltp_to_imu_euler.psi;
+}
+
+
+void ahrs_init(void) {
+ ahrs_float.status = AHRS_UNINIT;
+
+ /*
+ * Initialises our IMU alignement variables
+ * This should probably done in the IMU code instead
+ */
+ struct FloatEulers body_to_imu_euler =
+ {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
+ FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
+ FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
+
+ /* set ltp_to_body to zero */
+ FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);
+ FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler);
+ FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat);
+ FLOAT_RATES_ZERO(ahrs_float.body_rate);
+
+ /* set ltp_to_imu so that body is zero */
+ QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
+ RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
+ EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler);
+ FLOAT_RATES_ZERO(ahrs_float.imu_rate);
+}
+
+void ahrs_align(void)
+{
+ /* Compute an initial orientation using euler angles */
+ ahrs_float_get_euler_from_accel_mag(&ahrs_float.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
+
+ /* Convert initial orientation in quaternion and rotation matrice representations. */
+ FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler);
+ FLOAT_RMAT_OF_QUAT(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_quat);
+
+ /* Compute initial body orientation */
+ compute_body_orientation_and_rates();
+
+ /* use averaged gyro as initial value for bias */
+ struct Int32Rates bias0;
+ RATES_COPY(bias0, ahrs_aligner.lp_gyro);
+ RATES_FLOAT_OF_BFP(ahrs_impl.gyro_bias, bias0);
+
+ ahrs.status = AHRS_RUNNING;
+}
+
+
+void ahrs_propagate(void)
+{
+ /* convert imu data to floating point */
+ RATES_FLOAT_OF_BFP(gyro_float, imu.gyro);
+
+ /* unbias rate measurement */
+ RATES_SUB(gyro_float, ahrs_impl.gyro_bias);
+ /* and save rate in ahrs */
+ RATES_COPY(ahrs_float.imu_rate, gyro_float);
+
+ Matrix_update();
+ Normalize();
+ //INFO, ahrs struct only updated in ahrs_update_fw_estimator
+}
+
+void ahrs_update_accel(void)
+{
+ ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
+
+ //FIXME
+ /*if (gps_mode==3) { //Remove centrifugal acceleration.
+ accel_float.y += speed_3d*Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ
+ accel_float.z -= speed_3d*Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY
+ }
+ */
+
+ Drift_correction();
+}
+
+void ahrs_update_mag(void)
+{
+ //TODO
+}
+
+void Normalize(void)
+{
+ float error=0;
+ 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_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]);
+ 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_sqrt_count++;
+#endif
+ } else {
+ problem = TRUE;
+#if PERFORMANCE_REPORTING == 1
+ renorm_blowup_count++;
+#endif
+ }
+ Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
+
+ 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_sqrt_count++;
+#endif
+ } else {
+ problem = TRUE;
+#if PERFORMANCE_REPORTING == 1
+ renorm_blowup_count++;
+#endif
+ }
+ Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
+
+ 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_sqrt_count++;
+#endif
+ } else {
+ problem = TRUE;
+#if PERFORMANCE_REPORTING == 1
+ renorm_blowup_count++;
+#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;
+ DCM_Matrix[0][2]= 0.0f;
+ DCM_Matrix[1][0]= 0.0f;
+ DCM_Matrix[1][1]= 1.0f;
+ DCM_Matrix[1][2]= 0.0f;
+ DCM_Matrix[2][0]= 0.0f;
+ DCM_Matrix[2][1]= 0.0f;
+ DCM_Matrix[2][2]= 1.0f;
+ problem = FALSE;
+ }
+}
+
+/**************************************************/
+//FIXME
+/* extern short gps_course;
+ extern short gps_gspeed;
+ extern short gps_climb;
+ extern short gps_mode;
+*/
+#ifdef USE_MAGNETOMETER
+float MAG_Heading;
+#endif
+
+
+void Drift_correction(void)
+{
+ //Compensation the Roll, Pitch and Yaw drift.
+ static float Scaled_Omega_P[3];
+ static float Scaled_Omega_I[3];
+ float Accel_magnitude;
+ float Accel_weight;
+ float Integrator_magnitude;
+
+ // Local Working Variables
+ float errorRollPitch[3];
+ float errorYaw[3];
+ float errorCourse;
+ float ground_speed; // This is the velocity your "plane" is traveling in meters for second, 1Meters/Second= 3.6Km/H = 1.944 knots
+ float ground_course; //This is the runaway direction of you "plane" in degrees
+ float COGX; //Course overground X axis
+ float COGY; //Course overground Y axis
+
+ // hwarm
+ /* FIXME
+ ground_course=gps_course/10.-180.;
+ ground_speed=gps_gspeed/100.;
+ float ground_climb=gps_climb/100.;
+ speed_3d = sqrt(ground_speed*ground_speed+ground_climb*ground_climb);
+ */
+ //*****Roll and Pitch***************
+
+ // Calculate the magnitude of the accelerometer vector
+ Accel_magnitude = sqrt(accel_float.x*accel_float.x + accel_float.y*accel_float.y + accel_float.z*accel_float.z);
+ 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); //
+
+ #if PERFORMANCE_REPORTING == 1
+ {
+
+ 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_float.x,&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);
+
+ //*****YAW***************
+
+ #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
+ #else // Use GPS Ground course to correct yaw gyro drift
+ /* FIXME
+ if(gps_mode==3 && ground_speed>= 0.5) { //hwarm
+ COGX = cos(RadOfDeg(ground_course));
+ 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
+ }
+ */
+ #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);
+ }
+
+
+}
+/**************************************************/
+
+void Matrix_update(void)
+{
+ Vector_Add(&Omega[0], &gyro_float.p, &Omega_I[0]); //adding proportional term
+ Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
+
+ #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
+ Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
+ Update_Matrix[1][1]=0;
+ Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
+ Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
+ Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
+ Update_Matrix[2][2]=0;
+ #else // Uncorrected data (no drift correction)
+ Update_Matrix[0][0]=0;
+ Update_Matrix[0][1]=-G_Dt*gyro_float.r;//-z
+ Update_Matrix[0][2]=G_Dt*gyro_float.q;//y
+ Update_Matrix[1][0]=G_Dt*gyro_float.r;//z
+ Update_Matrix[1][1]=0;
+ Update_Matrix[1][2]=-G_Dt*gyro_float.p;
+ Update_Matrix[2][0]=-G_Dt*gyro_float.q;
+ Update_Matrix[2][1]=G_Dt*gyro_float.p;
+ Update_Matrix[2][2]=0;
+ #endif
+
+ Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
+
+ for(int x=0; x<3; x++) //Matrix Addition (update)
+ {
+ 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)
+ ahrs_float.ltp_to_imu_euler.phi = atan2(Accel_Vector[1],Accel_Vector[2]); // atan2(acc_y,acc_z)
+ ahrs_float.ltp_to_imu_euler.theta = -asin((Accel_Vector[0])/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
+ */
+static inline void compute_body_orientation_and_rates(void) {
+
+ FLOAT_QUAT_COMP_INV(ahrs_float.ltp_to_body_quat,
+ ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
+ FLOAT_RMAT_COMP_INV(ahrs_float.ltp_to_body_rmat,
+ ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
+ FLOAT_EULERS_OF_RMAT(ahrs_float.ltp_to_body_euler, ahrs_float.ltp_to_body_rmat);
+ FLOAT_RMAT_TRANSP_RATEMULT(ahrs_float.body_rate, ahrs_impl.body_to_imu_rmat, ahrs_float.imu_rate);
+
+}
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h
new file mode 100644
index 0000000000..e7a56bec67
--- /dev/null
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h
@@ -0,0 +1,78 @@
+/*
+ * Copyright (C) 2010 The Paparazzi Team
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** \file ahrs_float_dcm.h
+ * \brief Attitude estimation for fixedwings based on the DCM
+ *
+ */
+
+#ifndef AHRS_FLOAT_DCM_H
+#define AHRS_FLOAT_DCM_H
+
+#include
+#include "math/pprz_algebra_float.h"
+
+struct AhrsFloatDCM {
+ struct FloatRates gyro_bias;
+ struct FloatRates rate_correction;
+ /*
+ Holds float version of IMU alignement
+ in order to be able to run against the fixed point
+ version of the IMU
+ */
+ struct FloatQuat body_to_imu_quat;
+ struct FloatRMat body_to_imu_rmat;
+};
+extern struct AhrsFloatDCM ahrs_impl;
+
+extern float imu_roll_neutral;
+extern float imu_pitch_neutral;
+
+void ahrs_update_fw_estimator(void);
+
+// DCM Parameters
+
+//#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 Ki_YAW 0.00005
+
+#define GRAVITY 9.81
+
+
+#define OUTPUTMODE 1
+// Mode 0 = DCM integration without Ki gyro bias
+// Mode 1 = DCM integration with Kp and Ki
+// Mode 2 = direct accelerometer -> euler
+
+#define MAGNETOMETER 1
+extern float MAG_Heading;
+
+#define PERFORMANCE_REPORTING 0
+#if PERFORMANCE_REPORTING == 1
+extern int renorm_sqrt_count;
+extern int renorm_blowup_count;
+extern float imu_health;
+#endif
+
+#endif // AHRS_FLOAT_DCM_H
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm_algebra.h b/sw/airborne/subsystems/ahrs/ahrs_float_dcm_algebra.h
new file mode 100644
index 0000000000..c639ad12b1
--- /dev/null
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm_algebra.h
@@ -0,0 +1,57 @@
+//Algebra helper functions for DCM
+
+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];
+}
+
+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]);
+ vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]);
+ vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
+}
+
+static inline void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2)
+{
+ vectorOut[0]=vectorIn[0]*scale2;
+ vectorOut[1]=vectorIn[1]*scale2;
+ vectorOut[2]=vectorIn[2]*scale2;
+}
+
+static inline void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3])
+{
+ vectorOut[0]=vectorIn1[0]+vectorIn2[0];
+ vectorOut[1]=vectorIn1[1]+vectorIn2[1];
+ vectorOut[2]=vectorIn1[2]+vectorIn2[2];
+}
+
+/*
+ #define Matrix_Multiply( _m_a2b, _m_b2c, _m_a2c) { \
+ _m_a2c[0] = (_m_b2c[0]*_m_a2b[0] + _m_b2c[1]*_m_a2b[3] + _m_b2c[2]*_m_a2b[6]); \
+ _m_a2c[1] = (_m_b2c[0]*_m_a2b[1] + _m_b2c[1]*_m_a2b[4] + _m_b2c[2]*_m_a2b[7]); \
+ _m_a2c[2] = (_m_b2c[0]*_m_a2b[2] + _m_b2c[1]*_m_a2b[5] + _m_b2c[2]*_m_a2b[8]); \
+ _m_a2c[3] = (_m_b2c[3]*_m_a2b[0] + _m_b2c[4]*_m_a2b[3] + _m_b2c[5]*_m_a2b[6]); \
+ _m_a2c[4] = (_m_b2c[3]*_m_a2b[1] + _m_b2c[4]*_m_a2b[4] + _m_b2c[5]*_m_a2b[7]); \
+ _m_a2c[5] = (_m_b2c[3]*_m_a2b[2] + _m_b2c[4]*_m_a2b[5] + _m_b2c[5]*_m_a2b[8]); \
+ _m_a2c[6] = (_m_b2c[6]*_m_a2b[0] + _m_b2c[7]*_m_a2b[3] + _m_b2c[8]*_m_a2b[6]); \
+ _m_a2c[7] = (_m_b2c[6]*_m_a2b[1] + _m_b2c[7]*_m_a2b[4] + _m_b2c[8]*_m_a2b[7]); \
+ _m_a2c[8] = (_m_b2c[6]*_m_a2b[2] + _m_b2c[7]*_m_a2b[5] + _m_b2c[8]*_m_a2b[8]); \
+ }
+*/
+
+static inline void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])
+{
+ float op[3];
+ for(int x=0; x<3; x++)
+ {
+ for(int y=0; y<3; y++)
+ {
+ for(int w=0; w<3; w++)
+ {
+ op[w]=a[x][w]*b[w][y];
+ }
+ mat[x][y]=op[0]+op[1]+op[2];
+ }
+ }
+}