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_meas	vert accel measurement in m/s^2 with INT32_ACCEL_FRAC
thrust_applied	controller input [0 : MAX_PPRZ]
zd_ref	vertical 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.