Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
Data Structures | Macros | Functions | Variables
stabilization_adaptive.c File Reference

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 "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

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 h_ctl_guidance_loop(), and navigation_task().

+ Here is the caller graph for this function:

void h_ctl_init ( void  )

Definition at line 298 of file stabilization_adaptive.c.

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_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, 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:

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_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, 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:

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

Definition at line 271 of file stabilization_adaptive.c.

References 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:

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

Definition at line 282 of file stabilization_adaptive.c.

References h_ctl_ref, 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:

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

Definition at line 276 of file stabilization_adaptive.c.

References 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

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(), and file_logger_periodic().

bool 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(), h_ctl_init(), and send_desired().

bool 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(), and file_logger_periodic().

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(), and send_desired().

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(), autopilot_static_on_rc_frame(), and h_ctl_guidance_loop().

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(), h_ctl_roll_loop(), and send_ctl_a().

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(), gvf_control_2D(), 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 autopilot_static_on_rc_frame(), fly_to_xy(), gvf_control_2D(), h_ctl_roll_loop(), nav_without_gps(), send_desired(), and v_ctl_landing_loop().

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