#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h"
#include "math/pprz_algebra_float.h"
#include "state.h"
#include "generated/airframe.h"
#include "modules/radio_control/radio_control.h"
#include "modules/actuators/actuators.h"
#include "modules/core/abi.h"
#include "filters/low_pass_filter.h"
#include "math/wls/wls_alloc.h"
#include <stdio.h>
#include "modules/datalink/telemetry.h"

Include dependency graph for stabilization_indi.c:

Macros
#define	INDI_ALLOWED_G_FACTOR 2.0

#define	STABILIZATION_INDI_FILT_CUTOFF_P 20.0

#define	STABILIZATION_INDI_FILT_CUTOFF_Q 20.0

#define	STABILIZATION_INDI_FILT_CUTOFF_R 20.0

#define	STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE FALSE

#define	STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER FALSE

#define	STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD 8.0

#define	STABILIZATION_INDI_USE_ADAPTIVE false

#define	STABILIZATION_INDI_ADAPTIVE_MU 0.001f

#define	STABILIZATION_INDI_ACT_IS_SERVO {0}

#define	STABILIZATION_INDI_ACT_IS_THRUSTER_X {0}

#define	STABILIZATION_INDI_ACT_IS_THRUSTER_Y {0}

#define	STABILIZATION_INDI_YAW_DISTURBANCE_LIMIT 99999.f
	Limit the maximum specific moment that can be compensated (units rad/s^2)

#define	STABILIZATION_INDI_WLS_PRIORITIES {1000, 1000, 1, 100}

#define	STABILIZATION_INDI_WLS_WU {[0 ... INDI_NUM_ACT - 1] = 1.0}

#define	STABILIZATION_INDI_ACT_PREF {0.0}

Functions
static void	lms_estimation (void)
	Function that estimates the control effectiveness of each actuator online.

static void	get_actuator_state (void)
	Function that tries to get actuator feedback.

static void	calc_g1_element (float dx_error, int8_t i, int8_t j, float mu_extra)

static void	calc_g2_element (float dx_error, int8_t j, float mu_extra)

static void	bound_g_mat (void)

static void	init_filters (void)
	Function that resets the filters to zeros.

static struct FloatRates	filter_rates (struct FloatRates *rates)

static void	sum_g1_g2 (void)
	Function that sums g1 and g2 to obtain the g1g2 matrix It also undoes the scaling that was done to make the values readable.

static void	send_wls_v_stab (struct transport_tx trans, struct link_device dev)

static void	send_wls_u_stab (struct transport_tx trans, struct link_device dev)

static void	send_eff_mat_g_indi (struct transport_tx trans, struct link_device dev)

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

static void	send_att_full_indi (struct transport_tx trans, struct link_device dev)

void	stabilization_indi_init (void)
	Function that initializes important values upon engaging INDI.

void	stabilization_indi_enter (void)
	Function that resets important values upon engaging INDI.

void	stabilization_indi_update_filt_freq (float freq)

void	stabilization_indi_rate_run (bool in_flight, struct StabilizationSetpoint sp, struct ThrustSetpoint thrust, int32_t *cmd)

void	stabilization_indi_attitude_run (bool in_flight, struct StabilizationSetpoint att_sp, struct ThrustSetpoint thrust, int32_t *cmd)

struct FloatRates WEAK	stabilization_indi_attitude_controller (struct FloatQuat att, struct FloatQuat att_sp, struct FloatRates rates_ff)
	Default PD angular rate controller.

struct FloatRates WEAK	stabilization_indi_rate_controller (struct FloatRates rates, struct FloatRates sp)
	Default PD angular acceleration controller.

void WEAK	stabilization_indi_set_wls_settings (void)
	Function that sets the u_min, u_max and u_pref if function not elsewhere defined.

Variables
struct WLS_t	wls_stab_p

float	act_pref [INDI_NUM_ACT] = STABILIZATION_INDI_ACT_PREF

float	indi_v [INDI_OUTPUTS]

float *	Bwls [INDI_OUTPUTS]

int32_t	stabilization_att_indi_cmd [COMMANDS_NB]

struct Indi_gains	indi_gains

bool	indi_use_adaptive = STABILIZATION_INDI_USE_ADAPTIVE

bool	act_is_servo [INDI_NUM_ACT] = STABILIZATION_INDI_ACT_IS_SERVO

static bool	act_thrust_mat [3][INDI_NUM_ACT]

float	act_first_order_cutoff [INDI_NUM_ACT] = STABILIZATION_INDI_ACT_FREQ

float	act_dyn_discrete [INDI_NUM_ACT]

float	stablization_indi_yaw_dist_limit = STABILIZATION_INDI_YAW_DISTURBANCE_LIMIT

float	actuator_state_filt_vect [INDI_NUM_ACT]

struct FloatRates	angular_accel_ref = {0., 0., 0.}

struct FloatRates	angular_rate_ref = {0., 0., 0.}

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

float	actuator_state [INDI_NUM_ACT]

float	indi_u [INDI_NUM_ACT]

struct FloatVect3	stab_thrust_filt = { 0.f, 0.f, 0.f }

float	q_filt = 0.0

float	r_filt = 0.0

float	stabilization_indi_filter_freq = 20.0

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

static struct Int32Eulers	stab_att_sp_euler

static struct Int32Quat	stab_att_sp_quat

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]

int16_t	actuators_pprz [INDI_NUM_ACT+1]
	PPRZ command to each actuator Can be used to directly control actuators from the control algorithm if the command_laws are set up appropriately in the airframe file.

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

static struct FirstOrderLowPass	rates_filt_fo [3]

struct FloatVect3	body_accel_f

Macro Definition Documentation

◆ INDI_ALLOWED_G_FACTOR

#define INDI_ALLOWED_G_FACTOR 2.0

Definition at line 47 of file stabilization_indi.c.

◆ STABILIZATION_INDI_ACT_IS_SERVO

#define STABILIZATION_INDI_ACT_IS_SERVO {0}

Definition at line 91 of file stabilization_indi.c.

◆ STABILIZATION_INDI_ACT_IS_THRUSTER_X

#define STABILIZATION_INDI_ACT_IS_THRUSTER_X {0}

Definition at line 95 of file stabilization_indi.c.

◆ STABILIZATION_INDI_ACT_IS_THRUSTER_Y

#define STABILIZATION_INDI_ACT_IS_THRUSTER_Y {0}

Definition at line 99 of file stabilization_indi.c.

◆ STABILIZATION_INDI_ACT_PREF

#define STABILIZATION_INDI_ACT_PREF {0.0}

Definition at line 142 of file stabilization_indi.c.

◆ STABILIZATION_INDI_ADAPTIVE_MU

#define STABILIZATION_INDI_ADAPTIVE_MU 0.001f

Definition at line 87 of file stabilization_indi.c.

◆ STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE

#define STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE FALSE

Definition at line 69 of file stabilization_indi.c.

◆ STABILIZATION_INDI_FILT_CUTOFF_P

#define STABILIZATION_INDI_FILT_CUTOFF_P 20.0

Definition at line 52 of file stabilization_indi.c.

◆ STABILIZATION_INDI_FILT_CUTOFF_Q

#define STABILIZATION_INDI_FILT_CUTOFF_Q 20.0

Definition at line 58 of file stabilization_indi.c.

◆ STABILIZATION_INDI_FILT_CUTOFF_R

#define STABILIZATION_INDI_FILT_CUTOFF_R 20.0

Definition at line 64 of file stabilization_indi.c.

◆ STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER

#define STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER FALSE

Definition at line 73 of file stabilization_indi.c.

◆ STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD

#define STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD 8.0

Definition at line 79 of file stabilization_indi.c.

◆ STABILIZATION_INDI_USE_ADAPTIVE

#define STABILIZATION_INDI_USE_ADAPTIVE false

Definition at line 83 of file stabilization_indi.c.

◆ STABILIZATION_INDI_WLS_PRIORITIES

#define STABILIZATION_INDI_WLS_PRIORITIES {1000, 1000, 1, 100}

Definition at line 130 of file stabilization_indi.c.

◆ STABILIZATION_INDI_WLS_WU

#define STABILIZATION_INDI_WLS_WU {[0 ... INDI_NUM_ACT - 1] = 1.0}

Definition at line 136 of file stabilization_indi.c.

◆ STABILIZATION_INDI_YAW_DISTURBANCE_LIMIT

#define STABILIZATION_INDI_YAW_DISTURBANCE_LIMIT 99999.f

Limit the maximum specific moment that can be compensated (units rad/s^2)

Definition at line 106 of file stabilization_indi.c.

Function Documentation

◆ bound_g_mat()

static void bound_g_mat ( void )

static

Definition at line 1136 of file stabilization_indi.c.

References foo, g1_est, g1_init, g2_est, g2_init, and INDI_ALLOWED_G_FACTOR.

Referenced by lms_estimation().

Here is the caller graph for this function:

◆ calc_g1_element()

void calc_g1_element	(	float	ddx_error,
		int8_t	i,
		int8_t	j,
		float	mu
	)

static

Parameters

ddx_error	error in output change
i	row of the matrix element
j	column of the matrix element
mu	learning 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 960 of file stabilization_indi.c.

References du_estimation, foo, and g1_est.

Referenced by lms_estimation().

Here is the caller graph for this function:

◆ calc_g2_element()

void calc_g2_element	(	float	ddx_error,
		int8_t	j,
		float	mu
	)

static

Parameters

ddx_error	error in output change
j	column of the matrix element
mu	learning 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 974 of file stabilization_indi.c.

References ddu_estimation, foo, and g2_est.

Referenced by lms_estimation().

Here is the caller graph for this function:

◆ filter_rates()

static struct FloatRates filter_rates ( struct FloatRates * rates )

static

Definition at line 542 of file stabilization_indi.c.

References foo, FloatRates::p, rates_filt_fo, update_butterworth_2_low_pass(), and update_first_order_low_pass().

Referenced by stabilization_indi_rate_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_actuator_state()

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

References act_dyn_discrete, act_obs, actuator_state, float_vect_copy(), foo, indi_u, and UNUSED.

Referenced by stabilization_indi_rate_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ init_filters()

static void init_filters ( void )

static

Function that resets the filters to zeros.

Definition at line 503 of file stabilization_indi.c.

References acceleration_lowpass_filter, actuator_lowpass_filters, estimation_input_lowpass_filters, estimation_output_lowpass_filters, foo, init_butterworth_2_low_pass(), init_first_order_low_pass(), measurement_lowpass_filters, p, rates_filt_fo, STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF, STABILIZATION_INDI_FILT_CUTOFF, STABILIZATION_INDI_FILT_CUTOFF_P, STABILIZATION_INDI_FILT_CUTOFF_Q, STABILIZATION_INDI_FILT_CUTOFF_R, and stateGetBodyRates_f().

Referenced by stabilization_indi_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ lms_estimation()

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

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

Referenced by stabilization_indi_rate_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ send_ahrs_ref_quat()

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

static

Definition at line 354 of file stabilization_indi.c.

References dev, foo, 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:

◆ send_att_full_indi()

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

static

Definition at line 368 of file stabilization_indi.c.

References angular_accel_ref, angular_acceleration, angular_rate_ref, dev, EULERS_FLOAT_OF_BFP, float_eulers_of_quat_zxy(), foo, FloatRates::p, FloatEulers::phi, FloatEulers::psi, FloatRates::q, QUAT_FLOAT_OF_BFP, FloatRates::r, stab_att_sp_euler, stab_att_sp_quat, stateGetBodyRates_f(), stateGetNedToBodyEulers_f(), stateGetNedToBodyQuat_f(), and FloatEulers::theta.

Referenced by stabilization_indi_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ send_eff_mat_g_indi()

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

static

Definition at line 344 of file stabilization_indi.c.

References dev, foo, g1g2, and g2_est.

Referenced by stabilization_indi_init().

Here is the caller graph for this function:

◆ send_wls_u_stab()

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

static

Definition at line 339 of file stabilization_indi.c.

References dev, send_wls_u(), and wls_stab_p.

Referenced by stabilization_indi_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ send_wls_v_stab()

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

static

Definition at line 335 of file stabilization_indi.c.

References dev, send_wls_v(), and wls_stab_p.

Referenced by stabilization_indi_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ stabilization_indi_attitude_controller()

struct FloatRates WEAK stabilization_indi_attitude_controller	(	struct FloatQuat	att,
		struct FloatQuat	att_sp,
		struct FloatRates	rates_ff
	)

Default PD angular rate controller.

Takes the current attitude filtered state and setpoint and compute the desired rates. Can be redefined elsewhere to use an other control scheme.

Definition at line 821 of file stabilization_indi.c.

References Indi_gains::att, float_quat_inv_comp_norm_shortest(), float_quat_tilt_twist(), foo, indi_gains, FloatRates::p, FloatRates::q, FloatRates::r, Indi_gains::rate, and RATES_ADD.

Referenced by stabilization_indi_attitude_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ stabilization_indi_attitude_run()

void stabilization_indi_attitude_run	(	bool	in_flight,
		struct StabilizationSetpoint *	att_sp,
		struct ThrustSetpoint *	thrust,
		int32_t *	cmd
	)

Parameters

	in_flight	enable integrator only in flight
	att_sp	attitude stabilization setpoint
	thrust	thrust setpoint
[out]	output	command vector

Function that should be called to run the INDI controller

Definition at line 799 of file stabilization_indi.c.

References angular_rate_ref, foo, StabilizationSetpoint::sp, stab_att_sp_euler, stab_att_sp_quat, stab_sp_from_rates_f(), stab_sp_to_eulers_i(), stab_sp_to_quat_f(), stab_sp_to_quat_i(), stab_sp_to_rates_f(), stabilization_indi_attitude_controller(), stabilization_indi_rate_run(), and stateGetNedToBodyQuat_f().

Referenced by stabilization_attitude_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ stabilization_indi_enter()

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

References ddu_estimation, du_estimation, float_vect_zero(), and foo.

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:

◆ stabilization_indi_init()

void stabilization_indi_init ( void )

Function that initializes important values upon engaging INDI.

Definition at line 403 of file stabilization_indi.c.

References act_dyn_discrete, act_feedback_cb(), act_feedback_ev, act_first_order_cutoff, act_is_servo, act_thrust_mat, actuator_state_filt_vect, actuator_state_filt_vectd, actuator_state_filt_vectdd, Bwls, DefaultPeriodic, estimation_rate_d, estimation_rate_dd, float_vect_copy(), float_vect_zero(), foo, g1, g1_est, g1_init, g1g2, g2, g2_est, g2_init, init_filters(), num_thrusters, register_periodic_telemetry(), send_ahrs_ref_quat(), send_att_full_indi(), send_eff_mat_g_indi(), send_wls_u_stab(), send_wls_v_stab(), and sum_g1_g2().

Here is the call graph for this function:

◆ stabilization_indi_rate_controller()

struct FloatRates WEAK stabilization_indi_rate_controller	(	struct FloatRates	rates,
		struct FloatRates	sp
	)

Default PD angular acceleration controller.

Takes the current rates filtered state and setpoint and compute the desired acceleration. Can be redefined elsewhere to use an other control scheme.

Definition at line 855 of file stabilization_indi.c.

References foo, indi_gains, FloatRates::p, FloatRates::q, FloatRates::r, and Indi_gains::rate.

Referenced by stabilization_indi_rate_run().

Here is the caller graph for this function:

◆ stabilization_indi_rate_run()

void stabilization_indi_rate_run	(	bool	in_flight,
		struct StabilizationSetpoint *	sp,
		struct ThrustSetpoint *	thrust,
		int32_t *	cmd
	)

Parameters

in_flight	boolean that states if the UAV is in flight or not
sp	rate setpoint
thrust	thrust setpoint
cmd	output command array

Function that calculates the INDI commands

Definition at line 583 of file stabilization_indi.c.

References ACCELS_FLOAT_OF_BFP, act_is_servo, act_thrust_mat, actuator_lowpass_filters, actuator_state, actuator_state_filt_vect, actuator_state_filt_vectd, actuator_state_filt_vectdd, actuators_pprz, angular_accel_ref, angular_acceleration, autopilot_get_motors_on(), body_accel_f, Bwls, estimation_input_lowpass_filters, estimation_output_lowpass_filters, estimation_rate_d, estimation_rate_dd, filter_rates(), float_mat_vect_mul(), FLOAT_VECT3_ZERO, float_vect_diff(), float_vect_dot_product(), float_vect_zero(), foo, g1g2_pseudo_inv, g2, get_actuator_state(), INDI_G_SCALING, indi_u, indi_use_adaptive, indi_v, lms_estimation(), MAX_PPRZ, measurement_lowpass_filters, num_thrusters, SecondOrderLowPass::o, FloatRates::p, FloatRates::q, FloatRates::r, radio_control, stab_sp_to_rates_f(), stab_thrust_filt, stabilization_indi_rate_controller(), stabilization_indi_set_wls_settings(), stablization_indi_yaw_dist_limit, stateGetAccelBody_i(), stateGetBodyRates_f(), th_sp_to_incr_f(), th_sp_to_thrust_i(), THRUST_AXIS_X, THRUST_AXIS_Y, THRUST_AXIS_Z, ThrustSetpoint::type, WLS_t::u, update_butterworth_2_low_pass(), WLS_t::v, RadioControl::values, VECT3_ADD, wls_alloc(), wls_stab_p, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by stabilization_indi_attitude_run(), and stabilization_rate_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ stabilization_indi_set_wls_settings()

void stabilization_indi_set_wls_settings ( void )

Function that sets the u_min, u_max and u_pref if function not elsewhere defined.

Definition at line 869 of file stabilization_indi.c.

References act_is_servo, act_pref, foo, guidance_h, GUIDANCE_H_MODE_HOVER, GUIDANCE_H_MODE_NAV, MAX_PPRZ, HorizontalGuidance::mode, STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD, stateGetAirspeed_f(), WLS_t::u_max, WLS_t::u_min, WLS_t::u_pref, and wls_stab_p.

Referenced by stabilization_indi_rate_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ stabilization_indi_update_filt_freq()

void stabilization_indi_update_filt_freq ( float freq )

Definition at line 484 of file stabilization_indi.c.

References foo, init_butterworth_2_low_pass(), init_first_order_low_pass(), p, rates_filt_fo, stabilization_indi_filter_freq, and stateGetBodyRates_f().

Here is the call graph for this function:

◆ sum_g1_g2()

void sum_g1_g2 ( void )

static

Function that sums g1 and g2 to obtain the g1g2 matrix It also undoes the scaling that was done to make the values readable.

Definition at line 1059 of file stabilization_indi.c.

References foo, g1, g1g2, g2, and INDI_G_SCALING.

Referenced by lms_estimation(), and stabilization_indi_init().

Here is the caller graph for this function:

Variable Documentation

◆ acceleration_lowpass_filter

Butterworth2LowPass acceleration_lowpass_filter

Definition at line 320 of file stabilization_indi.c.

Referenced by init_filters(), and lms_estimation().

◆ act_dyn_discrete

float act_dyn_discrete[INDI_NUM_ACT]

Definition at line 234 of file stabilization_indi.c.

Referenced by get_actuator_state(), and stabilization_indi_init().

◆ act_first_order_cutoff

float act_first_order_cutoff[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_FREQ

Definition at line 233 of file stabilization_indi.c.

Referenced by stabilization_indi_init().

◆ act_is_servo

bool act_is_servo[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_IS_SERVO

Definition at line 205 of file stabilization_indi.c.

Referenced by ground_detect_periodic(), stabilization_indi_init(), stabilization_indi_rate_run(), and stabilization_indi_set_wls_settings().

◆ act_obs

float act_obs[INDI_NUM_ACT]

Definition at line 269 of file stabilization_indi.c.

Referenced by get_actuator_state().

◆ act_pref

float act_pref[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_PREF

Definition at line 167 of file stabilization_indi.c.

Referenced by ctrl_eff_scheduling_periodic(), and stabilization_indi_set_wls_settings().

◆ act_thrust_mat

bool act_thrust_mat[3][INDI_NUM_ACT]

static

Initial value:

= {
  STABILIZATION_INDI_ACT_IS_THRUSTER_X,
  STABILIZATION_INDI_ACT_IS_THRUSTER_Y,
 
  {0}
 
}

Definition at line 212 of file stabilization_indi.c.

Referenced by stabilization_indi_init(), and stabilization_indi_rate_run().

◆ actuator_lowpass_filters

Butterworth2LowPass actuator_lowpass_filters[INDI_NUM_ACT]

Definition at line 316 of file stabilization_indi.c.

Referenced by init_filters(), and stabilization_indi_rate_run().

◆ actuator_state

float actuator_state[INDI_NUM_ACT]

Definition at line 244 of file stabilization_indi.c.

Referenced by get_actuator_state(), and stabilization_indi_rate_run().

◆ actuator_state_filt_vect

float actuator_state_filt_vect[INDI_NUM_ACT]

Definition at line 240 of file stabilization_indi.c.

Referenced by eff_scheduling_cyfoam_periodic(), eff_scheduling_periodic_b(), eff_scheduling_rotwing_update_cmd(), eff_scheduling_rotwing_update_hover_motor_effectiveness(), ground_detect_periodic(), guidance_indi_hybrid_set_wls_settings(), stabilization_indi_init(), stabilization_indi_rate_run(), and stabilization_indi_set_wls_settings().

◆ actuator_state_filt_vectd

float actuator_state_filt_vectd[INDI_NUM_ACT]

Definition at line 256 of file stabilization_indi.c.

Referenced by lms_estimation(), stabilization_indi_init(), and stabilization_indi_rate_run().

◆ actuator_state_filt_vectdd

float actuator_state_filt_vectdd[INDI_NUM_ACT]

Definition at line 257 of file stabilization_indi.c.

Referenced by lms_estimation(), stabilization_indi_init(), and stabilization_indi_rate_run().

◆ actuators_pprz

int16_t actuators_pprz[INDI_NUM_ACT+1]

PPRZ command to each actuator Can be used to directly control actuators from the control algorithm if the command_laws are set up appropriately in the airframe file.

FIXME add an extra slot for specific case (e.g. rotwing in simulation)

Definition at line 314 of file stabilization_indi.c.

Referenced by actuators_hitl_periodic(), control_mixing_heewing_attitude_direct(), control_mixing_heewing_attitude_plane(), control_mixing_heewing_nav_run(), ekf_aw_wrapper_fetch(), nps_autopilot_run_step(), rotwing_state_periodic(), schdule_control_effectiveness(), stabilization_indi_rate_run(), and stabilization_indi_set_wls_settings().

◆ angular_accel_ref

struct FloatRates angular_accel_ref = {0., 0., 0.}

Definition at line 241 of file stabilization_indi.c.

Referenced by send_att_full_indi(), and stabilization_indi_rate_run().

◆ angular_acceleration

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

Definition at line 243 of file stabilization_indi.c.

Referenced by send_att_full_indi(), and stabilization_indi_rate_run().

◆ angular_rate_ref

struct FloatRates angular_rate_ref = {0., 0., 0.}

Definition at line 242 of file stabilization_indi.c.

Referenced by send_att_full_indi(), and stabilization_indi_attitude_run().

◆ body_accel_f

struct FloatVect3 body_accel_f

Definition at line 326 of file stabilization_indi.c.

Referenced by ekf_aw_wrapper_fetch(), lms_estimation(), stabilization_indi_rate_run(), and stateGetAccelBody_f().

◆ Bwls

float* Bwls[INDI_OUTPUTS]

Definition at line 172 of file stabilization_indi.c.

Referenced by eff_scheduling_cyfoam_periodic(), stabilization_indi_init(), and stabilization_indi_rate_run().

◆ ddu_estimation

float ddu_estimation[INDI_NUM_ACT]

Definition at line 261 of file stabilization_indi.c.

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

◆ du_estimation

float du_estimation[INDI_NUM_ACT]

Definition at line 260 of file stabilization_indi.c.

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

◆ estimation_input_lowpass_filters

Butterworth2LowPass estimation_input_lowpass_filters[INDI_NUM_ACT]

Definition at line 317 of file stabilization_indi.c.

Referenced by init_filters(), and stabilization_indi_rate_run().

◆ estimation_output_lowpass_filters

Butterworth2LowPass estimation_output_lowpass_filters[3]

Definition at line 319 of file stabilization_indi.c.

Referenced by init_filters(), and stabilization_indi_rate_run().

◆ estimation_rate_d

float estimation_rate_d[INDI_NUM_ACT]

Definition at line 258 of file stabilization_indi.c.

Referenced by stabilization_indi_init(), and stabilization_indi_rate_run().

◆ estimation_rate_dd

float estimation_rate_dd[INDI_NUM_ACT]

Definition at line 259 of file stabilization_indi.c.

Referenced by lms_estimation(), stabilization_indi_init(), and stabilization_indi_rate_run().

◆ g1

float g1[INDI_OUTPUTS][INDI_NUM_ACT]

Initial value:

= {STABILIZATION_INDI_G1_ROLL,
                                        STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW, STABILIZATION_INDI_G1_THRUST
                                       }

Definition at line 301 of file stabilization_indi.c.

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

◆ g1_est

float g1_est[INDI_OUTPUTS][INDI_NUM_ACT]

Definition at line 308 of file stabilization_indi.c.

Referenced by bound_g_mat(), calc_g1_element(), lms_estimation(), and stabilization_indi_init().

◆ g1_init

float g1_init[INDI_OUTPUTS][INDI_NUM_ACT]

Definition at line 310 of file stabilization_indi.c.

Referenced by bound_g_mat(), and stabilization_indi_init().

◆ g1g2

float g1g2[INDI_OUTPUTS][INDI_NUM_ACT]

◆ g1g2_pseudo_inv

float g1g2_pseudo_inv[INDI_NUM_ACT][INDI_OUTPUTS]

Definition at line 289 of file stabilization_indi.c.

Referenced by stabilization_indi_rate_run().

◆ g1g2_trans_mult

float g1g2_trans_mult[INDI_OUTPUTS][INDI_OUTPUTS]

Definition at line 254 of file stabilization_indi.c.

◆ g1g2inv

float g1g2inv[INDI_OUTPUTS][INDI_OUTPUTS]

Definition at line 255 of file stabilization_indi.c.

◆ g2

float g2[INDI_NUM_ACT] = STABILIZATION_INDI_G2

Definition at line 290 of file stabilization_indi.c.

Referenced by gain_scheduling_periodic(), lms_estimation(), stabilization_indi_init(), stabilization_indi_rate_run(), and sum_g1_g2().

◆ g2_est

float g2_est[INDI_NUM_ACT]

Definition at line 309 of file stabilization_indi.c.

Referenced by bound_g_mat(), calc_g2_element(), lms_estimation(), send_eff_mat_g_indi(), and stabilization_indi_init().

◆ g2_init

float g2_init[INDI_NUM_ACT]

Definition at line 311 of file stabilization_indi.c.

Referenced by bound_g_mat(), and stabilization_indi_init().

◆ indi_gains

struct Indi_gains indi_gains

Initial value:

= {
  .att = {
    STABILIZATION_INDI_REF_ERR_P,
    STABILIZATION_INDI_REF_ERR_Q,
    STABILIZATION_INDI_REF_ERR_R
  },
  .rate = {
    STABILIZATION_INDI_REF_RATE_P,
    STABILIZATION_INDI_REF_RATE_Q,
    STABILIZATION_INDI_REF_RATE_R
  },
}

Definition at line 185 of file stabilization_indi.c.

Referenced by eff_scheduling_periodic_b(), stabilization_indi_attitude_controller(), and stabilization_indi_rate_controller().

◆ indi_u

float indi_u[INDI_NUM_ACT]

Definition at line 245 of file stabilization_indi.c.

Referenced by get_actuator_state(), and stabilization_indi_rate_run().

◆ indi_use_adaptive

bool indi_use_adaptive = STABILIZATION_INDI_USE_ADAPTIVE

Definition at line 198 of file stabilization_indi.c.

Referenced by stabilization_indi_rate_run().

◆ indi_v

float indi_v[INDI_OUTPUTS]

Definition at line 171 of file stabilization_indi.c.

Referenced by stabilization_indi_rate_run().

◆ measurement_lowpass_filters

Butterworth2LowPass measurement_lowpass_filters[3]

Definition at line 318 of file stabilization_indi.c.

Referenced by init_filters(), and stabilization_indi_rate_run().

◆ mu1

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

Definition at line 264 of file stabilization_indi.c.

Referenced by lms_estimation().

◆ mu2

float mu2 = 0.002

Definition at line 266 of file stabilization_indi.c.

Referenced by lms_estimation().

◆ num_thrusters

int32_t num_thrusters

Definition at line 272 of file stabilization_indi.c.

Referenced by stabilization_indi_init(), and stabilization_indi_rate_run().

◆ q_filt

float q_filt = 0.0

Definition at line 248 of file stabilization_indi.c.

◆ r_filt

float r_filt = 0.0

Definition at line 249 of file stabilization_indi.c.

◆ rates_filt_fo

struct FirstOrderLowPass rates_filt_fo[3]

static

Definition at line 324 of file stabilization_indi.c.

Referenced by filter_rates(), init_filters(), and stabilization_indi_update_filt_freq().

◆ stab_att_sp_euler

struct Int32Eulers stab_att_sp_euler

static

Definition at line 274 of file stabilization_indi.c.

Referenced by send_att_full_indi(), and stabilization_indi_attitude_run().

◆ stab_att_sp_quat

struct Int32Quat stab_att_sp_quat

static

Definition at line 275 of file stabilization_indi.c.

Referenced by send_ahrs_ref_quat(), send_att_full_indi(), and stabilization_indi_attitude_run().

◆ stab_thrust_filt

struct FloatVect3 stab_thrust_filt = { 0.f, 0.f, 0.f }

Definition at line 246 of file stabilization_indi.c.

Referenced by guidance_indi_calcG_yxz(), guidance_indi_set_wls_settings(), and stabilization_indi_rate_run().

◆ stabilization_att_indi_cmd

int32_t stabilization_att_indi_cmd[COMMANDS_NB]

Definition at line 184 of file stabilization_indi.c.

◆ stabilization_indi_filter_freq

float stabilization_indi_filter_freq = 20.0

Definition at line 251 of file stabilization_indi.c.

Referenced by stabilization_indi_update_filt_freq().

◆ stablization_indi_yaw_dist_limit

float stablization_indi_yaw_dist_limit = STABILIZATION_INDI_YAW_DISTURBANCE_LIMIT

Definition at line 237 of file stabilization_indi.c.

Referenced by stabilization_indi_rate_run().

◆ wls_stab_p

struct WLS_t wls_stab_p

Initial value:

= {
  .nu        = INDI_NUM_ACT,
  .nv        = INDI_OUTPUTS,
  .gamma_sq  = 10000.0,
  .v         = {0.0},
  .Wv        = STABILIZATION_INDI_WLS_PRIORITIES,
  .Wu        = STABILIZATION_INDI_WLS_WU,
  .u_pref    = {0.0},
  .u_min     = {0.0},
  .u_max     = {0.0},
  .PC        = 0.0,
  .SC        = 0.0,
  .iter      = 0
}

Definition at line 152 of file stabilization_indi.c.

Referenced by send_wls_u_stab(), send_wls_v_stab(), stabilization_indi_rate_run(), and stabilization_indi_set_wls_settings().

Macros

Functions

Variables

Macro Definition Documentation

◆ INDI_ALLOWED_G_FACTOR

◆ STABILIZATION_INDI_ACT_IS_SERVO

◆ STABILIZATION_INDI_ACT_IS_THRUSTER_X

◆ STABILIZATION_INDI_ACT_IS_THRUSTER_Y

◆ STABILIZATION_INDI_ACT_PREF

◆ STABILIZATION_INDI_ADAPTIVE_MU

◆ STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE

◆ STABILIZATION_INDI_FILT_CUTOFF_P

◆ STABILIZATION_INDI_FILT_CUTOFF_Q

◆ STABILIZATION_INDI_FILT_CUTOFF_R

◆ STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER

◆ STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD

◆ STABILIZATION_INDI_USE_ADAPTIVE

◆ STABILIZATION_INDI_WLS_PRIORITIES

◆ STABILIZATION_INDI_WLS_WU

◆ STABILIZATION_INDI_YAW_DISTURBANCE_LIMIT

Function Documentation

◆ bound_g_mat()

◆ calc_g1_element()

◆ calc_g2_element()

◆ filter_rates()

◆ get_actuator_state()

◆ init_filters()

◆ lms_estimation()

◆ send_ahrs_ref_quat()

◆ send_att_full_indi()

◆ send_eff_mat_g_indi()

◆ send_wls_u_stab()

◆ send_wls_v_stab()

◆ stabilization_indi_attitude_controller()

◆ stabilization_indi_attitude_run()

◆ stabilization_indi_enter()

◆ stabilization_indi_init()

◆ stabilization_indi_rate_controller()

◆ stabilization_indi_rate_run()

◆ stabilization_indi_set_wls_settings()

◆ stabilization_indi_update_filt_freq()

◆ sum_g1_g2()

Variable Documentation

◆ acceleration_lowpass_filter

◆ act_dyn_discrete

◆ act_first_order_cutoff

◆ act_is_servo

◆ act_obs

◆ act_pref

◆ act_thrust_mat

◆ actuator_lowpass_filters

◆ actuator_state

◆ actuator_state_filt_vect

◆ actuator_state_filt_vectd

◆ actuator_state_filt_vectdd

◆ actuators_pprz

◆ angular_accel_ref

◆ angular_acceleration

◆ angular_rate_ref

◆ body_accel_f

◆ Bwls

◆ ddu_estimation

◆ du_estimation

◆ estimation_input_lowpass_filters

◆ estimation_output_lowpass_filters

◆ estimation_rate_d

◆ estimation_rate_dd

◆ g1

◆ g1_est

◆ g1_init

◆ g1g2

◆ g1g2_pseudo_inv

◆ g1g2_trans_mult

◆ g1g2inv

◆ g2

◆ g2_est

◆ g2_init

◆ indi_gains

◆ indi_u

◆ indi_use_adaptive