Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-30 11:37:06 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

[ahrs] convert float_dcm

Browse Source
This commit is contained in:
Felix Ruess
2014-10-13 19:24:03 +02:00
parent 0df0aefe48
commit 0f72aacdb4
2 changed files with 116 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
+102 -89
View File
@@ -30,8 +30,6 @@
#include "subsystems/ahrs.h" #include "subsystems/ahrs.h"
#include "subsystems/ahrs/ahrs_float_dcm.h" #include "subsystems/ahrs/ahrs_float_dcm.h"
#include "subsystems/ahrs/ahrs_float_utils.h" #include "subsystems/ahrs/ahrs_float_utils.h"
#include "subsystems/ahrs/ahrs_aligner.h"
#include "subsystems/imu.h"
#include "firmwares/fixedwing/autopilot.h" // launch detection #include "firmwares/fixedwing/autopilot.h" // launch detection
#include "subsystems/ahrs/ahrs_float_dcm_algebra.h" #include "subsystems/ahrs/ahrs_float_dcm_algebra.h"
@@ -55,7 +53,7 @@
#endif #endif
struct AhrsFloatDCM ahrs_impl; struct AhrsFloatDCM ahrs_dcm;
// Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down. // Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down.
// Positive pitch : nose up // Positive pitch : nose up
@@ -63,6 +61,7 @@ struct AhrsFloatDCM ahrs_impl;
// Positive yaw : clockwise // Positive yaw : clockwise
struct FloatVect3 accel_float = {0,0,0}; struct FloatVect3 accel_float = {0,0,0};
struct FloatVect3 mag_float = {0,0,0};
float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data
float Omega_P[3]= {0,0,0}; //Omega Proportional correction float Omega_P[3]= {0,0,0}; //Omega Proportional correction
@@ -102,34 +101,42 @@ static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat)
} }
} }
void ahrs_dcm_register(void)
void ahrs_init(void) { {
ahrs.status = AHRS_UNINIT; ahrs_register_impl(ahrs_dcm_init, ahrs_dcm_align, ahrs_dcm_propagate,
ahrs_dcm_update_accel, ahrs_dcm_update_mag, ahrs_dcm_update_gps);
/* Set ltp_to_imu so that body is zero */
memcpy(&ahrs_impl.ltp_to_imu_euler, orientationGetEulers_f(&imu.body_to_imu),
sizeof(struct FloatEulers));
FLOAT_RATES_ZERO(ahrs_impl.imu_rate);
/* set inital filter dcm */
set_dcm_matrix_from_rmat(orientationGetRMat_f(&imu.body_to_imu));
ahrs_impl.gps_speed = 0;
ahrs_impl.gps_acceleration = 0;
ahrs_impl.gps_course = 0;
ahrs_impl.gps_course_valid = FALSE;
ahrs_impl.gps_age = 100;
} }
void ahrs_align(void) void ahrs_dcm_init(struct OrientationReps* body_to_imu)
{
/* save body_to_imu pointer */
ahrs_dcm.body_to_imu = body_to_imu;
/* Set ltp_to_imu so that body is zero */
memcpy(&ahrs_dcm.ltp_to_imu_euler, orientationGetEulers_f(ahrs_dcm.body_to_imu),
sizeof(struct FloatEulers));
FLOAT_RATES_ZERO(ahrs_dcm.imu_rate);
/* set inital filter dcm */
set_dcm_matrix_from_rmat(orientationGetRMat_f(ahrs_dcm.body_to_imu));
ahrs_dcm.gps_speed = 0;
ahrs_dcm.gps_acceleration = 0;
ahrs_dcm.gps_course = 0;
ahrs_dcm.gps_course_valid = FALSE;
ahrs_dcm.gps_age = 100;
}
bool_t ahrs_dcm_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
struct Int32Vect3* lp_mag)
{ {
/* Compute an initial orientation using euler angles */ /* Compute an initial orientation using euler angles */
ahrs_float_get_euler_from_accel_mag(&ahrs_impl.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); ahrs_float_get_euler_from_accel_mag(&ahrs_dcm.ltp_to_imu_euler, lp_accel, lp_mag);
/* Convert initial orientation in quaternion and rotation matrice representations. */ /* Convert initial orientation in quaternion and rotation matrice representations. */
struct FloatRMat ltp_to_imu_rmat; struct FloatRMat ltp_to_imu_rmat;
float_rmat_of_eulers(&ltp_to_imu_rmat, &ahrs_impl.ltp_to_imu_euler); float_rmat_of_eulers(&ltp_to_imu_rmat, &ahrs_dcm.ltp_to_imu_euler);
/* set filter dcm */ /* set filter dcm */
set_dcm_matrix_from_rmat(&ltp_to_imu_rmat); set_dcm_matrix_from_rmat(&ltp_to_imu_rmat);
@@ -139,35 +146,35 @@ void ahrs_align(void)
/* use averaged gyro as initial value for bias */ /* use averaged gyro as initial value for bias */
struct Int32Rates bias0; struct Int32Rates bias0;
RATES_COPY(bias0, ahrs_aligner.lp_gyro); RATES_COPY(bias0, *lp_gyro);
RATES_FLOAT_OF_BFP(ahrs_impl.gyro_bias, bias0); RATES_FLOAT_OF_BFP(ahrs_dcm.gyro_bias, bias0);
ahrs.status = AHRS_RUNNING; return TRUE;
} }
void ahrs_propagate(float dt) void ahrs_dcm_propagate(struct Int32Rates* gyro, float dt)
{ {
/* convert imu data to floating point */ /* convert imu data to floating point */
struct FloatRates gyro_float; struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); RATES_FLOAT_OF_BFP(gyro_float, *gyro);
/* unbias rate measurement */ /* unbias rate measurement */
RATES_DIFF(ahrs_impl.imu_rate, gyro_float, ahrs_impl.gyro_bias); RATES_DIFF(ahrs_dcm.imu_rate, gyro_float, ahrs_dcm.gyro_bias);
/* Uncouple Motions */ /* Uncouple Motions */
#ifdef IMU_GYRO_P_Q #ifdef IMU_GYRO_P_Q
float dp=0,dq=0,dr=0; float dp=0,dq=0,dr=0;
dp += ahrs_impl.imu_rate.q * IMU_GYRO_P_Q; dp += ahrs_dcm.imu_rate.q * IMU_GYRO_P_Q;
dp += ahrs_impl.imu_rate.r * IMU_GYRO_P_R; dp += ahrs_dcm.imu_rate.r * IMU_GYRO_P_R;
dq += ahrs_impl.imu_rate.p * IMU_GYRO_Q_P; dq += ahrs_dcm.imu_rate.p * IMU_GYRO_Q_P;
dq += ahrs_impl.imu_rate.r * IMU_GYRO_Q_R; dq += ahrs_dcm.imu_rate.r * IMU_GYRO_Q_R;
dr += ahrs_impl.imu_rate.p * IMU_GYRO_R_P; dr += ahrs_dcm.imu_rate.p * IMU_GYRO_R_P;
dr += ahrs_impl.imu_rate.q * IMU_GYRO_R_Q; dr += ahrs_dcm.imu_rate.q * IMU_GYRO_R_Q;
ahrs_impl.imu_rate.p += dp; ahrs_dcm.imu_rate.p += dp;
ahrs_impl.imu_rate.q += dq; ahrs_dcm.imu_rate.q += dq;
ahrs_impl.imu_rate.r += dr; ahrs_dcm.imu_rate.r += dr;
#endif #endif
Matrix_update(dt); Matrix_update(dt);
@@ -177,34 +184,34 @@ void ahrs_propagate(float dt)
compute_ahrs_representations(); compute_ahrs_representations();
} }
void ahrs_update_gps(void) void ahrs_dcm_update_gps(void)
{ {
static float last_gps_speed_3d = 0; static float last_gps_speed_3d = 0;
#if USE_GPS #if USE_GPS
if (gps.fix == GPS_FIX_3D) { if (gps.fix == GPS_FIX_3D) {
ahrs_impl.gps_age = 0; ahrs_dcm.gps_age = 0;
ahrs_impl.gps_speed = gps.speed_3d/100.; ahrs_dcm.gps_speed = gps.speed_3d/100.;
if(gps.gspeed >= 500) { //got a 3d fix and ground speed is more than 5.0 m/s if(gps.gspeed >= 500) { //got a 3d fix and ground speed is more than 5.0 m/s
ahrs_impl.gps_course = ((float)gps.course)/1.e7; ahrs_dcm.gps_course = ((float)gps.course)/1.e7;
ahrs_impl.gps_course_valid = TRUE; ahrs_dcm.gps_course_valid = TRUE;
} else { } else {
ahrs_impl.gps_course_valid = FALSE; ahrs_dcm.gps_course_valid = FALSE;
} }
} else { } else {
ahrs_impl.gps_age = 100; ahrs_dcm.gps_age = 100;
} }
#endif #endif
ahrs_impl.gps_acceleration += ( ((ahrs_impl.gps_speed - last_gps_speed_3d)*4.0f) - ahrs_impl.gps_acceleration) / 5.0f; ahrs_dcm.gps_acceleration += ( ((ahrs_dcm.gps_speed - last_gps_speed_3d)*4.0f) - ahrs_dcm.gps_acceleration) / 5.0f;
last_gps_speed_3d = ahrs_impl.gps_speed; last_gps_speed_3d = ahrs_dcm.gps_speed;
} }
void ahrs_update_accel(float dt __attribute__((unused))) void ahrs_dcm_update_accel(struct Int32Vect3* accel, float dt __attribute__((unused)))
{ {
ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); ACCELS_FLOAT_OF_BFP(accel_float, *accel);
// DCM filter uses g-force as positive // DCM filter uses g-force as positive
// accelerometer measures [0 0 -g] in a static case // accelerometer measures [0 0 -g] in a static case
@@ -213,45 +220,47 @@ void ahrs_update_accel(float dt __attribute__((unused)))
accel_float.z = -accel_float.z; accel_float.z = -accel_float.z;
ahrs_impl.gps_age ++; ahrs_dcm.gps_age ++;
if (ahrs_impl.gps_age < 50) { //Remove centrifugal acceleration and longitudinal acceleration if (ahrs_dcm.gps_age < 50) { //Remove centrifugal acceleration and longitudinal acceleration
#if USE_AHRS_GPS_ACCELERATIONS #if USE_AHRS_GPS_ACCELERATIONS
PRINT_CONFIG_MSG("AHRS_FLOAT_DCM uses GPS acceleration.") PRINT_CONFIG_MSG("AHRS_FLOAT_DCM uses GPS acceleration.")
accel_float.x += ahrs_impl.gps_acceleration; // Longitudinal acceleration accel_float.x += ahrs_dcm.gps_acceleration; // Longitudinal acceleration
#endif #endif
accel_float.y += ahrs_impl.gps_speed * Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ accel_float.y += ahrs_dcm.gps_speed * Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ
accel_float.z -= ahrs_impl.gps_speed * Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY accel_float.z -= ahrs_dcm.gps_speed * Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY
} }
else else
{ {
ahrs_impl.gps_speed = 0; ahrs_dcm.gps_speed = 0;
ahrs_impl.gps_acceleration = 0; ahrs_dcm.gps_acceleration = 0;
ahrs_impl.gps_age = 100; ahrs_dcm.gps_age = 100;
} }
Drift_correction(); Drift_correction();
} }
void ahrs_update_mag(float dt __attribute__((unused))) void ahrs_dcm_update_mag(struct Int32Vect3* mag, float dt __attribute__((unused)))
{ {
#if USE_MAGNETOMETER #if USE_MAGNETOMETER
#warning MAGNETOMETER FEEDBACK NOT TESTED YET #warning MAGNETOMETER FEEDBACK NOT TESTED YET
MAG_FLOAT_OF_BFP(mag_float, *mag);
float cos_roll; float cos_roll;
float sin_roll; float sin_roll;
float cos_pitch; float cos_pitch;
float sin_pitch; float sin_pitch;
cos_roll = cosf(ahrs_impl.ltp_to_imu_euler.phi); cos_roll = cosf(ahrs_dcm.ltp_to_imu_euler.phi);
sin_roll = sinf(ahrs_impl.ltp_to_imu_euler.phi); sin_roll = sinf(ahrs_dcm.ltp_to_imu_euler.phi);
cos_pitch = cosf(ahrs_impl.ltp_to_imu_euler.theta); cos_pitch = cosf(ahrs_dcm.ltp_to_imu_euler.theta);
sin_pitch = sinf(ahrs_impl.ltp_to_imu_euler.theta); sin_pitch = sinf(ahrs_dcm.ltp_to_imu_euler.theta);
// Pitch&Roll Compensation: // Pitch&Roll Compensation:
MAG_Heading_X = imu.mag.x*cos_pitch+imu.mag.y*sin_roll*sin_pitch+imu.mag.z*cos_roll*sin_pitch; MAG_Heading_X = mag->x*cos_pitch+mag->y*sin_roll*sin_pitch+mag->z*cos_roll*sin_pitch;
MAG_Heading_Y = imu.mag.y*cos_roll-imu.mag.z*sin_roll; MAG_Heading_Y = mag->y*cos_roll-mag->z*sin_roll;
/* /*
* *
@@ -284,7 +293,11 @@ void ahrs_update_mag(float dt __attribute__((unused)))
#endif #endif
// Magnetic Heading // Magnetic Heading
// MAG_Heading = atan2(imu.mag.y, -imu.mag.x); // MAG_Heading = atan2(mag->y, -mag->x);
#else // !USE_MAGNETOMETER
// get rid of unused param warning...
mag = mag;
#endif #endif
} }
@@ -363,13 +376,13 @@ void Normalize(void)
// Reset on trouble // Reset on trouble
if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down! if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down!
set_dcm_matrix_from_rmat(orientationGetRMat_f(&imu.body_to_imu)); set_dcm_matrix_from_rmat(orientationGetRMat_f(ahrs_dcm.body_to_imu));
problem = FALSE; problem = FALSE;
} }
} }
void Drift_correction(void) void Drift_correction()
{ {
//Compensation the Roll, Pitch and Yaw drift. //Compensation the Roll, Pitch and Yaw drift.
static float Scaled_Omega_P[3]; static float Scaled_Omega_P[3];
@@ -426,8 +439,8 @@ void Drift_correction(void)
#else // Use GPS Ground course to correct yaw gyro drift #else // Use GPS Ground course to correct yaw gyro drift
if (ahrs_impl.gps_course_valid) { if (ahrs_dcm.gps_course_valid) {
float course = ahrs_impl.gps_course - M_PI; //This is the runaway direction of you "plane" in rad float course = ahrs_dcm.gps_course - M_PI; //This is the runaway direction of you "plane" in rad
float COGX = cosf(course); //Course overground X axis float COGX = cosf(course); //Course overground X axis
float COGY = sinf(course); //Course overground Y axis float COGY = sinf(course); //Course overground Y axis
@@ -444,8 +457,8 @@ void Drift_correction(void)
PRINT_CONFIG_MSG("AHRS_FLOAT_DCM uses magnetometer prior to takeoff and GPS during flight") PRINT_CONFIG_MSG("AHRS_FLOAT_DCM uses magnetometer prior to takeoff and GPS during flight")
else if (launch == FALSE) else if (launch == FALSE)
{ {
float COGX = imu.mag.x; // Non-Tilt-Compensated (for filter stability reasons) float COGX = mag_float.x; // Non-Tilt-Compensated (for filter stability reasons)
float COGY = imu.mag.y; // Non-Tilt-Compensated (for filter stability reasons) float COGY = mag_float.y; // Non-Tilt-Compensated (for filter stability reasons)
errorCourse=(DCM_Matrix[0][0]*COGY) - (DCM_Matrix[1][0]*COGX); //Calculating YAW error 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(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
@@ -469,7 +482,7 @@ PRINT_CONFIG_MSG("AHRS_FLOAT_DCM uses magnetometer prior to takeoff and GPS duri
void Matrix_update(float dt) void Matrix_update(float dt)
{ {
Vector_Add(&Omega[0], &ahrs_impl.imu_rate.p, &Omega_I[0]); //adding proportional term Vector_Add(&Omega[0], &ahrs_dcm.imu_rate.p, &Omega_I[0]); //adding proportional term
Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
#if OUTPUTMODE==1 // With corrected data (drift correction) #if OUTPUTMODE==1 // With corrected data (drift correction)
@@ -484,13 +497,13 @@ void Matrix_update(float dt)
Update_Matrix[2][2]=0; Update_Matrix[2][2]=0;
#else // Uncorrected data (no drift correction) #else // Uncorrected data (no drift correction)
Update_Matrix[0][0]=0; Update_Matrix[0][0]=0;
Update_Matrix[0][1]=-dt*ahrs_impl.imu_rate.r;//-z Update_Matrix[0][1]=-dt*ahrs_dcm.imu_rate.r;//-z
Update_Matrix[0][2]=dt*ahrs_impl.imu_rate.q;//y Update_Matrix[0][2]=dt*ahrs_dcm.imu_rate.q;//y
Update_Matrix[1][0]=dt*ahrs_impl.imu_rate.r;//z Update_Matrix[1][0]=dt*ahrs_dcm.imu_rate.r;//z
Update_Matrix[1][1]=0; Update_Matrix[1][1]=0;
Update_Matrix[1][2]=-dt*ahrs_impl.imu_rate.p; Update_Matrix[1][2]=-dt*ahrs_dcm.imu_rate.p;
Update_Matrix[2][0]=-dt*ahrs_impl.imu_rate.q; Update_Matrix[2][0]=-dt*ahrs_dcm.imu_rate.q;
Update_Matrix[2][1]=dt*ahrs_impl.imu_rate.p; Update_Matrix[2][1]=dt*ahrs_dcm.imu_rate.p;
Update_Matrix[2][2]=0; Update_Matrix[2][2]=0;
#endif #endif
@@ -510,14 +523,14 @@ void Matrix_update(float dt)
*/ */
static inline void set_body_orientation_and_rates(void) { static inline void set_body_orientation_and_rates(void) {
struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imu.body_to_imu); struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(ahrs_dcm.body_to_imu);
struct FloatRates body_rate; struct FloatRates body_rate;
float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_impl.imu_rate); float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_dcm.imu_rate);
stateSetBodyRates_f(&body_rate); stateSetBodyRates_f(&body_rate);
struct FloatRMat ltp_to_imu_rmat, ltp_to_body_rmat; struct FloatRMat ltp_to_imu_rmat, ltp_to_body_rmat;
float_rmat_of_eulers(&ltp_to_imu_rmat, &ahrs_impl.ltp_to_imu_euler); float_rmat_of_eulers(&ltp_to_imu_rmat, &ahrs_dcm.ltp_to_imu_euler);
float_rmat_comp_inv(&ltp_to_body_rmat, &ltp_to_imu_rmat, body_to_imu_rmat); float_rmat_comp_inv(&ltp_to_body_rmat, &ltp_to_imu_rmat, body_to_imu_rmat);
stateSetNedToBodyRMat_f(&ltp_to_body_rmat); stateSetNedToBodyRMat_f(&ltp_to_body_rmat);
@@ -526,14 +539,14 @@ static inline void set_body_orientation_and_rates(void) {
static inline void compute_ahrs_representations(void) { static inline void compute_ahrs_representations(void) {
#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes) #if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
ahrs_impl.ltp_to_imu_euler.phi = atan2(accel_float.y,accel_float.z); // atan2(acc_y,acc_z) ahrs_dcm.ltp_to_imu_euler.phi = atan2(accel_float.y,accel_float.z); // atan2(acc_y,acc_z)
ahrs_impl.ltp_to_imu_euler.theta = -asin((accel_float.x)/GRAVITY); // asin(acc_x) ahrs_dcm.ltp_to_imu_euler.theta = -asin((accel_float.x)/GRAVITY); // asin(acc_x)
ahrs_impl.ltp_to_imu_euler.psi = 0; ahrs_dcm.ltp_to_imu_euler.psi = 0;
#else #else
ahrs_impl.ltp_to_imu_euler.phi = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]); ahrs_dcm.ltp_to_imu_euler.phi = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
ahrs_impl.ltp_to_imu_euler.theta = -asin(DCM_Matrix[2][0]); ahrs_dcm.ltp_to_imu_euler.theta = -asin(DCM_Matrix[2][0]);
ahrs_impl.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]); ahrs_dcm.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
ahrs_impl.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ ahrs_dcm.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ
#endif #endif
set_body_orientation_and_rates(); set_body_orientation_and_rates();
sw/airborne/subsystems/ahrs/ahrs_float_dcm.h
+14 -2
View File
@@ -39,9 +39,12 @@ struct AhrsFloatDCM {
float gps_course; float gps_course;
bool_t gps_course_valid; bool_t gps_course_valid;
uint8_t gps_age; uint8_t gps_age;
};
extern struct AhrsFloatDCM ahrs_impl;
struct OrientationReps* body_to_imu;
};
extern struct AhrsFloatDCM ahrs_dcm;
#define DefaultAhrsImpl ahrs_dcm
// DCM Parameters // DCM Parameters
@@ -69,4 +72,13 @@ extern int renorm_blowup_count;
extern float imu_health; extern float imu_health;
#endif #endif
extern void ahrs_dcm_register(void);
extern void ahrs_dcm_init(struct OrientationReps* body_to_imu);
extern bool_t ahrs_dcm_align(struct Int32Rates* lp_gyro, struct Int32Vect3* lp_accel,
struct Int32Vect3* lp_mag);
extern void ahrs_dcm_propagate(struct Int32Rates* gyro, float dt);
extern void ahrs_dcm_update_accel(struct Int32Vect3* accel, float dt);
extern void ahrs_dcm_update_mag(struct Int32Vect3* mag, float dt);
extern void ahrs_dcm_update_gps(void);
#endif // AHRS_FLOAT_DCM_H #endif // AHRS_FLOAT_DCM_H