33 #ifndef ROTWING_MIN_SKEW_ANGLE_DEG_QUAD
34 #define ROTWING_MIN_SKEW_ANGLE_DEG_QUAD 10.0
37 #ifndef ROTWING_MIN_SKEW_ANGLE_COUNTER
38 #define ROTWING_MIN_SKEW_ANGLE_COUNTER 10
42 #ifndef ROTWING_SKEWING_COUNTER
43 #define ROTWING_SKEWING_COUNTER 10
47 #ifndef ROTWING_MAX_QUAD_AIRSPEED
48 #define ROTWING_MAX_QUAD_AIRSPEED 20.0
52 #ifndef ROTWING_HALF_SKEW_ANGLE_DEG
53 #define ROTWING_HALF_SKEW_ANGLE_DEG 55.0
56 #ifndef ROTWING_HALF_SKEW_ANGLE_RANG
57 #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
69 #ifndef ROTWING_MIN_FW_COUNTER
70 #define ROTWING_MIN_FW_COUNTER 10
74 #ifndef ROTWING_MIN_THRUST_IDLE
75 #define ROTWING_MIN_THRUST_IDLE 100
78 #ifndef ROTWING_MIN_THRUST_IDLE_COUNTER
79 #define ROTWING_MIN_THRUST_IDLE_COUNTER 10
83 #ifndef ROTWING_HOV_MOT_RUN_RPM_TH
84 #define ROTWING_HOV_MOT_RUN_RPM_TH 800
86 #ifndef ROTWING_HOV_MOT_OFF_RPM_TH
87 #define ROTWING_HOV_MOT_OFF_RPM_TH 50
90 #ifndef ROTWING_HOV_MOT_OFF_COUNTER
91 #define ROTWING_HOV_MOT_OFF_COUNTER 10
94 #ifndef ROTWING_STATE_USE_ROTATION_REF_MODEL
95 #define ROTWING_STATE_USE_ROTATION_REF_MODEL FALSE
99 #ifndef ROTWING_STATE_HOVER_PREF_PITCH
100 #define ROTWING_STATE_HOVER_PREF_PITCH 0.0
104 #ifndef ROTWING_STATE_TRANSITION_PREF_PITCH
105 #define ROTWING_STATE_TRANSITION_PREF_PITCH 3.0
109 #ifndef ROTWING_STATE_FW_PREF_PITCH
110 #define ROTWING_STATE_FW_PREF_PITCH 8.0
114 #ifndef USE_ROTMECH_VIRTUAL
115 #define USE_ROTMECH_VIRTUAL FALSE
118 #if USE_ROTMECH_VIRTUAL
120 #error "Rotmech dynamics are not provided. Please provide them in your airframe file."
125 #ifndef ADC_WING_ROTATION
126 #define ADC_WING_ROTATION FALSE
128 #if ADC_WING_ROTATION
133 #ifndef ROTWING_STATE_ACT_FEEDBACK_ID
134 #define ROTWING_STATE_ACT_FEEDBACK_ID ABI_BROADCAST
138 #define ROTWING_STATE_NUM_HOVER_RPM 4
152 #ifdef NAV_HYBRID_MAX_AIRSPEED
174 #if PERIODIC_TELEMETRY
180 pprz_msg_send_ROTATING_WING_STATE(trans,
dev, AC_ID,
213 #if PERIODIC_TELEMETRY
292 switch (configuration) {
327 switch (prev_state) {
379 if (current_thrust < ROTWING_MIN_THRUST_IDLE && rotwing_state.desired_state >
ROTWING_STATE_FW) {
618 float wing_angle_scheduled_sp_deg = 0;
621 wing_angle_scheduled_sp_deg = 0;
624 }
else if (airspeed > 10) {
627 wing_angle_scheduled_sp_deg = 0;
630 Bound(wing_angle_scheduled_sp_deg, 0., 90.)
649 float cpsi = cosf(
psi);
650 float spsi = sinf(
psi);
651 struct FloatVect2 airspeed_v = { spsi * airspeed, cpsi * airspeed };
653 VECT2_DIFF(windspeed_v, *groundspeed, airspeed_v);
669 float max_speed_decel = sqrtf(max_speed_decel2);
676 VECT2_COPY(desired_airspeed_v, groundspeed_sp);
677 VECT2_ADD(desired_airspeed_v, windspeed_v);
683 if (max_speed_decel < current_groundspeed) {
685 }
else if ((
desired_airspeed > 15) && ((current_groundspeed + airspeed_error) < max_speed_decel)) {
695 Bound(wing_rotation_percentage, -1., 1.);
697 float servo_pprz_cmd =
MAX_PPRZ * wing_rotation_percentage;
701 #if ROTWING_STATE_USE_ROTATION_REF_MODEL
703 static float rotwing_state_skew_p_cmd = -
MAX_PPRZ;
704 static float rotwing_state_skew_d_cmd = 0;
705 float speed_sp = 0.001 * (servo_pprz_cmd - rotwing_state_skew_p_cmd);
706 rotwing_state_skew_d_cmd += 0.003 * (
speed_sp - rotwing_state_skew_d_cmd);
707 rotwing_state_skew_p_cmd += rotwing_state_skew_d_cmd;
719 #if USE_ROTMECH_VIRTUAL
721 float dyn_dis = 1.0-exp(-ROTMECH_DYN / PERIODIC_FREQUENCY);
731 feedback.
idx = COMMAND_ROT_MECH;
733 feedback.set.position =
true;
744 for (
int i = 0; i < num_act_message; i++) {
746 for (
int i = 0; i < num_act_message; i++) {
748 if ((feedback_msg[i].set.position) && (feedback_msg[i].idx == COMMAND_ROT_MECH)) {
750 float wing_angle_rad = 0.5 * M_PI - feedback_msg[i].position;
759 int idx = feedback_msg[i].idx;
760 if (feedback_msg[i].set.rpm) {
761 if ((
idx == SERVO_MOTOR_FRONT_IDX) || (
idx == SERVO_BMOTOR_FRONT_IDX)) {
763 }
else if ((
idx == SERVO_MOTOR_RIGHT_IDX) || (
idx == SERVO_BMOTOR_RIGHT_IDX)) {
765 }
else if ((
idx == SERVO_MOTOR_BACK_IDX) || (
idx == SERVO_BMOTOR_BACK_IDX)) {
767 }
else if ((
idx == SERVO_MOTOR_LEFT_IDX) || (
idx == SERVO_BMOTOR_LEFT_IDX)) {
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
pprz_t commands[COMMANDS_NB]
Hardware independent code for commands handling.
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)
#define VECT2_ADD(_a, _b)
#define VECT2_DIFF(_c, _a, _b)
#define VECT2_COPY(_a, _b)
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
static float stateGetAirspeed_f(void)
Get airspeed (float).
struct FloatVect3 speed_sp
bool force_forward
forward flight for hybrid nav
struct FloatEulers eulers_zxy
state eulers in zxy order
struct FloatVect2 desired_airspeed
Hardware independent API for actuators (servos, motor controllers).
uint8_t idx
General index of the actuators (generated in airframe.h)
struct EnuCoor_f to
end WP position
float dist2_to_wp
squared distance to next waypoint
struct NavBase_t nav_rotorcraft_base
Basic Nav struct.
float nav_max_deceleration_sp
#define NAV_HYBRID_MAX_AIRSPEED
float nav_hybrid_pos_gain
vector in East North Up coordinates Units: meters
Rotorcraft specific autopilot interface and initialization.
struct HorizontalGuidance guidance_h
#define GUIDANCE_H_MODE_NAV
#define GUIDANCE_H_MODE_NONE
#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_CONFIGURATION_FREE
#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
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_STATE_FW_PREF_PITCH
#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
void rotwing_state_force_skew_off(void)
#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
float rotwing_state_max_fw_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
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_STATE_TRANSITION_PREF_PITCH
#define ROTWING_STATE_HOVER_PREF_PITCH
#define ROTWING_MIN_SKEW_ANGLE_DEG_QUAD
static void send_rotating_wing_state(struct transport_tx *trans, struct link_device *dev)
void rotwing_state_free_processor(void)
#define ROTWING_HALF_SKEW_ANGLE_DEG
#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 rotwing_state_set_fw_hov_mot_idle_settings(void)
void periodic_rotwing_state(void)
bool force_rotation_angle
uint8_t preferred_pitch_setting
bool hover_motors_disable
float preferred_pitch_value
struct Stabilization stabilization
#define STABILIZATION_MODE_ATTITUDE
uint8_t att_submode
current attitude sub-mode
#define STABILIZATION_ATT_SUBMODE_FORWARD
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.