Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
rotwing_state.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
30#include "modules/core/abi.h"
31#include "generated/airframe.h"
32
33#define ASSERT_EQUIVALENCE(primary, backup) \
34 _Static_assert(SERVO_##primary##_MIN == SERVO_##backup##_MIN, \
35 #primary " and " #backup " must have same MIN value"); \
36 _Static_assert(SERVO_##primary##_NEUTRAL == SERVO_##backup##_NEUTRAL, \
37 #primary " and " #backup " must have same NEUTRAL value"); \
38 _Static_assert(SERVO_##primary##_MAX == SERVO_##backup##_MAX, \
39 #primary " and " #backup " must have same MAX value")
40
45
46#ifdef SERVO_BROTATION_MECH_IDX
47ASSERT_EQUIVALENCE(ROTATION_MECH, BROTATION_MECH);
48#endif
49
50/* Minimum measured RPM to consider the hover motors running (RPM) */
51#ifndef ROTWING_QUAD_MIN_RPM
52#define ROTWING_QUAD_MIN_RPM 400
53#endif
54
55/* Minimum measured RPM to consider the pusher motor running (RPM) */
56#ifndef ROTWING_PUSH_MIN_RPM
57#define ROTWING_PUSH_MIN_RPM 300
58#endif
59
60/* Timeout for the RPM feedback (seconds) */
61#ifndef ROTWING_RPM_TIMEOUT
62#define ROTWING_RPM_TIMEOUT 1.0
63#endif
64
65/* Minimum thrust to consider the hover motors idling (PPRZ) */
66#ifndef ROTWING_QUAD_IDLE_MIN_THRUST
67#define ROTWING_QUAD_IDLE_MIN_THRUST 100
68#endif
69
70/* Minimum time for the hover motors to be considered idling (seconds) */
71#ifndef ROTWING_QUAD_IDLE_TIMEOUT
72#define ROTWING_QUAD_IDLE_TIMEOUT 0.4
73#endif
74
75/* Minimum measured skew angle to consider the rotating wing in fixedwing (deg) */
76#ifndef ROTWING_FW_SKEW_ANGLE
77#define ROTWING_FW_SKEW_ANGLE 85.0
78#endif
79
80/* Maximum measured skew angle to consider the rotating wing drone in quad (deg) */
81#ifndef ROTWING_QUAD_SKEW_ANGLE
82#define ROTWING_QUAD_SKEW_ANGLE 25.0
83#endif
84
85/* Maximum bank angle while the rotwing is skewing */
86#ifndef ROTWING_TRANSITION_MAX_BANK
87#define ROTWING_TRANSITION_MAX_BANK RadOfDeg(20)
88#endif
89
90/* Maximum climb speed while the rotwing is transitioning */
91#ifndef ROTWING_TRANSITION_MAX_CLIMB_SPEED
92#define ROTWING_TRANSITION_MAX_CLIMB_SPEED 0.5
93#endif
94
95/* Maximum descend speed while the rotwing is transitioning */
96#ifndef ROTWING_TRANSITION_MAX_DESCEND_SPEED
97#define ROTWING_TRANSITION_MAX_DESCEND_SPEED -0.5
98#endif
99
100/* TODO: Give a name.... */
101#ifndef ROTWING_SKEW_BACK_MARGIN
102#define ROTWING_SKEW_BACK_MARGIN 5.0
103#endif
104
105/* Maximum angle difference between the measured skew and modelled skew for skew failure detection */
106#ifndef ROTWING_SKEW_REF_MODEL_MAX_DIFF
107#define ROTWING_SKEW_REF_MODEL_MAX_DIFF 5.f
108#endif
109
110/* Skew angle at which the mininum airspeed starts its linear portion */
111#ifndef ROTWING_MIN_AIRSPEED_SLOPE_START_ANGLE
112#define ROTWING_MIN_AIRSPEED_SLOPE_START_ANGLE 30.0
113#endif
114
115/* Amount of time the airspeed needs to be below the FW_MIN_AIRSPEED */
116#ifndef ROTWING_FW_STALL_TIMEOUT
117#define ROTWING_FW_STALL_TIMEOUT 0.5
118#endif
119
120#ifndef ROTWING_STATE_MIN_FW_DIST
121#define ROTWING_STATE_MIN_FW_DIST 200.0
122#endif
123
124/* Sweep of the forced skew angle: lowest and highest skew angle (deg) and frequency (Hz) */
125#ifndef ROTWING_SKEW_SWEEP_MIN
126#define ROTWING_SKEW_SWEEP_MIN 0.f
127#endif
128
129#ifndef ROTWING_SKEW_SWEEP_MAX
130#define ROTWING_SKEW_SWEEP_MAX 60.f
131#endif
132
133#ifndef ROTWING_SKEW_SWEEP_FREQ
134#define ROTWING_SKEW_SWEEP_FREQ 0.1f
135#endif
136
137/* Fix for not having double can busses */
138#ifndef SERVO_BMOTOR_PUSH_IDX
139#define SERVO_BMOTOR_PUSH_IDX SERVO_MOTOR_PUSH_IDX
140#endif
141
142/* Fix for not having double can busses */
143#ifndef SERVO_BROTATION_MECH_IDX
144#define SERVO_BROTATION_MECH_IDX SERVO_ROTATION_MECH_IDX
145#endif
146
148#ifndef ROTWING_STATE_ACT_FEEDBACK_ID
149#define ROTWING_STATE_ACT_FEEDBACK_ID ABI_BROADCAST
150#endif
151
155#ifndef GUIDANCE_INDI_FWD_CLIMB_SPEED
156#define GUIDANCE_INDI_FWD_CLIMB_SPEED 4.0
157#endif
158
162#ifndef GUIDANCE_INDI_FWD_DESCEND_SPEED
163#define GUIDANCE_INDI_FWD_DESCEND_SPEED -4.0
164#endif
165
169#ifndef GUIDANCE_INDI_QUAD_CLIMB_SPEED
170#define GUIDANCE_INDI_QUAD_CLIMB_SPEED 2.0
171#endif
172
176#ifndef GUIDANCE_INDI_QUAD_DESCEND_SPEED
177#define GUIDANCE_INDI_QUAD_DESCEND_SPEED -2.0
178#endif
179
181static void rotwing_state_feedback_cb(uint8_t sender_id, struct act_feedback_t *feedback_msg, uint8_t num_act);
182static void rotwing_state_skew_sweep(bool run, float time);
184#if PERIODIC_TELEMETRY
186static void send_rotating_wing_state(struct transport_tx *trans, struct link_device *dev)
187{
188 // Set the status
190 status.value = 0;
191 status.skew_angle_valid = rotwing_state_skew_angle_valid();
192 status.hover_motors_enabled = rotwing_state.hover_motors_enabled;
193 status.hover_motors_idle = rotwing_state_hover_motors_idling();
194 status.hover_motors_running = rotwing_state_hover_motors_running();
195 status.pusher_motor_running = rotwing_state_pusher_motor_running();
196 status.skew_forced = rotwing_state.force_skew;
197
198 // Send the message
200 uint8_t nav_state = rotwing_state.nav_state;
201 pprz_msg_send_ROTATING_WING_STATE(trans, dev, AC_ID,
202 &state,
203 &nav_state,
204 &status.value,
205 &rotwing_state.meas_skew_angle_deg,
206 &rotwing_state.sp_skew_angle_deg,
208 &rotwing_state.min_airspeed,
209 &rotwing_state.max_airspeed);
210}
211#endif // PERIODIC_TELEMETRY
212
213#if PREFLIGHT_CHECKS
214/* Preflight checks */
216static struct preflight_check_t rotwing_state_skew_pfc;
217
218static void rotwing_state_skew_preflight(struct preflight_result_t *result) {
219 if(fabsf(rotwing_state.meas_skew_angle_deg - rotwing_state.sp_skew_angle_deg) < ROTWING_SKEW_REF_MODEL_MAX_DIFF) {
220 preflight_success(result, "Rotwing skew success");
221 } else {
222 preflight_error(result, "Rotwing skew error [meas: %.1f, sp: %.1f]", rotwing_state.meas_skew_angle_deg, rotwing_state.sp_skew_angle_deg);
223 }
224}
225#endif // PREFLIGHT_CHECKS
226
228{
229 // Initialize rotwing state
230 rotwing_state.state = ROTWING_STATE_FORCE_HOVER; // For takeoff
232 rotwing_state.hover_motors_enabled = true;
233 rotwing_state.fw_min_airspeed = ROTWING_FW_MIN_AIRSPEED;
234 rotwing_state.cruise_airspeed = ROTWING_FW_CRUISE_AIRSPEED;
235 rotwing_state.sp_skew_angle_deg = 0;
236 rotwing_state.meas_skew_angle_deg = 0;
237 rotwing_state.meas_skew_angle_time = 0;
238 rotwing_state.skew_cmd = 0;
239 rotwing_state.force_skew = false;
240 rotwing_state.skew_sweep.active = false;
241 rotwing_state.skew_sweep.min_deg = ROTWING_SKEW_SWEEP_MIN;
242 rotwing_state.skew_sweep.max_deg = ROTWING_SKEW_SWEEP_MAX;
243 rotwing_state.skew_sweep.freq = ROTWING_SKEW_SWEEP_FREQ;
244 for (int i = 0; i < 5; i++) {
245 rotwing_state.meas_rpm[i] = 0;
246 rotwing_state.meas_rpm_time[i] = 0;
247 }
248 rotwing_state.fail_skew_angle = false;
249 rotwing_state.fail_hover_motor = false;
250 rotwing_state.fail_pusher_motor = false;
251 rotwing_state.ref_model_skew_angle_deg = 0;
252 // Bind ABI messages
254
255#if PERIODIC_TELEMETRY
257#endif
258
259 /* Register preflight checks */
260#if PREFLIGHT_CHECKS
261 preflight_check_register(&rotwing_state_skew_pfc, rotwing_state_skew_preflight);
262#endif
263}
264
270 static float last_idle_time = 0;
271 // Check if hover motors are idling and reset timer
272 if(stabilization.cmd[COMMAND_THRUST] > ROTWING_QUAD_IDLE_MIN_THRUST) {
273 last_idle_time = get_sys_time_float();
274 }
275
276 // Check if we exceeded the idle timeout
277 if(get_sys_time_float() - last_idle_time > ROTWING_QUAD_IDLE_TIMEOUT) {
278 return true;
279 }
280 return false;
281}
282
288static void rotwing_state_skew_sweep(bool run, float time)
289{
290 static float start_time = -1.f;
291 if (!run) {
292 start_time = -1.f;
293 return;
294 }
295 if (start_time < 0.f) {
296 start_time = time;
297 }
298 float mid = (rotwing_state.skew_sweep.max_deg + rotwing_state.skew_sweep.min_deg) / 2.f;
299 float amplitude = (rotwing_state.skew_sweep.max_deg - rotwing_state.skew_sweep.min_deg) / 2.f;
300 rotwing_state.sp_skew_angle_deg = mid - amplitude * cosf(2.f * M_PI * rotwing_state.skew_sweep.freq * (time - start_time));
301}
302
304{
305 /* Get some genericly used variables */
306 static float last_stall_time = 0;
307 float current_time = get_sys_time_float();
308 float meas_airspeed = stateGetAirspeed_f();
309 float meas_skew_angle = rotwing_state.meas_skew_angle_deg;
310 Bound(meas_skew_angle, 0, 90); // Bound to prevent errors
311
312 if(meas_airspeed > rotwing_state.fw_min_airspeed) {
313 last_stall_time = current_time;
314 }
315
316 /* Forward flight as a fixed wing for the estimators (e.g. airspeed and sideslip fusion of ins_ekf2):
317 * the wing is skewed for forward flight and the pusher motor runs */
320
321 /* Override modes if flying with RC */
322 rotwing_state.state = rotwing_state.nav_state;
323 if (autopilot.mode == AP_MODE_FAILSAFE) {
325 }
326 else if (guidance_h.mode == GUIDANCE_H_MODE_NONE) {
327 // Kill mode
330 }
331 // ATT and ATT_FWD
335 } else {
337 }
338 }
339 }
340
341 /* Handle the quad motors on/off state */
343 rotwing_state.hover_motors_enabled = true;
344 }
345 else if((current_time - last_stall_time) < ROTWING_FW_STALL_TIMEOUT && rotwing_state_hover_motors_idling() && rotwing_state_pusher_motor_running() && meas_skew_angle >= ROTWING_FW_SKEW_ANGLE
346 && (gi_unbounded_airspeed_sp >= rotwing_state.fw_min_airspeed || rotwing_state.state != ROTWING_STATE_FREE)) {
347 rotwing_state.hover_motors_enabled = false;
348 }
349 else {
350 rotwing_state.hover_motors_enabled = true;
351 }
352
353
354 /* Calculate min/max airspeed bounds based on skew angle */
355 float skew_min_airspeed = ROTWING_FW_QUAD_MIN_AIRSPEED * (meas_skew_angle - ROTWING_MIN_AIRSPEED_SLOPE_START_ANGLE) / (90.f - ROTWING_MIN_AIRSPEED_SLOPE_START_ANGLE);
356 float skew_max_airspeed = ROTWING_QUAD_MAX_AIRSPEED + (ROTWING_FW_MAX_AIRSPEED - ROTWING_QUAD_MAX_AIRSPEED) * meas_skew_angle / ROTWING_FW_SKEW_ANGLE;
357 Bound(skew_min_airspeed, 0, rotwing_state.fw_min_airspeed);
358 Bound(skew_max_airspeed, ROTWING_QUAD_MAX_AIRSPEED, ROTWING_FW_MAX_AIRSPEED);
359
360 if(!rotwing_state_hover_motors_running() || !rotwing_state.hover_motors_enabled) {
361 skew_min_airspeed = rotwing_state.fw_min_airspeed;
362 skew_max_airspeed = ROTWING_FW_MAX_AIRSPEED;
363 }
364
365
366 /* Handle the skew angle setpoint */
367 rotwing_state_skew_sweep(rotwing_state.force_skew && rotwing_state.skew_sweep.active, current_time);
368 if (rotwing_state.force_skew) {
369 // Set by the GCS (or by the skew sweep)
370 }
371 else if(rotwing_state.state == ROTWING_STATE_FORCE_HOVER) {
372 rotwing_state.sp_skew_angle_deg = 0.f;
373 }
374 else if(!rotwing_state_hover_motors_running() || !rotwing_state.hover_motors_enabled || rotwing_state.state == ROTWING_STATE_FORCE_FW) {
375 rotwing_state.sp_skew_angle_deg = 90.f;
376 }
378 rotwing_state.sp_skew_angle_deg = 0.f;
379 }
381 rotwing_state.sp_skew_angle_deg = 0.f;
382 }
383 else {
384 // SKEWING function based on Vair
385 if (meas_airspeed < ROTWING_SKEW_DOWN_AIRSPEED) {
386 rotwing_state.sp_skew_angle_deg = 0.f;
387 } else if (meas_airspeed < ROTWING_SKEW_UP_AIRSPEED) {
388 // Hysteresis do nothing
389 } else if (meas_airspeed < ROTWING_QUAD_MAX_AIRSPEED) {
390 rotwing_state.sp_skew_angle_deg = ROTWING_SKEW_ANGLE_STEP;
391 } else {
392 rotwing_state.sp_skew_angle_deg = ((meas_airspeed - ROTWING_QUAD_MAX_AIRSPEED)) / (rotwing_state.fw_min_airspeed - ROTWING_QUAD_MAX_AIRSPEED) * (90.f - ROTWING_SKEW_ANGLE_STEP) + ROTWING_SKEW_ANGLE_STEP;
393 }
394 }
395 Bound(rotwing_state.sp_skew_angle_deg, 0.f, 90.f);
396
397
398 /* Handle the airspeed bounding */
399 Bound(rotwing_state.cruise_airspeed, rotwing_state.fw_min_airspeed, ROTWING_FW_MAX_AIRSPEED);
401 rotwing_state.min_airspeed = rotwing_state.cruise_airspeed;
402 rotwing_state.max_airspeed = rotwing_state.cruise_airspeed;
403 }
405 rotwing_state.min_airspeed = 0;
406 rotwing_state.max_airspeed = ROTWING_QUAD_NOPUSH_AIRSPEED;
407 }
409 rotwing_state.min_airspeed = skew_min_airspeed;
410 rotwing_state.max_airspeed = fmax(ROTWING_QUAD_MAX_AIRSPEED, skew_min_airspeed);
411 }
412 else if(rotwing_state.state == ROTWING_STATE_REQUEST_FW) {
413 rotwing_state.min_airspeed = fmin(rotwing_state.cruise_airspeed, skew_max_airspeed);
414 rotwing_state.max_airspeed = fmin(rotwing_state.cruise_airspeed, skew_max_airspeed);
415 }
416 else{
417 rotwing_state.min_airspeed = skew_min_airspeed;
418 rotwing_state.max_airspeed = skew_max_airspeed;
419 }
420
421 /* Bound max bank angle, climb speed and descend speed if the rotwing drone is transitioning and not in FREE mode */
422 if (meas_skew_angle < ROTWING_FW_SKEW_ANGLE && meas_skew_angle > ROTWING_QUAD_SKEW_ANGLE
423 && rotwing_state.state != ROTWING_STATE_FREE) {
427 } else {
431 }
432
433 // Override failing skewing while fwd
434 /*if(meas_skew_angle > 70 && rotwing_state_.skewing_failing) {
435 rotwing_state.min_airspeed = 0; // Vstall + margin
436 rotwing_state.max_airspeed = 0; // Max airspeed FW
437 }*/
438
440
441 /* Set navigation/guidance settings */
442 nav_max_deceleration_sp = ROTWING_FW_MAX_DECELERATION * meas_skew_angle / 90.f + ROTWING_QUAD_MAX_DECELERATION * (90.f - meas_skew_angle) / 90.f; //TODO: Do we really want to based this on the skew?
443
444
445 /* Calculate the skew command */
446 float servo_pprz_cmd = MAX_PPRZ * (rotwing_state.sp_skew_angle_deg - 45.f) / 45.f;
447 BoundAbs(servo_pprz_cmd, MAX_PPRZ);
448
449 // Rotate with second order filter
450 static float rotwing_state_skew_p_cmd = -MAX_PPRZ;
451 static float rotwing_state_skew_d_cmd = 0;
452
453 float speed_sp = ROTWING_SKEW_REF_MODEL_P_GAIN * (servo_pprz_cmd - rotwing_state_skew_p_cmd);
454 BoundAbs(speed_sp, ROTWING_SKEW_REF_MODEL_MAX_SPEED);
455 rotwing_state_skew_d_cmd += ROTWING_SKEW_REF_MODEL_D_GAIN * (speed_sp - rotwing_state_skew_d_cmd);
456 rotwing_state_skew_p_cmd += rotwing_state_skew_d_cmd;
457 BoundAbs(rotwing_state_skew_p_cmd, MAX_PPRZ);
458 rotwing_state.ref_model_skew_angle_deg = 45.0 / MAX_PPRZ * rotwing_state_skew_p_cmd + 45.0;
459
460#if (ROTWING_SKEW_REF_MODEL || USE_NPS)
461 rotwing_state.skew_cmd = rotwing_state_skew_p_cmd;
462#else
463 rotwing_state.skew_cmd = servo_pprz_cmd;
464#endif
465#ifdef COMMAND_ROT_MECH
466 commands[COMMAND_ROT_MECH] = rotwing_state.skew_cmd;
467#endif
468
469 /* Add simulation feedback for the skewing and RPM */
470#if USE_NPS
471 // Export to the index of the SKEW in the NPS_ACTUATOR_NAMES array
472#ifdef COMMAND_ROT_MECH
473 commands[COMMAND_ROT_MECH] = (rotwing_state.skew_cmd + MAX_PPRZ) / 2.f; // Scale to simulation command
474#else
475 actuators_pprz[INDI_NUM_ACT] = (rotwing_state.skew_cmd + MAX_PPRZ) / 2.f; // Scale to simulation command
476#endif
477 // SEND ABI Message to ctr_eff_sched, ourself and other modules that want Actuator position feedback
478 struct act_feedback_t feedback = {0};
479 feedback.idx = SERVO_ROTATION_MECH_IDX;
480 feedback.position = 0.5f * M_PI - RadOfDeg((float) rotwing_state.skew_cmd / MAX_PPRZ * 45.f + 45.f);
481 feedback.set.position = true;
482
483 // Send ABI message (or simulate failure)
484 if(!rotwing_state.fail_skew_angle) {
485 AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_BOARD_ID, &feedback, 1);
486 }
487
488 // Simulate to always have RPM if on and active feedback
489 rotwing_state.meas_rpm[0] = (actuators[SERVO_MOTOR_FRONT_IDX].pprz_val >= 0)? (ROTWING_QUAD_MIN_RPM + 100) : 0;
490 rotwing_state.meas_rpm[1] = (actuators[SERVO_MOTOR_RIGHT_IDX].pprz_val >= 0)? (ROTWING_QUAD_MIN_RPM + 100) : 0;
491 rotwing_state.meas_rpm[2] = (actuators[SERVO_MOTOR_BACK_IDX].pprz_val >= 0)? (ROTWING_QUAD_MIN_RPM + 100) : 0;
492 rotwing_state.meas_rpm[3] = (actuators[SERVO_MOTOR_LEFT_IDX].pprz_val >= 0)? (ROTWING_QUAD_MIN_RPM + 100) : 0;
493 rotwing_state.meas_rpm[4] = (actuators[SERVO_MOTOR_PUSH_IDX].pprz_val >= 0)? (ROTWING_PUSH_MIN_RPM + 100) : 0;
494 for(uint8_t i = 0; i < 5; i++) {
495 rotwing_state.meas_rpm_time[i] = current_time;
496 }
497
498#ifdef SITL
499 if(rotwing_state.fail_hover_motor) {
500 rotwing_state.meas_rpm[0] = 0;
501 rotwing_state.meas_rpm[1] = 0;
502 rotwing_state.meas_rpm[2] = 0;
503 rotwing_state.meas_rpm[3] = 0;
504 }
505
506 if(rotwing_state.fail_pusher_motor) {
507 rotwing_state.meas_rpm[4] = 0;
508 }
509#endif
510
511#endif
512}
513
514static void rotwing_state_feedback_cb(uint8_t __attribute__((unused)) sender_id,
515 struct act_feedback_t UNUSED *feedback_msg, uint8_t UNUSED num_act_message)
516{
517 float current_time = get_sys_time_float();
518 for (int i = 0; i < num_act_message; i++) {
519 int idx = feedback_msg[i].idx;
520
521 // Check for wing rotation feedback
522 if ((feedback_msg[i].set.position) && (idx == SERVO_ROTATION_MECH_IDX || idx == SERVO_BROTATION_MECH_IDX)) {
523 // Get wing rotation angle from sensor
524 float skew_angle_rad = 0.5 * M_PI - feedback_msg[i].position;
525 rotwing_state.meas_skew_angle_deg = DegOfRad(skew_angle_rad);
526 rotwing_state.meas_skew_angle_time = current_time;
527 }
528
529 // Get the RPM feedbacks of the motors
530 if (feedback_msg[i].set.rpm) {
531 if ((idx == SERVO_MOTOR_FRONT_IDX) || (idx == SERVO_BMOTOR_FRONT_IDX)) {
532 rotwing_state.meas_rpm[0] = feedback_msg->rpm;
533 rotwing_state.meas_rpm_time[0] = current_time;
534 } else if ((idx == SERVO_MOTOR_RIGHT_IDX) || (idx == SERVO_BMOTOR_RIGHT_IDX)) {
535 rotwing_state.meas_rpm[1] = feedback_msg->rpm;
536 rotwing_state.meas_rpm_time[1] = current_time;
537 } else if ((idx == SERVO_MOTOR_BACK_IDX) || (idx == SERVO_BMOTOR_BACK_IDX)) {
538 rotwing_state.meas_rpm[2] = feedback_msg->rpm;
539 rotwing_state.meas_rpm_time[2] = current_time;
540 } else if ((idx == SERVO_MOTOR_LEFT_IDX) || (idx == SERVO_BMOTOR_LEFT_IDX)) {
541 rotwing_state.meas_rpm[3] = feedback_msg->rpm;
542 rotwing_state.meas_rpm_time[3] = current_time;
543 } else if ((idx == SERVO_MOTOR_PUSH_IDX) || (idx == SERVO_BMOTOR_PUSH_IDX)) {
544 rotwing_state.meas_rpm[4] = feedback_msg->rpm;
545 rotwing_state.meas_rpm_time[4] = current_time;
546 }
547 }
548 }
549}
550
552 float current_time = get_sys_time_float();
553 // Check if hover motors are running
554 if (rotwing_state.meas_rpm[0] > ROTWING_QUAD_MIN_RPM
555 && rotwing_state.meas_rpm[1] > ROTWING_QUAD_MIN_RPM
556 && rotwing_state.meas_rpm[2] > ROTWING_QUAD_MIN_RPM
557 && rotwing_state.meas_rpm[3] > ROTWING_QUAD_MIN_RPM
558 && (current_time - rotwing_state.meas_rpm_time[0]) < ROTWING_RPM_TIMEOUT
559 && (current_time - rotwing_state.meas_rpm_time[1]) < ROTWING_RPM_TIMEOUT
560 && (current_time - rotwing_state.meas_rpm_time[2]) < ROTWING_RPM_TIMEOUT
561 && (current_time - rotwing_state.meas_rpm_time[3]) < ROTWING_RPM_TIMEOUT) {
562 return true;
563 } else {
564 return false;
565 }
566}
567
569 // Check if pusher motor is running
570 if (rotwing_state.meas_rpm[4] > ROTWING_PUSH_MIN_RPM && (get_sys_time_float() - rotwing_state.meas_rpm_time[4]) < ROTWING_RPM_TIMEOUT) {
571 return true;
572 } else {
573 return false;
574 }
575}
576
578 // Check if the measured skew angle is timed out and the reference model is matching
579 bool skew_timeout = (get_sys_time_float() - rotwing_state.meas_skew_angle_time) > ROTWING_RPM_TIMEOUT;
580 bool skew_angle_match = fabs(rotwing_state.meas_skew_angle_deg - rotwing_state.ref_model_skew_angle_deg) < ROTWING_SKEW_REF_MODEL_MAX_DIFF;
581
582 return (!skew_timeout && skew_angle_match);
583}
584
586 rotwing_state.nav_state = state;
587}
588
589
590/* TODO move these rotwing navigation helper functions to an appropriate module/file */
592 // Get circle waypoint coordinates in NED
593 struct FloatVect3 circ_ned = {.x = waypoints[wp_id].enu_f.y,
594 .y = waypoints[wp_id].enu_f.x,
595 .z = -waypoints[wp_id].enu_f.z};
596
597 // Get drone position coordinates in NED
598 struct FloatVect3 pos_ned = {.x = stateGetPositionNed_f()->x,
599 .y = stateGetPositionNed_f()->y,
600 .z = stateGetPositionNed_f()->z};
601
602 // Get vector pointing from drone to waypoint
603 struct FloatVect3 vect_pos_circ;
604 VECT3_DIFF(vect_pos_circ, circ_ned, pos_ned);
605
606 static struct FloatVect3 x_axis = {.x = 1, .y = 0, .z = 0};
607 static struct FloatVect3 z_axis = {.x = 0, .y = 0, .z = 1};
608 struct FloatVect3 att_NED;
609 struct FloatVect3 cross_att_circ;
610
612
613 VECT3_CROSS_PRODUCT(cross_att_circ, vect_pos_circ, att_NED);
614 float y = VECT3_DOT_PRODUCT(cross_att_circ, z_axis);
615 float x = VECT3_DOT_PRODUCT(vect_pos_circ, att_NED);
616
617 float body_to_wp_angle_rad = atan2f(y, x);
618
619 if (body_to_wp_angle_rad >= 0.f) {
620 return true; // CCW circle
621 } else {
622 return false; // CW circle
623 }
624}
625
627 struct EnuCoor_f wp = waypoints[wp_id].enu_f;
628 float dist2_to_wp = get_dist2_to_point(&wp);
629 float dist_to_wp = sqrtf(dist2_to_wp);
630
631 if (autopilot.mode == AP_MODE_NAV) {
632 if (dist_to_wp > ROTWING_STATE_MIN_FW_DIST) {
634 return true; // Necessary for flight plan
635 } else {
637 return false; // Necessary for flight plan
638 }
639 }
640
641 return false; // Necessary for flight plan
642}
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
Definition abi_common.h:101
#define ACT_FEEDBACK_BOARD_ID
struct pprz_autopilot autopilot
Global autopilot structure.
Definition autopilot.c:50
static uint8_t status
#define UNUSED(x)
Hardware independent code for commands handling.
float dist2_to_wp
Definition common_nav.c:39
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition common_nav.c:44
static struct uart_periph * dev
int start_time
Definition gps_sirf.c:192
void float_rmat_transp_vmult(struct FloatVect3 *vb, const struct FloatRMat *m_b2a, const struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
#define VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
#define VECT3_DIFF(_c, _a, _b)
#define VECT3_DOT_PRODUCT(_v1, _v2)
static struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
Definition state.h:1319
static void stateSetFixedWingFlight(bool fixed_wing)
Set if flying as a fixed wing (e.g. a hybrid in forward flight).
Definition state.h:1647
struct State state
Definition state.c:36
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition state.h:850
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition state.h:1615
struct FloatVect3 speed_sp
#define GUIDANCE_INDI_QUAD_DESCEND_SPEED
Descend speed when navigation is doing direct lines.
void guidance_set_max_bank_angle(float max_bank)
#define GUIDANCE_INDI_FWD_DESCEND_SPEED
Descend speed when navigation is making turns instead of direct lines.
void guidance_set_max_climb_speed(float max_climb_speed_quad, float max_climb_speed_fwd)
#define GUIDANCE_INDI_QUAD_CLIMB_SPEED
Climb speed when navigation is doing direct lines.
#define GUIDANCE_INDI_FWD_CLIMB_SPEED
Climb speed when navigation is making turns instead of direct lines.
void guidance_set_min_max_airspeed(float min_airspeed, float max_airspeed)
void guidance_set_max_descend_speed(float max_descend_speed_quad, float max_descend_speed_fwd)
float gi_unbounded_airspeed_sp
Hardware independent API for actuators (servos, motor controllers).
bool position
Position is set.
Definition actuators.h:48
struct act_feedback_t::act_feedback_set_t set
Bitset registering what is set as feedback.
uint8_t idx
General index of the actuators (generated in airframe.h).
Definition actuators.h:45
float position
In radians.
Definition actuators.h:55
#define MOTOR_LEFT
#define MOTOR_BACK
#define MOTOR_FRONT
#define MOTOR_RIGHT
float nav_max_deceleration_sp
static uint32_t idx
#define MAX_PPRZ
Definition paparazzi.h:8
float z
in meters
float x
in meters
float y
in meters
vector in East North Up coordinates Units: meters
void preflight_error(struct preflight_result_t *result, const char *fmt,...)
Register a preflight error used inside the preflight checking functions.
void preflight_success(struct preflight_result_t *result, const char *fmt,...)
Register a preflight success used inside the preflight checking functions.
void preflight_check_register(struct preflight_check_t *check, preflight_check_f func)
Register a preflight check and add it to the linked list.
Rotorcraft specific autopilot interface and initialization.
#define AP_MODE_NAV
#define AP_MODE_FAILSAFE
struct HorizontalGuidance guidance_h
Definition guidance_h.c:45
#define GUIDANCE_H_MODE_NONE
Definition guidance_h.h:56
#define GUIDANCE_H_MAX_BANK
Max bank controlled by guidance.
Definition guidance_h.h:64
float get_dist2_to_point(struct EnuCoor_f *p)
Returns squared horizontal distance to given point.
Definition navigation.c:308
bool rotwing_state_pusher_motor_running(void)
static void rotwing_state_feedback_cb(uint8_t sender_id, struct act_feedback_t *feedback_msg, uint8_t num_act)
#define ROTWING_RPM_TIMEOUT
#define ROTWING_SKEW_REF_MODEL_MAX_DIFF
bool rotwing_state_hover_motors_running(void)
#define ROTWING_FW_STALL_TIMEOUT
#define ROTWING_SKEW_SWEEP_MAX
#define ROTWING_FW_SKEW_ANGLE
#define ROTWING_TRANSITION_MAX_BANK
#define ROTWING_MIN_AIRSPEED_SLOPE_START_ANGLE
static void rotwing_state_skew_sweep(bool run, float time)
Sweep the skew angle setpoint between skew_sweep.min_deg and skew_sweep.max_deg at skew_sweep....
#define ROTWING_STATE_MIN_FW_DIST
void rotwing_state_init(void)
#define ROTWING_PUSH_MIN_RPM
#define ROTWING_TRANSITION_MAX_DESCEND_SPEED
#define ROTWING_TRANSITION_MAX_CLIMB_SPEED
bool rotwing_state_hover_motors_idling(void)
Check if hover motors are idling (COMMAND_THRUST < ROTWING_QUAD_IDLE_MIN_THRUST) for ROTWING_QUAD_IDL...
#define ROTWING_SKEW_BACK_MARGIN
#define SERVO_BMOTOR_PUSH_IDX
#define ROTWING_SKEW_SWEEP_FREQ
bool rotwing_state_skew_angle_valid(void)
#define ROTWING_SKEW_SWEEP_MIN
#define ROTWING_QUAD_IDLE_MIN_THRUST
#define SERVO_BROTATION_MECH_IDX
#define ROTWING_QUAD_SKEW_ANGLE
#define ASSERT_EQUIVALENCE(primary, backup)
#define ROTWING_QUAD_MIN_RPM
static void send_rotating_wing_state(struct transport_tx *trans, struct link_device *dev)
bool rotwing_state_choose_state_by_dist(uint8_t wp_id)
#define ROTWING_STATE_ACT_FEEDBACK_ID
ABI binding feedback data.
abi_event rotwing_state_feedback_ev
#define ROTWING_QUAD_IDLE_TIMEOUT
bool rotwing_state_choose_circle_direction(uint8_t wp_id)
struct rotwing_state_t rotwing_state
void rotwing_state_periodic(void)
void rotwing_state_set(enum rotwing_states_t state)
#define ROTWING_SKEW_ANGLE_STEP
rotwing_states_t
@ ROTWING_STATE_FREE
@ ROTWING_STATE_REQUEST_HOVER
@ ROTWING_STATE_FORCE_FW
@ ROTWING_STATE_FORCE_HOVER
@ ROTWING_STATE_REQUEST_FW
static const ShellCommand commands[]
Definition shell_arch.c:77
struct Stabilization stabilization
#define STABILIZATION_MODE_ATTITUDE
#define STABILIZATION_ATT_SUBMODE_FORWARD
#define STABILIZATION_MODE_NONE
Stabilization modes.
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...
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition sys_time.h:241
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition telemetry.h:66
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.