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

Go to the source code of this file.

Macros

#define STABILIZATION_INDI_FILT_CUTOFF   8.0
 
#define STABILIZATION_INDI_FILT_CUTOFF_R   STABILIZATION_INDI_FILT_CUTOFF
 
#define STABILIZATION_INDI_MAX_RATE   6.0
 
#define STABILIZATION_INDI_MAX_R   STABILIZATION_ATTITUDE_SP_MAX_R
 
#define STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF   4.0
 
#define INDI_EST_SCALE   0.001
 

Functions

static void stabilization_indi_calc_cmd (int32_t indi_commands[], struct Int32Quat *att_err, bool rate_control)
 Does the INDI calculations. More...
 
static void lms_estimation (void)
 This is a Least Mean Squares adaptive filter It estimates the actuator effectiveness online, by comparing the expected angular acceleration based on the inputs with the measured angular acceleration. More...
 
static void indi_init_filters (void)
 
static void send_att_indi (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)
 Set attitude quaternion setpoint from rpy. More...
 
void stabilization_indi_set_earth_cmd_i (struct Int32Vect2 *cmd, int32_t heading)
 Set attitude setpoint from command in earth axes. More...
 
static void filter_pqr (Butterworth2LowPass *filter, struct FloatRates *new_values)
 Update butterworth filter for p, q and r of a FloatRates struct. More...
 
static void finite_difference_from_filter (float *output, Butterworth2LowPass *filter)
 Caclulate finite difference form a filter array The filter already contains the previous values. More...
 
static void finite_difference (float output[3], float new[3], float old[3])
 Calculate derivative of an array via finite difference. More...
 
void stabilization_indi_run (bool in_flight, bool rate_control)
 runs stabilization indi More...
 
void stabilization_indi_read_rc (bool in_flight, bool in_carefree, bool coordinated_turn)
 This function reads rc commands. More...
 

Variables

struct Int32Eulers stab_att_sp_euler
 with INT32_ANGLE_FRAC More...
 
struct Int32Quat stab_att_sp_quat
 with INT32_QUAT_FRAC More...
 
static int32_t stabilization_att_indi_cmd [COMMANDS_NB]
 
struct IndiVariables indi
 

Macro Definition Documentation

#define INDI_EST_SCALE   0.001

Definition at line 83 of file stabilization_indi_simple.c.

Referenced by lms_estimation(), and send_att_indi().

#define STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF   4.0

Definition at line 71 of file stabilization_indi_simple.c.

Referenced by indi_init_filters(), and init_filters().

#define STABILIZATION_INDI_FILT_CUTOFF   8.0

Definition at line 50 of file stabilization_indi_simple.c.

Referenced by indi_init_filters(), and init_filters().

#define STABILIZATION_INDI_FILT_CUTOFF_R   STABILIZATION_INDI_FILT_CUTOFF

Definition at line 55 of file stabilization_indi_simple.c.

Referenced by indi_init_filters().

#define STABILIZATION_INDI_MAX_R   STABILIZATION_ATTITUDE_SP_MAX_R

Definition at line 67 of file stabilization_indi_simple.c.

#define STABILIZATION_INDI_MAX_RATE   6.0

Definition at line 59 of file stabilization_indi_simple.c.

Function Documentation

static void filter_pqr ( Butterworth2LowPass filter,
struct FloatRates new_values 
)
inlinestatic

Update butterworth filter for p, q and r of a FloatRates struct.

Parameters
filterThe filter array to use
new_valuesThe new values

Definition at line 249 of file stabilization_indi_simple.c.

References FloatRates::p, FloatRates::q, FloatRates::r, and update_butterworth_2_low_pass().

Referenced by lms_estimation(), and stabilization_indi_calc_cmd().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void finite_difference ( float  output[3],
float  new[3],
float  old[3] 
)
inlinestatic

Calculate derivative of an array via finite difference.

Parameters
output[3]The output array
new[3]The newest values
old[3]The values of the previous timestep

Definition at line 277 of file stabilization_indi_simple.c.

Referenced by lms_estimation().

+ Here is the caller graph for this function:

static void finite_difference_from_filter ( float *  output,
Butterworth2LowPass filter 
)
inlinestatic

Caclulate finite difference form a filter array The filter already contains the previous values.

Parameters
outputThe output array
filterThe filter array input

Definition at line 263 of file stabilization_indi_simple.c.

References SecondOrderLowPass::o.

Referenced by lms_estimation(), and stabilization_indi_calc_cmd().

+ Here is the caller graph for this function:

void indi_init_filters ( void  )
static
static void lms_estimation ( void  )
inlinestatic

This is a Least Mean Squares adaptive filter It estimates the actuator effectiveness online, by comparing the expected angular acceleration based on the inputs with the measured angular acceleration.

Definition at line 437 of file stabilization_indi_simple.c.

References IndiVariables::adaptive, IndiVariables::est, filter_pqr(), finite_difference(), finite_difference_from_filter(), float_vect_copy(), float_vect_scale(), IndiEstimation::g1, IndiVariables::g1, IndiEstimation::g2, IndiVariables::g2, INDI_EST_SCALE, IndiEstimation::mu, FloatRates::p, FloatRates::q, FloatRates::r, IndiEstimation::rate, IndiEstimation::rate_d, IndiEstimation::rate_dd, stateGetBodyRates_f(), IndiEstimation::u, IndiVariables::u_act_dyn, IndiEstimation::u_d, and IndiEstimation::u_dd.

Referenced by stabilization_indi_calc_cmd().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

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

Definition at line 141 of file stabilization_indi_simple.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_att_indi ( struct transport_tx *  trans,
struct link_device *  dev 
)
static
static void stabilization_indi_calc_cmd ( int32_t  indi_commands[],
struct Int32Quat att_err,
bool  rate_control 
)
inlinestatic
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 184 of file stabilization_indi_simple.c.

References IndiVariables::angular_accel_ref, ddu_estimation, du_estimation, FLOAT_RATES_ZERO, float_vect_zero(), indi_init_filters(), INDI_NUM_ACT, Int32Eulers::psi, stab_att_sp_euler, stabilization_attitude_get_heading_i(), IndiVariables::u_act_dyn, and IndiVariables::u_in.

+ Here is the call graph for this function:

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

This function reads rc commands.

Parameters
in_flightboolean that states if the UAV is in flight or not

Definition at line 420 of file stabilization_indi_simple.c.

References QUAT_BFP_OF_REAL, stab_att_sp_quat, stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(), and stabilization_attitude_read_rc_setpoint_quat_f().

+ Here is the call graph for this function:

void stabilization_indi_run ( bool  in_flight,
bool  rate_control 
)
void stabilization_indi_set_earth_cmd_i ( struct Int32Vect2 cmd,
int32_t  heading 
)

Set attitude setpoint from command in earth axes.

Parameters
cmdThe command in earth axes (North East)
headingThe desired heading

Definition at line 226 of file stabilization_indi_simple.c.

References heading, INT32_TRIG_FRAC, Int32Eulers::phi, PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, Int32Eulers::psi, quat_from_earth_cmd_i(), stab_att_sp_euler, stab_att_sp_quat, stateGetNedToBodyEulers_i(), Int32Eulers::theta, Int32Vect2::x, and Int32Vect2::y.

+ Here is the call graph for this function:

void stabilization_indi_set_failsafe_setpoint ( void  )

Function that calculates the failsafe setpoint.

Definition at line 197 of file stabilization_indi_simple.c.

References PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, Int32Quat::qi, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, stab_att_sp_quat, and stabilization_attitude_get_heading_i().

+ Here is the call graph for this function:

void stabilization_indi_set_rpy_setpoint_i ( struct Int32Eulers rpy)

Set attitude quaternion setpoint from rpy.

Parameters
rpyroll pitch yaw input

Definition at line 212 of file stabilization_indi_simple.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

struct IndiVariables indi

Definition at line 85 of file stabilization_indi_simple.c.

int32_t stabilization_att_indi_cmd[COMMANDS_NB]
static

Definition at line 77 of file stabilization_indi_simple.c.

Referenced by stabilization_indi_run().