Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
guidance_v_adpt.h File Reference
#include "paparazzi.h"
+ Include dependency graph for guidance_v_adpt.h:
+ This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Macros

#define GUIDANCE_V_ADAPT_NOISE_FACTOR   1.0
 Adapt noise factor. More...
 
#define GUIDANCE_V_ADAPT_MAX_ACCEL   4.0
 Filter is not fed if accel values are more than +/- MAX_ACCEL MAX_ACCEL is a positive value in m/s^2. More...
 
#define GUIDANCE_V_ADAPT_MAX_CMD   0.9
 Filter is not fed if command values are out of a % of 0/MAX_PPRZ MAX_CMD and MIN_CMD must be between 0 and 1 with MIN_CMD < MAX_CMD. More...
 
#define GUIDANCE_V_ADAPT_MIN_CMD   0.1
 
#define GV_ADAPT_X_FRAC   24
 
#define GV_ADAPT_P_FRAC   18
 
#define GV_ADAPT_X0_F   0.0015
 
#define GV_ADAPT_X0   BFP_OF_REAL(GV_ADAPT_X0_F, GV_ADAPT_X_FRAC)
 
#define GV_ADAPT_P0_F   0.1
 
#define GV_ADAPT_P0   BFP_OF_REAL(GV_ADAPT_P0_F, GV_ADAPT_P_FRAC)
 
#define GV_ADAPT_SYS_NOISE_F   0.00005
 
#define GV_ADAPT_SYS_NOISE   BFP_OF_REAL(GV_ADAPT_SYS_NOISE_F, GV_ADAPT_P_FRAC)
 
#define GV_ADAPT_MEAS_NOISE_HOVER_F   (50.0*GUIDANCE_V_ADAPT_NOISE_FACTOR)
 
#define GV_ADAPT_MEAS_NOISE_HOVER   BFP_OF_REAL(GV_ADAPT_MEAS_NOISE_HOVER_F, GV_ADAPT_P_FRAC)
 
#define GV_ADAPT_MEAS_NOISE_OF_ZD   (100.0*GUIDANCE_V_ADAPT_NOISE_FACTOR)
 
#define GV_ADAPT_MAX_ACCEL   ACCEL_BFP_OF_REAL(GUIDANCE_V_ADAPT_MAX_ACCEL)
 
#define GV_ADAPT_MAX_CMD   ((int32_t)(GUIDANCE_V_ADAPT_MAX_CMD*MAX_PPRZ))
 
#define GV_ADAPT_MIN_CMD   ((int32_t)(GUIDANCE_V_ADAPT_MIN_CMD*MAX_PPRZ))
 
#define GV_ADAPT_MAX_OUT   (BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC))
 
#define GV_ADAPT_MIN_OUT   (BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC) / MAX_PPRZ)
 
#define K_FRAC   12
 

Functions

static void gv_adapt_init (void)
 
static void gv_adapt_run (int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref)
 Adaptation function. More...
 

Variables

int32_t gv_adapt_X
 State of the estimator. More...
 
int32_t gv_adapt_P
 Covariance. More...
 
int32_t gv_adapt_Xmeas
 Measurement. More...
 

Macro Definition Documentation

#define GUIDANCE_V_ADAPT_MAX_ACCEL   4.0

Filter is not fed if accel values are more than +/- MAX_ACCEL MAX_ACCEL is a positive value in m/s^2.

Definition at line 48 of file guidance_v_adpt.h.

#define GUIDANCE_V_ADAPT_MAX_CMD   0.9

Filter is not fed if command values are out of a % of 0/MAX_PPRZ MAX_CMD and MIN_CMD must be between 0 and 1 with MIN_CMD < MAX_CMD.

Definition at line 55 of file guidance_v_adpt.h.

#define GUIDANCE_V_ADAPT_MIN_CMD   0.1

Definition at line 58 of file guidance_v_adpt.h.

#define GUIDANCE_V_ADAPT_NOISE_FACTOR   1.0

Adapt noise factor.

Smaller values will make the filter to adapter faster Bigger values (slower adaptation) make the filter more robust to external perturbations Factor should always be >0

Definition at line 41 of file guidance_v_adpt.h.

#define GV_ADAPT_MAX_ACCEL   ACCEL_BFP_OF_REAL(GUIDANCE_V_ADAPT_MAX_ACCEL)

Definition at line 102 of file guidance_v_adpt.h.

Referenced by gv_adapt_run().

#define GV_ADAPT_MAX_CMD   ((int32_t)(GUIDANCE_V_ADAPT_MAX_CMD*MAX_PPRZ))

Definition at line 105 of file guidance_v_adpt.h.

Referenced by gv_adapt_run().

#define GV_ADAPT_MAX_OUT   (BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC))

Definition at line 119 of file guidance_v_adpt.h.

Referenced by gv_adapt_run().

#define GV_ADAPT_MEAS_NOISE_HOVER   BFP_OF_REAL(GV_ADAPT_MEAS_NOISE_HOVER_F, GV_ADAPT_P_FRAC)

Definition at line 98 of file guidance_v_adpt.h.

Referenced by gv_adapt_run().

#define GV_ADAPT_MEAS_NOISE_HOVER_F   (50.0*GUIDANCE_V_ADAPT_NOISE_FACTOR)

Definition at line 97 of file guidance_v_adpt.h.

#define GV_ADAPT_MEAS_NOISE_OF_ZD   (100.0*GUIDANCE_V_ADAPT_NOISE_FACTOR)

Definition at line 99 of file guidance_v_adpt.h.

Referenced by gv_adapt_run().

#define GV_ADAPT_MIN_CMD   ((int32_t)(GUIDANCE_V_ADAPT_MIN_CMD*MAX_PPRZ))

Definition at line 106 of file guidance_v_adpt.h.

#define GV_ADAPT_MIN_OUT   (BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC) / MAX_PPRZ)

Definition at line 120 of file guidance_v_adpt.h.

Referenced by gv_adapt_run().

#define GV_ADAPT_P0   BFP_OF_REAL(GV_ADAPT_P0_F, GV_ADAPT_P_FRAC)

Definition at line 90 of file guidance_v_adpt.h.

Referenced by gv_adapt_init(), and gv_adapt_run().

#define GV_ADAPT_P0_F   0.1

Definition at line 89 of file guidance_v_adpt.h.

#define GV_ADAPT_P_FRAC   18

Definition at line 73 of file guidance_v_adpt.h.

Referenced by gv_adapt_run().

#define GV_ADAPT_SYS_NOISE   BFP_OF_REAL(GV_ADAPT_SYS_NOISE_F, GV_ADAPT_P_FRAC)

Definition at line 94 of file guidance_v_adpt.h.

Referenced by gv_adapt_run().

#define GV_ADAPT_SYS_NOISE_F   0.00005

Definition at line 93 of file guidance_v_adpt.h.

#define GV_ADAPT_X0   BFP_OF_REAL(GV_ADAPT_X0_F, GV_ADAPT_X_FRAC)

Definition at line 88 of file guidance_v_adpt.h.

Referenced by gv_adapt_init().

#define GV_ADAPT_X0_F   0.0015

Definition at line 87 of file guidance_v_adpt.h.

#define GV_ADAPT_X_FRAC   24

Definition at line 66 of file guidance_v_adpt.h.

Referenced by gv_adapt_run().

#define K_FRAC   12

Definition at line 128 of file guidance_v_adpt.h.

Referenced by gv_adapt_run().

Function Documentation

static void gv_adapt_init ( void  )
inlinestatic

Definition at line 123 of file guidance_v_adpt.h.

References gv_adapt_P, GV_ADAPT_P0, gv_adapt_X, and GV_ADAPT_X0.

Referenced by __attribute__(), and guidance_v_notify_in_flight().

+ Here is the caller graph for this function:

static void gv_adapt_run ( int32_t  zdd_meas,
int32_t  thrust_applied,
int32_t  zd_ref 
)
inlinestatic

Adaptation function.

Parameters
zdd_measvert accel measurement in m/s^2 with INT32_ACCEL_FRAC
thrust_appliedcontroller input [0 : MAX_PPRZ]
zd_refvertical speed reference in m/s with INT32_SPEED_FRAC

Definition at line 135 of file guidance_v_adpt.h.

References BFP_OF_REAL, E, GV_ADAPT_MAX_ACCEL, GV_ADAPT_MAX_CMD, GV_ADAPT_MAX_OUT, GV_ADAPT_MEAS_NOISE_HOVER, GV_ADAPT_MEAS_NOISE_OF_ZD, GV_ADAPT_MIN_OUT, gv_adapt_P, GV_ADAPT_P0, GV_ADAPT_P_FRAC, GV_ADAPT_SYS_NOISE, gv_adapt_X, GV_ADAPT_X_FRAC, gv_adapt_Xmeas, INT32_ACCEL_FRAC, INT32_SPEED_FRAC, K_FRAC, and ref.

Referenced by guidance_v_run().

+ Here is the caller graph for this function:

Variable Documentation

int32_t gv_adapt_P

Covariance.

fixed point representation with GV_ADAPT_P_FRAC Q13.18

Definition at line 82 of file guidance_v_adpt.h.

Referenced by gv_adapt_init(), and gv_adapt_run().

int32_t gv_adapt_X

State of the estimator.

fixed point representation with GV_ADAPT_X_FRAC Q13.18

Definition at line 81 of file guidance_v_adpt.h.

Referenced by gv_adapt_init(), and gv_adapt_run().

int32_t gv_adapt_Xmeas

Measurement.

Definition at line 83 of file guidance_v_adpt.h.

Referenced by gv_adapt_run().