Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 "modules/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()

◆ h_ctl_roll_loop()

◆ 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

◆ 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

◆ 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

◆ 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

◆ 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

◆ 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

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