28#include "generated/airframe.h"
39#ifndef SERVO_ROTATION_MECH_IDX
40#error ctrl_eff_sched_rotwing requires a servo named ROTATION_MECH_IDX
43#ifndef ROTWING_EFF_SCHED_IXX_BODY
44#error "NO ROTWING_EFF_SCHED_IXX_BODY defined"
47#ifndef ROTWING_EFF_SCHED_IYY_BODY
48#error "NO ROTWING_EFF_SCHED_IYY_BODY defined"
51#ifndef ROTWING_EFF_SCHED_IZZ
52#error "NO ROTWING_EFF_SCHED_IZZ defined"
55#ifndef ROTWING_EFF_SCHED_IXX_WING
56#error "NO ROTWING_EFF_SCHED_IXX_WING defined"
59#ifndef ROTWING_EFF_SCHED_IYY_WING
60#error "NO ROTWING_EFF_SCHED_IYY_WING defined"
63#ifndef ROTWING_EFF_SCHED_M
64#error "NO ROTWING_EFF_SCHED_M defined"
67#ifndef ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_PITCH
68#error "NO ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_PITCH defined"
71#ifndef ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_ROLL
72#error "NO ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_ROLL defined"
75#ifndef ROTWING_EFF_SCHED_HOVER_ROLL_PITCH_COEF
76#error "NO ROTWING_EFF_SCHED_HOVER_ROLL_PITCH_COEF defined"
79#ifndef ROTWING_EFF_SCHED_HOVER_ROLL_ROLL_COEF
80#error "NO ROTWING_EFF_SCHED_HOVER_ROLL_ROLL_COEF defined"
83#ifndef ROTWING_EFF_SCHED_K_ELEVATOR
84#error "NO ROTWING_EFF_SCHED_K_ELEVATOR defined"
87#ifndef ROTWING_EFF_SCHED_K_RUDDER
88#error "NO ROTWING_EFF_SCHED_K_RUDDER defined"
91#ifndef ROTWING_EFF_SCHED_K_AILERON
92#error "NO ROTWING_EFF_SCHED_K_AILERON defined"
95#ifndef ROTWING_EFF_SCHED_K_FLAPERON
96#error "NO ROTWING_EFF_SCHED_K_FLAPERON defined"
99#ifndef ROTWING_EFF_SCHED_K_PUSHER
100#error "NO ROTWING_EFF_SCHED_K_PUSHER defined"
103#ifndef ROTWING_EFF_SCHED_K_ELEVATOR_DEFLECTION
104#error "NO ROTWING_EFF_SCHED_K_ELEVATOR_DEFLECTION defined"
107#ifndef ROTWING_EFF_SCHED_D_RUDDER_D_PPRZ
108#error "NO ROTWING_EFF_SCHED_D_RUDDER_D_PPRZ defined"
111#ifndef ROTWING_EFF_SCHED_K_RPM_PPRZ_PUSHER
112#error "NO ROTWING_EFF_SCHED_K_RPM_PPRZ_PUSHER defined"
115#ifndef ROTWING_EFF_SCHED_K_LIFT_WING
116#error "NO ROTWING_EFF_SCHED_K_LIFT_WING defined"
119#ifndef ROTWING_EFF_SCHED_K_LIFT_FUSELAGE
120#error "NO ROTWING_EFF_SCHED_K_LIFT_FUSELAGE defined"
123#ifndef ROTWING_EFF_SCHED_K_LIFT_TAIL
124#error "NO ROTWING_EFF_SCHED_K_LIFT_TAIL defined"
128 .Ixx_body = ROTWING_EFF_SCHED_IXX_BODY,
129 .Iyy_body = ROTWING_EFF_SCHED_IYY_BODY,
130 .Izz = ROTWING_EFF_SCHED_IZZ,
131 .Ixx_wing = ROTWING_EFF_SCHED_IXX_WING,
132 .Iyy_wing = ROTWING_EFF_SCHED_IYY_WING,
133 .m = ROTWING_EFF_SCHED_M,
134 .DMdpprz_hover_pitch = ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_PITCH,
135 .DMdpprz_hover_roll = ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_ROLL,
136 .hover_roll_pitch_coef = ROTWING_EFF_SCHED_HOVER_ROLL_PITCH_COEF,
137 .hover_roll_roll_coef = ROTWING_EFF_SCHED_HOVER_ROLL_ROLL_COEF,
138 .k_elevator = ROTWING_EFF_SCHED_K_ELEVATOR,
139 .k_rudder = ROTWING_EFF_SCHED_K_RUDDER,
140 .k_aileron = ROTWING_EFF_SCHED_K_AILERON,
141 .k_flaperon = ROTWING_EFF_SCHED_K_FLAPERON,
142 .k_pusher = ROTWING_EFF_SCHED_K_PUSHER,
143 .k_elevator_deflection = ROTWING_EFF_SCHED_K_ELEVATOR_DEFLECTION,
144 .d_rudder_d_pprz = ROTWING_EFF_SCHED_D_RUDDER_D_PPRZ,
145 .k_rpm_pprz_pusher = ROTWING_EFF_SCHED_K_RPM_PPRZ_PUSHER,
146 .k_lift_wing = ROTWING_EFF_SCHED_K_LIFT_WING,
147 .k_lift_fuselage = ROTWING_EFF_SCHED_K_LIFT_FUSELAGE,
148 .k_lift_tail = ROTWING_EFF_SCHED_K_LIFT_TAIL
155 float output = value;
157 if (value < low_lim) {
159 }
else if (value > up_lim) {
163 if (value > up_lim) {
165 }
else if (value < low_lim) {
198#ifndef WING_ROTATION_CAN_ROTWING_ID
199#define WING_ROTATION_CAN_ROTWING_ID ABI_BROADCAST
206 for (
int i=0; i<num_act; i++){
207 if (pos_msg[i].set.position && (pos_msg[i].idx == SERVO_ROTATION_MECH_IDX))
210 eff_sched_var.wing_rotation_rad = 0.5 * M_PI - pos_msg[i].position;
232 float hover_thrust = 6000;
314 for (
uint8_t i = 0; i < 4; i++) {
319 dM_dpprz[i] = (
eff_sched_p.DMdpprz_hover_pitch[0] + 2*cmd_quat[i] *
eff_sched_p.DMdpprz_hover_pitch[1]) / 10000.;
323 dM_dpprz[i] = (
eff_sched_p.DMdpprz_hover_roll[0] + 2*cmd_quat[i] *
eff_sched_p.DMdpprz_hover_roll[1]) / 10000.;
329 float dM_dpprz_right = dM_dpprz[1];
330 float dM_dpprz_left = dM_dpprz[3];
333 roll_motor_p_eff_right =
bound_or_zero(roll_motor_p_eff_right, -1.f, -0.00001f);
336 roll_motor_p_eff_left =
bound_or_zero(roll_motor_p_eff_left, 0.00001f, 1.f);
339 Bound(roll_motor_q_eff, 0, 1);
347 g1g2[
RW_ap][COMMAND_MOTOR_RIGHT] = roll_motor_p_eff_right;
348 g1g2[
RW_aq][COMMAND_MOTOR_RIGHT] = roll_motor_q_eff;
351 g1g2[
RW_ap][COMMAND_MOTOR_LEFT] = roll_motor_p_eff_left;
352 g1g2[
RW_aq][COMMAND_MOTOR_LEFT] = -roll_motor_q_eff;
364 float elevator_ineffectiveness_scaling = (50-de)/40;
365 Bound(elevator_ineffectiveness_scaling, 0.5, 1.0);
367 float dMydpprz = dMyde *
eff_sched_p.k_elevator_deflection[1];
372 Bound(eff_y_elev, 0.00001, 0.1);
374 g1g2[
RW_aq][COMMAND_ELEVATOR] = eff_y_elev;
385 float dMzdpprz = dMzdr *
eff_sched_p.d_rudder_d_pprz;
390 Bound(eff_z_rudder, 0.000001, 0.1);
392 g1g2[
RW_ar][COMMAND_RUDDER] = eff_z_rudder;
399 Bound(eff_x_aileron, 0, 0.005);
400 g1g2[
RW_ap][COMMAND_AILERONS] = eff_x_aileron;
404 eff_y_aileron =
bound_or_zero(eff_y_aileron, 0.00003f, 0.005f);
405 g1g2[
RW_aq][COMMAND_AILERONS] = eff_y_aileron;
412 Bound(eff_x_flap_aileron, 0, 0.005);
413 g1g2[
RW_ap][COMMAND_FLAPS] = eff_x_flap_aileron;
423 float eff_pusher = (dFxdrpmP * drpmPdpprz /
eff_sched_p.m) / 10000.;
425 Bound(eff_pusher, 0.00030, 0.0015);
426 g1g2[
RW_aX][COMMAND_THRUST_X] = eff_pusher;
437 float lift_d = lift_d_wing + lift_d_fuselage + lift_d_tail;
441 Bound(lift_d, -130., 0.);
455 for (
uint8_t i = 0; i < INDI_NUM_ACT; i++) {
461 case COMMAND_ELEVATOR:
471 case COMMAND_THRUST_X:
491 Bound(fixed_wing_percentile, 0, 1);
492#define AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT 16
497 wls_guid_p.Wv[0] = Wv_original[0] * (1.0f + fixed_wing_percentile *
504 Bound(du_min_thrust_z, -50., 0.);
509 Bound(du_max_thrust_z, 0., 50.);
516 float quad_pitch_limit_rad = RadOfDeg(5.0);
520 float scheduled_pitch_angle = 0.f;
521 float pitch_angle_range = 3.;
523 Bound(meas_skew_angle, 0, 90);
527 max_pitch_limit_rad = quad_pitch_limit_rad;
530 Bound(pitch_progression, 0.f, 1.f);
531 scheduled_pitch_angle = pitch_angle_range * pitch_progression +
ROTWING_QUAD_PREF_PITCH*(1.f-pitch_progression);
532 wls_guid_p.Wu[1] =
Wu_gih_original[1] * (1.f - pitch_progression*0.99);
533 max_pitch_limit_rad = quad_pitch_limit_rad + (fwd_pitch_limit_rad - quad_pitch_limit_rad) * pitch_progression;
536 scheduled_pitch_angle = 8.;
537 max_pitch_limit_rad = fwd_pitch_limit_rad;
539 Bound(scheduled_pitch_angle, -5., 8.);
545 wls_guid_p.u_min[0] = -roll_limit_rad - roll_angle;
546 wls_guid_p.u_min[1] = min_pitch_limit_rad - pitch_angle;
549 wls_guid_p.u_max[0] = roll_limit_rad - roll_angle;
550 wls_guid_p.u_max[1] = max_pitch_limit_rad - pitch_angle;
553 wls_guid_p.u_min[2] = du_min_thrust_z;
554 wls_guid_p.u_max[2] = du_max_thrust_z;
556 wls_guid_p.u_min[2] = 0.;
557 wls_guid_p.u_max[2] = 0.;
562 wls_guid_p.u_max[3] = 9.0;
564 wls_guid_p.u_min[3] = 0.;
565 wls_guid_p.u_max[3] = 0.;
569 wls_guid_p.u_pref[0] = 0;
570 wls_guid_p.u_pref[1] = -pitch_angle + pitch_pref_rad;
571 wls_guid_p.u_pref[2] = wls_guid_p.u_max[2];
572 wls_guid_p.u_pref[3] = body_v[0];
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
Core autopilot interface common to all firmwares.
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)
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
void eff_scheduling_rotwing_update_MMOI(void)
#define WING_ROTATION_CAN_ROTWING_ID
ABI binding wing position data.
void eff_scheduling_rotwing_update_airspeed(void)
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)
#define AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT
void eff_scheduling_rotwing_update_wing_angle(void)
struct rotwing_eff_sched_param_t eff_sched_p
void eff_scheduling_rotwing_update_hover_motor_effectiveness(void)
static abi_event wing_position_ev
void eff_scheduling_rotwing_init(void)
void eff_scheduling_rotwing_update_elevator_effectiveness(void)
float eff_sched_pusher_time
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)
static void wing_position_cb(uint8_t sender_id UNUSED, struct act_feedback_t *pos_msg, uint8_t num_act)
void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle)
static float stateGetAirspeed_f(void)
Get airspeed (float).
float guidance_indi_max_bank
#define GUIDANCE_INDI_WLS_WU
float guidance_indi_pitch_pref_deg
#define GUIDANCE_INDI_WLS_PRIORITIES
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)
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 act_pref[INDI_NUM_ACT]
bool act_is_servo[INDI_NUM_ACT]
float g1g2[INDI_OUTPUTS][INDI_NUM_ACT]
float actuator_state_filt_vect[INDI_NUM_ACT]
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...
API to get/set the generic vehicle states.
int int32_t
Typedef defining 32 bit int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.