Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
wind_estimator.c File Reference

Original Simulink files available at https://github.com/enacuavlab/UKF_Wind_Estimation. More...

#include "modules/meteo/wind_estimator.h"
#include "modules/meteo/lib_ukf_wind_estimator/UKF_Wind_Estimator.h"
#include "mcu_periph/sys_time.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_geodetic_float.h"
#include "generated/modules.h"
#include "state.h"
#include <string.h>
#include <ch.h>
#include <hal.h>
#include "modules/datalink/downlink.h"
#include "modules/datalink/telemetry.h"
#include "modules/imu/imu.h"
+ Include dependency graph for wind_estimator.c:

Go to the source code of this file.

Macros

#define WE_UKF_KI   0.f
 Default parameters. More...
 
#define WE_UKF_ALPHA   0.5f
 
#define WE_UKF_BETA   2.f
 
#define WE_UKF_P0   0.2f
 
#define WE_UKF_R_GS   0.5f
 
#define WE_UKF_R_VA   0.5f
 
#define WE_UKF_R_AOA   0.002f
 
#define WE_UKF_R_SSA   0.002f
 
#define WE_UKF_Q_VA   0.1f
 
#define WE_UKF_Q_VA_SCALE   0.0001f
 
#define WE_UKF_Q_WIND   0.001f
 
#define SEND_WIND_ESTIMATOR   TRUE
 
#define LOG_WIND_ESTIMATOR   FALSE
 
#define MAT_EL(_m, _l, _c, _n)   _m[_l + _c * _n]
 

Functions

static void send_wind_estimator (struct transport_tx *trans, struct link_device *dev)
 
static THD_WORKING_AREA (wa_thd_windestimation, 8 *1024)
 
static void thd_windestimate (void *arg)
 
static MUTEX_DECL (we_ukf_mtx)
 
static SEMAPHORE_DECL (we_ukf_sem, 0)
 
void init_calculator (void)
 
static void wind_estimator_step (void)
 
void wind_estimator_periodic (void)
 
void wind_estimator_event (void)
 
void wind_estimator_init (void)
 
void wind_estimator_Set_R_GS (float _v)
 
void wind_estimator_Set_R_VA (float _v)
 
void wind_estimator_Set_R_AOA (float _v)
 
void wind_estimator_Set_R_SSA (float _v)
 
void wind_estimator_Set_Q_VA (float _v)
 
void wind_estimator_Set_Q_WIND (float _v)
 
void wind_estimator_Set_Q_VA_SCALE (float _v)
 

Variables

struct WindEstimator wind_estimator
 
static uint32_t time_step_before
 

Detailed Description

Original Simulink files available at https://github.com/enacuavlab/UKF_Wind_Estimation.

Definition in file wind_estimator.c.

Macro Definition Documentation

◆ LOG_WIND_ESTIMATOR

#define LOG_WIND_ESTIMATOR   FALSE

Definition at line 104 of file wind_estimator.c.

◆ MAT_EL

#define MAT_EL (   _m,
  _l,
  _c,
  _n 
)    _m[_l + _c * _n]

Definition at line 122 of file wind_estimator.c.

◆ SEND_WIND_ESTIMATOR

#define SEND_WIND_ESTIMATOR   TRUE

Definition at line 81 of file wind_estimator.c.

◆ WE_UKF_ALPHA

#define WE_UKF_ALPHA   0.5f

Definition at line 50 of file wind_estimator.c.

◆ WE_UKF_BETA

#define WE_UKF_BETA   2.f

Definition at line 53 of file wind_estimator.c.

◆ WE_UKF_KI

#define WE_UKF_KI   0.f

Default parameters.

Definition at line 47 of file wind_estimator.c.

◆ WE_UKF_P0

#define WE_UKF_P0   0.2f

Definition at line 56 of file wind_estimator.c.

◆ WE_UKF_Q_VA

#define WE_UKF_Q_VA   0.1f

Definition at line 71 of file wind_estimator.c.

◆ WE_UKF_Q_VA_SCALE

#define WE_UKF_Q_VA_SCALE   0.0001f

Definition at line 74 of file wind_estimator.c.

◆ WE_UKF_Q_WIND

#define WE_UKF_Q_WIND   0.001f

Definition at line 77 of file wind_estimator.c.

◆ WE_UKF_R_AOA

#define WE_UKF_R_AOA   0.002f

Definition at line 65 of file wind_estimator.c.

◆ WE_UKF_R_GS

#define WE_UKF_R_GS   0.5f

Definition at line 59 of file wind_estimator.c.

◆ WE_UKF_R_SSA

#define WE_UKF_R_SSA   0.002f

Definition at line 68 of file wind_estimator.c.

◆ WE_UKF_R_VA

#define WE_UKF_R_VA   0.5f

Definition at line 62 of file wind_estimator.c.

Function Documentation

◆ init_calculator()

◆ MUTEX_DECL()

static MUTEX_DECL ( we_ukf_mtx  )
static

◆ SEMAPHORE_DECL()

static SEMAPHORE_DECL ( we_ukf_sem  ,
 
)
static

◆ send_wind_estimator()

static void send_wind_estimator ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Definition at line 86 of file wind_estimator.c.

References WindEstimator::airspeed, dev, float_vect3_norm(), WindEstimator::wind, wind_estimator, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by wind_estimator_event(), and wind_estimator_init().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ thd_windestimate()

static void thd_windestimate ( void *  arg)
static

Definition at line 443 of file wind_estimator.c.

References wind_estimator_step().

Referenced by wind_estimator_init().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ THD_WORKING_AREA()

static THD_WORKING_AREA ( wa_thd_windestimation  ,
8 *  1024 
)
static

◆ wind_estimator_event()

◆ wind_estimator_init()

void wind_estimator_init ( void  )

Definition at line 464 of file wind_estimator.c.

References counter, DefaultPeriodic, init_calculator(), pprzLogFile, register_periodic_telemetry(), send_wind_estimator(), and thd_windestimate().

+ Here is the call graph for this function:

◆ wind_estimator_periodic()

◆ wind_estimator_Set_Q_VA()

void wind_estimator_Set_Q_VA ( float  _v)

◆ wind_estimator_Set_Q_VA_SCALE()

void wind_estimator_Set_Q_VA_SCALE ( float  _v)

◆ wind_estimator_Set_Q_WIND()

void wind_estimator_Set_Q_WIND ( float  _v)

◆ wind_estimator_Set_R_AOA()

void wind_estimator_Set_R_AOA ( float  _v)

◆ wind_estimator_Set_R_GS()

void wind_estimator_Set_R_GS ( float  _v)

◆ wind_estimator_Set_R_SSA()

void wind_estimator_Set_R_SSA ( float  _v)

◆ wind_estimator_Set_R_VA()

void wind_estimator_Set_R_VA ( float  _v)

◆ wind_estimator_step()

Variable Documentation

◆ time_step_before

uint32_t time_step_before
static

Definition at line 128 of file wind_estimator.c.

Referenced by init_calculator(), wind_estimator_periodic(), and wind_estimator_step().

◆ wind_estimator