Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
eff_scheduling_rotwing_V2.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2023 Tomaso De Ponti <T.M.L.DePonti@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
27#include "generated/airframe.h"
28#include "state.h"
30#include "modules/core/abi.h"
33
34#define FORCE_ONELOOP
35#ifdef FORCE_ONELOOP
38#else
40#endif
41
42#ifndef ROTWING_EFF_SCHED_IXX_BODY
43#error "NO ROTWING_EFF_SCHED_IXX_BODY defined"
44#endif
45
46#ifndef ROTWING_EFF_SCHED_IYY_BODY
47#error "NO ROTWING_EFF_SCHED_IYY_BODY defined"
48#endif
49
50#ifndef ROTWING_EFF_SCHED_IZZ
51#error "NO ROTWING_EFF_SCHED_IZZ defined"
52#endif
53
54#ifndef ROTWING_EFF_SCHED_IXX_WING
55#error "NO ROTWING_EFF_SCHED_IXX_WING defined"
56#endif
57
58#ifndef ROTWING_EFF_SCHED_IYY_WING
59#error "NO ROTWING_EFF_SCHED_IYY_WING defined"
60#endif
61
62#ifndef ROTWING_EFF_SCHED_M
63#error "NO ROTWING_EFF_SCHED_M defined"
64#endif
65
66/* Effectiveness Matrix definition */
67float G2_RW[EFF_MAT_COLS_NB] = {0};//ROTWING_EFF_SCHED_G2; //scaled by RW_G_SCALE
68float G1_RW[EFF_MAT_ROWS_NB][EFF_MAT_COLS_NB] = {0};//{ROTWING_EFF_SCHED_G1_ZERO, ROTWING_EFF_SCHED_G1_ZERO, ROTWING_EFF_SCHED_G1_THRUST, ROTWING_EFF_SCHED_G1_ROLL, ROTWING_EFF_SCHED_G1_PITCH, ROTWING_EFF_SCHED_G1_YAW}; //scaled by RW_G_SCALE
70static float flt_cut_ap = 2.0e-3;
71static float flt_cut = 1.0e-4;
72
75/* Temp variables*/
76bool airspeed_fake_on = false;
77float airspeed_fake = 0.0;
78float ele_eff = 19.36; // (0.88*22.0);
79float roll_eff = 5.5 ;//3.835;
80float yaw_eff = 0.390;
81float ele_min = 0.0;
82/* Define Forces and Moments tructs for each actuator*/
83struct RW_Model RW;
84
87void ele_pref_sched(void);
88void update_attitude(void);
89void sum_EFF_MAT_RW(void);
90void init_RW_Model(void);
91void calc_G1_G2_RW(void);
92
95#ifndef WING_ROTATION_CAN_ROTWING_ID
96#define WING_ROTATION_CAN_ROTWING_ID ABI_BROADCAST
97#endif
100
101float skew_meas = 0.0;
103{
104 for (int i=0; i<num_act; i++){
105 if (pos_msg[i].set.position && (pos_msg[i].idx == SERVO_ROTATION_MECH_IDX))
106 {
107 skew_meas = 0.5 * M_PI - pos_msg[i].position;
108 Bound(skew_meas, 0, 0.5 * M_PI);
109 }
110 }
111}
112
113#include "generated/modules.h"
124
126{
127 // Inertia and mass
128 RW.I.b_xx = 0.12879; // [kgm²] (0.0478 + 0.08099)
129 RW.I.b_yy = 0.94950; // [kgm²] (0.7546 + 0.1949)
130 RW.I.w_xx = 0.0; // [kgm²]
131 RW.I.w_yy = 0.0; // [kgm²]
132 RW.I.xx = RW.I.b_xx + RW.I.w_xx; // [kgm²]
133 RW.I.yy = RW.I.b_yy + RW.I.b_yy; // [kgm²]
134 RW.I.zz = 0.975; // [kgm²]
135 RW.m = 6.670; // [kg]
136 // Motor Front
137 RW.mF.dFdu = 3.835 / RW_G_SCALE; // [N / pprz]
138 RW.mF.dMdu = yaw_eff / RW_G_SCALE; // [Nm / pprz]
139 RW.mF.dMdud = 0.020 / RW_G_SCALE; // [Nm / pprz]
140 RW.mF.l = 0.423 ; // [m] 435
141 // Motor Right
142 RW.mR.dFdu = roll_eff / RW_G_SCALE; // [N / pprz]
143 RW.mR.dMdu = yaw_eff / RW_G_SCALE; // [Nm / pprz]
144 RW.mR.dMdud = 0.020 / RW_G_SCALE; // [Nm / pprz]
145 RW.mR.l = 0.408 ; // [m] 375
146 // Motor Back
147 RW.mB.dFdu = 3.835 / RW_G_SCALE; // [N / pprz]
148 RW.mB.dMdu = yaw_eff / RW_G_SCALE; // [Nm / pprz]
149 RW.mB.dMdud = 0.020 / RW_G_SCALE; // [Nm / pprz]
150 RW.mB.l = 0.423 ; // [m]
151 // Motor Left
152 RW.mL.dFdu = roll_eff / RW_G_SCALE; // [N / pprz]
153 RW.mL.dMdu = yaw_eff / RW_G_SCALE; // [Nm / pprz]
154 RW.mL.dMdud = 0.020 / RW_G_SCALE; // [Nm / pprz]
155 RW.mL.l = 0.408 ; // [m]
156 // Motor Pusher
157 RW.mP.dFdu = 3.468 / RW_G_SCALE; // [N / pprz]
158 RW.mP.dMdu = 0.000 / RW_G_SCALE; // [Nm / pprz]
159 RW.mP.dMdud = 0.000 / RW_G_SCALE; // [Nm / pprz]
160 RW.mP.l = 0.000 ; // [m]
161 // Elevator
162 RW.ele.dFdu = ele_eff / (RW_G_SCALE * RW_G_SCALE); // [N / pprz] old value: 24.81 29.70
163 RW.ele.dMdu = 0; // [Nm / pprz]
164 RW.ele.dMdud = 0; // [Nm / pprz]
165 RW.ele.l = 0.85; // [m]
166 RW.ele_pref = 0;
167 // Rudder
168 RW.rud.dFdu = 1.207 / (RW_G_SCALE * RW_G_SCALE); // [N / pprz]
169 RW.rud.dMdu = 0; // [Nm / pprz]
170 RW.rud.dMdud = 0; // [Nm / pprz]
171 RW.rud.l = 0.88; // [m]
172 // Aileron
173 RW.ail.dFdu = 7.678 / (RW_G_SCALE * RW_G_SCALE); // [N / pprz] (1.88 * 4.084)
174 RW.ail.dMdu = 0; // [Nm / pprz]
175 RW.ail.dMdud = 0; // [Nm / pprz]
176 RW.ail.l = 0.68; // [m]
177 // Flaperon
178 RW.flp.dFdu = 10.825 / (RW_G_SCALE * RW_G_SCALE); // [N / pprz] (1.88 * 5.758)
179 RW.flp.dMdu = 0; // [Nm / pprz]
180 RW.flp.dMdud = 0; // [Nm / pprz]
181 RW.flp.l = 0.36; // [m]
182 // Lift: k0 the v²+ k1 the sin² v² + k2 sin² v²
183 RW.wing.k0 = 0.336 + 0.0507 + 0.102;
184 RW.wing.k1 = 0.616;
185 RW.wing.k2 = 0.121;
186 // Initialize attitude
187 RW.att.phi = 0.0;
188 RW.att.theta = 0.0;
189 RW.att.psi = 0.0;
190 RW.att.sphi = 0.0;
191 RW.att.cphi = 0.0;
192 RW.att.stheta = 0.0;
193 RW.att.ctheta = 0.0;
194 RW.att.spsi = 0.0;
195 RW.att.cpsi = 0.0;
196 // Initialize skew variables to quad values
197 RW.skew.rad = 0.0; // ABI input
198 RW.skew.deg = 0.0;
199 RW.skew.cosr = 1.0;
200 RW.skew.sinr = 0.0;
201 RW.skew.cosr2 = 1.0;
202 RW.skew.sinr2 = 0.0;
203 RW.skew.sinr3 = 0.0;
204 // Initialize airspeed
205 RW.as = 0.0;
206 RW.as2 = 0.0;
207}
208
209/*Update the attitude*/
223/* Function to precalculate once some constant effectiveness values to improve efficiency*/
225{
226 // Inertia
227 RW.I.xx = RW.I.b_xx + RW.skew.cosr2 * RW.I.w_xx + RW.skew.sinr2 * RW.I.w_yy;
228 RW.I.yy = RW.I.b_yy + RW.skew.sinr2 * RW.I.w_xx + RW.skew.cosr2 * RW.I.w_yy;
229 Bound(RW.I.xx, 0.01, 100.);
230 Bound(RW.I.yy, 0.01, 100.);
231 // Motor Front
236 // Motor Right
242 // Motor Back
247 // Motor Left
253 // Motor Pusher
255 // Elevator
258 // Rudder
260 // Aileron
263
264 // Flaperon
267 // Lift and thrust
269 Bound(RW.wing.dLdtheta, 0.0, 1300.0);
271 Bound(RW.wing.L, 0.0, 350.0);
273 Bound(RW.T, 0.0, 180.0);
275}
276
286
287
294void sum_EFF_MAT_RW(void) {
295
296 // Thrust and Pusher force estimation
297 float L = RW.wing.L / RW.m; // Lift specific force
298 float dLdtheta = RW.wing.dLdtheta / RW.m; // Lift specific force derivative with pitch
299 float T = RW.T / RW.m; // Thrust specific force. Minus gravity is a guesstimate.
300 float P = RW.P / RW.m; // Pusher specific force
301 float T_L = T + L * RW.att.ctheta; // Thrust and Lift term
302 float P_L = P + L * RW.att.stheta; // Pusher and Lift term
303 int i = 0;
304 int j = 0;
305
306 for (i = 0; i < EFF_MAT_COLS_NB; i++) {
307 switch (i) {
308 case (COMMAND_MOTOR_FRONT):
309 case (COMMAND_MOTOR_BACK):
310 case (COMMAND_MOTOR_RIGHT):
311 case (COMMAND_MOTOR_LEFT):
314 EFF_MAT_RW[RW_aD][i] = (RW.att.cphi * RW.att.ctheta ) * G1_RW[RW_aZ][i];
315 EFF_MAT_RW[RW_ap][i] = (G1_RW[RW_ap][i]) ;
316 EFF_MAT_RW[RW_aq][i] = (G1_RW[RW_aq][i]) ;
317 EFF_MAT_RW[RW_ar][i] = (G1_RW[RW_ar][i] + G2_RW[i]) ;
318 break;
319 case (COMMAND_MOTOR_PUSHER):
322 EFF_MAT_RW[RW_aD][i] = (- RW.att.cphi * RW.att.stheta ) * G1_RW[RW_aX][i];
323 EFF_MAT_RW[RW_ap][i] = 0.0;
324 EFF_MAT_RW[RW_aq][i] = 0.0;
325 EFF_MAT_RW[RW_ar][i] = 0.0;
326 break;
327 case (COMMAND_ELEVATOR):
328 case (COMMAND_RUDDER):
329 case (COMMAND_AILERONS):
330 case (COMMAND_FLAPS):
331 EFF_MAT_RW[RW_aN][i] = 0.0;
332 EFF_MAT_RW[RW_aE][i] = 0.0;
333 EFF_MAT_RW[RW_aD][i] = 0.0;
334 EFF_MAT_RW[RW_ap][i] = G1_RW[RW_ap][i];
335 EFF_MAT_RW[RW_aq][i] = G1_RW[RW_aq][i];
336 EFF_MAT_RW[RW_ar][i] = G1_RW[RW_ar][i];
337 break;
338 case (COMMAND_ROLL):
342 EFF_MAT_RW[RW_ap][i] = 0.0;
343 EFF_MAT_RW[RW_aq][i] = 0.0;
344 EFF_MAT_RW[RW_ar][i] = 0.0;
345 break;
346 case (COMMAND_PITCH):
347 EFF_MAT_RW[RW_aN][i] = (-(RW.att.ctheta * RW.att.cpsi - RW.att.sphi * RW.att.stheta * RW.att.spsi) * T - (RW.att.cpsi * RW.att.stheta + RW.att.ctheta * RW.att.sphi * RW.att.spsi) * P - RW.att.sphi * RW.att.spsi * dLdtheta);
348 EFF_MAT_RW[RW_aE][i] = (-(RW.att.ctheta * RW.att.spsi + RW.att.sphi * RW.att.stheta * RW.att.cpsi) * T - (RW.att.spsi * RW.att.stheta - RW.att.cpsi * RW.att.ctheta * RW.att.sphi) * P + RW.att.sphi * RW.att.cpsi * dLdtheta);
349 EFF_MAT_RW[RW_aD][i] = ( RW.att.stheta * RW.att.cphi * T - RW.att.cphi * RW.att.ctheta * P - RW.att.cphi * dLdtheta) ;
350 EFF_MAT_RW[RW_ap][i] = 0.0;
351 EFF_MAT_RW[RW_aq][i] = 0.0;
352 EFF_MAT_RW[RW_ar][i] = 0.0;
353 break;
354 default:
355 break;
356 }
357 }
358 for (i = 0; i < EFF_MAT_ROWS_NB; i++) {
359 for(j = 0; j < EFF_MAT_COLS_NB; j++) {
360 float abs = fabs(EFF_MAT_RW[i][j]);
361 switch (i) {
362 case (RW_aN):
363 case (RW_aE):
364 case (RW_aD):
365 case (RW_aq):
366 case (RW_ar):
367 if (abs < flt_cut) {
368 EFF_MAT_RW[i][j] = 0.0;
369 }
370 break;
371 case (RW_ap):
372 if (abs < flt_cut_ap) {
373 EFF_MAT_RW[i][j] = 0.0;
374 }
375 break;
376 }
377 }
378 }
379}
380
382{
383 // Calculate sin and cosines of rotation
385 RW.skew.rad = skew_filt.o[0];
386 RW.skew.deg = RW.skew.rad / M_PI * 180.;
393#ifdef INS_EXT_VISION_ROTATION
394 // Define an INS external pose quaternion rotation from the wing rotation angle
395 struct FloatEulers rot_e = {0, 0, RW.skew.rad};
397#endif
398
399}
400float time = 0.0;
402{
404 Bound(RW.as, 0. , 30.);
405 RW.as2 = RW.as * RW.as;
406 Bound(RW.as2, 0. , 900.);
407 if(airspeed_fake_on) {
408 //float freq = 0.5;
409 //float amp = 3.0;
410 time = time + 0.002;
411 RW.as = airspeed_fake ;//+ amp * sinf(2.0 * M_PI * freq * time);
412 RW.as2 = RW.as * RW.as;
413 } else {
414 time = 0.0;
415 }
416}
417
419{
420 if (RW.as > ELE_MIN_AS){
423 } else {
425 }
426}
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
Definition abi_common.h:67
#define UNUSED(x)
void update_attitude(void)
float G2_RW[EFF_MAT_COLS_NB]
void calc_G1_G2_RW(void)
float G1_RW[EFF_MAT_ROWS_NB][EFF_MAT_COLS_NB]
float airspeed_fake
struct FloatEulers eulers_zxy_RW_EFF
static float flt_cut_ap
struct RW_Model RW
#define WING_ROTATION_CAN_ROTWING_ID
ABI binding wing position data.
float actuator_state_filt_vect[EFF_MAT_COLS_NB]
void eff_scheduling_rotwing_update_airspeed(void)
float EFF_MAT_RW[EFF_MAT_ROWS_NB][EFF_MAT_COLS_NB]
static Butterworth2LowPass skew_filt
void ele_pref_sched(void)
void eff_scheduling_rotwing_update_wing_angle(void)
static abi_event wing_position_ev
void eff_scheduling_rotwing_init(void)
void sum_EFF_MAT_RW(void)
Function that sums g1 and g2 to obtain the g1_g2 matrix.
bool airspeed_fake_on
void init_RW_Model(void)
static float flt_cut
void eff_scheduling_rotwing_periodic(void)
static void wing_position_cb(uint8_t sender_id UNUSED, struct act_feedback_t *pos_msg, uint8_t num_act)
#define RW_G_SCALE
struct wing_model wing
#define RW_aN
struct F_M_Body ele
#define RW_aE
struct F_M_Body mP
#define RW_aZ
struct F_M_Body mL
#define ELE_MAX_AS
#define RW_aX
#define EFF_MAT_COLS_NB
#define RW_ap
#define ELE_MIN_AS
#define RW_ar
#define ZERO_ELE_PPRZ
struct F_M_Body mR
struct F_M_Body mF
struct RW_attitude att
#define EFF_MAT_ROWS_NB
struct F_M_Body rud
struct RW_skew skew
#define RW_aq
#define RW_aD
struct F_M_Body flp
struct F_M_Body mB
struct F_M_Body ail
float phi
in radians
float theta
in radians
float psi
in radians
void float_eulers_of_quat_zxy(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch
void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e)
quat of euler roation 'ZYX'
euler angles
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition state.h:1294
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition state.h:1590
Integrated Navigation System interface.
Simple first order low pass filter with bilinear transform.
float o[2]
output history
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, float tau, float sample_time, float value)
Init a second order Butterworth filter.
static float update_butterworth_2_low_pass(Butterworth2LowPass *filter, float value)
Update second order Butterworth low pass filter state with a new value.
Second order low pass filter structure.
uint16_t foo
Definition main_demo5.c:58
Hardware independent API for actuators (servos, motor controllers).
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
float actuator_state_1l[ANDI_NUM_ACT]
API to get/set the generic vehicle states.
static float P[3][3]
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.