|
Paparazzi UAS
v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
|
Go to the documentation of this file.
35 #include "generated/airframe.h"
42 #if AHRS_PROPAGATE_RMAT && AHRS_PROPAGATE_QUAT
43 #error "You can only define either AHRS_PROPAGATE_RMAT or AHRS_PROPAGATE_QUAT, not both!"
45 #if !AHRS_PROPAGATE_RMAT && !AHRS_PROPAGATE_QUAT
46 #error "You have to define either AHRS_PROPAGATE_RMAT or AHRS_PROPAGATE_QUAT"
49 #if USE_MAGNETOMETER && AHRS_USE_GPS_HEADING
50 #warning "Using magnetometer _and_ GPS course to update heading. Probably better to <configure name="USE_MAGNETOMETER" value="0"/> if you want to use GPS course."
56 #ifndef AHRS_ACCEL_OMEGA
57 #define AHRS_ACCEL_OMEGA 0.063
59 #ifndef AHRS_ACCEL_ZETA
60 #define AHRS_ACCEL_ZETA 0.9
63 #ifndef AHRS_MAG_OMEGA
64 #define AHRS_MAG_OMEGA 0.04
67 #define AHRS_MAG_ZETA 0.9
71 #ifndef AHRS_GRAVITY_HEURISTIC_FACTOR
72 #define AHRS_GRAVITY_HEURISTIC_FACTOR 30
104 #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
119 struct FloatVect3 *lp_mag __attribute__((unused)))
152 #ifdef AHRS_PROPAGATE_LOW_PASS_RATES
153 const float alpha = 0.1;
165 #if AHRS_PROPAGATE_RMAT
170 #if AHRS_PROPAGATE_QUAT
207 #if AHRS_GPS_SPEED_IN_NEGATIVE_Z_DIRECTION
227 VECT3_DIFF(pseudo_gravity_measurement, imu_accel, acc_c_imu);
230 VECT3_COPY(pseudo_gravity_measurement, imu_accel);
236 #define FIR_FILTER_SIZE 8
237 static struct FloatVect3 filtered_gravity_measurement = {0., 0., 0.};
239 VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement);
249 const float g_meas_norm =
float_vect3_norm(&filtered_gravity_measurement) / 9.81;
286 #if AHRS_MAG_UPDATE_ALL_AXES
338 struct FloatVect2 measured_ltp_2d = {measured_ltp.
x, measured_ltp.
y};
346 measured_ltp_2d.
x *expected_ltp.
y - measured_ltp_2d.
y * expected_ltp.
x
378 const float cphi = cosf(ltp_to_imu_euler.
phi);
379 const float sphi = sinf(ltp_to_imu_euler.
phi);
380 const float ctheta = cosf(ltp_to_imu_euler.
theta);
381 const float stheta = sinf(ltp_to_imu_euler.
theta);
382 const float mn = ctheta * mag->
x + sphi * stheta * mag->
y + cphi * stheta * mag->
z;
383 const float me = 0. * mag->
x + cphi * mag->
y - sphi * mag->
z;
391 const float mag_rate_update_gain = 2.5;
393 const float mag_bias_update_gain = -2.5e-4;
400 #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN && USE_GPS
409 #if AHRS_USE_GPS_HEADING && USE_GPS
411 if (gps_s->fix >=
GPS_FIX_3D && gps_s->gspeed >= 500 &&
412 gps_s->cacc <= RadOfDeg(10 * 1e7)) {
415 float course = gps_s->course / 1e7;
454 const float heading_rate_update_gain = 2.5;
457 float heading_bias_update_gain;
464 if (fabs(residual_ltp.
z) < sinf(RadOfDeg(5.))) {
465 heading_bias_update_gain = -2.5e-4;
467 heading_bias_update_gain = 0.0;
488 struct FloatQuat q_h = *ltp_to_body_quat;
struct FloatRMat ltp_to_imu_rmat
uint8_t gravity_heuristic_factor
sets how strongly the gravity heuristic reduces accel correction.
void ahrs_fc_propagate(struct FloatRates *gyro, float dt)
void ahrs_fc_update_accel(struct FloatVect3 *accel, float dt)
#define FLOAT_ANGLE_NORMALIZE(_a)
void float_quat_comp(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
void float_eulers_of_rmat(struct FloatEulers *e, struct FloatRMat *rm)
#define VECT3_SMUL(_vo, _vi, _s)
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
void ahrs_fc_realign_heading(float heading)
Hard reset yaw to a heading.
#define FLOAT_RATES_ZERO(_r)
void ahrs_fc_update_mag(struct FloatVect3 *mag, float dt)
#define VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
void ahrs_fc_update_gps(struct GpsState *gps_s)
data structure for GPS information
#define VECT3_SDIV(_vo, _vi, _s)
struct FloatQuat ltp_to_imu_quat
#define VECT3_DIFF(_c, _a, _b)
static void float_vect2_normalize(struct FloatVect2 *v)
normalize 2D vector in place
Paparazzi floating point algebra.
#define VECT3_ADD(_a, _b)
void ahrs_fc_set_body_to_imu_quat(struct FloatQuat *q_b2i)
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
Paparazzi fixed point algebra.
void float_rmat_integrate_fi(struct FloatRMat *rm, struct FloatRates *omega, float dt)
in place first order integration of a rotation matrix
#define VECT2_COPY(_a, _b)
#define VECT3_RATES_CROSS_VECT3(_vo, _r1, _v2)
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
float mag_zeta
filter damping for correcting the gyro bias from magnetometer
void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b, struct FloatVect3 *va)
rotate 3D vector by rotation matrix.
#define FLOAT_RATES_LIN_CMB(_ro, _r1, _s1, _r2, _s2)
float ltp_vel_norm
velocity norm for gravity correction during coordinated turns
#define RATES_SUB(_a, _b)
static float float_vect3_norm(struct FloatVect3 *v)
Device independent GPS code (interface)
static struct FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).
uint16_t accel_cnt
number of propagations since last accel update
static void orientationSetQuat_f(struct OrientationReps *orientation, struct FloatQuat *quat)
Set vehicle body attitude from quaternion (float).
#define RATES_SUM(_c, _a, _b)
static void orientationSetIdentity(struct OrientationReps *orientation)
Set to identity orientation.
struct AhrsFloatCmpl ahrs_fc
struct OrientationReps ltp_to_body
static void float_quat_normalize(struct FloatQuat *q)
void float_rmat_of_quat(struct FloatRMat *rm, struct FloatQuat *q)
#define RMAT_ELMT(_rm, _row, _col)
void float_quat_of_rmat(struct FloatQuat *q, struct FloatRMat *rm)
Quaternion from rotation matrix.
float float_rmat_reorthogonalize(struct FloatRMat *rm)
void float_quat_integrate(struct FloatQuat *q, struct FloatRates *omega, float dt)
in place quaternion integration with constant rotational velocity
#define RATES_ADD_SCALED_VECT(_ro, _v, _s)
bool correct_gravity
enable gravity correction during coordinated turns
struct FloatRates imu_rate
bool ahrs_fc_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_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.
float mag_omega
filter cut-off frequency for correcting the attitude (heading) from magnetometer
#define AHRS_GRAVITY_HEURISTIC_FACTOR
by default use the gravity heuristic to reduce gain
struct FloatRates rate_correction
void ahrs_fc_update_mag_2d(struct FloatVect3 *mag, float dt)
void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a, struct FloatRates *ra)
rotate anglular rates by transposed rotation matrix.
void float_quat_comp_norm_shortest(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions with normalization.
static struct OrientationReps body_to_imu
void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
Composition (multiplication) of two quaternions with normalization.
Simple matrix helper macros.
void ahrs_fc_update_heading(float heading)
Update yaw based on a heading measurement.
static void ahrs_float_get_quat_from_accel_mag(struct FloatQuat *q, struct FloatVect3 *accel, struct FloatVect3 *mag)
void ahrs_fc_update_mag_full(struct FloatVect3 *mag, float dt)
void ahrs_fc_recompute_ltp_to_body(void)
#define VECT3_ASSIGN(_a, _x, _y, _z)
#define RATES_COPY(_a, _b)
void ahrs_fc_update_mag_2d_dumb(struct FloatVect3 *mag)
struct OrientationReps body_to_imu
void ahrs_fc_set_body_to_imu(struct OrientationReps *body_to_imu)
#define GPS_FIX_3D
3D GPS fix
uint16_t mag_cnt
number of propagations since last mag update
#define VECT3_COPY(_a, _b)
struct FloatRates gyro_bias
float accel_zeta
filter damping for correcting the gyro-bias from accels (pseudo-gravity measurement)
void float_quat_comp_inv(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
float accel_omega
filter cut-off frequency for correcting the attitude from accels (pseudo-gravity measurement)