28#include "generated/airframe.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_K_ELEVATOR
69#error "NO ROTWING_EFF_SCHED_K_ELEVATOR defined"
72#ifndef ROTWING_EFF_SCHED_K_RUDDER
73#error "NO ROTWING_EFF_SCHED_K_RUDDER defined"
76#ifndef ROTWING_EFF_SCHED_K_AILERON
77#error "NO ROTWING_EFF_SCHED_K_AILERON defined"
80#ifndef ROTWING_EFF_SCHED_K_FLAPERON
81#error "NO ROTWING_EFF_SCHED_K_FLAPERON defined"
84#ifndef ROTWING_EFF_SCHED_K_ELEVATOR_DEFLECTION
85#error "NO ROTWING_EFF_SCHED_K_ELEVATOR_DEFLECTION defined"
88#ifndef ROTWING_EFF_SCHED_D_RUDDER_D_PPRZ
89#error "NO ROTWING_EFF_SCHED_D_RUDDER_D_PPRZ defined"
92#ifndef ROTWING_EFF_SCHED_K_LIFT_WING
93#error "NO ROTWING_EFF_SCHED_K_LIFT_WING defined"
96#ifndef ROTWING_EFF_SCHED_K_LIFT_FUSELAGE
97#error "NO ROTWING_EFF_SCHED_K_LIFT_FUSELAGE defined"
100#ifndef ROTWING_EFF_SCHED_K_LIFT_TAIL
101#error "NO ROTWING_EFF_SCHED_K_LIFT_TAIL defined"
104#ifndef ROTWING_EFF_SCHED_CT_HOVER
105#error "NO ROTWING_EFF_SCHED_CT_HOVER defined"
108#ifndef ROTWING_EFF_SCHED_CT_PUSHER
109#error "NO ROTWING_EFF_SCHED_CT_PUSHER defined"
112#ifndef ROTWING_EFF_SCHED_ROLL_ARM
113#error "NO EFF_SCHEDULING_ROTWING_ROLL_ARM defined"
116#ifndef ROTWING_EFF_SCHED_PITCH_ARM
117#error "NO EFF_SCHEDULING_ROTWING_PITCH_ARM defined"
120#ifndef ROTWING_EFF_SCHED_ROLL_ARM_PITCH_MOTORS
121#error "NO ROTWING_EFF_SCHED_ROLL_ARM_PITCH_MOTORS defined"
124#ifndef ROTWING_EFF_SCHED_HOVER_MAX_RPM
125#error "NO ROTWING_EFF_SCHED_HOVER_MAX_RPM defined"
128#ifndef ROTWING_EFF_SCHED_PUSHER_MAX_RPM
129#error "NO ROTWING_EFF_SCHED_PUSHER_MAX_RPM defined"
132#ifndef ROTWING_EFF_SCHED_RPM_CTRL_HOVER_LIM_COEF
133#error "USING RPM CONTROL BUT NO ROTWING_EFF_SCHED_RPM_CTRL_HOVER_LIM_COEF defined"
136#ifndef ROTWING_EFF_SCHED_RPM_CTRL_PUSH_LIM_COEF
137#error "USING RPM CONTROL BUT NO ROTWING_EFF_SCHED_RPM_CTRL_PUSH_LIM_COEF defined"
140#ifndef ROTWING_PITCH_MOTOR_TILT_ANGLE_DEG
141#define ROTWING_PITCH_MOTOR_TILT_ANGLE_DEG 10.0
145 .Ixx_body = ROTWING_EFF_SCHED_IXX_BODY,
146 .Iyy_body = ROTWING_EFF_SCHED_IYY_BODY,
147 .Izz = ROTWING_EFF_SCHED_IZZ,
148 .Ixx_wing = ROTWING_EFF_SCHED_IXX_WING,
149 .Iyy_wing = ROTWING_EFF_SCHED_IYY_WING,
150 .m = ROTWING_EFF_SCHED_M,
151 .CT_hover = ROTWING_EFF_SCHED_CT_HOVER,
152 .CT_pusher = ROTWING_EFF_SCHED_CT_PUSHER,
153 .hover_max_rpm_squared = ROTWING_EFF_SCHED_HOVER_MAX_RPM * ROTWING_EFF_SCHED_HOVER_MAX_RPM,
154 .pusher_max_rpm_squared = ROTWING_EFF_SCHED_PUSHER_MAX_RPM * ROTWING_EFF_SCHED_PUSHER_MAX_RPM,
155 .k_elevator = ROTWING_EFF_SCHED_K_ELEVATOR,
156 .k_rudder = ROTWING_EFF_SCHED_K_RUDDER,
157 .k_aileron = ROTWING_EFF_SCHED_K_AILERON,
158 .k_flaperon = ROTWING_EFF_SCHED_K_FLAPERON,
159 .k_elevator_deflection = ROTWING_EFF_SCHED_K_ELEVATOR_DEFLECTION,
160 .d_rudder_d_pprz = ROTWING_EFF_SCHED_D_RUDDER_D_PPRZ,
161 .k_lift_wing = ROTWING_EFF_SCHED_K_LIFT_WING,
162 .k_lift_fuselage = ROTWING_EFF_SCHED_K_LIFT_FUSELAGE,
163 .k_lift_tail = ROTWING_EFF_SCHED_K_LIFT_TAIL,
164 .hover_rpm_lim_coef = ROTWING_EFF_SCHED_RPM_CTRL_HOVER_LIM_COEF,
165 .pusher_rpm_lim_coef = ROTWING_EFF_SCHED_RPM_CTRL_PUSH_LIM_COEF
173 float output = value;
175 if (value < low_lim) {
177 }
else if (value > up_lim) {
181 if (value > up_lim) {
183 }
else if (value < low_lim) {
216#ifndef WING_ROTATION_CAN_ROTWING_ID
217#define WING_ROTATION_CAN_ROTWING_ID ABI_BROADCAST
224 for (
int i=0; i<num_act; i++){
225 if (pos_msg[i].set.position && (pos_msg[i].idx == SERVO_ROTATION_MECH_IDX))
228 eff_sched_var.wing_rotation_rad = 0.5 * M_PI - pos_msg[i].position;
328 float dMdu_pitch = ROTWING_EFF_SCHED_PITCH_ARM *
eff_sched_p.CT_hover * scaling;
329 float dMdu_roll = ROTWING_EFF_SCHED_ROLL_ARM *
eff_sched_p.CT_hover * scaling;
331 float front_motor_pitch_eff = dMdu_pitch /
eff_sched_var.Iyy;
332 float back_motor_pitch_eff = -dMdu_pitch /
eff_sched_var.Iyy;
335 static const float roll_eff_pitch_motor_scaling = ROTWING_EFF_SCHED_ROLL_ARM_PITCH_MOTORS / ROTWING_EFF_SCHED_ROLL_ARM * sinf(RadOfDeg(
ROTWING_PITCH_MOTOR_TILT_ANGLE_DEG));
338 g1g2[
RW_aq][COMMAND_MOTOR_FRONT] = front_motor_pitch_eff;
342 g1g2[
RW_aq][COMMAND_MOTOR_BACK] = back_motor_pitch_eff;
347 right_motor_roll_eff =
bound_or_zero(right_motor_roll_eff, -1.f, -0.00001f);
350 right_motor_pitch_eff =
bound_or_zero(right_motor_pitch_eff, 0.00001f, front_motor_pitch_eff);
352 g1g2[
RW_ap][COMMAND_MOTOR_RIGHT] = right_motor_roll_eff;
353 g1g2[
RW_aq][COMMAND_MOTOR_RIGHT] = right_motor_pitch_eff;
357 left_motor_roll_eff =
bound_or_zero(left_motor_roll_eff, 0.00001f, 1.f);
360 left_motor_pitch_eff =
bound_or_zero(left_motor_pitch_eff, back_motor_pitch_eff, -0.00001f);
362 g1g2[
RW_ap][COMMAND_MOTOR_LEFT] = left_motor_roll_eff;
363 g1g2[
RW_aq][COMMAND_MOTOR_LEFT] = left_motor_pitch_eff;
375 float elevator_ineffectiveness_scaling = (50-de)/40;
376 Bound(elevator_ineffectiveness_scaling, 0.5, 1.0);
378 float dMydpprz = dMyde *
eff_sched_p.k_elevator_deflection[1];
383 Bound(eff_y_elev, 0.00001, 0.1);
385 g1g2[
RW_aq][COMMAND_ELEVATOR] = eff_y_elev;
396 float dMzdpprz = dMzdr *
eff_sched_p.d_rudder_d_pprz;
401 Bound(eff_z_rudder, 0.000001, 0.1);
403 g1g2[
RW_ar][COMMAND_RUDDER] = eff_z_rudder;
410 Bound(eff_x_aileron, 0, 0.005);
411 g1g2[
RW_ap][COMMAND_AILERONS] = eff_x_aileron;
415 eff_y_aileron =
bound_or_zero(eff_y_aileron, 0.00003f, 0.005f);
416 g1g2[
RW_aq][COMMAND_AILERONS] = eff_y_aileron;
423 Bound(eff_x_flap_aileron, 0, 0.005);
424 g1g2[
RW_ap][COMMAND_FLAPS] = eff_x_flap_aileron;
431 float drpmPdpprz =
eff_sched_p.CT_pusher * scaling;
435 Bound(eff_pusher, 0.00030, 0.0015);
437 g1g2[
RW_aX][COMMAND_THRUST_X] = eff_pusher;
446 float lift_d = lift_d_wing + lift_d_fuselage + lift_d_tail;
450 Bound(lift_d, -130., 0.);
464 for (
uint8_t i = 0; i < INDI_NUM_ACT; i++) {
471 case COMMAND_MOTOR_FRONT:
473 wls_stab_p.u_min[i] = (
int16_t)((
float)SERVO_MOTOR_FRONT_NEUTRAL * ((
float)SERVO_MOTOR_FRONT_NEUTRAL * ((
float)
MAX_PPRZ / (
float)SERVO_MOTOR_FRONT_MAX) / (
float)SERVO_MOTOR_FRONT_MAX));
475 case COMMAND_MOTOR_RIGHT:
477 wls_stab_p.u_min[i] = (
int16_t)((
float)SERVO_MOTOR_RIGHT_NEUTRAL * ((
float)SERVO_MOTOR_RIGHT_NEUTRAL * ((
float)
MAX_PPRZ / (
float)SERVO_MOTOR_RIGHT_MAX) / (
float)SERVO_MOTOR_RIGHT_MAX));
479 case COMMAND_MOTOR_BACK:
481 wls_stab_p.u_min[i] = (
int16_t)((
float)SERVO_MOTOR_BACK_NEUTRAL * ((
float)SERVO_MOTOR_BACK_NEUTRAL * ((
float)
MAX_PPRZ / (
float)SERVO_MOTOR_BACK_MAX) / (
float)SERVO_MOTOR_BACK_MAX));
483 case COMMAND_MOTOR_LEFT:
485 wls_stab_p.u_min[i] = (
int16_t)((
float)SERVO_MOTOR_LEFT_NEUTRAL * ((
float)SERVO_MOTOR_LEFT_NEUTRAL * ((
float)
MAX_PPRZ / (
float)SERVO_MOTOR_LEFT_MAX) / (
float)SERVO_MOTOR_LEFT_MAX));
487 case COMMAND_ELEVATOR:
497 case COMMAND_THRUST_X:
505 wls_stab_p.u_min[i] = Max((
int16_t)((
float)SERVO_MOTOR_PUSH_NEUTRAL * ((
float)SERVO_MOTOR_PUSH_NEUTRAL * ((
float)
MAX_PPRZ / (
float)SERVO_MOTOR_PUSH_MAX) / (
float)SERVO_MOTOR_PUSH_MAX)),
wls_stab_p.u_min[i]);
521 Bound(fixed_wing_percentile, 0, 1);
522#define AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT 16
527 wls_guid_p.Wv[0] = Wv_original[0] * (1.0f + fixed_wing_percentile *
537 Bound(du_min_thrust_z, -50., 0.);
542 Bound(du_max_thrust_z, 0., 50.);
549 float quad_pitch_limit_rad = RadOfDeg(5.0);
553 float scheduled_pitch_angle = 0.f;
554 float pitch_angle_range = 3.;
556 Bound(meas_skew_angle, 0, 90);
560 max_pitch_limit_rad = quad_pitch_limit_rad;
562 float pitch_progression = (airspeed -
rotwing_state.fw_min_airspeed) / 2.f;
563 Bound(pitch_progression, 0.f, 1.f);
564 scheduled_pitch_angle = pitch_angle_range * pitch_progression +
ROTWING_QUAD_PREF_PITCH*(1.f-pitch_progression);
565 wls_guid_p.Wu[1] =
Wu_gih_original[1] * (1.f - pitch_progression*0.99);
566 max_pitch_limit_rad = quad_pitch_limit_rad + (fwd_pitch_limit_rad - quad_pitch_limit_rad) * pitch_progression;
569 scheduled_pitch_angle = 8.;
570 max_pitch_limit_rad = fwd_pitch_limit_rad;
572 Bound(scheduled_pitch_angle, -5., 8.);
578 wls_guid_p.u_min[0] = -roll_limit_rad - roll_angle;
579 wls_guid_p.u_min[1] = min_pitch_limit_rad - pitch_angle;
582 wls_guid_p.u_max[0] = roll_limit_rad - roll_angle;
583 wls_guid_p.u_max[1] = max_pitch_limit_rad - pitch_angle;
586 wls_guid_p.u_min[2] = du_min_thrust_z;
587 wls_guid_p.u_max[2] = du_max_thrust_z;
589 wls_guid_p.u_min[2] = 0.;
590 wls_guid_p.u_max[2] = 0.;
595 wls_guid_p.u_max[3] = 9.0;
597 wls_guid_p.u_min[3] = 0.;
598 wls_guid_p.u_max[3] = 0.;
602 wls_guid_p.u_pref[0] = 0;
603 wls_guid_p.u_pref[1] = -pitch_angle + pitch_pref_rad;
604 wls_guid_p.u_pref[2] = wls_guid_p.u_max[2];
605 wls_guid_p.u_pref[3] = body_v[0];
618 float u = (u_squared > 0 ? 1 : (u_squared < 0 ? -1 : 0)) * sqrtf(fabsf(u_squared) / (
float)
MAX_PPRZ * max * max);
619 float u_unbounded = (u - neutral) / travel_up;
620 return (u_unbounded < 0) ? 0 : u_unbounded;
633 float u_can_cmd = pprz_cmd * travel_up + neutral;
634 return (u_can_cmd * u_can_cmd) / (max * max) * (
float)
MAX_PPRZ;
647 for (
uint8_t i = 0; i < INDI_NUM_ACT; i++) {
650 case COMMAND_MOTOR_FRONT:
656 case COMMAND_MOTOR_RIGHT:
662 case COMMAND_MOTOR_BACK:
668 case COMMAND_MOTOR_LEFT:
674 case COMMAND_THRUST_X:
695 float act_state_pprz;
697 cmd[COMMAND_THRUST] = 0;
699 for (
uint8_t i = 0; i < INDI_NUM_ACT; i++) {
702 case COMMAND_MOTOR_FRONT:
706 case COMMAND_MOTOR_RIGHT:
710 case COMMAND_MOTOR_BACK:
714 case COMMAND_MOTOR_LEFT:
718 case COMMAND_THRUST_X:
730 if (act_to_commands != NULL) {
734 cmd[COMMAND_THRUST] += act_state_pprz * (
int32_t) act_is_thruster_z[i];
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
Core autopilot interface common to all firmwares.
Hardware independent code for commands handling.
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
#define WING_ROTATION_CAN_ROTWING_ID
ABI binding wing position data.
#define AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT
struct rotwing_eff_sched_param_t eff_sched_p
static abi_event wing_position_ev
float eff_sched_pusher_time
static void wing_position_cb(uint8_t sender_id UNUSED, struct act_feedback_t *pos_msg, uint8_t num_act)
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)
static float u_squared_to_pprz_cmd(float u_squared, float max, float neutral, float travel_up)
Convert u^2 to PPRZ command.
void eff_scheduling_rotwing_update_MMOI(void)
void eff_scheduling_rotwing_update_airspeed(void)
void stabilization_indi_get_actuator_state(void)
Function that tries to get actuator feedback.
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)
static float pprz_cmd_to_u_squared(float pprz_cmd, float max, float neutral, float travel_up)
Convert PPRZ command back to u^2.
void eff_scheduling_rotwing_update_hover_motor_effectiveness(void)
void eff_scheduling_rotwing_init(void)
void eff_scheduling_rotwing_update_elevator_effectiveness(void)
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)
void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle)
#define ROTWING_PITCH_MOTOR_TILT_ANGLE_DEG
void stabilization_indi_commit_actuator_cmd(int32_t *cmd)
struct Electrical electrical
void float_eulers_of_quat_zxy(struct FloatEulers *e, const 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).
float guidance_indi_max_bank
#define GUIDANCE_INDI_WLS_WU
float guidance_indi_pitch_pref_deg
#define GUIDANCE_INDI_WLS_PRIORITIES
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).
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 actuator_state[ANDI_NUM_ACT]
float act_pref[INDI_NUM_ACT]
float * stabilization_indi_get_act_state(void)
bool act_is_servo[INDI_NUM_ACT]
float g1g2[INDI_OUTPUTS][INDI_NUM_ACT]
bool * stabilization_indi_get_act_is_thruster_z(void)
float * stabilization_indi_get_act_dyn(void)
float actuator_state_filt_vect[INDI_NUM_ACT]
float act_dyn_discrete[INDI_NUM_ACT]
uint8_t * stabilization_indi_get_act_to_commands(void)
int32_t stabilization_indi_get_num_thrusters(void)
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...
float indi_u[INDI_NUM_ACT]
float * stabilization_indi_get_indi_u(void)
API to get/set the generic vehicle states.
int int32_t
Typedef defining 32 bit int type.
short int16_t
Typedef defining 16 bit short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.