 |
Paparazzi UAS
v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
|
Paparazzi UAS
v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
Helicopter quaternion INDI attitude stabilization. More...
#include "paparazzi.h"#include "math/pprz_algebra_float.h"#include "math/pprz_algebra_int.h"#include "math/pprz_simple_matrix.h"#include "state.h"#include "generated/airframe.h"#include "autopilot.h"#include "firmwares/rotorcraft/stabilization/stabilization_attitude_heli_indi.h"#include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h"#include "filters/low_pass_filter.h"
Include dependency graph for stabilization_attitude_heli_indi.c:Go to the source code of this file.
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_heli_indi_init (void) |
| stabilization_attitude_heli_indi_init More... | |
| void | stabilization_attitude_run (bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd) |
| Attitude control run function. More... | |
| void | stabilization_attitude_enter (void) |
| Attitude control enter function. More... | |
Variables | |
| static struct Int32Quat | stab_att_sp_quat |
| static struct Int32Eulers | stab_att_sp_euler |
| static struct Int32Quat | sp_offset |
| struct HeliIndiGains | heli_indi_gains |
| static 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] |
Helicopter quaternion INDI attitude stabilization.
Definition in file stabilization_attitude_heli_indi.c.
| #define INDI_NOTCH_MIN_RPM STABILIZATION_ATTITUDE_HELI_INDI_NOTCH_MIN_RPM |
Definition at line 116 of file stabilization_attitude_heli_indi.c.
| #define INT32_INVG_FRAC 16 |
Definition at line 123 of file stabilization_attitude_heli_indi.c.
| #define INVG_00 STABILIZATION_ATTITUDE_HELI_INDI_GINV_ROLL_TO_ROLL |
Definition at line 119 of file stabilization_attitude_heli_indi.c.
| #define INVG_11 STABILIZATION_ATTITUDE_HELI_INDI_GINV_PITCH_TO_PITCH |
Definition at line 120 of file stabilization_attitude_heli_indi.c.
| #define INVG_22 STABILIZATION_ATTITUDE_HELI_INDI_GINV_YAW_TO_YAW |
Definition at line 121 of file stabilization_attitude_heli_indi.c.
| #define INVG_33 -50000 |
Definition at line 122 of file stabilization_attitude_heli_indi.c.
| #define STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_DEFAULT 40.0 |
Definition at line 99 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 105 of file stabilization_attitude_heli_indi.c.
| #define STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_ROLL STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_DEFAULT |
Definition at line 102 of file stabilization_attitude_heli_indi.c.
| #define STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_THRUST STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_DEFAULT |
Definition at line 111 of file stabilization_attitude_heli_indi.c.
| #define STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_YAW STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_DEFAULT |
Definition at line 108 of file stabilization_attitude_heli_indi.c.
| #define STABILIZATION_ATTITUDE_HELI_INDI_GINV_PITCH_TO_PITCH 13873 |
Definition at line 72 of file stabilization_attitude_heli_indi.c.
| #define STABILIZATION_ATTITUDE_HELI_INDI_GINV_ROLL_TO_ROLL 11681 |
Definition at line 69 of file stabilization_attitude_heli_indi.c.
| #define STABILIZATION_ATTITUDE_HELI_INDI_GINV_YAW_TO_YAW 730 |
Definition at line 75 of file stabilization_attitude_heli_indi.c.
| #define STABILIZATION_ATTITUDE_HELI_INDI_NOTCH_MIN_RPM 1500 |
Definition at line 114 of file stabilization_attitude_heli_indi.c.
| #define STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_DEFAULT 10.0 |
Definition at line 84 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 90 of file stabilization_attitude_heli_indi.c.
| #define STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_ROLL STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_DEFAULT |
Definition at line 87 of file stabilization_attitude_heli_indi.c.
| #define STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_THRUST STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_DEFAULT |
Definition at line 96 of file stabilization_attitude_heli_indi.c.
| #define STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_YAW STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_DEFAULT |
Definition at line 93 of file stabilization_attitude_heli_indi.c.
| #define STABILIZATION_ATTITUDE_HELI_INDI_PITCH_COMMAND_ROTATION -30.0 |
Definition at line 81 of file stabilization_attitude_heli_indi.c.
| #define STABILIZATION_ATTITUDE_HELI_INDI_PITCH_P 8 |
Definition at line 60 of file stabilization_attitude_heli_indi.c.
| #define STABILIZATION_ATTITUDE_HELI_INDI_ROLL_COMMAND_ROTATION 11.0 |
Definition at line 78 of file stabilization_attitude_heli_indi.c.
| #define STABILIZATION_ATTITUDE_HELI_INDI_ROLL_P 12 |
Definition at line 57 of file stabilization_attitude_heli_indi.c.
| #define STABILIZATION_ATTITUDE_HELI_INDI_STEADY_STATE_PITCH 0 |
Definition at line 54 of file stabilization_attitude_heli_indi.c.
| #define STABILIZATION_ATTITUDE_HELI_INDI_STEADY_STATE_ROLL 4.5 |
Definition at line 51 of file stabilization_attitude_heli_indi.c.
| #define STABILIZATION_ATTITUDE_HELI_INDI_USE_NOTCHFILTER 0 |
Definition at line 40 of file stabilization_attitude_heli_indi.c.
| #define STABILIZATION_ATTITUDE_HELI_INDI_YAW_D 30 |
Definition at line 66 of file stabilization_attitude_heli_indi.c.
| #define STABILIZATION_ATTITUDE_HELI_INDI_YAW_P 10 |
Definition at line 63 of file stabilization_attitude_heli_indi.c.
Definition at line 270 of file stabilization_attitude_heli_indi.c.
References actuator_lowpass_filters, INDI_DOF, and update_butterworth_2_low_pass_int().
Referenced by stabilization_attitude_heli_indi_init().
Here is the call graph for this function: Here is the caller graph for this function:Definition at line 159 of file stabilization_attitude_heli_indi.c.
References actuator_model, delayed_first_order_lowpass_filter_t::alpha, alpha_yaw_dec, alpha_yaw_inc, delayed_first_order_lowpass_filter_t::buffer, delayed_first_order_lowpass_propagate(), delayed_first_order_lowpass_filter_t::idx, INDI_PITCH, INDI_ROLL, INDI_THRUST, INDI_YAW, and temp_pitch.
Referenced by stabilization_attitude_heli_indi_init().
Here is the call graph for this function: Here is the caller graph for this function:Definition at line 260 of file stabilization_attitude_heli_indi.c.
References actuator_notchfilter, and indi_apply_notch_filters().
Referenced by stabilization_attitude_heli_indi_init().
Here is the call graph for this function: Here is the caller graph for this function: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 198 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_heli_indi_init().
Here is the caller graph for this function:Definition at line 277 of file stabilization_attitude_heli_indi.c.
References INDI_DOF, measurement_lowpass_filters, and update_butterworth_2_low_pass_int().
Referenced by stabilization_attitude_heli_indi_init().
Here is the call graph for this function: Here is the caller graph for this function:Definition at line 265 of file stabilization_attitude_heli_indi.c.
References indi_apply_notch_filters(), and measurement_notchfilter.
Referenced by stabilization_attitude_heli_indi_init().
Here is the call graph for this function: Here is the caller graph for this function:
|
inlinestatic |
Definition at line 241 of file stabilization_attitude_heli_indi.c.
References IndiController_int::enable_notch, heli_indi_ctl, INDI_NOTCH_MIN_RPM, INDI_PITCH, INDI_ROLL, INDI_THRUST, INDI_YAW, IndiController_int::motor_rpm, notch_filter_set_filter_frequency(), and notch_filter_update().
Referenced by indi_apply_actuator_notch_filters(), and indi_apply_measurement_notch_filters().
Here is the call graph for this function: Here is the caller graph for this function:| void stabilization_attitude_enter | ( | void | ) |
Attitude control enter function.
Definition at line 557 of file stabilization_attitude_heli_indi.c.
| void stabilization_attitude_heli_indi_init | ( | void | ) |
stabilization_attitude_heli_indi_init
Initialize the heli indi attitude controller.
Definition at line 334 of file stabilization_attitude_heli_indi.c.
References actuator_lowpass_filters, 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_lowpass_filters, 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_heli_indi_set_steadystate_pitch | ( | float | pitch | ) |
stabilization_attitude_heli_indi_set_steadystate_pitch
| pitch | neutral pitch angle [deg]. |
Change the neutral pitch angle.
Definition at line 290 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 315 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_init(), stabilization_attitude_heli_indi_set_steadystate_pitch(), and stabilization_attitude_heli_indi_set_steadystate_roll().
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
| roll | neutral 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 303 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_run | ( | bool | in_flight, |
| struct StabilizationSetpoint * | sp, | ||
| struct ThrustSetpoint * | thrust, | ||
| int32_t * | cmd | ||
| ) |
Attitude control run function.
| [in] | in_flight | true if in flight |
| [in] | sp | pointer to the stabilization setpoint structure |
| [in] | thrust | pointer to the thrust setoint structure |
| [out] | cmd | pointer to the output command vector |
Definition at line 422 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, heli_indi_gains, 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_euler, stab_att_sp_quat, stab_sp_to_eulers_i(), stab_sp_to_quat_i(), stateGetAccelNed_i(), stateGetBodyRates_i(), stateGetNedToBodyQuat_i(), stateGetNedToBodyRMat_i(), th_sp_to_thrust_i(), THRUST_AXIS_Z, IndiController_int::u_setpoint, Int32Vect3::x, NedCoor_i::x, Int32Vect3::y, NedCoor_i::y, HeliIndiGains::yaw_d, HeliIndiGains::yaw_p, Int32Vect3::z, and NedCoor_i::z.
Here is the call graph for this function:| Butterworth2LowPass_int actuator_lowpass_filters[INDI_DOF] |
Definition at line 144 of file stabilization_attitude_heli_indi.c.
Referenced by indi_apply_actuator_butterworth_filters(), and stabilization_attitude_heli_indi_init().
| struct delayed_first_order_lowpass_filter_t actuator_model[INDI_DOF] |
Definition at line 1 of file stabilization_attitude_heli_indi.c.
Referenced by indi_apply_actuator_models(), indi_apply_compensator_filters(), and stabilization_attitude_heli_indi_init().
| struct SecondOrderNotchFilter actuator_notchfilter[INDI_DOF] |
Definition at line 145 of file stabilization_attitude_heli_indi.c.
Referenced by indi_apply_actuator_notch_filters(), and stabilization_attitude_heli_indi_init().
| int32_t alpha_yaw_dec |
Definition at line 142 of file stabilization_attitude_heli_indi.c.
Referenced by indi_apply_actuator_models(), indi_apply_compensator_filters(), and stabilization_attitude_heli_indi_init().
| int32_t alpha_yaw_inc |
Definition at line 141 of file stabilization_attitude_heli_indi.c.
Referenced by indi_apply_actuator_models(), indi_apply_compensator_filters(), and stabilization_attitude_heli_indi_init().
|
static |
Definition at line 1 of file stabilization_attitude_heli_indi.c.
Referenced by indi_apply_notch_filters(), stabilization_attitude_heli_indi_init(), stabilization_attitude_heli_indi_set_steadystate_pitch(), stabilization_attitude_heli_indi_set_steadystate_pitchroll(), stabilization_attitude_heli_indi_set_steadystate_roll(), and stabilization_attitude_run().
| struct HeliIndiGains heli_indi_gains |
Definition at line 1 of file stabilization_attitude_heli_indi.c.
Referenced by stabilization_attitude_run().
| Butterworth2LowPass_int measurement_lowpass_filters[INDI_DOF] |
Definition at line 145 of file stabilization_attitude_heli_indi.c.
Referenced by indi_apply_measurement_butterworth_filters(), and stabilization_attitude_heli_indi_init().
| struct SecondOrderNotchFilter measurement_notchfilter[INDI_DOF] |
Definition at line 145 of file stabilization_attitude_heli_indi.c.
Referenced by indi_apply_measurement_notch_filters(), and stabilization_attitude_heli_indi_init().
|
static |
Definition at line 1 of file stabilization_attitude_heli_indi.c.
Referenced by stabilization_attitude_heli_indi_set_steadystate_pitchroll(), and stabilization_attitude_run().
|
static |
Definition at line 1 of file stabilization_attitude_heli_indi.c.
Referenced by stabilization_attitude_run().
|
static |
Definition at line 1 of file stabilization_attitude_heli_indi.c.
Referenced by stabilization_attitude_run().
1.9.1 
