 |
Paparazzi UAS
v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
|
|
Paparazzi UAS
v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
|
#include "generated/airframe.h"#include "subsystems/ahrs/ahrs_int_cmpl_quat.h"#include "subsystems/ahrs/ahrs_int_utils.h"#include "subsystems/gps.h"#include "math/pprz_trig_int.h"#include "math/pprz_algebra_int.h"
Include dependency graph for ahrs_int_cmpl_quat.c:Go to the source code of this file.
Macros | |
| #define | AHRS_ACCEL_OMEGA 0.063 |
| #define | AHRS_ACCEL_ZETA 0.9 |
| #define | AHRS_MAG_OMEGA 0.04 |
| #define | AHRS_MAG_ZETA 0.9 |
| #define | AHRS_GRAVITY_HEURISTIC_FACTOR 30 |
| by default use the gravity heuristic to reduce gain More... | |
| #define | AHRS_BIAS_UPDATE_HEADING_THRESHOLD 5.0 |
| don't update gyro bias if heading deviation is above this threshold in degrees More... | |
| #define | AHRS_HEADING_UPDATE_GPS_MIN_SPEED 5.0 |
| Minimum speed in m/s for heading update via GPS. More... | |
| #define | COMPUTATION_FRAC 16 |
| #define | ACC_FROM_CROSS_FRAC INT32_RATE_FRAC + INT32_SPEED_FRAC - INT32_ACCEL_FRAC - COMPUTATION_FRAC |
| #define | FIR_FILTER_SIZE 8 |
Functions | |
| static void UNUSED | ahrs_icq_update_mag_full (struct Int32Vect3 *mag, float dt) |
| static void | ahrs_icq_update_mag_2d (struct Int32Vect3 *mag, float dt) |
| void | ahrs_icq_init (void) |
| bool | ahrs_icq_align (struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag) |
| void | ahrs_icq_propagate (struct Int32Rates *gyro, float dt) |
| void | ahrs_icq_set_accel_gains (void) |
| update pre-computed inv_kp and inv_ki gains from acc_omega and acc_zeta More... | |
| void | ahrs_icq_update_accel (struct Int32Vect3 *accel, float dt) |
| void | ahrs_icq_update_mag (struct Int32Vect3 *mag, float dt) |
| void | ahrs_icq_set_mag_gains (void) |
| update pre-computed kp and ki gains from mag_omega and mag_zeta More... | |
| void | ahrs_icq_update_gps (struct GpsState *gps_s) |
| void | ahrs_icq_update_heading (int32_t heading) |
| Update yaw based on a heading measurement. More... | |
| void | ahrs_icq_realign_heading (int32_t heading) |
| Hard reset yaw to a heading. More... | |
| void | ahrs_icq_set_body_to_imu (struct OrientationReps *body_to_imu) |
| void | ahrs_icq_set_body_to_imu_quat (struct FloatQuat *q_b2i) |
Variables | |
| struct AhrsIntCmplQuat | ahrs_icq |
| Default Rate filter Low pass. More... | |
Quaternion complementary filter (fixed-point).
Estimate the attitude, heading and gyro bias.
Definition in file ahrs_int_cmpl_quat.c.
| #define ACC_FROM_CROSS_FRAC INT32_RATE_FRAC + INT32_SPEED_FRAC - INT32_ACCEL_FRAC - COMPUTATION_FRAC |
| #define AHRS_ACCEL_OMEGA 0.063 |
Definition at line 62 of file ahrs_int_cmpl_quat.c.
| #define AHRS_ACCEL_ZETA 0.9 |
Definition at line 65 of file ahrs_int_cmpl_quat.c.
| #define AHRS_BIAS_UPDATE_HEADING_THRESHOLD 5.0 |
don't update gyro bias if heading deviation is above this threshold in degrees
Definition at line 89 of file ahrs_int_cmpl_quat.c.
| #define AHRS_GRAVITY_HEURISTIC_FACTOR 30 |
by default use the gravity heuristic to reduce gain
Definition at line 84 of file ahrs_int_cmpl_quat.c.
| #define AHRS_HEADING_UPDATE_GPS_MIN_SPEED 5.0 |
Minimum speed in m/s for heading update via GPS.
Don't update heading from GPS course if GPS ground speed is below is this threshold
Definition at line 96 of file ahrs_int_cmpl_quat.c.
| #define AHRS_MAG_OMEGA 0.04 |
Definition at line 72 of file ahrs_int_cmpl_quat.c.
| #define AHRS_MAG_ZETA 0.9 |
Definition at line 75 of file ahrs_int_cmpl_quat.c.
| #define COMPUTATION_FRAC 16 |
| #define FIR_FILTER_SIZE 8 |
| bool ahrs_icq_align | ( | struct Int32Rates * | lp_gyro, |
| struct Int32Vect3 * | lp_accel, | ||
| struct Int32Vect3 * | lp_mag | ||
| ) |
Definition at line 158 of file ahrs_int_cmpl_quat.c.
References ahrs_icq, AHRS_ICQ_RUNNING, ahrs_int_get_quat_from_accel(), ahrs_int_get_quat_from_accel_mag(), AhrsIntCmplQuat::gyro_bias, AhrsIntCmplQuat::heading_aligned, AhrsIntCmplQuat::high_rez_bias, INT_RATES_LSHIFT, AhrsIntCmplQuat::is_aligned, AhrsIntCmplQuat::ltp_to_imu_quat, RATES_COPY, and AhrsIntCmplQuat::status.
Referenced by aligner_cb().
Here is the call graph for this function: Here is the caller graph for this function:| void ahrs_icq_init | ( | void | ) |
Definition at line 115 of file ahrs_int_cmpl_quat.c.
References AhrsIntCmplQuat::accel_cnt, AhrsIntCmplQuat::accel_omega, AhrsIntCmplQuat::accel_zeta, AHRS_ACCEL_OMEGA, AHRS_ACCEL_ZETA, AHRS_GRAVITY_HEURISTIC_FACTOR, ahrs_icq, ahrs_icq_set_accel_gains(), ahrs_icq_set_mag_gains(), AHRS_ICQ_UNINIT, AHRS_MAG_OMEGA, AHRS_MAG_ZETA, AhrsIntCmplQuat::correct_gravity, AhrsIntCmplQuat::gravity_heuristic_factor, AhrsIntCmplQuat::gyro_bias, AhrsIntCmplQuat::heading_aligned, AhrsIntCmplQuat::high_rez_bias, AhrsIntCmplQuat::imu_rate, int32_quat_identity(), INT_RATES_ZERO, AhrsIntCmplQuat::is_aligned, AhrsIntCmplQuat::ltp_to_imu_quat, AhrsIntCmplQuat::ltp_vel_norm_valid, MAG_BFP_OF_REAL, AhrsIntCmplQuat::mag_cnt, AhrsIntCmplQuat::mag_h, AhrsIntCmplQuat::mag_omega, AhrsIntCmplQuat::mag_zeta, AhrsIntCmplQuat::rate_correction, AhrsIntCmplQuat::status, and VECT3_ASSIGN.
Referenced by ahrs_icq_register().
Here is the call graph for this function: Here is the caller graph for this function:| void ahrs_icq_propagate | ( | struct Int32Rates * | gyro, |
| float | dt | ||
| ) |
Definition at line 187 of file ahrs_int_cmpl_quat.c.
References AhrsIntCmplQuat::accel_cnt, ahrs_icq, AhrsIntCmplQuat::gyro_bias, AhrsIntCmplQuat::high_rez_quat, AhrsIntCmplQuat::imu_rate, int32_quat_integrate_fi(), int32_quat_normalize(), INT_RATES_ZERO, AhrsIntCmplQuat::ltp_to_imu_quat, AhrsIntCmplQuat::mag_cnt, AhrsIntCmplQuat::rate_correction, RATES_ADD, RATES_COPY, RATES_DIFF, RATES_SDIV, and RATES_SMUL.
Referenced by gyro_cb().
Here is the call graph for this function: Here is the caller graph for this function:| void ahrs_icq_realign_heading | ( | int32_t | heading | ) |
Hard reset yaw to a heading.
Doesn't affect the bias. Sets ahrs_icq.heading_aligned to TRUE.
| heading | Heading in body frame, radians (CW/north) with INT32_ANGLE_FRAC |
Definition at line 627 of file ahrs_int_cmpl_quat.c.
References ahrs_icq, AhrsIntCmplQuat::body_to_imu, heading, AhrsIntCmplQuat::heading_aligned, int32_quat_comp(), int32_quat_comp_inv(), int32_quat_comp_norm_shortest(), int32_quat_inv_comp_norm_shortest(), int32_quat_normalize(), AhrsIntCmplQuat::ltp_to_imu_quat, orientationGetQuat_i(), PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, Int32Quat::qi, QUAT_COPY, Int32Quat::qx, Int32Quat::qy, and Int32Quat::qz.
Referenced by ahrs_icq_update_gps().
Here is the call graph for this function: Here is the caller graph for this function:| void ahrs_icq_set_accel_gains | ( | void | ) |
update pre-computed inv_kp and inv_ki gains from acc_omega and acc_zeta
Definition at line 220 of file ahrs_int_cmpl_quat.c.
References AhrsIntCmplQuat::accel_inv_ki, AhrsIntCmplQuat::accel_inv_kp, AhrsIntCmplQuat::accel_omega, AhrsIntCmplQuat::accel_zeta, and ahrs_icq.
Referenced by ahrs_icq_init(), ahrs_int_cmpl_quat_SetAccelOmega(), and ahrs_int_cmpl_quat_SetAccelZeta().
Here is the caller graph for this function:| void ahrs_icq_set_body_to_imu | ( | struct OrientationReps * | body_to_imu | ) |
Definition at line 662 of file ahrs_int_cmpl_quat.c.
References ahrs_icq_set_body_to_imu_quat(), body_to_imu, and orientationGetQuat_f().
Here is the call graph for this function:| void ahrs_icq_set_body_to_imu_quat | ( | struct FloatQuat * | q_b2i | ) |
Definition at line 667 of file ahrs_int_cmpl_quat.c.
References ahrs_icq, AhrsIntCmplQuat::body_to_imu, AhrsIntCmplQuat::is_aligned, AhrsIntCmplQuat::ltp_to_imu_quat, orientationGetQuat_i(), and orientationSetQuat_f().
Referenced by ahrs_icq_set_body_to_imu(), and body_to_imu_cb().
Here is the call graph for this function: Here is the caller graph for this function:| void ahrs_icq_set_mag_gains | ( | void | ) |
update pre-computed kp and ki gains from mag_omega and mag_zeta
Definition at line 401 of file ahrs_int_cmpl_quat.c.
References ahrs_icq, AhrsIntCmplQuat::mag_ki, AhrsIntCmplQuat::mag_kp, AhrsIntCmplQuat::mag_omega, and AhrsIntCmplQuat::mag_zeta.
Referenced by ahrs_icq_init(), ahrs_int_cmpl_quat_SetMagOmega(), and ahrs_int_cmpl_quat_SetMagZeta().
Here is the caller graph for this function:| void ahrs_icq_update_accel | ( | struct Int32Vect3 * | accel, |
| float | dt | ||
| ) |
Definition at line 241 of file ahrs_int_cmpl_quat.c.
References ACC_FROM_CROSS_FRAC, AhrsIntCmplQuat::accel_cnt, AhrsIntCmplQuat::accel_inv_ki, AhrsIntCmplQuat::accel_inv_kp, ACCELS_FLOAT_OF_BFP, ahrs_icq, AhrsIntCmplQuat::body_to_imu, c2, COMPUTATION_FRAC, AhrsIntCmplQuat::correct_gravity, FIR_FILTER_SIZE, FLOAT_VECT3_NORM, AhrsIntCmplQuat::gravity_heuristic_factor, AhrsIntCmplQuat::gyro_bias, AhrsIntCmplQuat::high_rez_bias, AhrsIntCmplQuat::imu_rate, int32_rmat_of_quat(), int32_rmat_transp_ratemult(), int32_rmat_vmult(), INT32_VECT3_RSHIFT, INT_RATES_RSHIFT, AhrsIntCmplQuat::ltp_to_imu_quat, AhrsIntCmplQuat::ltp_vel_norm, AhrsIntCmplQuat::ltp_vel_norm_valid, orientationGetRMat_i(), Int32Rates::p, Int64Rates::p, Int32Rates::q, Int64Rates::q, Int32Rates::r, Int64Rates::r, AhrsIntCmplQuat::rate_correction, RMAT_ELMT, VECT3_ADD, VECT3_COPY, VECT3_CROSS_PRODUCT, VECT3_DIFF, VECT3_RATES_CROSS_VECT3, VECT3_SDIV, VECT3_SMUL, AhrsIntCmplQuat::weight, Int32Vect3::x, Int32Vect3::y, and Int32Vect3::z.
Referenced by accel_cb().
Here is the call graph for this function: Here is the caller graph for this function:| void ahrs_icq_update_gps | ( | struct GpsState * | gps_s | ) |
Definition at line 529 of file ahrs_int_cmpl_quat.c.
References AHRS_HEADING_UPDATE_GPS_MIN_SPEED, ahrs_icq, ahrs_icq_realign_heading(), ahrs_icq_update_heading(), course, GPS_FIX_3D, AhrsIntCmplQuat::heading_aligned, INT32_ANGLE_FRAC, AhrsIntCmplQuat::ltp_vel_norm, AhrsIntCmplQuat::ltp_vel_norm_valid, and SPEED_BFP_OF_REAL.
Referenced by gps_cb().
Here is the call graph for this function: Here is the caller graph for this function:| void ahrs_icq_update_heading | ( | int32_t | heading | ) |
Update yaw based on a heading measurement.
e.g. from GPS course
| heading | Heading in body frame, radians (CW/north) with INT32_ANGLE_FRAC |
Definition at line 565 of file ahrs_int_cmpl_quat.c.
References AHRS_BIAS_UPDATE_HEADING_THRESHOLD, ahrs_icq, AhrsIntCmplQuat::body_to_imu, AhrsIntCmplQuat::gyro_bias, heading, AhrsIntCmplQuat::high_rez_bias, INT32_ANGLE_FRAC, INT32_ANGLE_NORMALIZE, int32_quat_comp_inv(), int32_rmat_of_quat(), int32_rmat_vmult(), INT_RATES_RSHIFT, AhrsIntCmplQuat::ltp_to_imu_quat, orientationGetQuat_i(), Int32Rates::p, Int64Rates::p, PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, Int32Rates::q, Int64Rates::q, Int32Rates::r, Int64Rates::r, AhrsIntCmplQuat::rate_correction, RMAT_ELMT, TRIG_BFP_OF_REAL, Int32Vect2::x, Int32Vect3::x, Int32Vect2::y, Int32Vect3::y, and Int32Vect3::z.
Referenced by ahrs_icq_update_gps().
Here is the call graph for this function: Here is the caller graph for this function:| void ahrs_icq_update_mag | ( | struct Int32Vect3 * | mag, |
| float | dt | ||
| ) |
Definition at line 384 of file ahrs_int_cmpl_quat.c.
References ahrs_icq, ahrs_icq_update_mag_2d(), ahrs_icq_update_mag_full(), and AhrsIntCmplQuat::mag_cnt.
Referenced by mag_cb().
Here is the call graph for this function: Here is the caller graph for this function:
|
inlinestatic |
Definition at line 463 of file ahrs_int_cmpl_quat.c.
References ahrs_icq, AhrsIntCmplQuat::gyro_bias, AhrsIntCmplQuat::high_rez_bias, INT32_MAG_FRAC, int32_rmat_of_quat(), int32_rmat_transp_vmult(), int32_rmat_vmult(), int32_vect2_normalize(), INT_RATES_RSHIFT, AhrsIntCmplQuat::ltp_to_imu_quat, AhrsIntCmplQuat::mag_cnt, AhrsIntCmplQuat::mag_h, AhrsIntCmplQuat::mag_ki, AhrsIntCmplQuat::mag_kp, Int32Rates::p, Int64Rates::p, Int32Rates::q, Int64Rates::q, Int32Rates::r, Int64Rates::r, AhrsIntCmplQuat::rate_correction, Int32Vect2::x, Int32Vect3::x, Int32Vect2::y, Int32Vect3::y, and Int32Vect3::z.
Referenced by ahrs_icq_update_mag().
Here is the call graph for this function: Here is the caller graph for this function:
|
inlinestatic |
Definition at line 410 of file ahrs_int_cmpl_quat.c.
References ahrs_icq, AhrsIntCmplQuat::gyro_bias, AhrsIntCmplQuat::high_rez_bias, int32_rmat_of_quat(), int32_rmat_vmult(), INT_RATES_RSHIFT, AhrsIntCmplQuat::ltp_to_imu_quat, AhrsIntCmplQuat::mag_cnt, AhrsIntCmplQuat::mag_h, AhrsIntCmplQuat::mag_ki, AhrsIntCmplQuat::mag_kp, Int32Rates::p, Int64Rates::p, Int32Rates::q, Int64Rates::q, Int32Rates::r, Int64Rates::r, AhrsIntCmplQuat::rate_correction, VECT3_CROSS_PRODUCT, Int32Vect3::x, Int32Vect3::y, and Int32Vect3::z.
Referenced by ahrs_icq_update_mag().
Here is the call graph for this function: Here is the caller graph for this function:| struct AhrsIntCmplQuat ahrs_icq |
Default Rate filter Low pass.
Definition at line 110 of file ahrs_int_cmpl_quat.c.
Referenced by accel_cb(), ahrs_icq_align(), ahrs_icq_init(), ahrs_icq_propagate(), ahrs_icq_realign_heading(), ahrs_icq_set_accel_gains(), ahrs_icq_set_body_to_imu_quat(), ahrs_icq_set_mag_gains(), ahrs_icq_update_accel(), ahrs_icq_update_gps(), ahrs_icq_update_heading(), ahrs_icq_update_mag(), ahrs_icq_update_mag_2d(), ahrs_icq_update_mag_full(), ahrs_int_cmpl_quat_SetAccelOmega(), ahrs_int_cmpl_quat_SetAccelZeta(), ahrs_int_cmpl_quat_SetMagOmega(), ahrs_int_cmpl_quat_SetMagZeta(), aligner_cb(), geo_mag_cb(), gyro_cb(), mag_cb(), send_bias(), send_euler(), send_filter_status(), send_geo_mag(), send_quat(), and set_body_state_from_quat().
1.8.17 
