Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
eff_scheduling_rotwing.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2023 Dennis van Wijngaarden <D.C.vanWijngaarden@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
27
28#include "generated/airframe.h"
29#include "state.h"
30
34#include "autopilot.h"
35
37#include "modules/core/abi.h"
38
39#ifndef SERVO_ROTATION_MECH_IDX
40#error ctrl_eff_sched_rotwing requires a servo named ROTATION_MECH_IDX
41#endif
42
43#ifndef ROTWING_EFF_SCHED_IXX_BODY
44#error "NO ROTWING_EFF_SCHED_IXX_BODY defined"
45#endif
46
47#ifndef ROTWING_EFF_SCHED_IYY_BODY
48#error "NO ROTWING_EFF_SCHED_IYY_BODY defined"
49#endif
50
51#ifndef ROTWING_EFF_SCHED_IZZ
52#error "NO ROTWING_EFF_SCHED_IZZ defined"
53#endif
54
55#ifndef ROTWING_EFF_SCHED_IXX_WING
56#error "NO ROTWING_EFF_SCHED_IXX_WING defined"
57#endif
58
59#ifndef ROTWING_EFF_SCHED_IYY_WING
60#error "NO ROTWING_EFF_SCHED_IYY_WING defined"
61#endif
62
63#ifndef ROTWING_EFF_SCHED_M
64#error "NO ROTWING_EFF_SCHED_M defined"
65#endif
66
67#ifndef ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_PITCH
68#error "NO ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_PITCH defined"
69#endif
70
71#ifndef ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_ROLL
72#error "NO ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_ROLL defined"
73#endif
74
75#ifndef ROTWING_EFF_SCHED_HOVER_ROLL_PITCH_COEF
76#error "NO ROTWING_EFF_SCHED_HOVER_ROLL_PITCH_COEF defined"
77#endif
78
79#ifndef ROTWING_EFF_SCHED_HOVER_ROLL_ROLL_COEF
80#error "NO ROTWING_EFF_SCHED_HOVER_ROLL_ROLL_COEF defined"
81#endif
82
83#ifndef ROTWING_EFF_SCHED_K_ELEVATOR
84#error "NO ROTWING_EFF_SCHED_K_ELEVATOR defined"
85#endif
86
87#ifndef ROTWING_EFF_SCHED_K_RUDDER
88#error "NO ROTWING_EFF_SCHED_K_RUDDER defined"
89#endif
90
91#ifndef ROTWING_EFF_SCHED_K_AILERON
92#error "NO ROTWING_EFF_SCHED_K_AILERON defined"
93#endif
94
95#ifndef ROTWING_EFF_SCHED_K_FLAPERON
96#error "NO ROTWING_EFF_SCHED_K_FLAPERON defined"
97#endif
98
99#ifndef ROTWING_EFF_SCHED_K_PUSHER
100#error "NO ROTWING_EFF_SCHED_K_PUSHER defined"
101#endif
102
103#ifndef ROTWING_EFF_SCHED_K_ELEVATOR_DEFLECTION
104#error "NO ROTWING_EFF_SCHED_K_ELEVATOR_DEFLECTION defined"
105#endif
106
107#ifndef ROTWING_EFF_SCHED_D_RUDDER_D_PPRZ
108#error "NO ROTWING_EFF_SCHED_D_RUDDER_D_PPRZ defined"
109#endif
110
111#ifndef ROTWING_EFF_SCHED_K_RPM_PPRZ_PUSHER
112#error "NO ROTWING_EFF_SCHED_K_RPM_PPRZ_PUSHER defined"
113#endif
114
115#ifndef ROTWING_EFF_SCHED_K_LIFT_WING
116#error "NO ROTWING_EFF_SCHED_K_LIFT_WING defined"
117#endif
118
119#ifndef ROTWING_EFF_SCHED_K_LIFT_FUSELAGE
120#error "NO ROTWING_EFF_SCHED_K_LIFT_FUSELAGE defined"
121#endif
122
123#ifndef ROTWING_EFF_SCHED_K_LIFT_TAIL
124#error "NO ROTWING_EFF_SCHED_K_LIFT_TAIL defined"
125#endif
126
128 .Ixx_body = ROTWING_EFF_SCHED_IXX_BODY,
129 .Iyy_body = ROTWING_EFF_SCHED_IYY_BODY,
130 .Izz = ROTWING_EFF_SCHED_IZZ,
131 .Ixx_wing = ROTWING_EFF_SCHED_IXX_WING,
132 .Iyy_wing = ROTWING_EFF_SCHED_IYY_WING,
133 .m = ROTWING_EFF_SCHED_M,
134 .DMdpprz_hover_pitch = ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_PITCH,
135 .DMdpprz_hover_roll = ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_ROLL,
136 .hover_roll_pitch_coef = ROTWING_EFF_SCHED_HOVER_ROLL_PITCH_COEF,
137 .hover_roll_roll_coef = ROTWING_EFF_SCHED_HOVER_ROLL_ROLL_COEF,
138 .k_elevator = ROTWING_EFF_SCHED_K_ELEVATOR,
139 .k_rudder = ROTWING_EFF_SCHED_K_RUDDER,
140 .k_aileron = ROTWING_EFF_SCHED_K_AILERON,
141 .k_flaperon = ROTWING_EFF_SCHED_K_FLAPERON,
142 .k_pusher = ROTWING_EFF_SCHED_K_PUSHER,
143 .k_elevator_deflection = ROTWING_EFF_SCHED_K_ELEVATOR_DEFLECTION,
144 .d_rudder_d_pprz = ROTWING_EFF_SCHED_D_RUDDER_D_PPRZ,
145 .k_rpm_pprz_pusher = ROTWING_EFF_SCHED_K_RPM_PPRZ_PUSHER,
146 .k_lift_wing = ROTWING_EFF_SCHED_K_LIFT_WING,
147 .k_lift_fuselage = ROTWING_EFF_SCHED_K_LIFT_FUSELAGE,
148 .k_lift_tail = ROTWING_EFF_SCHED_K_LIFT_TAIL
149};
150
152
153// for negative values, still should be low_lim < up_lim
154inline float bound_or_zero(float value, float low_lim, float up_lim) {
155 float output = value;
156 if (low_lim > 0.f) {
157 if (value < low_lim) {
158 output = 0.f;
159 } else if (value > up_lim) {
160 output = up_lim;
161 }
162 } else {
163 if (value > up_lim) {
164 output = 0.f;
165 } else if (value < low_lim) {
166 output = low_lim;
167 }
168 }
169 return output;
170}
171
173float roll_eff_slider = 12.f;
174
175static const float Wu_gih_original[GUIDANCE_INDI_HYBRID_U] = GUIDANCE_INDI_WLS_WU;
176
178
180inline void eff_scheduling_rotwing_update_MMOI(void);
181inline void eff_scheduling_rotwing_update_cmd(void);
190
191inline float guidance_indi_get_liftd(float pitch UNUSED, float theta UNUSED);
193inline void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle);
194
195
198#ifndef WING_ROTATION_CAN_ROTWING_ID
199#define WING_ROTATION_CAN_ROTWING_ID ABI_BROADCAST
200#endif
203
204static void wing_position_cb(uint8_t sender_id UNUSED, struct act_feedback_t *pos_msg, uint8_t num_act)
205{
206 for (int i=0; i<num_act; i++){
207 if (pos_msg[i].set.position && (pos_msg[i].idx == SERVO_ROTATION_MECH_IDX))
208 {
209 // Get wing rotation angle from sensor
210 eff_sched_var.wing_rotation_rad = 0.5 * M_PI - pos_msg[i].position;
211
212 // Bound wing rotation angle
213 Bound(eff_sched_var.wing_rotation_rad, 0, 0.5 * M_PI);
214 }
215 }
216}
217
219{
220 // Initialize variables to quad values
221 eff_sched_var.Ixx = eff_sched_p.Ixx_body + eff_sched_p.Ixx_wing;
222 eff_sched_var.Iyy = eff_sched_p.Iyy_body + eff_sched_p.Iyy_wing;
223 eff_sched_var.wing_rotation_rad = 0; // ABI input
224 eff_sched_var.wing_rotation_deg = 0;
225 eff_sched_var.cosr = 1;
226 eff_sched_var.sinr = 0;
227 eff_sched_var.cosr2 = 1;
228 eff_sched_var.sinr2 = 0;
229 eff_sched_var.sinr3 = 0;
230
231 // Set moment derivative variables
232 float hover_thrust = 6000;
233 eff_sched_var.pitch_motor_dMdpprz = (eff_sched_p.DMdpprz_hover_pitch[0] + 2*hover_thrust * eff_sched_p.DMdpprz_hover_pitch[1]) / 10000.; // Dmdpprz hover pitch for hover thrust
234 eff_sched_var.roll_motor_dMdpprz = (eff_sched_p.DMdpprz_hover_roll[0] + 2*hover_thrust * eff_sched_p.DMdpprz_hover_roll[1]) / 10000.; // Dmdpprz hover roll for hover thrust
235
236 eff_sched_var.cmd_elevator = 0;
237 eff_sched_var.cmd_pusher = 0;
238 eff_sched_var.cmd_pusher_scaled = 0;
239 eff_sched_var.cmd_T_mean_scaled = 0;
240
241 eff_sched_var.airspeed = 0;
242 eff_sched_var.airspeed2 = 0;
243
244 // Get wing angle
246}
247
266
268{
269 // Calculate sin and cosines of rotation
270 eff_sched_var.wing_rotation_deg = eff_sched_var.wing_rotation_rad / M_PI * 180.;
271
272 eff_sched_var.cosr = cosf(eff_sched_var.wing_rotation_rad);
273 eff_sched_var.sinr = sinf(eff_sched_var.wing_rotation_rad);
274
275 eff_sched_var.cosr2 = eff_sched_var.cosr * eff_sched_var.cosr;
276 eff_sched_var.sinr2 = eff_sched_var.sinr * eff_sched_var.sinr;
277
278 eff_sched_var.sinr3 = eff_sched_var.sinr2 * eff_sched_var.sinr;
279
280}
281
283{
284 eff_sched_var.Ixx = eff_sched_p.Ixx_body + eff_sched_var.cosr2 * eff_sched_p.Ixx_wing + eff_sched_var.sinr2 * eff_sched_p.Iyy_wing;
285 eff_sched_var.Iyy = eff_sched_p.Iyy_body + eff_sched_var.sinr2 * eff_sched_p.Ixx_wing + eff_sched_var.cosr2 * eff_sched_p.Iyy_wing;
286
287 // Bound inertia
288 Bound(eff_sched_var.Ixx, 0.01, 100.);
289 Bound(eff_sched_var.Iyy, 0.01, 100.);
290}
291
293{
294 // These indexes depend on the INDI sequence, not the actuator IDX
295 eff_sched_var.cmd_elevator = actuator_state_filt_vect[5];
297 eff_sched_var.cmd_pusher_scaled = actuator_state_filt_vect[8] * 0.000853229; // Scaled with 8181 / 9600 / 1000
298 eff_sched_var.cmd_T_mean_scaled = (actuator_state_filt_vect[0] + actuator_state_filt_vect[1] + actuator_state_filt_vect[2] + actuator_state_filt_vect[3]) / 4. * 0.000853229; // Scaled with 8181 / 9600 / 1000
299}
300
302{
304 Bound(eff_sched_var.airspeed, 0. , 30.);
305 eff_sched_var.airspeed2 = eff_sched_var.airspeed * eff_sched_var.airspeed;
306 Bound(eff_sched_var.airspeed2, 0. , 900.);
307}
308
310{
311 float cmd_quat[4];
312 float dM_dpprz[4];
313 // Quadratic thrust (and therefore moment) model of the hover propellers
314 for (uint8_t i = 0; i < 4; i++) {
315 cmd_quat[i] = actuator_state_filt_vect[i];
316 Bound(cmd_quat[i], 2500, MAX_PPRZ);
317
318 if(i==0 || i==2) { // pitch motors
319 dM_dpprz[i] = (eff_sched_p.DMdpprz_hover_pitch[0] + 2*cmd_quat[i] * eff_sched_p.DMdpprz_hover_pitch[1]) / 10000.;
320 // Bound dM_dpprz to half and 2 times the hover effectiveness
321 Bound(dM_dpprz[i], eff_sched_var.pitch_motor_dMdpprz * 0.5, eff_sched_var.pitch_motor_dMdpprz * 2.0);
322 } else { // roll motors
323 dM_dpprz[i] = (eff_sched_p.DMdpprz_hover_roll[0] + 2*cmd_quat[i] * eff_sched_p.DMdpprz_hover_roll[1]) / 10000.;
324 Bound(dM_dpprz[i], eff_sched_var.roll_motor_dMdpprz * 0.5, eff_sched_var.roll_motor_dMdpprz * 2.0);
325 }
326 }
327
328 // Roll motor effectiveness
329 float dM_dpprz_right = dM_dpprz[1];
330 float dM_dpprz_left = dM_dpprz[3];
331
332 float roll_motor_p_eff_right = -(dM_dpprz_right * eff_sched_var.cosr + eff_sched_p.hover_roll_roll_coef[0] * eff_sched_var.wing_rotation_rad * eff_sched_var.wing_rotation_rad * eff_sched_var.airspeed * eff_sched_var.cosr) / eff_sched_var.Ixx;
333 roll_motor_p_eff_right = bound_or_zero(roll_motor_p_eff_right, -1.f, -0.00001f);
334
335 float roll_motor_p_eff_left = (dM_dpprz_left * eff_sched_var.cosr + eff_sched_p.hover_roll_roll_coef[0] * eff_sched_var.wing_rotation_rad * eff_sched_var.wing_rotation_rad * eff_sched_var.airspeed * eff_sched_var.cosr) / eff_sched_var.Ixx;
336 roll_motor_p_eff_left = bound_or_zero(roll_motor_p_eff_left, 0.00001f, 1.f);
337
338 float roll_motor_q_eff = (eff_sched_p.hover_roll_pitch_coef[0] * eff_sched_var.wing_rotation_rad + eff_sched_p.hover_roll_pitch_coef[1] * eff_sched_var.wing_rotation_rad * eff_sched_var.wing_rotation_rad * eff_sched_var.sinr) / eff_sched_var.Iyy;
339 Bound(roll_motor_q_eff, 0, 1);
340
341 // Update front pitch motor q effectiveness
342 g1g2[RW_aq][COMMAND_MOTOR_FRONT] = dM_dpprz[0] / eff_sched_var.Iyy; // pitch effectiveness front motor
343
344 // Update back motor q effectiveness
345 g1g2[RW_aq][COMMAND_MOTOR_BACK] = -dM_dpprz[2] / eff_sched_var.Iyy; // pitch effectiveness back motor
346
347 g1g2[RW_ap][COMMAND_MOTOR_RIGHT] = roll_motor_p_eff_right; // roll effectiveness right motor (no airspeed compensation)
348 g1g2[RW_aq][COMMAND_MOTOR_RIGHT] = roll_motor_q_eff; // pitch effectiveness right motor
349
350 // Update left motor p and q effectiveness
351 g1g2[RW_ap][COMMAND_MOTOR_LEFT] = roll_motor_p_eff_left; // roll effectiveness left motor
352 g1g2[RW_aq][COMMAND_MOTOR_LEFT] = -roll_motor_q_eff; // pitch effectiveness left motor
353}
354
356{
357 float de = eff_sched_p.k_elevator_deflection[0] + eff_sched_p.k_elevator_deflection[1] * eff_sched_var.cmd_elevator;
358
359 float dMyde = (eff_sched_p.k_elevator[0] * de * eff_sched_var.airspeed2 +
360 eff_sched_p.k_elevator[1] * eff_sched_var.cmd_pusher_scaled * eff_sched_var.cmd_pusher_scaled * eff_sched_var.airspeed +
361 eff_sched_p.k_elevator[2] * eff_sched_var.airspeed2) / 10000.;
362
363 // scale the effectiveness of the elevator down if it has a large deflection to encourage it to become flat quickly (pragmatic, not physically inpsired)
364 float elevator_ineffectiveness_scaling = (50-de)/40;
365 Bound(elevator_ineffectiveness_scaling, 0.5, 1.0);
366
367 float dMydpprz = dMyde * eff_sched_p.k_elevator_deflection[1];
368
369 // Convert moment to effectiveness
370 float eff_y_elev = dMydpprz / eff_sched_var.Iyy;
371
372 Bound(eff_y_elev, 0.00001, 0.1);
373
374 g1g2[RW_aq][COMMAND_ELEVATOR] = eff_y_elev;
375}
376
378{
379 float dMzdr = (eff_sched_p.k_rudder[0] * eff_sched_var.cmd_pusher_scaled * eff_sched_var.cmd_T_mean_scaled +
380 eff_sched_p.k_rudder[1] * eff_sched_var.cmd_T_mean_scaled * eff_sched_var.airspeed2 * eff_sched_var.cosr +
381 eff_sched_p.k_rudder[2] * eff_sched_var.airspeed2) / 10000.;
382
383 // Convert moment to effectiveness
384
385 float dMzdpprz = dMzdr * eff_sched_p.d_rudder_d_pprz;
386
387 // Convert moment to effectiveness
388 float eff_z_rudder = dMzdpprz / eff_sched_p.Izz;
389
390 Bound(eff_z_rudder, 0.000001, 0.1);
391
392 g1g2[RW_ar][COMMAND_RUDDER] = eff_z_rudder;
393}
394
396{
397 float dMxdpprz = (eff_sched_p.k_aileron * eff_sched_var.airspeed2 * eff_sched_var.sinr3) / 1000000.;
398 float eff_x_aileron = dMxdpprz / eff_sched_var.Ixx;
399 Bound(eff_x_aileron, 0, 0.005);
400 g1g2[RW_ap][COMMAND_AILERONS] = eff_x_aileron;
401
402 float dMydpprz = 4.0*(eff_sched_p.k_aileron * eff_sched_var.airspeed2 * eff_sched_var.sinr2 * eff_sched_var.cosr) / 1000000.;
403 float eff_y_aileron = dMydpprz / eff_sched_var.Iyy;
404 eff_y_aileron = bound_or_zero(eff_y_aileron, 0.00003f, 0.005f);
405 g1g2[RW_aq][COMMAND_AILERONS] = eff_y_aileron;
406}
407
409{
410 float dMxdpprz = (eff_sched_p.k_flaperon * eff_sched_var.airspeed2 * eff_sched_var.sinr3) / 1000000.;
411 float eff_x_flap_aileron = dMxdpprz / eff_sched_var.Ixx;
412 Bound(eff_x_flap_aileron, 0, 0.005);
413 g1g2[RW_ap][COMMAND_FLAPS] = eff_x_flap_aileron;
414}
415
417{
418 float rpmP = eff_sched_p.k_rpm_pprz_pusher[0] + eff_sched_p.k_rpm_pprz_pusher[1] * eff_sched_var.cmd_pusher + eff_sched_p.k_rpm_pprz_pusher[2] * eff_sched_var.cmd_pusher * eff_sched_var.cmd_pusher;
419
420 float dFxdrpmP = eff_sched_p.k_pusher[0]*rpmP + eff_sched_p.k_pusher[1] * eff_sched_var.airspeed;
421 float drpmPdpprz = eff_sched_p.k_rpm_pprz_pusher[1] + 2. * eff_sched_p.k_rpm_pprz_pusher[2] * eff_sched_var.cmd_pusher;
422
423 float eff_pusher = (dFxdrpmP * drpmPdpprz / eff_sched_p.m) / 10000.;
424
425 Bound(eff_pusher, 0.00030, 0.0015);
426 g1g2[RW_aX][COMMAND_THRUST_X] = eff_pusher;
427}
428
430
432{
433 float lift_d_wing = (eff_sched_p.k_lift_wing[0] + eff_sched_p.k_lift_wing[1] * eff_sched_var.sinr2) * eff_sched_var.airspeed2 / eff_sched_p.m;
434 float lift_d_fuselage = eff_sched_p.k_lift_fuselage * eff_sched_var.airspeed2 / eff_sched_p.m;
435 float lift_d_tail = eff_sched_p.k_lift_tail * eff_sched_var.airspeed2 / eff_sched_p.m;
436
437 float lift_d = lift_d_wing + lift_d_fuselage + lift_d_tail;
438 if (eff_sched_var.wing_rotation_deg < 60.) {
439 lift_d = 0.0;
440 }
441 Bound(lift_d, -130., 0.);
443}
444
445// Override standard LIFT_D function
446float guidance_indi_get_liftd(float pitch UNUSED, float theta UNUSED) {
448}
449
451{
452 int32_t flap_saturation_limit;
453
454 // Calculate the min and max increments
455 for (uint8_t i = 0; i < INDI_NUM_ACT; i++) {
456 wls_stab_p.u_min[i] = -MAX_PPRZ * act_is_servo[i];
457 wls_stab_p.u_max[i] = MAX_PPRZ;
458 wls_stab_p.u_pref[i] = act_pref[i];
459
460 switch(i) {
461 case COMMAND_ELEVATOR:
462 wls_stab_p.u_pref[i] = actuator_state_filt_vect[i]; // Set change in prefered state to 0 for elevator
463 wls_stab_p.u_min[i] = 0; // cmd 0 is lowest position for elevator
464 break;
465 case COMMAND_FLAPS:
466 // If an offset is used, limit the max differential command to prevent unilateral saturation.
467 flap_saturation_limit = MAX_PPRZ - abs(rw_flap_offset);
468 wls_stab_p.u_min[i] = -flap_saturation_limit;
469 wls_stab_p.u_max[i] = flap_saturation_limit;
470 break;
471 case COMMAND_THRUST_X:
472 // dt (min to max) MAX_PPRZ / (dt * f) dt_min == 0.002
473 Bound(eff_sched_pusher_time, 0.002, 5.);
474 float max_increment = MAX_PPRZ / (eff_sched_pusher_time * 500);
475 wls_stab_p.u_min[i] = actuators_pprz[i] - max_increment;
476 wls_stab_p.u_max[i] = actuators_pprz[i] + max_increment;
477 break;
478 default:
479 break;
480 }
481
482 Bound(wls_stab_p.u_min[i], -MAX_PPRZ*act_is_servo[i], MAX_PPRZ);
483 Bound(wls_stab_p.u_max[i], -MAX_PPRZ*act_is_servo[i], MAX_PPRZ);
484 }
485}
486
487void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle)
488{
489 // adjust weights
490 float fixed_wing_percentile = (rotwing_state_hover_motors_idling())? 1:0; // TODO: when hover props go below 40%, ...
491 Bound(fixed_wing_percentile, 0, 1);
492#define AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT 16
493
494 float Wv_original[GUIDANCE_INDI_HYBRID_V] = GUIDANCE_INDI_WLS_PRIORITIES;
495
496 // Increase importance of forward acceleration in forward flight
497 wls_guid_p.Wv[0] = Wv_original[0] * (1.0f + fixed_wing_percentile *
498 AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT); // stall n low hover motor_off (weight 16x more important than vertical weight)
499
500 float du_min_thrust_z = ((MAX_PPRZ - actuator_state_filt_vect[COMMAND_MOTOR_FRONT]) * g1g2[RW_aZ][COMMAND_MOTOR_FRONT]
501 + (MAX_PPRZ - actuator_state_filt_vect[COMMAND_MOTOR_RIGHT]) * g1g2[RW_aZ][COMMAND_MOTOR_RIGHT]
502 + (MAX_PPRZ - actuator_state_filt_vect[COMMAND_MOTOR_BACK]) * g1g2[RW_aZ][COMMAND_MOTOR_BACK]
503 + (MAX_PPRZ - actuator_state_filt_vect[COMMAND_MOTOR_LEFT]) * g1g2[RW_aZ][COMMAND_MOTOR_LEFT]) * rotwing_state_hover_motors_running();
504 Bound(du_min_thrust_z, -50., 0.);
505 float du_max_thrust_z = -(actuator_state_filt_vect[COMMAND_MOTOR_FRONT] * g1g2[RW_aZ][COMMAND_MOTOR_FRONT]
506 + actuator_state_filt_vect[COMMAND_MOTOR_RIGHT] * g1g2[RW_aZ][COMMAND_MOTOR_RIGHT]
507 + actuator_state_filt_vect[COMMAND_MOTOR_BACK] * g1g2[RW_aZ][COMMAND_MOTOR_BACK]
508 + actuator_state_filt_vect[COMMAND_MOTOR_LEFT] * g1g2[RW_aZ][COMMAND_MOTOR_LEFT]);
509 Bound(du_max_thrust_z, 0., 50.);
510
511 float roll_limit_rad = guidance_indi_max_bank;
512 float max_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MAX_PITCH);
513 float min_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MIN_PITCH);
514
515 float fwd_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MAX_PITCH);
516 float quad_pitch_limit_rad = RadOfDeg(5.0);
517
519
520 float scheduled_pitch_angle = 0.f;
521 float pitch_angle_range = 3.;
522 float meas_skew_angle = rotwing_state.meas_skew_angle_deg;
523 Bound(meas_skew_angle, 0, 90); // Bound to prevent errors
524 if (meas_skew_angle < ROTWING_SKEW_ANGLE_STEP) {
525 scheduled_pitch_angle = ROTWING_QUAD_PREF_PITCH;
526 wls_guid_p.Wu[1] = Wu_gih_original[1];
527 max_pitch_limit_rad = quad_pitch_limit_rad;
528 } else {
529 float pitch_progression = (airspeed - rotwing_state.fw_min_airspeed) / 2.f;
530 Bound(pitch_progression, 0.f, 1.f);
531 scheduled_pitch_angle = pitch_angle_range * pitch_progression + ROTWING_QUAD_PREF_PITCH*(1.f-pitch_progression);
532 wls_guid_p.Wu[1] = Wu_gih_original[1] * (1.f - pitch_progression*0.99);
533 max_pitch_limit_rad = quad_pitch_limit_rad + (fwd_pitch_limit_rad - quad_pitch_limit_rad) * pitch_progression;
534 }
536 scheduled_pitch_angle = 8.;
537 max_pitch_limit_rad = fwd_pitch_limit_rad;
538 }
539 Bound(scheduled_pitch_angle, -5., 8.);
540 guidance_indi_pitch_pref_deg = scheduled_pitch_angle;
541
542 float pitch_pref_rad = RadOfDeg(guidance_indi_pitch_pref_deg);
543
544 // Set lower limits
545 wls_guid_p.u_min[0] = -roll_limit_rad - roll_angle; //roll
546 wls_guid_p.u_min[1] = min_pitch_limit_rad - pitch_angle; // pitch
547
548 // Set upper limits limits
549 wls_guid_p.u_max[0] = roll_limit_rad - roll_angle; //roll
550 wls_guid_p.u_max[1] = max_pitch_limit_rad - pitch_angle; // pitch
551
553 wls_guid_p.u_min[2] = du_min_thrust_z;
554 wls_guid_p.u_max[2] = du_max_thrust_z;
555 } else {
556 wls_guid_p.u_min[2] = 0.;
557 wls_guid_p.u_max[2] = 0.;
558 }
559
561 wls_guid_p.u_min[3] = (-actuator_state_filt_vect[COMMAND_THRUST_X] * g1g2[RW_aX][COMMAND_THRUST_X]);
562 wls_guid_p.u_max[3] = 9.0; // Hacky value to prevent drone from pitching down in transition
563 } else {
564 wls_guid_p.u_min[3] = 0.;
565 wls_guid_p.u_max[3] = 0.;
566 }
567
568 // Set prefered states
569 wls_guid_p.u_pref[0] = 0; // prefered delta roll angle
570 wls_guid_p.u_pref[1] = -pitch_angle + pitch_pref_rad;// prefered delta pitch angle
571 wls_guid_p.u_pref[2] = wls_guid_p.u_max[2]; // Low thrust better for efficiency
572 wls_guid_p.u_pref[3] = body_v[0]; // solve the body acceleration
573}
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
Definition abi_common.h:101
Core autopilot interface common to all firmwares.
#define UNUSED(x)
#define RW_aZ
#define RW_aX
#define RW_ap
#define RW_ar
#define RW_aq
void stabilization_indi_set_wls_settings(void)
Function that sets the u_min, u_max and u_pref if function not elsewhere defined.
void eff_scheduling_rotwing_update_flaperon_effectiveness(void)
void eff_scheduling_rotwing_update_rudder_effectiveness(void)
void eff_scheduling_rotwing_update_aileron_effectiveness(void)
float eff_scheduling_rotwing_lift_d
static const float Wu_gih_original[GUIDANCE_INDI_HYBRID_U]
struct rotwing_eff_sched_var_t eff_sched_var
void eff_scheduling_rotwing_update_MMOI(void)
#define WING_ROTATION_CAN_ROTWING_ID
ABI binding wing position data.
void eff_scheduling_rotwing_update_airspeed(void)
float roll_eff_slider
int32_t rw_flap_offset
void eff_scheduling_rotwing_update_cmd(void)
void eff_scheduling_rotwing_schedule_liftd(void)
float guidance_indi_get_liftd(float pitch UNUSED, float theta UNUSED)
#define AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT
void eff_scheduling_rotwing_update_wing_angle(void)
struct rotwing_eff_sched_param_t eff_sched_p
void eff_scheduling_rotwing_update_hover_motor_effectiveness(void)
static abi_event wing_position_ev
void eff_scheduling_rotwing_init(void)
void eff_scheduling_rotwing_update_elevator_effectiveness(void)
float eff_sched_pusher_time
float bound_or_zero(float value, float low_lim, float up_lim)
void eff_scheduling_rotwing_update_pusher_effectiveness(void)
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)
void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle)
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition state.h:1615
float guidance_indi_max_bank
#define GUIDANCE_INDI_WLS_WU
float guidance_indi_pitch_pref_deg
#define GUIDANCE_INDI_WLS_PRIORITIES
A guidance mode based on Incremental Nonlinear Dynamic Inversion Come to ICRA2016 to learn more!
#define GUIDANCE_INDI_MAX_PITCH
#define GUIDANCE_INDI_MIN_PITCH
Hardware independent API for actuators (servos, motor controllers).
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
#define MAX_PPRZ
Definition paparazzi.h:8
bool rotwing_state_pusher_motor_running(void)
bool rotwing_state_hover_motors_running(void)
bool rotwing_state_hover_motors_idling(void)
Check if hover motors are idling (COMMAND_THRUST < ROTWING_QUAD_IDLE_MIN_THRUST) for ROTWING_QUAD_IDL...
struct rotwing_state_t rotwing_state
#define ROTWING_QUAD_PREF_PITCH
#define ROTWING_SKEW_ANGLE_STEP
struct WLS_t wls_stab_p
float act_pref[INDI_NUM_ACT]
bool act_is_servo[INDI_NUM_ACT]
float g1g2[INDI_OUTPUTS][INDI_NUM_ACT]
float actuator_state_filt_vect[INDI_NUM_ACT]
int16_t actuators_pprz[INDI_NUM_ACT+1]
PPRZ command to each actuator Can be used to directly control actuators from the control algorithm if...
API to get/set the generic vehicle states.
int int32_t
Typedef defining 32 bit int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.