Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 
26 #ifndef ONELOOP_ANDI_H
27 #define ONELOOP_ANDI_H
28 
32 #include "generated/airframe.h"
33 
34 #ifndef ANDI_NUM_ACT
35 #define ANDI_NUM_ACT COMMANDS_NB_REAL
36 #endif
37 
38 #ifndef ANDI_NUM_VIRTUAL_ACT
39 #define ANDI_NUM_VIRTUAL_ACT COMMANDS_NB_VIRTUAL
40 #endif
41 
42 // Number of virtual actuators (e.g. Phi, Theta). For now 2 and only 2 are supported but in the future this can be further developed.
43 #if ANDI_NUM_VIRTUAL_ACT < 2
44 #error "You must specify the number of virtual actuators to be at least 2"
45 #define ANDI_NUM_VIRTUAL_ACT 2
46 #endif
47 
48 #define ANDI_NUM_ACT_TOT (ANDI_NUM_ACT + ANDI_NUM_VIRTUAL_ACT)
49 
50 #ifndef ANDI_OUTPUTS
51 #error "You must specify the number of controlled axis (outputs)"
52 #define ANDI_OUTPUTS 6
53 #endif
54 #define ANDI_G_SCALING 1000.0f
55 
57 #define CTRL_ANDI 0
58 #define CTRL_INDI 1
59 
60 extern bool ctrl_off;
62 extern float actuator_state_1l[ANDI_NUM_ACT];
63 extern float nu[6];
65 extern float andi_u[ANDI_NUM_ACT_TOT];
66 extern float andi_du[ANDI_NUM_ACT_TOT];
67 extern float psi_des_deg;
68 extern bool heading_manual;
69 extern bool yaw_stick_in_auto;
70 extern float fwd_sideslip_gain;
71 extern struct FloatEulers eulers_zxy_des;
72 extern float psi_des_rad;
73 extern float k_as;
74 extern float max_as;
75 
76 /*Chirp test Variables*/
77 extern bool chirp_on;
78 extern float f0_chirp;
79 extern float f1_chirp;
80 extern float t_chirp;
81 extern float A_chirp;
82 extern int8_t chirp_axis;
83 
84 // Delete once hybrid nav is fixed //////////////////////////////////////////////////////////////////////////////////
86  float pos_gain;
87  float pos_gainz;
88  float speed_gain;
89  float speed_gainz;
90  float heading_bank_gain;
91  float liftd_asq;
92  float liftd_p80;
93  float liftd_p50;
94 };
96 //extern bool force_forward;
99  float pos[3];
100  float vel[3];
101  float acc[3];
102  float jer[3];
103 };
104 
106  float pos[3];
107  float vel[3];
108  float acc[3];
109 };
110 
112  float att[3];
113  float att_d[3];
114  float att_2d[3];
115  float att_3d[3];
116 };
117 
119  float att[3];
120  float att_d[3];
121  float att_2d[3];
122 };
124  bool half_loop;
126  struct OneloopGuidanceRef gui_ref; // Guidance References
127  struct OneloopGuidanceState gui_state; // Guidance State
128  struct OneloopStabilizationRef sta_ref; // Stabilization References
129  struct OneloopStabilizationState sta_state; // Stabilization State
130 
131 };
132 
133 extern struct OneloopGeneral oneloop_andi;
134 
136  float omega_n;
137  float zeta;
138  float p3;
139 };
141  float k1[3];
142  float k2[3];
143  float k3[3];
144 };
146  float k2;
147  float k3;
148 };
149 
150 extern int16_t temp_pitch;
151 /*Declaration of Reference Model and Error Controller Gains*/
152 extern struct PolePlacement p_att_e;
153 extern struct PolePlacement p_att_rm;
154 /*Position Loop*/
155 extern struct PolePlacement p_pos_e;
156 extern struct PolePlacement p_pos_rm;
157 /*Altitude Loop*/
158 extern struct PolePlacement p_alt_e;
159 extern struct PolePlacement p_alt_rm;
160 /*Heading Loop*/
161 extern struct PolePlacement p_head_e;
162 extern struct PolePlacement p_head_rm;
163 /*Gains of EC and RM*/
164 extern struct Gains3rdOrder k_att_e;
165 extern struct Gains3rdOrder k_att_rm;
166 extern struct Gains2ndOrder k_head_e;
167 extern struct Gains2ndOrder k_head_rm;
168 extern struct Gains3rdOrder k_pos_e;
169 extern struct Gains3rdOrder k_pos_rm;
170 extern void oneloop_andi_init(void);
171 extern void oneloop_andi_enter(bool half_loop_sp, int ctrl_type);
173 extern void oneloop_andi_run(bool in_flight, bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v);
174 extern void oneloop_andi_RM(bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v, bool in_flight_oneloop);
175 extern void oneloop_andi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn);
176 extern void oneloop_from_nav(bool in_flight);
177 #endif // ONELOOP_ANDI_H
euler angles
void oneloop_andi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
struct PolePlacement p_pos_e
Definition: oneloop_andi.c:412
struct OneloopGuidanceState gui_state
Definition: oneloop_andi.h:127
struct OneloopStabilizationState sta_state
Definition: oneloop_andi.h:129
struct PolePlacement p_head_rm
Definition: oneloop_andi.c:419
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.
struct Gains3rdOrder k_att_rm
Definition: oneloop_andi.c:422
struct guidance_indi_hybrid_params gih_params
struct FloatEulers eulers_zxy_des
Definition: oneloop_andi.c:340
struct PolePlacement p_att_rm
Definition: oneloop_andi.c:410
float g1g2_1l[ANDI_OUTPUTS][ANDI_NUM_ACT_TOT]
int16_t temp_pitch
Definition: oneloop_andi.c:324
float max_as
Definition: oneloop_andi.c:269
struct Gains3rdOrder k_att_e
Definition: oneloop_andi.c:421
float psi_des_rad
Definition: oneloop_andi.c:343
bool ctrl_off
Definition: oneloop_andi.c:357
struct Gains3rdOrder k_pos_e
Definition: oneloop_andi.c:423
struct Gains2ndOrder k_head_rm
float t_chirp
Definition: oneloop_andi.c:403
float nu[6]
Definition: oneloop_andi.c:330
#define ANDI_NUM_ACT
Definition: oneloop_andi.h:35
float f0_chirp
Definition: oneloop_andi.c:401
void oneloop_from_nav(bool in_flight)
Function that maps navigation inputs to the oneloop controller for the generated autopilot.
struct PolePlacement p_alt_rm
Definition: oneloop_andi.c:416
struct OneloopGuidanceRef gui_ref
Definition: oneloop_andi.h:126
struct Gains2ndOrder k_head_e
bool chirp_on
Definition: oneloop_andi.c:397
void oneloop_andi_set_failsafe_setpoint(void)
struct OneloopStabilizationRef sta_ref
Definition: oneloop_andi.h:128
float andi_u[ANDI_NUM_ACT_TOT]
Definition: oneloop_andi.c:327
float fwd_sideslip_gain
Definition: oneloop_andi.c:277
float f1_chirp
Definition: oneloop_andi.c:402
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.
struct PolePlacement p_att_e
Definition: oneloop_andi.c:409
struct PolePlacement p_alt_e
Definition: oneloop_andi.c:415
float A_chirp
Definition: oneloop_andi.c:404
struct PolePlacement p_head_e
Definition: oneloop_andi.c:418
int8_t chirp_axis
Definition: oneloop_andi.c:405
#define ANDI_NUM_ACT_TOT
Definition: oneloop_andi.h:48
float andi_du[ANDI_NUM_ACT_TOT]
Definition: oneloop_andi.c:328
bool heading_manual
Definition: oneloop_andi.c:350
struct OneloopGeneral oneloop_andi
Definition: oneloop_andi.c:315
struct Gains3rdOrder k_pos_rm
Definition: oneloop_andi.c:424
float psi_des_deg
Definition: oneloop_andi.c:344
void oneloop_andi_init(void)
Init function of Oneloop ANDI controller
#define ANDI_OUTPUTS
Definition: oneloop_andi.h:52
float k_as
Definition: oneloop_andi.c:323
struct PolePlacement p_pos_rm
Definition: oneloop_andi.c:413
bool yaw_stick_in_auto
Definition: oneloop_andi.c:355
float act_state_filt_vect_1l[ANDI_NUM_ACT]
float actuator_state_1l[ANDI_NUM_ACT]
Definition: oneloop_andi.c:333
General stabilization interface for rotorcrafts.
Common data structures shared by euler and quaternion int implementations.
Rotorcraft attitude reference generation.
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
signed char int8_t
Typedef defining 8 bit char type.
Definition: vl53l1_types.h:103