Paparazzi UAS  v5.15_devel-81-gd13dafb
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
stabilization_indi.c File Reference
+ Include dependency graph for stabilization_indi.c:

Go to the source code of this file.

Macros

#define INDI_ALLOWED_G_FACTOR   2.0
 
#define STABILIZATION_INDI_MAX_RATE   6.0
 Maximum rate you can request in RC rate mode (rad/s) More...
 

Functions

static void lms_estimation (void)
 Function that estimates the control effectiveness of each actuator online. More...
 
static void get_actuator_state (void)
 Function that tries to get actuator feedback. More...
 
static void calc_g1_element (float ddx_error, int8_t i, int8_t j, float mu)
 
static void calc_g2_element (float ddx_error, int8_t j, float mu)
 
static void calc_g1g2_pseudo_inv (void)
 Function that calculates the pseudo-inverse of (G1+G2). More...
 
static void bound_g_mat (void)
 
static void rpm_cb (uint8_t sender_id, uint16_t *rpm, uint8_t num_act)
 
static void thrust_cb (uint8_t sender_id, float thrust_increment)
 
void init_filters (void)
 Function that resets the filters to zeros. More...
 
static void send_indi_g (struct transport_tx *trans, struct link_device *dev)
 
static void send_ahrs_ref_quat (struct transport_tx *trans, struct link_device *dev)
 
void stabilization_indi_init (void)
 Function that initializes important values upon engaging INDI. More...
 
void stabilization_indi_enter (void)
 Function that resets important values upon engaging INDI. More...
 
void stabilization_indi_set_failsafe_setpoint (void)
 Function that calculates the failsafe setpoint. More...
 
void stabilization_indi_set_rpy_setpoint_i (struct Int32Eulers *rpy)
 
void stabilization_indi_set_earth_cmd_i (struct Int32Vect2 *cmd, int32_t heading)
 
static void stabilization_indi_calc_cmd (struct Int32Quat *att_err, bool rate_control, bool in_flight)
 
void stabilization_indi_run (bool in_flight, bool rate_control)
 
void stabilization_indi_read_rc (bool in_flight, bool in_carefree, bool coordinated_turn)
 
static void rpm_cb (uint8_t sender_id, uint16_t UNUSED *rpm, uint8_t UNUSED num_act)
 
static void thrust_cb (uint8_t UNUSED sender_id, float thrust_increment)
 ABI callback that obtains the thrust increment from guidance INDI. More...
 

Variables

float du_min [INDI_NUM_ACT]
 
float du_max [INDI_NUM_ACT]
 
float du_pref [INDI_NUM_ACT]
 
float indi_v [INDI_OUTPUTS]
 
float * Bwls [INDI_OUTPUTS]
 
int num_iter = 0
 
int32_t stabilization_att_indi_cmd [COMMANDS_NB]
 
struct ReferenceSystem reference_acceleration
 
bool indi_use_adaptive = false
 
bool act_is_servo [INDI_NUM_ACT] = {0}
 
float act_pref [INDI_NUM_ACT] = {0.0}
 
float act_dyn [INDI_NUM_ACT] = STABILIZATION_INDI_ACT_DYN
 
static float Wv [INDI_OUTPUTS] = {1000, 1000, 1, 100}
 
float actuator_state_filt_vect [INDI_NUM_ACT]
 
struct FloatRates angular_accel_ref = {0., 0., 0.}
 
float angular_acceleration [3] = {0., 0., 0.}
 
float actuator_state [INDI_NUM_ACT]
 
float indi_u [INDI_NUM_ACT]
 
float indi_du [INDI_NUM_ACT]
 
float g2_times_du
 
float g1g2_trans_mult [INDI_OUTPUTS][INDI_OUTPUTS]
 
float g1g2inv [INDI_OUTPUTS][INDI_OUTPUTS]
 
float actuator_state_filt_vectd [INDI_NUM_ACT]
 
float actuator_state_filt_vectdd [INDI_NUM_ACT]
 
float estimation_rate_d [INDI_NUM_ACT]
 
float estimation_rate_dd [INDI_NUM_ACT]
 
float du_estimation [INDI_NUM_ACT]
 
float ddu_estimation [INDI_NUM_ACT]
 
float mu1 [INDI_OUTPUTS] = {0.00001, 0.00001, 0.000003, 0.000002}
 
float mu2 = 0.002
 
float act_obs [INDI_NUM_ACT]
 
int32_t num_thrusters
 
struct Int32Eulers stab_att_sp_euler
 with INT32_ANGLE_FRAC More...
 
struct Int32Quat stab_att_sp_quat
 with INT32_QUAT_FRAC More...
 
abi_event rpm_ev
 
abi_event thrust_ev
 
float indi_thrust_increment
 
bool indi_thrust_increment_set = false
 
float g1g2_pseudo_inv [INDI_NUM_ACT][INDI_OUTPUTS]
 
float g2 [INDI_NUM_ACT] = STABILIZATION_INDI_G2
 
float g1 [INDI_OUTPUTS][INDI_NUM_ACT]
 
float g1g2 [INDI_OUTPUTS][INDI_NUM_ACT]
 
float g1_est [INDI_OUTPUTS][INDI_NUM_ACT]
 
float g2_est [INDI_NUM_ACT]
 
float g1_init [INDI_OUTPUTS][INDI_NUM_ACT]
 
float g2_init [INDI_NUM_ACT]
 
Butterworth2LowPass actuator_lowpass_filters [INDI_NUM_ACT]
 
Butterworth2LowPass estimation_input_lowpass_filters [INDI_NUM_ACT]
 
Butterworth2LowPass measurement_lowpass_filters [3]
 
Butterworth2LowPass estimation_output_lowpass_filters [3]
 
Butterworth2LowPass acceleration_lowpass_filter
 
struct FloatVect3 body_accel_f
 

Macro Definition Documentation

#define INDI_ALLOWED_G_FACTOR   2.0

Definition at line 49 of file stabilization_indi.c.

Referenced by bound_g_mat().

#define STABILIZATION_INDI_MAX_RATE   6.0

Maximum rate you can request in RC rate mode (rad/s)

Definition at line 103 of file stabilization_indi.c.

Referenced by stabilization_indi_calc_cmd().

Function Documentation

static void bound_g_mat ( void  )
static

Definition at line 740 of file stabilization_indi.c.

References g1_est, g1_init, g2_est, g2_init, INDI_ALLOWED_G_FACTOR, INDI_NUM_ACT, and INDI_OUTPUTS.

Referenced by lms_estimation().

+ Here is the caller graph for this function:

void calc_g1_element ( float  ddx_error,
int8_t  i,
int8_t  j,
float  mu 
)
static
Parameters
ddx_errorerror in output change
irow of the matrix element
jcolumn of the matrix element
mulearning rate

Function that calculates an element of the G1 matrix. The elements are stored in a different matrix, because the old matrix is necessary to caclulate more elements.

Definition at line 571 of file stabilization_indi.c.

References du_estimation, and g1_est.

Referenced by lms_estimation().

+ Here is the caller graph for this function:

void calc_g1g2_pseudo_inv ( void  )
static

Function that calculates the pseudo-inverse of (G1+G2).

Definition at line 666 of file stabilization_indi.c.

References float_mat_inv_4d(), float_vect_scale(), g1, g1g2, g1g2_pseudo_inv, g1g2_trans_mult, g1g2inv, g2, INDI_G_SCALING, INDI_NUM_ACT, and INDI_OUTPUTS.

Referenced by lms_estimation(), and stabilization_indi_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void calc_g2_element ( float  ddx_error,
int8_t  j,
float  mu 
)
static
Parameters
ddx_errorerror in output change
jcolumn of the matrix element
mulearning rate

Function that calculates an element of the G2 matrix. The elements are stored in a different matrix, because the old matrix is necessary to caclulate more elements.

Definition at line 585 of file stabilization_indi.c.

References ddu_estimation, and g2_est.

Referenced by lms_estimation().

+ Here is the caller graph for this function:

void get_actuator_state ( void  )
static

Function that tries to get actuator feedback.

If this is not available it will use a first order filter to approximate the actuator state. It is also possible to model rate limits (unit: PPRZ/loop cycle)

Definition at line 535 of file stabilization_indi.c.

References act_dyn, act_obs, actuator_state, float_vect_copy(), INDI_NUM_ACT, indi_u, and UNUSED.

Referenced by stabilization_indi_calc_cmd().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void init_filters ( void  )

Function that resets the filters to zeros.

Definition at line 266 of file stabilization_indi.c.

References INDI_NUM_ACT, init_butterworth_2_low_pass(), STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF, and STABILIZATION_INDI_FILT_CUTOFF.

Referenced by stabilization_indi_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void lms_estimation ( void  )
static

Function that estimates the control effectiveness of each actuator online.

It is assumed that disturbances do not play a large role. All elements of the G1 and G2 matrices are be estimated.

Definition at line 595 of file stabilization_indi.c.

References ACCELS_FLOAT_OF_BFP, actuator_state_filt_vectd, actuator_state_filt_vectdd, body_accel_f, bound_g_mat(), calc_g1_element(), calc_g1g2_pseudo_inv(), calc_g2_element(), ddu_estimation, du_estimation, estimation_rate_dd, float_vect_copy(), float_vect_smul(), g1, g1_est, g2, g2_est, INDI_NUM_ACT, INDI_OUTPUTS, mu1, mu2, SecondOrderLowPass::o, stateGetAccelBody_i(), update_butterworth_2_low_pass(), and FloatVect3::z.

Referenced by stabilization_indi_calc_cmd().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void rpm_cb ( uint8_t  sender_id,
uint16_t rpm,
uint8_t  num_act 
)
static

Referenced by stabilization_indi_init().

+ Here is the caller graph for this function:

static void rpm_cb ( uint8_t  sender_id,
uint16_t UNUSED rpm,
uint8_t UNUSED  num_act 
)
static

Definition at line 719 of file stabilization_indi.c.

References act_obs, and MAX_PPRZ.

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

Definition at line 186 of file stabilization_indi.c.

References Int32Quat::qi, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, stab_att_sp_quat, and stateGetNedToBodyQuat_i().

Referenced by stabilization_indi_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

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

Definition at line 177 of file stabilization_indi.c.

References g1_est, g2_est, and INDI_NUM_ACT.

Referenced by stabilization_indi_init().

+ Here is the caller graph for this function:

void stabilization_indi_enter ( void  )

Function that resets important values upon engaging INDI.

Don't reset inputs and filters, because it is unlikely to switch stabilization in flight, and there are multiple modes that use (the same) stabilization. Resetting the controller is not so nice when you are flying. FIXME: Ideally we should detect when coming from something that is not INDI

Definition at line 254 of file stabilization_indi.c.

Referenced by stabilization_attitude_enter(), and stabilization_rate_enter().

+ Here is the caller graph for this function:

void stabilization_indi_init ( void  )

Function that initializes important values upon engaging INDI.

Definition at line 204 of file stabilization_indi.c.

Referenced by stabilization_attitude_init(), and stabilization_rate_init().

+ Here is the caller graph for this function:

void stabilization_indi_read_rc ( bool  in_flight,
bool  in_carefree,
bool  coordinated_turn 
)

Definition at line 517 of file stabilization_indi.c.

Referenced by stabilization_attitude_read_rc().

+ Here is the caller graph for this function:

void stabilization_indi_run ( bool  in_flight,
bool  rate_control 
)
Parameters
enable_integrator
rate_controlboolean that determines if we are in rate control or attitude control

Function that should be called to run the INDI controller

Definition at line 475 of file stabilization_indi.c.

Referenced by stabilization_attitude_run(), and stabilization_rate_run().

+ Here is the caller graph for this function:

void stabilization_indi_set_earth_cmd_i ( struct Int32Vect2 cmd,
int32_t  heading 
)
Parameters
cmd2D command in North East axes
headingHeading of the setpoint

Function that calculates the setpoint quaternion from a command in earth axes

Definition at line 321 of file stabilization_indi.c.

Referenced by stabilization_attitude_set_earth_cmd_i().

+ Here is the caller graph for this function:

void stabilization_indi_set_failsafe_setpoint ( void  )

Function that calculates the failsafe setpoint.

Definition at line 292 of file stabilization_indi.c.

Referenced by stabilization_attitude_set_failsafe_setpoint().

+ Here is the caller graph for this function:

void stabilization_indi_set_rpy_setpoint_i ( struct Int32Eulers rpy)
Parameters
rpyrpy from which to calculate quaternion setpoint

Function that calculates the setpoint quaternion from rpy

Definition at line 307 of file stabilization_indi.c.

Referenced by stabilization_attitude_set_rpy_setpoint_i().

+ Here is the caller graph for this function:

static void thrust_cb ( uint8_t  sender_id,
float  thrust_increment 
)
static

Referenced by stabilization_indi_init().

+ Here is the caller graph for this function:

static void thrust_cb ( uint8_t UNUSED  sender_id,
float  thrust_increment 
)
static

ABI callback that obtains the thrust increment from guidance INDI.

Definition at line 734 of file stabilization_indi.c.

References indi_thrust_increment, and indi_thrust_increment_set.

Variable Documentation

Butterworth2LowPass acceleration_lowpass_filter

Definition at line 169 of file stabilization_indi.c.

float act_dyn[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_DYN

Definition at line 99 of file stabilization_indi.c.

Referenced by get_actuator_state().

bool act_is_servo[INDI_NUM_ACT] = {0}

Definition at line 88 of file stabilization_indi.c.

Referenced by stabilization_indi_calc_cmd(), and stabilization_indi_init().

float act_obs[INDI_NUM_ACT]

Definition at line 138 of file stabilization_indi.c.

Referenced by get_actuator_state(), and rpm_cb().

float act_pref[INDI_NUM_ACT] = {0.0}

Definition at line 96 of file stabilization_indi.c.

Referenced by stabilization_indi_calc_cmd().

Butterworth2LowPass actuator_lowpass_filters[INDI_NUM_ACT]

Definition at line 165 of file stabilization_indi.c.

float actuator_state[INDI_NUM_ACT]

Definition at line 117 of file stabilization_indi.c.

Referenced by get_actuator_state(), and stabilization_indi_calc_cmd().

float actuator_state_filt_vect[INDI_NUM_ACT]

Definition at line 114 of file stabilization_indi.c.

Referenced by stabilization_indi_calc_cmd(), and stabilization_indi_init().

float actuator_state_filt_vectd[INDI_NUM_ACT]
float actuator_state_filt_vectdd[INDI_NUM_ACT]
struct FloatRates angular_accel_ref = {0., 0., 0.}

Definition at line 115 of file stabilization_indi.c.

float angular_acceleration[3] = {0., 0., 0.}

Definition at line 116 of file stabilization_indi.c.

Referenced by stabilization_indi_calc_cmd(), and stabilization_indi_run().

struct FloatVect3 body_accel_f

Definition at line 171 of file stabilization_indi.c.

Referenced by lms_estimation().

float* Bwls[INDI_OUTPUTS]

Definition at line 55 of file stabilization_indi.c.

Referenced by stabilization_indi_calc_cmd(), and stabilization_indi_init().

float ddu_estimation[INDI_NUM_ACT]

Definition at line 130 of file stabilization_indi.c.

Referenced by calc_g2_element(), lms_estimation(), and stabilization_indi_enter().

float du_estimation[INDI_NUM_ACT]

Definition at line 129 of file stabilization_indi.c.

Referenced by calc_g1_element(), lms_estimation(), and stabilization_indi_enter().

float du_max[INDI_NUM_ACT]

Definition at line 52 of file stabilization_indi.c.

Referenced by stabilization_indi_calc_cmd().

float du_min[INDI_NUM_ACT]

Definition at line 51 of file stabilization_indi.c.

Referenced by stabilization_indi_calc_cmd().

float du_pref[INDI_NUM_ACT]

Definition at line 53 of file stabilization_indi.c.

Referenced by stabilization_indi_calc_cmd().

Butterworth2LowPass estimation_input_lowpass_filters[INDI_NUM_ACT]

Definition at line 166 of file stabilization_indi.c.

Butterworth2LowPass estimation_output_lowpass_filters[3]

Definition at line 168 of file stabilization_indi.c.

float estimation_rate_d[INDI_NUM_ACT]

Definition at line 127 of file stabilization_indi.c.

Referenced by stabilization_indi_init(), and stabilization_indi_run().

float estimation_rate_dd[INDI_NUM_ACT]
Initial value:
= {STABILIZATION_INDI_G1_ROLL,
STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW, STABILIZATION_INDI_G1_THRUST
}

Definition at line 156 of file stabilization_indi.c.

Referenced by calc_g1g2_pseudo_inv(), gain_scheduling_periodic(), lms_estimation(), and stabilization_indi_init().

float g1_init[INDI_OUTPUTS][INDI_NUM_ACT]

Definition at line 162 of file stabilization_indi.c.

Referenced by bound_g_mat(), and stabilization_indi_init().

float g1g2_pseudo_inv[INDI_NUM_ACT][INDI_OUTPUTS]

Definition at line 154 of file stabilization_indi.c.

Referenced by calc_g1g2_pseudo_inv(), and stabilization_indi_calc_cmd().

float g1g2_trans_mult[INDI_OUTPUTS][INDI_OUTPUTS]

Definition at line 123 of file stabilization_indi.c.

Referenced by calc_g1g2_pseudo_inv().

float g1g2inv[INDI_OUTPUTS][INDI_OUTPUTS]

Definition at line 124 of file stabilization_indi.c.

Referenced by calc_g1g2_pseudo_inv().

float g2_init[INDI_NUM_ACT]

Definition at line 163 of file stabilization_indi.c.

Referenced by bound_g_mat(), and stabilization_indi_init().

float g2_times_du

Definition at line 120 of file stabilization_indi.c.

Referenced by stabilization_indi_calc_cmd().

float indi_du[INDI_NUM_ACT]

Definition at line 119 of file stabilization_indi.c.

Referenced by stabilization_indi_calc_cmd().

float indi_thrust_increment

Definition at line 151 of file stabilization_indi.c.

Referenced by stabilization_indi_calc_cmd(), and thrust_cb().

bool indi_thrust_increment_set = false
float indi_u[INDI_NUM_ACT]

Definition at line 118 of file stabilization_indi.c.

Referenced by get_actuator_state(), and stabilization_indi_calc_cmd().

bool indi_use_adaptive = false

Definition at line 78 of file stabilization_indi.c.

Referenced by stabilization_indi_calc_cmd().

float indi_v[INDI_OUTPUTS]

Definition at line 54 of file stabilization_indi.c.

Referenced by stabilization_indi_calc_cmd().

Butterworth2LowPass measurement_lowpass_filters[3]

Definition at line 167 of file stabilization_indi.c.

float mu1[INDI_OUTPUTS] = {0.00001, 0.00001, 0.000003, 0.000002}

Definition at line 133 of file stabilization_indi.c.

Referenced by lms_estimation().

float mu2 = 0.002

Definition at line 135 of file stabilization_indi.c.

Referenced by lms_estimation().

int num_iter = 0

Definition at line 56 of file stabilization_indi.c.

Referenced by stabilization_indi_calc_cmd().

int32_t num_thrusters

Definition at line 141 of file stabilization_indi.c.

Referenced by stabilization_indi_calc_cmd(), and stabilization_indi_init().

struct ReferenceSystem reference_acceleration
Initial value:
= {
STABILIZATION_INDI_REF_ERR_P,
STABILIZATION_INDI_REF_ERR_Q,
STABILIZATION_INDI_REF_ERR_R,
STABILIZATION_INDI_REF_RATE_P,
STABILIZATION_INDI_REF_RATE_Q,
STABILIZATION_INDI_REF_RATE_R,
}

Definition at line 66 of file stabilization_indi.c.

abi_event rpm_ev

Definition at line 146 of file stabilization_indi.c.

int32_t stabilization_att_indi_cmd[COMMANDS_NB]

Definition at line 65 of file stabilization_indi.c.

abi_event thrust_ev

Definition at line 149 of file stabilization_indi.c.

float Wv[INDI_OUTPUTS] = {1000, 1000, 1, 100}
static

Definition at line 110 of file stabilization_indi.c.

Referenced by stabilization_indi_calc_cmd().