Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
ahrs_float_invariant.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014-2015 Jean-Philippe Condomines, Gautier Hattenberger
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
27 #ifndef AHRS_FLOAT_INVARIANT_H
28 #define AHRS_FLOAT_INVARIANT_H
29 
30 #include "subsystems/ahrs.h"
33 
36 #define INV_STATE_DIM 9
37 
40 struct inv_state {
41  struct FloatQuat quat;
42  struct FloatRates bias;
43  float cs;
44  float as;
45 };
46 
49 #define INV_MEASURE_DIM 6
50 
53 struct inv_measures {
54  struct FloatVect3 accel;
55  struct FloatVect3 mag;
56 };
57 
60 #define INV_COMMAND_DIM 3
61 
64 struct inv_command {
65  struct FloatRates rates;
66 };
67 
71  struct FloatVect3 LE;
72  struct FloatVect3 ME;
73  float NE;
74  float OE;
75 };
76 
79 struct inv_gains {
80  float lx;
81  float ly;
82  float lz;
83  float mx;
84  float my;
85  float mz;
86  float n;
87  float o;
88 };
89 
92 struct AhrsFloatInv {
93  struct inv_state state;
94  struct inv_measures meas;
95  struct inv_command cmd;
97  struct inv_gains gains;
98 
99  bool reset;
100 
103 
106 };
107 
108 extern struct AhrsFloatInv ahrs_float_inv;
109 
110 extern void ahrs_float_invariant_init(void);
111 extern void ahrs_float_inv_set_body_to_imu_quat(struct FloatQuat *q_b2i);
112 extern void ahrs_float_invariant_align(struct FloatRates *lp_gyro,
113  struct FloatVect3 *lp_accel,
114  struct FloatVect3 *lp_mag);
115 extern void ahrs_float_invariant_propagate(struct FloatRates* gyro, float dt);
116 extern void ahrs_float_invariant_update_accel(struct FloatVect3* accel);
117 extern void ahrs_float_invariant_update_mag(struct FloatVect3* mag);
118 
119 #endif /* AHRS_FLOAT_INVARIANT_H */
120 
OrientationReps
Definition: pprz_orientation_conversion.h:79
inv_correction_gains::ME
struct FloatVect3 ME
Correction gains on gyro biases.
Definition: ahrs_float_invariant.h:72
AhrsFloatInv::gains
struct inv_gains gains
tuning gains
Definition: ahrs_float_invariant.h:97
ahrs_float_invariant_update_accel
void ahrs_float_invariant_update_accel(struct FloatVect3 *accel)
Definition: ahrs_float_invariant.c:231
inv_command::rates
struct FloatRates rates
Input gyro rates.
Definition: ahrs_float_invariant.h:65
inv_state
Invariant filter state.
Definition: ahrs_float_invariant.h:40
inv_measures::accel
struct FloatVect3 accel
Measured accelerometers.
Definition: ahrs_float_invariant.h:54
inv_correction_gains
Invariant filter correction gains.
Definition: ahrs_float_invariant.h:70
pprz_algebra_float.h
Paparazzi floating point algebra.
inv_state::bias
struct FloatRates bias
Estimated gyro biases.
Definition: ahrs_float_invariant.h:42
ahrs_float_invariant_update_mag
void ahrs_float_invariant_update_mag(struct FloatVect3 *mag)
Definition: ahrs_float_invariant.c:239
ahrs_float_inv_set_body_to_imu_quat
void ahrs_float_inv_set_body_to_imu_quat(struct FloatQuat *q_b2i)
Definition: ahrs_float_invariant.c:384
inv_gains
Invariant filter tuning gains.
Definition: ahrs_float_invariant.h:79
ahrs_float_inv
struct AhrsFloatInv ahrs_float_inv
Definition: ahrs_float_invariant.c:95
inv_gains::ly
float ly
Tuning parameter of accel and mag on attitude (lateral subsystem)
Definition: ahrs_float_invariant.h:81
FloatVect3
Definition: pprz_algebra_float.h:54
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
inv_gains::lx
float lx
Tuning parameter of accel and mag on attitude (longitudinal subsystem)
Definition: ahrs_float_invariant.h:80
AhrsFloatInv::state
struct inv_state state
state vector
Definition: ahrs_float_invariant.h:93
inv_gains::n
float n
Tuning parameter of accel and mag on accel bias (scaling subsystem)
Definition: ahrs_float_invariant.h:86
AhrsFloatInv::reset
bool reset
flag to request reset/reinit the filter
Definition: ahrs_float_invariant.h:99
inv_gains::my
float my
Tuning parameter of accel and mag on gyro bias (lateral subsystem)
Definition: ahrs_float_invariant.h:84
ahrs_float_invariant_propagate
void ahrs_float_invariant_propagate(struct FloatRates *gyro, float dt)
Definition: ahrs_float_invariant.c:170
inv_correction_gains::NE
float NE
Correction gains on accel bias.
Definition: ahrs_float_invariant.h:73
AhrsFloatInv::is_aligned
bool is_aligned
Definition: ahrs_float_invariant.h:105
AhrsFloatInv::mag_h
struct FloatVect3 mag_h
Definition: ahrs_float_invariant.h:104
ahrs.h
AhrsFloatInv::body_to_imu
struct OrientationReps body_to_imu
body_to_imu rotation
Definition: ahrs_float_invariant.h:102
inv_command
Invariant filter command vector.
Definition: ahrs_float_invariant.h:64
inv_correction_gains::OE
float OE
Correction gains on magnetometer sensitivity.
Definition: ahrs_float_invariant.h:74
inv_correction_gains::LE
struct FloatVect3 LE
Correction gains on attitude.
Definition: ahrs_float_invariant.h:71
ahrs_float_invariant_align
void ahrs_float_invariant_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
Definition: ahrs_float_invariant.c:156
ahrs_float_invariant_init
void ahrs_float_invariant_init(void)
Definition: ahrs_float_invariant.c:131
inv_measures
Invariant filter measurement vector.
Definition: ahrs_float_invariant.h:53
inv_gains::o
float o
Tuning parameter of accel and mag on mag bias (scaling subsystem)
Definition: ahrs_float_invariant.h:87
pprz_orientation_conversion.h
inv_gains::mx
float mx
Tuning parameter of accel and mag on gyro bias (longitudinal subsystem)
Definition: ahrs_float_invariant.h:83
inv_state::cs
float cs
Estimates magnetometer sensitivity.
Definition: ahrs_float_invariant.h:43
inv_state::as
float as
Estimated accelerometer sensitivity.
Definition: ahrs_float_invariant.h:44
AhrsFloatInv::cmd
struct inv_command cmd
command vector
Definition: ahrs_float_invariant.h:95
inv_measures::mag
struct FloatVect3 mag
Measured magnetic field.
Definition: ahrs_float_invariant.h:55
AhrsFloatInv::meas
struct inv_measures meas
measurement vector
Definition: ahrs_float_invariant.h:94
AhrsFloatInv
Invariant filter structure.
Definition: ahrs_float_invariant.h:92
inv_gains::lz
float lz
Tuning parameter of accel and mag on attitude (heading subsystem)
Definition: ahrs_float_invariant.h:82
inv_gains::mz
float mz
Tuning parameter of accel and mag on gyro bias (heading subsystem)
Definition: ahrs_float_invariant.h:85
AhrsFloatInv::corr
struct inv_correction_gains corr
correction gains
Definition: ahrs_float_invariant.h:96
inv_state::quat
struct FloatQuat quat
Estimated attitude (quaternion)
Definition: ahrs_float_invariant.h:41
FloatRates
angular rates
Definition: pprz_algebra_float.h:93