stabilization_indi_simple.h File Reference

#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h"
#include "filters/low_pass_filter.h"

Include dependency graph for stabilization_indi_simple.h:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Data Structures
struct	ReferenceSystem
struct	IndiEstimation
struct	IndiVariables

Functions
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...
void	stabilization_indi_run (bool enable_integrator, 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 Int32Quat	stab_att_sp_quat
	with INT32_QUAT_FRAC More...
struct Int32Eulers	stab_att_sp_euler
	with INT32_ANGLE_FRAC More...
struct IndiVariables	indi

---

Data Structure Documentation

struct ReferenceSystem

Definition at line 34 of file stabilization_indi.h.

Data Fields
float	err_p
float	err_q
float	err_r
float	rate_p
float	rate_q
float	rate_r

struct IndiEstimation

Definition at line 52 of file stabilization_indi_simple.h.

Collaboration diagram for IndiEstimation:

Data Fields
struct FloatRates	g1
float	g2
float	mu
Butterworth2LowPass	rate[3]
float	rate_d[3]
float	rate_dd[3]
Butterworth2LowPass	u[3]
float	u_d[3]
float	u_dd[3]

struct IndiVariables

Definition at line 64 of file stabilization_indi_simple.h.

Collaboration diagram for IndiVariables:

Data Fields
bool	adaptive	Enable adataptive estimation.
struct FloatRates	angular_accel_ref
float	attitude_max_yaw_rate	Maximum yaw rate in atttiude control in rad/s.
struct FloatRates	du
struct IndiEstimation	est	Estimation parameters for adaptive INDI.
struct FloatRates	g1
float	g2
float	max_rate	Maximum rate in rate control in rad/s.
Butterworth2LowPass	rate[3]
float	rate_d[3]
struct ReferenceSystem	reference_acceleration
Butterworth2LowPass	u[3]
struct FloatRates	u_act_dyn
struct FloatRates	u_in

Function Documentation

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 253 of file stabilization_indi.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.

Referenced by stabilization_attitude_enter(), and stabilization_rate_enter().

Here is the call graph for this function:

Here is the caller graph for this function:

void stabilization_indi_init

(

void

)

Function that initializes important values upon engaging INDI.

Definition at line 203 of file stabilization_indi.c.

References act_is_servo, actuator_state_filt_vect, actuator_state_filt_vectd, actuator_state_filt_vectdd, Bwls, calc_g1g2_pseudo_inv(), DefaultPeriodic, estimation_rate_d, estimation_rate_dd, float_vect_copy(), float_vect_zero(), g1, g1_est, g1_init, g1g2, g2, g2_est, g2_init, indi_init_filters(), INDI_NUM_ACT, INDI_OUTPUTS, init_filters(), num_thrusters, register_periodic_telemetry(), rpm_cb(), RPM_SENSOR_ID, send_ahrs_ref_quat(), send_att_indi(), send_indi_g(), thrust_cb(), and THRUST_INCREMENT_ID.

Referenced by stabilization_attitude_init(), and stabilization_rate_init().

Here is the call graph for this function:

Here is the caller 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_flight

boolean that states if the UAV is in flight or not

Definition at line 519 of file stabilization_indi.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().

Referenced by stabilization_attitude_read_rc().

Here is the call graph for this function:

Here is the caller graph for this function:

void stabilization_indi_run	(	bool	in_flight,
		bool	rate_control
	)

runs stabilization indi

Parameters

enable_integrator
rate_control	boolean that determines if we are in rate control or attitude control

Function that should be called to run the INDI controller

Parameters

in_flight	not used
rate_control	rate control enabled, otherwise attitude control

Definition at line 477 of file stabilization_indi.c.

References angular_acceleration, estimation_rate_d, estimation_rate_dd, indi_thrust_increment_set, int32_quat_inv_comp(), int32_quat_normalize(), int32_quat_wrap_shortest(), MAX_PPRZ, SecondOrderLowPass::o, FloatRates::p, FloatRates::q, FloatRates::r, stab_att_sp_quat, stabilization_att_indi_cmd, stabilization_cmd, stabilization_indi_calc_cmd(), stateGetBodyRates_f(), stateGetNedToBodyQuat_i(), and update_butterworth_2_low_pass().

Referenced by stabilization_attitude_run(), and stabilization_rate_run().

Here is the call graph for this function:

Here is the caller graph for this function:

void stabilization_indi_set_earth_cmd_i	(	struct Int32Vect2 *	cmd,
		int32_t	heading
	)

Set attitude setpoint from command in earth axes.

Parameters

cmd	2D command in North East axes
heading	Heading of the setpoint

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

Parameters

cmd	The command in earth axes (North East)
heading	The desired heading

Definition at line 320 of file stabilization_indi.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.

Referenced by stabilization_attitude_set_earth_cmd_i().

Here is the call graph for this function:

Here is the caller graph for this function:

void stabilization_indi_set_failsafe_setpoint

(

void

)

Function that calculates the failsafe setpoint.

Definition at line 291 of file stabilization_indi.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().

Referenced by stabilization_attitude_set_failsafe_setpoint().

Here is the call graph for this function:

Here is the caller graph for this function:

void stabilization_indi_set_rpy_setpoint_i

(

struct Int32Eulers *

rpy

)

Set attitude quaternion setpoint from rpy.

Parameters

rpy	rpy from which to calculate quaternion setpoint

Function that calculates the setpoint quaternion from rpy

Parameters

rpy	roll pitch yaw input

Definition at line 306 of file stabilization_indi.c.

References int32_quat_of_eulers(), stab_att_sp_euler, and stab_att_sp_quat.

Referenced by stabilization_attitude_set_rpy_setpoint_i().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

struct IndiVariables indi

Definition at line 85 of file stabilization_indi_simple.c.

struct Int32Eulers stab_att_sp_euler

with INT32_ANGLE_FRAC

Definition at line 45 of file stabilization_attitude_euler_float.c.

Referenced by guidance_flip_run(), guidance_hybrid_set_cmd_i(), reset_psi_ref_from_body(), send_att(), send_att_ref(), stabilization_attitude_enter(), stabilization_attitude_init(), stabilization_attitude_read_rc(), stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(), stabilization_attitude_read_rc_setpoint_quat_f(), stabilization_attitude_run(), stabilization_attitude_set_earth_cmd_i(), stabilization_attitude_set_failsafe_setpoint(), stabilization_attitude_set_rpy_setpoint_i(), stabilization_indi_enter(), stabilization_indi_set_earth_cmd_i(), and stabilization_indi_set_rpy_setpoint_i().

struct Int32Quat stab_att_sp_quat

with INT32_QUAT_FRAC

Definition at line 127 of file stabilization_attitude_heli_indi.c.

Referenced by guidance_hybrid_set_cmd_i(), send_ahrs_ref_quat(), stabilization_attitude_init(), stabilization_attitude_read_rc(), stabilization_attitude_run(), stabilization_attitude_set_earth_cmd_i(), stabilization_attitude_set_failsafe_setpoint(), stabilization_attitude_set_rpy_setpoint_i(), stabilization_attitude_set_setpoint_rp_quat_f(), stabilization_indi_read_rc(), stabilization_indi_run(), stabilization_indi_set_earth_cmd_i(), stabilization_indi_set_failsafe_setpoint(), and stabilization_indi_set_rpy_setpoint_i().