Paparazzi UAS  v6.1.0_stable
Paparazzi is a free software Unmanned Aircraft System.
Data Structures | Macros | Functions | Variables
wind_estimation_quadrotor.c File Reference
#include "modules/meteo/wind_estimation_quadrotor.h"
#include "filters/linear_kalman_filter.h"
#include "math/pprz_isa.h"
#include "state.h"
#include "generated/airframe.h"
#include "modules/datalink/downlink.h"
#include "generated/modules.h"
#include "modules/core/abi.h"
#include "modules/air_data/air_data.h"
#include "modules/datalink/telemetry.h"
+ Include dependency graph for wind_estimation_quadrotor.c:

Go to the source code of this file.

Data Structures

struct  wind_estimation_quadrotor
 

Macros

#define WE_QUAD_STATUS_IDLE   0
 
#define WE_QUAD_STATUS_RUN   1
 
#define WE_QUAD_STATE_SIZE   4
 
#define WE_QUAD_CMD_SIZE   2
 
#define WE_QUAD_MEAS_SIZE   2
 
#define WE_QUAD_VA_X   0
 
#define WE_QUAD_W_X   1
 
#define WE_QUAD_VA_Y   2
 
#define WE_QUAD_W_Y   3
 
#define WE_QUAD_P0_VA   1.f
 
#define WE_QUAD_P0_W   1.f
 
#define WE_QUAD_Q_VA   .05f
 
#define WE_QUAD_Q_W   .001f
 
#define WE_QUAD_R   .5f
 
#define WE_QUAD_UPDATE_STATE   TRUE
 

Functions

static void send_wind (struct transport_tx *trans, struct link_device *dev)
 
static void wind_estimation_quadrotor_reset (void)
 
void wind_estimation_quadrotor_init (void)
 
void wind_estimation_quadrotor_periodic (void)
 
void wind_estimation_quadrotor_stop (void)
 
void wind_estimation_quadrotor_start (void)
 
float wind_estimation_quadrotor_SetQva (float Q_va)
 
float wind_estimation_quadrotor_SetQw (float Q_w)
 
float wind_estimation_quadrotor_SetR (float R)
 
void wind_estimation_quadrotor_report (void)
 

Variables

static const float we_dt = WIND_ESTIMATION_QUADROTOR_PERIODIC_PERIOD
 
static struct wind_estimation_quadrotor we_quad
 
struct wind_estimation_quadrotor_params we_quad_params
 

Detailed Description

Author
Gautier Hattenberger gauti.nosp@m.er.h.nosp@m.atten.nosp@m.berg.nosp@m.er@en.nosp@m.ac.f.nosp@m.r Wind estimation from quadrotor motion

Definition in file wind_estimation_quadrotor.c.

Data Structure Documentation

◆ wind_estimation_quadrotor

struct wind_estimation_quadrotor

Definition at line 81 of file wind_estimation_quadrotor.c.

+ Collaboration diagram for wind_estimation_quadrotor:
Data Fields
struct linear_kalman_filter filter
uint8_t status

Macro Definition Documentation

◆ WE_QUAD_CMD_SIZE

#define WE_QUAD_CMD_SIZE   2

Definition at line 44 of file wind_estimation_quadrotor.c.

◆ WE_QUAD_MEAS_SIZE

#define WE_QUAD_MEAS_SIZE   2

Definition at line 45 of file wind_estimation_quadrotor.c.

◆ WE_QUAD_P0_VA

#define WE_QUAD_P0_VA   1.f

Definition at line 53 of file wind_estimation_quadrotor.c.

◆ WE_QUAD_P0_W

#define WE_QUAD_P0_W   1.f

Definition at line 57 of file wind_estimation_quadrotor.c.

◆ WE_QUAD_Q_VA

#define WE_QUAD_Q_VA   .05f

Definition at line 61 of file wind_estimation_quadrotor.c.

◆ WE_QUAD_Q_W

#define WE_QUAD_Q_W   .001f

Definition at line 65 of file wind_estimation_quadrotor.c.

◆ WE_QUAD_R

#define WE_QUAD_R   .5f

Definition at line 69 of file wind_estimation_quadrotor.c.

◆ WE_QUAD_STATE_SIZE

#define WE_QUAD_STATE_SIZE   4

Definition at line 43 of file wind_estimation_quadrotor.c.

◆ WE_QUAD_STATUS_IDLE

#define WE_QUAD_STATUS_IDLE   0

Definition at line 40 of file wind_estimation_quadrotor.c.

◆ WE_QUAD_STATUS_RUN

#define WE_QUAD_STATUS_RUN   1

Definition at line 41 of file wind_estimation_quadrotor.c.

◆ WE_QUAD_UPDATE_STATE

#define WE_QUAD_UPDATE_STATE   TRUE

Definition at line 74 of file wind_estimation_quadrotor.c.

◆ WE_QUAD_VA_X

#define WE_QUAD_VA_X   0

Definition at line 47 of file wind_estimation_quadrotor.c.

◆ WE_QUAD_VA_Y

#define WE_QUAD_VA_Y   2

Definition at line 49 of file wind_estimation_quadrotor.c.

◆ WE_QUAD_W_X

#define WE_QUAD_W_X   1

Definition at line 48 of file wind_estimation_quadrotor.c.

◆ WE_QUAD_W_Y

#define WE_QUAD_W_Y   3

Definition at line 50 of file wind_estimation_quadrotor.c.

Function Documentation

◆ send_wind()

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

Definition at line 213 of file wind_estimation_quadrotor.c.

References dev, wind_estimation_quadrotor::filter, float_vect2_norm(), we_quad, WE_QUAD_VA_X, WE_QUAD_VA_Y, WE_QUAD_W_X, WE_QUAD_W_Y, and linear_kalman_filter::X.

Referenced by wind_estimation_quadrotor_init().

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

◆ wind_estimation_quadrotor_init()

void wind_estimation_quadrotor_init ( void  )

Definition at line 98 of file wind_estimation_quadrotor.c.

References linear_kalman_filter::A, linear_kalman_filter::B, linear_kalman_filter::C, DefaultPeriodic, wind_estimation_quadrotor::filter, linear_kalman_filter_init(), linear_kalman_filter::Q, wind_estimation_quadrotor_params::Q_va, wind_estimation_quadrotor_params::Q_w, wind_estimation_quadrotor_params::R, linear_kalman_filter::R, register_periodic_telemetry(), send_wind(), wind_estimation_quadrotor::status, we_dt, we_quad, WE_QUAD_CMD_SIZE, WE_QUAD_MEAS_SIZE, we_quad_params, WE_QUAD_Q_VA, WE_QUAD_Q_W, WE_QUAD_R, WE_QUAD_STATE_SIZE, WE_QUAD_STATUS_IDLE, and wind_estimation_quadrotor_reset().

+ Here is the call graph for this function:

◆ wind_estimation_quadrotor_periodic()

void wind_estimation_quadrotor_periodic ( void  )

Definition at line 133 of file wind_estimation_quadrotor.c.

References AIRSPEED_WE_QUAD_ID, eas_from_tas(), wind_estimation_quadrotor::filter, float_rmat_transp_vmult(), float_vect2_norm(), linear_kalman_filter_predict(), linear_kalman_filter_update(), MAT33_ELMT, Max, PPRZ_ISA_GRAVITY, stateGetNedToBodyRMat_f(), stateGetSpeedNed_f(), stateSetHorizontalWindspeed_f(), we_quad, WE_QUAD_CMD_SIZE, WE_QUAD_MEAS_SIZE, WE_QUAD_VA_X, WE_QUAD_VA_Y, WE_QUAD_W_X, WE_QUAD_W_Y, linear_kalman_filter::X, FloatVect3::x, NedCoor_f::x, mesonh.mesonh_atmosphere::Y, FloatVect3::y, and NedCoor_f::y.

+ Here is the call graph for this function:

◆ wind_estimation_quadrotor_report()

void wind_estimation_quadrotor_report ( void  )

Definition at line 208 of file wind_estimation_quadrotor.c.

References DefaultChannel, DefaultDevice, wind_estimation_quadrotor::filter, we_quad, WE_QUAD_STATE_SIZE, and linear_kalman_filter::X.

◆ wind_estimation_quadrotor_reset()

static void wind_estimation_quadrotor_reset ( void  )
static

Definition at line 89 of file wind_estimation_quadrotor.c.

References wind_estimation_quadrotor::filter, float_vect_zero(), linear_kalman_filter::P, we_quad, WE_QUAD_P0_VA, WE_QUAD_P0_W, WE_QUAD_STATE_SIZE, and linear_kalman_filter::X.

Referenced by wind_estimation_quadrotor_init(), and wind_estimation_quadrotor_start().

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

◆ wind_estimation_quadrotor_SetQva()

float wind_estimation_quadrotor_SetQva ( float  Q_va)

Definition at line 184 of file wind_estimation_quadrotor.c.

References wind_estimation_quadrotor::filter, linear_kalman_filter::Q, wind_estimation_quadrotor_params::Q_va, we_quad, and we_quad_params.

◆ wind_estimation_quadrotor_SetQw()

float wind_estimation_quadrotor_SetQw ( float  Q_w)

Definition at line 192 of file wind_estimation_quadrotor.c.

References wind_estimation_quadrotor::filter, linear_kalman_filter::Q, wind_estimation_quadrotor_params::Q_w, we_quad, and we_quad_params.

◆ wind_estimation_quadrotor_SetR()

float wind_estimation_quadrotor_SetR ( float  R)

Definition at line 200 of file wind_estimation_quadrotor.c.

References wind_estimation_quadrotor::filter, wind_estimation_quadrotor_params::R, linear_kalman_filter::R, we_quad, and we_quad_params.

◆ wind_estimation_quadrotor_start()

void wind_estimation_quadrotor_start ( void  )

Definition at line 178 of file wind_estimation_quadrotor.c.

References wind_estimation_quadrotor::status, we_quad, WE_QUAD_STATUS_RUN, and wind_estimation_quadrotor_reset().

+ Here is the call graph for this function:

◆ wind_estimation_quadrotor_stop()

void wind_estimation_quadrotor_stop ( void  )

Definition at line 169 of file wind_estimation_quadrotor.c.

References stateSetHorizontalWindspeed_f(), wind_estimation_quadrotor::status, we_quad, and WE_QUAD_STATUS_IDLE.

+ Here is the call graph for this function:

Variable Documentation

◆ we_dt

const float we_dt = WIND_ESTIMATION_QUADROTOR_PERIODIC_PERIOD
static

Definition at line 77 of file wind_estimation_quadrotor.c.

Referenced by wind_estimation_quadrotor_init().

◆ we_quad

struct wind_estimation_quadrotor we_quad
static

Definition at line 86 of file wind_estimation_quadrotor.c.

Referenced by send_wind(), wind_estimation_quadrotor_init(), wind_estimation_quadrotor_periodic(), wind_estimation_quadrotor_report(), wind_estimation_quadrotor_reset(), wind_estimation_quadrotor_SetQva(), wind_estimation_quadrotor_SetQw(), wind_estimation_quadrotor_SetR(), wind_estimation_quadrotor_start(), and wind_estimation_quadrotor_stop().

◆ we_quad_params

struct wind_estimation_quadrotor_params we_quad_params

Definition at line 87 of file wind_estimation_quadrotor.c.

Referenced by wind_estimation_quadrotor_init(), wind_estimation_quadrotor_SetQva(), wind_estimation_quadrotor_SetQw(), and wind_estimation_quadrotor_SetR().