31 #include "generated/airframe.h"
42 #ifdef AHRS_PROPAGATE_LOW_PASS_RATES
46 #if USE_MAGNETOMETER && AHRS_USE_GPS_HEADING
47 #warning "Using both magnetometer and GPS course to update heading. Probably better to configure USE_MAGNETOMETER=0 if you want to use GPS course."
50 #if !USE_MAGNETOMETER && !AHRS_USE_GPS_HEADING
51 #warning "Please use either USE_MAGNETOMETER or AHRS_USE_GPS_HEADING."
54 #if AHRS_USE_GPS_HEADING && !USE_GPS
55 #error "AHRS_USE_GPS_HEADING needs USE_GPS to be TRUE"
61 #ifndef AHRS_ACCEL_OMEGA
62 #define AHRS_ACCEL_OMEGA 0.063
64 #ifndef AHRS_ACCEL_ZETA
65 #define AHRS_ACCEL_ZETA 0.9
71 #ifndef AHRS_MAG_OMEGA
72 #define AHRS_MAG_OMEGA 0.04
75 #define AHRS_MAG_ZETA 0.9
83 #ifndef AHRS_GRAVITY_HEURISTIC_FACTOR
84 #define AHRS_GRAVITY_HEURISTIC_FACTOR 30
88 #ifndef AHRS_BIAS_UPDATE_HEADING_THRESHOLD
89 #define AHRS_BIAS_UPDATE_HEADING_THRESHOLD 5.0
95 #ifndef AHRS_HEADING_UPDATE_GPS_MIN_SPEED
96 #define AHRS_HEADING_UPDATE_GPS_MIN_SPEED 5.0
100 #ifdef AHRS_PROPAGATE_LOW_PASS_RATES
101 #ifndef AHRS_PROPAGATE_LOW_PASS_RATES_MUL
102 #define AHRS_PROPAGATE_LOW_PASS_RATES_MUL 2
105 #ifndef AHRS_PROPAGATE_LOW_PASS_RATES_DIV
106 #define AHRS_PROPAGATE_LOW_PASS_RATES_DIV 3
144 #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
196 #ifdef AHRS_PROPAGATE_LOW_PASS_RATES
269 #define COMPUTATION_FRAC 16
270 #define ACC_FROM_CROSS_FRAC INT32_RATE_FRAC + INT32_SPEED_FRAC - INT32_ACCEL_FRAC - COMPUTATION_FRAC
273 #if AHRS_GPS_SPEED_IN_NEGATIVE_Z_DIRECTION
295 VECT3_DIFF(pseudo_gravity_measurement, *accel, acc_c_imu);
297 VECT3_COPY(pseudo_gravity_measurement, *accel);
305 #define FIR_FILTER_SIZE 8
306 static struct Int32Vect3 filtered_gravity_measurement = {0, 0, 0};
308 VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement);
347 Bound(inv_rate_scale, 8192, 4194304);
374 Bound(inv_bias_gain, 8, 65536)
391 #if AHRS_MAG_UPDATE_ALL_AXES
477 struct Int32Vect2 measured_ltp_2d = {measured_ltp.
x, measured_ltp.
y};
484 (measured_ltp_2d.
x * expected_ltp.
y - measured_ltp_2d.
y * expected_ltp.
x) / (1 << 5)
531 #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN && USE_GPS
540 #if AHRS_USE_GPS_HEADING && USE_GPS
544 static const uint32_t max_cacc = RadOfDeg(10 * 1e7);
546 gps_s->gspeed >= gps_min_speed &&
547 gps_s->cacc <= max_cacc) {
613 int32_t sin_max_angle_deviation;
615 if (ABS(residual_ltp.
z) < sin_max_angle_deviation) {
Quaternion complementary filter (fixed-point).
#define VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
#define RATES_SDIV(_ro, _ri, _s)
static void ahrs_int_get_quat_from_accel_mag(struct Int32Quat *q, struct Int32Vect3 *accel, struct Int32Vect3 *mag)
struct AhrsIntCmplQuat ahrs_icq
Default Rate filter Low pass.
void ahrs_icq_update_mag(struct Int32Vect3 *mag, float dt)
static void orientationSetQuat_f(struct OrientationReps *orientation, struct FloatQuat *quat)
Set vehicle body attitude from quaternion (float).
#define VECT3_ADD(_a, _b)
float accel_omega
filter cut-off frequency for correcting the attitude from accels.
static void int32_quat_normalize(struct Int32Quat *q)
normalize a quaternion inplace
static struct Int32RMat * orientationGetRMat_i(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (int).
struct OrientationReps body_to_imu
static void int32_vect2_normalize(struct Int32Vect2 *v, uint8_t frac)
normalize 2D vector inplace
#define INT_RATES_LSHIFT(_o, _i, _r)
void int32_quat_comp(struct Int32Quat *a2c, struct Int32Quat *a2b, struct Int32Quat *b2c)
Composition (multiplication) of two quaternions.
#define RATES_ADD(_a, _b)
#define ACC_FROM_CROSS_FRAC
#define INT_RATES_RSHIFT(_o, _i, _r)
struct Int32Rates rate_correction
uint16_t accel_cnt
number of propagations since last accel update
void ahrs_icq_set_body_to_imu(struct OrientationReps *body_to_imu)
#define INT_RATES_ZERO(_e)
#define VECT3_COPY(_a, _b)
#define AHRS_GRAVITY_HEURISTIC_FACTOR
by default use the gravity heuristic to reduce gain
#define VECT3_DIFF(_c, _a, _b)
void ahrs_icq_update_gps(struct GpsState *gps_s)
#define VECT3_ASSIGN(_a, _x, _y, _z)
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.
Paparazzi fixed point trig functions.
#define INT32_VECT3_RSHIFT(_o, _i, _r)
#define GPS_FIX_3D
3D GPS fix
#define RATES_DIFF(_c, _a, _b)
static void UNUSED ahrs_icq_update_mag_full(struct Int32Vect3 *mag, float dt)
uint8_t gravity_heuristic_factor
sets how strongly the gravity heuristic reduces accel correction.
#define FLOAT_VECT3_NORM(_v)
void ahrs_icq_update_accel(struct Int32Vect3 *accel, float dt)
uint16_t mag_cnt
number of propagations since last mag update
void ahrs_icq_propagate(struct Int32Rates *gyro, float dt)
void ahrs_icq_set_body_to_imu_quat(struct FloatQuat *q_b2i)
#define QUAT_COPY(_qo, _qi)
int32_t r
in rad/s with INT32_RATE_FRAC
#define PPRZ_ITRIG_SIN(_s, _a)
#define AHRS_BIAS_UPDATE_HEADING_THRESHOLD
don't update gyro bias if heading deviation is above this threshold in degrees
void int32_rmat_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_a2b, struct Int32Vect3 *va)
rotate 3D vector by rotation matrix.
struct Int32Rates gyro_bias
#define TRIG_BFP_OF_REAL(_tf)
float mag_zeta
filter damping for correcting the gyro bias from magnetometer.
struct Int32Quat ltp_to_imu_quat
#define MAG_BFP_OF_REAL(_af)
void int32_quat_inv_comp_norm_shortest(struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c)
Composition (multiplication) of two quaternions with normalization.
data structure for GPS information
void ahrs_icq_update_heading(int32_t heading)
Update yaw based on a heading measurement.
void int32_quat_comp_norm_shortest(struct Int32Quat *a2c, struct Int32Quat *a2b, struct Int32Quat *b2c)
Composition (multiplication) of two quaternions with normalization.
Device independent GPS code (interface)
float accel_zeta
filter damping for correcting the gyro-bias from accels.
#define INT32_ANGLE_NORMALIZE(_a)
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
#define VECT3_SDIV(_vo, _vi, _s)
static void ahrs_icq_update_mag_2d(struct Int32Vect3 *mag, float dt)
static void int32_quat_identity(struct Int32Quat *q)
initialises a quaternion to identity
Ahrs implementation specifc values.
void int32_quat_comp_inv(struct Int32Quat *a2b, struct Int32Quat *a2c, struct Int32Quat *b2c)
Composition (multiplication) of two quaternions.
float mag_omega
filter cut-off frequency for correcting the attitude (heading) from magnetometer. ...
void ahrs_icq_set_accel_gains(void)
update pre-computed inv_kp and inv_ki gains from acc_omega and acc_zeta
static void ahrs_int_get_quat_from_accel(struct Int32Quat *q, struct Int32Vect3 *accel)
#define RATES_SMUL(_ro, _ri, _s)
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Utility functions for fixed point AHRS implementations.
struct Int64Rates high_rez_bias
void ahrs_icq_realign_heading(int32_t heading)
Hard reset yaw to a heading.
#define VECT3_SMUL(_vo, _vi, _s)
bool correct_gravity
enable gravity vector correction by removing centrifugal acceleration
static struct Int32Quat * orientationGetQuat_i(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (int).
#define RMAT_ELMT(_rm, _row, _col)
struct Int64Quat high_rez_quat
struct Int32Rates imu_rate
void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a, struct Int32Vect3 *va)
rotate 3D vector by transposed rotation matrix.
bool ahrs_icq_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
int32_t p
in rad/s with INT32_RATE_FRAC
static struct OrientationReps body_to_imu
#define SPEED_BFP_OF_REAL(_af)
#define RATES_COPY(_a, _b)
void int32_rmat_transp_ratemult(struct Int32Rates *rb, struct Int32RMat *m_b2a, struct Int32Rates *ra)
rotate anglular rates by transposed rotation matrix.
#define AHRS_HEADING_UPDATE_GPS_MIN_SPEED
Minimum speed in m/s for heading update via GPS.
int32_t q
in rad/s with INT32_RATE_FRAC
void ahrs_icq_set_mag_gains(void)
update pre-computed kp and ki gains from mag_omega and mag_zeta
void int32_rmat_of_quat(struct Int32RMat *rm, struct Int32Quat *q)
Convert unit quaternion to rotation matrix.
static struct FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).
#define PPRZ_ITRIG_COS(_c, _a)
enum AhrsICQStatus status
status of the AHRS, AHRS_ICQ_UNINIT or AHRS_ICQ_RUNNING
Paparazzi fixed point algebra.
#define VECT3_RATES_CROSS_VECT3(_vo, _r1, _v2)