27#include "generated/airframe.h"
42#ifndef ROTWING_EFF_SCHED_IXX_BODY
43#error "NO ROTWING_EFF_SCHED_IXX_BODY defined"
46#ifndef ROTWING_EFF_SCHED_IYY_BODY
47#error "NO ROTWING_EFF_SCHED_IYY_BODY defined"
50#ifndef ROTWING_EFF_SCHED_IZZ
51#error "NO ROTWING_EFF_SCHED_IZZ defined"
54#ifndef ROTWING_EFF_SCHED_IXX_WING
55#error "NO ROTWING_EFF_SCHED_IXX_WING defined"
58#ifndef ROTWING_EFF_SCHED_IYY_WING
59#error "NO ROTWING_EFF_SCHED_IYY_WING defined"
62#ifndef ROTWING_EFF_SCHED_M
63#error "NO ROTWING_EFF_SCHED_M defined"
110#ifndef WING_ROTATION_CAN_ROTWING_ID
111#define WING_ROTATION_CAN_ROTWING_ID ABI_BROADCAST
119 for (
int i=0; i<num_act; i++){
120 if (pos_msg[i].set.position && (pos_msg[i].idx == SERVO_ROTATION_MECH_IDX))
122 skew_meas = 0.5 * M_PI - pos_msg[i].position;
128#include "generated/modules.h"
135 float tau_skew = 1.0 / (2.0 * M_PI * 5.0);
136 float tau_att = 1.0 / (2.0 * M_PI * 2.0);
137 float sample_time = 1.0 / PERIODIC_FREQUENCY;
147 RW.I.b_xx = ROTWING_EFF_SCHED_IXX_BODY;
148 RW.I.b_yy = ROTWING_EFF_SCHED_IYY_BODY;
149 RW.I.w_xx = ROTWING_EFF_SCHED_IXX_WING;
150 RW.I.w_yy = ROTWING_EFF_SCHED_IYY_WING;
151 RW.I.xx = (
RW.I.b_xx +
RW.I.w_xx);
152 RW.I.yy =
RW.I.b_yy +
RW.I.w_yy;
153 RW.I.zz = ROTWING_EFF_SCHED_IZZ;
154 RW.m = ROTWING_EFF_SCHED_M;
162 printf(
"RW.mF.dFdu: %f\n",
RW.mF.dFdu);
163 RW.mF.l = ROTWING_EFF_SCHED_MF_l;
169 RW.mR.l = ROTWING_EFF_SCHED_MR_l;
175 RW.mB.l = ROTWING_EFF_SCHED_MB_l;
181 RW.mL.l = ROTWING_EFF_SCHED_ML_l;
187 RW.mP.l = ROTWING_EFF_SCHED_MP_l;
211 RW.wing.k0 = 0.336 + 0.0507 + 0.102;
262 float sigma = (
RW.I.b_xx*
RW.I.b_yy +
RW.I.b_xx*
RW.I.w_yy*
RW.skew.cosr2 +
RW.I.b_yy*
RW.I.w_xx*
RW.skew.cosr2 +
RW.I.w_xx*
RW.I.w_yy*
RW.skew.cosr4 +
RW.I.b_xx*
RW.I.w_xx*
RW.skew.sinr2 +
RW.I.b_yy*
RW.I.w_yy*
RW.skew.sinr2 +
RW.I.w_xx*
RW.I.w_yy*
RW.skew.sinr4 + 2*
RW.I.w_xx*
RW.I.w_yy*
RW.skew.cosr2*
RW.skew.sinr2);
263 I_inv[x][x] = (
RW.I.w_yy*
RW.skew.cosr2 +
RW.I.w_xx*
RW.skew.sinr2 +
RW.I.b_yy)/(sigma);
264 I_inv[x][y] = (
RW.skew.cosr*
RW.skew.sinr*(
RW.I.w_xx -
RW.I.w_yy))/sigma;
267 I_inv[y][y] = (
RW.I.w_xx*
RW.skew.cosr2 +
RW.I.w_yy*
RW.skew.sinr2 +
RW.I.b_xx)/sigma;
279 float sigma1 =
I_inv[y][y]*
RW.skew.sinr -
I_inv[x][y]*
RW.skew.cosr;
280 float sigma2 =
I_inv[x][x]*
RW.skew.cosr -
I_inv[x][y]*
RW.skew.sinr;
286 G2_RW[COMMAND_MOTOR_FRONT] = -
G2_on*
RW.mF.dMdud *
I_inv[z][z] * PERIODIC_FREQUENCY;
289 G1_RW[
RW_ap][COMMAND_MOTOR_RIGHT] = -
RW.mR.dFdu *
RW.mR.l * sigma2;
290 G1_RW[
RW_aq][COMMAND_MOTOR_RIGHT] =
RW.mR.dFdu *
RW.mR.l * sigma1;
292 G2_RW[COMMAND_MOTOR_RIGHT] =
G2_on*
RW.mR.dMdud *
I_inv[z][z] * PERIODIC_FREQUENCY;
298 G2_RW[COMMAND_MOTOR_BACK] = -
G2_on*
RW.mB.dMdud *
I_inv[z][z] * PERIODIC_FREQUENCY;
302 G1_RW[
RW_aq][COMMAND_MOTOR_LEFT] = -
RW.mL.dFdu *
RW.mL.l * sigma1;
304 G2_RW[COMMAND_MOTOR_LEFT] =
G2_on*
RW.mL.dMdud *
I_inv[z][z] * PERIODIC_FREQUENCY;
314 float Mx_a =
RW.ail.dFdu *
RW.as2 *
RW.ail.l *
RW.skew.sinr3;
315 float My_a =
RW.ail.dFdu *
RW.as2 *
RW.ail.l * (
RW.skew.cosr-
RW.skew.cosr3);
319 float Mx_f =
RW.flp.dFdu *
RW.as2 *
RW.flp.l *
RW.skew.sinr3;
320 float My_f =
RW.flp.dFdu *
RW.as2 *
RW.flp.l * (
RW.skew.cosr-
RW.skew.cosr3);
324 RW.wing.dLdtheta = (
RW.wing.k0 +
RW.wing.k1 *
RW.skew.sinr2) *
RW.as2;
325 Bound(
RW.wing.dLdtheta, 0.0, 1300.0);
326 RW.wing.L =
RW.wing.k0 *
RW.att.theta *
RW.as2 +
RW.wing.k1 *
RW.att.theta *
RW.skew.sinr2 *
RW.as2 +
RW.wing.k2 *
RW.skew.sinr2 *
RW.as2;
327 Bound(
RW.wing.L, 0.0, 350.0);
351 float L =
RW.wing.L /
RW.m;
352 float dLdtheta =
RW.wing.dLdtheta /
RW.m;
353 float T =
RW.T /
RW.m;
354 float P =
RW.P /
RW.m;
355 float T_L =
T + L *
RW.att.ctheta;
356 float P_L =
P + L *
RW.att.stheta;
362 case (COMMAND_MOTOR_FRONT):
363 case (COMMAND_MOTOR_BACK):
364 case (COMMAND_MOTOR_RIGHT):
365 case (COMMAND_MOTOR_LEFT):
373 case (COMMAND_MOTOR_PUSHER):
381 case (COMMAND_ELEVATOR):
382 case (COMMAND_RUDDER):
383 case (COMMAND_AILERONS):
384 case (COMMAND_FLAPS):
401 EFF_MAT_RW[
RW_aN][i] = (-(
RW.att.ctheta *
RW.att.cpsi -
RW.att.sphi *
RW.att.stheta *
RW.att.spsi) *
T - (
RW.att.cpsi *
RW.att.stheta +
RW.att.ctheta *
RW.att.sphi *
RW.att.spsi) *
P -
RW.att.sphi *
RW.att.spsi * dLdtheta);
402 EFF_MAT_RW[
RW_aE][i] = (-(
RW.att.ctheta *
RW.att.spsi +
RW.att.sphi *
RW.att.stheta *
RW.att.cpsi) *
T - (
RW.att.spsi *
RW.att.stheta -
RW.att.cpsi *
RW.att.ctheta *
RW.att.sphi) *
P +
RW.att.sphi *
RW.att.cpsi * dLdtheta);
466 RW.skew.deg =
RW.skew.rad / M_PI * 180.;
467 RW.skew.cosr = cosf(
RW.skew.rad);
468 RW.skew.sinr = sinf(
RW.skew.rad);
469 RW.skew.cosr2 =
RW.skew.cosr *
RW.skew.cosr;
470 RW.skew.sinr2 =
RW.skew.sinr *
RW.skew.sinr;
471 RW.skew.sinr3 =
RW.skew.sinr2 *
RW.skew.sinr;
472 RW.skew.cosr3 =
RW.skew.cosr2 *
RW.skew.cosr;
473 RW.skew.cosr4 =
RW.skew.cosr2 *
RW.skew.cosr2;
474 RW.skew.sinr4 =
RW.skew.sinr2 *
RW.skew.sinr2;
475#ifdef INS_EXT_VISION_ROTATION
485 Bound(
RW.as, 0. , 30.);
487 Bound(
RW.as2, 0. , 900.);
505 return k1*u*u + k2*u + k3;
509 float out= 2.0*k1*u + k2;
510 Bound(out, 1.0e-3, 1.0e-2);
544 Bound(
RW.T, 30.0, 180.0);
554 float T_mF =
calc_thrust_curve(ROTWING_EFF_SCHED_MF_k1, ROTWING_EFF_SCHED_MF_k2, ROTWING_EFF_SCHED_MF_k3, 4800.0);
555 float T_mR =
calc_thrust_curve(ROTWING_EFF_SCHED_MR_k1, ROTWING_EFF_SCHED_MR_k2, ROTWING_EFF_SCHED_MR_k3, 4800.0);
556 float T_mB =
calc_thrust_curve(ROTWING_EFF_SCHED_MB_k1, ROTWING_EFF_SCHED_MB_k2, ROTWING_EFF_SCHED_MB_k3, 4800.0);
557 float T_mL =
calc_thrust_curve(ROTWING_EFF_SCHED_ML_k1, ROTWING_EFF_SCHED_ML_k2, ROTWING_EFF_SCHED_ML_k3, 4800.0);
558 RW.T = T_mF + T_mR + T_mB + T_mL;
559 Bound(
RW.T, 0.0, 180.0);
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
float G2_RW[EFF_MAT_COLS_NB]
float G1_RW[EFF_MAT_ROWS_NB][EFF_MAT_COLS_NB]
struct FloatEulers eulers_zxy_RW_EFF
static Butterworth2LowPass psi_filt
float EFF_MAT_RW[EFF_MAT_ROWS_NB][EFF_MAT_COLS_NB]
void eff_scheduling_rotwing_init(void)
static Butterworth2LowPass theta_filt
static Butterworth2LowPass phi_filt
#define WING_ROTATION_CAN_ROTWING_ID
ABI binding wing position data.
static abi_event wing_position_ev
static void wing_position_cb(uint8_t sender_id UNUSED, struct act_feedback_t *pos_msg, uint8_t num_act)
void update_attitude(void)
float calc_thrust_curve_d(float k1, float k2, float u)
void calc_all_thrust_curve(void)
void eff_scheduling_rotwing_update_airspeed(void)
void ele_pref_sched(void)
void eff_scheduling_rotwing_update_wing_angle(void)
float calc_thrust_curve(float k1, float k2, float k3, float u)
void sum_EFF_MAT_RW(void)
Function that sums g1 and g2 to obtain the g1_g2 matrix.
void eff_scheduling_rotwing_periodic(void)
void init_all_thrust_curve(void)
static Butterworth2LowPass skew_filt
void float_quat_of_eulers(struct FloatQuat *q, const struct FloatEulers *e)
quat of euler roation 'ZYX'
static struct FloatEulers * stateGetNedToBodyEulersZxy_f(void)
Get vehicle body attitude ZXY euler angles (float).
static float stateGetAirspeed_f(void)
Get airspeed (float).
Integrated Navigation System interface.
Simple first order low pass filter with bilinear transform.
struct SecondOrderLowPass Butterworth2LowPass
Second order Butterworth low pass filter.
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, const float tau, const float sample_time, const float value)
Init a second order Butterworth filter.
static float update_butterworth_2_low_pass(Butterworth2LowPass *filter, const float value)
Update second order Butterworth low pass filter state with a new value.
Hardware independent API for actuators (servos, motor controllers).
float actuator_state_1l[ANDI_NUM_ACT_TOT]
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
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.