Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
oneloop_andi.h
Go to the documentation of this file.
1/*
2 * Copyright (C) 2023 Tomaso De Ponti <tmldeponti@tudelft.nl>
3 *
4 * This file is part of paparazzi
5 *
6 * paparazzi is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2, or (at your option)
9 * any later version.
10 *
11 * paparazzi is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with paparazzi; see the file COPYING. If not, see
18 * <http://www.gnu.org/licenses/>.
19 */
20
25
26#ifndef ONELOOP_ANDI_H
27#define ONELOOP_ANDI_H
28//====================================================================================================================================
29// Include the header files needed
33#include "generated/airframe.h"
36//====================================================================================================================================
37// Define macros used in the C file
38#ifndef ANDI_NUM_ACT
39#define ANDI_NUM_ACT COMMANDS_NB_REAL
40#endif
41
42#ifndef ANDI_NUM_VIRTUAL_ACT
43#define ANDI_NUM_VIRTUAL_ACT COMMANDS_NB_VIRTUAL
44#endif
45
46// Number of virtual actuators (e.g. Phi, Theta).
47// For now 2 and only 2 are supported but in the future this can be further developed.
48#if ANDI_NUM_VIRTUAL_ACT < 2
49#error "You must specify the number of virtual actuators to be at least 2"
50#define ANDI_NUM_VIRTUAL_ACT 2
51#endif
52
53#define ANDI_NUM_ACT_TOT (ANDI_NUM_ACT + ANDI_NUM_VIRTUAL_ACT)
54
55#ifndef ANDI_OUTPUTS
56#error "You must specify the number of controlled axis (outputs)"
57#define ANDI_OUTPUTS 6
58#endif
59#define ANDI_G_SCALING 1000.0f
60
62#define CTRL_ANDI 0
63#define CTRL_INDI 1
64//====================================================================================================================================
65// Declaration of global variables
66extern float k_P;
67extern float k_D;
68extern float k1_NE_tune;
69extern float k2_NE_tune;
70extern bool ctrl_off; // Turn off stabilization control with the quad motors
71extern float act_state_filt_vect_1l[ANDI_NUM_ACT]; // Filtered actuator state vector to synch with the feedback signals
72extern float actuator_state_1l[ANDI_NUM_ACT_TOT]; // Actuator state vector (including virtual actuators)
73extern float nu[6]; // Virtual control vector
74extern float g1g2_1l[ANDI_OUTPUTS][ANDI_NUM_ACT_TOT]; // Control effectiveness matrix
75extern float andi_u[ANDI_NUM_ACT_TOT]; // Control command vector
76extern float andi_du[ANDI_NUM_ACT_TOT]; // Control command increment vector
77extern float psi_des_deg; // Desired heading [deg]
78extern bool heading_manual; // Specify the heading with a slider
79extern bool yaw_stick_in_auto; // Adjust the heading in auto mode with the yaw stick
80extern float fwd_sideslip_gain; // Forward sideslip gain
81extern struct FloatEulers eulers_zxy_des; // Desired ZXY Euler angles
82extern float psi_des_rad; // Desired heading [rad]
83extern float max_as; // Maximum airspeed [m/s]
84extern float gi_unbounded_airspeed_sp; // Unbounded airspeed setpoint [m/s] (mimics guidance_indi_hybrid)
85extern float oneloop_andi_sigma; // Bandwidth of replacement actuator dynamics [rad/s]
86extern bool use_dyn_filter; // Replace actuator dynamics with a dynamic filter INDI-LL
87extern bool chirp_on; // Turn on chirp
88extern bool multiple_chirp_run; // Boolean that logs if we are doing multiple automatic chirps
89extern float f0_chirp; // Start frequency of the chirp [Hz]
90extern float f1_chirp; // End frequency of the chirp [Hz]
91extern float t_chirp; // Duration of the chirp [s]
92extern float A_chirp; // Amplitude of the chirp
93extern int8_t chirp_axis; // Axis to apply the chirp
94extern bool drop_yaw; // Drop yaw control
95//====================================================================================================================================
96// Delete once hybrid nav is fixed
98 float pos_gain;
99 float pos_gainz;
100 float speed_gain;
101 float speed_gainz;
102 float heading_bank_gain;
103 float liftd_asq;
104 float liftd_p80;
105 float liftd_p50;
106};
108//====================================================================================================================================
109// Declaration of the oneloop struct
111 float pos[3];
112 float vel[3];
113 float acc[3];
114 float jer[3];
115};
117 float pos[3];
118 float vel[3];
119 float acc[3];
120};
122 float att[3];
123 float att_d[3];
124 float att_2d[3];
125 float att_3d[3];
126};
128 float att[3];
129 float att_d[3];
130 float att_2d[3];
131};
135 struct OneloopGuidanceRef gui_ref; // Guidance References
136 struct OneloopGuidanceState gui_state; // Guidance State
137 struct OneloopStabilizationRef sta_ref; // Stabilization References
138 struct OneloopStabilizationState sta_state; // Stabilization State
139};
140extern struct OneloopGeneral oneloop_andi;
141//====================================================================================================================================
142// Declaration of the Controller gains/poles structs
144 float omega_n;
145 float zeta;
146 float p3;
147};
149 float k1[3];
150 float k2[3];
151 float k3[3];
152};
154 float k2;
155 float k3;
156};
157/*Declaration of Reference Model and Error Controller Gains*/
158extern struct PolePlacement p_att_e;
159extern struct PolePlacement p_att_rm;
160/*Position Loop*/
161extern struct PolePlacement p_pos_e;
162extern struct PolePlacement p_pos_rm;
163/*Altitude Loop*/
164extern struct PolePlacement p_alt_e;
165extern struct PolePlacement p_alt_rm;
166/*Heading Loop*/
167extern struct PolePlacement p_head_e;
168extern struct PolePlacement p_head_rm;
169/*Gains of EC and RM*/
170extern struct Gains3rdOrder k_att_e;
171extern struct Gains3rdOrder k_att_rm;
172extern struct Gains2ndOrder k_head_e;
173extern struct Gains2ndOrder k_head_rm;
174extern struct Gains3rdOrder k_pos_e;
175extern struct Gains3rdOrder k_pos_rm;
176//====================================================================================================================================
177// Declaration of filtering structs
178enum FilterType { // enumeration of filter types
179 LOWPASS_1, // First order low pass
180 BUTTERWORTH_2, // Second order Butterworth
181 BUTTERWORTH_4, // Fourth order Butterworth
182 NOTCH // Notch filter
183};
184struct LP_t { // Low pass filter structure
185 float tau; // Time constant [s]
186 float freq; // Cut-off frequency (currently used) [Hz]
187 float freq_set; // Desired cut-off frequency (to be set) [Hz]
188 float bandwidth; // Bandwidth of notch [Hz]
189 float meas; // Current measurement
190 float meas_prev; // Previous measurement
191 float out; // Filter output
192 enum FilterType filter_type; // Type of filter used for the structural modes
193 union {
194 struct FirstOrderLowPass lp1; // First order low pass filter struct
195 Butterworth2LowPass bw2; // Second order Butterworth filter struct
196 Butterworth4LowPass bw4; // Fourth order Butterworth filter struct
197 struct SecondOrderNotchFilter notch; // Second order notch filter struct
198 } meas_filt; // Union of different filter types
199};
200struct Oneloop_LP_t { // Struct containing all feedback low pass filters
201 struct LP_t p; // Roll rate filter
202 struct LP_t q; // Pitch rate filter
203 struct LP_t r; // Yaw rate filter
204 struct LP_t p_dot; // Roll acceleration filter
205 struct LP_t q_dot; // Pitch acceleration filter
206 struct LP_t r_dot; // Yaw acceleration filter
207 struct LP_t ax; // X acceleration filter
208 struct LP_t ay; // Y acceleration filter
209 struct LP_t az; // Z acceleration filter
210};
211struct Oneloop_DynFilt_t { // Dynamics Cancelling filter
212 float fs; // Sampling frequency [Hz]
213 float sigma; // Bandwidth of new dynamics [rad/s]
214 float varepsilon; // Bandwidth of the original dynamics [rad/s]
215 float u_c; // Current command input
216 float u_c_0; // Previous command input
217 float mu_c_0; // Previous filtered command output
218 float mu_c; // Current filtered command output
219};
220extern struct Oneloop_LP_t LP; // Instance of the struct containing all feedback low pass filters
221//====================================================================================================================================
222// Declaration of functions
223extern void oneloop_andi_init(void);
224extern void oneloop_andi_enter(bool half_loop_sp, int ctrl_type);
226extern void oneloop_andi_run(bool in_flight, bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v);
227extern void oneloop_andi_RM(bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v, bool in_flight_oneloop);
228extern void oneloop_andi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn);
229extern void oneloop_from_nav(bool in_flight);
230extern void guidance_set_min_max_airspeed(float min_airspeed, float max_airspeed);
231// Guidance limit setters (called by rotwing_state)
232extern void guidance_set_max_bank_angle(float max_bank);
233extern void guidance_set_max_climb_speed(float max_climb_speed_quad, float max_climb_speed_fwd);
234extern void guidance_set_max_descend_speed(float max_descend_speed_quad, float max_descend_speed_fwd);
235
236#endif // ONELOOP_ANDI_H
euler angles
int32_t max_airspeed
struct guidance_indi_hybrid_params gih_params
float gi_unbounded_airspeed_sp
Simple first order low pass filter with bilinear transform.
struct SecondOrderLowPass Butterworth2LowPass
Second order Butterworth low pass filter.
Fourth order Butterworth low pass filter.
First order low pass filter structure.
struct PolePlacement p_pos_e
bool use_dyn_filter
float k2_NE_tune
float k_D
struct PolePlacement p_head_rm
struct Gains3rdOrder k_att_rm
struct FloatEulers eulers_zxy_des
struct PolePlacement p_att_rm
float max_as
struct Gains3rdOrder k_att_e
float psi_des_rad
bool ctrl_off
struct Gains3rdOrder k_pos_e
float nu[ANDI_OUTPUTS]
float t_chirp
bool drop_yaw
float k_P
float f0_chirp
struct PolePlacement p_alt_rm
float oneloop_andi_sigma
bool chirp_on
float andi_u[ANDI_NUM_ACT_TOT]
float fwd_sideslip_gain
float f1_chirp
struct PolePlacement p_att_e
struct PolePlacement p_alt_e
float A_chirp
struct PolePlacement p_head_e
int8_t chirp_axis
float andi_du[ANDI_NUM_ACT_TOT]
bool heading_manual
float k1_NE_tune
struct OneloopGeneral oneloop_andi
struct Gains3rdOrder k_pos_rm
float psi_des_deg
struct PolePlacement p_pos_rm
bool yaw_stick_in_auto
float actuator_state_1l[ANDI_NUM_ACT_TOT]
bool multiple_chirp_run
struct Oneloop_LP_t LP
void oneloop_andi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
void oneloop_andi_RM(bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v, bool in_flight_oneloop)
Function to generate the reference signals for the oneloop controller.
void oneloop_andi_enter(bool half_loop_sp, int ctrl_type)
Function that resets important values upon engaging Oneloop ANDI.
float g1g2_1l[ANDI_OUTPUTS][ANDI_NUM_ACT_TOT]
void guidance_set_max_bank_angle(float max_bank)
Guidance limit setters called by rotwing_state (also implemented by guidance_indi_hybrid).
void guidance_set_max_climb_speed(float max_climb_speed_quad, float max_climb_speed_fwd)
struct Gains2ndOrder k_head_rm
#define ANDI_NUM_ACT
void oneloop_from_nav(bool in_flight)
Function that maps navigation inputs to the oneloop controller for the generated autopilot.
struct Gains2ndOrder k_head_e
void oneloop_andi_set_failsafe_setpoint(void)
void guidance_set_min_max_airspeed(float min_airspeed, float max_airspeed)
FilterType
@ BUTTERWORTH_2
@ BUTTERWORTH_4
@ LOWPASS_1
@ NOTCH
void oneloop_andi_run(bool in_flight, bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v)
Main function that runs the controller and performs control allocation.
#define ANDI_NUM_ACT_TOT
void guidance_set_max_descend_speed(float max_descend_speed_quad, float max_descend_speed_fwd)
void oneloop_andi_init(void)
Init function of Oneloop ANDI controller.
#define ANDI_OUTPUTS
float act_state_filt_vect_1l[ANDI_NUM_ACT]
float max_bank
Definition oneloop_nB.c:264
struct OneloopGuidanceState gui_state
struct OneloopStabilizationState sta_state
float meas
float bandwidth
float meas_prev
union LP_t::@324202153354356061211053020005360346147215120230 meas_filt
float tau
struct OneloopGuidanceRef gui_ref
struct LP_t ax
struct OneloopStabilizationRef sta_ref
FilterType
Definition oneloop_nB.h:252
float freq
struct LP_t r_dot
struct LP_t p_dot
float freq_set
struct LP_t p
float out
struct LP_t az
struct LP_t q
enum FilterType filter_type
struct LP_t ay
struct LP_t q_dot
struct LP_t r
General stabilization interface for rotorcrafts.
Common data structures shared by euler and quaternion int implementations.
Rotorcraft attitude reference generation.
signed char int8_t
Typedef defining 8 bit char type.