|
Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
ANDI stabilization controller for tiltbody rotorcraft. More...
#include "firmwares/rotorcraft/stabilization.h"#include "firmwares/rotorcraft/stabilization/stabilization_rate.h"#include "generated/airframe.h"#include <stdio.h>#include "filters/low_pass_filter.h"Go to the source code of this file.
Data Structures | |
| struct | AttQuat |
| Structure representing attitude in quaternion form along with its first three derivatives, used for reference. More... | |
| struct | AttStateQuat |
| Structure representing attitude in quaternion form along with its first two derivatives, used for state. More... | |
| struct | LinState |
| Structure representing linear state with velocity and acceleration vectors. More... | |
| struct | ThrustRef |
| Structure representing (specific) thrust reference and its derivative. More... | |
| struct | PolesOrder3Vect3 |
| Structure defining third-order pole parameters for vectorized 3D quantities. More... | |
| struct | PolesOrder2Vect3 |
| Structure defining second-order pole parameters for vectorized 3D quantities. More... | |
| struct | GainsOrder2Vect3 |
| Structure defining second-order gain parameters for vectorized 3D quantities. More... | |
| struct | GainsOrder3Vect3 |
| Structure defining third-order gain parameters for vectorized 3D quantities. More... | |
Macros | |
| #define | ANDI_NUM_ACT COMMANDS_NB_REAL |
Functions | |
| 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. | |
| struct FloatVect3 | evaluate_obm_forces (const struct FloatRates *rates, const struct FloatVect3 *vel_body, const float actuator_state[ANDI_NUM_ACT]) |
| Evaluate total force acting on the vehicle from the OBM. | |
| struct FloatVect3 | evaluate_obm_moments (const struct FloatRates *rates, const struct FloatVect3 *vel_body, const float actuator_state[ANDI_NUM_ACT]) |
| Evaluate total moments acting on the vehicle from the OBM. | |
| void | evaluate_obm_f_stb_u (float fu_mat[ANDI_NUM_ACT *ANDI_OUTPUTS], const struct FloatRates *rates, const struct FloatVect3 *vel_body, const float actuator_state[ANDI_NUM_ACT]) |
| Evaluate the state-dependent control effectiveness matrix F_u for stabilization. | |
| void | evaluate_obm_f_stb_x (float nu_obm[ANDI_OUTPUTS], const struct FloatRates *rates, const struct FloatVect3 *vel_body, const struct FloatVect3 *ang_accel, const struct FloatVect3 *accel_body, const float actuator_state[ANDI_NUM_ACT]) |
| Evaluate the state-dependent contribution F_x * x_dot for stabilization. | |
| float | evaluate_obm_thrust_z (const float actuator_state[ANDI_NUM_ACT]) |
| Compute total thrust produced by the current actuator state. | |
Variables | |
| 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 |
| float | andi_p_thrust_rm |
ANDI stabilization controller for tiltbody rotorcraft.
Provides declarations, configuration parameters, and interfaces for the ANDI controller implemented in stabilization_andi.c.
Definition in file stabilization_andi.h.
| struct AttQuat |
Structure representing attitude in quaternion form along with its first three derivatives, used for reference.
This structure encapsulates the attitude represented as a quaternion, its first derivative (angular rates), second derivative (angular accelerations), and third derivative (angular jerks). It is primarily used for reference representation in the ANDI controller.
Definition at line 60 of file stabilization_andi.h.
| Data Fields | ||
|---|---|---|
| struct FloatQuat | att | |
| struct FloatVect3 | att_2d | |
| struct FloatVect3 | att_3d | |
| struct FloatRates | att_d | |
| struct AttStateQuat |
Structure representing attitude in quaternion form along with its first two derivatives, used for state.
This structure encapsulates the attitude represented as a quaternion, its first derivative (angular rates), and second derivative (angular accelerations). It is primarily used for state representation in the ANDI controller.
Definition at line 74 of file stabilization_andi.h.
| Data Fields | ||
|---|---|---|
| struct FloatQuat | att | |
| struct FloatVect3 | att_2d | |
| struct FloatRates | att_d | |
| struct LinState |
Structure representing linear state with velocity and acceleration vectors.
This structure encapsulates the linear state of the vehicle, including its velocity and acceleration as 3D vectors.
Definition at line 86 of file stabilization_andi.h.
| Data Fields | ||
|---|---|---|
| struct FloatVect3 | acc | |
| struct FloatVect3 | vel | |
| struct ThrustRef |
Structure representing (specific) thrust reference and its derivative.
This structure encapsulates the thrust reference, including the thrust value and its first derivative.
Definition at line 97 of file stabilization_andi.h.
| Data Fields | ||
|---|---|---|
| float | thrust | |
| float | thrust_d | |
| struct PolesOrder3Vect3 |
Structure defining third-order pole parameters for vectorized 3D quantities.
This structure holds the natural frequencies, damping ratios, and the pseudo actuator dynamics.
Definition at line 108 of file stabilization_andi.h.
| Data Fields | ||
|---|---|---|
| struct FloatVect3 | omega_a | |
| struct FloatVect3 | omega_n | |
| struct FloatVect3 | zeta | |
| struct PolesOrder2Vect3 |
Structure defining second-order pole parameters for vectorized 3D quantities.
This structure holds the natural frequencies and damping ratios.
Definition at line 119 of file stabilization_andi.h.
| Data Fields | ||
|---|---|---|
| struct FloatVect3 | omega_a | |
| struct FloatVect3 | zeta | |
| struct GainsOrder2Vect3 |
Structure defining second-order gain parameters for vectorized 3D quantities.
Definition at line 127 of file stabilization_andi.h.
| Data Fields | ||
|---|---|---|
| struct FloatVect3 | k1 | |
| struct FloatVect3 | k2 | |
| struct GainsOrder3Vect3 |
Structure defining third-order gain parameters for vectorized 3D quantities.
Definition at line 135 of file stabilization_andi.h.
| Data Fields | ||
|---|---|---|
| struct FloatVect3 | k1 | |
| struct FloatVect3 | k2 | |
| struct FloatVect3 | k3 | |
| #define ANDI_NUM_ACT COMMANDS_NB_REAL |
Definition at line 45 of file stabilization_andi.h.
| void evaluate_obm_f_stb_u | ( | float | fu_mat[ANDI_NUM_ACT *ANDI_OUTPUTS], |
| const struct FloatRates * | rates, | ||
| const struct FloatVect3 * | vel_body, | ||
| const float | actuator_state[ANDI_NUM_ACT] ) |
Evaluate the state-dependent control effectiveness matrix F_u for stabilization.
This function computes the mapping from actuator inputs to aerodynamic/motor outputs for the stabilization model. The output is written into the provided, pre-allocated array fu_mat of size ANDI_NUM_ACT * ANDI_OUTPUTS.
The produced matrix represents the partial derivatives of the modeled outputs with respect to actuator commands (∂f/∂u) evaluated at the current state and actuator conditions. It is intended to be implemented per-airframe (see obm_cyclone for an example).
| [out] | fu_mat | Pre-allocated array (size ANDI_NUM_ACT * ANDI_OUTPUTS) receiving the flattened control-effectiveness matrix. Convention: element for output i and actuator j should be written to fu_mat[i * ANDI_NUM_ACT + j]. |
| [in] | rates | Current angular rates (typically p, q, r) used by the model (rad/s). |
| [in] | vel_body | Body-frame linear velocity vector (u, v, w) (m/s). |
| [in] | actuator_state | Current actuator commands/deflections array (length ANDI_NUM_ACT). Units and normalization depend on the airframe and actuator type (e.g. radians, throttle fraction). |
Definition at line 319 of file obm_cyclone.c.
References actuator_state, ANDI_NUM_ACT, ANDI_OUTPUTS, ce_mat_tmp, cyclone_f_stb_u(), obm_coefficients, FloatVect3::x, FloatVect3::y, and FloatVect3::z.
Referenced by stabilization_andi_enter(), stabilization_andi_init(), and stabilization_andi_run().
| void evaluate_obm_f_stb_x | ( | float | nu_obm[ANDI_OUTPUTS], |
| const struct FloatRates * | rates, | ||
| const struct FloatVect3 * | vel_body, | ||
| const struct FloatVect3 * | ang_accel, | ||
| const struct FloatVect3 * | accel_body, | ||
| const float | actuator_state[ANDI_NUM_ACT] ) |
Evaluate the state-dependent contribution F_x * x_dot for stabilization.
Computes nu_obm = F_x * x_dot, the portion of the open-body-model outputs that depends on the current state rates/accelerations (i.e. the state-dependent term). This is the term that captures how changes in state drive the modeled outputs independent of actuator inputs.
| [out] | nu_obm | Pre-allocated output vector of length ANDI_OUTPUTS receiving the computed F_x * x_dot values. |
| [in] | rates | Current angular rates (p, q, r) used by the model (rad/s). |
| [in] | vel_body | Body-frame velocity vector (u, v, w) (m/s). |
| [in] | ang_accel | Body angular accelerations (p_dot, q_dot, r_dot) (rad/s^2). |
| [in] | accel_body | Body-frame linear accelerations (ax, ay, az) (m/s^2). |
| [in] | actuator_state | Current actuator commands/deflections array (length ANDI_NUM_ACT). Some terms of F_x may be actuator-state dependent (e.g. rotor wake effects). |
Definition at line 345 of file obm_cyclone.c.
References actuator_state, ANDI_NUM_ACT, ANDI_OUTPUTS, cyclone_f_stb_x(), nu_obm, obm_coefficients, FloatRates::p, FloatRates::q, FloatRates::r, FloatVect3::x, FloatVect3::y, and FloatVect3::z.
Referenced by stabilization_andi_run().
| struct FloatVect3 evaluate_obm_forces | ( | const struct FloatRates * | rates, |
| const struct FloatVect3 * | vel_body, | ||
| const float | actuator_state[ANDI_NUM_ACT] ) |
Evaluate total force acting on the vehicle from the OBM.
Computes the full set of forces modeled by the on board model for the given airframe as a function of the state and input.
This function is used for complementary filtering in ANDI.
| [in] | rates | Current angular rates (p, q, r) used by the model (rad/s). |
| [in] | vel_body | Body-frame linear velocity vector (u, v, w) (m/s). |
| [in] | actuator_state | Current actuator commands/deflections array (length ANDI_NUM_ACT). Units and normalization depend on the airframe and actuator type (e.g. radians, throttle fraction). |
Definition at line 273 of file obm_cyclone.c.
References actuator_state, ANDI_NUM_ACT, cyclone_obm_forces(), evaluate_obm_forces(), obm_coefficients, FloatVect3::x, FloatVect3::y, and FloatVect3::z.
Referenced by evaluate_obm_forces(), and stabilization_andi_run().
| struct FloatVect3 evaluate_obm_moments | ( | const struct FloatRates * | rates, |
| const struct FloatVect3 * | vel_body, | ||
| const float | actuator_state[ANDI_NUM_ACT] ) |
Evaluate total moments acting on the vehicle from the OBM.
Computes the full set of moments modeled by the on board model for the given airframe as a function of the state and input.
This function is used for complementary filtering in ANDI.
| [in] | rates | Current angular rates (p, q, r) used by the model (rad/s). |
| [in] | vel_body | Body-frame linear velocity vector (u, v, w) (m/s). |
| [in] | actuator_state | Current actuator commands/deflections array (length ANDI_NUM_ACT). Units and normalization depend on the airframe and actuator type (e.g. radians, throttle fraction). |
Definition at line 294 of file obm_cyclone.c.
References actuator_state, ANDI_NUM_ACT, cyclone_obm_moments(), evaluate_obm_moments(), moments, obm_coefficients, FloatVect3::x, FloatVect3::y, and FloatVect3::z.
Referenced by evaluate_obm_moments(), and stabilization_andi_run().
| float evaluate_obm_thrust_z | ( | const float | actuator_state[ANDI_NUM_ACT] | ) |
Compute total thrust produced by the current actuator state.
Returns the aggregate thrust generated by the set of actuators described by actuator_state. This value is typically used by the stabilization/obm code when converting actuator commands to net force for direct thrust control.
| [in] | actuator_state | Current actuator commands/deflections array (length ANDI_NUM_ACT). Units and normalization depend on the airframe (e.g. rotor collective, throttle fraction). |
Definition at line 375 of file obm_cyclone.c.
References actuator_state, ANDI_NUM_ACT, and obm_coefficients.
Referenced by stabilization_andi_enter(), and stabilization_andi_run().
| 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.
|
extern |
Definition at line 287 of file stabilization_andi.c.
Referenced by stabilization_andi_init(), and stabilization_andi_run().
|
extern |
Definition at line 304 of file stabilization_andi.c.
Referenced by stabilization_andi_init(), and stabilization_andi_run().
|
extern |
Definition at line 263 of file stabilization_andi.c.
Referenced by stabilization_andi_init(), and stabilization_andi_run().
|
extern |
Definition at line 275 of file stabilization_andi.c.
Referenced by stabilization_andi_init(), and stabilization_andi_run().
|
extern |
Definition at line 321 of file stabilization_andi.c.
Referenced by stabilization_andi_init(), and stabilization_andi_run().
|
extern |
Definition at line 322 of file stabilization_andi.c.
Referenced by stabilization_andi_init(), and stabilization_andi_run().