31#include "generated/airframe.h"
42#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
46#ifdef AHRS_FLOATING_HEADING
50#if USE_MAGNETOMETER && AHRS_USE_GPS_HEADING
51#warning "Using both magnetometer and GPS course to update heading. Probably better to configure USE_MAGNETOMETER=0 if you want to use GPS course."
54#if !USE_MAGNETOMETER && !AHRS_USE_GPS_HEADING &&!AHRS_FLOATING_HEADING
55#warning "Please use either USE_MAGNETOMETER, AHRS_USE_GPS_HEADING or accept AHRS_FLOATING_HEADING."
58#if AHRS_USE_GPS_HEADING && !USE_GPS
59#error "AHRS_USE_GPS_HEADING needs USE_GPS to be TRUE"
65#ifndef AHRS_ACCEL_OMEGA
66#define AHRS_ACCEL_OMEGA 0.063
68#ifndef AHRS_ACCEL_ZETA
69#define AHRS_ACCEL_ZETA 0.9
76#define AHRS_MAG_OMEGA 0.04
79#define AHRS_MAG_ZETA 0.9
87#ifndef AHRS_GRAVITY_HEURISTIC_FACTOR
88#define AHRS_GRAVITY_HEURISTIC_FACTOR 30
92#ifndef AHRS_BIAS_UPDATE_HEADING_THRESHOLD
93#define AHRS_BIAS_UPDATE_HEADING_THRESHOLD 5.0
99#ifndef AHRS_HEADING_UPDATE_GPS_MIN_SPEED
100#define AHRS_HEADING_UPDATE_GPS_MIN_SPEED 5.0
104#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
105#ifndef AHRS_PROPAGATE_LOW_PASS_RATES_MUL
106#define AHRS_PROPAGATE_LOW_PASS_RATES_MUL 2
109#ifndef AHRS_PROPAGATE_LOW_PASS_RATES_DIV
110#define AHRS_PROPAGATE_LOW_PASS_RATES_DIV 3
125 ahrs_icq.ltp_vel_norm_valid =
false;
147#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
199#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
231 ahrs_icq.accel_inv_kp = 4096 * 9.81 /
272#define COMPUTATION_FRAC 16
273#define ACC_FROM_CROSS_FRAC INT32_RATE_FRAC + INT32_SPEED_FRAC - INT32_ACCEL_FRAC - COMPUTATION_FRAC
276#if AHRS_GPS_SPEED_IN_NEGATIVE_Z_DIRECTION
291 VECT3_DIFF(pseudo_gravity_measurement, *accel, acc_c_body);
293 VECT3_COPY(pseudo_gravity_measurement, *accel);
301#define FIR_FILTER_SIZE 8
302 static struct Int32Vect3 filtered_gravity_measurement = {0, 0, 0};
304 VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement);
308 if (
ahrs_icq.gravity_heuristic_factor) {
318 ahrs_icq.weight = 1.0 -
ahrs_icq.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10;
343 Bound(inv_rate_scale, 8192, 4194304);
345 ahrs_icq.rate_correction.p -= residual.
x / inv_rate_scale;
346 ahrs_icq.rate_correction.q -= residual.
y / inv_rate_scale;
347 ahrs_icq.rate_correction.r -= residual.
z / inv_rate_scale;
370 Bound(inv_bias_gain, 8, 65536)
371 ahrs_icq.high_rez_bias.p += (residual.
x / inv_bias_gain) << 5;
372 ahrs_icq.high_rez_bias.q += (residual.
y / inv_bias_gain) << 5;
373 ahrs_icq.high_rez_bias.r += (residual.
z / inv_bias_gain) << 5;
387#if AHRS_MAG_UPDATE_ALL_AXES
433 ahrs_icq.rate_correction.p += residual.
x / inv_rate_gain;
434 ahrs_icq.rate_correction.q += residual.
y / inv_rate_gain;
435 ahrs_icq.rate_correction.r += residual.
z / inv_rate_gain;
449 ahrs_icq.high_rez_bias.p -= residual.
x * bias_gain;
450 ahrs_icq.high_rez_bias.q -= residual.
y * bias_gain;
451 ahrs_icq.high_rez_bias.r -= residual.
z * bias_gain;
473 struct Int32Vect2 measured_ltp_2d = {measured_ltp.
x, measured_ltp.
y};
480 (measured_ltp_2d.
x * expected_ltp.
y - measured_ltp_2d.
y * expected_ltp.
x) / (1 << 5)
501 ahrs_icq.rate_correction.p += (residual_body.
x / inv_rate_gain);
502 ahrs_icq.rate_correction.q += (residual_body.
y / inv_rate_gain);
503 ahrs_icq.rate_correction.r += (residual_body.
z / inv_rate_gain);
517 ahrs_icq.high_rez_bias.p -= (residual_body.
x * bias_gain);
518 ahrs_icq.high_rez_bias.q -= (residual_body.
y * bias_gain);
519 ahrs_icq.high_rez_bias.r -= (residual_body.
z * bias_gain);
527#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN && USE_GPS
532 ahrs_icq.ltp_vel_norm_valid =
false;
536#if AHRS_USE_GPS_HEADING && USE_GPS
540 static const uint32_t max_cacc = RadOfDeg(10 * 1e7);
542 gps_s->gspeed >= gps_min_speed &&
543 gps_s->cacc <= max_cacc) {
593 ahrs_icq.rate_correction.p += residual_body.
x / 4;
594 ahrs_icq.rate_correction.q += residual_body.
y / 4;
595 ahrs_icq.rate_correction.r += residual_body.
z / 4;
604 int32_t sin_max_angle_deviation;
606 if (ABS(residual_ltp.
z) < sin_max_angle_deviation) {
#define AHRS_GRAVITY_HEURISTIC_FACTOR
by default use the gravity heuristic to reduce gain
bool ahrs_icq_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
void ahrs_icq_update_accel(struct Int32Vect3 *accel, float dt)
#define ACC_FROM_CROSS_FRAC
struct AhrsIntCmplQuat ahrs_icq
Default Rate filter Low pass.
#define AHRS_BIAS_UPDATE_HEADING_THRESHOLD
don't update gyro bias if heading deviation is above this threshold in degrees
void ahrs_icq_update_gps(struct GpsState *gps_s)
void ahrs_icq_update_heading(int32_t heading)
Update yaw based on a heading measurement.
static void ahrs_icq_update_mag_2d(struct Int32Vect3 *mag, float dt)
void ahrs_icq_realign_heading(int32_t heading)
Hard reset yaw to a heading.
void ahrs_icq_update_mag(struct Int32Vect3 *mag, float dt)
void ahrs_icq_set_accel_gains(void)
update pre-computed inv_kp and inv_ki gains from acc_omega and acc_zeta
void ahrs_icq_set_mag_gains(void)
update pre-computed kp and ki gains from mag_omega and mag_zeta
static void UNUSED ahrs_icq_update_mag_full(struct Int32Vect3 *mag, float dt)
void ahrs_icq_propagate(struct Int32Rates *gyro, float dt)
#define AHRS_HEADING_UPDATE_GPS_MIN_SPEED
Minimum speed in m/s for heading update via GPS.
Quaternion complementary filter (fixed-point).
Ahrs implementation specifc values.
Utility functions for fixed point AHRS implementations.
static void ahrs_int_get_quat_from_accel(struct Int32Quat *q, struct Int32Vect3 *accel)
static void ahrs_int_get_quat_from_accel_mag(struct Int32Quat *q, struct Int32Vect3 *accel, struct Int32Vect3 *mag)
Device independent GPS code (interface).
#define GPS_FIX_3D
3D GPS fix
data structure for GPS information
#define FLOAT_VECT3_NORM(_v)
#define VECT3_SDIV(_vo, _vi, _s)
#define VECT3_RATES_CROSS_VECT3(_vo, _r1, _v2)
#define RATES_COPY(_a, _b)
#define VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
#define RATES_SMUL(_ro, _ri, _s)
#define VECT3_SMUL(_vo, _vi, _s)
#define VECT3_ASSIGN(_a, _x, _y, _z)
#define QUAT_COPY(_qo, _qi)
#define RMAT_ELMT(_rm, _row, _col)
#define RATES_ADD(_a, _b)
#define VECT3_COPY(_a, _b)
#define RATES_DIFF(_c, _a, _b)
#define VECT3_DIFF(_c, _a, _b)
#define VECT3_ADD(_a, _b)
#define RATES_SDIV(_ro, _ri, _s)
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
static void int32_quat_normalize(struct Int32Quat *q)
normalize a quaternion inplace
#define INT_RATES_ZERO(_e)
void int32_quat_comp_norm_shortest(struct Int32Quat *a2c, struct Int32Quat *a2b, struct Int32Quat *b2c)
Composition (multiplication) of two quaternions with normalization.
void int32_rmat_of_quat(struct Int32RMat *rm, struct Int32Quat *q)
Convert unit quaternion to rotation matrix.
#define INT_RATES_LSHIFT(_o, _i, _r)
void int32_quat_inv_comp_norm_shortest(struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c)
Composition (multiplication) of two quaternions with normalization.
static void int32_vect2_normalize(struct Int32Vect2 *v, uint8_t frac)
normalize 2D vector inplace
void int32_rmat_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_a2b, struct Int32Vect3 *va)
rotate 3D vector by rotation matrix.
static void int32_quat_identity(struct Int32Quat *q)
initialises a quaternion to identity
#define INT_RATES_RSHIFT(_o, _i, _r)
#define MAG_BFP_OF_REAL(_af)
#define INT32_VECT3_RSHIFT(_o, _i, _r)
void int32_quat_integrate_fi(struct Int32Quat *q, struct Int64Quat *hr, struct Int32Rates *omega, int freq)
in place quaternion first order integration with constant rotational velocity.
void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a, struct Int32Vect3 *va)
rotate 3D vector by transposed rotation matrix.
#define TRIG_BFP_OF_REAL(_tf)
#define SPEED_BFP_OF_REAL(_af)
#define INT32_ANGLE_NORMALIZE(_a)
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
Paparazzi fixed point algebra.
Paparazzi fixed point trig functions.
#define PPRZ_ITRIG_SIN(_s, _a)
#define PPRZ_ITRIG_COS(_c, _a)
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.