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_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 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.