Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
ahrs_float_lkf.c File Reference
#include "ahrs_float_lkf.h"
#include "subsystems/imu.h"
#include "subsystems/ahrs/ahrs_aligner.h"
#include "generated/airframe.h"
#include "math/pprz_algebra_float.h"
#include <stdio.h>
+ Include dependency graph for ahrs_float_lkf.c:

Go to the source code of this file.

Macros

#define BAFL_DEBUG
 
#define BAFL_g   9.81
 
#define BAFL_hx   1.0
 
#define BAFL_hy   0.0
 
#define BAFL_hz   1.0
 
#define BAFL_Q_GYRO   1e-04
 
#define BAFL_Q_ATT   0
 
#define BAFL_SIGMA_ACCEL   1000.0
 
#define BAFL_SIGMA_MAG   20.
 
#define BAFL_DT   (1./512.)
 
#define FLOAT_MAT33_INVERT(_mo, _mi)
 
#define AHRS_TO_BFP()
 
#define AHRS_LTP_TO_BODY()
 

Functions

static void ahrs_do_update_accel (void)
 
static void ahrs_do_update_mag (void)
 
void ahrs_init (void)
 AHRS initialization. More...
 
void ahrs_align (void)
 Aligns the AHRS. More...
 
static void ahrs_lowpass_accel (void)
 
void ahrs_propagate (void)
 Propagation. More...
 
void ahrs_update_accel (void)
 Update AHRS state with accerleration measurements. More...
 
void ahrs_update_mag (void)
 Update AHRS state with magnetometer measurements. More...
 
void ahrs_update (void)
 

Variables

struct FloatQuat bafl_quat
 
struct FloatRates bafl_bias
 
struct FloatRates bafl_rates
 
struct FloatEulers bafl_eulers
 
struct FloatRMat bafl_dcm
 
struct FloatQuat bafl_q_a_err
 
struct FloatQuat bafl_q_m_err
 
struct FloatRates bafl_b_a_err
 
struct FloatRates bafl_b_m_err
 
struct FloatQuat bafl_qtemp
 
struct FloatQuat bafl_qr
 
float bafl_qnorm
 
float bafl_phi_accel
 
float bafl_theta_accel
 
float bafl_P [BAFL_SSIZE][BAFL_SSIZE]
 
float bafl_X [BAFL_SSIZE]
 
float bafl_F [3][3]
 
float bafl_T [6][6]
 
float bafl_Pprio [BAFL_SSIZE][BAFL_SSIZE]
 
float bafl_tempP [BAFL_SSIZE][BAFL_SSIZE]
 
float bafl_K [6][3]
 
float bafl_tempK [6][3]
 
float bafl_S [3][3]
 
float bafl_tempS [3][6]
 
float bafl_invS [3][3]
 
struct FloatVect3 bafl_ya
 
struct FloatVect3 bafl_ym
 
float bafl_H [3][3]
 
struct FloatVect3 bafl_accel_measure
 
struct FloatVect3 bafl_accel_last
 
struct FloatVect3 bafl_mag
 
float bafl_qom [4][4]
 
struct FloatVect3 bafl_h
 
float bafl_Q_gyro
 
float bafl_Q_att
 
float bafl_sigma_accel
 
float bafl_sigma_mag
 
float bafl_R_accel
 
float bafl_R_mag
 

Macro Definition Documentation

#define AHRS_LTP_TO_BODY ( )
Value:
{ \
/* Compute LTP to BODY quaternion */ \
/* Compute LTP to BODY rotation matrix */ \
/* compute LTP to BODY eulers */ \
/* compute body rates */ \
}
struct Int32Rates body_rate
Rotational velocity in body frame.
Definition: ahrs.h:52
#define INT32_RMAT_TRANSP_RATEMULT(_vb, _m_b2a, _va)
struct Int32RMat body_to_imu_rmat
rotation from body to imu frame as a rotation matrix
Definition: imu.h:52
struct Ahrs ahrs
global AHRS state (fixed point version)
Definition: ahrs.c:24
struct Int32Rates imu_rate
Rotational velocity in IMU frame.
Definition: ahrs.h:47
struct Int32RMat ltp_to_body_rmat
Rotation from LocalTangentPlane to body frame as Rotation Matrix.
Definition: ahrs.h:51
#define INT32_QUAT_COMP_INV(_a2b, _a2c, _b2c)
struct Int32Quat ltp_to_imu_quat
Rotation from LocalTangentPlane to IMU frame as unit quaternion.
Definition: ahrs.h:44
struct Int32Quat ltp_to_body_quat
Rotation from LocalTangentPlane to body frame as unit quaternion.
Definition: ahrs.h:49
#define INT32_EULERS_OF_RMAT(_e, _rm)
#define INT32_RMAT_COMP_INV(_m_a2b, _m_a2c, _m_b2c)
struct Int32Eulers ltp_to_body_euler
Rotation from LocalTangentPlane to body frame as Euler angles.
Definition: ahrs.h:50
struct Int32Quat body_to_imu_quat
rotation from body to imu frame as a unit quaternion
Definition: imu.h:51
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:50
struct Int32RMat ltp_to_imu_rmat
Rotation from LocalTangentPlane to IMU frame as Rotation Matrix.
Definition: ahrs.h:46

Definition at line 201 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel(), ahrs_do_update_mag(), and ahrs_propagate().

#define AHRS_TO_BFP ( )
Value:
{ \
/* IMU rate */ \
/* LTP to IMU eulers */ \
/* LTP to IMU quaternion */ \
/* LTP to IMU rotation matrix */ \
}
#define QUAT_BFP_OF_REAL(_qi, _qf)
Definition: pprz_algebra.h:612
#define RMAT_BFP_OF_REAL(_ei, _ef)
Definition: pprz_algebra.h:581
struct Ahrs ahrs
global AHRS state (fixed point version)
Definition: ahrs.c:24
struct Int32Rates imu_rate
Rotational velocity in IMU frame.
Definition: ahrs.h:47
struct Int32Quat ltp_to_imu_quat
Rotation from LocalTangentPlane to IMU frame as unit quaternion.
Definition: ahrs.h:44
struct FloatEulers bafl_eulers
#define RATES_BFP_OF_REAL(_ri, _rf)
Definition: pprz_algebra.h:625
struct FloatRMat bafl_dcm
struct Int32Eulers ltp_to_imu_euler
Rotation from LocalTangentPlane to IMU frame as Euler angles.
Definition: ahrs.h:45
#define EULERS_BFP_OF_REAL(_ei, _ef)
Definition: pprz_algebra.h:575
struct Int32RMat ltp_to_imu_rmat
Rotation from LocalTangentPlane to IMU frame as Rotation Matrix.
Definition: ahrs.h:46
struct FloatQuat bafl_quat
struct FloatRates bafl_rates

Definition at line 190 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel(), ahrs_do_update_mag(), and ahrs_propagate().

#define BAFL_DEBUG

Definition at line 36 of file ahrs_float_lkf.c.

#define BAFL_DT   (1./512.)

Definition at line 159 of file ahrs_float_lkf.c.

Referenced by ahrs_propagate().

#define BAFL_g   9.81

Definition at line 124 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel().

#define BAFL_hx   1.0

Definition at line 126 of file ahrs_float_lkf.c.

Referenced by ahrs_init().

#define BAFL_hy   0.0

Definition at line 127 of file ahrs_float_lkf.c.

Referenced by ahrs_init().

#define BAFL_hz   1.0

Definition at line 128 of file ahrs_float_lkf.c.

Referenced by ahrs_init().

#define BAFL_Q_ATT   0

Definition at line 140 of file ahrs_float_lkf.c.

Referenced by ahrs_init().

#define BAFL_Q_GYRO   1e-04

Definition at line 139 of file ahrs_float_lkf.c.

Referenced by ahrs_init().

#define BAFL_SIGMA_ACCEL   1000.0

Definition at line 151 of file ahrs_float_lkf.c.

Referenced by ahrs_init().

#define BAFL_SIGMA_MAG   20.

Definition at line 152 of file ahrs_float_lkf.c.

Referenced by ahrs_init().

#define FLOAT_MAT33_INVERT (   _mo,
  _mi 
)
Value:
{ \
float _det = 0.; \
_det = _mi[0][0] * (_mi[2][2] * _mi[1][1] - _mi[2][1] * _mi[1][2]) \
- _mi[1][0] * (_mi[2][2] * _mi[0][1] - _mi[2][1] * _mi[0][2]) \
+ _mi[2][0] * (_mi[1][2] * _mi[0][1] - _mi[1][1] * _mi[0][2]); \
if (_det != 0.) { /* ? test for zero? */ \
_mo[0][0] = (_mi[1][1] * _mi[2][2] - _mi[1][2] * _mi[2][1]) / _det; \
_mo[0][1] = (_mi[0][2] * _mi[2][1] - _mi[0][1] * _mi[2][2]) / _det; \
_mo[0][2] = (_mi[0][1] * _mi[1][2] - _mi[0][2] * _mi[1][1]) / _det; \
\
_mo[1][0] = (_mi[1][2] * _mi[2][0] - _mi[1][0] * _mi[2][2]) / _det; \
_mo[1][1] = (_mi[0][0] * _mi[2][2] - _mi[0][2] * _mi[2][0]) / _det; \
_mo[1][2] = (_mi[0][2] * _mi[1][0] - _mi[0][0] * _mi[1][2]) / _det; \
\
_mo[2][0] = (_mi[1][0] * _mi[2][1] - _mi[1][1] * _mi[2][0]) / _det; \
_mo[2][1] = (_mi[0][1] * _mi[2][0] - _mi[0][0] * _mi[2][1]) / _det; \
_mo[2][2] = (_mi[0][0] * _mi[1][1] - _mi[0][1] * _mi[1][0]) / _det; \
} else { \
printf("ERROR! Not invertible!\n"); \
for (int _i=0; _i<3; _i++) { \
for (int _j=0; _j<3; _j++) { \
_mo[_i][_j] = 0.0; \
} \
} \
} \
}

Definition at line 163 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel(), and ahrs_do_update_mag().

Function Documentation

void ahrs_align ( void  )

Aligns the AHRS.

Called after ahrs_aligner has run to set initial attitude and biases. Must set the ahrs status to AHRS_RUNNING. Needs to be implemented by each AHRS algorithm.

Definition at line 251 of file ahrs_float_lkf.c.

References ACCELS_FLOAT_OF_BFP, ahrs, ahrs_aligner, AHRS_RUNNING, bafl_accel_measure, bafl_bias, AhrsAligner::lp_accel, AhrsAligner::lp_gyro, RATES_FLOAT_OF_BFP, and Ahrs::status.

static void ahrs_lowpass_accel ( void  )
inlinestatic

Definition at line 257 of file ahrs_float_lkf.c.

References Imu::accel, ACCELS_FLOAT_OF_BFP, bafl_accel_last, bafl_accel_measure, bafl_phi_accel, bafl_theta_accel, imu, VECT3_ADD, VECT3_SDIV, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by ahrs_propagate().

+ Here is the caller graph for this function:

void ahrs_propagate ( void  )
void ahrs_update ( void  )

Definition at line 849 of file ahrs_float_lkf.c.

References ahrs_update_accel(), and ahrs_update_mag().

+ Here is the call graph for this function:

void ahrs_update_accel ( void  )

Update AHRS state with accerleration measurements.

Reads the global imu data struct. Needs to be implemented by each AHRS algorithm.

Definition at line 408 of file ahrs_float_lkf.c.

References ahrs_do_update_accel().

Referenced by ahrs_update().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void ahrs_update_mag ( void  )

Update AHRS state with magnetometer measurements.

Reads the global imu data struct. Needs to be implemented by each AHRS algorithm.

Definition at line 639 of file ahrs_float_lkf.c.

References ahrs_do_update_mag().

Referenced by ahrs_update().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

Variable Documentation

struct FloatVect3 bafl_accel_last

Definition at line 117 of file ahrs_float_lkf.c.

Referenced by ahrs_lowpass_accel().

struct FloatVect3 bafl_accel_measure

Definition at line 116 of file ahrs_float_lkf.c.

Referenced by ahrs_align(), ahrs_do_update_accel(), and ahrs_lowpass_accel().

struct FloatRates bafl_b_a_err

Definition at line 58 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel().

struct FloatRates bafl_b_m_err

Definition at line 60 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_mag().

struct FloatRates bafl_bias
struct FloatRMat bafl_dcm

Definition at line 51 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel(), ahrs_do_update_mag(), and ahrs_propagate().

struct FloatEulers bafl_eulers

Definition at line 49 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel(), ahrs_do_update_mag(), and ahrs_propagate().

float bafl_F[3][3]

Definition at line 85 of file ahrs_float_lkf.c.

float bafl_H[3][3]

Definition at line 113 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel(), and ahrs_do_update_mag().

struct FloatVect3 bafl_h

Definition at line 129 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_mag(), and ahrs_init().

float bafl_invS[3][3]

Definition at line 102 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel(), and ahrs_do_update_mag().

float bafl_K[6][3]

Definition at line 98 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel(), and ahrs_do_update_mag().

struct FloatVect3 bafl_mag

Definition at line 119 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_mag().

float bafl_P[BAFL_SSIZE][BAFL_SSIZE]
float bafl_phi_accel

Definition at line 69 of file ahrs_float_lkf.c.

Referenced by ahrs_lowpass_accel().

float bafl_Pprio[BAFL_SSIZE][BAFL_SSIZE]

Definition at line 95 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel(), and ahrs_do_update_mag().

struct FloatQuat bafl_q_a_err

Definition at line 54 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel().

float bafl_Q_att

Definition at line 142 of file ahrs_float_lkf.c.

Referenced by ahrs_init(), and ahrs_propagate().

float bafl_Q_gyro

Definition at line 141 of file ahrs_float_lkf.c.

Referenced by ahrs_init(), and ahrs_propagate().

struct FloatQuat bafl_q_m_err

Definition at line 56 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_mag().

float bafl_qnorm

Definition at line 66 of file ahrs_float_lkf.c.

Referenced by ahrs_propagate().

float bafl_qom[4][4]

Definition at line 122 of file ahrs_float_lkf.c.

struct FloatQuat bafl_qr

Definition at line 64 of file ahrs_float_lkf.c.

Referenced by ahrs_propagate().

struct FloatQuat bafl_qtemp

Definition at line 62 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel(), ahrs_do_update_mag(), and ahrs_propagate().

struct FloatQuat bafl_quat
float bafl_R_accel

Definition at line 155 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel().

float bafl_R_mag

Definition at line 156 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_mag().

struct FloatRates bafl_rates

Definition at line 47 of file ahrs_float_lkf.c.

Referenced by ahrs_propagate().

float bafl_S[3][3]

Definition at line 100 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel(), and ahrs_do_update_mag().

float bafl_sigma_accel

Definition at line 153 of file ahrs_float_lkf.c.

float bafl_sigma_mag

Definition at line 154 of file ahrs_float_lkf.c.

float bafl_T[6][6]

Definition at line 90 of file ahrs_float_lkf.c.

Referenced by ahrs_init(), and ahrs_propagate().

float bafl_tempK[6][3]

Definition at line 99 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel(), and ahrs_do_update_mag().

float bafl_tempP[BAFL_SSIZE][BAFL_SSIZE]

Definition at line 97 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel(), ahrs_do_update_mag(), and ahrs_propagate().

float bafl_tempS[3][6]

Definition at line 101 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel(), and ahrs_do_update_mag().

float bafl_theta_accel

Definition at line 70 of file ahrs_float_lkf.c.

Referenced by ahrs_lowpass_accel().

float bafl_X[BAFL_SSIZE]

Definition at line 78 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel(), and ahrs_do_update_mag().

struct FloatVect3 bafl_ya

Definition at line 103 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_accel().

struct FloatVect3 bafl_ym

Definition at line 104 of file ahrs_float_lkf.c.

Referenced by ahrs_do_update_mag().