stabilization_adaptive.c File Reference

#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 "autopilot.h"
#include "subsystems/datalink/telemetry.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)
static void	send_calibration (struct transport_tx *trans, struct link_device *dev)
static void	send_tune_roll (struct transport_tx *trans, struct link_device *dev)
static void	send_ctl_a (struct transport_tx *trans, struct link_device *dev)
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	h_ctl_disabled
bool	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	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>

<section name="HORIZONTAL CONTROL" prefix="H_CTL_"> <define name="YAW_LOOP" value="TRUE"> <define name="YAW_DGAIN" value="5000."> </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

HCtlAdaptRef

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

AIRSPEED_RATIO_MAX

#define AIRSPEED_RATIO_MAX   2.

Definition at line 202 of file stabilization_adaptive.c.

AIRSPEED_RATIO_MIN

#define AIRSPEED_RATIO_MIN   0.5

Definition at line 201 of file stabilization_adaptive.c.

H_CTL_COURSE_DGAIN

#define H_CTL_COURSE_DGAIN   0.

Definition at line 226 of file stabilization_adaptive.c.

H_CTL_COURSE_PRE_BANK_CORRECTION

#define H_CTL_COURSE_PRE_BANK_CORRECTION   1.

Definition at line 223 of file stabilization_adaptive.c.

H_CTL_PITCH_DGAIN

#define H_CTL_PITCH_DGAIN   0.

Definition at line 247 of file stabilization_adaptive.c.

H_CTL_PITCH_IGAIN

#define H_CTL_PITCH_IGAIN   0.

Definition at line 250 of file stabilization_adaptive.c.

H_CTL_PITCH_KFFA

#define H_CTL_PITCH_KFFA   0.

Definition at line 253 of file stabilization_adaptive.c.

H_CTL_PITCH_KFFD

#define H_CTL_PITCH_KFFD   0.

Definition at line 256 of file stabilization_adaptive.c.

H_CTL_REF_DT

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

H_CTL_REF_MAX_P

#define H_CTL_REF_MAX_P   RadOfDeg(150.)

Definition at line 134 of file stabilization_adaptive.c.

H_CTL_REF_MAX_P_DOT

#define H_CTL_REF_MAX_P_DOT   RadOfDeg(500.)

Definition at line 137 of file stabilization_adaptive.c.

H_CTL_REF_MAX_Q

#define H_CTL_REF_MAX_Q   RadOfDeg(150.)

Definition at line 140 of file stabilization_adaptive.c.

H_CTL_REF_MAX_Q_DOT

#define H_CTL_REF_MAX_Q_DOT   RadOfDeg(500.)

Definition at line 143 of file stabilization_adaptive.c.

H_CTL_REF_W_P

#define H_CTL_REF_W_P   5.

Definition at line 116 of file stabilization_adaptive.c.

H_CTL_REF_W_Q

#define H_CTL_REF_W_Q   5.

Definition at line 122 of file stabilization_adaptive.c.

H_CTL_REF_W_R

#define H_CTL_REF_W_R   5.

Definition at line 128 of file stabilization_adaptive.c.

H_CTL_REF_XI_P

#define H_CTL_REF_XI_P   0.85

Definition at line 119 of file stabilization_adaptive.c.

H_CTL_REF_XI_Q

#define H_CTL_REF_XI_Q   0.85

Definition at line 125 of file stabilization_adaptive.c.

H_CTL_REF_XI_R

#define H_CTL_REF_XI_R   0.85

Definition at line 131 of file stabilization_adaptive.c.

H_CTL_ROLL_IGAIN

#define H_CTL_ROLL_IGAIN   0.

Definition at line 235 of file stabilization_adaptive.c.

H_CTL_ROLL_KFFA

#define H_CTL_ROLL_KFFA   0.

Definition at line 238 of file stabilization_adaptive.c.

H_CTL_ROLL_KFFD

#define H_CTL_ROLL_KFFD   0.

Definition at line 241 of file stabilization_adaptive.c.

H_CTL_ROLL_RATE_GAIN

#define H_CTL_ROLL_RATE_GAIN   0.

Definition at line 232 of file stabilization_adaptive.c.

H_CTL_YAW_KFFD

#define H_CTL_YAW_KFFD   0.

Definition at line 261 of file stabilization_adaptive.c.

KFFA_UPDATE

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

KFFD_UPDATE

#define KFFD_UPDATE   0.05

Definition at line 446 of file stabilization_adaptive.c.

PITCH_TRIM_RATE_LIMITER

#define PITCH_TRIM_RATE_LIMITER   3.

Definition at line 209 of file stabilization_adaptive.c.

USE_GYRO_PITCH_RATE

#define USE_GYRO_PITCH_RATE   TRUE

Definition at line 265 of file stabilization_adaptive.c.

Function Documentation

h_ctl_attitude_loop()

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:

h_ctl_course_loop()

void h_ctl_course_loop

(

void

)

Definition at line 374 of file stabilization_adaptive.c.

Referenced by h_ctl_guidance_loop(), and navigation_task().

Here is the caller graph for this function:

h_ctl_init()

void h_ctl_init

(

void

)

Definition at line 298 of file stabilization_adaptive.c.

h_ctl_pitch_loop()

static void h_ctl_pitch_loop ( void  )

inlinestatic

Definition at line 549 of file stabilization_adaptive.c.

References airspeed_ratio2, AP_MODE_MANUAL, autopilot, autopilot_get_mode(), h_ctl_elevator_setpoint, h_ctl_pitch_dgain, h_ctl_pitch_igain, h_ctl_pitch_Kffa, h_ctl_pitch_Kffd, h_ctl_pitch_loop_setpoint, h_ctl_pitch_of_roll, h_ctl_pitch_pgain, h_ctl_pitch_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, pprz_autopilot::launch, Max, HCtlAdaptRef::max_q, HCtlAdaptRef::max_q_dot, HCtlAdaptRef::pitch_accel, HCtlAdaptRef::pitch_angle, HCtlAdaptRef::pitch_rate, 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:

h_ctl_roll_loop()

static void h_ctl_roll_loop ( void  )

inlinestatic

Definition at line 448 of file stabilization_adaptive.c.

References airspeed_ratio2, AP_MODE_MANUAL, autopilot, autopilot_get_mode(), h_ctl_aileron_of_throttle, h_ctl_aileron_setpoint, h_ctl_ref, H_CTL_REF_DT, H_CTL_REF_W_P, H_CTL_REF_XI_P, h_ctl_roll_attitude_gain, h_ctl_roll_igain, h_ctl_roll_Kffa, h_ctl_roll_Kffd, h_ctl_roll_rate_gain, h_ctl_roll_setpoint, h_ctl_roll_sum_err, H_CTL_ROLL_SUM_ERR_MAX, KFFA_UPDATE, KFFD_UPDATE, pprz_autopilot::launch, Max, HCtlAdaptRef::max_p, HCtlAdaptRef::max_p_dot, FloatRates::p, FloatEulers::phi, 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:

send_calibration()

static void send_calibration ( struct transport_tx *  trans, struct link_device *  dev  )

static

Definition at line 271 of file stabilization_adaptive.c.

References dev, v_ctl_auto_throttle_submode, and v_ctl_auto_throttle_sum_err.

Referenced by h_ctl_init().

Here is the caller graph for this function:

send_ctl_a()

static void send_ctl_a ( struct transport_tx *  trans, struct link_device *  dev  )

static

Definition at line 282 of file stabilization_adaptive.c.

References dev, h_ctl_aileron_setpoint, h_ctl_elevator_setpoint, h_ctl_pitch_loop_setpoint, h_ctl_pitch_sum_err, h_ctl_ref, h_ctl_roll_setpoint, h_ctl_roll_sum_err, HCtlAdaptRef::pitch_angle, HCtlAdaptRef::roll_angle, and stateGetNedToBodyEulers_f().

Referenced by h_ctl_init().

Here is the call graph for this function:

Here is the caller graph for this function:

send_tune_roll()

static void send_tune_roll ( struct transport_tx *  trans, struct link_device *  dev  )

static

Definition at line 276 of file stabilization_adaptive.c.

References dev, h_ctl_roll_setpoint, p, stateGetBodyRates_f(), and stateGetNedToBodyEulers_f().

Referenced by h_ctl_init().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

airspeed_ratio2

float airspeed_ratio2

Definition at line 200 of file stabilization_adaptive.c.

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

h_ctl_aileron_of_throttle

float h_ctl_aileron_of_throttle

Definition at line 195 of file stabilization_adaptive.c.

Referenced by h_ctl_init(), and h_ctl_roll_loop().

h_ctl_aileron_setpoint

pprz_t h_ctl_aileron_setpoint

Definition at line 164 of file stabilization_adaptive.c.

Referenced by attitude_loop(), file_logger_write_row(), h_ctl_init(), h_ctl_roll_loop(), and send_ctl_a().

h_ctl_auto1_rate

bool h_ctl_auto1_rate

Definition at line 91 of file stabilization_adaptive.c.

Referenced by navigation_task().

h_ctl_course_dgain

float h_ctl_course_dgain

Definition at line 84 of file stabilization_adaptive.c.

Referenced by h_ctl_course_loop(), and h_ctl_init().

h_ctl_course_pgain

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().

h_ctl_course_pre_bank

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().

h_ctl_course_pre_bank_correction

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().

h_ctl_course_setpoint

float h_ctl_course_setpoint

Definition at line 80 of file stabilization_adaptive.c.

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

h_ctl_disabled

bool h_ctl_disabled

Definition at line 88 of file stabilization_adaptive.c.

Referenced by h_ctl_attitude_loop(), and h_ctl_init().

h_ctl_elevator_of_roll

float h_ctl_elevator_of_roll

Definition at line 196 of file stabilization_adaptive.c.

Referenced by h_ctl_init().

h_ctl_elevator_setpoint

pprz_t h_ctl_elevator_setpoint

Definition at line 178 of file stabilization_adaptive.c.

Referenced by attitude_loop(), file_logger_write_row(), h_ctl_init(), h_ctl_pitch_loop(), and send_ctl_a().

h_ctl_pitch_dgain

float h_ctl_pitch_dgain

Definition at line 173 of file stabilization_adaptive.c.

Referenced by h_ctl_init(), and h_ctl_pitch_loop().

h_ctl_pitch_igain

float h_ctl_pitch_igain

Definition at line 174 of file stabilization_adaptive.c.

Referenced by h_ctl_init(), and h_ctl_pitch_loop().

h_ctl_pitch_Kffa

float h_ctl_pitch_Kffa

Definition at line 176 of file stabilization_adaptive.c.

Referenced by h_ctl_init(), and h_ctl_pitch_loop().

h_ctl_pitch_Kffd

float h_ctl_pitch_Kffd

Definition at line 177 of file stabilization_adaptive.c.

Referenced by h_ctl_init(), and h_ctl_pitch_loop().

h_ctl_pitch_loop_setpoint

float h_ctl_pitch_loop_setpoint

Definition at line 171 of file stabilization_adaptive.c.

Referenced by h_ctl_init(), h_ctl_pitch_loop(), send_ctl_a(), and send_desired().

h_ctl_pitch_of_roll

float h_ctl_pitch_of_roll

Definition at line 197 of file stabilization_adaptive.c.

Referenced by h_ctl_init(), and h_ctl_pitch_loop().

h_ctl_pitch_pgain

float h_ctl_pitch_pgain

Definition at line 172 of file stabilization_adaptive.c.

Referenced by h_ctl_init(), and h_ctl_pitch_loop().

h_ctl_pitch_setpoint

float h_ctl_pitch_setpoint

Definition at line 170 of file stabilization_adaptive.c.

Referenced by attitude_loop(), autopilot_static_on_rc_frame(), h_ctl_guidance_loop(), h_ctl_init(), and h_ctl_pitch_loop().

h_ctl_pitch_sum_err

float h_ctl_pitch_sum_err

Definition at line 175 of file stabilization_adaptive.c.

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

h_ctl_ref

struct HCtlAdaptRef h_ctl_ref

Definition at line 112 of file stabilization_adaptive.c.

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

h_ctl_roll_attitude_gain

float h_ctl_roll_attitude_gain

Definition at line 158 of file stabilization_adaptive.c.

Referenced by h_ctl_init(), and h_ctl_roll_loop().

h_ctl_roll_igain

float h_ctl_roll_igain

Definition at line 160 of file stabilization_adaptive.c.

Referenced by h_ctl_init(), and h_ctl_roll_loop().

h_ctl_roll_Kffa

float h_ctl_roll_Kffa

Definition at line 162 of file stabilization_adaptive.c.

Referenced by h_ctl_init(), and h_ctl_roll_loop().

h_ctl_roll_Kffd

float h_ctl_roll_Kffd

Definition at line 163 of file stabilization_adaptive.c.

Referenced by h_ctl_init(), and h_ctl_roll_loop().

h_ctl_roll_max_setpoint

float h_ctl_roll_max_setpoint

Definition at line 85 of file stabilization_adaptive.c.

Referenced by collective_tracking_control(), fly_to_xy(), gvf_control_2D(), gvf_parametric_low_level_control_2D(), gvf_parametric_low_level_control_3D(), h_ctl_course_loop(), and h_ctl_init().

h_ctl_roll_pgain

float h_ctl_roll_pgain

Definition at line 167 of file stabilization_adaptive.c.

h_ctl_roll_rate_gain

float h_ctl_roll_rate_gain

Definition at line 159 of file stabilization_adaptive.c.

Referenced by h_ctl_init(), and h_ctl_roll_loop().

h_ctl_roll_setpoint

float h_ctl_roll_setpoint

Definition at line 157 of file stabilization_adaptive.c.

Referenced by autopilot_static_on_rc_frame(), collective_tracking_control(), fly_to_xy(), gvf_control_2D(), gvf_parametric_low_level_control_2D(), gvf_parametric_low_level_control_3D(), h_ctl_course_loop(), h_ctl_init(), h_ctl_roll_loop(), nav_without_gps(), send_ctl_a(), send_desired(), send_tune_roll(), and v_ctl_landing_loop().

h_ctl_roll_slew

float h_ctl_roll_slew

Definition at line 165 of file stabilization_adaptive.c.

h_ctl_roll_sum_err

float h_ctl_roll_sum_err

Definition at line 161 of file stabilization_adaptive.c.

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

use_airspeed_ratio

bool use_airspeed_ratio

Definition at line 199 of file stabilization_adaptive.c.

Referenced by h_ctl_init().

v_ctl_pitch_dash_trim

float v_ctl_pitch_dash_trim

Definition at line 205 of file stabilization_adaptive.c.

Referenced by h_ctl_init().

v_ctl_pitch_loiter_trim

float v_ctl_pitch_loiter_trim

Definition at line 204 of file stabilization_adaptive.c.

Referenced by h_ctl_init().