28 #include "generated/airframe.h"
38 #include "generated/radio.h"
40 #ifndef SERVO_ROTATION_MECH_IDX
41 #error ctrl_eff_sched_rotwing requires a servo named ROTATION_MECH_IDX
44 #ifndef ROTWING_EFF_SCHED_IXX_BODY
45 #error "NO ROTWING_EFF_SCHED_IXX_BODY defined"
48 #ifndef ROTWING_EFF_SCHED_IYY_BODY
49 #error "NO ROTWING_EFF_SCHED_IYY_BODY defined"
52 #ifndef ROTWING_EFF_SCHED_IZZ
53 #error "NO ROTWING_EFF_SCHED_IZZ defined"
56 #ifndef ROTWING_EFF_SCHED_IXX_WING
57 #error "NO ROTWING_EFF_SCHED_IXX_WING defined"
60 #ifndef ROTWING_EFF_SCHED_IYY_WING
61 #error "NO ROTWING_EFF_SCHED_IYY_WING defined"
64 #ifndef ROTWING_EFF_SCHED_M
65 #error "NO ROTWING_EFF_SCHED_M defined"
68 #ifndef ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_PITCH
69 #error "NO ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_PITCH defined"
72 #ifndef ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_ROLL
73 #error "NO ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_ROLL defined"
76 #ifndef ROTWING_EFF_SCHED_HOVER_ROLL_PITCH_COEF
77 #error "NO ROTWING_EFF_SCHED_HOVER_ROLL_PITCH_COEF defined"
80 #ifndef ROTWING_EFF_SCHED_HOVER_ROLL_ROLL_COEF
81 #error "NO ROTWING_EFF_SCHED_HOVER_ROLL_ROLL_COEF defined"
84 #ifndef ROTWING_EFF_SCHED_K_ELEVATOR
85 #error "NO ROTWING_EFF_SCHED_K_ELEVATOR defined"
88 #ifndef ROTWING_EFF_SCHED_K_RUDDER
89 #error "NO ROTWING_EFF_SCHED_K_RUDDER defined"
92 #ifndef ROTWING_EFF_SCHED_K_AILERON
93 #error "NO ROTWING_EFF_SCHED_K_AILERON defined"
96 #ifndef ROTWING_EFF_SCHED_K_FLAPERON
97 #error "NO ROTWING_EFF_SCHED_K_FLAPERON defined"
100 #ifndef ROTWING_EFF_SCHED_K_PUSHER
101 #error "NO ROTWING_EFF_SCHED_K_PUSHER defined"
104 #ifndef ROTWING_EFF_SCHED_K_ELEVATOR_DEFLECTION
105 #error "NO ROTWING_EFF_SCHED_K_ELEVATOR_DEFLECTION defined"
108 #ifndef ROTWING_EFF_SCHED_D_RUDDER_D_PPRZ
109 #error "NO ROTWING_EFF_SCHED_D_RUDDER_D_PPRZ defined"
112 #ifndef ROTWING_EFF_SCHED_K_RPM_PPRZ_PUSHER
113 #error "NO ROTWING_EFF_SCHED_K_RPM_PPRZ_PUSHER defined"
116 #ifndef ROTWING_EFF_SCHED_K_LIFT_WING
117 #error "NO ROTWING_EFF_SCHED_K_LIFT_WING defined"
120 #ifndef ROTWING_EFF_SCHED_K_LIFT_FUSELAGE
121 #error "NO ROTWING_EFF_SCHED_K_LIFT_FUSELAGE defined"
124 #ifndef ROTWING_EFF_SCHED_K_LIFT_TAIL
125 #error "NO ROTWING_EFF_SCHED_K_LIFT_TAIL defined"
129 .
Ixx_body = ROTWING_EFF_SCHED_IXX_BODY,
130 .Iyy_body = ROTWING_EFF_SCHED_IYY_BODY,
131 .Izz = ROTWING_EFF_SCHED_IZZ,
132 .Ixx_wing = ROTWING_EFF_SCHED_IXX_WING,
133 .Iyy_wing = ROTWING_EFF_SCHED_IYY_WING,
134 .m = ROTWING_EFF_SCHED_M,
135 .DMdpprz_hover_pitch = ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_PITCH,
136 .DMdpprz_hover_roll = ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_ROLL,
137 .hover_roll_pitch_coef = ROTWING_EFF_SCHED_HOVER_ROLL_PITCH_COEF,
138 .hover_roll_roll_coef = ROTWING_EFF_SCHED_HOVER_ROLL_ROLL_COEF,
139 .k_elevator = ROTWING_EFF_SCHED_K_ELEVATOR,
140 .k_rudder = ROTWING_EFF_SCHED_K_RUDDER,
141 .k_aileron = ROTWING_EFF_SCHED_K_AILERON,
142 .k_flaperon = ROTWING_EFF_SCHED_K_FLAPERON,
143 .k_pusher = ROTWING_EFF_SCHED_K_PUSHER,
144 .k_elevator_deflection = ROTWING_EFF_SCHED_K_ELEVATOR_DEFLECTION,
145 .d_rudder_d_pprz = ROTWING_EFF_SCHED_D_RUDDER_D_PPRZ,
146 .k_rpm_pprz_pusher = ROTWING_EFF_SCHED_K_RPM_PPRZ_PUSHER,
147 .k_lift_wing = ROTWING_EFF_SCHED_K_LIFT_WING,
148 .k_lift_fuselage = ROTWING_EFF_SCHED_K_LIFT_FUSELAGE,
149 .k_lift_tail = ROTWING_EFF_SCHED_K_LIFT_TAIL
156 float output = value;
158 if (value < low_lim) {
160 }
else if (value > up_lim) {
164 if (value > up_lim) {
166 }
else if (value < low_lim) {
196 #ifndef WING_ROTATION_CAN_ROTWING_ID
197 #define WING_ROTATION_CAN_ROTWING_ID ABI_BROADCAST
204 for (
int i=0; i<num_act; i++){
205 if (pos_msg[i].set.position && (pos_msg[i].idx == SERVO_ROTATION_MECH_IDX))
230 float hover_thrust = 6000;
312 for (
uint8_t i = 0; i < 4; i++) {
327 float dM_dpprz_right = dM_dpprz[1];
328 float dM_dpprz_left = dM_dpprz[3];;
331 roll_motor_p_eff_right =
bound_or_zero(roll_motor_p_eff_right, -1.f, -0.00001f);
336 roll_motor_p_eff_left += roll_motor_airspeed_compensation;
338 roll_motor_p_eff_left =
bound_or_zero(roll_motor_p_eff_left, 0.00001f, 1.f);
341 Bound(roll_motor_q_eff, 0, 1);
349 #ifdef RADIO_CONTROL_EFF_SWITCH
353 g1g2[0][1] = roll_motor_p_eff_right;
356 g1g2[0][1] = roll_motor_p_eff_right;
359 g1g2[1][1] = roll_motor_q_eff;
362 #ifdef RADIO_CONTROL_EFF_SWITCH
366 g1g2[0][3] = roll_motor_p_eff_left;
369 g1g2[0][3] = roll_motor_p_eff_left;
371 g1g2[1][3] = -roll_motor_q_eff;
383 float elevator_ineffectiveness_scaling = (50-de)/40;
384 Bound(elevator_ineffectiveness_scaling, 0.5, 1.0);
391 Bound(eff_y_elev, 0.00001, 0.1);
393 g1g2[1][5] = eff_y_elev;
409 Bound(eff_z_rudder, 0.000001, 0.1);
411 g1g2[2][4] = eff_z_rudder;
418 Bound(eff_x_aileron, 0, 0.005)
419 g1g2[0][6] = eff_x_aileron;
423 eff_y_aileron =
bound_or_zero(eff_y_aileron, 0.00003f, 0.005f);
424 g1g2[1][6] = eff_y_aileron;
431 Bound(eff_x_flap_aileron, 0, 0.005)
432 g1g2[0][7] = eff_x_flap_aileron;
442 float eff_pusher = (dFxdrpmP * drpmPdpprz /
eff_sched_p.
m) / 10000.;
444 Bound(eff_pusher, 0.00030, 0.0015);
445 g1g2[4][8] = eff_pusher;
456 float lift_d = lift_d_wing + lift_d_fuselage + lift_d_tail;
460 Bound(lift_d, -130., 0.);
472 for (
uint8_t i = 0; i < INDI_NUM_ACT; i++) {
483 BoundAbs(flap_saturation_limit,
MAX_PPRZ);
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
struct pprz_autopilot autopilot
Global autopilot structure.
Core autopilot interface common to all firmwares.
bool in_flight
in flight status
if(GpsFixValid() &&e_identification_started)
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
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)
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)
float DMdpprz_hover_roll[2]
float pitch_motor_dMdpprz
float k_elevator_deflection[2]
float k_rpm_pprz_pusher[3]
float hover_roll_pitch_coef[2]
float DMdpprz_hover_pitch[2]
float hover_roll_roll_coef[2]
static float stateGetAirspeed_f(void)
Get airspeed (float).
Hardware independent API for actuators (servos, motor controllers).
struct RadioControl radio_control
Generic interface for radio control modules.
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
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]
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.
float u_pref[WLS_N_U_MAX]