|
Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
ANDI stabilization controller for tiltbody rotorcraft. More...
#include "firmwares/rotorcraft/stabilization/stabilization_andi.h"#include "firmwares/rotorcraft/stabilization/stabilization_attitude.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"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) |
ANDI stabilization controller for tiltbody rotorcraft.
Implements Adaptive Nonlinear Dynamic Inversion (ANDI) for attitude and thrust control with on-board model compensation.
Assumptions:
FIXME: WLS allocation normalization depends on control effectiveness scaling (state-dependent). Consider normalizing costs by max achievable derivatives per actuator/output.
Definition in file stabilization_andi.c.
|
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.
| 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 506 of file stabilization_andi.c.
References actuators_t4_obs.
Referenced by stabilization_andi_init().
| bool autopilot_in_flight_end_detection | ( | bool motors_on | ) |
Definition at line 1413 of file stabilization_andi.c.
|
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.
| [in] | poles | Pointer to PolesOrder2Vect3 containing omega_n and zeta for each axis. |
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().
|
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.
| [in] | poles | Pointer to PolesOrder3Vect3 containing omega_n, zeta, and p1 for each axis. |
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().
|
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.
| [in] | poles | Pointer to PolesOrder2Vect3 containing omega_n and zeta for each axis. |
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().
|
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.
| [in] | poles | Pointer to PolesOrder3Vect3 containing omega_n, zeta, and p1 for each axis. |
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().
|
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:
| [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 915 of file stabilization_andi.c.
References act_min, and ANDI_NUM_ACT.
Referenced by stabilization_andi_run().
|
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
| [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 940 of file stabilization_andi.c.
References act_max, act_min, and ANDI_NUM_ACT.
Referenced by stabilization_andi_run().
|
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:
| [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 889 of file stabilization_andi.c.
References act_max, and ANDI_NUM_ACT.
Referenced by stabilization_andi_run().
|
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.
| [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 964 of file stabilization_andi.c.
References ANDI_NUM_ACT.
|
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.
| [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. |
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().
|
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.
| [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. |
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().
|
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.
| [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. |
Definition at line 865 of file stabilization_andi.c.
References nu, thrust_ref, and thrust_state.
Referenced by stabilization_andi_run().
|
inlinestatic |
Definition at line 258 of file stabilization_andi.c.
Referenced by compute_error_gains_order_2_vect_3().
|
inlinestatic |
Definition at line 250 of file stabilization_andi.c.
Referenced by compute_error_gains_order_3_vect_3().
|
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().
|
inlinestatic |
Definition at line 251 of file stabilization_andi.c.
Referenced by compute_error_gains_order_3_vect_3().
|
inlinestatic |
Definition at line 252 of file stabilization_andi.c.
Referenced by compute_error_gains_order_3_vect_3().
|
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.
| [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 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().
|
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.
| [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 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().
|
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.
| [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 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().
|
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.
| [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 766 of file stabilization_andi.c.
References ThrustRef::thrust, ThrustRef::thrust_d, thrust_des, and thrust_ref.
Referenced by stabilization_andi_run().
|
inlinestatic |
Definition at line 260 of file stabilization_andi.c.
References ec_k2_order2_f().
Referenced by compute_reference_gains_order_2_vect_3().
|
inlinestatic |
Definition at line 254 of file stabilization_andi.c.
Referenced by compute_reference_gains_order_3_vect_3().
|
inlinestatic |
Definition at line 261 of file stabilization_andi.c.
References ec_k2_order2_f().
Referenced by compute_reference_gains_order_2_vect_3().
|
inlinestatic |
Definition at line 255 of file stabilization_andi.c.
Referenced by compute_reference_gains_order_3_vect_3().
|
inlinestatic |
Definition at line 256 of file stabilization_andi.c.
Referenced by compute_reference_gains_order_3_vect_3().
|
static |
Definition at line 427 of file stabilization_andi.c.
References ANDI_NUM_ACT, ce_mat, and dev.
Referenced by stabilization_andi_init().
|
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().
|
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().
|
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().
| 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.
| void stabilization_andi_init | ( | void | ) |
Definition at line 975 of file stabilization_andi.c.
References ABI_BROADCAST, 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, 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(), 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, 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(), thrust_bounds_max, thrust_bounds_min, THRUST_MIN, thrust_ref, thrust_state, and u_cmd.
| 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:
| [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. |
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.
| 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().
| void stabilization_attitude_run | ( | bool | in_flight, |
| struct StabilizationSetpoint * | sp, | ||
| struct ThrustSetpoint * | thrust, | ||
| int32_t * | cmd ) |
Attitude control run function.
| [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 1388 of file stabilization_andi.c.
Referenced by guidance_flip_run(), guidance_module_run(), stabilization_run(), and vertical_ctrl_module_run().
| void stabilization_rate_enter | ( | void | ) |
Definition at line 1376 of file stabilization_andi.c.
Referenced by stabilization_mode_changed().
| struct StabilizationSetpoint stabilization_rate_read_rc | ( | struct RadioControl * | rc | ) |
Definition at line 1396 of file stabilization_andi.c.
References FLOAT_RATES_ZERO, MAX_PPRZ, FloatRates::p, PITCH_RATE_DEADBAND_EXCEEDED, FloatRates::q, FloatRates::r, RC_RATE_MAX, RC_RATE_P, RC_RATE_Q, RC_RATE_R, ROLL_RATE_DEADBAND_EXCEEDED, stab_sp_from_rates_f(), stabilization_rate_read_rc(), and YAW_RATE_DEADBAND_EXCEEDED.
Referenced by rc_cb(), and stabilization_rate_read_rc().
| 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().
| const uint8_t ACTUATOR_DELAY[ANDI_NUM_ACT] = {0} |
Definition at line 104 of file stabilization_andi.c.
Referenced by stabilization_andi_init().
| const bool ACTUATOR_IS_SERVO[ANDI_NUM_ACT] = {0} |
Definition at line 92 of file stabilization_andi.c.
| float actuator_meas[ANDI_NUM_ACT] |
Definition at line 364 of file stabilization_andi.c.
Referenced by fetch_actuators_t4(), and stabilization_andi_run().
| struct TransportDelay actuator_obm_delay[ANDI_NUM_ACT] |
Definition at line 357 of file stabilization_andi.c.
Referenced by stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().
| struct FirstOrderZOHLowPass actuator_obm_zohlpf[ANDI_NUM_ACT] |
Definition at line 356 of file stabilization_andi.c.
Referenced by stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().
| 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().
| float actuator_state[ANDI_NUM_ACT] |
Definition at line 358 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(), stabilization_andi_run(), stabilization_indi_commit_actuator_cmd(), stabilization_indi_get_act_state(), stabilization_indi_get_actuator_state(), and stabilization_indi_rate_run().
| float actuator_state_lpf[ANDI_NUM_ACT] |
Definition at line 359 of file stabilization_andi.c.
| abi_event actuators_t4_in_event |
Definition at line 363 of file stabilization_andi.c.
Referenced by stabilization_andi_init().
| struct ActuatorsT4In actuators_t4_obs |
Definition at line 362 of file stabilization_andi.c.
Referenced by actuators_t4_in_callback(), stabilization_andi_enter(), and stabilization_andi_run().
| struct GainsOrder3Vect3 andi_k_att_ec |
Definition at line 341 of file stabilization_andi.c.
Referenced by stabilization_andi_init(), and stabilization_andi_run().
| struct GainsOrder3Vect3 andi_k_att_rm |
Definition at line 342 of file stabilization_andi.c.
Referenced by stabilization_andi_init(), and stabilization_andi_run().
| struct GainsOrder2Vect3 andi_k_rate_ec |
Definition at line 339 of file stabilization_andi.c.
Referenced by stabilization_andi_init(), and stabilization_andi_run().
| struct GainsOrder2Vect3 andi_k_rate_rm |
Definition at line 340 of file stabilization_andi.c.
Referenced by stabilization_andi_init(), and stabilization_andi_run().
| float andi_k_thrust_ec |
Definition at line 343 of file stabilization_andi.c.
Referenced by stabilization_andi_init(), and stabilization_andi_run().
| float andi_k_thrust_rm |
Definition at line 344 of file stabilization_andi.c.
Referenced by stabilization_andi_init(), and stabilization_andi_run().
| struct PolesOrder3Vect3 andi_p_att_ec |
Definition at line 287 of file stabilization_andi.c.
Referenced by stabilization_andi_init(), and stabilization_andi_run().
| struct PolesOrder3Vect3 andi_p_att_rm |
Definition at line 304 of file stabilization_andi.c.
Referenced by stabilization_andi_init(), and stabilization_andi_run().
| struct PolesOrder2Vect3 andi_p_rate_ec |
Definition at line 263 of file stabilization_andi.c.
Referenced by stabilization_andi_init(), and stabilization_andi_run().
| struct PolesOrder2Vect3 andi_p_rate_rm |
Definition at line 275 of file stabilization_andi.c.
Referenced by stabilization_andi_init(), and stabilization_andi_run().
| 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().
| 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().
| const float ANDI_RELAX_OBM = 1.0f |
Definition at line 86 of file stabilization_andi.c.
Referenced by stabilization_andi_run().
| struct FloatRates angular_rates_obm |
Definition at line 349 of file stabilization_andi.c.
Referenced by stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().
| struct Butterworth2ComplementaryVect3 attitude_accel_cf |
Definition at line 348 of file stabilization_andi.c.
Referenced by stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().
| struct AttQuat attitude_bounds |
Definition at line 384 of file stabilization_andi.c.
Referenced by stabilization_andi_init(), and stabilization_andi_run().
| struct FloatQuat attitude_des |
Definition at line 380 of file stabilization_andi.c.
Referenced by send_stab_attitude_stabilization_andi(), and stabilization_andi_run().
| struct FirstOrderComplementaryVect3 attitude_rates_cf |
Definition at line 347 of file stabilization_andi.c.
Referenced by stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().
| struct AttQuat attitude_ref |
Definition at line 375 of file stabilization_andi.c.
Referenced by send_stab_attitude_stabilization_andi(), stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().
| struct AttStateQuat attitude_state_cf |
Definition at line 370 of file stabilization_andi.c.
Referenced by send_stab_attitude_stabilization_andi(), stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().
| float ce_mat[ANDI_NUM_ACT *ANDI_OUTPUTS] |
Definition at line 389 of file stabilization_andi.c.
Referenced by send_eff_mat_stabilization_andi(), stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().
| float du_cmd[ANDI_NUM_ACT] |
Definition at line 392 of file stabilization_andi.c.
Referenced by send_wls_u_stabilization_andi(), stabilization_andi_init(), and stabilization_andi_run().
| 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().
| 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().
| struct Butterworth2ComplementaryVect3 linear_accel_cf |
Definition at line 352 of file stabilization_andi.c.
Referenced by stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().
| struct LinState linear_state_cf |
Definition at line 371 of file stabilization_andi.c.
Referenced by stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().
| struct FirstOrderComplementaryVect3 linear_vel_cf |
Definition at line 351 of file stabilization_andi.c.
Referenced by stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().
| struct FloatVect3 linear_velocity_obm |
Definition at line 353 of file stabilization_andi.c.
Referenced by stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().
| float nu_ec[ANDI_OUTPUTS] |
Definition at line 397 of file stabilization_andi.c.
Referenced by stabilization_andi_init(), and stabilization_andi_run().
| float nu_obj[ANDI_OUTPUTS] |
Definition at line 396 of file stabilization_andi.c.
Referenced by send_wls_v_stabilization_andi(), stabilization_andi_init(), and stabilization_andi_run().
| float nu_obm[ANDI_OUTPUTS] |
Definition at line 398 of file stabilization_andi.c.
Referenced by evaluate_obm_f_stb_x(), stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().
| struct FloatRates rates_des |
Definition at line 379 of file stabilization_andi.c.
Referenced by stabilization_andi_run().
| struct FloatRates rates_prev |
Definition at line 367 of file stabilization_andi.c.
Referenced by stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().
| 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().
| const bool SCHEDULE_EFF = false |
Definition at line 74 of file stabilization_andi.c.
Referenced by stabilization_andi_run().
| struct ThrustRef thrust_bounds_max |
Definition at line 386 of file stabilization_andi.c.
Referenced by stabilization_andi_init(), and stabilization_andi_run().
| struct ThrustRef thrust_bounds_min |
Definition at line 385 of file stabilization_andi.c.
Referenced by stabilization_andi_init(), and stabilization_andi_run().
| float thrust_des |
Definition at line 381 of file stabilization_andi.c.
Referenced by generate_reference_thrust(), and stabilization_andi_run().
| const float THRUST_MIN = 0.0f |
Definition at line 136 of file stabilization_andi.c.
Referenced by stabilization_andi_init().
| struct ThrustRef thrust_ref |
Definition at line 376 of file stabilization_andi.c.
Referenced by control_error_thrust(), generate_reference_thrust(), stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().
| float thrust_state |
Definition at line 372 of file stabilization_andi.c.
Referenced by control_error_thrust(), stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().
| float u_cmd[ANDI_NUM_ACT] |
Definition at line 393 of file stabilization_andi.c.
Referenced by stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().
| const bool USE_STATE_DYNAMICS = false |
Definition at line 80 of file stabilization_andi.c.
Referenced by stabilization_andi_run().
| struct WLS_t wls_stab_p |
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().
| 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().
| 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().