Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 
31 
33 #include "modules/core/abi.h"
34 
35 // Quad state identification
36 #ifndef ROTWING_MIN_SKEW_ANGLE_DEG_QUAD
37 #define ROTWING_MIN_SKEW_ANGLE_DEG_QUAD 10.0
38 #endif
39 #ifndef ROTWING_MIN_SKEW_ANGLE_COUNTER
40 #define ROTWING_MIN_SKEW_ANGLE_COUNTER 10 // Minimum number of loops the skew angle is below ROTWING_MIN_SKEW_ANGLE_COUNTER
41 #endif
42 
43 // Skewing state identification
44 #ifndef ROTWING_SKEWING_COUNTER
45 #define ROTWING_SKEWING_COUNTER 10 // Minimum number of loops the skew angle is in between QUAD and FW
46 #endif
47 
48 // maximum quad airspeed to force quad state
49 #ifndef ROTWING_MAX_QUAD_AIRSPEED
50 #define ROTWING_MAX_QUAD_AIRSPEED 20.0
51 #endif
52 
53 // Half skew state identification
54 #ifndef ROTWING_HALF_SKEW_ANGLE_DEG
55 #define ROTWING_HALF_SKEW_ANGLE_DEG 55.0
56 #endif
57 #ifndef ROTWING_HALF_SKEW_ANGLE_RANG
58 #define ROTWING_HALF_SKEW_ANGLE_HALF_RANGE 10.0
59 #endif
60 #ifndef ROTWING_HALF_SKEW_COUNTER
61 #define ROTWING_HALF_SKEW_COUNTER 10 // Minimum number of loops the skew angle is at HALF_SKEW_ANGLE_DEG +/- ROTWING_HALF_SKEW_ANGLE_HALF_RANGE to trigger ROTWING_HALF_SKEW_ANGLE state
62 #endif
63 
64 // FW state identification
65 #ifndef ROTWING_MIN_FW_SKEW_ANGLE_DEG
66 #define ROTWING_MIN_FW_SKEW_ANGLE_DEG 80.0 // Minimum wing angle to fly in fixed wing state
67 #endif
68 #ifndef ROTWING_MIN_FW_COUNTER
69 #define ROTWING_MIN_FW_COUNTER 10 // Minimum number of loops the skew angle is above the MIN_FW_SKEW_ANGLE
70 #endif
71 
72 // FW idle state identification
73 #ifndef ROTWING_MIN_THRUST_IDLE
74 #define ROTWING_MIN_THRUST_IDLE 100
75 #endif
76 #ifndef ROTWING_MIN_THRUST_IDLE_COUNTER
77 #define ROTWING_MIN_THRUST_IDLE_COUNTER 10
78 #endif
79 
80 // FW hov mot off state identification
81 #ifndef ROTWING_HOV_MOT_RUN_RPM_TH
82 #define ROTWING_HOV_MOT_RUN_RPM_TH 800
83 #endif
84 #ifndef ROTWING_HOV_MOT_OFF_RPM_TH
85 #define ROTWING_HOV_MOT_OFF_RPM_TH 50
86 #endif
87 #ifndef ROTWING_HOV_MOT_OFF_COUNTER
88 #define ROTWING_HOV_MOT_OFF_COUNTER 10
89 #endif
90 
91 #ifndef ROTWING_STATE_USE_ROTATION_REF_MODEL
92 #define ROTWING_STATE_USE_ROTATION_REF_MODEL FALSE
93 #endif
94 
95 
96 // Hover preferred pitch (deg)
97 #ifndef ROTWING_STATE_HOVER_PREF_PITCH
98 #define ROTWING_STATE_HOVER_PREF_PITCH 0.0
99 #endif
100 
101 // Transition preffered pitch (deg)
102 #ifndef ROTWING_STATE_TRANSITION_PREF_PITCH
103 #define ROTWING_STATE_TRANSITION_PREF_PITCH 3.0
104 #endif
105 
106 // Forward preffered pitch (deg)
107 #ifndef ROTWING_STATE_FW_PREF_PITCH
108 #define ROTWING_STATE_FW_PREF_PITCH 8.0
109 #endif
110 
111 // stream ADC data if ADC rotation sensor
112 #ifndef ADC_WING_ROTATION
113 #define ADC_WING_ROTATION FALSE
114 #endif
115 #if ADC_WING_ROTATION
117 #endif
120 #ifndef ROTWING_STATE_ACT_FEEDBACK_ID
121 #define ROTWING_STATE_ACT_FEEDBACK_ID ABI_BROADCAST
122 #endif
124 static void rotwing_state_feedback_cb(uint8_t sender_id, struct act_feedback_t *feedback_msg, uint8_t num_act);
125 #define ROTWING_STATE_NUM_HOVER_RPM 4
127 
131 
137 
139 
142 
143 float Wu_gih_original[GUIDANCE_INDI_HYBRID_U] = GUIDANCE_INDI_WLS_WU;
144 
145 inline void rotwing_check_set_current_state(void);
146 inline void rotwing_switch_state(void);
147 
148 inline void rotwing_state_set_hover_settings(void);
149 inline void rotwing_state_set_skewing_settings(void);
150 inline void rotwing_state_set_fw_settings(void);
153 
154 inline void rotwing_state_set_state_settings(void);
155 inline void rotwing_state_skewer(void);
156 inline void rotwing_state_free_processor(void);
157 
158 inline void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle);
159 
160 #if PERIODIC_TELEMETRY
162 static void send_rotating_wing_state(struct transport_tx *trans, struct link_device *dev)
163 {
164  uint16_t adc_dummy = 0;
165  pprz_msg_send_ROTATING_WING_STATE(trans, dev, AC_ID,
170  &adc_dummy,
172 }
173 #endif // PERIODIC_TELEMETRY
174 
176 {
177  // Bind ABI messages
179 
180  // Start the drone in a desired hover state
184 
190 
191 #if PERIODIC_TELEMETRY
192  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTATING_WING_STATE, send_rotating_wing_state);
193 #endif
194 }
195 
197 {
198  // Check and set the current state
200 
201  // Check and update desired state
203  // Run the free state requester if free configuration requestes
206  }
208  } else if (guidance_h.mode == GUIDANCE_H_MODE_NONE) {
212  } else {
214  }
215  }
216  }
217 
218  // Run the wing skewer
220 
221  //TODO: incorparate motor active / disbaling depending on called flight state
222  // Switch on motors if flight mode is attitude
225  }
226 
227 
228 }
229 
230 // Function to request a state
232 {
235  }
236 }
237 
238 // Function to prefer configuration
240 {
241  switch (configuration) {
245  break;
249  break;
253  break;
256  break;
257  }
258 }
259 
261 {
262  // if !in_flight, set state to hover
263  if (!autopilot.in_flight) {
270  return;
271  }
272 
273  // States can be checked according to wing angle sensor, setpoints .....
274  uint8_t prev_state = rotwing_state.current_state;
275  switch (prev_state) {
276  case ROTWING_STATE_HOVER:
277  // Check if state needs to be set to skewing
280  } else {
282  }
283 
284  // Switch state if necessary
288  }
289  break;
290 
292  // Check if state needs to be set to hover
295  } else {
297  }
298 
299  // Check if state needs to be set to fixed wing
302  } else {
304  }
305 
306  // Switch state if necessary
310  }
311 
315  }
316  break;
317 
318  case ROTWING_STATE_FW:
319  // Check if state needs to be set to skewing
322  } else {
324  }
325 
326  // Check if state needs to be set to fixed wing with hover motors idle (If hover thrust below threshold)
329  } else {
331  }
332 
333  // Switch state if necessary
343  }
344  break;
345 
347  // Check if state needs to be set to fixed wing with hover motors activated
348  if (stabilization.cmd[COMMAND_THRUST] > ROTWING_MIN_THRUST_IDLE
351  } else {
353  }
354 
355  // Check if state needs to be set to fixed wing with hover motors off (if zero rpm on hover motors)
356  if (rotwing_state_hover_rpm[0] == 0
357  && rotwing_state_hover_rpm[1] == 0
358  && rotwing_state_hover_rpm[2] == 0
359  && rotwing_state_hover_rpm[3] == 0) {
360 #if !USE_NPS
362 #endif
363  } else {
365  }
366 
367  // Switch state if necessary
373  && rotwing_state_fw_counter == 0) {
377  }
378  break;
379 
381  // Check if state needs to be set to fixed wing with hover motors idle (if rpm on hover motors)
387  } else {
389  }
390 
391  // Switch state if necessary
395  }
396  break;
397 
398  default:
399  break;
400  }
401 }
402 
403 // Function that handles settings for switching state(s)
405 {
406  switch (rotwing_state.current_state) {
407  case ROTWING_STATE_HOVER:
410  } else {
412  }
413  break;
414 
418  } else {
420  }
421  break;
422 
423  case ROTWING_STATE_FW:
428  } else {
430  }
431  break;
432 
438  } else {
440  }
441  break;
442 
446  } else {
448  }
449  break;
450  }
451 }
452 
454 {
465 }
466 
468 {
469  // Wing may be skewed to quad when desired state is hover and skewing settings set by state machine
472  } else {
474  }
482  rotwing_state_settings.nav_max_speed = 100; // Big as we use airspeed guidance
484 }
485 
487 {
496  rotwing_state_settings.nav_max_speed = 100; // Big as we use airspeed guidance
498 }
499 
501 {
510  rotwing_state_settings.nav_max_speed = 100; // Big as we use airspeed guidance
512 }
513 
515 {
524  rotwing_state_settings.nav_max_speed = 100; // Big as we use airspeed guidance
526 }
527 
529 {
530 
536  break;
539  break;
542  break;
543  }
544  } else {
546  }
547 
549 
551 
553 
556 
557  // TO DO: pitch angle now hard coded scheduled by wing angle
558 
559  // Stall protection handled by hover_motors_active boolean
560 
561  // TO DO: Climb and descend speeds now handled by guidance airspeed
562 }
563 
565 {
567  float wing_angle_scheduled_sp_deg = 0;
568  float airspeed = stateGetAirspeed_f();
569  if (airspeed < 8) {
570  wing_angle_scheduled_sp_deg = 0;
571  } else if (airspeed < 10 && (rotwing_state.desired_state > ROTWING_STATE_HOVER)) {
572  wing_angle_scheduled_sp_deg = 55;
573  } else if (airspeed > 10) {
574  wing_angle_scheduled_sp_deg = ((airspeed - 10.)) / 4. * 35. + 55.;
575  } else {
576  wing_angle_scheduled_sp_deg = 0;
577  }
578 
579  Bound(wing_angle_scheduled_sp_deg, 0., 90.)
580  rotwing_state_skewing.wing_angle_deg_sp = wing_angle_scheduled_sp_deg;
581  }
582 }
583 
585 {
586  // Get current speed vector
587  struct EnuCoor_f *groundspeed = stateGetSpeedEnu_f();
588  float current_groundspeed = FLOAT_VECT2_NORM(*groundspeed);
589 
590  // Get current airspeed
591  float airspeed = stateGetAirspeed_f();
592 
593  // Get windspeed vector
594  struct FloatEulers eulers_zxy;
596 
597  float psi = eulers_zxy.psi;
598  float cpsi = cosf(psi);
599  float spsi = sinf(psi);
600  struct FloatVect2 airspeed_v = { spsi * airspeed, cpsi * airspeed };
601  struct FloatVect2 windspeed_v;
602  VECT2_DIFF(windspeed_v, *groundspeed, airspeed_v);
603 
604  // Get goto target information
605  struct FloatVect2 pos_error;
606  struct EnuCoor_f *pos = stateGetPositionEnu_f();
607  VECT2_DIFF(pos_error, nav_rotorcraft_base.goto_wp.to, *pos);
608 
609  /*
610  Calculations
611  */
612  // speed over pos_error projection
613  struct FloatVect2 pos_error_norm;
614  VECT2_COPY(pos_error_norm, pos_error);
615  float_vect2_normalize(&pos_error_norm);
616  float dist_to_target = sqrtf(nav_rotorcraft_base.goto_wp.dist2_to_wp);
617  float max_speed_decel2 = fabsf(2.f * dist_to_target * nav_max_deceleration_sp); // dist_to_wp can only be positive, but just in case
618  float max_speed_decel = sqrtf(max_speed_decel2);
619 
620  // Check if speed setpoint above set airspeed
621  struct FloatVect2 desired_airspeed_v;
622  struct FloatVect2 groundspeed_sp;
623  groundspeed_sp.x = pos_error.x * nav_hybrid_pos_gain;
624  groundspeed_sp.y = pos_error.y * nav_hybrid_pos_gain;
625  VECT2_COPY(desired_airspeed_v, groundspeed_sp);
626  VECT2_ADD(desired_airspeed_v, windspeed_v);
627 
628  float desired_airspeed = FLOAT_VECT2_NORM(desired_airspeed_v);
629  float airspeed_error = guidance_indi_max_airspeed - airspeed;
630 
631  // Request hybrid if we have to decelerate and approaching target
632  if (max_speed_decel < current_groundspeed) {
634  } else if ((desired_airspeed > 15) && ((current_groundspeed + airspeed_error) < max_speed_decel)) {
636  }
637 }
638 
640 {
641 
642  // calc rotation percentage of setpoint (0 deg = -1, 45 deg = 0, 90 deg = 1)
643  float wing_rotation_percentage = (rotwing_state_skewing.wing_angle_deg_sp - 45.) / 45.;
644  Bound(wing_rotation_percentage, -1., 1.);
645 
646  float servo_pprz_cmd = MAX_PPRZ * wing_rotation_percentage;
647  // Calulcate rotation_cmd
648  Bound(servo_pprz_cmd, -MAX_PPRZ, MAX_PPRZ);
649 
650 #if ROTWING_STATE_USE_ROTATION_REF_MODEL
651  // Rotate with second order filter
652  static float rotwing_state_skew_p_cmd = -MAX_PPRZ;
653  static float rotwing_state_skew_d_cmd = 0;
654  float speed_sp = 0.001 * (servo_pprz_cmd - rotwing_state_skew_p_cmd);
655  rotwing_state_skew_d_cmd += 0.003 * (speed_sp - rotwing_state_skew_d_cmd);
656  rotwing_state_skew_p_cmd += rotwing_state_skew_d_cmd;
657  Bound(rotwing_state_skew_p_cmd, -MAX_PPRZ, MAX_PPRZ);
658  rotwing_state_skewing.servo_pprz_cmd = rotwing_state_skew_p_cmd;
659 #else
660  // Directly controlling the wing rotation
661  rotwing_state_skewing.servo_pprz_cmd = servo_pprz_cmd;
662 #endif
663 
664 #if USE_NPS
665  // Export to the index of the SKEW in the NPS_ACTUATOR_NAMES array
666  actuators_pprz[INDI_NUM_ACT] = (rotwing_state_skewing.servo_pprz_cmd + MAX_PPRZ) / 2.; // Scale to simulation command
667 
668  // Simulate wing angle from command
670 
671  // SEND ABI Message to ctr_eff_sched and other modules that want Actuator position feedback
672  struct act_feedback_t feedback;
673  feedback.idx = SERVO_ROTATION_MECH_IDX;
674  feedback.position = 0.5 * M_PI - RadOfDeg(rotwing_state_skewing.wing_angle_deg);
675  feedback.set.position = true;
676 
677  // Send ABI message
678  AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_UAVCAN_ID, &feedback, 1);
679 #endif
680 }
681 
682 static void rotwing_state_feedback_cb(uint8_t __attribute__((unused)) sender_id,
683  struct act_feedback_t UNUSED *feedback_msg, uint8_t UNUSED num_act_message)
684 {
685  for (int i = 0; i < num_act_message; i++) {
686 
687  for (int i = 0; i < num_act_message; i++) {
688  // Check for wing rotation feedback
689  if ((feedback_msg[i].set.position) && (feedback_msg[i].idx == SERVO_ROTATION_MECH_IDX)) {
690  // Get wing rotation angle from sensor
691  float wing_angle_rad = 0.5 * M_PI - feedback_msg[i].position;
692  rotwing_state_skewing.wing_angle_deg = DegOfRad(wing_angle_rad);
693 
694  // Bound wing rotation angle
695  Bound(rotwing_state_skewing.wing_angle_deg, 0, 90.);
696  }
697  }
698 
699  // Sanity check that index is valid
700  int idx = feedback_msg[i].idx;
701  if (feedback_msg[i].set.rpm) {
702  if ((idx == SERVO_MOTOR_FRONT_IDX) || (idx == SERVO_BMOTOR_FRONT_IDX)) {
703  rotwing_state_hover_rpm[0] = feedback_msg->rpm;
704  } else if ((idx == SERVO_MOTOR_RIGHT_IDX) || (idx == SERVO_BMOTOR_RIGHT_IDX)) {
705  rotwing_state_hover_rpm[1] = feedback_msg->rpm;
706  } else if ((idx == SERVO_MOTOR_BACK_IDX) || (idx == SERVO_BMOTOR_BACK_IDX)) {
707  rotwing_state_hover_rpm[2] = feedback_msg->rpm;
708  } else if ((idx == SERVO_MOTOR_LEFT_IDX) || (idx == SERVO_BMOTOR_LEFT_IDX)) {
709  rotwing_state_hover_rpm[3] = feedback_msg->rpm;
710  }
711  }
712  }
713 }
714 
716  // Check if hover motors are running
721  return true;
722  } else {
723  return false;
724  }
725 }
726 
727 void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle)
728 {
729  // adjust weights
730  float fixed_wing_percentage = !hover_motors_active; // TODO: when hover props go below 40%, ...
731  Bound(fixed_wing_percentage, 0, 1);
732 #define AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT 16
733 
734  float Wv_original[GUIDANCE_INDI_HYBRID_V] = GUIDANCE_INDI_WLS_PRIORITIES;
735 
736  // Increase importance of forward acceleration in forward flight
737  Wv_gih[0] = Wv_original[0] * (1.0f + fixed_wing_percentage *
738  AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT); // stall n low hover motor_off (weight 16x more important than vertical weight)
739 
740  struct FloatEulers eulers_zxy;
742 
743  // Evaluate motors_on boolean
744  if (!hover_motors_active) {
745  if (stateGetAirspeed_f() < 15.) {
746  hover_motors_active = true;
748  } else if (eulers_zxy.theta > RadOfDeg(15.0)) {
749  hover_motors_active = true;
751  }
752  } else {
754  }
755 
756  float du_min_thrust_z = ((MAX_PPRZ - actuator_state_filt_vect[0]) * g1g2[3][0] + (MAX_PPRZ -
759  Bound(du_min_thrust_z, -50., 0.);
760  float du_max_thrust_z = -(actuator_state_filt_vect[0] * g1g2[3][0] + actuator_state_filt_vect[1] * g1g2[3][1] +
762  Bound(du_max_thrust_z, 0., 50.);
763 
764  float roll_limit_rad = guidance_indi_max_bank;
765  float max_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MAX_PITCH);
766  float min_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MIN_PITCH);
767 
768  float fwd_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MAX_PITCH);
769  float quad_pitch_limit_rad = RadOfDeg(5.0);
770 
771  float scheduled_pitch_angle = 0;
772  float pitch_angle_range = 3.;
774  scheduled_pitch_angle = 0;
775  Wu_gih[1] = Wu_gih_original[1];
776  max_pitch_limit_rad = quad_pitch_limit_rad;
777  } else {
778  float pitch_progression = (rotwing_state_skewing.wing_angle_deg - 55) / 35.;
779  scheduled_pitch_angle = pitch_angle_range * pitch_progression;
780  Wu_gih[1] = Wu_gih_original[1] * (1.f - pitch_progression*0.9);
781  max_pitch_limit_rad = quad_pitch_limit_rad + (fwd_pitch_limit_rad - quad_pitch_limit_rad) * pitch_progression;
782  }
783  if (!hover_motors_active) {
784  scheduled_pitch_angle = 8.;
785  max_pitch_limit_rad = fwd_pitch_limit_rad;
786  }
787  Bound(scheduled_pitch_angle, -5., 8.);
788  guidance_indi_pitch_pref_deg = scheduled_pitch_angle;
789 
790  float pitch_pref_rad = RadOfDeg(guidance_indi_pitch_pref_deg);
791 
792  // Set lower limits
793  du_min_gih[0] = -roll_limit_rad - roll_angle; //roll
794  du_min_gih[1] = min_pitch_limit_rad - pitch_angle; // pitch
795  du_min_gih[2] = du_min_thrust_z;
796  du_min_gih[3] = (-actuator_state_filt_vect[8] * g1g2[4][8]);
797 
798  // Set upper limits limits
799  du_max_gih[0] = roll_limit_rad - roll_angle; //roll
800  du_max_gih[1] = max_pitch_limit_rad - pitch_angle; // pitch
801  du_max_gih[2] = du_max_thrust_z;
802  du_max_gih[3] = 9.0; // Hacky value to prevent drone from pitching down in transition
803 
804  // Set prefered states
805  du_pref_gih[0] = 0; // prefered delta roll angle
806  du_pref_gih[1] = -pitch_angle + pitch_pref_rad;// prefered delta pitch angle
807  du_pref_gih[2] = du_max_gih[2]; // Low thrust better for efficiency
808  du_pref_gih[3] = body_v[0]; // solve the body acceleration
809 }
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
Definition: abi_common.h:67
#define ACT_FEEDBACK_UAVCAN_ID
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:49
bool in_flight
in flight status
Definition: autopilot.h:70
uint8_t last_wp UNUSED
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
static void float_vect2_normalize(struct FloatVect2 *v)
normalize 2D vector in place
#define FLOAT_VECT2_NORM(_v)
euler angles
#define VECT2_ADD(_a, _b)
Definition: pprz_algebra.h:74
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:68
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition: state.h:1131
struct State state
Definition: state.c:36
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:917
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1407
float guidance_indi_max_bank
struct FloatVect3 speed_sp
Definition: guidance_indi.c:73
bool force_forward
forward flight for hybrid nav
float guidance_indi_pitch_pref_deg
struct FloatEulers eulers_zxy
state eulers in zxy order
float guidance_indi_max_airspeed
struct FloatVect2 desired_airspeed
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).
uint8_t idx
General index of the actuators (generated in airframe.h)
Definition: actuators.h:45
struct NavGoto_t goto_wp
Definition: nav_base.h:63
struct EnuCoor_f to
end WP position
Definition: nav_base.h:37
float dist2_to_wp
squared distance to next waypoint
Definition: nav_base.h:38
struct NavBase_t nav_rotorcraft_base
Basic Nav struct.
float nav_max_deceleration_sp
float nav_max_speed
float nav_hybrid_pos_gain
float nav_goto_max_speed
Specific navigation functions for hybrid aircraft.
static uint32_t idx
#define MAX_PPRZ
Definition: paparazzi.h:8
vector in East North Up coordinates Units: meters
Rotorcraft specific autopilot interface and initialization.
struct HorizontalGuidance guidance_h
Definition: guidance_h.c:45
#define GUIDANCE_H_MODE_NAV
Definition: guidance_h.h:58
#define GUIDANCE_H_MODE_NONE
Definition: guidance_h.h:56
void init_rotwing_state(void)
static void rotwing_state_feedback_cb(uint8_t sender_id, struct act_feedback_t *feedback_msg, uint8_t num_act)
struct RotWingStateSettings rotwing_state_settings
float Wu_gih_original[GUIDANCE_INDI_HYBRID_U]
void rotwing_state_set_fw_settings(void)
bool rotwing_state_hover_motors_running(void)
void rotwing_switch_state(void)
#define ROTWING_MIN_SKEW_ANGLE_COUNTER
Definition: rotwing_state.c:40
#define ROTWING_MIN_FW_SKEW_ANGLE_DEG
Definition: rotwing_state.c:66
void rotwing_state_skew_actuator_periodic(void)
#define ROTWING_MIN_FW_COUNTER
Definition: rotwing_state.c:69
void request_rotwing_state(uint8_t state)
void rotwing_state_set_state_settings(void)
#define ROTWING_MAX_QUAD_AIRSPEED
Definition: rotwing_state.c:50
#define ROTWING_MIN_THRUST_IDLE
Definition: rotwing_state.c:74
struct RotwingState rotwing_state
uint8_t rotwing_state_hover_counter
#define ROTWING_HOV_MOT_OFF_RPM_TH
Definition: rotwing_state.c:85
float rotwing_state_max_hover_speed
struct RotWingStateSkewing rotwing_state_skewing
#define ROTWING_MIN_THRUST_IDLE_COUNTER
Definition: rotwing_state.c:77
uint8_t rotwing_state_fw_idle_counter
uint8_t rotwing_state_skewing_counter
uint8_t rotwing_state_fw_counter
#define ROTWING_STATE_NUM_HOVER_RPM
bool bool_disable_hover_motors
#define ROTWING_HOV_MOT_RUN_RPM_TH
Definition: rotwing_state.c:82
#define AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT
void rotwing_state_set_fw_hov_mot_off_settings(void)
int32_t rotwing_state_hover_rpm[ROTWING_STATE_NUM_HOVER_RPM]
uint8_t rotwing_state_fw_m_off_counter
void rotwing_state_set_skewing_settings(void)
void rotwing_check_set_current_state(void)
void rotwing_state_skewer(void)
void rotwing_state_set_hover_settings(void)
#define ROTWING_MIN_SKEW_ANGLE_DEG_QUAD
Definition: rotwing_state.c:37
static void send_rotating_wing_state(struct transport_tx *trans, struct link_device *dev)
void rotwing_state_free_processor(void)
bool hover_motors_active
#define ROTWING_STATE_ACT_FEEDBACK_ID
ABI binding feedback data.
void rotwing_request_configuration(uint8_t configuration)
abi_event rotwing_state_feedback_ev
#define ROTWING_HOV_MOT_OFF_COUNTER
Definition: rotwing_state.c:88
void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle)
void rotwing_state_set_fw_hov_mot_idle_settings(void)
void periodic_rotwing_state(void)
uint8_t current_state
Definition: rotwing_state.h:48
#define ROTWING_CONFIGURATION_HYBRID
Definition: rotwing_state.h:43
#define ROTWING_STATE_FW
Definition: rotwing_state.h:35
uint8_t requested_config
Definition: rotwing_state.h:50
uint8_t desired_state
Definition: rotwing_state.h:49
#define ROTWING_STATE_SKEWING
Definition: rotwing_state.h:34
#define ROTWING_STATE_FW_HOV_MOT_IDLE
Definition: rotwing_state.h:36
#define ROTWING_STATE_HOVER
Rotwing States.
Definition: rotwing_state.h:33
#define ROTWING_STATE_PITCH_FW_SETTING
Definition: rotwing_state.h:59
#define ROTWING_STATE_WING_FW_SETTING
Definition: rotwing_state.h:55
#define ROTWING_CONFIGURATION_HOVER
Rotwing Configurations.
Definition: rotwing_state.h:42
#define ROTWING_CONFIGURATION_FREE
Definition: rotwing_state.h:45
#define ROTWING_STATE_PITCH_TRANSITION_SETTING
Definition: rotwing_state.h:58
#define ROTWING_STATE_PITCH_QUAD_SETTING
Definition: rotwing_state.h:57
#define ROTWING_STATE_FW_HOV_MOT_OFF
Definition: rotwing_state.h:37
#define ROTWING_STATE_WING_QUAD_SETTING
Definition: rotwing_state.h:53
#define ROTWING_STATE_WING_SCHEDULING_SETTING
Definition: rotwing_state.h:54
#define ROTWING_CONFIGURATION_EFFICIENT
Definition: rotwing_state.h:44
struct Stabilization stabilization
Definition: stabilization.c:41
uint8_t mode
current mode
#define STABILIZATION_MODE_ATTITUDE
Definition: stabilization.h:41
uint8_t att_submode
current attitude sub-mode
#define STABILIZATION_ATT_SUBMODE_FORWARD
Definition: stabilization.h:47
int32_t cmd[COMMANDS_NB]
output command vector, range from [-MAX_PPRZ:MAX_PPRZ] (store for messages)
float g1g2[INDI_OUTPUTS][INDI_NUM_ACT]
float actuator_state_filt_vect[INDI_NUM_ACT]
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_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 short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98