30#include "generated/airframe.h"
34#ifndef AHRS_MADGWICK_BETA
35#define AHRS_MADGWICK_BETA 0.1f
116 const float s0 = _4q0 * q2q2 + _2q2 * a.
x + _4q0 * q1q1 - _2q1 * a.
y;
117 const float s1 = _4q1 * q3q3 - _2q3 * a.
x + 4.0f * q0q0 * q1 - _2q0 * a.
y - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * a.
z;
118 const float s2 = 4.0f * q0q0 * q2 + _2q0 * a.
x + _4q2 * q3q3 - _2q3 * a.
y - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * a.
z;
119 const float s3 = 4.0f * q1q1 * q3 - _2q1 * a.
x + 4.0f * q2q2 * q3 - _2q2 * a.
y;
120 const float beta_inv_grad_norm =
AHRS_MADGWICK_BETA / sqrtf(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
123 qdot.
qi -= s0 * beta_inv_grad_norm;
124 qdot.
qx -= s1 * beta_inv_grad_norm;
125 qdot.
qy -= s2 * beta_inv_grad_norm;
126 qdot.
qz -= s3 * beta_inv_grad_norm;
Utility functions for floating point AHRS implementations.
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 AhrsMadgwick ahrs_madgwick
void ahrs_madgwick_propagate(struct FloatRates *gyro, float dt)
void ahrs_madgwick_update_accel(struct FloatVect3 *accel)
void ahrs_madgwick_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel)
static void init_state(void)
void ahrs_madgwick_init(void)
#define AHRS_MADGWICK_BETA
AHRS using Madgwick implementation.
Madgwick filter structure.
static void float_quat_normalize(struct FloatQuat *q)
static float float_vect3_norm(const struct FloatVect3 *v)
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
#define FLOAT_VECT3_ZERO(_v)
#define FLOAT_RATES_ZERO(_r)
void float_quat_derivative(struct FloatQuat *qd, const struct FloatRates *r, const struct FloatQuat *q)
Quaternion derivative from rotational velocity.
#define VECT3_SDIV(_vo, _vi, _s)
#define RATES_DIFF(_c, _a, _b)
Paparazzi floating point algebra.