Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
bool reset
flag to request reset/reinit the filter
float lx
Tuning parameter of accel and mag on attitude (longitudinal subsystem)
Dispatcher to register actual AHRS implementations.
void ahrs_float_invariant_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
struct FloatVect3 ME
Correction gains on gyro biases.
float OE
Correction gains on magnetometer sensitivity.
struct OrientationReps body_to_imu
body_to_imu rotation
float n
Tuning parameter of accel and mag on accel bias (scaling subsystem)
struct FloatVect3 accel
Measured accelerometers.
float o
Tuning parameter of accel and mag on mag bias (scaling subsystem)
float mx
Tuning parameter of accel and mag on gyro bias (longitudinal subsystem)
struct FloatQuat quat
Estimated attitude (quaternion)
Invariant filter tuning gains.
void ahrs_float_invariant_update_accel(struct FloatVect3 *accel)
float dt
Invariant filter command vector.
struct inv_command cmd
command vector
struct FloatVect3 LE
Correction gains on attitude.
float mz
Tuning parameter of accel and mag on gyro bias (heading subsystem)
float my
Tuning parameter of accel and mag on gyro bias (lateral subsystem)
Roation quaternion.
float ly
Tuning parameter of accel and mag on attitude (lateral subsystem)
Paparazzi floating point algebra.
struct AhrsFloatInv ahrs_float_inv
float NE
Correction gains on accel bias.
struct inv_correction_gains corr
correction gains
struct FloatRates rates
Input gyro rates.
Invariant filter correction gains.
float lz
Tuning parameter of accel and mag on attitude (heading subsystem)
Invariant filter measurement vector.
struct FloatVect3 mag
Measured magnetic field.
struct FloatRates bias
Estimated gyro biases.
void ahrs_float_inv_set_body_to_imu_quat(struct FloatQuat *q_b2i)
void ahrs_float_invariant_propagate(struct FloatRates *gyro, float dt)
float cs
Estimates magnetometer sensitivity.
void ahrs_float_invariant_update_mag(struct FloatVect3 *mag)
struct inv_measures meas
measurement vector
struct FloatVect3 mag_h
Generic orientation representation and conversions.
void ahrs_float_invariant_init(void)
struct inv_gains gains
tuning gains
Invariant filter state.
angular rates
float as
Estimated accelerometer sensitivity.
struct inv_state state
state vector
Invariant filter structure.