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 
61 extern float actuator_state_1l[ANDI_NUM_ACT];
62 extern float nu[6];
64 extern float andi_u[ANDI_NUM_ACT_TOT];
65 extern float andi_du[ANDI_NUM_ACT_TOT];
66 extern float psi_des_deg;
67 extern bool heading_manual;
68 extern bool yaw_stick_in_auto;
69 extern float fwd_sideslip_gain;
70 extern struct FloatEulers eulers_zxy_des;
71 extern float psi_des_rad;
72 
73 /*Chirp test Variables*/
74 extern bool chirp_on;
75 extern float f0_chirp;
76 extern float f1_chirp;
77 extern float t_chirp;
78 extern float A_chirp;
79 extern int8_t chirp_axis;
80 
81 // Delete once hybrid nav is fixed //////////////////////////////////////////////////////////////////////////////////
83  float pos_gain;
84  float pos_gainz;
85  float speed_gain;
86  float speed_gainz;
87  float heading_bank_gain;
88  float liftd_asq;
89  float liftd_p80;
90  float liftd_p50;
91 };
93 extern bool force_forward;
96  float pos[3];
97  float vel[3];
98  float acc[3];
99  float jer[3];
100 };
101 
103  float pos[3];
104  float vel[3];
105  float acc[3];
106 };
107 
109  float att[3];
110  float att_d[3];
111  float att_2d[3];
112  float att_3d[3];
113 };
114 
116  float att[3];
117  float att_d[3];
118  float att_2d[3];
119 };
121  bool half_loop;
123  struct OneloopGuidanceRef gui_ref; // Guidance References
124  struct OneloopGuidanceState gui_state; // Guidance State
125  struct OneloopStabilizationRef sta_ref; // Stabilization References
126  struct OneloopStabilizationState sta_state; // Stabilization State
127 
128 };
129 
130 extern struct OneloopGeneral oneloop_andi;
131 
133  float omega_n;
134  float zeta;
135  float p3;
136 };
138  float k1[3];
139  float k2[3];
140  float k3[3];
141 };
143  float k2;
144  float k3;
145 };
146 
147 /*Declaration of Reference Model and Error Controller Gains*/
148 extern struct PolePlacement p_att_e;
149 extern struct PolePlacement p_att_rm;
150 /*Position Loop*/
151 extern struct PolePlacement p_pos_e;
152 extern struct PolePlacement p_pos_rm;
153 /*Altitude Loop*/
154 extern struct PolePlacement p_alt_e;
155 extern struct PolePlacement p_alt_rm;
156 /*Heading Loop*/
157 extern struct PolePlacement p_head_e;
158 extern struct PolePlacement p_head_rm;
159 /*Gains of EC and RM*/
160 extern struct Gains3rdOrder k_att_e;
161 extern struct Gains3rdOrder k_att_rm;
162 extern struct Gains2ndOrder k_head_e;
163 extern struct Gains2ndOrder k_head_rm;
164 extern struct Gains3rdOrder k_pos_e;
165 extern struct Gains3rdOrder k_pos_rm;
166 extern void oneloop_andi_init(void);
167 extern void oneloop_andi_enter(bool half_loop_sp, int ctrl_type);
169 extern void oneloop_andi_run(bool in_flight, bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v);
170 extern void oneloop_andi_RM(bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v);
171 extern void oneloop_andi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn);
172 extern void oneloop_from_nav(bool in_flight);
173 #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:375
bool force_forward
forward flight for hybrid nav
struct OneloopGuidanceState gui_state
Definition: oneloop_andi.h:124
struct OneloopStabilizationState sta_state
Definition: oneloop_andi.h:126
struct PolePlacement p_head_rm
Definition: oneloop_andi.c:382
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:385
struct guidance_indi_hybrid_params gih_params
struct FloatEulers eulers_zxy_des
Definition: oneloop_andi.c:337
struct PolePlacement p_att_rm
Definition: oneloop_andi.c:373
float g1g2_1l[ANDI_OUTPUTS][ANDI_NUM_ACT_TOT]
Definition: oneloop_andi.c:395
struct Gains3rdOrder k_att_e
Definition: oneloop_andi.c:384
float psi_des_rad
Definition: oneloop_andi.c:340
struct Gains3rdOrder k_pos_e
Definition: oneloop_andi.c:386
struct Gains2ndOrder k_head_rm
float t_chirp
Definition: oneloop_andi.c:366
float nu[6]
Definition: oneloop_andi.c:328
#define ANDI_NUM_ACT
Definition: oneloop_andi.h:35
float f0_chirp
Definition: oneloop_andi.c:364
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:379
struct OneloopGuidanceRef gui_ref
Definition: oneloop_andi.h:123
void oneloop_andi_RM(bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v)
Function to generate the reference signals for the oneloop controller.
struct Gains2ndOrder k_head_e
bool chirp_on
Definition: oneloop_andi.c:360
void oneloop_andi_set_failsafe_setpoint(void)
struct OneloopStabilizationRef sta_ref
Definition: oneloop_andi.h:125
float andi_u[ANDI_NUM_ACT_TOT]
Definition: oneloop_andi.c:325
float fwd_sideslip_gain
Definition: oneloop_andi.c:284
float f1_chirp
Definition: oneloop_andi.c:365
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:372
struct PolePlacement p_alt_e
Definition: oneloop_andi.c:378
float A_chirp
Definition: oneloop_andi.c:367
struct PolePlacement p_head_e
Definition: oneloop_andi.c:381
int8_t chirp_axis
Definition: oneloop_andi.c:368
#define ANDI_NUM_ACT_TOT
Definition: oneloop_andi.h:48
float andi_du[ANDI_NUM_ACT_TOT]
Definition: oneloop_andi.c:326
bool heading_manual
Definition: oneloop_andi.c:343
struct OneloopGeneral oneloop_andi
Definition: oneloop_andi.c:316
struct Gains3rdOrder k_pos_rm
Definition: oneloop_andi.c:387
float psi_des_deg
Definition: oneloop_andi.c:341
void oneloop_andi_init(void)
Init function of Oneloop ANDI controller
#define ANDI_OUTPUTS
Definition: oneloop_andi.h:52
struct PolePlacement p_pos_rm
Definition: oneloop_andi.c:376
bool yaw_stick_in_auto
Definition: oneloop_andi.c:344
float act_state_filt_vect_1l[ANDI_NUM_ACT]
float actuator_state_1l[ANDI_NUM_ACT]
Definition: oneloop_andi.c:330
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.
Definition: vl53l1_types.h:103