Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
eff_scheduling_rotwing_rpm_ctrl.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"
39
40#ifndef SERVO_ROTATION_MECH_IDX
41#error ctrl_eff_sched_rotwing requires a servo named ROTATION_MECH_IDX
42#endif
43
44#ifndef ROTWING_EFF_SCHED_IXX_BODY
45#error "NO ROTWING_EFF_SCHED_IXX_BODY defined"
46#endif
47
48#ifndef ROTWING_EFF_SCHED_IYY_BODY
49#error "NO ROTWING_EFF_SCHED_IYY_BODY defined"
50#endif
51
52#ifndef ROTWING_EFF_SCHED_IZZ
53#error "NO ROTWING_EFF_SCHED_IZZ defined"
54#endif
55
56#ifndef ROTWING_EFF_SCHED_IXX_WING
57#error "NO ROTWING_EFF_SCHED_IXX_WING defined"
58#endif
59
60#ifndef ROTWING_EFF_SCHED_IYY_WING
61#error "NO ROTWING_EFF_SCHED_IYY_WING defined"
62#endif
63
64#ifndef ROTWING_EFF_SCHED_M
65#error "NO ROTWING_EFF_SCHED_M defined"
66#endif
67
68#ifndef ROTWING_EFF_SCHED_K_ELEVATOR
69#error "NO ROTWING_EFF_SCHED_K_ELEVATOR defined"
70#endif
71
72#ifndef ROTWING_EFF_SCHED_K_RUDDER
73#error "NO ROTWING_EFF_SCHED_K_RUDDER defined"
74#endif
75
76#ifndef ROTWING_EFF_SCHED_K_AILERON
77#error "NO ROTWING_EFF_SCHED_K_AILERON defined"
78#endif
79
80#ifndef ROTWING_EFF_SCHED_K_FLAPERON
81#error "NO ROTWING_EFF_SCHED_K_FLAPERON defined"
82#endif
83
84#ifndef ROTWING_EFF_SCHED_K_ELEVATOR_DEFLECTION
85#error "NO ROTWING_EFF_SCHED_K_ELEVATOR_DEFLECTION defined"
86#endif
87
88#ifndef ROTWING_EFF_SCHED_D_RUDDER_D_PPRZ
89#error "NO ROTWING_EFF_SCHED_D_RUDDER_D_PPRZ defined"
90#endif
91
92#ifndef ROTWING_EFF_SCHED_K_LIFT_WING
93#error "NO ROTWING_EFF_SCHED_K_LIFT_WING defined"
94#endif
95
96#ifndef ROTWING_EFF_SCHED_K_LIFT_FUSELAGE
97#error "NO ROTWING_EFF_SCHED_K_LIFT_FUSELAGE defined"
98#endif
99
100#ifndef ROTWING_EFF_SCHED_K_LIFT_TAIL
101#error "NO ROTWING_EFF_SCHED_K_LIFT_TAIL defined"
102#endif
103
104#ifndef ROTWING_EFF_SCHED_CT_HOVER
105#error "NO ROTWING_EFF_SCHED_CT_HOVER defined"
106#endif
107
108#ifndef ROTWING_EFF_SCHED_CT_PUSHER
109#error "NO ROTWING_EFF_SCHED_CT_PUSHER defined"
110#endif
111
112#ifndef ROTWING_EFF_SCHED_ROLL_ARM
113#error "NO EFF_SCHEDULING_ROTWING_ROLL_ARM defined"
114#endif
115
116#ifndef ROTWING_EFF_SCHED_PITCH_ARM
117#error "NO EFF_SCHEDULING_ROTWING_PITCH_ARM defined"
118#endif
119
120#ifndef ROTWING_EFF_SCHED_ROLL_ARM_PITCH_MOTORS
121#error "NO ROTWING_EFF_SCHED_ROLL_ARM_PITCH_MOTORS defined"
122#endif
123
124#ifndef ROTWING_EFF_SCHED_HOVER_MAX_RPM
125#error "NO ROTWING_EFF_SCHED_HOVER_MAX_RPM defined"
126#endif
127
128#ifndef ROTWING_EFF_SCHED_PUSHER_MAX_RPM
129#error "NO ROTWING_EFF_SCHED_PUSHER_MAX_RPM defined"
130#endif
131
132#ifndef ROTWING_EFF_SCHED_RPM_CTRL_HOVER_LIM_COEF
133#error "USING RPM CONTROL BUT NO ROTWING_EFF_SCHED_RPM_CTRL_HOVER_LIM_COEF defined"
134#endif
135
136#ifndef ROTWING_EFF_SCHED_RPM_CTRL_PUSH_LIM_COEF
137#error "USING RPM CONTROL BUT NO ROTWING_EFF_SCHED_RPM_CTRL_PUSH_LIM_COEF defined"
138#endif
139
140#ifndef ROTWING_PITCH_MOTOR_TILT_ANGLE_DEG
141#define ROTWING_PITCH_MOTOR_TILT_ANGLE_DEG 10.0
142#endif
143
145 .Ixx_body = ROTWING_EFF_SCHED_IXX_BODY,
146 .Iyy_body = ROTWING_EFF_SCHED_IYY_BODY,
147 .Izz = ROTWING_EFF_SCHED_IZZ,
148 .Ixx_wing = ROTWING_EFF_SCHED_IXX_WING,
149 .Iyy_wing = ROTWING_EFF_SCHED_IYY_WING,
150 .m = ROTWING_EFF_SCHED_M,
151 .CT_hover = ROTWING_EFF_SCHED_CT_HOVER,
152 .CT_pusher = ROTWING_EFF_SCHED_CT_PUSHER,
153 .hover_max_rpm_squared = ROTWING_EFF_SCHED_HOVER_MAX_RPM * ROTWING_EFF_SCHED_HOVER_MAX_RPM,
154 .pusher_max_rpm_squared = ROTWING_EFF_SCHED_PUSHER_MAX_RPM * ROTWING_EFF_SCHED_PUSHER_MAX_RPM,
155 .k_elevator = ROTWING_EFF_SCHED_K_ELEVATOR,
156 .k_rudder = ROTWING_EFF_SCHED_K_RUDDER,
157 .k_aileron = ROTWING_EFF_SCHED_K_AILERON,
158 .k_flaperon = ROTWING_EFF_SCHED_K_FLAPERON,
159 .k_elevator_deflection = ROTWING_EFF_SCHED_K_ELEVATOR_DEFLECTION,
160 .d_rudder_d_pprz = ROTWING_EFF_SCHED_D_RUDDER_D_PPRZ,
161 .k_lift_wing = ROTWING_EFF_SCHED_K_LIFT_WING,
162 .k_lift_fuselage = ROTWING_EFF_SCHED_K_LIFT_FUSELAGE,
163 .k_lift_tail = ROTWING_EFF_SCHED_K_LIFT_TAIL,
164 .hover_rpm_lim_coef = ROTWING_EFF_SCHED_RPM_CTRL_HOVER_LIM_COEF,
165 .pusher_rpm_lim_coef = ROTWING_EFF_SCHED_RPM_CTRL_PUSH_LIM_COEF
166};
167
170
171// for negative values, still should be low_lim < up_lim
172inline float bound_or_zero(float value, float low_lim, float up_lim) {
173 float output = value;
174 if (low_lim > 0.f) {
175 if (value < low_lim) {
176 output = 0.f;
177 } else if (value > up_lim) {
178 output = up_lim;
179 }
180 } else {
181 if (value > up_lim) {
182 output = 0.f;
183 } else if (value < low_lim) {
184 output = low_lim;
185 }
186 }
187 return output;
188}
189
191float roll_eff_slider = 12.f;
192
193static const float Wu_gih_original[GUIDANCE_INDI_HYBRID_U] = GUIDANCE_INDI_WLS_WU;
194
196
198inline void eff_scheduling_rotwing_update_MMOI(void);
199inline void eff_scheduling_rotwing_update_cmd(void);
208
209inline float guidance_indi_get_liftd(float pitch UNUSED, float theta UNUSED);
211inline void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle);
212
213
216#ifndef WING_ROTATION_CAN_ROTWING_ID
217#define WING_ROTATION_CAN_ROTWING_ID ABI_BROADCAST
218#endif
221
222static void wing_position_cb(uint8_t sender_id UNUSED, struct act_feedback_t *pos_msg, uint8_t num_act)
223{
224 for (int i=0; i<num_act; i++){
225 if (pos_msg[i].set.position && (pos_msg[i].idx == SERVO_ROTATION_MECH_IDX))
226 {
227 // Get wing rotation angle from sensor
228 eff_sched_var.wing_rotation_rad = 0.5 * M_PI - pos_msg[i].position;
229
230 // Bound wing rotation angle
231 Bound(eff_sched_var.wing_rotation_rad, 0, 0.5 * M_PI);
232 }
233 }
234}
235
237{
238 // Initialize variables to quad values
239 eff_sched_var.Ixx = eff_sched_p.Ixx_body + eff_sched_p.Ixx_wing;
240 eff_sched_var.Iyy = eff_sched_p.Iyy_body + eff_sched_p.Iyy_wing;
241 eff_sched_var.wing_rotation_rad = 0; // ABI input
242 eff_sched_var.wing_rotation_deg = 0;
243 eff_sched_var.cosr = 1;
244 eff_sched_var.sinr = 0;
245 eff_sched_var.cosr2 = 1;
246 eff_sched_var.sinr2 = 0;
247 eff_sched_var.sinr3 = 0;
248
249 eff_sched_var.cmd_elevator = 0;
250 eff_sched_var.cmd_pusher = 0;
251 eff_sched_var.cmd_pusher_scaled = 0;
252 eff_sched_var.cmd_T_mean_scaled = 0;
253
254 eff_sched_var.airspeed = 0;
255 eff_sched_var.airspeed2 = 0;
256
257 // Get wing angle
259}
260
279
281{
282 // Calculate sin and cosines of rotation
283 eff_sched_var.wing_rotation_deg = eff_sched_var.wing_rotation_rad / M_PI * 180.;
284
285 eff_sched_var.cosr = cosf(eff_sched_var.wing_rotation_rad);
286 eff_sched_var.sinr = sinf(eff_sched_var.wing_rotation_rad);
287
288 eff_sched_var.cosr2 = eff_sched_var.cosr * eff_sched_var.cosr;
289 eff_sched_var.sinr2 = eff_sched_var.sinr * eff_sched_var.sinr;
290
291 eff_sched_var.sinr3 = eff_sched_var.sinr2 * eff_sched_var.sinr;
292
293}
294
296{
297 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;
298 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;
299
300 // Bound inertia
301 Bound(eff_sched_var.Ixx, 0.01, 100.);
302 Bound(eff_sched_var.Iyy, 0.01, 100.);
303}
304
306{
307 // These indexes depend on the INDI sequence, not the actuator IDX
308 eff_sched_var.cmd_elevator = actuator_state_filt_vect[5];
310 eff_sched_var.cmd_pusher_scaled = actuator_state_filt_vect[8] * 0.000853229; // Scaled with 8181 / 9600 / 1000
311 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
312}
313
315{
317 Bound(eff_sched_var.airspeed, 0. , 30.);
318 eff_sched_var.airspeed2 = eff_sched_var.airspeed * eff_sched_var.airspeed;
319 Bound(eff_sched_var.airspeed2, 0. , 900.);
320}
321
323{
324 // Scaling so CAN_cmd squared fits into MAX_PPRZ
325 float scaling = eff_sched_p.hover_max_rpm_squared / MAX_PPRZ;
326
327 // Calculate the change in moment per change in RPM squared
328 float dMdu_pitch = ROTWING_EFF_SCHED_PITCH_ARM * eff_sched_p.CT_hover * scaling;
329 float dMdu_roll = ROTWING_EFF_SCHED_ROLL_ARM * eff_sched_p.CT_hover * scaling;
330
331 float front_motor_pitch_eff = dMdu_pitch / eff_sched_var.Iyy;
332 float back_motor_pitch_eff = -dMdu_pitch / eff_sched_var.Iyy;
333
334 // pitch motor has roll effectiveness due to z offset from c.g. and tilt
335 static const float roll_eff_pitch_motor_scaling = ROTWING_EFF_SCHED_ROLL_ARM_PITCH_MOTORS / ROTWING_EFF_SCHED_ROLL_ARM * sinf(RadOfDeg(ROTWING_PITCH_MOTOR_TILT_ANGLE_DEG));
336
337 // Update front motor roll and pitch effectiveness
338 g1g2[RW_aq][COMMAND_MOTOR_FRONT] = front_motor_pitch_eff; // pitch effectiveness front motor
339 g1g2[RW_ap][COMMAND_MOTOR_FRONT] = dMdu_pitch * roll_eff_pitch_motor_scaling / eff_sched_var.Ixx; // Roll effectiveness front motor
340
341 // Update back motor roll and pitch effectiveness
342 g1g2[RW_aq][COMMAND_MOTOR_BACK] = back_motor_pitch_eff; // pitch effectiveness back motor
343 g1g2[RW_ap][COMMAND_MOTOR_BACK] = -dMdu_pitch * roll_eff_pitch_motor_scaling / eff_sched_var.Ixx; // Roll effectiveness back motor
344
345 // Update right motor roll and pitch effectiveness
346 float right_motor_roll_eff = -(dMdu_roll * eff_sched_var.cosr) / eff_sched_var.Ixx;
347 right_motor_roll_eff = bound_or_zero(right_motor_roll_eff, -1.f, -0.00001f);
348
349 float right_motor_pitch_eff = (dMdu_roll * eff_sched_var.sinr) / eff_sched_var.Iyy;
350 right_motor_pitch_eff = bound_or_zero(right_motor_pitch_eff, 0.00001f, front_motor_pitch_eff);
351
352 g1g2[RW_ap][COMMAND_MOTOR_RIGHT] = right_motor_roll_eff; // roll effectiveness right motor
353 g1g2[RW_aq][COMMAND_MOTOR_RIGHT] = right_motor_pitch_eff; // pitch effectiveness right motor
354
355 // Update left motor roll and pitch effectiveness
356 float left_motor_roll_eff = (dMdu_roll * eff_sched_var.cosr) / eff_sched_var.Ixx;
357 left_motor_roll_eff = bound_or_zero(left_motor_roll_eff, 0.00001f, 1.f);
358
359 float left_motor_pitch_eff = -(dMdu_roll * eff_sched_var.sinr) / eff_sched_var.Iyy;
360 left_motor_pitch_eff = bound_or_zero(left_motor_pitch_eff, back_motor_pitch_eff, -0.00001f);
361
362 g1g2[RW_ap][COMMAND_MOTOR_LEFT] = left_motor_roll_eff; // roll effectiveness left motor
363 g1g2[RW_aq][COMMAND_MOTOR_LEFT] = left_motor_pitch_eff; // pitch effectiveness left motor
364}
365
367{
368 float de = eff_sched_p.k_elevator_deflection[0] + eff_sched_p.k_elevator_deflection[1] * eff_sched_var.cmd_elevator;
369
370 float dMyde = (eff_sched_p.k_elevator[0] * de * eff_sched_var.airspeed2 +
371 eff_sched_p.k_elevator[1] * eff_sched_var.cmd_pusher_scaled * eff_sched_var.cmd_pusher_scaled * eff_sched_var.airspeed +
372 eff_sched_p.k_elevator[2] * eff_sched_var.airspeed2) / 10000.;
373
374 // scale the effectiveness of the elevator down if it has a large deflection to encourage it to become flat quickly (pragmatic, not physically inpsired)
375 float elevator_ineffectiveness_scaling = (50-de)/40;
376 Bound(elevator_ineffectiveness_scaling, 0.5, 1.0);
377
378 float dMydpprz = dMyde * eff_sched_p.k_elevator_deflection[1];
379
380 // Convert moment to effectiveness
381 float eff_y_elev = dMydpprz / eff_sched_var.Iyy;
382
383 Bound(eff_y_elev, 0.00001, 0.1);
384
385 g1g2[RW_aq][COMMAND_ELEVATOR] = eff_y_elev;
386}
387
389{
390 float dMzdr = (eff_sched_p.k_rudder[0] * eff_sched_var.cmd_pusher_scaled * eff_sched_var.cmd_T_mean_scaled +
391 eff_sched_p.k_rudder[1] * eff_sched_var.cmd_T_mean_scaled * eff_sched_var.airspeed2 * eff_sched_var.cosr +
392 eff_sched_p.k_rudder[2] * eff_sched_var.airspeed2) / 10000.;
393
394 // Convert moment to effectiveness
395
396 float dMzdpprz = dMzdr * eff_sched_p.d_rudder_d_pprz;
397
398 // Convert moment to effectiveness
399 float eff_z_rudder = dMzdpprz / eff_sched_p.Izz;
400
401 Bound(eff_z_rudder, 0.000001, 0.1);
402
403 g1g2[RW_ar][COMMAND_RUDDER] = eff_z_rudder;
404}
405
407{
408 float dMxdpprz = (eff_sched_p.k_aileron * eff_sched_var.airspeed2 * eff_sched_var.sinr3) / 1000000.;
409 float eff_x_aileron = dMxdpprz / eff_sched_var.Ixx;
410 Bound(eff_x_aileron, 0, 0.005);
411 g1g2[RW_ap][COMMAND_AILERONS] = eff_x_aileron;
412
413 float dMydpprz = 4.0*(eff_sched_p.k_aileron * eff_sched_var.airspeed2 * eff_sched_var.sinr2 * eff_sched_var.cosr) / 1000000.;
414 float eff_y_aileron = dMydpprz / eff_sched_var.Iyy;
415 eff_y_aileron = bound_or_zero(eff_y_aileron, 0.00003f, 0.005f);
416 g1g2[RW_aq][COMMAND_AILERONS] = eff_y_aileron;
417}
418
420{
421 float dMxdpprz = (eff_sched_p.k_flaperon * eff_sched_var.airspeed2 * eff_sched_var.sinr3) / 1000000.;
422 float eff_x_flap_aileron = dMxdpprz / eff_sched_var.Ixx;
423 Bound(eff_x_flap_aileron, 0, 0.005);
424 g1g2[RW_ap][COMMAND_FLAPS] = eff_x_flap_aileron;
425}
426
428{
429 // Scaling so CAN_cmd squared fits into MAX_PPRZ
430 float scaling = eff_sched_p.pusher_max_rpm_squared / MAX_PPRZ;
431 float drpmPdpprz = eff_sched_p.CT_pusher * scaling;
432 float eff_pusher = (drpmPdpprz / eff_sched_p.m);
433
434 // Bound effectiveness (shouldn't be necessary with the new structure but just in case)
435 Bound(eff_pusher, 0.00030, 0.0015);
436
437 g1g2[RW_aX][COMMAND_THRUST_X] = eff_pusher;
438}
439
441{
442 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;
443 float lift_d_fuselage = eff_sched_p.k_lift_fuselage * eff_sched_var.airspeed2 / eff_sched_p.m;
444 float lift_d_tail = eff_sched_p.k_lift_tail * eff_sched_var.airspeed2 / eff_sched_p.m;
445
446 float lift_d = lift_d_wing + lift_d_fuselage + lift_d_tail;
447 if (eff_sched_var.wing_rotation_deg < 60.) {
448 lift_d = 0.0;
449 }
450 Bound(lift_d, -130., 0.);
452}
453
454// Override standard LIFT_D function
455float guidance_indi_get_liftd(float pitch UNUSED, float theta UNUSED) {
457}
458
460{
461 int32_t flap_saturation_limit;
462
463 // Calculate the min and max increments
464 for (uint8_t i = 0; i < INDI_NUM_ACT; i++) {
465 wls_stab_p.u_min[i] = -MAX_PPRZ * act_is_servo[i];
466 wls_stab_p.u_max[i] = MAX_PPRZ;
467 wls_stab_p.u_pref[i] = act_pref[i];
468
469 switch(i) {
470 // With RPM control we need to limit the max command to the hover motors based on the battery voltage
471 case COMMAND_MOTOR_FRONT:
472 wls_stab_p.u_max[i] = eff_sched_p.hover_rpm_lim_coef[0] + electrical.vsupply * eff_sched_p.hover_rpm_lim_coef[1];
473 wls_stab_p.u_min[i] = (int16_t)((float)SERVO_MOTOR_FRONT_NEUTRAL * ((float)SERVO_MOTOR_FRONT_NEUTRAL * ((float)MAX_PPRZ / (float)SERVO_MOTOR_FRONT_MAX) / (float)SERVO_MOTOR_FRONT_MAX));
474 break;
475 case COMMAND_MOTOR_RIGHT:
476 wls_stab_p.u_max[i] = eff_sched_p.hover_rpm_lim_coef[0] + electrical.vsupply * eff_sched_p.hover_rpm_lim_coef[1];
477 wls_stab_p.u_min[i] = (int16_t)((float)SERVO_MOTOR_RIGHT_NEUTRAL * ((float)SERVO_MOTOR_RIGHT_NEUTRAL * ((float)MAX_PPRZ / (float)SERVO_MOTOR_RIGHT_MAX) / (float)SERVO_MOTOR_RIGHT_MAX));
478 break;
479 case COMMAND_MOTOR_BACK:
480 wls_stab_p.u_max[i] = eff_sched_p.hover_rpm_lim_coef[0] + electrical.vsupply * eff_sched_p.hover_rpm_lim_coef[1];
481 wls_stab_p.u_min[i] = (int16_t)((float)SERVO_MOTOR_BACK_NEUTRAL * ((float)SERVO_MOTOR_BACK_NEUTRAL * ((float)MAX_PPRZ / (float)SERVO_MOTOR_BACK_MAX) / (float)SERVO_MOTOR_BACK_MAX));
482 break;
483 case COMMAND_MOTOR_LEFT:
484 wls_stab_p.u_max[i] = eff_sched_p.hover_rpm_lim_coef[0] + electrical.vsupply * eff_sched_p.hover_rpm_lim_coef[1];
485 wls_stab_p.u_min[i] = (int16_t)((float)SERVO_MOTOR_LEFT_NEUTRAL * ((float)SERVO_MOTOR_LEFT_NEUTRAL * ((float)MAX_PPRZ / (float)SERVO_MOTOR_LEFT_MAX) / (float)SERVO_MOTOR_LEFT_MAX));
486 break;
487 case COMMAND_ELEVATOR:
488 wls_stab_p.u_pref[i] = actuator_state_filt_vect[i]; // Set change in prefered state to 0 for elevator
489 wls_stab_p.u_min[i] = 0; // cmd 0 is lowest position for elevator
490 break;
491 case COMMAND_FLAPS:
492 // If an offset is used, limit the max differential command to prevent unilateral saturation.
493 flap_saturation_limit = MAX_PPRZ - abs(rw_flap_offset);
494 wls_stab_p.u_min[i] = -flap_saturation_limit;
495 wls_stab_p.u_max[i] = flap_saturation_limit;
496 break;
497 case COMMAND_THRUST_X:
498 // dt (min to max) MAX_PPRZ / (dt * f) dt_min == 0.002
499 Bound(eff_sched_pusher_time, 0.002, 5.);
500 float max_increment = MAX_PPRZ / (eff_sched_pusher_time * 500);
501 wls_stab_p.u_min[i] = actuators_pprz[i] - max_increment;
502 wls_stab_p.u_max[i] = actuators_pprz[i] + max_increment;
503
504 // With RPM control we need to limit the max command to the pusher motor based on the battery voltage
505 wls_stab_p.u_min[i] = Max((int16_t)((float)SERVO_MOTOR_PUSH_NEUTRAL * ((float)SERVO_MOTOR_PUSH_NEUTRAL * ((float)MAX_PPRZ / (float)SERVO_MOTOR_PUSH_MAX) / (float)SERVO_MOTOR_PUSH_MAX)), wls_stab_p.u_min[i]);
506 wls_stab_p.u_max[i] = Min(eff_sched_p.pusher_rpm_lim_coef[0] + electrical.vsupply * eff_sched_p.pusher_rpm_lim_coef[1], wls_stab_p.u_max[i]);
507 break;
508 default:
509 break;
510 }
511
512 Bound(wls_stab_p.u_min[i], -MAX_PPRZ*act_is_servo[i], MAX_PPRZ);
513 Bound(wls_stab_p.u_max[i], -MAX_PPRZ*act_is_servo[i], MAX_PPRZ);
514 }
515}
516
517void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle)
518{
519 // adjust weights
520 float fixed_wing_percentile = (rotwing_state_hover_motors_idling())? 1:0; // TODO: when hover props go below 40%, ...
521 Bound(fixed_wing_percentile, 0, 1);
522#define AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT 16
523
524 float Wv_original[GUIDANCE_INDI_HYBRID_V] = GUIDANCE_INDI_WLS_PRIORITIES;
525
526 // Increase importance of forward acceleration in forward flight
527 wls_guid_p.Wv[0] = Wv_original[0] * (1.0f + fixed_wing_percentile *
528 AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT); // stall n low hover motor_off (weight 16x more important than vertical weight)
529
530 struct FloatEulers eulers_zxy;
532
533 float du_min_thrust_z = ((MAX_PPRZ - actuator_state_filt_vect[COMMAND_MOTOR_FRONT]) * g1g2[RW_aZ][COMMAND_MOTOR_FRONT]
534 + (MAX_PPRZ - actuator_state_filt_vect[COMMAND_MOTOR_RIGHT]) * g1g2[RW_aZ][COMMAND_MOTOR_RIGHT]
535 + (MAX_PPRZ - actuator_state_filt_vect[COMMAND_MOTOR_BACK]) * g1g2[RW_aZ][COMMAND_MOTOR_BACK]
536 + (MAX_PPRZ - actuator_state_filt_vect[COMMAND_MOTOR_LEFT]) * g1g2[RW_aZ][COMMAND_MOTOR_LEFT]) * rotwing_state_hover_motors_running();
537 Bound(du_min_thrust_z, -50., 0.);
538 float du_max_thrust_z = -(actuator_state_filt_vect[COMMAND_MOTOR_FRONT] * g1g2[RW_aZ][COMMAND_MOTOR_FRONT]
539 + actuator_state_filt_vect[COMMAND_MOTOR_RIGHT] * g1g2[RW_aZ][COMMAND_MOTOR_RIGHT]
540 + actuator_state_filt_vect[COMMAND_MOTOR_BACK] * g1g2[RW_aZ][COMMAND_MOTOR_BACK]
541 + actuator_state_filt_vect[COMMAND_MOTOR_LEFT] * g1g2[RW_aZ][COMMAND_MOTOR_LEFT]);
542 Bound(du_max_thrust_z, 0., 50.);
543
544 float roll_limit_rad = guidance_indi_max_bank;
545 float max_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MAX_PITCH);
546 float min_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MIN_PITCH);
547
548 float fwd_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MAX_PITCH);
549 float quad_pitch_limit_rad = RadOfDeg(5.0);
550
551 float airspeed = stateGetAirspeed_f();
552
553 float scheduled_pitch_angle = 0.f;
554 float pitch_angle_range = 3.;
555 float meas_skew_angle = rotwing_state.meas_skew_angle_deg;
556 Bound(meas_skew_angle, 0, 90); // Bound to prevent errors
557 if (meas_skew_angle < ROTWING_SKEW_ANGLE_STEP) {
558 scheduled_pitch_angle = ROTWING_QUAD_PREF_PITCH;
559 wls_guid_p.Wu[1] = Wu_gih_original[1];
560 max_pitch_limit_rad = quad_pitch_limit_rad;
561 } else {
562 float pitch_progression = (airspeed - rotwing_state.fw_min_airspeed) / 2.f;
563 Bound(pitch_progression, 0.f, 1.f);
564 scheduled_pitch_angle = pitch_angle_range * pitch_progression + ROTWING_QUAD_PREF_PITCH*(1.f-pitch_progression);
565 wls_guid_p.Wu[1] = Wu_gih_original[1] * (1.f - pitch_progression*0.99);
566 max_pitch_limit_rad = quad_pitch_limit_rad + (fwd_pitch_limit_rad - quad_pitch_limit_rad) * pitch_progression;
567 }
569 scheduled_pitch_angle = 8.;
570 max_pitch_limit_rad = fwd_pitch_limit_rad;
571 }
572 Bound(scheduled_pitch_angle, -5., 8.);
573 guidance_indi_pitch_pref_deg = scheduled_pitch_angle;
574
575 float pitch_pref_rad = RadOfDeg(guidance_indi_pitch_pref_deg);
576
577 // Set lower limits
578 wls_guid_p.u_min[0] = -roll_limit_rad - roll_angle; //roll
579 wls_guid_p.u_min[1] = min_pitch_limit_rad - pitch_angle; // pitch
580
581 // Set upper limits limits
582 wls_guid_p.u_max[0] = roll_limit_rad - roll_angle; //roll
583 wls_guid_p.u_max[1] = max_pitch_limit_rad - pitch_angle; // pitch
584
586 wls_guid_p.u_min[2] = du_min_thrust_z;
587 wls_guid_p.u_max[2] = du_max_thrust_z;
588 } else {
589 wls_guid_p.u_min[2] = 0.;
590 wls_guid_p.u_max[2] = 0.;
591 }
592
594 wls_guid_p.u_min[3] = (-actuator_state_filt_vect[COMMAND_THRUST_X] * g1g2[RW_aX][COMMAND_THRUST_X]);
595 wls_guid_p.u_max[3] = 9.0; // Hacky value to prevent drone from pitching down in transition
596 } else {
597 wls_guid_p.u_min[3] = 0.;
598 wls_guid_p.u_max[3] = 0.;
599 }
600
601 // Set prefered states
602 wls_guid_p.u_pref[0] = 0; // prefered delta roll angle
603 wls_guid_p.u_pref[1] = -pitch_angle + pitch_pref_rad;// prefered delta pitch angle
604 wls_guid_p.u_pref[2] = wls_guid_p.u_max[2]; // Low thrust better for efficiency
605 wls_guid_p.u_pref[3] = body_v[0]; // solve the body acceleration
606}
607
616static float u_squared_to_pprz_cmd(float u_squared, float max, float neutral, float travel_up)
617{
618 float u = (u_squared > 0 ? 1 : (u_squared < 0 ? -1 : 0)) * sqrtf(fabsf(u_squared) / (float)MAX_PPRZ * max * max);
619 float u_unbounded = (u - neutral) / travel_up;
620 return (u_unbounded < 0) ? 0 : u_unbounded;
621}
622
631static float pprz_cmd_to_u_squared(float pprz_cmd, float max, float neutral, float travel_up)
632{
633 float u_can_cmd = pprz_cmd * travel_up + neutral;
634 return (u_can_cmd * u_can_cmd) / (max * max) * (float)MAX_PPRZ;
635}
636
638{
642
643 float u_pprz_cmd;
644 float act_pprz;
645
646 //actuator dynamics
647 for (uint8_t i = 0; i < INDI_NUM_ACT; i++) {
648 switch(i) {
649 // With RPM control we have to do a conversion step from allocation back to actuator state
650 case COMMAND_MOTOR_FRONT:
651 u_pprz_cmd = u_squared_to_pprz_cmd(indi_u[i], SERVO_MOTOR_FRONT_MAX, SERVO_MOTOR_FRONT_NEUTRAL, SERVO_MOTOR_FRONT_TRAVEL_UP);
652 act_pprz = u_squared_to_pprz_cmd(actuator_state[i], SERVO_MOTOR_FRONT_MAX, SERVO_MOTOR_FRONT_NEUTRAL, SERVO_MOTOR_FRONT_TRAVEL_UP);
653 actuator_state[i] = act_pprz + act_dyn_discrete[i] * (u_pprz_cmd - act_pprz);
654 actuator_state[i] = pprz_cmd_to_u_squared(actuator_state[i], SERVO_MOTOR_FRONT_MAX, SERVO_MOTOR_FRONT_NEUTRAL, SERVO_MOTOR_FRONT_TRAVEL_UP);
655 break;
656 case COMMAND_MOTOR_RIGHT:
657 u_pprz_cmd = u_squared_to_pprz_cmd(indi_u[i], SERVO_MOTOR_RIGHT_MAX, SERVO_MOTOR_RIGHT_NEUTRAL, SERVO_MOTOR_RIGHT_TRAVEL_UP);
658 act_pprz = u_squared_to_pprz_cmd(actuator_state[i], SERVO_MOTOR_RIGHT_MAX, SERVO_MOTOR_RIGHT_NEUTRAL, SERVO_MOTOR_RIGHT_TRAVEL_UP);
659 actuator_state[i] = act_pprz + act_dyn_discrete[i] * (u_pprz_cmd - act_pprz);
660 actuator_state[i] = pprz_cmd_to_u_squared(actuator_state[i], SERVO_MOTOR_RIGHT_MAX, SERVO_MOTOR_RIGHT_NEUTRAL, SERVO_MOTOR_RIGHT_TRAVEL_UP);
661 break;
662 case COMMAND_MOTOR_BACK:
663 u_pprz_cmd = u_squared_to_pprz_cmd(indi_u[i], SERVO_MOTOR_BACK_MAX, SERVO_MOTOR_BACK_NEUTRAL, SERVO_MOTOR_BACK_TRAVEL_UP);
664 act_pprz = u_squared_to_pprz_cmd(actuator_state[i], SERVO_MOTOR_BACK_MAX, SERVO_MOTOR_BACK_NEUTRAL, SERVO_MOTOR_BACK_TRAVEL_UP);
665 actuator_state[i] = act_pprz + act_dyn_discrete[i] * (u_pprz_cmd - act_pprz);
666 actuator_state[i] = pprz_cmd_to_u_squared(actuator_state[i], SERVO_MOTOR_BACK_MAX, SERVO_MOTOR_BACK_NEUTRAL, SERVO_MOTOR_BACK_TRAVEL_UP);
667 break;
668 case COMMAND_MOTOR_LEFT:
669 u_pprz_cmd = u_squared_to_pprz_cmd(indi_u[i], SERVO_MOTOR_LEFT_MAX, SERVO_MOTOR_LEFT_NEUTRAL, SERVO_MOTOR_LEFT_TRAVEL_UP);
670 act_pprz = u_squared_to_pprz_cmd(actuator_state[i], SERVO_MOTOR_LEFT_MAX, SERVO_MOTOR_LEFT_NEUTRAL, SERVO_MOTOR_LEFT_TRAVEL_UP);
671 actuator_state[i] = act_pprz + act_dyn_discrete[i] * (u_pprz_cmd - act_pprz);
672 actuator_state[i] = pprz_cmd_to_u_squared(actuator_state[i], SERVO_MOTOR_LEFT_MAX, SERVO_MOTOR_LEFT_NEUTRAL, SERVO_MOTOR_LEFT_TRAVEL_UP);
673 break;
674 case COMMAND_THRUST_X:
675 u_pprz_cmd = u_squared_to_pprz_cmd(indi_u[i], SERVO_MOTOR_PUSH_MAX, SERVO_MOTOR_PUSH_NEUTRAL, SERVO_MOTOR_PUSH_TRAVEL_UP);
676 act_pprz = u_squared_to_pprz_cmd(actuator_state[i], SERVO_MOTOR_PUSH_MAX, SERVO_MOTOR_PUSH_NEUTRAL, SERVO_MOTOR_PUSH_TRAVEL_UP);
677 actuator_state[i] = act_pprz + act_dyn_discrete[i] * (u_pprz_cmd - act_pprz);
678 actuator_state[i] = pprz_cmd_to_u_squared(actuator_state[i], SERVO_MOTOR_PUSH_MAX, SERVO_MOTOR_PUSH_NEUTRAL, SERVO_MOTOR_PUSH_TRAVEL_UP);
679 break;
680 default:
682 break;
683 }
684 }
685}
686
688{
691 bool* act_is_thruster_z = stabilization_indi_get_act_is_thruster_z();
693
694 float u_pprz_cmd;
695 float act_state_pprz;
696
697 cmd[COMMAND_THRUST] = 0;
698
699 for (uint8_t i = 0; i < INDI_NUM_ACT; i++) {
700 switch(i) {
701 // With RPM control we have to do a conversion step from allocation back to actuator state
702 case COMMAND_MOTOR_FRONT:
703 u_pprz_cmd = u_squared_to_pprz_cmd(indi_u[i], SERVO_MOTOR_FRONT_MAX, SERVO_MOTOR_FRONT_NEUTRAL, SERVO_MOTOR_FRONT_TRAVEL_UP);
704 act_state_pprz = u_squared_to_pprz_cmd(actuator_state[i], SERVO_MOTOR_FRONT_MAX, SERVO_MOTOR_FRONT_NEUTRAL, SERVO_MOTOR_FRONT_TRAVEL_UP);
705 break;
706 case COMMAND_MOTOR_RIGHT:
707 u_pprz_cmd = u_squared_to_pprz_cmd(indi_u[i], SERVO_MOTOR_RIGHT_MAX, SERVO_MOTOR_RIGHT_NEUTRAL, SERVO_MOTOR_RIGHT_TRAVEL_UP);
708 act_state_pprz = u_squared_to_pprz_cmd(actuator_state[i], SERVO_MOTOR_RIGHT_MAX, SERVO_MOTOR_RIGHT_NEUTRAL, SERVO_MOTOR_RIGHT_TRAVEL_UP);
709 break;
710 case COMMAND_MOTOR_BACK:
711 u_pprz_cmd = u_squared_to_pprz_cmd(indi_u[i], SERVO_MOTOR_BACK_MAX, SERVO_MOTOR_BACK_NEUTRAL, SERVO_MOTOR_BACK_TRAVEL_UP);
712 act_state_pprz = u_squared_to_pprz_cmd(actuator_state[i], SERVO_MOTOR_BACK_MAX, SERVO_MOTOR_BACK_NEUTRAL, SERVO_MOTOR_BACK_TRAVEL_UP);
713 break;
714 case COMMAND_MOTOR_LEFT:
715 u_pprz_cmd = u_squared_to_pprz_cmd(indi_u[i], SERVO_MOTOR_LEFT_MAX, SERVO_MOTOR_LEFT_NEUTRAL, SERVO_MOTOR_LEFT_TRAVEL_UP);
716 act_state_pprz = u_squared_to_pprz_cmd(actuator_state[i], SERVO_MOTOR_LEFT_MAX, SERVO_MOTOR_LEFT_NEUTRAL, SERVO_MOTOR_LEFT_TRAVEL_UP);
717 break;
718 case COMMAND_THRUST_X:
719 u_pprz_cmd = u_squared_to_pprz_cmd(indi_u[i], SERVO_MOTOR_PUSH_MAX, SERVO_MOTOR_PUSH_NEUTRAL, SERVO_MOTOR_PUSH_TRAVEL_UP);
720 act_state_pprz = u_squared_to_pprz_cmd(actuator_state[i], SERVO_MOTOR_PUSH_MAX, SERVO_MOTOR_PUSH_NEUTRAL, SERVO_MOTOR_PUSH_TRAVEL_UP);
721 break;
722 default:
723 u_pprz_cmd = indi_u[i];
724 act_state_pprz = actuator_state[i];
725 break;
726 }
727
728 actuators_pprz[i] = u_pprz_cmd;
729
730 if (act_to_commands != NULL) {
731 cmd[act_to_commands[i]] = actuators_pprz[i];
732 }
733
734 cmd[COMMAND_THRUST] += act_state_pprz * (int32_t) act_is_thruster_z[i];
735 }
736 cmd[COMMAND_THRUST] /= stabilization_indi_get_num_thrusters();
737}
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)
Hardware independent code for commands handling.
#define RW_aZ
#define RW_aX
#define RW_ap
#define RW_ar
#define RW_aq
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
#define WING_ROTATION_CAN_ROTWING_ID
ABI binding wing position data.
float roll_eff_slider
int32_t rw_flap_offset
#define AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT
struct rotwing_eff_sched_param_t eff_sched_p
static abi_event wing_position_ev
float eff_sched_pusher_time
static void wing_position_cb(uint8_t sender_id UNUSED, struct act_feedback_t *pos_msg, uint8_t num_act)
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)
static float u_squared_to_pprz_cmd(float u_squared, float max, float neutral, float travel_up)
Convert u^2 to PPRZ command.
void eff_scheduling_rotwing_update_MMOI(void)
void eff_scheduling_rotwing_update_airspeed(void)
void stabilization_indi_get_actuator_state(void)
Function that tries to get actuator feedback.
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)
void eff_scheduling_rotwing_update_wing_angle(void)
static float pprz_cmd_to_u_squared(float pprz_cmd, float max, float neutral, float travel_up)
Convert PPRZ command back to u^2.
void eff_scheduling_rotwing_update_hover_motor_effectiveness(void)
void eff_scheduling_rotwing_init(void)
void eff_scheduling_rotwing_update_elevator_effectiveness(void)
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)
void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle)
#define ROTWING_PITCH_MOTOR_TILT_ANGLE_DEG
void stabilization_indi_commit_actuator_cmd(int32_t *cmd)
struct Electrical electrical
Definition electrical.c:92
#define Min(x, y)
Definition esc_dshot.c:110
void float_eulers_of_quat_zxy(struct FloatEulers *e, const struct FloatQuat *q)
euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch
euler angles
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition state.h:1313
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
struct FloatEulers eulers_zxy
state eulers in zxy order
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
float actuator_state[ANDI_NUM_ACT]
struct WLS_t wls_stab_p
float act_pref[INDI_NUM_ACT]
float * stabilization_indi_get_act_state(void)
bool act_is_servo[INDI_NUM_ACT]
float g1g2[INDI_OUTPUTS][INDI_NUM_ACT]
bool * stabilization_indi_get_act_is_thruster_z(void)
float * stabilization_indi_get_act_dyn(void)
float actuator_state_filt_vect[INDI_NUM_ACT]
float act_dyn_discrete[INDI_NUM_ACT]
uint8_t * stabilization_indi_get_act_to_commands(void)
int32_t stabilization_indi_get_num_thrusters(void)
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...
float indi_u[INDI_NUM_ACT]
float * stabilization_indi_get_indi_u(void)
API to get/set the generic vehicle states.
int int32_t
Typedef defining 32 bit int type.
short int16_t
Typedef defining 16 bit short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.