Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
ins_mekf_wind.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2017 Marton Brossard <martin.brossard@mines-paristech.fr>
3  * Gautier Hattenberger <gautier.hattenberger@enac.fr>
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, see
19  * <http://www.gnu.org/licenses/>.
20  */
21 
30 #ifndef INS_MEKF_WIND_H
31 #define INS_MEKF_WIND_H
32 
33 #ifdef __cplusplus
34 extern "C" {
35 #endif
36 
37 #include "std.h"
40 
41 // Settings
43  float Q_gyro;
44  float Q_accel;
45  float Q_rates_bias;
46  float Q_accel_bias;
47  float Q_baro_bias;
48  float Q_wind;
49  float R_speed;
50  float R_pos;
51  float R_speed_z;
52  float R_pos_z;
53  float R_mag;
54  float R_baro;
55  float R_airspeed;
56  float R_aoa;
57  float R_aos;
58  bool disable_wind;
59 };
60 
62 
63 // Init functions
64 extern void ins_mekf_wind_init(void);
65 extern void ins_mekf_wind_align(struct FloatRates *gyro_bias,
66  struct FloatQuat *quat);
67 extern void ins_mekf_wind_set_mag_h(const struct FloatVect3 *mag_h);
68 extern void ins_mekf_wind_reset(void);
69 
70 // Filtering functions
71 extern void ins_mekf_wind_propagate(struct FloatRates* gyro,
72  struct FloatVect3* accel, float dt);
73 extern void ins_mekf_wind_propagate_ahrs(struct FloatRates* gyro,
74  struct FloatVect3* accel, float dt);
75 extern void ins_mekf_wind_update_mag(struct FloatVect3* mag, bool attitude_only);
76 extern void ins_mekf_wind_update_baro(float baro_alt);
77 extern void ins_mekf_wind_update_pos_speed(struct FloatVect3 *pos,
78  struct FloatVect3 *speed);
79 extern void ins_mekf_wind_update_airspeed(float airspeed);
80 extern void ins_mekf_wind_update_incidence(float aoa, float aos);
81 
82 // Getter/Setter functions
83 extern struct NedCoor_f ins_mekf_wind_get_pos_ned(void);
84 extern void ins_mekf_wind_set_pos_ned(struct NedCoor_f *p);
85 extern struct NedCoor_f ins_mekf_wind_get_speed_ned(void);
86 extern void ins_mekf_wind_set_speed_ned(struct NedCoor_f *s);
87 extern struct NedCoor_f ins_mekf_wind_get_accel_ned(void);
88 extern struct FloatQuat ins_mekf_wind_get_quat(void);
89 extern void ins_mekf_wind_set_quat(struct FloatQuat *quat);
90 extern struct FloatRates ins_mekf_wind_get_body_rates(void);
91 extern struct NedCoor_f ins_mekf_wind_get_wind_ned(void);
92 extern struct NedCoor_f ins_mekf_wind_get_airspeed_body(void);
93 extern float ins_mekf_wind_get_airspeed_norm(void);
94 extern struct FloatVect3 ins_mekf_wind_get_accel_bias(void);
95 extern struct FloatRates ins_mekf_wind_get_rates_bias(void);
96 extern float ins_mekf_wind_get_baro_bias(void);
97 
98 // Settings handlers
99 extern void ins_mekf_wind_update_params(void);
100 
101 #define ins_mekf_wind_update_Q_gyro(_v) { \
102  ins_mekf_wind_params.Q_gyro = _v; \
103  ins_mekf_wind_update_params(); \
104 }
105 
106 #define ins_mekf_wind_update_Q_accel(_v) { \
107  ins_mekf_wind_params.Q_accel = _v; \
108  ins_mekf_wind_update_params(); \
109 }
110 
111 #define ins_mekf_wind_update_Q_rates_bias(_v) { \
112  ins_mekf_wind_params.Q_rates_bias = _v; \
113  ins_mekf_wind_update_params(); \
114 }
115 
116 #define ins_mekf_wind_update_Q_accel_bias(_v) { \
117  ins_mekf_wind_params.Q_accel_bias = _v; \
118  ins_mekf_wind_update_params(); \
119 }
120 
121 #define ins_mekf_wind_update_Q_baro_bias(_v) { \
122  ins_mekf_wind_params.Q_baro_bias = _v; \
123  ins_mekf_wind_update_params(); \
124 }
125 
126 #define ins_mekf_wind_update_Q_wind(_v) { \
127  ins_mekf_wind_params.Q_wind = _v; \
128  ins_mekf_wind_update_params(); \
129 }
130 
131 #define ins_mekf_wind_update_R_speed(_v) { \
132  ins_mekf_wind_params.R_speed = _v; \
133  ins_mekf_wind_update_params(); \
134 }
135 
136 #define ins_mekf_wind_update_R_pos(_v) { \
137  ins_mekf_wind_params.R_pos = _v; \
138  ins_mekf_wind_update_params(); \
139 }
140 
141 #define ins_mekf_wind_update_R_speed_z(_v) { \
142  ins_mekf_wind_params.R_speed_z = _v; \
143  ins_mekf_wind_update_params(); \
144 }
145 
146 #define ins_mekf_wind_update_R_pos_z(_v) { \
147  ins_mekf_wind_params.R_pos_z = _v; \
148  ins_mekf_wind_update_params(); \
149 }
150 
151 #define ins_mekf_wind_update_R_mag(_v) { \
152  ins_mekf_wind_params.R_mag = _v; \
153  ins_mekf_wind_update_params(); \
154 }
155 
156 #define ins_mekf_wind_update_R_baro(_v) { \
157  ins_mekf_wind_params.R_baro = _v; \
158  ins_mekf_wind_update_params(); \
159 }
160 
161 #define ins_mekf_wind_update_R_airspeed(_v) { \
162  ins_mekf_wind_params.R_airspeed = _v; \
163  ins_mekf_wind_update_params(); \
164 }
165 
166 #define ins_mekf_wind_update_R_aoa(_v) { \
167  ins_mekf_wind_params.R_aoa = _v; \
168  ins_mekf_wind_update_params(); \
169 }
170 
171 #define ins_mekf_wind_update_R_aos(_v) { \
172  ins_mekf_wind_params.R_aos = _v; \
173  ins_mekf_wind_update_params(); \
174 }
175 
176 
177 #ifdef __cplusplus
178 }
179 #endif
180 
181 #endif /* INS_MEKF_WIND_H */
182 
float baro_alt
Roation quaternion.
angular rates
static float p[2][2]
float Q_accel
accel process noise
Definition: ins_mekf_wind.h:44
float ins_mekf_wind_get_baro_bias(void)
void ins_mekf_wind_set_quat(struct FloatQuat *quat)
struct NedCoor_f ins_mekf_wind_get_wind_ned(void)
void ins_mekf_wind_update_incidence(float aoa, float aos)
void ins_mekf_wind_set_pos_ned(struct NedCoor_f *p)
float ins_mekf_wind_get_airspeed_norm(void)
void ins_mekf_wind_propagate_ahrs(struct FloatRates *gyro, struct FloatVect3 *accel, float dt)
AHRS-only propagation + accel correction.
void ins_mekf_wind_update_baro(float baro_alt)
float Q_wind
wind process noise
Definition: ins_mekf_wind.h:48
float R_aos
sideslip angle measurement noise
Definition: ins_mekf_wind.h:57
struct NedCoor_f ins_mekf_wind_get_accel_ned(void)
void ins_mekf_wind_update_params(void)
void ins_mekf_wind_init(void)
Init function.
void ins_mekf_wind_set_mag_h(const struct FloatVect3 *mag_h)
struct NedCoor_f ins_mekf_wind_get_pos_ned(void)
Getter/Setter functions.
float R_baro
baro measurement noise
Definition: ins_mekf_wind.h:54
struct FloatRates ins_mekf_wind_get_body_rates(void)
void ins_mekf_wind_propagate(struct FloatRates *gyro, struct FloatVect3 *accel, float dt)
Full INS propagation.
void ins_mekf_wind_set_speed_ned(struct NedCoor_f *s)
float R_mag
mag measurement noise
Definition: ins_mekf_wind.h:53
void ins_mekf_wind_update_mag(struct FloatVect3 *mag, bool attitude_only)
struct ins_mekf_wind_parameters ins_mekf_wind_params
struct FloatVect3 ins_mekf_wind_get_accel_bias(void)
bool disable_wind
disable wind estimation
Definition: ins_mekf_wind.h:58
struct NedCoor_f ins_mekf_wind_get_airspeed_body(void)
struct FloatQuat ins_mekf_wind_get_quat(void)
float R_aoa
angle of attack measurement noise
Definition: ins_mekf_wind.h:56
void ins_mekf_wind_align(struct FloatRates *gyro_bias, struct FloatQuat *quat)
void ins_mekf_wind_reset(void)
float R_pos_z
vertical pos measurement noise
Definition: ins_mekf_wind.h:52
float Q_rates_bias
rates bias process noise
Definition: ins_mekf_wind.h:45
float R_airspeed
airspeed measurement noise
Definition: ins_mekf_wind.h:55
float Q_baro_bias
baro bias process noise
Definition: ins_mekf_wind.h:47
float Q_accel_bias
accel bias process noise
Definition: ins_mekf_wind.h:46
float R_pos
pos measurement noise
Definition: ins_mekf_wind.h:50
struct FloatRates ins_mekf_wind_get_rates_bias(void)
float R_speed
speed measurement noise
Definition: ins_mekf_wind.h:49
struct NedCoor_f ins_mekf_wind_get_speed_ned(void)
float R_speed_z
vertical speed measurement noise
Definition: ins_mekf_wind.h:51
float Q_gyro
gyro process noise
Definition: ins_mekf_wind.h:43
void ins_mekf_wind_update_airspeed(float airspeed)
void ins_mekf_wind_update_pos_speed(struct FloatVect3 *pos, struct FloatVect3 *speed)
static uint32_t s
Paparazzi floating point algebra.
Paparazzi floating point math for geodetic calculations.
vector in North East Down coordinates Units: meters