Paparazzi UAS  v5.15_devel-112-g521f3cf
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ahrs_int_cmpl_quat.h File Reference

Quaternion complementary filter (fixed-point). More...

#include "subsystems/ahrs.h"
#include "subsystems/gps.h"
#include "std.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_orientation_conversion.h"
+ Include dependency graph for ahrs_int_cmpl_quat.h:
+ This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Data Structures

struct  AhrsIntCmplQuat
 Ahrs implementation specifc values. More...
 

Enumerations

enum  AhrsICQStatus { AHRS_ICQ_UNINIT, AHRS_ICQ_RUNNING }
 

Functions

void ahrs_icq_init (void)
 
void ahrs_icq_set_body_to_imu (struct OrientationReps *body_to_imu)
 
void ahrs_icq_set_body_to_imu_quat (struct FloatQuat *q_b2i)
 
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_update_accel (struct Int32Vect3 *accel, float dt)
 
void ahrs_icq_update_mag (struct Int32Vect3 *mag, float dt)
 
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_accel_gains (void)
 update pre-computed inv_kp and inv_ki gains from acc_omega and acc_zeta More...
 
static void ahrs_int_cmpl_quat_SetAccelOmega (float omega)
 
static void ahrs_int_cmpl_quat_SetAccelZeta (float zeta)
 
void ahrs_icq_set_mag_gains (void)
 update pre-computed kp and ki gains from mag_omega and mag_zeta More...
 
static void ahrs_int_cmpl_quat_SetMagOmega (float omega)
 
static void ahrs_int_cmpl_quat_SetMagZeta (float zeta)
 

Variables

struct AhrsIntCmplQuat ahrs_icq
 Default Rate filter Low pass. More...
 

Detailed Description

Quaternion complementary filter (fixed-point).

Estimate the attitude, heading and gyro bias.

Definition in file ahrs_int_cmpl_quat.h.


Data Structure Documentation

struct AhrsIntCmplQuat

Ahrs implementation specifc values.

Definition at line 48 of file ahrs_int_cmpl_quat.h.

+ Collaboration diagram for AhrsIntCmplQuat:
Data Fields
uint16_t accel_cnt number of propagations since last accel update
float accel_inv_ki
float accel_inv_kp
float accel_omega filter cut-off frequency for correcting the attitude from accels.

(pseudo-gravity measurement) only update through ahrs_int_cmpl_quat_SetAccelOmega

float accel_zeta filter damping for correcting the gyro-bias from accels.

(pseudo-gravity measurement) only update through ahrs_int_cmpl_quat_SetAccelZeta

struct OrientationReps body_to_imu
bool correct_gravity enable gravity vector correction by removing centrifugal acceleration
uint8_t gravity_heuristic_factor sets how strongly the gravity heuristic reduces accel correction.

Set to zero in order to disable gravity heuristic.

struct Int32Rates gyro_bias
bool heading_aligned
struct Int64Rates high_rez_bias
struct Int64Quat high_rez_quat
struct Int32Rates imu_rate
bool is_aligned
struct Int32Quat ltp_to_imu_quat
int32_t ltp_vel_norm
bool ltp_vel_norm_valid
uint16_t mag_cnt number of propagations since last mag update
struct Int32Vect3 mag_h
float mag_ki
float mag_kp
float mag_omega filter cut-off frequency for correcting the attitude (heading) from magnetometer.

only update through ahrs_int_cmpl_quat_SetMagOmega

float mag_zeta filter damping for correcting the gyro bias from magnetometer.

only update through ahrs_int_cmpl_quat_SetMagZeta

struct Int32Rates rate_correction
enum AhrsICQStatus status status of the AHRS, AHRS_ICQ_UNINIT or AHRS_ICQ_RUNNING
float weight

Enumeration Type Documentation

Enumerator
AHRS_ICQ_UNINIT 
AHRS_ICQ_RUNNING 

Definition at line 40 of file ahrs_int_cmpl_quat.h.

Function Documentation

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_realign_heading ( int32_t  heading)

Hard reset yaw to a heading.

Doesn't affect the bias. Sets ahrs_icq.heading_aligned to TRUE.

Parameters
headingHeading 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, 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(), 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_gps ( struct GpsState gps_s)
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:

static void ahrs_int_cmpl_quat_SetAccelOmega ( float  omega)
inlinestatic

Definition at line 136 of file ahrs_int_cmpl_quat.h.

References AhrsIntCmplQuat::accel_omega, ahrs_icq, and ahrs_icq_set_accel_gains().

+ Here is the call graph for this function:

static void ahrs_int_cmpl_quat_SetAccelZeta ( float  zeta)
inlinestatic

Definition at line 142 of file ahrs_int_cmpl_quat.h.

References AhrsIntCmplQuat::accel_zeta, ahrs_icq, and ahrs_icq_set_accel_gains().

+ Here is the call graph for this function:

static void ahrs_int_cmpl_quat_SetMagOmega ( float  omega)
inlinestatic

Definition at line 151 of file ahrs_int_cmpl_quat.h.

References ahrs_icq, ahrs_icq_set_mag_gains(), and AhrsIntCmplQuat::mag_omega.

+ Here is the call graph for this function:

static void ahrs_int_cmpl_quat_SetMagZeta ( float  zeta)
inlinestatic

Definition at line 157 of file ahrs_int_cmpl_quat.h.

References ahrs_icq, ahrs_icq_set_mag_gains(), and AhrsIntCmplQuat::mag_zeta.

+ Here is the call graph for this function:

Variable Documentation