#include "modules/ctrl/eff_scheduling_rotwing_nB.h"
#include "generated/airframe.h"
#include "state.h"
#include "modules/actuators/actuators.h"
#include "modules/core/abi.h"
#include "filters/low_pass_filter.h"
#include "modules/ins/ins_ext_pose.h"
#include "firmwares/rotorcraft/oneloop/oneloop_nB.h"
#include "generated/modules.h"

Include dependency graph for eff_scheduling_rotwing_nB.c:

Macros
#define	FORCE_ONELOOP

#define	WING_ROTATION_CAN_ROTWING_ID ABI_BROADCAST
	ABI binding wing position data.

Functions
void	eff_scheduling_rotwing_update_wing_angle (void)

void	eff_scheduling_rotwing_update_airspeed (void)

void	ele_pref_sched (void)

void	update_attitude (void)

void	sum_EFF_MAT_RW (void)
	Function that sums g1 and g2 to obtain the g1_g2 matrix.

void	init_RW_Model (void)

void	calc_G1_G2_RW (void)

float	calc_thrust_curve (float k1, float k2, float k3, float u)

float	calc_thrust_curve_d (float k1, float k2, float u)

void	calc_all_thrust_curve (void)

void	init_all_thrust_curve (void)

static void	wing_position_cb (uint8_t sender_id UNUSED, struct act_feedback_t *pos_msg, uint8_t num_act)

void	eff_scheduling_rotwing_init (void)

void	eff_scheduling_rotwing_periodic (void)

Variables
float	actuator_state_filt_vect [EFF_MAT_COLS_NB] = {0}

bool	manual_roll = false

bool	manual_pitch = false

bool	manual_yaw = false

float	G2_RW [EFF_MAT_COLS_NB] = {0}

float	G1_RW [EFF_MAT_ROWS_NB][EFF_MAT_COLS_NB] = {0}

float	EFF_MAT_RW [EFF_MAT_ROWS_NB][EFF_MAT_COLS_NB] = {0}

float	I_inv [3][3] = {0}

static float	flt_cut_a = 1.0e-6

static float	flt_cut_ap = 2.0e-6

static float	flt_cut = 1.0e-6

struct FloatEulers	eulers_zxy_RW_EFF

static Butterworth2LowPass	skew_filt

static Butterworth2LowPass	phi_filt

static Butterworth2LowPass	theta_filt

static Butterworth2LowPass	psi_filt

int	G2_on = 0

bool	airspeed_fake_on = false

float	airspeed_fake = 0.0

float	ele_eff = 19.36

float	roll_eff = 8.9

float	yaw_eff = 0.237

float	ele_min = 0.0

struct RW_Model	RW

int	thrust_curve = 1.0

float	temp_mQ_k = 1.37

static abi_event	wing_position_ev

float	skew_meas = 0.0

static float	sim_time = 0.0f

Detailed Description

Author: Tomaso De Ponti T.M.L.nosp@m..DeP.nosp@m.onti@.nosp@m.tude.nosp@m.lft.n.nosp@m.l The control effectiveness scheduler for the rotating wing drone type

Definition in file eff_scheduling_rotwing_nB.c.

Macro Definition Documentation

◆ FORCE_ONELOOP

#define FORCE_ONELOOP

Definition at line 34 of file eff_scheduling_rotwing_nB.c.

◆ WING_ROTATION_CAN_ROTWING_ID

#define WING_ROTATION_CAN_ROTWING_ID ABI_BROADCAST

ABI binding wing position data.

Definition at line 111 of file eff_scheduling_rotwing_nB.c.

Function Documentation

◆ calc_all_thrust_curve()

void calc_all_thrust_curve ( void )

Definition at line 499 of file eff_scheduling_rotwing_nB.c.

References actuator_state_1l, RW_Model::att, calc_thrust_curve_d(), RW_attitude::cphi, RW_attitude::ctheta, F_M_Body::dFdu, foo, RW_Model::m, RW_Model::mB, RW_Model::mF, RW_Model::mL, RW_Model::mR, RW, RW_G_SCALE, RW_Model::T, temp_mQ_k, and thrust_curve.

Referenced by calc_G1_G2_RW().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ calc_G1_G2_RW()

void calc_G1_G2_RW ( void )

Definition at line 256 of file eff_scheduling_rotwing_nB.c.

References RW_Model::ail, RW_Model::as2, RW_Model::att, I::b_xx, I::b_yy, calc_all_thrust_curve(), RW_skew::cosr, RW_skew::cosr2, RW_skew::cosr3, RW_skew::cosr4, F_M_Body::dFdu, wing_model::dLdtheta, F_M_Body::dMdu, F_M_Body::dMdud, foo, G1_RW, G2_on, G2_RW, RW_Model::I, I_inv, wing_model::k0, wing_model::k1, wing_model::k2, F_M_Body::l, wing_model::L, RW_Model::m, RW_Model::mB, RW_Model::mF, RW_Model::mL, RW_Model::mR, RW_Model::P, RW, RW_ap, RW_aq, RW_ar, RW_aZ, RW_skew::sinr, RW_skew::sinr2, RW_skew::sinr3, RW_skew::sinr4, RW_Model::skew, RW_attitude::theta, I::w_xx, I::w_yy, RW_Model::wing, and I::zz.

Referenced by eff_scheduling_rotwing_periodic().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ calc_thrust_curve()

float calc_thrust_curve	(	float	k1,
		float	k2,
		float	k3,
		float	u
	)

Definition at line 489 of file eff_scheduling_rotwing_nB.c.

Referenced by init_all_thrust_curve().

Here is the caller graph for this function:

◆ calc_thrust_curve_d()

float calc_thrust_curve_d	(	float	k1,
		float	k2,
		float	u
	)

Definition at line 493 of file eff_scheduling_rotwing_nB.c.

References foo.

Referenced by calc_all_thrust_curve(), and init_all_thrust_curve().

Here is the caller graph for this function:

◆ eff_scheduling_rotwing_init()

void eff_scheduling_rotwing_init ( void )

Definition at line 130 of file eff_scheduling_rotwing_nB.c.

References foo, init_butterworth_2_low_pass(), init_RW_Model(), phi_filt, psi_filt, skew_filt, theta_filt, update_attitude(), wing_position_cb(), wing_position_ev, and WING_ROTATION_CAN_ROTWING_ID.

Here is the call graph for this function:

◆ eff_scheduling_rotwing_periodic()

void eff_scheduling_rotwing_periodic ( void )

Definition at line 315 of file eff_scheduling_rotwing_nB.c.

References calc_G1_G2_RW(), eff_scheduling_rotwing_update_airspeed(), eff_scheduling_rotwing_update_wing_angle(), ele_pref_sched(), sum_EFF_MAT_RW(), and update_attitude().

Here is the call graph for this function:

◆ eff_scheduling_rotwing_update_airspeed()

void eff_scheduling_rotwing_update_airspeed ( void )

inline

Definition at line 460 of file eff_scheduling_rotwing_nB.c.

References airspeed_fake, airspeed_fake_on, RW_Model::as, RW_Model::as2, foo, RW, sim_time, and stateGetBodyRates_f().

Referenced by eff_scheduling_rotwing_periodic().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ eff_scheduling_rotwing_update_wing_angle()

void eff_scheduling_rotwing_update_wing_angle ( void )

inline

Definition at line 438 of file eff_scheduling_rotwing_nB.c.

References RW_skew::cosr, RW_skew::cosr2, RW_skew::cosr3, RW_skew::cosr4, RW_skew::deg, float_quat_of_eulers(), foo, SecondOrderLowPass::o, RW_skew::rad, RW, RW_skew::sinr, RW_skew::sinr2, RW_skew::sinr3, RW_skew::sinr4, RW_Model::skew, skew_filt, skew_meas, and update_butterworth_2_low_pass().

Referenced by eff_scheduling_rotwing_periodic().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ ele_pref_sched()

void ele_pref_sched ( void )

Definition at line 479 of file eff_scheduling_rotwing_nB.c.

References RW_Model::as, ELE_MAX_AS, ele_min, ELE_MIN_AS, RW_Model::ele_pref, foo, RW, and ZERO_ELE_PPRZ.

Referenced by eff_scheduling_rotwing_periodic().

Here is the caller graph for this function:

◆ init_all_thrust_curve()

void init_all_thrust_curve ( void )

Definition at line 532 of file eff_scheduling_rotwing_nB.c.

References calc_thrust_curve(), calc_thrust_curve_d(), F_M_Body::dFdu, foo, RW_Model::mB, RW_Model::mF, RW_Model::mL, RW_Model::mR, RW, and RW_Model::T.

Referenced by init_RW_Model().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ init_RW_Model()

void init_RW_Model ( void )

Definition at line 144 of file eff_scheduling_rotwing_nB.c.

References RW_Model::ail, RW_Model::as, RW_Model::as2, RW_Model::att, I::b_xx, I::b_yy, RW_skew::cosr, RW_skew::cosr2, RW_attitude::cphi, RW_attitude::cpsi, RW_attitude::ctheta, RW_skew::deg, F_M_Body::dFdu, F_M_Body::dMdu, F_M_Body::dMdud, RW_Model::ele, ele_eff, RW_Model::ele_pref, RW_Model::flp, foo, RW_Model::I, init_all_thrust_curve(), wing_model::k0, wing_model::k1, wing_model::k2, F_M_Body::l, RW_Model::m, RW_Model::mB, RW_Model::mF, RW_Model::mL, RW_Model::mP, RW_Model::mR, RW_attitude::phi, RW_attitude::psi, RW_skew::rad, RW_Model::rud, RW, RW_G_SCALE, RW_skew::sinr, RW_skew::sinr2, RW_skew::sinr3, RW_Model::skew, RW_attitude::sphi, RW_attitude::spsi, RW_attitude::stheta, RW_attitude::theta, I::w_xx, I::w_yy, RW_Model::wing, I::xx, I::yy, and I::zz.

Referenced by eff_scheduling_rotwing_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ sum_EFF_MAT_RW()

void sum_EFF_MAT_RW ( void )

Function that sums g1 and g2 to obtain the g1_g2 matrix.

It also undoes the scaling that was done to make the values readable FIXME: make this function into a for loop to make it more adaptable to different configurations

Definition at line 332 of file eff_scheduling_rotwing_nB.c.

References RW_Model::att, COMMAND_PITCH, COMMAND_ROLL, RW_attitude::cphi, RW_attitude::cpsi, RW_attitude::ctheta, wing_model::dLdtheta, EFF_MAT_COLS_NB, EFF_MAT_ROWS_NB, EFF_MAT_RW, flt_cut, flt_cut_a, flt_cut_ap, foo, G1_RW, G2_RW, wing_model::L, RW_Model::m, manual_pitch, manual_roll, manual_yaw, RW_Model::P, P, RW, RW_aD, RW_aE, RW_aN, RW_ap, RW_aq, RW_ar, RW_aZ, RW_attitude::sphi, RW_attitude::spsi, RW_attitude::stheta, RW_Model::T, and RW_Model::wing.