28 #include "generated/airframe.h"
36 #ifndef SERVO_ROTATION_MECH_IDX
37 #error ctrl_eff_sched_rot_wing requires a servo named ROTATION_MECH_IDX
40 #ifndef ROT_WING_EFF_SCHED_IXX_BODY
41 #error "NO ROT_WING_EFF_SCHED_IXX_BODY defined"
44 #ifndef ROT_WING_EFF_SCHED_IYY_BODY
45 #error "NO ROT_WING_EFF_SCHED_IYY_BODY defined"
48 #ifndef ROT_WING_EFF_SCHED_IZZ
49 #error "NO ROT_WING_EFF_SCHED_IZZ defined"
52 #ifndef ROT_WING_EFF_SCHED_IXX_WING
53 #error "NO ROT_WING_EFF_SCHED_IXX_WING defined"
56 #ifndef ROT_WING_EFF_SCHED_IYY_WING
57 #error "NO ROT_WING_EFF_SCHED_IYY_WING defined"
60 #ifndef ROT_WING_EFF_SCHED_M
61 #error "NO ROT_WING_EFF_SCHED_M defined"
64 #ifndef ROT_WING_EFF_SCHED_DM_DPPRZ_HOVER_PITCH
65 #error "NO ROT_WING_EFF_SCHED_DM_DPPRZ_HOVER_PITCH defined"
68 #ifndef ROT_WING_EFF_SCHED_DM_DPPRZ_HOVER_ROLL
69 #error "NO ROT_WING_EFF_SCHED_DM_DPPRZ_HOVER_ROLL defined"
72 #ifndef ROT_WING_EFF_SCHED_HOVER_ROLL_PITCH_COEF
73 #error "NO ROT_WING_EFF_SCHED_HOVER_ROLL_PITCH_COEF defined"
76 #ifndef ROT_WING_EFF_SCHED_HOVER_ROLL_ROLL_COEF
77 #error "NO ROT_WING_EFF_SCHED_HOVER_ROLL_ROLL_COEF defined"
80 #ifndef ROT_WING_EFF_SCHED_K_ELEVATOR
81 #error "NO ROT_WING_EFF_SCHED_K_ELEVATOR defined"
84 #ifndef ROT_WING_EFF_SCHED_K_RUDDER
85 #error "NO ROT_WING_EFF_SCHED_K_RUDDER defined"
88 #ifndef ROT_WING_EFF_SCHED_K_AILERON
89 #error "NO ROT_WING_EFF_SCHED_K_AILERON defined"
92 #ifndef ROT_WING_EFF_SCHED_K_FLAPERON
93 #error "NO ROT_WING_EFF_SCHED_K_FLAPERON defined"
96 #ifndef ROT_WING_EFF_SCHED_K_PUSHER
97 #error "NO ROT_WING_EFF_SCHED_K_PUSHER defined"
100 #ifndef ROT_WING_EFF_SCHED_K_ELEVATOR_DEFLECTION
101 #error "NO ROT_WING_EFF_SCHED_K_ELEVATOR_DEFLECTION defined"
104 #ifndef ROT_WING_EFF_SCHED_D_RUDDER_D_PPRZ
105 #error "NO ROT_WING_EFF_SCHED_D_RUDDER_D_PPRZ defined"
108 #ifndef ROT_WING_EFF_SCHED_K_RPM_PPRZ_PUSHER
109 #error "NO ROT_WING_EFF_SCHED_K_RPM_PPRZ_PUSHER defined"
112 #ifndef ROT_WING_EFF_SCHED_K_LIFT_WING
113 #error "NO ROT_WING_EFF_SCHED_K_LIFT_WING defined"
116 #ifndef ROT_WING_EFF_SCHED_K_LIFT_FUSELAGE
117 #error "NO ROT_WING_EFF_SCHED_K_LIFT_FUSELAGE defined"
120 #ifndef ROT_WING_EFF_SCHED_K_LIFT_TAIL
121 #error "NO ROT_WING_EFF_SCHED_K_LIFT_TAIL defined"
125 .
Ixx_body = ROT_WING_EFF_SCHED_IXX_BODY,
126 .Iyy_body = ROT_WING_EFF_SCHED_IYY_BODY,
127 .Izz = ROT_WING_EFF_SCHED_IZZ,
128 .Ixx_wing = ROT_WING_EFF_SCHED_IXX_WING,
129 .Iyy_wing = ROT_WING_EFF_SCHED_IYY_WING,
130 .m = ROT_WING_EFF_SCHED_M,
131 .DMdpprz_hover_roll = ROT_WING_EFF_SCHED_DM_DPPRZ_HOVER_ROLL,
132 .hover_roll_pitch_coef = ROT_WING_EFF_SCHED_HOVER_ROLL_PITCH_COEF,
133 .hover_roll_roll_coef = ROT_WING_EFF_SCHED_HOVER_ROLL_ROLL_COEF,
134 .k_elevator = ROT_WING_EFF_SCHED_K_ELEVATOR,
135 .k_rudder = ROT_WING_EFF_SCHED_K_RUDDER,
136 .k_aileron = ROT_WING_EFF_SCHED_K_AILERON,
137 .k_flaperon = ROT_WING_EFF_SCHED_K_FLAPERON,
138 .k_pusher = ROT_WING_EFF_SCHED_K_PUSHER,
139 .k_elevator_deflection = ROT_WING_EFF_SCHED_K_ELEVATOR_DEFLECTION,
140 .d_rudder_d_pprz = ROT_WING_EFF_SCHED_D_RUDDER_D_PPRZ,
141 .k_rpm_pprz_pusher = ROT_WING_EFF_SCHED_K_RPM_PPRZ_PUSHER,
142 .k_lift_wing = ROT_WING_EFF_SCHED_K_LIFT_WING,
143 .k_lift_fuselage = ROT_WING_EFF_SCHED_K_LIFT_FUSELAGE,
144 .k_lift_tail = ROT_WING_EFF_SCHED_K_LIFT_TAIL
167 #ifndef WING_ROTATION_CAN_ROT_WING_ID
168 #define WING_ROTATION_CAN_ROT_WING_ID ABI_BROADCAST
175 for (
int i=0; i<num_act; i++){
176 if (pos_msg[i].set.position && (pos_msg[i].idx == SERVO_ROTATION_MECH_IDX))
292 Bound(roll_motor_p_eff_right, -1, -0.00001);
296 roll_motor_p_eff_left += roll_motor_airspeed_compensation;
297 Bound(roll_motor_p_eff_left, 0.00001, 1);
300 Bound(roll_motor_q_eff, 0, 1);
303 g1g2[1][0] = pitch_motor_q_eff;
306 g1g2[1][2] = -pitch_motor_q_eff;
309 g1g2[0][1] = roll_motor_p_eff_right;
310 g1g2[1][1] = roll_motor_q_eff;
313 g1g2[0][3] = roll_motor_p_eff_left;
314 g1g2[1][3] = -roll_motor_q_eff;
330 Bound(eff_y_elev, 0.00001, 0.1);
332 g1g2[1][5] = eff_y_elev;
348 Bound(eff_z_rudder, 0.000001, 0.1);
350 g1g2[2][4] = eff_z_rudder;
357 Bound(eff_x_aileron, 0, 0.005)
358 g1g2[0][6] = eff_x_aileron;
365 Bound(eff_x_flap_aileron, 0, 0.005)
366 g1g2[0][7] = eff_x_flap_aileron;
376 float eff_pusher = (dFxdrpmP * drpmPdpprz /
eff_sched_p.
m) / 10000.;
378 Bound(eff_pusher, 0.00030, 0.0015);
379 g1g2[4][8] = eff_pusher;
390 float lift_d = lift_d_wing + lift_d_fuselage + lift_d_tail;
394 Bound(lift_d, -130., 0.);
406 for (
uint8_t i = 0; i < INDI_NUM_ACT; i++) {
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
if(GpsFixValid() &&e_identification_started)
void eff_scheduling_rot_wing_update_wing_angle(void)
void eff_scheduling_rot_wing_init(void)
void stabilization_indi_set_wls_settings(void)
Function that sets the du_min, du_max and du_pref if function not elsewhere defined.
void eff_scheduling_rot_wing_update_elevator_effectiveness(void)
void eff_scheduling_rot_wing_periodic(void)
struct rot_wing_eff_sched_param_t eff_sched_p
void eff_scheduling_rot_wing_schedule_liftd(void)
struct rot_wing_eff_sched_var_t eff_sched_var
void eff_scheduling_rot_wing_update_pusher_effectiveness(void)
float guidance_indi_get_liftd(float pitch UNUSED, float theta UNUSED)
#define WING_ROTATION_CAN_ROT_WING_ID
ABI binding wing position data.
void eff_scheduling_rot_wing_update_aileron_effectiveness(void)
static abi_event wing_position_ev
void eff_scheduling_rot_wing_update_hover_motor_effectiveness(void)
void eff_scheduling_rot_wing_update_cmd(void)
void eff_scheduling_rot_wing_update_rudder_effectiveness(void)
static void wing_position_cb(uint8_t sender_id UNUSED, struct act_feedback_t *pos_msg, uint8_t num_act)
void eff_scheduling_rot_wing_update_flaperon_effectiveness(void)
float eff_scheduling_rot_wing_lift_d
void eff_scheduling_rot_wing_update_MMOI(void)
void eff_scheduling_rot_wing_update_airspeed(void)
float hover_roll_roll_coef[2]
float k_rpm_pprz_pusher[3]
float DMdpprz_hover_roll[2]
float pitch_motor_dMdpprz
float k_elevator_deflection[2]
float hover_roll_pitch_coef[2]
static float stateGetAirspeed_f(void)
Get airspeed (float).
Hardware independent API for actuators (servos, motor controllers).
float act_pref[INDI_NUM_ACT]
float u_min_stab_indi[INDI_NUM_ACT]
bool act_is_servo[INDI_NUM_ACT]
float u_max_stab_indi[INDI_NUM_ACT]
float g1g2[INDI_OUTPUTS][INDI_NUM_ACT]
float u_pref_stab_indi[INDI_NUM_ACT]
float actuator_state_filt_vect[INDI_NUM_ACT]
API to get/set the generic vehicle states.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.