Fixed wing adaptive control. More...

#include "std.h"
#include "led.h"
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
#include "firmwares/fixedwing/stabilization/stabilization_adaptive.h"
#include "state.h"
#include "firmwares/fixedwing/nav.h"
#include "generated/airframe.h"
#include <CTRL_TYPE_H>
#include "firmwares/fixedwing/autopilot.h"

Include dependency graph for stabilization_adaptive.c:

Go to the source code of this file.

Data Structures
struct	HCtlAdaptRef

Macros
#define	H_CTL_REF_W_P 5.

#define	H_CTL_REF_XI_P 0.85

#define	H_CTL_REF_W_Q 5.

#define	H_CTL_REF_XI_Q 0.85

#define	H_CTL_REF_W_R 5.

#define	H_CTL_REF_XI_R 0.85

#define	H_CTL_REF_MAX_P RadOfDeg(150.)

#define	H_CTL_REF_MAX_P_DOT RadOfDeg(500.)

#define	H_CTL_REF_MAX_Q RadOfDeg(150.)

#define	H_CTL_REF_MAX_Q_DOT RadOfDeg(500.)

#define	AIRSPEED_RATIO_MIN 0.5

#define	AIRSPEED_RATIO_MAX 2.

#define	PITCH_TRIM_RATE_LIMITER 3.

#define	H_CTL_COURSE_PRE_BANK_CORRECTION 1.

#define	H_CTL_COURSE_DGAIN 0.

#define	H_CTL_ROLL_RATE_GAIN 0.

#define	H_CTL_ROLL_IGAIN 0.

#define	H_CTL_ROLL_KFFA 0.

#define	H_CTL_ROLL_KFFD 0.

#define	H_CTL_PITCH_DGAIN 0.

#define	H_CTL_PITCH_IGAIN 0.

#define	H_CTL_PITCH_KFFA 0.

#define	H_CTL_PITCH_KFFD 0.

#define	H_CTL_YAW_KFFD 0.

#define	USE_GYRO_PITCH_RATE TRUE

#define	H_CTL_REF_DT (1./CONTROL_FREQUENCY)
	Define reference generator time step default to control frequency and ahrs propagation freq if control is triggered by ahrs. More...

#define	KFFA_UPDATE 0.1
	Adaptive control tuning parameters activate with USE_KFF_UPDATE. More...

#define	KFFD_UPDATE 0.05

Functions
static void	h_ctl_roll_loop (void)

static void	h_ctl_pitch_loop (void)

void	h_ctl_init (void)

void	h_ctl_course_loop (void)

void	h_ctl_attitude_loop (void)

Variables
float	h_ctl_course_setpoint

float	h_ctl_course_pre_bank

float	h_ctl_course_pre_bank_correction

float	h_ctl_course_pgain

float	h_ctl_course_dgain

float	h_ctl_roll_max_setpoint

bool_t	h_ctl_disabled

bool_t	h_ctl_auto1_rate

struct HCtlAdaptRef	h_ctl_ref

float	h_ctl_roll_setpoint

float	h_ctl_roll_attitude_gain

float	h_ctl_roll_rate_gain

float	h_ctl_roll_igain

float	h_ctl_roll_sum_err

float	h_ctl_roll_Kffa

float	h_ctl_roll_Kffd

pprz_t	h_ctl_aileron_setpoint

float	h_ctl_roll_slew

float	h_ctl_roll_pgain

float	h_ctl_pitch_setpoint

float	h_ctl_pitch_loop_setpoint

float	h_ctl_pitch_pgain

float	h_ctl_pitch_dgain

float	h_ctl_pitch_igain

float	h_ctl_pitch_sum_err

float	h_ctl_pitch_Kffa

float	h_ctl_pitch_Kffd

pprz_t	h_ctl_elevator_setpoint

float	h_ctl_aileron_of_throttle

float	h_ctl_elevator_of_roll

float	h_ctl_pitch_of_roll

bool_t	use_airspeed_ratio

float	airspeed_ratio2

float	v_ctl_pitch_loiter_trim

float	v_ctl_pitch_dash_trim

Detailed Description

Fixed wing adaptive control.

to use the attitude referece generator define <define name="USE_ANGLE_REF">

to use the adaptive part for roll and/or pitch define <define name="USE_KFF_UPDATE_ROLL"> <define name="USE_KFF_UPDATE_PITCH">

to use the yaw damper <section name="AUTO1" prefix="AUTO1_"> <define name="MAX_YAW_RATE" value="RadOfDeg(100)"> </section>

in addition "ny" can be trimed to minimize the sideslip angle <section name="HORIZONTAL CONTROL" prefix="H_CTL_"> <define name="YAW_TRIM_NY" value="TRUE"> <define name="YAW_NY_IGAIN" value="5000."> </section>

to use the automatic flap control define <section name="HORIZONTAL CONTROL" prefix="H_CTL_"> <define name="CL_LOOP" value="TRUE"> <define name="CL_LOOP_USE_AIRSPEED_SETPOINT" value="TRUE"> <define name="CL_FLAPS_STALL" value="0.8"> <define name="CL_FLAPS_NOMINAL" value="0."> <define name="CL_FLAPS_RACE" value="-0.5"> <define name="CL_DEADBAND" value="1."> </section>

the actual flap setting can also be increased by the loadfactor "nz" <section name="HORIZONTAL CONTROL" prefix="H_CTL_"> <define name="CL_LOOP_INCREASE_FLAPS_WITH_LOADFACTOR" value="TRUE"> </section>

Definition in file stabilization_adaptive.c.

Data Structure Documentation

struct HCtlAdaptRef

Definition at line 93 of file stabilization_adaptive.c.

Data Fields
float	max_p
float	max_p_dot
float	max_q
float	max_q_dot
float	max_r
float	max_r_dot
float	pitch_accel
float	pitch_angle
float	pitch_rate
float	roll_accel
float	roll_angle
float	roll_rate
float	yaw_accel
float	yaw_angle
float	yaw_rate

Macro Definition Documentation

#define AIRSPEED_RATIO_MAX 2.

Definition at line 202 of file stabilization_adaptive.c.

#define AIRSPEED_RATIO_MIN 0.5

Definition at line 201 of file stabilization_adaptive.c.

#define H_CTL_COURSE_DGAIN 0.

Definition at line 226 of file stabilization_adaptive.c.

Referenced by h_ctl_init().

#define H_CTL_COURSE_PRE_BANK_CORRECTION 1.

Definition at line 223 of file stabilization_adaptive.c.

Referenced by h_ctl_init().

#define H_CTL_PITCH_DGAIN 0.

Definition at line 247 of file stabilization_adaptive.c.

Referenced by h_ctl_init().

#define H_CTL_PITCH_IGAIN 0.

Definition at line 250 of file stabilization_adaptive.c.

Referenced by h_ctl_init().

#define H_CTL_PITCH_KFFA 0.

Definition at line 253 of file stabilization_adaptive.c.

Referenced by h_ctl_init().

#define H_CTL_PITCH_KFFD 0.

Definition at line 256 of file stabilization_adaptive.c.

Referenced by h_ctl_init().

#define H_CTL_REF_DT (1./CONTROL_FREQUENCY)

Define reference generator time step default to control frequency and ahrs propagation freq if control is triggered by ahrs.

Definition at line 437 of file stabilization_adaptive.c.

Referenced by h_ctl_pitch_loop(), and h_ctl_roll_loop().

#define H_CTL_REF_MAX_P RadOfDeg(150.)

Definition at line 134 of file stabilization_adaptive.c.

Referenced by h_ctl_init().

#define H_CTL_REF_MAX_P_DOT RadOfDeg(500.)

Definition at line 137 of file stabilization_adaptive.c.

Referenced by h_ctl_init().

#define H_CTL_REF_MAX_Q RadOfDeg(150.)

Definition at line 140 of file stabilization_adaptive.c.

Referenced by h_ctl_init().

#define H_CTL_REF_MAX_Q_DOT RadOfDeg(500.)

Definition at line 143 of file stabilization_adaptive.c.

Referenced by h_ctl_init().

#define H_CTL_REF_W_P 5.

Definition at line 116 of file stabilization_adaptive.c.

Referenced by h_ctl_roll_loop().

#define H_CTL_REF_W_Q 5.

Definition at line 122 of file stabilization_adaptive.c.

Referenced by h_ctl_pitch_loop().

#define H_CTL_REF_W_R 5.

Definition at line 128 of file stabilization_adaptive.c.

#define H_CTL_REF_XI_P 0.85

Definition at line 119 of file stabilization_adaptive.c.

Referenced by h_ctl_roll_loop().

#define H_CTL_REF_XI_Q 0.85

Definition at line 125 of file stabilization_adaptive.c.

Referenced by h_ctl_pitch_loop().

#define H_CTL_REF_XI_R 0.85

Definition at line 131 of file stabilization_adaptive.c.

#define H_CTL_ROLL_IGAIN 0.

Definition at line 235 of file stabilization_adaptive.c.

Referenced by h_ctl_init().

#define H_CTL_ROLL_KFFA 0.

Definition at line 238 of file stabilization_adaptive.c.

Referenced by h_ctl_init().

#define H_CTL_ROLL_KFFD 0.

Definition at line 241 of file stabilization_adaptive.c.

Referenced by h_ctl_init().

#define H_CTL_ROLL_RATE_GAIN 0.

Definition at line 232 of file stabilization_adaptive.c.

Referenced by h_ctl_init().

#define H_CTL_YAW_KFFD 0.

Definition at line 261 of file stabilization_adaptive.c.

#define KFFA_UPDATE 0.1

Adaptive control tuning parameters activate with USE_KFF_UPDATE.

!!! under development, use with care !!!

Definition at line 445 of file stabilization_adaptive.c.

Referenced by h_ctl_pitch_loop(), and h_ctl_roll_loop().

#define KFFD_UPDATE 0.05

Definition at line 446 of file stabilization_adaptive.c.

Referenced by h_ctl_pitch_loop(), and h_ctl_roll_loop().

#define PITCH_TRIM_RATE_LIMITER 3.

Definition at line 209 of file stabilization_adaptive.c.

#define USE_GYRO_PITCH_RATE TRUE

Definition at line 265 of file stabilization_adaptive.c.

Function Documentation

void h_ctl_attitude_loop ( void )

Definition at line 413 of file stabilization_adaptive.c.

Referenced by attitude_loop().

Here is the caller graph for this function:

void h_ctl_course_loop ( void )

Definition at line 374 of file stabilization_adaptive.c.

Referenced by navigation_task().

Here is the caller graph for this function:

void h_ctl_init ( void )

Definition at line 298 of file stabilization_adaptive.c.

Referenced by init_ap().

Here is the caller graph for this function:

static void h_ctl_pitch_loop ( void )

inlinestatic

Definition at line 549 of file stabilization_adaptive.c.

References airspeed_ratio2, h_ctl_pitch_loop_setpoint, h_ctl_pitch_sum_err, H_CTL_PITCH_SUM_ERR_MAX, h_ctl_ref, H_CTL_REF_DT, H_CTL_REF_W_Q, H_CTL_REF_XI_Q, if(), KFFA_UPDATE, KFFD_UPDATE, launch, Max, HCtlAdaptRef::max_q, HCtlAdaptRef::max_q_dot, HCtlAdaptRef::pitch_accel, HCtlAdaptRef::pitch_angle, HCtlAdaptRef::pitch_rate, pprz_mode, PPRZ_MODE_MANUAL, FloatRates::q, stateGetBodyRates_f(), stateGetNedToBodyEulers_f(), FloatEulers::theta, and TRIM_PPRZ.

Referenced by h_ctl_attitude_loop().

Here is the call graph for this function:

Here is the caller graph for this function:

static void h_ctl_roll_loop ( void )

inlinestatic

Definition at line 448 of file stabilization_adaptive.c.

References airspeed_ratio2, h_ctl_aileron_of_throttle, h_ctl_ref, H_CTL_REF_DT, H_CTL_REF_W_P, H_CTL_REF_XI_P, h_ctl_roll_setpoint, H_CTL_ROLL_SUM_ERR_MAX, KFFA_UPDATE, KFFD_UPDATE, launch, Max, HCtlAdaptRef::max_p, HCtlAdaptRef::max_p_dot, FloatRates::p, FloatEulers::phi, pprz_mode, PPRZ_MODE_MANUAL, HCtlAdaptRef::roll_accel, HCtlAdaptRef::roll_angle, HCtlAdaptRef::roll_rate, stateGetBodyRates_f(), stateGetNedToBodyEulers_f(), TRIM_PPRZ, and v_ctl_throttle_setpoint.

Referenced by h_ctl_attitude_loop().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

float airspeed_ratio2

Definition at line 200 of file stabilization_adaptive.c.

Referenced by h_ctl_pitch_loop(), and h_ctl_roll_loop().

float h_ctl_aileron_of_throttle

Definition at line 195 of file stabilization_adaptive.c.

Referenced by h_ctl_roll_loop().

pprz_t h_ctl_aileron_setpoint

Definition at line 164 of file stabilization_adaptive.c.

Referenced by attitude_loop().

bool_t h_ctl_auto1_rate

Definition at line 91 of file stabilization_adaptive.c.

Referenced by navigation_task().

float h_ctl_course_dgain

Definition at line 84 of file stabilization_adaptive.c.

Referenced by h_ctl_course_loop(), and h_ctl_init().

float h_ctl_course_pgain

Definition at line 83 of file stabilization_adaptive.c.

Referenced by fly_to_xy(), h_ctl_course_loop(), and h_ctl_init().

float h_ctl_course_pre_bank

Definition at line 81 of file stabilization_adaptive.c.

Referenced by h_ctl_course_loop(), h_ctl_init(), and nav_periodic_task().

float h_ctl_course_pre_bank_correction

Definition at line 82 of file stabilization_adaptive.c.

Referenced by h_ctl_course_loop(), and h_ctl_init().

float h_ctl_course_setpoint

Definition at line 80 of file stabilization_adaptive.c.

Referenced by fly_to_xy(), h_ctl_course_loop(), and h_ctl_init().

bool_t h_ctl_disabled

Definition at line 88 of file stabilization_adaptive.c.

Referenced by h_ctl_attitude_loop(), and h_ctl_init().

float h_ctl_elevator_of_roll

Definition at line 196 of file stabilization_adaptive.c.

pprz_t h_ctl_elevator_setpoint

Definition at line 178 of file stabilization_adaptive.c.

Referenced by attitude_loop().

float h_ctl_pitch_dgain

Definition at line 173 of file stabilization_adaptive.c.

float h_ctl_pitch_igain

Definition at line 174 of file stabilization_adaptive.c.

float h_ctl_pitch_Kffa

Definition at line 176 of file stabilization_adaptive.c.

float h_ctl_pitch_Kffd

Definition at line 177 of file stabilization_adaptive.c.

float h_ctl_pitch_loop_setpoint

Definition at line 171 of file stabilization_adaptive.c.

Referenced by h_ctl_pitch_loop().

float h_ctl_pitch_of_roll

Definition at line 197 of file stabilization_adaptive.c.

float h_ctl_pitch_pgain

Definition at line 172 of file stabilization_adaptive.c.

float h_ctl_pitch_setpoint

Definition at line 170 of file stabilization_adaptive.c.

Referenced by attitude_loop(), and telecommand_task().

float h_ctl_pitch_sum_err

Definition at line 175 of file stabilization_adaptive.c.

Referenced by h_ctl_pitch_loop().

struct HCtlAdaptRef h_ctl_ref

Definition at line 112 of file stabilization_adaptive.c.

Referenced by h_ctl_init(), h_ctl_pitch_loop(), and h_ctl_roll_loop().

float h_ctl_roll_attitude_gain

Definition at line 158 of file stabilization_adaptive.c.

float h_ctl_roll_igain

Definition at line 160 of file stabilization_adaptive.c.

float h_ctl_roll_Kffa

Definition at line 162 of file stabilization_adaptive.c.

float h_ctl_roll_Kffd

Definition at line 163 of file stabilization_adaptive.c.

float h_ctl_roll_max_setpoint

Definition at line 85 of file stabilization_adaptive.c.

Referenced by fly_to_xy(), h_ctl_course_loop(), and h_ctl_init().

float h_ctl_roll_pgain

Definition at line 167 of file stabilization_adaptive.c.

float h_ctl_roll_rate_gain

Definition at line 159 of file stabilization_adaptive.c.

float h_ctl_roll_setpoint

Definition at line 157 of file stabilization_adaptive.c.

Referenced by fly_to_xy(), h_ctl_roll_loop(), nav_without_gps(), and telecommand_task().

float h_ctl_roll_slew

Definition at line 165 of file stabilization_adaptive.c.

float h_ctl_roll_sum_err

Definition at line 161 of file stabilization_adaptive.c.

bool_t use_airspeed_ratio

Definition at line 199 of file stabilization_adaptive.c.

float v_ctl_pitch_dash_trim

Definition at line 205 of file stabilization_adaptive.c.

float v_ctl_pitch_loiter_trim

Definition at line 204 of file stabilization_adaptive.c.

Data Structures

Macros

Functions

Variables

Detailed Description

Data Structure Documentation

Macro Definition Documentation

Function Documentation

Variable Documentation