36 #ifndef ROTWING_MIN_SKEW_ANGLE_DEG_QUAD
37 #define ROTWING_MIN_SKEW_ANGLE_DEG_QUAD 10.0
39 #ifndef ROTWING_MIN_SKEW_ANGLE_COUNTER
40 #define ROTWING_MIN_SKEW_ANGLE_COUNTER 10
44 #ifndef ROTWING_SKEWING_COUNTER
45 #define ROTWING_SKEWING_COUNTER 10
49 #ifndef ROTWING_MAX_QUAD_AIRSPEED
50 #define ROTWING_MAX_QUAD_AIRSPEED 20.0
54 #ifndef ROTWING_HALF_SKEW_ANGLE_DEG
55 #define ROTWING_HALF_SKEW_ANGLE_DEG 55.0
57 #ifndef ROTWING_HALF_SKEW_ANGLE_RANG
58 #define ROTWING_HALF_SKEW_ANGLE_HALF_RANGE 10.0
60 #ifndef ROTWING_HALF_SKEW_COUNTER
61 #define ROTWING_HALF_SKEW_COUNTER 10
65 #ifndef ROTWING_MIN_FW_SKEW_ANGLE_DEG
66 #define ROTWING_MIN_FW_SKEW_ANGLE_DEG 80.0
68 #ifndef ROTWING_MIN_FW_COUNTER
69 #define ROTWING_MIN_FW_COUNTER 10
73 #ifndef ROTWING_MIN_THRUST_IDLE
74 #define ROTWING_MIN_THRUST_IDLE 100
76 #ifndef ROTWING_MIN_THRUST_IDLE_COUNTER
77 #define ROTWING_MIN_THRUST_IDLE_COUNTER 10
81 #ifndef ROTWING_HOV_MOT_OFF_RPM_TH
82 #define ROTWING_HOV_MOT_OFF_RPM_TH 50
84 #ifndef ROTWING_HOV_MOT_OFF_COUNTER
85 #define ROTWING_HOV_MOT_OFF_COUNTER 10
88 #ifndef ROTWING_STATE_USE_ROTATION_REF_MODEL
89 #define ROTWING_STATE_USE_ROTATION_REF_MODEL FALSE
94 #ifndef ROTWING_STATE_HOVER_PREF_PITCH
95 #define ROTWING_STATE_HOVER_PREF_PITCH 0.0
99 #ifndef ROTWING_STATE_TRANSITION_PREF_PITCH
100 #define ROTWING_STATE_TRANSITION_PREF_PITCH 3.0
104 #ifndef ROTWING_STATE_FW_PREF_PITCH
105 #define ROTWING_STATE_FW_PREF_PITCH 8.0
109 #ifndef ADC_WING_ROTATION
110 #define ADC_WING_ROTATION FALSE
112 #if ADC_WING_ROTATION
117 #ifndef ROTWING_STATE_ACT_FEEDBACK_ID
118 #define ROTWING_STATE_ACT_FEEDBACK_ID ABI_BROADCAST
122 #define ROTWING_STATE_NUM_HOVER_RPM 4
155 #if PERIODIC_TELEMETRY
160 pprz_msg_send_ROTATING_WING_STATE(trans,
dev, AC_ID,
185 #if PERIODIC_TELEMETRY
227 switch (configuration) {
255 switch (prev_state) {
546 float wing_angle_scheduled_sp_deg = 0;
549 wing_angle_scheduled_sp_deg = 0;
551 wing_angle_scheduled_sp_deg = 55;
552 }
else if (airspeed > 10) {
553 wing_angle_scheduled_sp_deg = ((airspeed - 10.)) / 4. * 35. + 55.;
555 wing_angle_scheduled_sp_deg = 0;
558 Bound(wing_angle_scheduled_sp_deg, 0., 90.)
568 Bound(wing_rotation_percentage, -1., 1.);
574 #if ROTWING_STATE_USE_ROTATION_REF_MODEL
576 static float rotwing_state_skew_p_cmd = -
MAX_PPRZ;
577 static float rotwing_state_skew_d_cmd = 0;
579 rotwing_state_skew_d_cmd += 0.003 * (
speed_sp - rotwing_state_skew_d_cmd);
580 rotwing_state_skew_p_cmd += rotwing_state_skew_d_cmd;
597 feedback.
idx = SERVO_ROTATION_MECH_IDX;
599 feedback.set.position =
true;
609 for (
int i = 0; i < num_act_message; i++) {
611 for (
int i = 0; i < num_act_message; i++) {
613 if ((feedback_msg[i].set.position) && (feedback_msg[i].idx == SERVO_ROTATION_MECH_IDX)) {
615 float wing_angle_rad = 0.5 * M_PI - feedback_msg[i].position;
624 int idx = feedback_msg[i].idx;
625 if (feedback_msg[i].set.rpm) {
626 if ((
idx == SERVO_MOTOR_FRONT_IDX) || (
idx == SERVO_BMOTOR_FRONT_IDX)) {
628 }
else if ((
idx == SERVO_MOTOR_RIGHT_IDX) || (
idx == SERVO_BMOTOR_RIGHT_IDX)) {
630 }
else if ((
idx == SERVO_MOTOR_BACK_IDX) || (
idx == SERVO_BMOTOR_BACK_IDX)) {
632 }
else if ((
idx == SERVO_MOTOR_LEFT_IDX) || (
idx == SERVO_BMOTOR_LEFT_IDX)) {
641 float pitch_priority_factor = 11.;
642 float roll_priority_factor = 10.;
643 float thrust_priority_factor = 7.;
644 float pusher_priority_factor = 30.;
646 float horizontal_accel_weight = 10.;
647 float vertical_accel_weight = 10.;
650 Wu_gih[0] = roll_priority_factor * 10.414;
651 Wu_gih[1] = pitch_priority_factor * 27.53;
652 Wu_gih[2] = thrust_priority_factor * 0.626;
653 Wu_gih[3] = pusher_priority_factor * 1.0;
660 Bound(fixed_wing_percentage, 0, 1);
661 #define AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT 16
663 Wv_gih[0] = horizontal_accel_weight * (1.0f + fixed_wing_percentage *
665 Wv_gih[1] = horizontal_accel_weight;
666 Wv_gih[2] = vertical_accel_weight;
687 Bound(du_min_thrust_z, -50., 0.);
690 Bound(du_max_thrust_z, 0., 50.);
692 float roll_limit_rad = 2.0;
696 float scheduled_pitch_angle = 0;
697 float pitch_angle_range = 3.;
699 scheduled_pitch_angle = 0;
702 scheduled_pitch_angle = pitch_angle_range * pitch_progression;
705 scheduled_pitch_angle = 8.;
707 Bound(scheduled_pitch_angle, -5., 8.);
713 du_min_gih[0] = -roll_limit_rad - roll_angle;
714 du_min_gih[1] = min_pitch_limit_rad - pitch_angle;
715 du_min_gih[2] = du_min_thrust_z;
719 du_max_gih[0] = roll_limit_rad - roll_angle;
720 du_max_gih[1] = max_pitch_limit_rad - pitch_angle;
721 du_max_gih[2] = du_max_thrust_z;
726 du_pref_gih[1] = -pitch_angle + pitch_pref_rad;
727 du_pref_gih[2] = du_max_gih[2];
728 du_pref_gih[3] = body_v[0];
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
#define ACT_FEEDBACK_UAVCAN_ID
struct pprz_autopilot autopilot
Global autopilot structure.
bool in_flight
in flight status
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 struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
static float stateGetAirspeed_f(void)
Get airspeed (float).
struct FloatVect3 speed_sp
bool force_forward
forward flight for hybrid nav
float guidance_indi_pitch_pref_deg
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).
uint8_t idx
General index of the actuators (generated in airframe.h)
Specific navigation functions for hybrid aircraft.
Rotorcraft specific autopilot interface and initialization.
struct HorizontalGuidance guidance_h
#define GUIDANCE_H_MODE_FORWARD
#define GUIDANCE_H_MODE_NAV
#define GUIDANCE_H_MODE_ATTITUDE
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
void rotwing_state_set_fw_settings(void)
void rotwing_switch_state(void)
#define ROTWING_MIN_SKEW_ANGLE_COUNTER
#define ROTWING_MIN_FW_SKEW_ANGLE_DEG
void rotwing_state_skew_actuator_periodic(void)
#define ROTWING_MIN_FW_COUNTER
void request_rotwing_state(uint8_t state)
void rotwing_state_set_state_settings(void)
#define ROTWING_MAX_QUAD_AIRSPEED
#define ROTWING_MIN_THRUST_IDLE
struct RotwingState rotwing_state
uint8_t rotwing_state_hover_counter
#define ROTWING_HOV_MOT_OFF_RPM_TH
float rotwing_state_max_hover_speed
struct RotWingStateSkewing rotwing_state_skewing
#define ROTWING_MIN_THRUST_IDLE_COUNTER
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 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
static void send_rotating_wing_state(struct transport_tx *trans, struct link_device *dev)
#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
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)
bool force_rotation_angle
bool hover_motors_disable
#define ROTWING_CONFIGURATION_HYBRID
#define ROTWING_STATE_SKEWING
#define ROTWING_STATE_FW_HOV_MOT_IDLE
#define ROTWING_STATE_HOVER
Rotwing States.
#define ROTWING_STATE_PITCH_FW_SETTING
#define ROTWING_STATE_WING_FW_SETTING
#define ROTWING_CONFIGURATION_HOVER
Rotwing Configurations.
#define ROTWING_STATE_PITCH_TRANSITION_SETTING
#define ROTWING_STATE_PITCH_QUAD_SETTING
#define ROTWING_STATE_FW_HOV_MOT_OFF
#define ROTWING_STATE_WING_QUAD_SETTING
#define ROTWING_STATE_WING_SCHEDULING_SETTING
#define ROTWING_CONFIGURATION_EFFICIENT
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
float g1g2[INDI_OUTPUTS][INDI_NUM_ACT]
float actuator_state_filt_vect[INDI_NUM_ACT]
static const struct usb_device_descriptor dev
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.