Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
guidance_v_adapt.c File Reference

Adaptation block of the vertical guidance. More...

#include "firmwares/rotorcraft/guidance/guidance_v_adapt.h"
#include "paparazzi.h"
#include "math/pprz_algebra_int.h"
#include "generated/airframe.h"
+ Include dependency graph for guidance_v_adapt.c:

Go to the source code of this file.

Macros

#define GUIDANCE_V_ADAPT_INITIAL_HOVER_THROTTLE   0.3
 Initial hover throttle as factor of MAX_PPRZ. More...
 
#define GUIDANCE_V_ADAPT_MIN_HOVER_THROTTLE   0.2
 Minimum hover throttle as factor of MAX_PPRZ. More...
 
#define GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE   0.75
 Maximum hover throttle as factor of MAX_PPRZ. More...
 
#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. 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_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_P0_F   0.1
 
#define K_FRAC   12
 

Functions

void gv_adapt_init (void)
 
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...
 
static const int32_t gv_adapt_P0 = BFP_OF_REAL(GV_ADAPT_P0_F, GV_ADAPT_P_FRAC)
 
static const int32_t gv_adapt_X0
 

Detailed Description

Adaptation block of the vertical guidance.

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

Definition in file guidance_v_adapt.c.

Macro Definition Documentation

#define GUIDANCE_V_ADAPT_INITIAL_HOVER_THROTTLE   0.3

Initial hover throttle as factor of MAX_PPRZ.

Should be a value between GUIDANCE_V_ADAPT_MIN_HOVER_THROTTLE and GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE. It is better to start with low thrust and let it rise as the adaptive filter finds the vehicle needs more thrust.

Definition at line 43 of file guidance_v_adapt.c.

#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 79 of file guidance_v_adapt.c.

Referenced by gv_adapt_run().

#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 86 of file guidance_v_adapt.c.

Referenced by gv_adapt_run().

#define GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE   0.75

Maximum hover throttle as factor of MAX_PPRZ.

With the default of 0.75 the nominal hover throttle will never go over 75% of max throttle.

Definition at line 61 of file guidance_v_adapt.c.

Referenced by gv_adapt_run().

#define GUIDANCE_V_ADAPT_MIN_CMD   0.1

Definition at line 89 of file guidance_v_adapt.c.

Referenced by gv_adapt_run().

#define GUIDANCE_V_ADAPT_MIN_HOVER_THROTTLE   0.2

Minimum hover throttle as factor of MAX_PPRZ.

With the default of 0.2 the nominal hover throttle will never go lower than 20%.

Definition at line 52 of file guidance_v_adapt.c.

Referenced by gv_adapt_run().

#define GUIDANCE_V_ADAPT_NOISE_FACTOR   1.0

Adapt noise factor.

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

Definition at line 71 of file guidance_v_adapt.c.

#define GV_ADAPT_MEAS_NOISE_HOVER   BFP_OF_REAL(GV_ADAPT_MEAS_NOISE_HOVER_F, GV_ADAPT_P_FRAC)

Definition at line 106 of file guidance_v_adapt.c.

Referenced by gv_adapt_run().

#define GV_ADAPT_MEAS_NOISE_HOVER_F   (50.0*GUIDANCE_V_ADAPT_NOISE_FACTOR)

Definition at line 105 of file guidance_v_adapt.c.

#define GV_ADAPT_MEAS_NOISE_OF_ZD   (100.0*GUIDANCE_V_ADAPT_NOISE_FACTOR)

Definition at line 107 of file guidance_v_adapt.c.

Referenced by gv_adapt_run().

#define GV_ADAPT_P0_F   0.1

Definition at line 110 of file guidance_v_adapt.c.

#define GV_ADAPT_SYS_NOISE   BFP_OF_REAL(GV_ADAPT_SYS_NOISE_F, GV_ADAPT_P_FRAC)

Definition at line 102 of file guidance_v_adapt.c.

Referenced by gv_adapt_run().

#define GV_ADAPT_SYS_NOISE_F   0.00005

Definition at line 100 of file guidance_v_adapt.c.

#define K_FRAC   12

Definition at line 121 of file guidance_v_adapt.c.

Referenced by gv_adapt_run().

Function Documentation

void gv_adapt_init ( void  )

Definition at line 115 of file guidance_v_adapt.c.

References gv_adapt_P0, and gv_adapt_X0.

Referenced by guidance_v_init(), guidance_v_notify_in_flight(), and guidance_v_run().

+ Here is the caller graph for this function:

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

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 128 of file guidance_v_adapt.c.

References ACCEL_BFP_OF_REAL, BFP_OF_REAL, E, GUIDANCE_V_ADAPT_MAX_ACCEL, GUIDANCE_V_ADAPT_MAX_CMD, GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE, GUIDANCE_V_ADAPT_MIN_CMD, GUIDANCE_V_ADAPT_MIN_HOVER_THROTTLE, GV_ADAPT_MEAS_NOISE_HOVER, GV_ADAPT_MEAS_NOISE_OF_ZD, gv_adapt_P, gv_adapt_P0, GV_ADAPT_P_FRAC, GV_ADAPT_SYS_NOISE, gv_adapt_X, GV_ADAPT_X_FRAC, INT32_ACCEL_FRAC, INT32_SPEED_FRAC, K_FRAC, MAX_PPRZ, 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 95 of file guidance_v_adapt.c.

Referenced by gv_adapt_run(), and send_vert_loop().

const int32_t gv_adapt_P0 = BFP_OF_REAL(GV_ADAPT_P0_F, GV_ADAPT_P_FRAC)
static

Definition at line 111 of file guidance_v_adapt.c.

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 94 of file guidance_v_adapt.c.

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

const int32_t gv_adapt_X0
static
Initial value:
#define GV_ADAPT_X_FRAC
#define BFP_OF_REAL(_vr, _frac)
#define MAX_PPRZ
Definition: paparazzi.h:8
#define GUIDANCE_V_ADAPT_INITIAL_HOVER_THROTTLE
Initial hover throttle as factor of MAX_PPRZ.

Definition at line 112 of file guidance_v_adapt.c.

Referenced by gv_adapt_init().

int32_t gv_adapt_Xmeas

Measurement.

Definition at line 96 of file guidance_v_adapt.c.

Referenced by send_vert_loop().