Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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 
27 #include "modules/meteo/wind_estimator.h"
28 #include "modules/meteo/lib_ukf_wind_estimator/UKF_Wind_Estimator.h"
29 #include "mcu_periph/sys_time.h"
30 #include "math/pprz_algebra_float.h"
31 #include "math/pprz_geodetic_float.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 
84 #include "subsystems/datalink/downlink.h"
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
100 #include "subsystems/datalink/telemetry.h"
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
109 #include "modules/loggers/sdlog_chibios.h"
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
125 struct WindEstimator wind_estimator;
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 {
147  UKF_Wind_Estimator_initialize();
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 
186  ukf_init.ki = WE_UKF_KI;
187  ukf_init.alpha = WE_UKF_ALPHA;
188  ukf_init.beta = WE_UKF_BETA;
189  ukf_params.dt = WIND_ESTIMATOR_PERIODIC_PERIOD; // actually measured later
190 
191  wind_estimator.data_available = false;
192  wind_estimator.reset = false;
193 
194  wind_estimator.r_gs = WE_UKF_R_GS;
195  wind_estimator.r_va = WE_UKF_R_VA;
196  wind_estimator.r_aoa = WE_UKF_R_AOA;
197  wind_estimator.r_ssa = WE_UKF_R_SSA;
198  wind_estimator.q_va = WE_UKF_Q_VA;
199  wind_estimator.q_wind = WE_UKF_Q_WIND;
200  wind_estimator.q_va_scale = WE_UKF_Q_VA_SCALE;
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,
245  ukf_U.sideslip
246  );
247  }
248 #endif
249 
250  // estimate wind if airspeed is high enough
251  if (ukf_U.va > 5.0f) {
252  // run estimation
253  UKF_Wind_Estimator_step();
254  // update output structure
255  wind_estimator.airspeed.x = ukf_Y.xout[0];
256  wind_estimator.airspeed.y = ukf_Y.xout[1];
257  wind_estimator.airspeed.z = ukf_Y.xout[2];
258  wind_estimator.wind.x = ukf_Y.xout[3];
259  wind_estimator.wind.y = ukf_Y.xout[4];
260  wind_estimator.wind.z = ukf_Y.xout[5];
261  // set ready flag
262  wind_estimator.data_available = true;
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",
275  wind_estimator.airspeed.x,
276  wind_estimator.airspeed.y,
277  wind_estimator.airspeed.z,
278  wind_estimator.wind.x,
279  wind_estimator.wind.y,
280  wind_estimator.wind.z,
281  ukf_Y.xout[6],
282  time_step_before
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 /*----------------------------------------------------*/
292 void wind_estimator_periodic(void)
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 = {
308  stateGetAccelNed_f()->x,
309  stateGetAccelNed_f()->y,
310  stateGetAccelNed_f()->z
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 = {
318  .x = ACCEL_FLOAT_OF_BFP(imu.accel.x),
319  .y = ACCEL_FLOAT_OF_BFP(imu.accel.y),
320  .z = ACCEL_FLOAT_OF_BFP(imu.accel.z)
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
343  ukf_U.q[0] = stateGetNedToBodyQuat_f()->qi;
344  ukf_U.q[1] = stateGetNedToBodyQuat_f()->qx;
345  ukf_U.q[2] = stateGetNedToBodyQuat_f()->qy;
346  ukf_U.q[3] = stateGetNedToBodyQuat_f()->qz;
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;
394  time_step_before = get_sys_time_msec();
395  } else {
396  ukf_params.dt = (get_sys_time_msec() - time_step_before) / 1000.f;
397  time_step_before = get_sys_time_msec();
398  }
399 #ifndef SITL
400  chMtxUnlock(&we_ukf_mtx);
401  // send signal to computation thread
402  chSemSignal(&we_ukf_sem);
403  }
404 #else
405  wind_estimator_step();
406 #endif
407 }
408 
409 /*----------------- wind_estimator_event -------------*/
410 /* Fonction put data in State */
411 /*----------------------------------------------------*/
412 void wind_estimator_event(void)
413 {
414  if (wind_estimator.data_available) {
415 #ifndef SITL
416  if (chMtxTryLock(&we_ukf_mtx)) {
417 #endif
418  struct FloatVect2 wind_ne = {
419  wind_estimator.wind.x,
420  wind_estimator.wind.y
421  };
422  stateSetHorizontalWindspeed_f(&wind_ne); //NEED CHECK
423  stateSetVerticalWindspeed_f(wind_estimator.wind.z); //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 
432  wind_estimator.data_available = false;
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
455  wind_estimator_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 /*----------------------------------------------------*/
465 void wind_estimator_init(void)
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)
499  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_WIND_INFO_RET, send_wind_estimator);
500 #endif
501 }
502 
503 void wind_estimator_Set_R_GS(float _v)
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 
511 void wind_estimator_Set_R_VA(float _v)
512 {
513  wind_estimator.r_va = _v;
514  MAT_EL(ukf_params.R, 3, 3, 6) = powf(_v, 2);
515 }
516 
517 void wind_estimator_Set_R_AOA(float _v)
518 {
519  wind_estimator.r_aoa = _v;
520  MAT_EL(ukf_params.R, 4, 4, 6) = powf(_v, 2);
521 }
522 
523 void wind_estimator_Set_R_SSA(float _v)
524 {
525  wind_estimator.r_ssa = _v;
526  MAT_EL(ukf_params.R, 5, 5, 6) = powf(_v, 2);
527 }
528 
529 void wind_estimator_Set_Q_VA(float _v)
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 
537 void wind_estimator_Set_Q_WIND(float _v)
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 
545 void wind_estimator_Set_Q_VA_SCALE(float _v)
546 {
547  wind_estimator.q_va_scale = _v;
548  MAT_EL(ukf_params.Q, 6, 6, 7) = powf(_v, 2);
549 }
550 
init_calculator
void init_calculator(void)
Definition: wind_estimator.c:145
FloatQuat::qz
float qz
Definition: pprz_algebra_float.h:67
ExtU::q
real32_T q[4]
Definition: UKF_Wind_Estimator.h:41
ukf_init_tag::alpha
real32_T alpha
Definition: UKF_Wind_Estimator.h:59
pprzLogFile
FileDes pprzLogFile
Definition: sdlog_chibios.c:86
WE_UKF_Q_VA_SCALE
#define WE_UKF_Q_VA_SCALE
Definition: wind_estimator.c:74
WE_UKF_BETA
#define WE_UKF_BETA
Definition: wind_estimator.c:53
NedCoor_f::z
float z
in meters
Definition: pprz_geodetic_float.h:66
WE_UKF_KI
#define WE_UKF_KI
Default parameters.
Definition: wind_estimator.c:47
WindEstimator::q_va_scale
float q_va_scale
noise associated to airspeed scale factor model
Definition: wind_estimator.h:48
ExtU
Definition: UKF_Wind_Estimator.h:38
ExtY
Definition: UKF_Wind_Estimator.h:49
Imu::accel
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:39
UKF_Wind_Estimator_step
void UKF_Wind_Estimator_step(void)
Definition: UKF_Wind_Estimator.c:1088
stateGetAccelNed_f
static struct NedCoor_f * stateGetAccelNed_f(void)
Get acceleration in NED coordinates (float).
Definition: state.h:1038
wind_estimator_Set_R_VA
void wind_estimator_Set_R_VA(float _v)
Definition: wind_estimator.c:511
wind_estimator_periodic
void wind_estimator_periodic(void)
Definition: wind_estimator.c:292
Int32Vect3::z
int32_t z
Definition: pprz_algebra_int.h:91
UKF_Wind_Estimator.h
WE_UKF_R_SSA
#define WE_UKF_R_SSA
Definition: wind_estimator.c:68
stateGetNedToBodyEulers_f
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
stateGetBodyRates_f
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1200
WindEstimator
Definition: wind_estimator.h:35
uint32_t
unsigned long uint32_t
Definition: types.h:18
THD_WORKING_AREA
static THD_WORKING_AREA(wa_thd_windestimation, 8 *1024)
WindEstimator::airspeed
struct FloatVect3 airspeed
airspeed vector in body frame
Definition: wind_estimator.h:36
WindEstimator::r_aoa
float r_aoa
noise associated to angle of attack measurement
Definition: wind_estimator.h:44
wind_estimator_step
static void wind_estimator_step(void)
Definition: wind_estimator.c:206
ukf_init_tag::P0
real32_T P0[49]
Definition: UKF_Wind_Estimator.h:57
UKF_Wind_Estimator_initialize
void UKF_Wind_Estimator_initialize(void)
Definition: UKF_Wind_Estimator.c:2098
stateSetHorizontalWindspeed_f
static void stateSetHorizontalWindspeed_f(struct FloatVect2 *h_windspeed)
Set horizontal windspeed (float).
Definition: state.h:1291
WE_UKF_Q_VA
#define WE_UKF_Q_VA
Definition: wind_estimator.c:71
WE_UKF_ALPHA
#define WE_UKF_ALPHA
Definition: wind_estimator.c:50
pprz_algebra_float.h
Paparazzi floating point algebra.
ukf_params
ukf_params_type ukf_params
Definition: UKF_Wind_Estimator.c:24
counter
int32_t counter
Definition: avoid_navigation.c:55
VECT3_ADD
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:147
ukf_init_tag::beta
real32_T beta
Definition: UKF_Wind_Estimator.h:60
ExtU::vk
real32_T vk[3]
Definition: UKF_Wind_Estimator.h:42
WindEstimator::q_va
float q_va
noise associated to airspeed vector model
Definition: wind_estimator.h:46
FloatEulers::theta
float theta
in radians
Definition: pprz_algebra_float.h:86
msg
uint8_t msg[10]
Buffer used for general comunication over SPI (out buffer)
Definition: high_speed_logger_direct_memory.c:134
sdlog_chibios.h
FloatVect2
Definition: pprz_algebra_float.h:49
FloatVect3
Definition: pprz_algebra_float.h:54
float_rmat_vmult
void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b, struct FloatVect3 *va)
rotate 3D vector by rotation matrix.
Definition: pprz_algebra_float.c:110
telemetry.h
imu.h
stateGetAngleOfAttack_f
static float stateGetAngleOfAttack_f(void)
Get angle of attack (float).
Definition: state.h:1416
pprz_geodetic_float.h
Paparazzi floating point math for geodetic calculations.
stateGetSpeedNed_f
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition: state.h:908
WindEstimator::wind
struct FloatVect3 wind
wind vector in NED frame
Definition: wind_estimator.h:37
wind_estimator_Set_R_GS
void wind_estimator_Set_R_GS(float _v)
Definition: wind_estimator.c:503
wind_estimator_Set_R_AOA
void wind_estimator_Set_R_AOA(float _v)
Definition: wind_estimator.c:517
ExtU::va
real32_T va
Definition: UKF_Wind_Estimator.h:43
float_vect3_norm
static float float_vect3_norm(struct FloatVect3 *v)
Definition: pprz_algebra_float.h:171
ACCEL_FLOAT_OF_BFP
#define ACCEL_FLOAT_OF_BFP(_ai)
Definition: pprz_algebra_int.h:221
ukf_init_tag::x0
real32_T x0[7]
Definition: UKF_Wind_Estimator.h:56
ukf_Y
ExtY ukf_Y
Definition: UKF_Wind_Estimator.c:33
FloatEulers::phi
float phi
in radians
Definition: pprz_algebra_float.h:85
WindEstimator::r_gs
float r_gs
noise associated to ground speed measurement
Definition: wind_estimator.h:42
stateGetNedToBodyQuat_f
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition: state.h:1131
WE_UKF_R_GS
#define WE_UKF_R_GS
Definition: wind_estimator.c:59
wind_estimator_event
void wind_estimator_event(void)
Definition: wind_estimator.c:412
stateGetAirspeed_f
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1407
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
stateSetVerticalWindspeed_f
static void stateSetVerticalWindspeed_f(float v_windspeed)
Set vertical windspeed (float).
Definition: state.h:1300
WE_UKF_R_AOA
#define WE_UKF_R_AOA
Definition: wind_estimator.c:65
sys_time.h
Architecture independent timing functions.
get_sys_time_msec
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
Definition: sys_time_arch.c:78
uint8_t
unsigned char uint8_t
Definition: types.h:14
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
FloatRates::r
float r
in rad/s
Definition: pprz_algebra_float.h:96
WindEstimator::data_available
bool data_available
new data available
Definition: wind_estimator.h:38
ukf_U
ExtU ukf_U
Definition: UKF_Wind_Estimator.c:30
Int32Vect3::y
int32_t y
Definition: pprz_algebra_int.h:90
ukf_params_tag::R
real32_T R[36]
Definition: UKF_Wind_Estimator.h:65
WE_UKF_Q_WIND
#define WE_UKF_Q_WIND
Definition: wind_estimator.c:77
thd_windestimate
static void thd_windestimate(void *arg)
Definition: wind_estimator.c:444
wind_estimator.h
ExtY::xout
real32_T xout[7]
Definition: UKF_Wind_Estimator.h:50
WindEstimator::q_wind
float q_wind
noise associated to wind vector model
Definition: wind_estimator.h:47
imu
struct Imu imu
global IMU state
Definition: imu.c:108
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
stateGetNedToBodyRMat_f
static struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
Definition: state.h:1137
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
time_step_before
static uint32_t time_step_before
Definition: wind_estimator.c:128
wind_estimator_Set_Q_VA
void wind_estimator_Set_Q_VA(float _v)
Definition: wind_estimator.c:529
wind_estimator
struct WindEstimator wind_estimator
Definition: wind_estimator.c:125
stateGetSideslip_f
static float stateGetSideslip_f(void)
Get sideslip (float).
Definition: state.h:1425
FloatQuat::qx
float qx
Definition: pprz_algebra_float.h:65
FloatRates::q
float q
in rad/s
Definition: pprz_algebra_float.h:95
NedCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:65
ExtU::accel
real32_T accel[3]
Definition: UKF_Wind_Estimator.h:40
ukf_init_tag::ki
real32_T ki
Definition: UKF_Wind_Estimator.h:58
wind_estimator_init
void wind_estimator_init(void)
Definition: wind_estimator.c:465
ukf_init_tag
Definition: UKF_Wind_Estimator.h:55
ExtU::rates
real32_T rates[3]
Definition: UKF_Wind_Estimator.h:39
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
wind_estimator_Set_Q_WIND
void wind_estimator_Set_Q_WIND(float _v)
Definition: wind_estimator.c:537
WE_UKF_P0
#define WE_UKF_P0
Definition: wind_estimator.c:56
WE_UKF_R_VA
#define WE_UKF_R_VA
Definition: wind_estimator.c:62
MAT_EL
#define MAT_EL(_m, _l, _c, _n)
Definition: wind_estimator.c:122
ukf_params_tag::dt
real32_T dt
Definition: UKF_Wind_Estimator.h:66
FloatQuat::qi
float qi
Definition: pprz_algebra_float.h:64
WindEstimator::reset
bool reset
reset filter flag
Definition: wind_estimator.h:39
WindEstimator::r_ssa
float r_ssa
noise associated to sideslip angle measurement
Definition: wind_estimator.h:45
ukf_init
ukf_init_type ukf_init
Definition: UKF_Wind_Estimator.c:23
Int32Vect3::x
int32_t x
Definition: pprz_algebra_int.h:89
ukf_params_tag
Definition: UKF_Wind_Estimator.h:63
FloatRMat
rotation matrix
Definition: pprz_algebra_float.h:77
FloatVect3::z
float z
Definition: pprz_algebra_float.h:57
NedCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:64
send_wind_estimator
static void send_wind_estimator(struct transport_tx *trans, struct link_device *dev)
Definition: wind_estimator.c:86
FloatEulers
euler angles
Definition: pprz_algebra_float.h:84
FloatQuat::qy
float qy
Definition: pprz_algebra_float.h:66
MUTEX_DECL
static MUTEX_DECL(we_ukf_mtx)
state.h
ukf_DW
DW ukf_DW
Definition: UKF_Wind_Estimator.c:27
wind_estimator_Set_Q_VA_SCALE
void wind_estimator_Set_Q_VA_SCALE(float _v)
Definition: wind_estimator.c:545
DW
Definition: UKF_Wind_Estimator.h:30
ExtU::sideslip
real32_T sideslip
Definition: UKF_Wind_Estimator.h:45
WindEstimator::r_va
float r_va
noise associated to airspeed norm measurement
Definition: wind_estimator.h:43
ExtU::aoa
real32_T aoa
Definition: UKF_Wind_Estimator.h:44
FloatRates::p
float p
in rad/s
Definition: pprz_algebra_float.h:94
wind_estimator_Set_R_SSA
void wind_estimator_Set_R_SSA(float _v)
Definition: wind_estimator.c:523
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
ukf_params_tag::Q
real32_T Q[49]
Definition: UKF_Wind_Estimator.h:64
SEMAPHORE_DECL
static SEMAPHORE_DECL(we_ukf_sem, 0)