#include "modules/ctrl/eff_scheduling_plusquad.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_plusquad.c:

Macros
#define	FORCE_ONELOOP

Functions
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)

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	roll_mult = 1.0

float	pitch_mult = 1.0

float	yaw_mult = 1.0

float	thrust_mult = 1.0

float	cutoff_prev = 1.0

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-3

static float	flt_cut = 1.0e-4

struct FloatEulers	eulers_zxy_RW_EFF

static Butterworth2LowPass	phi_filt

static Butterworth2LowPass	theta_filt

static Butterworth2LowPass	psi_filt

int	G2_on = 1

float	roll_eff = 8.9

float	yaw_eff = 0.237

float	ele_min = 0.0

struct RW_Model	RW

int	thrust_curve = 2

float	temp_mQ_k = 1.37

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 plus quad drone type

Definition in file eff_scheduling_plusquad.c.

Macro Definition Documentation

◆ FORCE_ONELOOP

#define FORCE_ONELOOP

Definition at line 34 of file eff_scheduling_plusquad.c.

Function Documentation

◆ calc_all_thrust_curve()

void calc_all_thrust_curve ( void )

Definition at line 346 of file eff_scheduling_plusquad.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_Model::T, thrust_curve, and thrust_mult.

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 188 of file eff_scheduling_plusquad.c.

References calc_all_thrust_curve(), F_M_Body::dFdu, F_M_Body::dMdu, F_M_Body::dMdud, foo, G1_RW, G2_on, G2_RW, RW_Model::I, I_inv, F_M_Body::l, RW_Model::m, RW_Model::mB, RW_Model::mF, RW_Model::mL, RW_Model::mR, RW, RW_ap, RW_aq, RW_ar, RW_aZ, I::xx, I::yy, 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 336 of file eff_scheduling_plusquad.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 340 of file eff_scheduling_plusquad.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 102 of file eff_scheduling_plusquad.c.

References foo, init_butterworth_2_low_pass(), init_RW_Model(), oneloop_nB_filt_cutoff, phi_filt, psi_filt, theta_filt, and update_attitude().

Here is the call graph for this function:

◆ eff_scheduling_rotwing_periodic()

void eff_scheduling_rotwing_periodic ( void )

Definition at line 236 of file eff_scheduling_plusquad.c.

References calc_G1_G2_RW(), sum_EFF_MAT_RW(), and update_attitude().

Here is the call graph for this function:

◆ init_all_thrust_curve()

void init_all_thrust_curve ( void )

Definition at line 379 of file eff_scheduling_plusquad.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 113 of file eff_scheduling_plusquad.c.

References RW_Model::att, RW_attitude::cphi, RW_attitude::cpsi, RW_attitude::ctheta, cutoff_prev, F_M_Body::dFdu, F_M_Body::dMdu, F_M_Body::dMdud, foo, RW_Model::I, init_all_thrust_curve(), F_M_Body::l, RW_Model::m, RW_Model::mB, RW_Model::mF, RW_Model::mL, RW_Model::mR, oneloop_nB_filt_cutoff, RW_attitude::phi, RW_attitude::psi, RW, RW_G_SCALE, RW_attitude::sphi, RW_attitude::spsi, RW_attitude::stheta, RW_attitude::theta, 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 250 of file eff_scheduling_plusquad.c.

References RW_Model::att, COMMAND_PITCH, COMMAND_ROLL, RW_attitude::cphi, RW_attitude::cpsi, RW_attitude::ctheta, EFF_MAT_COLS_NB, EFF_MAT_ROWS_NB, EFF_MAT_RW, flt_cut, flt_cut_a, flt_cut_ap, foo, G1_RW, G2_RW, RW_Model::m, manual_pitch, manual_roll, manual_yaw, pitch_mult, roll_mult, 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 yaw_mult.