Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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

◆ GUIDANCE_V_ADAPT_INITIAL_HOVER_THROTTLE

#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. If the nominal hover throttle is defined use it otherwise 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 47 of file guidance_v_adapt.c.

◆ GUIDANCE_V_ADAPT_MAX_ACCEL

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

◆ GUIDANCE_V_ADAPT_MAX_CMD

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

◆ GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE

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

◆ GUIDANCE_V_ADAPT_MIN_CMD

#define GUIDANCE_V_ADAPT_MIN_CMD   0.1

Definition at line 94 of file guidance_v_adapt.c.

◆ GUIDANCE_V_ADAPT_MIN_HOVER_THROTTLE

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

◆ GUIDANCE_V_ADAPT_NOISE_FACTOR

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

◆ GV_ADAPT_MEAS_NOISE_HOVER

#define GV_ADAPT_MEAS_NOISE_HOVER   BFP_OF_REAL(GV_ADAPT_MEAS_NOISE_HOVER_F, GV_ADAPT_P_FRAC)

Definition at line 111 of file guidance_v_adapt.c.

◆ GV_ADAPT_MEAS_NOISE_HOVER_F

#define GV_ADAPT_MEAS_NOISE_HOVER_F   (50.0*GUIDANCE_V_ADAPT_NOISE_FACTOR)

Definition at line 110 of file guidance_v_adapt.c.

◆ GV_ADAPT_MEAS_NOISE_OF_ZD

#define GV_ADAPT_MEAS_NOISE_OF_ZD   (100.0*GUIDANCE_V_ADAPT_NOISE_FACTOR)

Definition at line 112 of file guidance_v_adapt.c.

◆ GV_ADAPT_P0_F

#define GV_ADAPT_P0_F   0.1

Definition at line 115 of file guidance_v_adapt.c.

◆ GV_ADAPT_SYS_NOISE

#define GV_ADAPT_SYS_NOISE   BFP_OF_REAL(GV_ADAPT_SYS_NOISE_F, GV_ADAPT_P_FRAC)

Definition at line 107 of file guidance_v_adapt.c.

◆ GV_ADAPT_SYS_NOISE_F

#define GV_ADAPT_SYS_NOISE_F   0.00005

Definition at line 105 of file guidance_v_adapt.c.

◆ K_FRAC

#define K_FRAC   12

Definition at line 126 of file guidance_v_adapt.c.

Function Documentation

◆ gv_adapt_init()

void gv_adapt_init ( void  )

Definition at line 120 of file guidance_v_adapt.c.

References gv_adapt_P, gv_adapt_P0, gv_adapt_X, 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:

◆ gv_adapt_run()

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 133 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, gv_adapt_Xmeas, INT32_ACCEL_FRAC, INT32_SPEED_FRAC, K, K_FRAC, MAX_PPRZ, and ref.

Referenced by guidance_v_run().

+ Here is the caller graph for this function:

Variable Documentation

◆ gv_adapt_P

int32_t gv_adapt_P

Covariance.

fixed point representation with GV_ADAPT_P_FRAC Q13.18

Definition at line 100 of file guidance_v_adapt.c.

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

◆ gv_adapt_P0

const int32_t gv_adapt_P0 = BFP_OF_REAL(GV_ADAPT_P0_F, GV_ADAPT_P_FRAC)
static

Definition at line 116 of file guidance_v_adapt.c.

Referenced by gv_adapt_init(), and gv_adapt_run().

◆ gv_adapt_X

int32_t gv_adapt_X

State of the estimator.

fixed point representation with GV_ADAPT_X_FRAC Q13.18

Definition at line 99 of file guidance_v_adapt.c.

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

◆ gv_adapt_X0

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

Definition at line 117 of file guidance_v_adapt.c.

Referenced by gv_adapt_init().

◆ gv_adapt_Xmeas

int32_t gv_adapt_Xmeas

Measurement.

Definition at line 101 of file guidance_v_adapt.c.

Referenced by gv_adapt_run(), and send_vert_loop().