Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
guidance_v_adpt.h File Reference

Adaptation bloc of the vertical guidance. More...

#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_X0   0.003
 Initial estimation. More...
 
#define GUIDANCE_V_ADAPT_MAX_ACCEL   4.0
 Filter is not fed if accel values are more than +/- MAX_ACCEL. More...
 
#define GUIDANCE_V_ADAPT_MAX_CMD   0.9
 Filter is not fed if command values are out of a % of 0/MAX_PPRZ. 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   GUIDANCE_V_ADAPT_X0
 
#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...
 

Detailed Description

Adaptation bloc of the vertical guidance.

This is a dimension one kalman filter estimating the ratio of vertical acceleration over thrust command ( ~ invert of the mass ) needed by the invert dynamic model to produce a nominal command

Definition in file guidance_v_adpt.h.

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 57 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 64 of file guidance_v_adpt.h.

#define GUIDANCE_V_ADAPT_MIN_CMD   0.1

Definition at line 67 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 GUIDANCE_V_ADAPT_X0   0.003

Initial estimation.

The initial value can be adapted for faster converging time. It is usually recommended to start with a low value (overestimation of the mass), as it is helping for a smooth takeoff.

Definition at line 50 of file guidance_v_adpt.h.

#define GV_ADAPT_MAX_ACCEL   ACCEL_BFP_OF_REAL(GUIDANCE_V_ADAPT_MAX_ACCEL)

Definition at line 113 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 116 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 130 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 109 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 108 of file guidance_v_adpt.h.

#define GV_ADAPT_MEAS_NOISE_OF_ZD   (100.0*GUIDANCE_V_ADAPT_NOISE_FACTOR)

Definition at line 110 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 117 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 131 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 99 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 98 of file guidance_v_adpt.h.

#define GV_ADAPT_P_FRAC   18

Definition at line 82 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 105 of file guidance_v_adpt.h.

Referenced by gv_adapt_run().

#define GV_ADAPT_SYS_NOISE_F   0.00005

Definition at line 103 of file guidance_v_adpt.h.

#define GV_ADAPT_X0   BFP_OF_REAL(GV_ADAPT_X0_F, GV_ADAPT_X_FRAC)

Definition at line 97 of file guidance_v_adpt.h.

Referenced by gv_adapt_init().

#define GV_ADAPT_X0_F   GUIDANCE_V_ADAPT_X0

Definition at line 96 of file guidance_v_adpt.h.

#define GV_ADAPT_X_FRAC   24

Definition at line 75 of file guidance_v_adpt.h.

Referenced by gv_adapt_run(), and run_hover_loop().

#define K_FRAC   12

Definition at line 139 of file guidance_v_adpt.h.

Referenced by gv_adapt_run().

Function Documentation

static void gv_adapt_init ( void  )
inlinestatic

Definition at line 134 of file guidance_v_adpt.h.

References gv_adapt_P, GV_ADAPT_P0, gv_adapt_X, and GV_ADAPT_X0.

Referenced by guidance_v_init(), 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 146 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 91 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 90 of file guidance_v_adpt.h.

Referenced by gv_adapt_init(), gv_adapt_run(), and run_hover_loop().

int32_t gv_adapt_Xmeas

Measurement.

Definition at line 92 of file guidance_v_adpt.h.

Referenced by gv_adapt_run().