Paparazzi UAS  v5.15_devel-50-g4d7045d
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
wind_estimator.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2016 Johan Maurin, 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 
29 #include "mcu_periph/sys_time.h"
32 #include "generated/modules.h"
33 #include "state.h"
34 #include <string.h>
35 #ifndef SITL // no chibios threads in sim
36 #if !USE_CHIBIOS_RTOS
37 #error Only Chibios is supported
38 #endif
39 #include <ch.h>
40 #include <hal.h>
41 #endif
42 
46 #ifndef WE_UKF_KI
47 #define WE_UKF_KI 0.f // >= 0 to ensure that covariance is positive semi-definite
48 #endif
49 #ifndef WE_UKF_ALPHA
50 #define WE_UKF_ALPHA 0.5f // sigma point dispersion
51 #endif
52 #ifndef WE_UKF_BETA
53 #define WE_UKF_BETA 2.f // 2 is the best choice when distribution is gaussian
54 #endif
55 #ifndef WE_UKF_P0
56 #define WE_UKF_P0 0.2f // initial covariance diagonal element
57 #endif
58 #ifndef WE_UKF_R_GS
59 #define WE_UKF_R_GS 0.5f // ground speed measurement confidence
60 #endif
61 #ifndef WE_UKF_R_VA
62 #define WE_UKF_R_VA 0.5f // airspeed measurement confidence
63 #endif
64 #ifndef WE_UKF_R_AOA
65 #define WE_UKF_R_AOA 0.002f // angle of attack measurement confidence
66 #endif
67 #ifndef WE_UKF_R_SSA
68 #define WE_UKF_R_SSA 0.002f // sideslip angle measurement confidence
69 #endif
70 #ifndef WE_UKF_Q_VA
71 #define WE_UKF_Q_VA 0.1f // airspeed model confidence
72 #endif
73 #ifndef WE_UKF_Q_VA_SCALE
74 #define WE_UKF_Q_VA_SCALE 0.0001f // airspeed scale factor model confidence
75 #endif
76 #ifndef WE_UKF_Q_WIND
77 #define WE_UKF_Q_WIND 0.001f // wind model confidence
78 #endif
79 
80 #ifndef SEND_WIND_ESTIMATOR
81 #define SEND_WIND_ESTIMATOR TRUE
82 #endif
83 
85 
86 static void send_wind_estimator(struct transport_tx *trans, struct link_device *dev)
87 {
88  uint8_t flags = 7; // send all data
89  float upwind = -wind_estimator.wind.z;
90  float airspeed = float_vect3_norm(&wind_estimator.airspeed);
91  pprz_msg_send_WIND_INFO_RET(trans, dev, AC_ID,
92  &flags,
93  &wind_estimator.wind.y, // east
94  &wind_estimator.wind.x, // north
95  &upwind,
96  &airspeed);
97 }
98 
99 #if PERIODIC_TELEMETRY
101 #endif
102 
103 #ifndef LOG_WIND_ESTIMATOR
104 #define LOG_WIND_ESTIMATOR FALSE
105 #endif
106 
107 #if LOG_WIND_ESTIMATOR
108 #ifndef SITL
110 #define PrintLog sdLogWriteLog
111 #define LogFileIsOpen() (pprzLogFile != -1)
112 #else // SITL: print in a file
113 #include <stdio.h>
114 #define PrintLog fprintf
115 #define LogFileIsOpen() (pprzLogFile != NULL)
116 static FILE* pprzLogFile = NULL;
117 #endif
118 static bool log_we_started;
119 #endif
120 
121 // matrix element
122 #define MAT_EL(_m, _l, _c, _n) _m[_l + _c * _n]
123 
124 // wind estimator public structure
126 
127 // local variables
128 static uint32_t time_step_before; // last periodic time
129 
130 /* Thread declaration
131  * MATLAB UKF is using at least 6.6KB of stack
132  */
133 #ifndef SITL
134 static THD_WORKING_AREA(wa_thd_windestimation, 8 * 1024);
135 static __attribute__((noreturn)) void thd_windestimate(void *arg);
136 
137 static MUTEX_DECL(we_ukf_mtx); // mutex for data acces protection
138 static SEMAPHORE_DECL(we_ukf_sem, 0); // semaaphore for sending signal
139 #endif
140 
141 /*----------------init_calculator---------------------*/
142 /* Init fonction to init different type of variable */
143 /* for the calculator before to calculate */
144 /*----------------------------------------------------*/
145 void init_calculator(void)
146 {
148 
149  // FIXME would be better to force Matlab to do this in initialize function
150  // zero input vector
151  memset(&ukf_U, 0, sizeof(ExtU));
152  // zero output vector
153  memset(&ukf_Y, 0, sizeof(ExtY));
154  // zero internal structure
155  memset(&ukf_DW, 0, sizeof(DW));
156  // zero init structure
157  memset(&ukf_init, 0, sizeof(ukf_init_type));
158  // zero params structure
159  memset(&ukf_params, 0, sizeof(ukf_params_type));
160 
161  ukf_init.x0[6] = 1.0f; // initial airspeed scale factor
162 
163  MAT_EL(ukf_init.P0, 0, 0, 7) = WE_UKF_P0;
164  MAT_EL(ukf_init.P0, 1, 1, 7) = WE_UKF_P0;
165  MAT_EL(ukf_init.P0, 2, 2, 7) = WE_UKF_P0;
166  MAT_EL(ukf_init.P0, 3, 3, 7) = WE_UKF_P0;
167  MAT_EL(ukf_init.P0, 4, 4, 7) = WE_UKF_P0;
168  MAT_EL(ukf_init.P0, 5, 5, 7) = WE_UKF_P0;
169  MAT_EL(ukf_init.P0, 6, 6, 7) = WE_UKF_P0;
170 
171  MAT_EL(ukf_params.R, 0, 0, 6) = powf(WE_UKF_R_GS, 2);
172  MAT_EL(ukf_params.R, 1, 1, 6) = powf(WE_UKF_R_GS, 2);
173  MAT_EL(ukf_params.R, 2, 2, 6) = powf(WE_UKF_R_GS, 2);
174  MAT_EL(ukf_params.R, 3, 3, 6) = powf(WE_UKF_R_VA, 2);
175  MAT_EL(ukf_params.R, 4, 4, 6) = powf(WE_UKF_R_AOA, 2);
176  MAT_EL(ukf_params.R, 5, 5, 6) = powf(WE_UKF_R_SSA, 2);
177 
178  MAT_EL(ukf_params.Q, 0, 0, 7) = powf(WE_UKF_Q_VA, 2);
179  MAT_EL(ukf_params.Q, 1, 1, 7) = powf(WE_UKF_Q_VA, 2);
180  MAT_EL(ukf_params.Q, 2, 2, 7) = powf(WE_UKF_Q_VA, 2);
181  MAT_EL(ukf_params.Q, 3, 3, 7) = powf(WE_UKF_Q_WIND, 2);
182  MAT_EL(ukf_params.Q, 4, 4, 7) = powf(WE_UKF_Q_WIND, 2);
183  MAT_EL(ukf_params.Q, 5, 5, 7) = powf(WE_UKF_Q_WIND, 2);
184  MAT_EL(ukf_params.Q, 6, 6, 7) = powf(WE_UKF_Q_VA_SCALE, 2);
185 
189  ukf_params.dt = WIND_ESTIMATOR_PERIODIC_PERIOD; // actually measured later
190 
192  wind_estimator.reset = false;
193 
201 
202  time_step_before = 0;
203 }
204 
205 // Run one step and save result
206 static inline void wind_estimator_step(void)
207 {
208 #if LOG_WIND_ESTIMATOR
209  if (LogFileIsOpen()) {
210  if (!log_we_started) {
211  // print header with initial parameters
212  int i;
213  PrintLog(pprzLogFile, "# Wind Estimator\n#\n");
214  PrintLog(pprzLogFile, "# Q = diag( ");
215  for (i = 0; i < 7; i++)
216  PrintLog(pprzLogFile, "%.8f ", MAT_EL(ukf_params.Q, i, i, 7));
217  PrintLog(pprzLogFile, ")\n");
218  PrintLog(pprzLogFile, "# R = diag( ");
219  for (i = 0; i < 5; i++)
220  PrintLog(pprzLogFile, "%.8f ", MAT_EL(ukf_params.R, i, i, 5));
221  PrintLog(pprzLogFile, ")\n");
222  PrintLog(pprzLogFile, "# ki = %.5f\n", ukf_init.ki);
223  PrintLog(pprzLogFile, "# alpha = %.5f\n", ukf_init.alpha);
224  PrintLog(pprzLogFile, "# beta = %.5f\n", ukf_init.beta);
225  PrintLog(pprzLogFile, "#\n");
226  PrintLog(pprzLogFile, "p q r ax ay az q1 q2 q3 q4 vkx vky vkz va aoa ssa u v w wx wy wz vas t\n");
227  log_we_started = true;
228  }
229  PrintLog(pprzLogFile, "%.5f %.5f %.5f %.3f %.3f %.3f %.4f %.4f %.4f %.4f %.5f %.5f %.5f %.5f %.5f %.5f ",
230  ukf_U.rates[0],
231  ukf_U.rates[1],
232  ukf_U.rates[2],
233  ukf_U.accel[0],
234  ukf_U.accel[1],
235  ukf_U.accel[2],
236  ukf_U.q[0],
237  ukf_U.q[1],
238  ukf_U.q[2],
239  ukf_U.q[3],
240  ukf_U.vk[0],
241  ukf_U.vk[1],
242  ukf_U.vk[2],
243  ukf_U.va,
244  ukf_U.aoa,
246  );
247  }
248 #endif
249 
250  // estimate wind if airspeed is high enough
251  if (ukf_U.va > 5.0f) {
252  // run estimation
254  // update output structure
261  // set ready flag
263  } else {
264  wind_estimator.airspeed.x = 0.f;
265  wind_estimator.airspeed.y = 0.f;
266  wind_estimator.airspeed.z = 0.f;
267  wind_estimator.wind.x = 0.f;
268  wind_estimator.wind.y = 0.f;
269  wind_estimator.wind.z = 0.f;
270  }
271 
272 #if LOG_WIND_ESTIMATOR
273  if (log_we_started) {
274  PrintLog(pprzLogFile, "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %d\n",
281  ukf_Y.xout[6],
283  );
284  }
285 #endif
286 }
287 
288 #include "subsystems/imu.h"
289 /*----------------wind_estimator_periodic-------------*/
290 /* Put Data from State in struct use by the Thread */
291 /*----------------------------------------------------*/
293 {
294  // try to lock mutex without blocking
295  // if it fails, it means that previous computation took too long
296 #ifndef SITL
297  if (chMtxTryLock(&we_ukf_mtx)) {
298 #endif
299  if (wind_estimator.reset) {
300  init_calculator();
301  }
302  // update input vector from state interface
303  ukf_U.rates[0] = stateGetBodyRates_f()->p; // rad/s
304  ukf_U.rates[1] = stateGetBodyRates_f()->q; // rad/s
305  ukf_U.rates[2] = stateGetBodyRates_f()->r; // rad/s
306  // transform data in body frame
307  struct FloatVect3 accel_ned = {
311  };
312 // struct FloatRMat *ned_to_body = stateGetNedToBodyRMat_f();
313 // struct FloatVect3 accel_body;
314 // float_rmat_vmult(&accel_body, ned_to_body, &accel_ned);
315 
317  struct FloatVect3 accel_body = {
321  };
322  struct FloatVect3 tmp = accel_body;
323  //struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&body_to_imu);
324  //int32_rmat_transp_vmult(&accel_body, body_to_imu_rmat, accel);
325  // tilt and remove gravity
326  struct FloatVect3 gravity_ned = { 0.f, 0.f, 9.81f };
327  struct FloatVect3 gravity_body = { 0.f, 0.f, 0.f };
328 #if 0
329  struct FloatRMat *ned_to_body_rmat = stateGetNedToBodyRMat_f();
330  float_rmat_vmult(&gravity_body, ned_to_body_rmat, &gravity_ned);
331  VECT3_ADD(accel_body, gravity_body);
332 #else
333  struct FloatEulers *b2i_e = stateGetNedToBodyEulers_f();
334  accel_body.x += -9.81*sinf(b2i_e->theta);
335  accel_body.y += 9.81*cosf(b2i_e->theta)*sinf(b2i_e->phi);
336  accel_body.z += 9.81*cosf(b2i_e->theta)*cosf(b2i_e->phi);
337 #endif
338 
340  ukf_U.accel[0] = accel_body.x; // m/s^2
341  ukf_U.accel[1] = accel_body.y; // m/s^2
342  ukf_U.accel[2] = accel_body.z; // m/s^2
347  ukf_U.vk[0] = stateGetSpeedNed_f()->x; // m/s
348  ukf_U.vk[1] = stateGetSpeedNed_f()->y; // m/s
349  ukf_U.vk[2] = stateGetSpeedNed_f()->z; // m/s
350  ukf_U.va = stateGetAirspeed_f(); // m/s
351  ukf_U.aoa = stateGetAngleOfAttack_f(); // rad.
352  ukf_U.sideslip = stateGetSideslip_f(); // rad.
353 
355  // update input vector from state interface
356  //ukf_U.rates[0] = 0.;
357  //ukf_U.rates[1] = 0.;
358  //ukf_U.rates[2] = 0.;
359  //ukf_U.accel[0] = 0.;
360  //ukf_U.accel[1] = 0.;
361  //ukf_U.accel[2] = 0.;
362  //ukf_U.q[0] = 1.0;
363  //ukf_U.q[1] = 0.;
364  //ukf_U.q[2] = 0.;
365  //ukf_U.q[3] = 0.;
366  //ukf_U.vk[0] = 15.;
367  //ukf_U.vk[1] = 0.;
368  //ukf_U.vk[2] = 0.;
369  //ukf_U.va = 15.;
370  //ukf_U.aoa = 0.;
371  //ukf_U.sideslip = 0.;
372 
373  //DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, 16, (float *)(&ukf_U));
374 
375  float msg[] = {
376  tmp.x,
377  tmp.y,
378  tmp.z,
379  gravity_body.x,
380  gravity_body.y,
381  gravity_body.z,
382  accel_body.x,
383  accel_body.y,
384  accel_body.z,
385  accel_ned.x,
386  accel_ned.y,
387  accel_ned.z
388  };
389  DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, 12, msg);
390 
391  // compute DT and set input vector
392  if (time_step_before == 0) {
393  ukf_params.dt = WIND_ESTIMATOR_PERIODIC_PERIOD;
395  } else {
398  }
399 #ifndef SITL
400  chMtxUnlock(&we_ukf_mtx);
401  // send signal to computation thread
402  chSemSignal(&we_ukf_sem);
403  }
404 #else
406 #endif
407 }
408 
409 /*----------------- wind_estimator_event -------------*/
410 /* Fonction put data in State */
411 /*----------------------------------------------------*/
413 {
415 #ifndef SITL
416  if (chMtxTryLock(&we_ukf_mtx)) {
417 #endif
418  struct FloatVect2 wind_ne = {
421  };
422  stateSetHorizontalWindspeed_f(&wind_ne); //NEED CHECK
424 
425  // TODO do something with corrected airspeed norm
426 
427 #if SEND_WIND_ESTIMATOR
428  // send over telemetry
429  send_wind_estimator(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
430 #endif
431 
433 #ifndef SITL
434  chMtxUnlock(&we_ukf_mtx);
435  }
436 #endif
437  }
438 }
439 
440 /*------------------thd_windestimate------------------*/
441 /* Thread fonction */
442 /*----------------------------------------------------*/
443 #ifndef SITL
444 static void thd_windestimate(void *arg)
445 {
446  (void) arg;
447  chRegSetThreadName("wind estimation");
448 
449  while (true) {
450  // wait for incoming request signal
451  chSemWait(&we_ukf_sem);
452  // lock state
453  chMtxLock(&we_ukf_mtx);
454  // run estimation step
456  // unlock
457  chMtxUnlock(&we_ukf_mtx);
458  }
459 }
460 #endif
461 
462 /*-----------------wind_estimator_init----------------*/
463 /* Init the Thread and the calculator */
464 /*----------------------------------------------------*/
466 {
467  init_calculator();
468 
469 #if LOG_WIND_ESTIMATOR
470  log_we_started = false;
471 #if SITL
472  // open log file for writing
473  // path should be specified in airframe file
474  uint32_t counter = 0;
475  char filename[512];
476  snprintf(filename, 512, "%s/we_ukf_%05d.csv", STRINGIFY(WE_LOG_PATH), counter);
477  // check availale name
478  while ((pprzLogFile = fopen(filename, "r"))) {
479  fclose(pprzLogFile);
480  snprintf(filename, 512, "%s/we_ukf_%05d.csv", STRINGIFY(WE_LOG_PATH), ++counter);
481  }
482  pprzLogFile = fopen(filename, "w");
483  if (pprzLogFile == NULL) {
484  printf("Failed to open WE log file '%s'\n",filename);
485  } else {
486  printf("Opening WE log file '%s'\n",filename);
487  }
488 #endif
489 #endif
490 
491 #ifndef SITL
492  // Start wind estimation thread
493  chThdCreateStatic(wa_thd_windestimation, sizeof(wa_thd_windestimation),
494  NORMALPRIO + 2, thd_windestimate, NULL);
495 #endif
496 
497 #if PERIODIC_TELEMETRY
498  // register for periodic telemetry (for logging with flight recorder)
500 #endif
501 }
502 
504 {
505  wind_estimator.r_gs = _v;
506  MAT_EL(ukf_params.R, 0, 0, 6) = powf(_v, 2);
507  MAT_EL(ukf_params.R, 1, 1, 6) = powf(_v, 2);
508  MAT_EL(ukf_params.R, 2, 2, 6) = powf(_v, 2);
509 }
510 
512 {
513  wind_estimator.r_va = _v;
514  MAT_EL(ukf_params.R, 3, 3, 6) = powf(_v, 2);
515 }
516 
518 {
519  wind_estimator.r_aoa = _v;
520  MAT_EL(ukf_params.R, 4, 4, 6) = powf(_v, 2);
521 }
522 
524 {
525  wind_estimator.r_ssa = _v;
526  MAT_EL(ukf_params.R, 5, 5, 6) = powf(_v, 2);
527 }
528 
530 {
531  wind_estimator.q_va = _v;
532  MAT_EL(ukf_params.Q, 0, 0, 7) = powf(_v, 2);
533  MAT_EL(ukf_params.Q, 1, 1, 7) = powf(_v, 2);
534  MAT_EL(ukf_params.Q, 2, 2, 7) = powf(_v, 2);
535 }
536 
538 {
539  wind_estimator.q_wind = _v;
540  MAT_EL(ukf_params.Q, 3, 3, 7) = powf(_v, 2);
541  MAT_EL(ukf_params.Q, 4, 4, 7) = powf(_v, 2);
542  MAT_EL(ukf_params.Q, 5, 5, 7) = powf(_v, 2);
543 }
544 
546 {
548  MAT_EL(ukf_params.Q, 6, 6, 7) = powf(_v, 2);
549 }
550 
void wind_estimator_Set_R_AOA(float _v)
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:147
float q_va
noise associated to airspeed vector model
void init_calculator(void)
bool data_available
new data available
#define WE_UKF_BETA
void wind_estimator_Set_Q_VA_SCALE(float _v)
float y
in meters
float phi
in radians
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
real32_T aoa
static MUTEX_DECL(we_ukf_mtx)
Periodic telemetry system header (includes downlink utility and generated code).
float r_aoa
noise associated to angle of attack measurement
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
Definition: sys_time_arch.c:78
float r
in rad/s
static void wind_estimator_step(void)
void UKF_Wind_Estimator_step(void)
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1407
float q_va_scale
noise associated to airspeed scale factor model
struct WindEstimator wind_estimator
#define WE_UKF_Q_WIND
real32_T x0[7]
void wind_estimator_init(void)
float r_ssa
noise associated to sideslip angle measurement
float q_wind
noise associated to wind vector model
struct Imu imu
global IMU state
Definition: imu.c:108
float q
in rad/s
float p
in rad/s
void wind_estimator_Set_R_VA(float _v)
real32_T sideslip
void wind_estimator_Set_Q_WIND(float _v)
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition: state.h:1131
euler angles
float z
in meters
static struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
Definition: state.h:1137
static float float_vect3_norm(struct FloatVect3 *v)
Paparazzi floating point math for geodetic calculations.
float theta
in radians
static void send_wind_estimator(struct transport_tx *trans, struct link_device *dev)
void wind_estimator_Set_Q_VA(float _v)
void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b, struct FloatVect3 *va)
rotate 3D vector by rotation matrix.
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:39
#define WE_UKF_R_SSA
ExtU ukf_U
real32_T xout[7]
Paparazzi floating point algebra.
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition: state.h:908
Architecture independent timing functions.
static struct NedCoor_f * stateGetAccelNed_f(void)
Get acceleration in NED coordinates (float).
Definition: state.h:1038
#define ACCEL_FLOAT_OF_BFP(_ai)
Wind Estimator based on generated library from Matlab.
static uint32_t time_step_before
unsigned long uint32_t
Definition: types.h:18
int32_t counter
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
static SEMAPHORE_DECL(we_ukf_sem, 0)
#define WE_UKF_KI
Default parameters.
real32_T vk[3]
FileDes pprzLogFile
Definition: sdlog_chibios.c:85
#define WE_UKF_Q_VA_SCALE
real32_T q[4]
Inertial Measurement Unit interface.
static float stateGetAngleOfAttack_f(void)
Get angle of attack (float).
Definition: state.h:1416
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1200
void wind_estimator_Set_R_SSA(float _v)
bool reset
reset filter flag
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
#define WE_UKF_R_VA
#define WE_UKF_R_GS
#define WE_UKF_R_AOA
real32_T P0[49]
void UKF_Wind_Estimator_initialize(void)
static void stateSetVerticalWindspeed_f(float v_windspeed)
Set vertical windspeed (float).
Definition: state.h:1300
unsigned char uint8_t
Definition: types.h:14
ukf_init_type ukf_init
API to get/set the generic vehicle states.
void wind_estimator_periodic(void)
float r_gs
noise associated to ground speed measurement
static float stateGetSideslip_f(void)
Get sideslip (float).
Definition: state.h:1425
struct FloatVect3 airspeed
airspeed vector in body frame
static void thd_windestimate(void *arg)
static THD_WORKING_AREA(wa_thd_windestimation, 8 *1024)
#define WE_UKF_ALPHA
#define MAT_EL(_m, _l, _c, _n)
rotation matrix
#define WE_UKF_Q_VA
ukf_params_type ukf_params
real32_T rates[3]
real32_T accel[3]
void wind_estimator_event(void)
ExtY ukf_Y
uint8_t msg[10]
Buffer used for general comunication over SPI (out buffer)
struct FloatVect3 wind
wind vector in NED frame
real32_T va
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
#define WE_UKF_P0
float x
in meters
float r_va
noise associated to airspeed norm measurement
void wind_estimator_Set_R_GS(float _v)
DW ukf_DW
static void stateSetHorizontalWindspeed_f(struct FloatVect2 *h_windspeed)
Set horizontal windspeed (float).
Definition: state.h:1291