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
stabilization_attitude_heli_indi.c File Reference

Helicopter quaternion INDI attitude stabilization. More...

+ Include dependency graph for stabilization_attitude_heli_indi.c:

Go to the source code of this file.

Macros

#define STABILIZATION_ATTITUDE_HELI_INDI_USE_NOTCHFILTER   0
 
#define STABILIZATION_ATTITUDE_HELI_INDI_STEADY_STATE_ROLL   4.5
 
#define STABILIZATION_ATTITUDE_HELI_INDI_STEADY_STATE_PITCH   0
 
#define STABILIZATION_ATTITUDE_HELI_INDI_ROLL_P   12
 
#define STABILIZATION_ATTITUDE_HELI_INDI_PITCH_P   8
 
#define STABILIZATION_ATTITUDE_HELI_INDI_YAW_P   10
 
#define STABILIZATION_ATTITUDE_HELI_INDI_YAW_D   30
 
#define STABILIZATION_ATTITUDE_HELI_INDI_GINV_ROLL_TO_ROLL   11681
 
#define STABILIZATION_ATTITUDE_HELI_INDI_GINV_PITCH_TO_PITCH   13873
 
#define STABILIZATION_ATTITUDE_HELI_INDI_GINV_YAW_TO_YAW   730
 
#define STABILIZATION_ATTITUDE_HELI_INDI_ROLL_COMMAND_ROTATION   11.0
 
#define STABILIZATION_ATTITUDE_HELI_INDI_PITCH_COMMAND_ROTATION   -30.0
 
#define STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_DEFAULT   10.0
 
#define STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_ROLL   STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_DEFAULT
 
#define STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_PITCH   STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_DEFAULT
 
#define STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_YAW   STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_DEFAULT
 
#define STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_THRUST   STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_DEFAULT
 
#define STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_DEFAULT   40.0
 
#define STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_ROLL   STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_DEFAULT
 
#define STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_PITCH   STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_DEFAULT
 
#define STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_YAW   STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_DEFAULT
 
#define STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_THRUST   STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_DEFAULT
 
#define STABILIZATION_ATTITUDE_HELI_INDI_NOTCH_MIN_RPM   1500
 
#define INDI_NOTCH_MIN_RPM   STABILIZATION_ATTITUDE_HELI_INDI_NOTCH_MIN_RPM
 
#define INVG_00   STABILIZATION_ATTITUDE_HELI_INDI_GINV_ROLL_TO_ROLL
 
#define INVG_11   STABILIZATION_ATTITUDE_HELI_INDI_GINV_PITCH_TO_PITCH
 
#define INVG_22   STABILIZATION_ATTITUDE_HELI_INDI_GINV_YAW_TO_YAW
 
#define INVG_33   -50000
 
#define INT32_INVG_FRAC   16
 

Functions

void indi_apply_actuator_notch_filters (int32_t _out[], int32_t _in[])
 
void indi_apply_measurement_notch_filters (int32_t _out[], int32_t _in[])
 
void indi_apply_actuator_butterworth_filters (int32_t _out[], int32_t _in[])
 
void indi_apply_measurement_butterworth_filters (int32_t _out[], int32_t _in[])
 
static void indi_apply_actuator_models (int32_t _out[], int32_t _in[])
 
static void indi_apply_compensator_filters (int32_t _out[], int32_t _in[])
 The main idea of this function is to slow down a certain actuator, so that the actuator dynamics filtered by the compensater are equal to another slower actuator. More...
 
static void indi_apply_notch_filters (struct SecondOrderNotchFilter *filter, int32_t _out[], int32_t _in[])
 
void stabilization_attitude_heli_indi_set_steadystate_pitch (float pitch)
 stabilization_attitude_heli_indi_set_steadystate_pitch More...
 
void stabilization_attitude_heli_indi_set_steadystate_roll (float roll)
 stabilization_attitude_heli_indi_set_steadystate_roll More...
 
void stabilization_attitude_heli_indi_set_steadystate_pitchroll ()
 stabilization_attitude_heli_indi_set_steadystate_pitchroll More...
 
void stabilization_attitude_init (void)
 stabilization_attitude_init More...
 
void stabilization_attitude_run (bool in_flight)
 
void stabilization_attitude_enter (void)
 
void stabilization_attitude_set_failsafe_setpoint (void)
 
void stabilization_attitude_set_rpy_setpoint_i (struct Int32Eulers *rpy)
 
void stabilization_attitude_set_earth_cmd_i (struct Int32Vect2 *cmd, int32_t heading)
 
void stabilization_attitude_read_rc (bool in_flight, bool in_carefree, bool coordinated_turn)
 

Variables

struct Int32Quat stab_att_sp_quat
 with INT32_QUAT_FRAC More...
 
struct Int32Eulers stab_att_sp_euler
 with INT32_ANGLE_FRAC More...
 
struct Int32Quat sp_offset
 
struct HeliIndiGains heli_indi_gains
 
struct IndiController_int heli_indi_ctl
 
struct
delayed_first_order_lowpass_filter_t 
actuator_model [INDI_DOF]
 
int32_t alpha_yaw_inc
 
int32_t alpha_yaw_dec
 
Butterworth2LowPass_int actuator_lowpass_filters [INDI_DOF]
 
Butterworth2LowPass_int measurement_lowpass_filters [INDI_DOF]
 
struct SecondOrderNotchFilter actuator_notchfilter [INDI_DOF]
 
struct SecondOrderNotchFilter measurement_notchfilter [INDI_DOF]
 

Detailed Description

Helicopter quaternion INDI attitude stabilization.

Definition in file stabilization_attitude_heli_indi.c.

Macro Definition Documentation

#define INDI_NOTCH_MIN_RPM   STABILIZATION_ATTITUDE_HELI_INDI_NOTCH_MIN_RPM

Definition at line 118 of file stabilization_attitude_heli_indi.c.

Referenced by indi_apply_notch_filters().

#define INT32_INVG_FRAC   16

Definition at line 125 of file stabilization_attitude_heli_indi.c.

Referenced by stabilization_attitude_run().

#define INVG_33   -50000

Definition at line 124 of file stabilization_attitude_heli_indi.c.

Referenced by stabilization_attitude_init().

#define STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_DEFAULT   40.0

Definition at line 101 of file stabilization_attitude_heli_indi.c.

#define STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_PITCH   STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_DEFAULT

Definition at line 107 of file stabilization_attitude_heli_indi.c.

Referenced by stabilization_attitude_init().

#define STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_ROLL   STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_DEFAULT

Definition at line 104 of file stabilization_attitude_heli_indi.c.

Referenced by stabilization_attitude_init().

#define STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_THRUST   STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_DEFAULT

Definition at line 113 of file stabilization_attitude_heli_indi.c.

Referenced by stabilization_attitude_init().

#define STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_YAW   STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_DEFAULT

Definition at line 110 of file stabilization_attitude_heli_indi.c.

Referenced by stabilization_attitude_init().

#define STABILIZATION_ATTITUDE_HELI_INDI_GINV_PITCH_TO_PITCH   13873

Definition at line 74 of file stabilization_attitude_heli_indi.c.

#define STABILIZATION_ATTITUDE_HELI_INDI_GINV_ROLL_TO_ROLL   11681

Definition at line 71 of file stabilization_attitude_heli_indi.c.

#define STABILIZATION_ATTITUDE_HELI_INDI_GINV_YAW_TO_YAW   730

Definition at line 77 of file stabilization_attitude_heli_indi.c.

#define STABILIZATION_ATTITUDE_HELI_INDI_NOTCH_MIN_RPM   1500

Definition at line 116 of file stabilization_attitude_heli_indi.c.

#define STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_DEFAULT   10.0

Definition at line 86 of file stabilization_attitude_heli_indi.c.

#define STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_PITCH   STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_DEFAULT

Definition at line 92 of file stabilization_attitude_heli_indi.c.

Referenced by stabilization_attitude_init().

#define STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_ROLL   STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_DEFAULT

Definition at line 89 of file stabilization_attitude_heli_indi.c.

Referenced by stabilization_attitude_init().

#define STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_THRUST   STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_DEFAULT

Definition at line 98 of file stabilization_attitude_heli_indi.c.

Referenced by stabilization_attitude_init().

#define STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_YAW   STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_DEFAULT

Definition at line 95 of file stabilization_attitude_heli_indi.c.

Referenced by stabilization_attitude_init().

#define STABILIZATION_ATTITUDE_HELI_INDI_PITCH_COMMAND_ROTATION   -30.0

Definition at line 83 of file stabilization_attitude_heli_indi.c.

Referenced by stabilization_attitude_init().

#define STABILIZATION_ATTITUDE_HELI_INDI_PITCH_P   8

Definition at line 62 of file stabilization_attitude_heli_indi.c.

#define STABILIZATION_ATTITUDE_HELI_INDI_ROLL_COMMAND_ROTATION   11.0

Definition at line 80 of file stabilization_attitude_heli_indi.c.

Referenced by stabilization_attitude_init().

#define STABILIZATION_ATTITUDE_HELI_INDI_ROLL_P   12

Definition at line 59 of file stabilization_attitude_heli_indi.c.

#define STABILIZATION_ATTITUDE_HELI_INDI_STEADY_STATE_PITCH   0

Definition at line 56 of file stabilization_attitude_heli_indi.c.

Referenced by stabilization_attitude_init().

#define STABILIZATION_ATTITUDE_HELI_INDI_STEADY_STATE_ROLL   4.5

Definition at line 53 of file stabilization_attitude_heli_indi.c.

Referenced by stabilization_attitude_init().

#define STABILIZATION_ATTITUDE_HELI_INDI_USE_NOTCHFILTER   0

Definition at line 42 of file stabilization_attitude_heli_indi.c.

#define STABILIZATION_ATTITUDE_HELI_INDI_YAW_D   30

Definition at line 68 of file stabilization_attitude_heli_indi.c.

#define STABILIZATION_ATTITUDE_HELI_INDI_YAW_P   10

Definition at line 65 of file stabilization_attitude_heli_indi.c.

Function Documentation

void indi_apply_actuator_butterworth_filters ( int32_t  _out[],
int32_t  _in[] 
)

Definition at line 278 of file stabilization_attitude_heli_indi.c.

References INDI_DOF, and update_butterworth_2_low_pass_int().

Referenced by stabilization_attitude_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void indi_apply_actuator_models ( int32_t  _out[],
int32_t  _in[] 
)
inlinestatic
void indi_apply_actuator_notch_filters ( int32_t  _out[],
int32_t  _in[] 
)

Definition at line 268 of file stabilization_attitude_heli_indi.c.

References actuator_notchfilter, and indi_apply_notch_filters().

Referenced by stabilization_attitude_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void indi_apply_compensator_filters ( int32_t  _out[],
int32_t  _in[] 
)
inlinestatic

The main idea of this function is to slow down a certain actuator, so that the actuator dynamics filtered by the compensater are equal to another slower actuator.

In this case, the tail rotor is slower than collective pitch. The collective pitch signal can be reduced so that the total dynamics match the dynamics of the tail rotor. This prevents the tail from breaking out. But at the moment thrust is not implemented so this is not used.

Definition at line 206 of file stabilization_attitude_heli_indi.c.

References actuator_model, delayed_first_order_lowpass_filter_t::alpha, alpha, alpha_yaw_dec, alpha_yaw_inc, DELAYED_FIRST_ORDER_LOWPASS_FILTER_FILTER_ALPHA_SHIFT, INDI_PITCH, INDI_ROLL, INDI_THRUST, INDI_YAW, and INDI_YAW_BUFFER_SIZE.

Referenced by stabilization_attitude_init().

+ Here is the caller graph for this function:

void indi_apply_measurement_butterworth_filters ( int32_t  _out[],
int32_t  _in[] 
)

Definition at line 285 of file stabilization_attitude_heli_indi.c.

References INDI_DOF, and update_butterworth_2_low_pass_int().

Referenced by stabilization_attitude_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void indi_apply_measurement_notch_filters ( int32_t  _out[],
int32_t  _in[] 
)

Definition at line 273 of file stabilization_attitude_heli_indi.c.

References indi_apply_notch_filters(), and measurement_notchfilter.

Referenced by stabilization_attitude_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void indi_apply_notch_filters ( struct SecondOrderNotchFilter filter,
int32_t  _out[],
int32_t  _in[] 
)
inlinestatic
void stabilization_attitude_enter ( void  )

Definition at line 565 of file stabilization_attitude_heli_indi.c.

References Int32Eulers::psi, stab_att_sp_euler, and stabilization_attitude_get_heading_i().

+ Here is the call graph for this function:

void stabilization_attitude_heli_indi_set_steadystate_pitch ( float  pitch)

stabilization_attitude_heli_indi_set_steadystate_pitch

Parameters
pitchneutral pitch angle [deg].

Change the neutral pitch angle.

Definition at line 298 of file stabilization_attitude_heli_indi.c.

References heli_indi_ctl, IndiController_int::sp_offset_pitch, and stabilization_attitude_heli_indi_set_steadystate_pitchroll().

+ Here is the call graph for this function:

void stabilization_attitude_heli_indi_set_steadystate_pitchroll ( void  )

stabilization_attitude_heli_indi_set_steadystate_pitchroll

Updates the neutral pitch and roll angles and calculates the compensation quaternion.

Definition at line 323 of file stabilization_attitude_heli_indi.c.

References float_quat_of_orientation_vect(), heli_indi_ctl, QUAT_BFP_OF_REAL, sp_offset, IndiController_int::sp_offset_pitch, IndiController_int::sp_offset_roll, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by stabilization_attitude_heli_indi_set_steadystate_pitch(), stabilization_attitude_heli_indi_set_steadystate_roll(), and stabilization_attitude_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void stabilization_attitude_heli_indi_set_steadystate_roll ( float  roll)

stabilization_attitude_heli_indi_set_steadystate_roll

Parameters
rollneutral roll angle [deg].

Change the neutral roll angle. Especially useful for helicopters, since they need a small roll angle in hover to compensate the tail force.

Definition at line 311 of file stabilization_attitude_heli_indi.c.

References heli_indi_ctl, IndiController_int::sp_offset_roll, and stabilization_attitude_heli_indi_set_steadystate_pitchroll().

+ Here is the call graph for this function:

void stabilization_attitude_init ( void  )

stabilization_attitude_init

Initialize the heli indi attitude controller.

Definition at line 342 of file stabilization_attitude_heli_indi.c.

References actuator_model, actuator_notchfilter, delayed_first_order_lowpass_filter_t::alpha, alpha_yaw_dec, alpha_yaw_inc, ANGLE_BFP_OF_REAL, IndiController_int::apply_actuator_filters, IndiController_int::apply_actuator_models, IndiController_int::apply_compensator_filters, IndiController_int::apply_measurement_filters, DELAYED_FIRST_ORDER_LOWPASS_FILTER_FILTER_ALPHA_SHIFT, delayed_first_order_lowpass_initialize(), IndiController_int::enable_notch, FALSE, heli_indi_ctl, indi_apply_actuator_butterworth_filters(), indi_apply_actuator_models(), indi_apply_actuator_notch_filters(), indi_apply_compensator_filters(), indi_apply_measurement_butterworth_filters(), indi_apply_measurement_notch_filters(), INDI_PITCH, INDI_ROLL, INDI_THRUST, INDI_YAW, init_butterworth_2_low_pass_int(), IndiController_int::invG, INVG_00, INVG_11, INVG_22, INVG_33, MAX_PPRZ, measurement_notchfilter, IndiController_int::motor_rpm, notch_filter_init(), IndiController_int::pitch_comp_angle, IndiController_int::roll_comp_angle, IndiController_int::sp_offset_pitch, IndiController_int::sp_offset_roll, STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_PITCH, STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_ROLL, STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_THRUST, STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_YAW, STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_PITCH, STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_ROLL, STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_THRUST, STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_YAW, STABILIZATION_ATTITUDE_HELI_INDI_PITCH_COMMAND_ROTATION, STABILIZATION_ATTITUDE_HELI_INDI_ROLL_COMMAND_ROTATION, stabilization_attitude_heli_indi_set_steadystate_pitchroll(), STABILIZATION_ATTITUDE_HELI_INDI_STEADY_STATE_PITCH, STABILIZATION_ATTITUDE_HELI_INDI_STEADY_STATE_ROLL, and TRUE.

+ Here is the call graph for this function:

void stabilization_attitude_read_rc ( bool  in_flight,
bool  in_carefree,
bool  coordinated_turn 
)
void stabilization_attitude_run ( bool  in_flight)

Definition at line 434 of file stabilization_attitude_heli_indi.c.

References __k, IndiController_int::actuator_out, IndiController_int::apply_actuator_filters, IndiController_int::apply_actuator_models, IndiController_int::apply_compensator_filters, IndiController_int::apply_measurement_filters, autopilot_get_motors_on(), IndiController_int::command_out, IndiController_int::du, IndiController_int::enable_notch, IndiController_int::error, IndiController_int::filtered_actuator, IndiController_int::filtered_measurement, heli_indi_ctl, INDI_DOF, INDI_NR_FILTERS, INDI_PITCH, INDI_ROLL, INDI_THRUST, INDI_YAW, INT32_INVG_FRAC, int32_quat_comp_inv(), int32_quat_inv_comp(), int32_quat_normalize(), int32_quat_wrap_shortest(), int32_rmat_vmult(), int32_vect_copy(), int32_vect_diff(), int32_vect_sum(), IndiController_int::invG, MAT_MUL_VECT, MAX_PPRZ, IndiController_int::measurement, IndiController_int::motor_rpm, Int32Rates::p, IndiController_int::pitch_comp_angle, HeliIndiGains::pitch_p, pprz_itrig_cos(), pprz_itrig_sin(), Int32Rates::q, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, Int32Rates::r, IndiController_int::reference, IndiController_int::roll_comp_angle, HeliIndiGains::roll_p, rpm_sensor_get_rpm(), sp_offset, stab_att_sp_quat, stabilization_cmd, stateGetAccelNed_i(), stateGetBodyRates_i(), stateGetNedToBodyQuat_i(), stateGetNedToBodyRMat_i(), IndiController_int::u_setpoint, NedCoor_i::x, Int32Vect3::x, NedCoor_i::y, Int32Vect3::y, HeliIndiGains::yaw_d, HeliIndiGains::yaw_p, NedCoor_i::z, and Int32Vect3::z.

+ Here is the call graph for this function:

void stabilization_attitude_set_earth_cmd_i ( struct Int32Vect2 cmd,
int32_t  heading 
)
void stabilization_attitude_set_failsafe_setpoint ( void  )
void stabilization_attitude_set_rpy_setpoint_i ( struct Int32Eulers rpy)

Definition at line 581 of file stabilization_attitude_heli_indi.c.

References int32_quat_of_eulers(), stab_att_sp_euler, and stab_att_sp_quat.

+ Here is the call graph for this function:

Variable Documentation

Butterworth2LowPass_int actuator_lowpass_filters[INDI_DOF]

Definition at line 146 of file stabilization_attitude_heli_indi.c.

Butterworth2LowPass_int measurement_lowpass_filters[INDI_DOF]

Definition at line 147 of file stabilization_attitude_heli_indi.c.