ANDI stabilization controller for tiltbody rotorcraft. More...

#include "firmwares/rotorcraft/stabilization/stabilization_andi.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.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 "math/wls/wls_alloc.h"
#include "modules/nav/nav_rotorcraft_hybrid.h"
#include "firmwares/rotorcraft/navigation.h"
#include "modules/rotwing_drone/rotwing_state.h"
#include "modules/core/commands.h"
#include "autopilot.h"
#include "filters/low_pass_filter.h"
#include "filters/low_pass_filter_types.h"
#include "filters/complementary_filter.h"
#include "filters/complementary_filter_types.h"
#include "filters/transport_delay.h"
#include "filters/transport_delay_types.h"
#include "filters/low_pass_zoh_filter.h"
#include "filters/low_pass_zoh_filter_types.h"
#include <stdio.h>
#include "modules/datalink/telemetry.h"

Include dependency graph for stabilization_andi.c:

Go to the source code of this file.

Functions
static void	actuators_t4_in_callback (uint8_t sender_id, struct ActuatorsT4In actuators_t4_in_ptr, float actuators_t4_extra_data_in_ptr)
	Callback function to handle incoming actuator telemetry data.

static void	fetch_actuators_t4 (float actuator_meas[ANDI_NUM_ACT], const struct ActuatorsT4In *actuators_t4_in_ptr)
	Extract actuator states from ActuatorsT4In struct.

static struct GainsOrder3Vect3	compute_reference_gains_order_3_vect_3 (const struct PolesOrder3Vect3 *poles)
	Compute reference-model gains for a 3rd-order 3D system.

static struct GainsOrder2Vect3	compute_reference_gains_order_2_vect_3 (const struct PolesOrder2Vect3 *poles)
	Compute reference-model gains for a 2nd-order 3D system.

static struct GainsOrder3Vect3	compute_error_gains_order_3_vect_3 (const struct PolesOrder3Vect3 *poles)
	Compute error-compensation gains for a 3rd-order 3D system.

static struct GainsOrder2Vect3	compute_error_gains_order_2_vect_3 (const struct PolesOrder2Vect3 *poles)
	Compute error-compensation gains for a 2nd-order 3D system.

static void	generate_reference_rate (float dt, const struct FloatRates rate_des, const struct GainsOrder2Vect3 k_rate_rm, const struct AttQuat bounds, struct AttQuat att_ref)
	Generates a bounded second-order reference signal for attitude rate control.

static void	generate_reference_attitude (float dt, const struct FloatQuat att_des, const struct GainsOrder3Vect3 k_att_rm, const struct AttQuat bounds, struct AttQuat att_ref)
	Generates a bounded second-order reference signal for quaternion-based attitude control.

static void	generate_reference_thrust (float dt, float thrust_des, const float k_thrust_rm, const struct ThrustRef bounds_min, const struct ThrustRef bounds_max, struct ThrustRef *thrust_ref)
	Generates a bounded second-order reference signal for thrust control.

static struct FloatVect3	control_error_rate (const struct AttQuat att_ref, const struct AttStateQuat att_state, const struct GainsOrder2Vect3 *k_rate_ec)
	Computes the control error command for attitude rate regulation.

static struct FloatVect3	control_error_attitude (const struct AttQuat att_ref, const struct AttStateQuat att_state, const struct GainsOrder3Vect3 *k_att_ec)
	Computes the attitude rate error control command.

static float	control_error_thrust (const struct ThrustRef *thrust_ref, const float thrust_state, const float k_thrust_ec)
	Computes the thrust control command based on desired and current thrust.

static void	compute_wls_upper_bounds (float u_d_max[ANDI_NUM_ACT], const float act_state[ANDI_NUM_ACT], const float act_max[ANDI_NUM_ACT], const float act_rate_max[ANDI_NUM_ACT], const float actuator_bandwidth[ANDI_NUM_ACT])
	Compute upper bounds for actuator rate commands based on actuator state and constraints.

static void	compute_wls_lower_bounds (float u_d_min[ANDI_NUM_ACT], const float act_state[ANDI_NUM_ACT], const float act_min[ANDI_NUM_ACT], const float act_rate_min[ANDI_NUM_ACT], const float actuator_bandwidth[ANDI_NUM_ACT])
	Compute lower bounds for actuator rate commands based on actuator state and constraints.

static void	compute_wls_u_scaler (float u_scaler[ANDI_NUM_ACT], const float act_min[ANDI_NUM_ACT], const float act_max[ANDI_NUM_ACT])
	Compute input scaling factors for normalizing weighted least squares.

static void	compute_wls_v_scaler (float v_scaler[ANDI_NUM_ACT], const float v[ANDI_NUM_ACT])
	Compute output scaling factors for normalizing weighted least squares outputs.

static float	ec_k1_order3_f (const float omega_n, const float zeta, const float omega_a)

static float	ec_k2_order3_f (const float omega_n, const float zeta, const float omega_a)

static float	ec_k3_order3_f (const float omega_n, const float zeta, const float omega_a)

static float	rm_k1_order3_f (const float omega_n, const float zeta, const float omega_a)

static float	rm_k2_order3_f (const float omega_n, const float zeta, const float omega_a)

static float	rm_k3_order3_f (const float omega_n, const float zeta, const float omega_a)

static float	ec_k1_order2_f (const float omega_a, const float zeta)

static float	ec_k2_order2_f (const float omega_a, const float zeta)

static float	rm_k1_order2_f (const float omega_a, const float zeta)

static float	rm_k2_order2_f (const float omega_a, const float zeta)

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

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

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

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

void	stabilization_andi_init (void)

void	stabilization_andi_enter (void)
	Initializes the ANDI stabilization controller state upon entering stabilization mode.

void	stabilization_andi_run (bool use_rate_control, bool in_flight, struct StabilizationSetpoint stab_setpoint, struct ThrustSetpoint thrust_setpoint, int32_t *cmd)
	Main ANDI stabilization control loop.

void	stabilization_attitude_enter (void)
	Attitude control enter function.

void	stabilization_rate_enter (void)

void	stabilization_rate_run (bool in_flight, struct StabilizationSetpoint rate_sp, struct ThrustSetpoint thrust, int32_t *cmd)

void	stabilization_attitude_run (bool in_flight, struct StabilizationSetpoint sp, struct ThrustSetpoint thrust, int32_t *cmd)
	Attitude control run function.

struct StabilizationSetpoint	stabilization_rate_read_rc (struct RadioControl *rc)

bool	autopilot_in_flight_end_detection (bool motors_on)

Variables
const bool	SCHEDULE_EFF = false

const bool	USE_STATE_DYNAMICS = false

const float	ANDI_RELAX_OBM = 1.0f

const bool	ACTUATOR_IS_SERVO [ANDI_NUM_ACT] = {0}

const uint8_t	ACTUATOR_DELAY [ANDI_NUM_ACT] = {0}

const float	ACTUATOR_PREF [ANDI_NUM_ACT] = {0.0f}

const float	RC_RATE_MAX [ANDI_OUTPUTS] = {[0 ... ANDI_OUTPUTS - 1] = 5.0f}

const float	THRUST_MIN = 0.0f

const float	WLS_WV [ANDI_OUTPUTS] = {[0 ... ANDI_OUTPUTS - 1] = 1.0f}

float	WLS_WU [ANDI_NUM_ACT] = {[0 ... ANDI_NUM_ACT - 1] = 1.0f}
	Normalized actuator cost for WLS allocation.

struct PolesOrder2Vect3	andi_p_rate_ec

struct PolesOrder2Vect3	andi_p_rate_rm

struct PolesOrder3Vect3	andi_p_att_ec

struct PolesOrder3Vect3	andi_p_att_rm

float	andi_p_thrust_ec = STABILIZATION_ANDI_POLE_THRUST_EC

float	andi_p_thrust_rm = STABILIZATION_ANDI_POLE_THRUST_RM

struct WLS_t	wls_stab_p

struct GainsOrder2Vect3	andi_k_rate_ec

struct GainsOrder2Vect3	andi_k_rate_rm

struct GainsOrder3Vect3	andi_k_att_ec

struct GainsOrder3Vect3	andi_k_att_rm

float	andi_k_thrust_ec

float	andi_k_thrust_rm

struct FirstOrderComplementaryVect3	attitude_rates_cf

struct Butterworth2ComplementaryVect3	attitude_accel_cf

struct FloatRates	angular_rates_obm

struct FirstOrderComplementaryVect3	linear_vel_cf

struct Butterworth2ComplementaryVect3	linear_accel_cf

struct FloatVect3	linear_velocity_obm

struct FirstOrderZOHLowPass	actuator_obm_zohlpf [ANDI_NUM_ACT]

struct TransportDelay	actuator_obm_delay [ANDI_NUM_ACT]

float	actuator_state [ANDI_NUM_ACT]

float	actuator_state_lpf [ANDI_NUM_ACT]

struct ActuatorsT4In	actuators_t4_obs

abi_event	actuators_t4_in_event

float	actuator_meas [ANDI_NUM_ACT]

struct FloatRates	rates_prev

struct AttStateQuat	attitude_state_cf

struct LinState	linear_state_cf

float	thrust_state

struct AttQuat	attitude_ref

struct ThrustRef	thrust_ref

struct FloatRates	rates_des

struct FloatQuat	attitude_des

float	thrust_des

struct AttQuat	attitude_bounds

struct ThrustRef	thrust_bounds_min

struct ThrustRef	thrust_bounds_max

float	ce_mat [ANDI_NUM_ACT *ANDI_OUTPUTS]

float	du_min [ANDI_NUM_ACT]

float	du_max [ANDI_NUM_ACT]

float	du_cmd [ANDI_NUM_ACT]

float	u_cmd [ANDI_NUM_ACT]

float	nu_obj [ANDI_OUTPUTS]

float	nu_ec [ANDI_OUTPUTS]

float	nu_obm [ANDI_OUTPUTS]

Detailed Description

ANDI stabilization controller for tiltbody rotorcraft.

Implements Adaptive Nonlinear Dynamic Inversion (ANDI) for attitude and thrust control with on-board model compensation.

Assumptions:

Airframe is a tiltbody with 2 elevons (indices 0,1) and 2 motors (indices 2,3) in tractor configuration.
ESC neutral value equals zero thrust (usually 0).
Motors controlled in squared RPM to linearize thrust curve.

FIXME: WLS allocation normalization depends on control effectiveness scaling (state-dependent). Consider normalizing costs by max achievable derivatives per actuator/output.

Author: Justin Dubois j.p.g.nosp@m..dub.nosp@m.ois@s.nosp@m.tude.nosp@m.nt.tu.nosp@m.delf.nosp@m.t.nl

Definition in file stabilization_andi.c.

Function Documentation

◆ actuators_t4_in_callback()

static void actuators_t4_in_callback	(	uint8_t	sender_id,
		struct ActuatorsT4In *	actuators_t4_in_ptr,
		float *	actuators_t4_extra_data_in_ptr
	)

static

Callback function to handle incoming actuator telemetry data.

This function copies the data from the input struct pointer actuators_t4_in_ptr into a local static instance actuator_obs for further processing or monitoring.

Parameters

sender_id	Identifier of the sender of the telemetry data.
actuators_t4_in_ptr	Pointer to the incoming ActuatorsT4In struct containing ESC telemetry, servo angles, loads, and other actuator info.
actuators_t4_extra_data_in_ptr	Pointer to additional extra actuator data (unused).

Definition at line 507 of file stabilization_andi.c.

References actuators_t4_obs, and foo.

Referenced by stabilization_andi_init().

Here is the caller graph for this function:

◆ autopilot_in_flight_end_detection()

bool autopilot_in_flight_end_detection ( bool motors_on )

Definition at line 1414 of file stabilization_andi.c.

◆ compute_error_gains_order_2_vect_3()

static struct GainsOrder2Vect3 compute_error_gains_order_2_vect_3 ( const struct PolesOrder2Vect3 * poles )

static

Compute error-compensation gains for a 2nd-order 3D system.

Each axis gain (x, y, z) is computed using ec_k*_order2_f() with omega_n and zeta parameters.

Parameters

[in] poles Pointer to PolesOrder2Vect3 containing omega_n and zeta for each axis.

Returns: Struct containing k1, k2 gains for x, y, z.

Definition at line 613 of file stabilization_andi.c.

References ec_k1_order2_f(), ec_k2_order2_f(), foo, gains, GainsOrder2Vect3::k1, and FloatVect3::x.

Referenced by stabilization_andi_init(), and stabilization_andi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ compute_error_gains_order_3_vect_3()

static struct GainsOrder3Vect3 compute_error_gains_order_3_vect_3 ( const struct PolesOrder3Vect3 * poles )

static

Compute error-compensation gains for a 3rd-order 3D system.

Each axis gain (x, y, z) is computed using ec_k*_order3_f() with omega_n, zeta, and p1 parameters.

Parameters

[in] poles Pointer to PolesOrder3Vect3 containing omega_n, zeta, and p1 for each axis.

Returns: Struct containing k1, k2, k3 gains for x, y, z.

Definition at line 590 of file stabilization_andi.c.

References ec_k1_order3_f(), ec_k2_order3_f(), ec_k3_order3_f(), foo, gains, GainsOrder3Vect3::k1, and FloatVect3::x.

Referenced by stabilization_andi_init(), and stabilization_andi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ compute_reference_gains_order_2_vect_3()

static struct GainsOrder2Vect3 compute_reference_gains_order_2_vect_3 ( const struct PolesOrder2Vect3 * poles )

static

Compute reference-model gains for a 2nd-order 3D system.

Each axis gain (x, y, z) is computed using rm_k*_order2_f() with omega_n and zeta parameters.

Parameters

[in] poles Pointer to PolesOrder2Vect3 containing omega_n and zeta for each axis.

Returns: Struct containing k1, k2 gains for x, y, z.

Definition at line 571 of file stabilization_andi.c.

References foo, gains, GainsOrder2Vect3::k1, rm_k1_order2_f(), rm_k2_order2_f(), and FloatVect3::x.

Referenced by stabilization_andi_init(), and stabilization_andi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ compute_reference_gains_order_3_vect_3()

static struct GainsOrder3Vect3 compute_reference_gains_order_3_vect_3 ( const struct PolesOrder3Vect3 * poles )

static

Compute reference-model gains for a 3rd-order 3D system.

Each axis gain (x, y, z) is computed using rm_k*_order3_f() with omega_n, zeta, and p1 parameters.

Parameters

[in] poles Pointer to PolesOrder3Vect3 containing omega_n, zeta, and p1 for each axis.

Returns: Struct containing k1, k2, k3 gains for x, y, z.

Definition at line 548 of file stabilization_andi.c.

References foo, gains, GainsOrder3Vect3::k1, rm_k1_order3_f(), rm_k2_order3_f(), rm_k3_order3_f(), and FloatVect3::x.

Referenced by stabilization_andi_init(), and stabilization_andi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ compute_wls_lower_bounds()

static void compute_wls_lower_bounds	(	float	u_d_min[ANDI_NUM_ACT],
		const float	act_state[ANDI_NUM_ACT],
		const float	act_min[ANDI_NUM_ACT],
		const float	act_rate_min[ANDI_NUM_ACT],
		const float	actuator_bandwidth[ANDI_NUM_ACT]
	)

static

Compute lower bounds for actuator rate commands based on actuator state and constraints.

This function calculates the minimum allowable actuator rate commands (u_d_min) for each actuator to ensure that:

The actuator position does not go below its specified minimum (act_min) in the next timestep.
The actuator rate does not go below its minimum allowed rate (act_rate_min). The computed lower bound is the maximum of the position-based rate limit and the minimum rate limit. Additionally, if the computed lower bound is greater than zero (i.e., actuator cannot reverse direction), it is clamped to zero.

Parameters

[out]	u_d_min	Array to store the computed lower bounds for actuator rates.
[in]	act_state	Current states (positions) of the actuators.
[in]	act_min	Minimum allowable positions for the actuators.
[in]	act_rate_min	Minimum allowable rates for the actuators.
[in]	actuator_bandwidth	Bandwidth of the actuators used to compute rate limits.

Definition at line 916 of file stabilization_andi.c.

References act_min, ANDI_NUM_ACT, and foo.

Referenced by stabilization_andi_run().

Here is the caller graph for this function:

◆ compute_wls_u_scaler()

static void compute_wls_u_scaler	(	float	u_scaler[ANDI_NUM_ACT],
		const float	act_min[ANDI_NUM_ACT],
		const float	act_max[ANDI_NUM_ACT]
	)

static

Compute input scaling factors for normalizing weighted least squares.

This function calculates the input scaling factors (u_scaler) for each actuator based on the provided minimum (act_min) and maximum (act_max) actuator values. The scaling factor is computed as the inverse of the range (max - min). If the range is zero, the scaling factor is set to 1.0 to avoid division by zero.

u_norm = u_scaler * u

Parameters

[out]	u_scaler	Array to store the computed input scaling factors.
[in]	act_min	Array of minimum actuator values.
[in]	act_max	Array of maximum actuator values.

Definition at line 941 of file stabilization_andi.c.

References act_max, act_min, ANDI_NUM_ACT, and foo.

Referenced by stabilization_andi_run().

Here is the caller graph for this function:

◆ compute_wls_upper_bounds()

static void compute_wls_upper_bounds	(	float	u_d_max[ANDI_NUM_ACT],
		const float	act_state[ANDI_NUM_ACT],
		const float	act_max[ANDI_NUM_ACT],
		const float	act_rate_max[ANDI_NUM_ACT],
		const float	actuator_bandwidth[ANDI_NUM_ACT]
	)

static

Compute upper bounds for actuator rate commands based on actuator state and constraints.

This function calculates the maximum allowable actuator rate commands (u_d_max) for each actuator to ensure that:

The actuator position does not exceed its specified maximum (act_max) in the next timestep.
The actuator rate does not exceed its maximum allowed rate (act_rate_max). The computed upper bound is the minimum of the position-based rate limit and the maximum rate limit.

Parameters

[out]	u_d_max	Array to store the computed upper bounds for actuator rates.
[in]	act_state	Current states (positions) of the actuators.
[in]	act_max	Maximum allowable positions for the actuators.
[in]	act_rate_max	Maximum allowable rates for the actuators.
[in]	actuator_bandwidth	Bandwidth of the actuators used to compute rate limits.

Definition at line 890 of file stabilization_andi.c.

References act_max, ANDI_NUM_ACT, and foo.

Referenced by stabilization_andi_run().

Here is the caller graph for this function:

◆ compute_wls_v_scaler()

static void compute_wls_v_scaler	(	float	v_scaler[ANDI_NUM_ACT],
		const float	v[ANDI_NUM_ACT]
	)

static

Compute output scaling factors for normalizing weighted least squares outputs.

For each output i this sets v_scaler[i] = 1.0f / abs(v[i]) when v[i] is non-zero; otherwise v_scaler[i] is set to 1.0f to avoid a division-by-zero. The normalized output used in WLS is then

v_norm = v_scaler * v.

Parameters

[out]	v_scaler	Array of length ANDI_NUM_ACT to store the computed inverse scaling factors.
[in]	v	Array of length ANDI_NUM_ACT containing reference/scaling values used to compute the inverse.

Definition at line 965 of file stabilization_andi.c.

References ANDI_NUM_ACT, and foo.

◆ control_error_attitude()

static struct FloatVect3 control_error_attitude	(	const struct AttQuat *	att_ref,
		const struct AttStateQuat *	att_state,
		const struct GainsOrder3Vect3 *	k_att_ec
	)

static

Computes the attitude rate error control command.

This function calculates the control input vector nu to correct the attitude error and the associated angular rate errors using proportional-derivative gains. The quaternion error between the reference and current attitudes is also factored in the control law.

Parameters

[in]	att_ref	Pointer to the reference attitude and rate states (quaternion-based).
[in]	att_state	Pointer to the current attitude and rate states.
[in]	k_att_ec	Pointer to gain parameters struct containing proportional, derivative, and jerk gains.

Returns: The computed virtual control input vector.

Definition at line 826 of file stabilization_andi.c.

References att_state, float_quat_inv_comp_norm_shortest(), foo, nu, FloatRates::p, FloatRates::q, FloatRates::r, and FloatVect3::x.

Referenced by stabilization_andi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ control_error_rate()

static struct FloatVect3 control_error_rate	(	const struct AttQuat *	att_ref,
		const struct AttStateQuat *	att_state,
		const struct GainsOrder2Vect3 *	k_rate_ec
	)

static

Computes the control error command for attitude rate regulation.

This function calculates the virtual control input vector nu required to reduce the error between the reference and current angular rate states. It uses proportional and derivative gains on the differences of rate and rate derivative components, respectively, and adds feedforward jerk.

Parameters

[in]	att_ref	Pointer to the reference attitude and rate state structure.
[in]	att_state	Pointer to the current attitude and rate state structure.
[in]	k_rate_ec	Pointer to gain parameters structure, containing proportional and derivative gains.

Returns: A FloatVect3 structure representing the computed virtual control input vector.

Definition at line 795 of file stabilization_andi.c.

References att_state, foo, nu, FloatRates::p, FloatRates::q, FloatRates::r, and FloatVect3::x.

Referenced by stabilization_andi_run().

Here is the caller graph for this function:

◆ control_error_thrust()

static float control_error_thrust	(	const struct ThrustRef *	thrust_ref,
		const float	thrust_state,
		const float	k_thrust_ec
	)

static

Computes the thrust control command based on desired and current thrust.

This function calculates a corrected thrust command using a simple proportional feedback law. The correction term is scaled by the thrust error gain k_thrust_ec to reduce the difference between the desired thrust (thrust_ref->thrust) and the current thrust (thrust_state). The resulting command is based on the desired thrust feedforward input thrust_ref->thrust_d.

Parameters

[in]	thrust_ref	Pointer to a structure containing the desired thrust values.
[in]	thrust_state	Current measured thrust value.
[in]	k_thrust_ec	Proportional gain applied to the thrust error correction.

Returns: The computed virtual control thrust input.

Definition at line 866 of file stabilization_andi.c.

References foo, nu, ThrustRef::thrust, ThrustRef::thrust_d, thrust_ref, and thrust_state.

Referenced by stabilization_andi_run().

Here is the caller graph for this function:

◆ ec_k1_order2_f()

static float ec_k1_order2_f	(	const float	omega_a,
		const float	zeta
	)

inlinestatic

Definition at line 257 of file stabilization_andi.c.

Referenced by compute_error_gains_order_2_vect_3().

Here is the caller graph for this function:

◆ ec_k1_order3_f()

static float ec_k1_order3_f	(	const float	omega_n,
		const float	zeta,
		const float	omega_a
	)

inlinestatic

Definition at line 249 of file stabilization_andi.c.

Referenced by compute_error_gains_order_3_vect_3().

Here is the caller graph for this function:

◆ ec_k2_order2_f()

static float ec_k2_order2_f	(	const float	omega_a,
		const float	zeta
	)

inlinestatic

Definition at line 258 of file stabilization_andi.c.

Referenced by compute_error_gains_order_2_vect_3(), rm_k1_order2_f(), and rm_k2_order2_f().

Here is the caller graph for this function:

◆ ec_k2_order3_f()

static float ec_k2_order3_f	(	const float	omega_n,
		const float	zeta,
		const float	omega_a
	)

inlinestatic

Definition at line 250 of file stabilization_andi.c.

Referenced by compute_error_gains_order_3_vect_3().

Here is the caller graph for this function:

◆ ec_k3_order3_f()

static float ec_k3_order3_f	(	const float	omega_n,
		const float	zeta,
		const float	omega_a
	)

inlinestatic

Definition at line 251 of file stabilization_andi.c.

Referenced by compute_error_gains_order_3_vect_3().

Here is the caller graph for this function:

◆ fetch_actuators_t4()

static void fetch_actuators_t4	(	float	actuator_meas[ANDI_NUM_ACT],
		const struct ActuatorsT4In *	actuators_t4_in_ptr
	)

static

Extract actuator states from ActuatorsT4In struct.

This function converts selected actuator telemetry data from the input struct into floating-point values representing angles in radians and rotational speeds in radians per second, storing them in the provided actuator_state array.

FIXME: Avoid hardcoding indices, conversion factors, and inversions; This requires additional functionality in actuators_t4. FIXME: Put actuator feedback / state management in a separate module. FIXME: All ESC type actuators will be squared rpm for thrust linearization.

Parameters

[out]	actuator_state	Array of floats with size ANDI_NUM_ACT where the converted actuator states will be stored. The caller must allocate this.
[in]	actuators_t4_in_ptr	Pointer to the input struct containing actuator telemetry, including servo angles (in 1e-2 degrees) and ESC RPM.

Definition at line 532 of file stabilization_andi.c.

References actuator_meas, and foo.

Referenced by stabilization_andi_enter(), and stabilization_andi_run().

Here is the caller graph for this function:

◆ generate_reference_attitude()

static void generate_reference_attitude	(	float	dt,
		const struct FloatQuat *	att_des,
		const struct GainsOrder3Vect3 *	k_att_rm,
		const struct AttQuat *	bounds,
		struct AttQuat *	att_ref
	)

static

Generates a bounded second-order reference signal for quaternion-based attitude control.

Computes smooth angular rate, acceleration, and jerk references using quaternion error feedback. Bounds are applied to limit reference values and maintain system stability. The function updates the reference states in place within the provided att_ref structure, which includes quaternion attitude and associated rate states.

Parameters

[in]	dt	Sampling interval in seconds.
[in]	att_des	Desired attitude as a unit quaternion.
[in]	k_att_rm	Gain parameters (proportional and derivative gains for rate control).
[in]	bounds	Limits on rate references and derivatives (angular velocity and higher).
[in,out]	att_ref	Reference model state containing attitude quaternion and rate state, updated in place.

FIXME: The bounds introduce nonlinearities that may destabilize the reference model for large changes in attitude. Currently removed, consider revising the bounding strategy. FIXME: Quaternion error calculation assumes small angle errors; may need to be revised for large attitude changes.

Definition at line 704 of file stabilization_andi.c.

References float_quat_integrate(), float_quat_inv_comp_norm_shortest(), float_quat_normalize(), foo, Gains3rdOrder::k1, Gains3rdOrder::k2, Gains3rdOrder::k3, and k_att_rm.

Referenced by stabilization_andi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ generate_reference_rate()

static void generate_reference_rate	(	float	dt,
		const struct FloatRates *	rate_des,
		const struct GainsOrder2Vect3 *	k_rate_rm,
		const struct AttQuat *	bounds,
		struct AttQuat *	att_ref
	)

static

Generates a bounded second-order reference signal for attitude rate control.

This function computes smooth angular rate, acceleration, and jerk references based on the desired rates, applying bounded limits to ensure stability and safe dynamic behavior. It updates the reference states in place within the provided att_ref structure, which is a quaternion-based reference with associated rate states.

Parameters

[in]	dt	The sampling interval in seconds.
[in]	rate_des	Desired angular rates (p, q, r) as a `FloatRates` structure.
[in]	k_rate_rm	Gain parameters as `GainsOrder2Vect3`, for the proportional and derivative terms.
[in]	bounds	Limits for the desired rates, rates derivatives, and accelerations, in `AttRefEulers`.
[in,out]	att_ref	The reference model states (attitude quaternion, rates, and derivatives), updated in place to produce the reference command.

Definition at line 640 of file stabilization_andi.c.

References float_quat_identity(), and foo.

Referenced by stabilization_andi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ generate_reference_thrust()

static void generate_reference_thrust	(	float	dt,
		float	thrust_des,
		const float	k_thrust_rm,
		const struct ThrustRef *	bounds_min,
		const struct ThrustRef *	bounds_max,
		struct ThrustRef *	thrust_ref
	)

static

Generates a bounded second-order reference signal for thrust control.

Applies bounded limits to a desired thrust input and computes smoothed thrust rate and integrated thrust references. The reference states are updated in place inside the provided thrust_ref structure.

Parameters

[in]	dt	Sampling interval in seconds.
[in]	thrust_des	Desired thrust input value.
[in]	k_thrust_rm	Gain parameter for thrust rate control (proportional gain).
[in]	bounds	Maximum allowable thrust magnitude.
[in,out]	thrust_ref	Pointer to ThrustRef struct holding thrust reference states.

Definition at line 767 of file stabilization_andi.c.

References foo, ThrustRef::thrust, ThrustRef::thrust_d, thrust_des, and thrust_ref.

Referenced by stabilization_andi_run().

Here is the caller graph for this function:

◆ rm_k1_order2_f()

static float rm_k1_order2_f	(	const float	omega_a,
		const float	zeta
	)

inlinestatic

Definition at line 259 of file stabilization_andi.c.

References ec_k2_order2_f().

Referenced by compute_reference_gains_order_2_vect_3().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ rm_k1_order3_f()

static float rm_k1_order3_f	(	const float	omega_n,
		const float	zeta,
		const float	omega_a
	)

inlinestatic

Definition at line 253 of file stabilization_andi.c.

Referenced by compute_reference_gains_order_3_vect_3().

Here is the caller graph for this function:

◆ rm_k2_order2_f()

static float rm_k2_order2_f	(	const float	omega_a,
		const float	zeta
	)

inlinestatic

Definition at line 260 of file stabilization_andi.c.

References ec_k2_order2_f().

Referenced by compute_reference_gains_order_2_vect_3().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ rm_k2_order3_f()

static float rm_k2_order3_f	(	const float	omega_n,
		const float	zeta,
		const float	omega_a
	)

inlinestatic

Definition at line 254 of file stabilization_andi.c.

Referenced by compute_reference_gains_order_3_vect_3().

Here is the caller graph for this function:

◆ rm_k3_order3_f()

static float rm_k3_order3_f	(	const float	omega_n,
		const float	zeta,
		const float	omega_a
	)

inlinestatic

Definition at line 255 of file stabilization_andi.c.

Referenced by compute_reference_gains_order_3_vect_3().

Here is the caller graph for this function:

◆ send_eff_mat_stabilization_andi()

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

static

Definition at line 426 of file stabilization_andi.c.

References ANDI_NUM_ACT, ce_mat, dev, and foo.

Referenced by stabilization_andi_init().

Here is the caller graph for this function:

◆ send_stab_attitude_stabilization_andi()

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

static

Definition at line 437 of file stabilization_andi.c.

References actuator_state, ANDI_OUTPUTS, AttQuat::att, AttStateQuat::att, AttQuat::att_2d, AttStateQuat::att_2d, AttQuat::att_3d, AttQuat::att_d, AttStateQuat::att_d, attitude_des, attitude_ref, attitude_state_cf, dev, and foo.

Referenced by stabilization_andi_init().

Here is the caller graph for this function:

◆ send_wls_u_stabilization_andi()

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

static

Definition at line 413 of file stabilization_andi.c.

References ANDI_NUM_ACT, dev, du_cmd, du_max, du_min, foo, and WLS_WU.

Referenced by stabilization_andi_init().

Here is the caller graph for this function:

◆ send_wls_v_stabilization_andi()

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

static

Definition at line 402 of file stabilization_andi.c.

References ANDI_OUTPUTS, dev, foo, WLS_t::gamma_sq, WLS_t::iter, nu_obj, wls_stab_p, and WLS_WV.

Referenced by stabilization_andi_init().

Here is the caller graph for this function:

◆ stabilization_andi_enter()

void stabilization_andi_enter ( void )

Initializes the ANDI stabilization controller state upon entering stabilization mode.

This function resets all relevant state variables, filters, and actuator states to match the current measurements when entering the ANDI stabilization mode. It ensures that the controller starts from a consistent state when entering stabilization.

NOTE: This function currently assumes that the actuator feedback message is being received. FIXME: Transient free initialization for the cascaded complementary filters is not implemented.

Definition at line 1094 of file stabilization_andi.c.

References LinState::acc, actuator_obm_delay, actuator_obm_zohlpf, ACTUATOR_PREF, actuator_state, actuators_t4_obs, ANDI_NUM_ACT, ANDI_OUTPUTS, angular_rates_obm, AttQuat::att, AttStateQuat::att, AttQuat::att_2d, AttStateQuat::att_2d, AttQuat::att_3d, AttQuat::att_d, AttStateQuat::att_d, attitude_accel_cf, attitude_rates_cf, attitude_ref, attitude_state_cf, ce_mat, evaluate_obm_f_stb_u(), evaluate_obm_thrust_z(), fetch_actuators_t4(), float_quat_vmult(), float_vect_zero(), linear_accel_cf, linear_state_cf, linear_vel_cf, linear_velocity_obm, nu_obm, FloatRates::p, FloatRates::q, FloatRates::r, rates_prev, reset_butterworth_2_complementary_vect3(), reset_first_order_complementary_rates(), reset_first_order_complementary_vect3(), reset_first_order_zoh_low_pass_array(), reset_transport_delay_array(), stateGetBodyRates_f(), stateGetNedToBodyQuat_f(), stateGetSpeedNed_f(), ThrustRef::thrust, ThrustRef::thrust_d, thrust_ref, thrust_state, u_cmd, LinState::vel, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Here is the call graph for this function:

◆ stabilization_andi_init()

void stabilization_andi_init ( void )

Definition at line 976 of file stabilization_andi.c.

References ABI_BROADCAST, LinState::acc, ACTUATOR_DELAY, actuator_obm_delay, actuator_obm_zohlpf, ACTUATOR_PREF, actuator_state, actuators_t4_in_callback(), actuators_t4_in_event, andi_k_att_ec, andi_k_att_rm, andi_k_rate_ec, andi_k_rate_rm, andi_k_thrust_ec, andi_k_thrust_rm, ANDI_NUM_ACT, andi_p_att_ec, andi_p_att_rm, andi_p_rate_ec, andi_p_rate_rm, andi_p_thrust_ec, andi_p_thrust_rm, angular_rates_obm, AttQuat::att, AttStateQuat::att, AttQuat::att_2d, AttStateQuat::att_2d, AttQuat::att_3d, AttQuat::att_d, AttStateQuat::att_d, attitude_accel_cf, attitude_bounds, attitude_rates_cf, attitude_ref, attitude_state_cf, ce_mat, compute_error_gains_order_2_vect_3(), compute_error_gains_order_3_vect_3(), compute_reference_gains_order_2_vect_3(), compute_reference_gains_order_3_vect_3(), DefaultPeriodic, du_cmd, evaluate_obm_f_stb_u(), float_quat_identity(), float_vect_zero(), foo, init_butterworth_2_complementary_vect3(), init_first_order_complementary_vect3(), init_first_order_zoh_low_pass_array(), init_transport_delay_array(), linear_accel_cf, linear_state_cf, linear_vel_cf, linear_velocity_obm, nu_ec, nu_obj, nu_obm, FloatRates::p, FloatRates::q, FloatRates::r, rates_prev, register_periodic_telemetry(), send_eff_mat_stabilization_andi(), send_stab_attitude_stabilization_andi(), send_wls_u_stabilization_andi(), send_wls_v_stabilization_andi(), ThrustRef::thrust, thrust_bounds_max, thrust_bounds_min, ThrustRef::thrust_d, THRUST_MIN, thrust_ref, thrust_state, u_cmd, LinState::vel, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Here is the call graph for this function:

◆ stabilization_andi_run()

void stabilization_andi_run	(	bool	use_rate_control,
		bool	in_flight,
		struct StabilizationSetpoint *	stab_setpoint,
		struct ThrustSetpoint *	thrust_setpoint,
		int32_t *	cmd
	)

Main ANDI stabilization control loop.

This function implements the core logic of the ANDI stabilization controller. The following steps are performed each time the function is called:

Fetch measurements:

Linear state (velocity and acceleration)
Attitude state (quaternion, angular rates, angular accelerations)
Actuator states (from T4 feedback and/or on-board model estimates)

Filtering:

Apply cascaded complementary filtering to obtain an undelayed estimates of attitude and linear states. (used for state dependent model contribution (and scheduling))
Apply low-pass filtering to obtain synchronized (delayed) estimates of attitude and thrust states. (used for control error computation)

Reference generation:

Generate reference models for attitude and thrust using desired setpoints and reference model gains.
Apply low-pass filtering to the references to synchronize with measurement lag.

Pseudo-command computation:

Compute error controller pseudo command for attitude and thrust reference and feedback using parallel error controller.
Compute state dependent on-board model pseudo command contribution based on undelayed state estimates.
Combine pseudo commands to form the overall virtual control input vector.

Actuator command allocation:

Update the control effectiveness matrix based on the current state.
Compute actuator commands using weighted least squares allocation with bounds and scaling.

Parameters

[in]	use_rate_control	Flag indicating whether to use rate control mode.
[in]	in_flight	Flag indicating whether the vehicle is in flight.
[in,out]	stab_setpoint	Pointer to the stabilization setpoint structure.
[in,out]	thrust_setpoint	Pointer to the thrust setpoint structure.
[out]	cmd	Pointer to the output command array for actuators.

Note: The attitude stabilization mode is ATTITUDE_HEADING_MODE. A heading estimate and setpoint is needed. Consider implementing ATTITUDE_HEADING_RATE_MODE to avoid needing a heading estimate.

FIXME: The function currently recomputes gains at each call. This could be optimized to only recompute when parameters change. FIXME: The choice of actuator feedback source (T4 vs on-board model) should be configurable.

Definition at line 1185 of file stabilization_andi.c.

References LinState::acc, actuator_meas, actuator_obm_delay, actuator_obm_zohlpf, actuator_state, actuators_t4_obs, andi_k_att_ec, andi_k_att_rm, andi_k_rate_ec, andi_k_rate_rm, andi_k_thrust_ec, andi_k_thrust_rm, ANDI_NUM_ACT, ANDI_OUTPUTS, andi_p_att_ec, andi_p_att_rm, andi_p_rate_ec, andi_p_rate_rm, andi_p_thrust_ec, andi_p_thrust_rm, ANDI_RELAX_OBM, angular_rates_obm, AttStateQuat::att, AttStateQuat::att_2d, AttStateQuat::att_d, attitude_accel_cf, attitude_bounds, attitude_des, attitude_rates_cf, attitude_ref, attitude_state_cf, ce_mat, commands, compute_error_gains_order_2_vect_3(), compute_error_gains_order_3_vect_3(), compute_reference_gains_order_2_vect_3(), compute_reference_gains_order_3_vect_3(), compute_wls_lower_bounds(), compute_wls_u_scaler(), compute_wls_upper_bounds(), control_error_attitude(), control_error_rate(), control_error_thrust(), du_cmd, du_max, du_min, evaluate_obm_f_stb_u(), evaluate_obm_f_stb_x(), evaluate_obm_forces(), evaluate_obm_moments(), evaluate_obm_thrust_z(), fetch_actuators_t4(), float_quat_vmult(), float_rates_vect3_integrate_fi(), float_vect3_integrate_fi(), float_vect_zero(), foo, generate_reference_attitude(), generate_reference_rate(), generate_reference_thrust(), get_butterworth_2_complementary_vect3(), get_first_order_complementary_rates(), get_first_order_complementary_vect3(), get_first_order_zoh_low_pass_array(), get_transport_delay_array(), linear_accel_cf, linear_state_cf, linear_vel_cf, linear_velocity_obm, MAX_PPRZ, nu_ec, nu_obj, nu_obm, FloatRates::p, FloatRates::q, FloatRates::r, rates_des, rates_prev, SCHEDULE_EFF, stab_sp_to_quat_f(), stab_sp_to_rates_f(), stateGetAccelBody_f(), stateGetBodyRates_f(), stateGetNedToBodyQuat_f(), stateGetSpeedNed_f(), th_sp_to_thrust_f(), THRUST_AXIS_Z, thrust_bounds_max, thrust_bounds_min, thrust_des, thrust_ref, thrust_state, WLS_t::u, u_cmd, WLS_t::u_max, WLS_t::u_min, WLS_t::u_pref, update_butterworth_2_complementary_vect3(), update_first_order_complementary_rates(), update_first_order_complementary_vect3(), update_first_order_zoh_low_pass_array(), update_transport_delay_array(), USE_STATE_DYNAMICS, WLS_t::v, LinState::vel, wls_alloc(), wls_stab_p, WLS_WU, WLS_WV, WLS_t::Wu, and WLS_t::Wv.

Here is the call graph for this function:

◆ stabilization_attitude_enter()

void stabilization_attitude_enter ( void )

Attitude control enter function.

Definition at line 1372 of file stabilization_andi.c.

Referenced by ins_ekf2_publish_attitude(), and stabilization_mode_changed().

Here is the caller graph for this function:

◆ stabilization_attitude_run()

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

Attitude control run function.

Parameters

[in]	in_flight	true if in flight
[in]	sp	pointer to the stabilization setpoint structure
[in]	thrust	pointer to the thrust setoint structure
[out]	cmd	pointer to the output command vector

Definition at line 1389 of file stabilization_andi.c.

Referenced by guidance_flip_run(), guidance_module_run(), stabilization_run(), and vertical_ctrl_module_run().

Here is the caller graph for this function:

◆ stabilization_rate_enter()

void stabilization_rate_enter ( void )

Definition at line 1377 of file stabilization_andi.c.

Referenced by stabilization_mode_changed().

Here is the caller graph for this function:

◆ stabilization_rate_read_rc()

struct StabilizationSetpoint stabilization_rate_read_rc ( struct RadioControl * rc )

Definition at line 1397 of file stabilization_andi.c.

References FLOAT_RATES_ZERO, foo, MAX_PPRZ, PITCH_RATE_DEADBAND_EXCEEDED, RC_RATE_MAX, RC_RATE_P, RC_RATE_Q, RC_RATE_R, ROLL_RATE_DEADBAND_EXCEEDED, stab_sp_from_rates_f(), and YAW_RATE_DEADBAND_EXCEEDED.

Referenced by rc_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ stabilization_rate_run()

void stabilization_rate_run	(	bool	in_flight,
		struct StabilizationSetpoint *	rate_sp,
		struct ThrustSetpoint *	thrust,
		int32_t *	cmd
	)

Definition at line 1382 of file stabilization_andi.c.

Referenced by stabilization_run().

Here is the caller graph for this function:

Variable Documentation

◆ ACTUATOR_DELAY

const uint8_t ACTUATOR_DELAY[ANDI_NUM_ACT] = {0}

Definition at line 103 of file stabilization_andi.c.

Referenced by stabilization_andi_init().

◆ ACTUATOR_IS_SERVO

const bool ACTUATOR_IS_SERVO[ANDI_NUM_ACT] = {0}

Definition at line 91 of file stabilization_andi.c.

◆ actuator_meas

float actuator_meas[ANDI_NUM_ACT]

Definition at line 363 of file stabilization_andi.c.

Referenced by fetch_actuators_t4(), and stabilization_andi_run().

◆ actuator_obm_delay

struct TransportDelay actuator_obm_delay[ANDI_NUM_ACT]

Definition at line 356 of file stabilization_andi.c.

Referenced by stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().

◆ actuator_obm_zohlpf

struct FirstOrderZOHLowPass actuator_obm_zohlpf[ANDI_NUM_ACT]

Definition at line 355 of file stabilization_andi.c.

Referenced by stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().

◆ ACTUATOR_PREF

const float ACTUATOR_PREF[ANDI_NUM_ACT] = {0.0f}

Definition at line 123 of file stabilization_andi.c.

Referenced by stabilization_andi_enter(), and stabilization_andi_init().

◆ actuator_state

float actuator_state[ANDI_NUM_ACT]

Definition at line 357 of file stabilization_andi.c.

Referenced by evaluate_obm_f_stb_u(), evaluate_obm_f_stb_x(), evaluate_obm_forces(), evaluate_obm_moments(), evaluate_obm_thrust_z(), send_stab_attitude_stabilization_andi(), stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().

◆ actuator_state_lpf

float actuator_state_lpf[ANDI_NUM_ACT]

Definition at line 358 of file stabilization_andi.c.

◆ actuators_t4_in_event

abi_event actuators_t4_in_event

Definition at line 362 of file stabilization_andi.c.

Referenced by stabilization_andi_init().

◆ actuators_t4_obs

struct ActuatorsT4In actuators_t4_obs

Definition at line 361 of file stabilization_andi.c.

Referenced by actuators_t4_in_callback(), stabilization_andi_enter(), and stabilization_andi_run().

◆ andi_k_att_ec

struct GainsOrder3Vect3 andi_k_att_ec

Definition at line 340 of file stabilization_andi.c.

Referenced by stabilization_andi_init(), and stabilization_andi_run().

◆ andi_k_att_rm

struct GainsOrder3Vect3 andi_k_att_rm

Definition at line 341 of file stabilization_andi.c.

Referenced by stabilization_andi_init(), and stabilization_andi_run().

◆ andi_k_rate_ec

struct GainsOrder2Vect3 andi_k_rate_ec

Definition at line 338 of file stabilization_andi.c.

Referenced by stabilization_andi_init(), and stabilization_andi_run().

◆ andi_k_rate_rm

struct GainsOrder2Vect3 andi_k_rate_rm

Definition at line 339 of file stabilization_andi.c.

Referenced by stabilization_andi_init(), and stabilization_andi_run().

◆ andi_k_thrust_ec

float andi_k_thrust_ec

Definition at line 342 of file stabilization_andi.c.

Referenced by stabilization_andi_init(), and stabilization_andi_run().

◆ andi_k_thrust_rm

float andi_k_thrust_rm

Definition at line 343 of file stabilization_andi.c.

Referenced by stabilization_andi_init(), and stabilization_andi_run().

◆ andi_p_att_ec

struct PolesOrder3Vect3 andi_p_att_ec

Initial value:

= {
  .omega_n = {
    .x = STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_N_X,
    .y = STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_N_Y,
    .z = STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_N_Z
  },
  .zeta = {
    .x = STABILIZATION_ANDI_POLE_ATT_EC_ZETA_X,
    .y = STABILIZATION_ANDI_POLE_ATT_EC_ZETA_Y,
    .z = STABILIZATION_ANDI_POLE_ATT_EC_ZETA_Z
  },
  .omega_a = {
    .x = STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_A_X,
    .y = STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_A_Y,
    .z = STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_A_Z
  }
}

Definition at line 286 of file stabilization_andi.c.

Referenced by stabilization_andi_init(), and stabilization_andi_run().

◆ andi_p_att_rm

struct PolesOrder3Vect3 andi_p_att_rm

Initial value:

= {
  .omega_n = {
    .x = STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_N_X,
    .y = STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_N_Y,
    .z = STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_N_Z
  },
  .zeta = {
    .x = STABILIZATION_ANDI_POLE_ATT_RM_ZETA_X,
    .y = STABILIZATION_ANDI_POLE_ATT_RM_ZETA_Y,
    .z = STABILIZATION_ANDI_POLE_ATT_RM_ZETA_Z
  },
  .omega_a = {
    .x = STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_A_X,
    .y = STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_A_Y,
    .z = STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_A_Z
  }
}

Definition at line 303 of file stabilization_andi.c.

Referenced by stabilization_andi_init(), and stabilization_andi_run().

◆ andi_p_rate_ec

struct PolesOrder2Vect3 andi_p_rate_ec

Initial value:

= {
  .omega_a = {
    .x = STABILIZATION_ANDI_POLE_RATE_EC_OMEGA_A_X,
    .y = STABILIZATION_ANDI_POLE_RATE_EC_OMEGA_A_Y,
    .z = STABILIZATION_ANDI_POLE_RATE_EC_OMEGA_A_Z
  },
  .zeta = {
    .x = STABILIZATION_ANDI_POLE_RATE_EC_ZETA_X,
    .y = STABILIZATION_ANDI_POLE_RATE_EC_ZETA_Y,
    .z = STABILIZATION_ANDI_POLE_RATE_EC_ZETA_Z
  }
}

Definition at line 262 of file stabilization_andi.c.

Referenced by stabilization_andi_init(), and stabilization_andi_run().

◆ andi_p_rate_rm

struct PolesOrder2Vect3 andi_p_rate_rm

Initial value:

= {
  .omega_a = {
    .x = STABILIZATION_ANDI_POLE_RATE_RM_OMEGA_A_X,
    .y = STABILIZATION_ANDI_POLE_RATE_RM_OMEGA_A_Y,
    .z = STABILIZATION_ANDI_POLE_RATE_RM_OMEGA_A_Z
  },
  .zeta = {
    .x = STABILIZATION_ANDI_POLE_RATE_RM_ZETA_X,
    .y = STABILIZATION_ANDI_POLE_RATE_RM_ZETA_Y,
    .z = STABILIZATION_ANDI_POLE_RATE_RM_ZETA_Z
  }
}

Definition at line 274 of file stabilization_andi.c.

Referenced by stabilization_andi_init(), and stabilization_andi_run().

◆ andi_p_thrust_ec

float andi_p_thrust_ec = STABILIZATION_ANDI_POLE_THRUST_EC

Definition at line 320 of file stabilization_andi.c.

Referenced by stabilization_andi_init(), and stabilization_andi_run().

◆ andi_p_thrust_rm

float andi_p_thrust_rm = STABILIZATION_ANDI_POLE_THRUST_RM

Definition at line 321 of file stabilization_andi.c.

Referenced by stabilization_andi_init(), and stabilization_andi_run().

◆ ANDI_RELAX_OBM

const float ANDI_RELAX_OBM = 1.0f

Definition at line 85 of file stabilization_andi.c.

Referenced by stabilization_andi_run().

◆ angular_rates_obm

struct FloatRates angular_rates_obm

Definition at line 348 of file stabilization_andi.c.

Referenced by stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().

◆ attitude_accel_cf

struct Butterworth2ComplementaryVect3 attitude_accel_cf

Definition at line 347 of file stabilization_andi.c.

Referenced by stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().

◆ attitude_bounds

struct AttQuat attitude_bounds

Definition at line 383 of file stabilization_andi.c.

Referenced by stabilization_andi_init(), and stabilization_andi_run().

◆ attitude_des

struct FloatQuat attitude_des

Definition at line 379 of file stabilization_andi.c.

Referenced by send_stab_attitude_stabilization_andi(), and stabilization_andi_run().

◆ attitude_rates_cf

struct FirstOrderComplementaryVect3 attitude_rates_cf

Definition at line 346 of file stabilization_andi.c.

Referenced by stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().

◆ attitude_ref

struct AttQuat attitude_ref

Definition at line 374 of file stabilization_andi.c.

Referenced by send_stab_attitude_stabilization_andi(), stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().

◆ attitude_state_cf

struct AttStateQuat attitude_state_cf

Definition at line 369 of file stabilization_andi.c.

Referenced by send_stab_attitude_stabilization_andi(), stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().

◆ ce_mat

float ce_mat[ANDI_NUM_ACT *ANDI_OUTPUTS]

Definition at line 388 of file stabilization_andi.c.

Referenced by send_eff_mat_stabilization_andi(), stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().

◆ du_cmd

float du_cmd[ANDI_NUM_ACT]

Definition at line 391 of file stabilization_andi.c.

Referenced by send_wls_u_stabilization_andi(), stabilization_andi_init(), and stabilization_andi_run().

◆ du_max

float du_max[ANDI_NUM_ACT]

Definition at line 390 of file stabilization_andi.c.

Referenced by send_wls_u_stabilization_andi(), and stabilization_andi_run().

◆ du_min

float du_min[ANDI_NUM_ACT]

Definition at line 389 of file stabilization_andi.c.

Referenced by send_wls_u_stabilization_andi(), and stabilization_andi_run().

◆ linear_accel_cf

struct Butterworth2ComplementaryVect3 linear_accel_cf

Definition at line 351 of file stabilization_andi.c.

Referenced by stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().

◆ linear_state_cf

struct LinState linear_state_cf

Definition at line 370 of file stabilization_andi.c.

Referenced by stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().

◆ linear_vel_cf

struct FirstOrderComplementaryVect3 linear_vel_cf

Definition at line 350 of file stabilization_andi.c.

Referenced by stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().

◆ linear_velocity_obm

struct FloatVect3 linear_velocity_obm

Definition at line 352 of file stabilization_andi.c.

Referenced by stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().

◆ nu_ec

float nu_ec[ANDI_OUTPUTS]

Definition at line 396 of file stabilization_andi.c.

Referenced by stabilization_andi_init(), and stabilization_andi_run().

◆ nu_obj

float nu_obj[ANDI_OUTPUTS]

Definition at line 395 of file stabilization_andi.c.

Referenced by send_wls_v_stabilization_andi(), stabilization_andi_init(), and stabilization_andi_run().

◆ nu_obm

float nu_obm[ANDI_OUTPUTS]

Definition at line 397 of file stabilization_andi.c.

Referenced by evaluate_obm_f_stb_x(), stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().

◆ rates_des

struct FloatRates rates_des

Definition at line 378 of file stabilization_andi.c.

Referenced by stabilization_andi_run().

◆ rates_prev

struct FloatRates rates_prev

Definition at line 366 of file stabilization_andi.c.

Referenced by stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().

◆ RC_RATE_MAX

const float RC_RATE_MAX[ANDI_OUTPUTS] = {[0 ... ANDI_OUTPUTS - 1] = 5.0f}

Definition at line 129 of file stabilization_andi.c.

Referenced by stabilization_rate_read_rc().

◆ SCHEDULE_EFF

const bool SCHEDULE_EFF = false

Definition at line 73 of file stabilization_andi.c.

Referenced by stabilization_andi_run().

◆ thrust_bounds_max

struct ThrustRef thrust_bounds_max

Definition at line 385 of file stabilization_andi.c.

Referenced by stabilization_andi_init(), and stabilization_andi_run().

◆ thrust_bounds_min

struct ThrustRef thrust_bounds_min

Definition at line 384 of file stabilization_andi.c.

Referenced by stabilization_andi_init(), and stabilization_andi_run().

◆ thrust_des

float thrust_des

Definition at line 380 of file stabilization_andi.c.

Referenced by generate_reference_thrust(), and stabilization_andi_run().

◆ THRUST_MIN

const float THRUST_MIN = 0.0f

Definition at line 135 of file stabilization_andi.c.

Referenced by stabilization_andi_init().

◆ thrust_ref

struct ThrustRef thrust_ref

Definition at line 375 of file stabilization_andi.c.

Referenced by control_error_thrust(), generate_reference_thrust(), stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().

◆ thrust_state

float thrust_state

Definition at line 371 of file stabilization_andi.c.

Referenced by control_error_thrust(), stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().

◆ u_cmd

float u_cmd[ANDI_NUM_ACT]

Definition at line 392 of file stabilization_andi.c.

Referenced by stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().

◆ USE_STATE_DYNAMICS

const bool USE_STATE_DYNAMICS = false

Definition at line 79 of file stabilization_andi.c.

Referenced by stabilization_andi_run().

◆ wls_stab_p

struct WLS_t wls_stab_p

Initial value:

= {
  .nu = ANDI_NUM_ACT,
  .nv = ANDI_OUTPUTS,
  .gamma_sq = 100000.0,
  .u_pref = {0.0f},
  .u_min = {0.0f},
  .u_max = {0.0f},
  .PC = 0.0f,
  .SC = 0.0f,
  .iter = 0
}

Definition at line 325 of file stabilization_andi.c.

Referenced by schdule_control_effectiveness(), send_wls_v_stabilization_andi(), stabilization_andi_run(), and stabilization_indi_set_wls_settings().

◆ WLS_WU

float WLS_WU[ANDI_NUM_ACT] = {[0 ... ANDI_NUM_ACT - 1] = 1.0f}

Normalized actuator cost for WLS allocation.

Each value corresponds to the relative cost of using each actuator, normalized over the possible range of u_dot for that actuator. FIXME: This normalization is not ideal or constant, as it depends on the scaling of the control.

Definition at line 172 of file stabilization_andi.c.

Referenced by send_wls_u_stabilization_andi(), and stabilization_andi_run().

◆ WLS_WV

const float WLS_WV[ANDI_OUTPUTS] = {[0 ... ANDI_OUTPUTS - 1] = 1.0f}

Definition at line 160 of file stabilization_andi.c.

Referenced by send_wls_v_stabilization_andi(), and stabilization_andi_run().

Functions

Variables

Detailed Description

Function Documentation

◆ actuators_t4_in_callback()

◆ autopilot_in_flight_end_detection()

◆ compute_error_gains_order_2_vect_3()

◆ compute_error_gains_order_3_vect_3()

◆ compute_reference_gains_order_2_vect_3()

◆ compute_reference_gains_order_3_vect_3()

◆ compute_wls_lower_bounds()

◆ compute_wls_u_scaler()

◆ compute_wls_upper_bounds()

◆ compute_wls_v_scaler()

◆ control_error_attitude()

◆ control_error_rate()

◆ control_error_thrust()

◆ ec_k1_order2_f()

◆ ec_k1_order3_f()

◆ ec_k2_order2_f()

◆ ec_k2_order3_f()

◆ ec_k3_order3_f()

◆ fetch_actuators_t4()

◆ generate_reference_attitude()

◆ generate_reference_rate()

◆ generate_reference_thrust()

◆ rm_k1_order2_f()

◆ rm_k1_order3_f()

◆ rm_k2_order2_f()

◆ rm_k2_order3_f()

◆ rm_k3_order3_f()

◆ send_eff_mat_stabilization_andi()

◆ send_stab_attitude_stabilization_andi()

◆ send_wls_u_stabilization_andi()

◆ send_wls_v_stabilization_andi()

◆ stabilization_andi_enter()

◆ stabilization_andi_init()

◆ stabilization_andi_run()

◆ stabilization_attitude_enter()

◆ stabilization_attitude_run()

◆ stabilization_rate_enter()

◆ stabilization_rate_read_rc()

◆ stabilization_rate_run()

Variable Documentation

◆ ACTUATOR_DELAY

◆ ACTUATOR_IS_SERVO

◆ actuator_meas

◆ actuator_obm_delay

◆ actuator_obm_zohlpf

◆ ACTUATOR_PREF

◆ actuator_state

◆ actuator_state_lpf

◆ actuators_t4_in_event

◆ actuators_t4_obs

◆ andi_k_att_ec

◆ andi_k_att_rm

◆ andi_k_rate_ec

◆ andi_k_rate_rm

◆ andi_k_thrust_ec

◆ andi_k_thrust_rm

◆ andi_p_att_ec

◆ andi_p_att_rm

◆ andi_p_rate_ec

◆ andi_p_rate_rm

◆ andi_p_thrust_ec

◆ andi_p_thrust_rm

◆ ANDI_RELAX_OBM

◆ angular_rates_obm

◆ attitude_accel_cf

◆ attitude_bounds

◆ attitude_des

◆ attitude_rates_cf

◆ attitude_ref

◆ attitude_state_cf

◆ ce_mat

◆ du_cmd

◆ du_max

◆ du_min

◆ linear_accel_cf

◆ linear_state_cf