45#if FLOAT_DCM_SEND_DEBUG
48#include "pprzlink/messages.h"
67float DCM_Matrix[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
83#if PERFORMANCE_REPORTING == 1
84int renorm_sqrt_count = 0;
85int renorm_blowup_count = 0;
92 for (
int i = 0; i < 3; i++) {
93 for (
int j = 0; j < 3; j++) {
121 struct FloatVect3 *lp_mag __attribute__((unused)))
156 float dp = 0, dq = 0, dr = 0;
157 dp +=
ahrs_dcm.body_rate.q * IMU_GYRO_P_Q;
158 dp +=
ahrs_dcm.body_rate.r * IMU_GYRO_P_R;
159 dq +=
ahrs_dcm.body_rate.p * IMU_GYRO_Q_P;
160 dq +=
ahrs_dcm.body_rate.r * IMU_GYRO_Q_R;
161 dr +=
ahrs_dcm.body_rate.p * IMU_GYRO_R_P;
162 dr +=
ahrs_dcm.body_rate.q * IMU_GYRO_R_Q;
178 static float last_gps_speed_3d = 0;
183 ahrs_dcm.gps_speed = gps_s->speed_3d / 100.;
186 ahrs_dcm.gps_course = ((float)gps_s->course) / 1.e7;
196 ahrs_dcm.gps_acceleration += (((
ahrs_dcm.gps_speed - last_gps_speed_3d) * 4.0f) -
ahrs_dcm.gps_acceleration) / 5.0f;
197 last_gps_speed_3d =
ahrs_dcm.gps_speed;
212#if USE_AHRS_GPS_ACCELERATIONS
237 cos_roll = cosf(
ahrs_dcm.ltp_to_body_euler.phi);
238 sin_roll = sinf(
ahrs_dcm.ltp_to_body_euler.phi);
239 cos_pitch = cosf(
ahrs_dcm.ltp_to_body_euler.theta);
240 sin_pitch = sinf(
ahrs_dcm.ltp_to_body_euler.theta);
244 MAG_Heading_X = mag->
x * cos_pitch + mag->
y * sin_roll * sin_pitch + mag->
z * cos_roll * sin_pitch;
267#if FLOAT_DCM_SEND_DEBUG
289 float temporary[3][3];
310 if (renorm < 1.5625f && renorm > 0.64f) {
311 renorm = .5 * (3 - renorm);
312 }
else if (renorm < 100.0f && renorm > 0.01f) {
313 renorm = 1. / sqrtf(renorm);
314#if PERFORMANCE_REPORTING == 1
319#if PERFORMANCE_REPORTING == 1
320 renorm_blowup_count++;
327 if (renorm < 1.5625f && renorm > 0.64f) {
328 renorm = .5 * (3 - renorm);
329 }
else if (renorm < 100.0f && renorm > 0.01f) {
330 renorm = 1. / sqrtf(renorm);
331#if PERFORMANCE_REPORTING == 1
336#if PERFORMANCE_REPORTING == 1
337 renorm_blowup_count++;
344 if (renorm < 1.5625f && renorm > 0.64f) {
345 renorm = .5 * (3 - renorm);
346 }
else if (renorm < 100.0f && renorm > 0.01f) {
347 renorm = 1. / sqrtf(renorm);
348#if PERFORMANCE_REPORTING == 1
353#if PERFORMANCE_REPORTING == 1
354 renorm_blowup_count++;
370#ifndef ACCEL_WEIGHT_FILTER
371#define ACCEL_WEIGHT_FILTER 8
377#ifndef ACCEL_WEIGHT_BAND
378#define ACCEL_WEIGHT_BAND 1.f
384 static float Scaled_Omega_P[3];
385 static float Scaled_Omega_I[3];
386 static float Accel_filtered = 0.f;
387 float Accel_magnitude;
389 float Integrator_magnitude;
392 float errorRollPitch[3];
400 Accel_magnitude = Accel_magnitude /
GRAVITY;
401#if ACCEL_WEIGHT_FILTER
404 Accel_filtered = Accel_magnitude;
408 Accel_weight = Clip(1.f - (2.f / Max(0.1f,
ACCEL_WEIGHT_BAND)) * fabsf(1.f - Accel_filtered), 0.f, 1.f);
411#if PERFORMANCE_REPORTING == 1
414 float tempfloat = ((Accel_weight - 0.5) * 256.0f);
415 imu_health += tempfloat;
416 Bound(imu_health, 129, 65405);
448 float COGX = cosf(
course);
449 float COGY = sinf(
course);
461#if USE_MAGNETOMETER_ONGROUND
462 PRINT_CONFIG_MSG(
"AHRS_FLOAT_DCM uses magnetometer prior to takeoff and GPS during flight")
480 if (Integrator_magnitude > RadOfDeg(300)) {
517 for (
int x = 0; x < 3; x++) {
518 for (
int y = 0; y < 3; y++) {
void ahrs_dcm_propagate(struct FloatRates *gyro, float dt)
void Drift_correction(void)
void ahrs_dcm_update_gps(struct GpsState *gps_s UNUSED)
void ahrs_dcm_update_mag(struct FloatVect3 *mag)
static void compute_ahrs_representations(void)
float Temporary_Matrix[3][3]
void ahrs_dcm_update_accel(struct FloatVect3 *accel)
void Matrix_update(float dt)
static void set_dcm_matrix_from_rmat(struct FloatRMat *rmat)
#define ACCEL_WEIGHT_FILTER
struct AhrsFloatDCM ahrs_dcm
bool ahrs_dcm_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
#define ACCEL_WEIGHT_BAND
float Update_Matrix[3][3]
struct FloatVect3 accel_float
struct FloatVect3 mag_float
Attitude estimation for fixedwings based on the DCM.
#define AHRS_FLOAT_MIN_SPEED_GPS_COURSE
Algebra helper functions for DCM.
static void Vector_Add(float vectorOut[3], float vectorIn1[3], float vectorIn2[3])
static void Matrix_Multiply(float a[3][3], float b[3][3], float mat[3][3])
static void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2)
static float Vector_Dot_Product(float vector1[3], float vector2[3])
static void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3])
Utility functions for floating point AHRS implementations.
static void ahrs_float_get_quat_from_accel_mag(struct FloatQuat *q, struct FloatVect3 *accel, struct FloatVect3 *mag)
static void ahrs_float_get_quat_from_accel(struct FloatQuat *q, struct FloatVect3 *accel)
Compute a quaternion representing roll and pitch from an accelerometer measurement.
struct pprz_autopilot autopilot
Global autopilot structure.
Core autopilot interface common to all firmwares.
Common code for AP and FBW telemetry.
Device independent GPS code (interface).
#define GPS_FIX_3D
3D GPS fix
data structure for GPS information
#define FLOAT_EULERS_ZERO(_e)
static void float_rmat_identity(struct FloatRMat *rm)
initialises a rotation matrix to identity
void float_eulers_of_quat(struct FloatEulers *e, const struct FloatQuat *q)
euler rotation 'ZYX'
void float_rmat_of_quat(struct FloatRMat *rm, const struct FloatQuat *q)
#define FLOAT_RATES_ZERO(_r)
#define RMAT_ELMT(_rm, _row, _col)
#define RATES_DIFF(_c, _a, _b)
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
arch independent LED (Light Emitting Diodes) API
Paparazzi floating point algebra.
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.