Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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.
#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)

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

◆ 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.

Referenced by init_calculator().

◆ WE_UKF_BETA

#define WE_UKF_BETA   2.f

Definition at line 53 of file wind_estimator.c.

Referenced by init_calculator().

◆ WE_UKF_KI

#define WE_UKF_KI   0.f

Default parameters.

Definition at line 47 of file wind_estimator.c.

Referenced by init_calculator().

◆ WE_UKF_P0

#define WE_UKF_P0   0.2f

Definition at line 56 of file wind_estimator.c.

Referenced by init_calculator().

◆ WE_UKF_Q_VA

#define WE_UKF_Q_VA   0.1f

Definition at line 71 of file wind_estimator.c.

Referenced by init_calculator().

◆ WE_UKF_Q_VA_SCALE

#define WE_UKF_Q_VA_SCALE   0.0001f

Definition at line 74 of file wind_estimator.c.

Referenced by init_calculator().

◆ WE_UKF_Q_WIND

#define WE_UKF_Q_WIND   0.001f

Definition at line 77 of file wind_estimator.c.

Referenced by init_calculator().

◆ WE_UKF_R_AOA

#define WE_UKF_R_AOA   0.002f

Definition at line 65 of file wind_estimator.c.

Referenced by init_calculator().

◆ WE_UKF_R_GS

#define WE_UKF_R_GS   0.5f

Definition at line 59 of file wind_estimator.c.

Referenced by init_calculator().

◆ WE_UKF_R_SSA

#define WE_UKF_R_SSA   0.002f

Definition at line 68 of file wind_estimator.c.

Referenced by init_calculator().

◆ WE_UKF_R_VA

#define WE_UKF_R_VA   0.5f

Definition at line 62 of file wind_estimator.c.

Referenced by init_calculator().

Function Documentation

◆ init_calculator()

void init_calculator ( void )

◆ MUTEX_DECL()

MUTEX_DECL ( we_ukf_mtx )
static

◆ SEMAPHORE_DECL()

SEMAPHORE_DECL ( we_ukf_sem ,
0  )
static

◆ send_wind_estimator()

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

Definition at line 86 of file wind_estimator.c.

References dev, float_vect3_norm(), and wind_estimator.

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()

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()

THD_WORKING_AREA ( wa_thd_windestimation ,
8 * 1024 )
static

◆ wind_estimator_event()

void wind_estimator_event ( void )

Definition at line 411 of file wind_estimator.c.

References DefaultChannel, DefaultDevice, send_wind_estimator(), stateSetHorizontalWindspeed_f(), stateSetVerticalWindspeed_f(), and wind_estimator.

Here is the call graph for this function:

◆ 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)

Definition at line 528 of file wind_estimator.c.

References MAT_EL, ukf_params, and wind_estimator.

◆ wind_estimator_Set_Q_VA_SCALE()

void wind_estimator_Set_Q_VA_SCALE ( float _v)

Definition at line 544 of file wind_estimator.c.

References MAT_EL, ukf_params, and wind_estimator.

◆ wind_estimator_Set_Q_WIND()

void wind_estimator_Set_Q_WIND ( float _v)

Definition at line 536 of file wind_estimator.c.

References MAT_EL, ukf_params, and wind_estimator.

◆ wind_estimator_Set_R_AOA()

void wind_estimator_Set_R_AOA ( float _v)

Definition at line 516 of file wind_estimator.c.

References MAT_EL, ukf_params, and wind_estimator.

◆ wind_estimator_Set_R_GS()

void wind_estimator_Set_R_GS ( float _v)

Definition at line 502 of file wind_estimator.c.

References MAT_EL, ukf_params, and wind_estimator.

◆ wind_estimator_Set_R_SSA()

void wind_estimator_Set_R_SSA ( float _v)

Definition at line 522 of file wind_estimator.c.

References MAT_EL, ukf_params, and wind_estimator.

◆ wind_estimator_Set_R_VA()

void wind_estimator_Set_R_VA ( float _v)

Definition at line 510 of file wind_estimator.c.

References MAT_EL, ukf_params, and wind_estimator.

◆ wind_estimator_step()

void wind_estimator_step ( void )
inlinestatic

Definition at line 206 of file wind_estimator.c.

References LogFileIsOpen, MAT_EL, pprzLogFile, time_step_before, ukf_init, ukf_params, ukf_U, UKF_Wind_Estimator_step(), ukf_Y, and wind_estimator.

Referenced by thd_windestimate(), and wind_estimator_periodic().

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

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