Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
stabilization_andi.c File Reference

ANDI stabilization controller for tiltbody rotorcraft. More...

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)
 nu_obj: Total pseudo command sent to the actuators nu_ec: Pseudo command from the error controller nu_obm: Pseudo command from the on board model state dependent term nu_reconstructed: Reconstructed angular acceleration from the actuator commands.
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()

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

nu_obj: Total pseudo command sent to the actuators nu_ec: Pseudo command from the error controller nu_obm: Pseudo command from the on board model state dependent term nu_reconstructed: Reconstructed angular acceleration from the actuator commands.

Callback function to handle incoming actuator telemetry data.

This is used to verify the on board model. If the OBM is perfect, then nu_reconstructed should be equal to the measured state plus any external disturbances.

nu_obj = nu_ec + nu_obm nu_reconstructed = Ce * u_meas - nu_obm NOTE: This is wrong equation, fix it.

FIXME: nu_reconstructed has been repurposed and no longer is the reconstructed value. Update the telemetry message accordingly.

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_idIdentifier of the sender of the telemetry data.
actuators_t4_in_ptrPointer to the incoming ActuatorsT4In struct containing ESC telemetry, servo angles, loads, and other actuator info.
actuators_t4_extra_data_in_ptrPointer to additional extra actuator data (unused).

Definition at line 506 of file stabilization_andi.c.

References actuators_t4_obs.

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 1413 of file stabilization_andi.c.

◆ compute_error_gains_order_2_vect_3()

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]polesPointer to PolesOrder2Vect3 containing omega_n and zeta for each axis.
Returns
Struct containing k1, k2 gains for x, y, z.

Definition at line 612 of file stabilization_andi.c.

References compute_error_gains_order_2_vect_3(), ec_k1_order2_f(), ec_k2_order2_f(), and gains.

Referenced by compute_error_gains_order_2_vect_3(), 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()

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]polesPointer 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 589 of file stabilization_andi.c.

References compute_error_gains_order_3_vect_3(), ec_k1_order3_f(), ec_k2_order3_f(), ec_k3_order3_f(), and gains.

Referenced by compute_error_gains_order_3_vect_3(), 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()

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]polesPointer to PolesOrder2Vect3 containing omega_n and zeta for each axis.
Returns
Struct containing k1, k2 gains for x, y, z.

Definition at line 570 of file stabilization_andi.c.

References compute_reference_gains_order_2_vect_3(), gains, rm_k1_order2_f(), and rm_k2_order2_f().

Referenced by compute_reference_gains_order_2_vect_3(), 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()

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]polesPointer 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 547 of file stabilization_andi.c.

References compute_reference_gains_order_3_vect_3(), gains, rm_k1_order3_f(), rm_k2_order3_f(), and rm_k3_order3_f().

Referenced by compute_reference_gains_order_3_vect_3(), 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()

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:

  1. The actuator position does not go below its specified minimum (act_min) in the next timestep.
  2. 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_minArray to store the computed lower bounds for actuator rates.
[in]act_stateCurrent states (positions) of the actuators.
[in]act_minMinimum allowable positions for the actuators.
[in]act_rate_minMinimum allowable rates for the actuators.
[in]actuator_bandwidthBandwidth of the actuators used to compute rate limits.

Definition at line 915 of file stabilization_andi.c.

References act_min, and ANDI_NUM_ACT.

Referenced by stabilization_andi_run().

Here is the caller graph for this function:

◆ compute_wls_u_scaler()

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_scalerArray to store the computed input scaling factors.
[in]act_minArray of minimum actuator values.
[in]act_maxArray of maximum actuator values.

Definition at line 940 of file stabilization_andi.c.

References act_max, act_min, and ANDI_NUM_ACT.

Referenced by stabilization_andi_run().

Here is the caller graph for this function:

◆ compute_wls_upper_bounds()

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:

  1. The actuator position does not exceed its specified maximum (act_max) in the next timestep.
  2. 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_maxArray to store the computed upper bounds for actuator rates.
[in]act_stateCurrent states (positions) of the actuators.
[in]act_maxMaximum allowable positions for the actuators.
[in]act_rate_maxMaximum allowable rates for the actuators.
[in]actuator_bandwidthBandwidth of the actuators used to compute rate limits.

Definition at line 889 of file stabilization_andi.c.

References act_max, and ANDI_NUM_ACT.

Referenced by stabilization_andi_run().

Here is the caller graph for this function:

◆ compute_wls_v_scaler()

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_scalerArray of length ANDI_NUM_ACT to store the computed inverse scaling factors.
[in]vArray of length ANDI_NUM_ACT containing reference/scaling values used to compute the inverse.

Definition at line 964 of file stabilization_andi.c.

References ANDI_NUM_ACT.

◆ control_error_attitude()

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_refPointer to the reference attitude and rate states (quaternion-based).
[in]att_statePointer to the current attitude and rate states.
[in]k_att_ecPointer to gain parameters struct containing proportional, derivative, and jerk gains.
Returns
The computed virtual control input vector.

Definition at line 825 of file stabilization_andi.c.

References att_state, control_error_attitude(), float_quat_inv_comp_norm_shortest(), nu, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by control_error_attitude(), and stabilization_andi_run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ control_error_rate()

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_refPointer to the reference attitude and rate state structure.
[in]att_statePointer to the current attitude and rate state structure.
[in]k_rate_ecPointer to gain parameters structure, containing proportional and derivative gains.
Returns
A FloatVect3 structure representing the computed virtual control input vector.

Definition at line 794 of file stabilization_andi.c.

References att_state, control_error_rate(), nu, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by control_error_rate(), and stabilization_andi_run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ control_error_thrust()

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_refPointer to a structure containing the desired thrust values.
[in]thrust_stateCurrent measured thrust value.
[in]k_thrust_ecProportional gain applied to the thrust error correction.
Returns
The computed virtual control thrust input.

Definition at line 865 of file stabilization_andi.c.

References nu, thrust_ref, and thrust_state.

Referenced by stabilization_andi_run().

Here is the caller graph for this function:

◆ ec_k1_order2_f()

float ec_k1_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().

Here is the caller graph for this function:

◆ ec_k1_order3_f()

float ec_k1_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_k2_order2_f()

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

Definition at line 259 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()

float ec_k2_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:

◆ ec_k3_order3_f()

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

Definition at line 252 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()

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_stateArray of floats with size ANDI_NUM_ACT where the converted actuator states will be stored. The caller must allocate this.
[in]actuators_t4_in_ptrPointer to the input struct containing actuator telemetry, including servo angles (in 1e-2 degrees) and ESC RPM.

Definition at line 531 of file stabilization_andi.c.

References actuator_meas, ANDI_NUM_ACT, ActuatorsT4In::esc_1_rpm, ActuatorsT4In::esc_2_rpm, ActuatorsT4In::servo_1_angle, and ActuatorsT4In::servo_6_angle.

Referenced by stabilization_andi_enter(), and stabilization_andi_run().

Here is the caller graph for this function:

◆ generate_reference_attitude()

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]dtSampling interval in seconds.
[in]att_desDesired attitude as a unit quaternion.
[in]k_att_rmGain parameters (proportional and derivative gains for rate control).
[in]boundsLimits on rate references and derivatives (angular velocity and higher).
[in,out]att_refReference 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 703 of file stabilization_andi.c.

References AttQuat::att, AttQuat::att_2d, AttQuat::att_3d, AttQuat::att_d, float_quat_integrate(), float_quat_inv_comp_norm_shortest(), float_quat_normalize(), k_att_rm, FloatRates::p, FloatRates::q, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, FloatRates::r, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by stabilization_andi_run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ generate_reference_rate()

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]dtThe sampling interval in seconds.
[in]rate_desDesired angular rates (p, q, r) as a FloatRates structure.
[in]k_rate_rmGain parameters as GainsOrder2Vect3, for the proportional and derivative terms.
[in]boundsLimits for the desired rates, rates derivatives, and accelerations, in AttRefEulers.
[in,out]att_refThe reference model states (attitude quaternion, rates, and derivatives), updated in place to produce the reference command.

Definition at line 639 of file stabilization_andi.c.

References AttQuat::att, AttQuat::att_2d, AttQuat::att_3d, AttQuat::att_d, float_quat_identity(), GainsOrder2Vect3::k1, GainsOrder2Vect3::k2, FloatRates::p, FloatRates::q, FloatRates::r, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by stabilization_andi_run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ generate_reference_thrust()

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]dtSampling interval in seconds.
[in]thrust_desDesired thrust input value.
[in]k_thrust_rmGain parameter for thrust rate control (proportional gain).
[in]boundsMaximum allowable thrust magnitude.
[in,out]thrust_refPointer to ThrustRef struct holding thrust reference states.

Definition at line 766 of file stabilization_andi.c.

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

float rm_k1_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_k1_order3_f()

float rm_k1_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_k2_order2_f()

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

Definition at line 261 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()

float rm_k2_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:

◆ rm_k3_order3_f()

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

Definition at line 256 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()

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

Definition at line 427 of file stabilization_andi.c.

References ANDI_NUM_ACT, ce_mat, and dev.

Referenced by stabilization_andi_init().

Here is the caller graph for this function:

◆ send_stab_attitude_stabilization_andi()

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

Definition at line 436 of file stabilization_andi.c.

References actuator_state, ANDI_OUTPUTS, attitude_des, attitude_ref, attitude_state_cf, and dev.

Referenced by stabilization_andi_init().

Here is the caller graph for this function:

◆ send_wls_u_stabilization_andi()

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

Definition at line 414 of file stabilization_andi.c.

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

Referenced by stabilization_andi_init().

Here is the caller graph for this function:

◆ send_wls_v_stabilization_andi()

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

Definition at line 403 of file stabilization_andi.c.

References ANDI_OUTPUTS, dev, 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 1093 of file stabilization_andi.c.

References actuator_obm_delay, actuator_obm_zohlpf, ACTUATOR_PREF, actuator_state, actuators_t4_obs, ANDI_NUM_ACT, ANDI_OUTPUTS, angular_rates_obm, 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, 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(), thrust_ref, thrust_state, and u_cmd.

Here is the call graph for this function:

◆ stabilization_andi_init()

◆ 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:

  1. 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)
  1. 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)
  1. 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.
  1. 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.
  1. 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_controlFlag indicating whether to use rate control mode.
[in]in_flightFlag indicating whether the vehicle is in flight.
[in,out]stab_setpointPointer to the stabilization setpoint structure.
[in,out]thrust_setpointPointer to the thrust setpoint structure.
[out]cmdPointer 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 1184 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(), 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, u_cmd, 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, LinState::vel, wls_alloc(), wls_stab_p, WLS_WU, WLS_WV, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Here is the call graph for this function:

◆ stabilization_attitude_enter()

void stabilization_attitude_enter ( void )

Attitude control enter function.

Definition at line 1371 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_flighttrue if in flight
[in]sppointer to the stabilization setpoint structure
[in]thrustpointer to the thrust setoint structure
[out]cmdpointer to the output command vector

Definition at line 1388 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 1376 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)

◆ stabilization_rate_run()

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

Definition at line 1381 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 104 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 92 of file stabilization_andi.c.

◆ actuator_meas

float actuator_meas[ANDI_NUM_ACT]

Definition at line 364 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]

◆ actuator_obm_zohlpf

◆ ACTUATOR_PREF

const float ACTUATOR_PREF[ANDI_NUM_ACT] = {0.0f}

Definition at line 124 of file stabilization_andi.c.

Referenced by stabilization_andi_enter(), and stabilization_andi_init().

◆ actuator_state

◆ actuator_state_lpf

float actuator_state_lpf[ANDI_NUM_ACT]

Definition at line 359 of file stabilization_andi.c.

◆ actuators_t4_in_event

abi_event actuators_t4_in_event

Definition at line 363 of file stabilization_andi.c.

Referenced by stabilization_andi_init().

◆ actuators_t4_obs

struct ActuatorsT4In actuators_t4_obs

◆ andi_k_att_ec

struct GainsOrder3Vect3 andi_k_att_ec

Definition at line 341 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 342 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 339 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 340 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 343 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 344 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 287 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 304 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 263 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 275 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 321 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 322 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 86 of file stabilization_andi.c.

Referenced by stabilization_andi_run().

◆ angular_rates_obm

struct FloatRates angular_rates_obm

◆ attitude_accel_cf

◆ attitude_bounds

struct AttQuat attitude_bounds

Definition at line 384 of file stabilization_andi.c.

Referenced by stabilization_andi_init(), and stabilization_andi_run().

◆ attitude_des

struct FloatQuat attitude_des

◆ attitude_rates_cf

◆ attitude_ref

◆ attitude_state_cf

◆ ce_mat

◆ du_cmd

◆ du_max

float du_max[ANDI_NUM_ACT]

Definition at line 391 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 390 of file stabilization_andi.c.

Referenced by send_wls_u_stabilization_andi(), and stabilization_andi_run().

◆ linear_accel_cf

◆ linear_state_cf

struct LinState linear_state_cf

◆ linear_vel_cf

◆ linear_velocity_obm

struct FloatVect3 linear_velocity_obm

◆ nu_ec

float nu_ec[ANDI_OUTPUTS]

Definition at line 397 of file stabilization_andi.c.

Referenced by stabilization_andi_init(), and stabilization_andi_run().

◆ nu_obj

◆ nu_obm

◆ rates_des

struct FloatRates rates_des

Definition at line 379 of file stabilization_andi.c.

Referenced by stabilization_andi_run().

◆ rates_prev

struct FloatRates rates_prev

◆ RC_RATE_MAX

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

Definition at line 130 of file stabilization_andi.c.

Referenced by stabilization_rate_read_rc().

◆ SCHEDULE_EFF

const bool SCHEDULE_EFF = false

Definition at line 74 of file stabilization_andi.c.

Referenced by stabilization_andi_run().

◆ thrust_bounds_max

struct ThrustRef thrust_bounds_max

Definition at line 386 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 385 of file stabilization_andi.c.

Referenced by stabilization_andi_init(), and stabilization_andi_run().

◆ thrust_des

float thrust_des

Definition at line 381 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 136 of file stabilization_andi.c.

Referenced by stabilization_andi_init().

◆ thrust_ref

◆ thrust_state

◆ u_cmd

◆ USE_STATE_DYNAMICS

const bool USE_STATE_DYNAMICS = false

Definition at line 80 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
}
#define ANDI_NUM_ACT
#define ANDI_OUTPUTS

Definition at line 326 of file stabilization_andi.c.

Referenced by schdule_control_effectiveness(), send_wls_u_stab(), send_wls_v_stab(), send_wls_v_stabilization_andi(), stabilization_andi_run(), stabilization_indi_rate_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 173 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 161 of file stabilization_andi.c.

Referenced by send_wls_v_stabilization_andi(), and stabilization_andi_run().