#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude.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 "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	INDI_HROTTLE_LIMIT_AIRSPEED_FWD 8.0

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 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	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...

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. 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_quat_setpoint_i (struct Int32Quat *quat)

void	stabilization_indi_set_earth_cmd_i (struct Int32Vect2 *cmd, int32_t heading)

void	stabilization_indi_set_stab_sp (struct StabilizationSetpoint *sp)
	Set attitude setpoint from stabilization setpoint struct. More...

void	stabilization_indi_rate_run (struct FloatRates rate_sp, bool in_flight)

void	stabilization_indi_attitude_run (struct Int32Quat quat_sp, bool in_flight)

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 Indi_gains	indi_gains

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	indi_Wu [INDI_NUM_ACT] = {[0 ... INDI_NUM_ACT-1] = 1.0}
	Weighting of different actuators in the cost function. More...

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	q_filt = 0.0

float	r_filt = 0.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

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

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

◆ INDI_HROTTLE_LIMIT_AIRSPEED_FWD

#define INDI_HROTTLE_LIMIT_AIRSPEED_FWD 8.0

Definition at line 72 of file stabilization_indi.c.

◆ STABILIZATION_INDI_FILT_CUTOFF_P

#define STABILIZATION_INDI_FILT_CUTOFF_P 20.0

Definition at line 54 of file stabilization_indi.c.

◆ STABILIZATION_INDI_FILT_CUTOFF_Q

#define STABILIZATION_INDI_FILT_CUTOFF_Q 20.0

Definition at line 60 of file stabilization_indi.c.

◆ STABILIZATION_INDI_FILT_CUTOFF_R

#define STABILIZATION_INDI_FILT_CUTOFF_R 20.0

Definition at line 66 of file stabilization_indi.c.

Function Documentation

◆ bound_g_mat()

static void bound_g_mat ( void )

static

Definition at line 870 of file stabilization_indi.c.

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

References du_estimation, g1_est, and mu.

Referenced by lms_estimation().

Here is the caller graph for this function:

◆ calc_g1g2_pseudo_inv()

void calc_g1g2_pseudo_inv ( void )

static

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

Make sure to sum of G1 and G2 before running this!

Definition at line 809 of file stabilization_indi.c.

References float_mat_inv_4d(), float_vect_scale(), g1g2, g1g2_pseudo_inv, g1g2_trans_mult, and g1g2inv.

Referenced by lms_estimation(), and stabilization_indi_init().

Here is the call graph for this function:

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

References ddu_estimation, g2_est, and mu.

Referenced by lms_estimation().

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

References act_dyn, act_obs, actuator_state, float_vect_copy(), 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()

void init_filters ( void )

Function that resets the filters to zeros.

Definition at line 304 of file stabilization_indi.c.

References acceleration_lowpass_filter, actuator_lowpass_filters, estimation_input_lowpass_filters, estimation_output_lowpass_filters, 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 708 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_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, mu1, mu2, SecondOrderLowPass::o, 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:

◆ rpm_cb() [1/2]

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:

◆ rpm_cb() [2/2]

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

static

Definition at line 849 of file stabilization_indi.c.

References act_obs, MAX_PPRZ, and rpm.

◆ send_ahrs_ref_quat()

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

static

Definition at line 223 of file stabilization_indi.c.

References dev, 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_indi_g()

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

static

Definition at line 214 of file stabilization_indi.c.

References dev, g1_est, and g2_est.

Referenced by stabilization_indi_init().

Here is the caller graph for this function:

◆ stabilization_indi_attitude_run()

void stabilization_indi_attitude_run	(	struct Int32Quat	quat_sp,
		bool	in_flight
	)

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

Definition at line 586 of file stabilization_indi.c.

References Indi_gains::att, float_quat_inv_comp_norm_shortest(), float_quat_tilt_twist(), indi_gains, indi_thrust_increment_set, FloatRates::p, FloatRates::q, QUAT_FLOAT_OF_BFP, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, FloatRates::r, Indi_gains::rate, stabilization_indi_rate_run(), stateGetNedToBodyQuat_f(), FloatVect3::x, FloatVect3::y, and FloatVect3::z.

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

References ddu_estimation, du_estimation, float_vect_zero(), Int32Eulers::psi, stab_att_sp_euler, and stabilization_attitude_get_heading_i().

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 241 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, init_filters(), num_thrusters, register_periodic_telemetry(), rpm_cb(), rpm_ev, RPM_SENSOR_ID, send_ahrs_ref_quat(), send_indi_g(), sum_g1_g2(), thrust_cb(), thrust_ev, and THRUST_INCREMENT_ID.

Here is the call graph for this function:

◆ stabilization_indi_rate_run()

void stabilization_indi_rate_run	(	struct FloatRates	rate_sp,
		bool	in_flight
	)

Parameters

att_err	attitude error
rate_control	boolean that states if we are in rate control or attitude control
in_flight	boolean that states if the UAV is in flight or not

Function that calculates the INDI commands

Definition at line 410 of file stabilization_indi.c.

References act_is_servo, act_pref, actuator_lowpass_filters, actuator_state, actuator_state_filt_vect, actuator_state_filt_vectd, actuator_state_filt_vectdd, angular_accel_ref, angular_acceleration, autopilot_get_motors_on(), Bwls, du_max, du_min, du_pref, estimation_input_lowpass_filters, estimation_output_lowpass_filters, estimation_rate_d, estimation_rate_dd, float_vect_copy(), float_vect_sum(), g1g2_pseudo_inv, g2, g2_times_du, get_actuator_state(), guidance_h, GUIDANCE_H_MODE_HOVER, GUIDANCE_H_MODE_NAV, indi_du, INDI_G_SCALING, indi_gains, INDI_HROTTLE_LIMIT_AIRSPEED_FWD, indi_thrust_increment, indi_thrust_increment_set, indi_u, indi_use_adaptive, indi_v, indi_Wu, lms_estimation(), MAX_PPRZ, measurement_lowpass_filters, HorizontalGuidance::mode, num_iter, num_thrusters, SecondOrderLowPass::o, FloatRates::p, FloatRates::q, FloatRates::r, Indi_gains::rate, rates_filt_fo, stabilization_cmd, stateGetAirspeed_f(), stateGetBodyRates_f(), update_butterworth_2_low_pass(), update_first_order_low_pass(), wls_alloc(), and Wv.

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_read_rc()

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

Definition at line 630 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:

◆ stabilization_indi_set_earth_cmd_i()

void stabilization_indi_set_earth_cmd_i	(	struct Int32Vect2 *	cmd,
		int32_t	heading
	)

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

Definition at line 375 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:

◆ stabilization_indi_set_failsafe_setpoint()

void stabilization_indi_set_failsafe_setpoint ( void )

Function that calculates the failsafe setpoint.

Definition at line 337 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:

◆ stabilization_indi_set_quat_setpoint_i()

void stabilization_indi_set_quat_setpoint_i ( struct Int32Quat * quat )

Parameters

quat	quaternion setpoint

Definition at line 363 of file stabilization_indi.c.

References int32_eulers_of_quat(), stab_att_sp_euler, and stab_att_sp_quat.

Referenced by stabilization_attitude_set_quat_setpoint_i().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ stabilization_indi_set_rpy_setpoint_i()

void stabilization_indi_set_rpy_setpoint_i ( struct Int32Eulers * rpy )

Parameters

rpy	rpy from which to calculate quaternion setpoint

Function that calculates the setpoint quaternion from rpy

Definition at line 352 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:

◆ stabilization_indi_set_stab_sp()

void stabilization_indi_set_stab_sp ( struct StabilizationSetpoint * sp )

Set attitude setpoint from stabilization setpoint struct.

Parameters

sp	Stabilization setpoint structure

Definition at line 397 of file stabilization_indi.c.

References stab_att_sp_euler, stab_att_sp_quat, stab_sp_to_eulers_i(), and stab_sp_to_quat_i().

Referenced by stabilization_attitude_set_stab_sp().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ sum_g1_g2()

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.

Definition at line 791 of file stabilization_indi.c.

References g1, g1g2, g2, and INDI_G_SCALING.

Referenced by lms_estimation(), and stabilization_indi_init().

Here is the caller graph for this function:

◆ thrust_cb() [1/2]

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:

◆ thrust_cb() [2/2]

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

References indi_thrust_increment, and indi_thrust_increment_set.

Variable Documentation

◆ acceleration_lowpass_filter

Butterworth2LowPass acceleration_lowpass_filter

Definition at line 204 of file stabilization_indi.c.

Referenced by init_filters(), and lms_estimation().

◆ act_dyn

float act_dyn[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_DYN

Definition at line 127 of file stabilization_indi.c.

Referenced by get_actuator_state().

◆ act_is_servo

bool act_is_servo[INDI_NUM_ACT] = {0}

Definition at line 116 of file stabilization_indi.c.

Referenced by stabilization_indi_init(), and stabilization_indi_rate_run().

◆ act_obs

float act_obs[INDI_NUM_ACT]

Definition at line 173 of file stabilization_indi.c.

Referenced by get_actuator_state(), and rpm_cb().

◆ act_pref

float act_pref[INDI_NUM_ACT] = {0.0}

Definition at line 124 of file stabilization_indi.c.

Referenced by ctrl_eff_scheduling_periodic(), and stabilization_indi_rate_run().

◆ actuator_lowpass_filters

Butterworth2LowPass actuator_lowpass_filters[INDI_NUM_ACT]

Definition at line 200 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 149 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 146 of file stabilization_indi.c.

Referenced by ctrl_eff_scheduling_periodic(), ctrl_eff_scheduling_periodic_b(), stabilization_indi_init(), and stabilization_indi_rate_run().

◆ actuator_state_filt_vectd

float actuator_state_filt_vectd[INDI_NUM_ACT]

Definition at line 160 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 161 of file stabilization_indi.c.

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

◆ angular_accel_ref

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

Definition at line 146 of file stabilization_indi.c.

Referenced by stabilization_indi_rate_run().

◆ angular_acceleration

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

Definition at line 148 of file stabilization_indi.c.

Referenced by stabilization_indi_rate_run().

◆ body_accel_f

struct FloatVect3 body_accel_f

Definition at line 204 of file stabilization_indi.c.

Referenced by lms_estimation().

◆ Bwls

float* Bwls[INDI_OUTPUTS]

Definition at line 79 of file stabilization_indi.c.

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

◆ ddu_estimation

float ddu_estimation[INDI_NUM_ACT]

Definition at line 165 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 164 of file stabilization_indi.c.

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

◆ du_max

float du_max[INDI_NUM_ACT]

Definition at line 76 of file stabilization_indi.c.

Referenced by stabilization_indi_rate_run().

◆ du_min

float du_min[INDI_NUM_ACT]

Definition at line 75 of file stabilization_indi.c.

Referenced by stabilization_indi_rate_run().

◆ du_pref

float du_pref[INDI_NUM_ACT]

Definition at line 77 of file stabilization_indi.c.

Referenced by stabilization_indi_rate_run().

◆ estimation_input_lowpass_filters

Butterworth2LowPass estimation_input_lowpass_filters[INDI_NUM_ACT]

Definition at line 201 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 203 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 162 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 163 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 191 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 195 of file stabilization_indi.c.

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

◆ g1_init

float g1_init[INDI_OUTPUTS][INDI_NUM_ACT]

Definition at line 197 of file stabilization_indi.c.

Referenced by bound_g_mat(), and stabilization_indi_init().

◆ g1g2

float g1g2[INDI_OUTPUTS][INDI_NUM_ACT]

Definition at line 194 of file stabilization_indi.c.

Referenced by calc_g1g2_pseudo_inv(), ctrl_eff_scheduling_periodic_a(), ctrl_eff_scheduling_periodic_b(), schdule_control_effectiveness(), stabilization_indi_init(), and sum_g1_g2().

◆ g1g2_pseudo_inv

float g1g2_pseudo_inv[INDI_NUM_ACT][INDI_OUTPUTS]

Definition at line 189 of file stabilization_indi.c.

Referenced by calc_g1g2_pseudo_inv(), and stabilization_indi_rate_run().

◆ g1g2_trans_mult

float g1g2_trans_mult[INDI_OUTPUTS][INDI_OUTPUTS]

Definition at line 158 of file stabilization_indi.c.

Referenced by calc_g1g2_pseudo_inv().

◆ g1g2inv

float g1g2inv[INDI_OUTPUTS][INDI_OUTPUTS]

Definition at line 159 of file stabilization_indi.c.

Referenced by calc_g1g2_pseudo_inv().

◆ g2

float g2[INDI_NUM_ACT] = STABILIZATION_INDI_G2

Definition at line 190 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 196 of file stabilization_indi.c.

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

◆ g2_init

float g2_init[INDI_NUM_ACT]

Definition at line 198 of file stabilization_indi.c.

Referenced by bound_g_mat(), and stabilization_indi_init().

◆ g2_times_du

float g2_times_du

Definition at line 152 of file stabilization_indi.c.

Referenced by stabilization_indi_rate_run().

◆ indi_du

float indi_du[INDI_NUM_ACT]

Definition at line 151 of file stabilization_indi.c.

Referenced by stabilization_indi_rate_run().

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

Referenced by ctrl_eff_scheduling_periodic_b(), stabilization_indi_attitude_run(), and stabilization_indi_rate_run().

◆ indi_thrust_increment

float indi_thrust_increment

Definition at line 186 of file stabilization_indi.c.

Referenced by stabilization_indi_rate_run(), and thrust_cb().

◆ indi_thrust_increment_set

bool indi_thrust_increment_set = false

Definition at line 187 of file stabilization_indi.c.

Referenced by stabilization_indi_attitude_run(), stabilization_indi_rate_run(), and thrust_cb().

◆ indi_u

float indi_u[INDI_NUM_ACT]

Definition at line 150 of file stabilization_indi.c.

Referenced by get_actuator_state(), and stabilization_indi_rate_run().

◆ indi_use_adaptive

bool indi_use_adaptive = false

Definition at line 106 of file stabilization_indi.c.

Referenced by stabilization_indi_rate_run().

◆ indi_v

float indi_v[INDI_OUTPUTS]

Definition at line 78 of file stabilization_indi.c.

Referenced by stabilization_indi_rate_run().

◆ indi_Wu

float indi_Wu[INDI_NUM_ACT] = {[0 ... INDI_NUM_ACT-1] = 1.0}

Weighting of different actuators in the cost function.

Definition at line 142 of file stabilization_indi.c.

Referenced by stabilization_indi_rate_run().

◆ measurement_lowpass_filters

Butterworth2LowPass measurement_lowpass_filters[3]

Definition at line 202 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 168 of file stabilization_indi.c.

Referenced by lms_estimation().

◆ mu2

float mu2 = 0.002

Definition at line 170 of file stabilization_indi.c.

Referenced by lms_estimation().

◆ num_iter

int num_iter = 0

Definition at line 80 of file stabilization_indi.c.

Referenced by stabilization_indi_rate_run().

◆ num_thrusters

int32_t num_thrusters

Definition at line 176 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 154 of file stabilization_indi.c.

◆ r_filt

float r_filt = 0.0

Definition at line 155 of file stabilization_indi.c.

◆ rates_filt_fo

struct FirstOrderLowPass rates_filt_fo[3]

static

Definition at line 204 of file stabilization_indi.c.

Referenced by init_filters(), and stabilization_indi_rate_run().

◆ rpm_ev

abi_event rpm_ev

Definition at line 181 of file stabilization_indi.c.

Referenced by stabilization_indi_init().

◆ stab_att_sp_euler

struct Int32Eulers stab_att_sp_euler

with INT32_ANGLE_FRAC

Definition at line 176 of file stabilization_indi.c.

Referenced by stabilization_indi_enter(), stabilization_indi_set_earth_cmd_i(), stabilization_indi_set_quat_setpoint_i(), stabilization_indi_set_rpy_setpoint_i(), and stabilization_indi_set_stab_sp().

◆ stab_att_sp_quat

struct Int32Quat stab_att_sp_quat

with INT32_QUAT_FRAC

Definition at line 176 of file stabilization_indi.c.

Referenced by send_ahrs_ref_quat(), stabilization_indi_read_rc(), stabilization_indi_set_earth_cmd_i(), stabilization_indi_set_failsafe_setpoint(), stabilization_indi_set_quat_setpoint_i(), stabilization_indi_set_rpy_setpoint_i(), and stabilization_indi_set_stab_sp().

◆ stabilization_att_indi_cmd

int32_t stabilization_att_indi_cmd[COMMANDS_NB]

Definition at line 89 of file stabilization_indi.c.

◆ thrust_ev

abi_event thrust_ev

Definition at line 184 of file stabilization_indi.c.

Referenced by stabilization_indi_init().

◆ Wv

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

static

Definition at line 133 of file stabilization_indi.c.

Referenced by stabilization_indi_rate_run(), and wls_alloc().

Macros

Functions

Variables

Macro Definition Documentation

◆ INDI_ALLOWED_G_FACTOR

◆ INDI_HROTTLE_LIMIT_AIRSPEED_FWD

◆ STABILIZATION_INDI_FILT_CUTOFF_P

◆ STABILIZATION_INDI_FILT_CUTOFF_Q

◆ STABILIZATION_INDI_FILT_CUTOFF_R

Function Documentation

◆ bound_g_mat()

◆ calc_g1_element()

◆ calc_g1g2_pseudo_inv()

◆ calc_g2_element()

◆ get_actuator_state()

◆ init_filters()

◆ lms_estimation()

◆ rpm_cb() [1/2]

◆ rpm_cb() [2/2]

◆ send_ahrs_ref_quat()

◆ send_indi_g()

◆ stabilization_indi_attitude_run()

◆ stabilization_indi_enter()

◆ stabilization_indi_init()

◆ stabilization_indi_rate_run()

◆ stabilization_indi_read_rc()

◆ stabilization_indi_set_earth_cmd_i()

◆ stabilization_indi_set_failsafe_setpoint()

◆ stabilization_indi_set_quat_setpoint_i()

◆ stabilization_indi_set_rpy_setpoint_i()

◆ stabilization_indi_set_stab_sp()

◆ sum_g1_g2()

◆ thrust_cb() [1/2]

◆ thrust_cb() [2/2]

Variable Documentation

◆ acceleration_lowpass_filter

◆ act_dyn

◆ act_is_servo

◆ act_obs

◆ act_pref

◆ actuator_lowpass_filters

◆ actuator_state

◆ actuator_state_filt_vect

◆ actuator_state_filt_vectd

◆ actuator_state_filt_vectdd

◆ angular_accel_ref

◆ angular_acceleration

◆ body_accel_f

◆ Bwls

◆ ddu_estimation

◆ du_estimation

◆ du_max

◆ du_min

◆ du_pref

◆ 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

◆ g2_times_du

◆ indi_du

◆ indi_gains

◆ indi_thrust_increment

◆ indi_thrust_increment_set

◆ indi_u

◆ indi_use_adaptive

◆ indi_v

◆ indi_Wu

◆ measurement_lowpass_filters

◆ mu1

◆ mu2