eff_scheduling_rotwing.c File Reference

#include "modules/ctrl/eff_scheduling_rotwing.h"
#include "generated/airframe.h"
#include "state.h"
#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
#include "firmwares/rotorcraft/guidance/guidance_indi_hybrid.h"
#include "modules/rotwing_drone/rotwing_state.h"
#include "autopilot.h"
#include "modules/actuators/actuators.h"
#include "modules/core/abi.h"
#include "modules/radio_control/radio_control.h"

Include dependency graph for eff_scheduling_rotwing.c:

Go to the source code of this file.

Macros
#define	WING_ROTATION_CAN_ROTWING_ID   ABI_BROADCAST
	ABI binding wing position data. More...
#define	AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT   16

Functions
float	bound_or_zero (float value, float low_lim, float up_lim)
void	eff_scheduling_rotwing_update_wing_angle (void)
void	eff_scheduling_rotwing_update_MMOI (void)
void	eff_scheduling_rotwing_update_cmd (void)
void	eff_scheduling_rotwing_update_airspeed (void)
void	eff_scheduling_rotwing_update_hover_motor_effectiveness (void)
void	eff_scheduling_rotwing_update_elevator_effectiveness (void)
void	eff_scheduling_rotwing_update_rudder_effectiveness (void)
void	eff_scheduling_rotwing_update_aileron_effectiveness (void)
void	eff_scheduling_rotwing_update_flaperon_effectiveness (void)
void	eff_scheduling_rotwing_update_pusher_effectiveness (void)
void	eff_scheduling_rotwing_schedule_liftd (void)
float	guidance_indi_get_liftd (float pitch UNUSED, float theta UNUSED)
void	stabilization_indi_set_wls_settings (void)
	Function that sets the u_min, u_max and u_pref if function not elsewhere defined. More...
void	guidance_indi_hybrid_set_wls_settings (float body_v[3], float roll_angle, float pitch_angle)
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
struct rotwing_eff_sched_param_t	eff_sched_p
int32_t	rw_flap_offset = 0
float	eff_sched_pusher_time = 0.002
float	roll_eff_slider = 12.f
static const float	Wu_gih_original [GUIDANCE_INDI_HYBRID_U] = GUIDANCE_INDI_WLS_WU
struct rotwing_eff_sched_var_t	eff_sched_var
static abi_event	wing_position_ev
float	eff_scheduling_rotwing_lift_d = 0.0f

Detailed Description

Author

Dennis van Wijngaarden D.C.v.nosp@m.anWi.nosp@m.jngaa.nosp@m.rden.nosp@m.@tude.nosp@m.lft..nosp@m.nl The control effectiveness scheduler for the rotating wing drone type

Definition in file eff_scheduling_rotwing.c.

Macro Definition Documentation

AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT

#define AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT   16

WING_ROTATION_CAN_ROTWING_ID

#define WING_ROTATION_CAN_ROTWING_ID   ABI_BROADCAST

ABI binding wing position data.

Definition at line 200 of file eff_scheduling_rotwing.c.

Function Documentation

bound_or_zero()

float bound_or_zero ( float  value, float  low_lim, float  up_lim  )

inline

Definition at line 155 of file eff_scheduling_rotwing.c.

Referenced by eff_scheduling_rotwing_update_aileron_effectiveness(), and eff_scheduling_rotwing_update_hover_motor_effectiveness().

Here is the caller graph for this function:

eff_scheduling_rotwing_init()

void eff_scheduling_rotwing_init

(

void

)

Definition at line 219 of file eff_scheduling_rotwing.c.

References rotwing_eff_sched_var_t::airspeed, rotwing_eff_sched_var_t::airspeed2, rotwing_eff_sched_var_t::cmd_elevator, rotwing_eff_sched_var_t::cmd_pusher, rotwing_eff_sched_var_t::cmd_pusher_scaled, rotwing_eff_sched_var_t::cmd_T_mean_scaled, rotwing_eff_sched_var_t::cosr, rotwing_eff_sched_var_t::cosr2, rotwing_eff_sched_param_t::DMdpprz_hover_pitch, rotwing_eff_sched_param_t::DMdpprz_hover_roll, eff_sched_p, eff_sched_var, rotwing_eff_sched_var_t::Ixx, rotwing_eff_sched_param_t::Ixx_body, rotwing_eff_sched_param_t::Ixx_wing, rotwing_eff_sched_var_t::Iyy, rotwing_eff_sched_param_t::Iyy_body, rotwing_eff_sched_param_t::Iyy_wing, rotwing_eff_sched_var_t::pitch_motor_dMdpprz, rotwing_eff_sched_var_t::roll_motor_dMdpprz, rotwing_eff_sched_var_t::sinr, rotwing_eff_sched_var_t::sinr2, rotwing_eff_sched_var_t::sinr3, wing_position_cb(), wing_position_ev, WING_ROTATION_CAN_ROTWING_ID, rotwing_eff_sched_var_t::wing_rotation_deg, and rotwing_eff_sched_var_t::wing_rotation_rad.

Here is the call graph for this function:

eff_scheduling_rotwing_periodic()

void eff_scheduling_rotwing_periodic

(

void

)

Definition at line 249 of file eff_scheduling_rotwing.c.

References eff_scheduling_rotwing_schedule_liftd(), eff_scheduling_rotwing_update_aileron_effectiveness(), eff_scheduling_rotwing_update_airspeed(), eff_scheduling_rotwing_update_cmd(), eff_scheduling_rotwing_update_elevator_effectiveness(), eff_scheduling_rotwing_update_flaperon_effectiveness(), eff_scheduling_rotwing_update_hover_motor_effectiveness(), eff_scheduling_rotwing_update_MMOI(), eff_scheduling_rotwing_update_pusher_effectiveness(), eff_scheduling_rotwing_update_rudder_effectiveness(), and eff_scheduling_rotwing_update_wing_angle().

Here is the call graph for this function:

eff_scheduling_rotwing_schedule_liftd()

void eff_scheduling_rotwing_schedule_liftd ( void  )

inline

Definition at line 453 of file eff_scheduling_rotwing.c.

References rotwing_eff_sched_var_t::airspeed2, eff_sched_p, eff_sched_var, eff_scheduling_rotwing_lift_d, if(), rotwing_eff_sched_param_t::k_lift_fuselage, rotwing_eff_sched_param_t::k_lift_tail, rotwing_eff_sched_param_t::k_lift_wing, rotwing_eff_sched_param_t::m, rotwing_eff_sched_var_t::sinr2, and rotwing_eff_sched_var_t::wing_rotation_deg.

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_aileron_effectiveness()

void eff_scheduling_rotwing_update_aileron_effectiveness ( void  )

inline

Definition at line 417 of file eff_scheduling_rotwing.c.

References rotwing_eff_sched_var_t::airspeed2, bound_or_zero(), rotwing_eff_sched_var_t::cosr, eff_sched_p, eff_sched_var, g1g2, rotwing_eff_sched_var_t::Ixx, rotwing_eff_sched_var_t::Iyy, rotwing_eff_sched_param_t::k_aileron, rotwing_eff_sched_var_t::sinr2, and rotwing_eff_sched_var_t::sinr3.

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_airspeed()

void eff_scheduling_rotwing_update_airspeed ( void  )

inline

Definition at line 302 of file eff_scheduling_rotwing.c.

References rotwing_eff_sched_var_t::airspeed, rotwing_eff_sched_var_t::airspeed2, eff_sched_var, and stateGetAirspeed_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_cmd()

void eff_scheduling_rotwing_update_cmd ( void  )

inline

Definition at line 293 of file eff_scheduling_rotwing.c.

References actuator_state_filt_vect, rotwing_eff_sched_var_t::cmd_elevator, rotwing_eff_sched_var_t::cmd_pusher, rotwing_eff_sched_var_t::cmd_pusher_scaled, rotwing_eff_sched_var_t::cmd_T_mean_scaled, and eff_sched_var.

Referenced by eff_scheduling_rotwing_periodic().

Here is the caller graph for this function:

eff_scheduling_rotwing_update_elevator_effectiveness()

void eff_scheduling_rotwing_update_elevator_effectiveness ( void  )

inline

Definition at line 377 of file eff_scheduling_rotwing.c.

References rotwing_eff_sched_var_t::airspeed, rotwing_eff_sched_var_t::airspeed2, rotwing_eff_sched_var_t::cmd_elevator, rotwing_eff_sched_var_t::cmd_pusher_scaled, eff_sched_p, eff_sched_var, g1g2, rotwing_eff_sched_var_t::Iyy, rotwing_eff_sched_param_t::k_elevator, and rotwing_eff_sched_param_t::k_elevator_deflection.

Referenced by eff_scheduling_rotwing_periodic().

Here is the caller graph for this function:

eff_scheduling_rotwing_update_flaperon_effectiveness()

void eff_scheduling_rotwing_update_flaperon_effectiveness ( void  )

inline

Definition at line 430 of file eff_scheduling_rotwing.c.

References rotwing_eff_sched_var_t::airspeed2, eff_sched_p, eff_sched_var, g1g2, rotwing_eff_sched_var_t::Ixx, rotwing_eff_sched_param_t::k_flaperon, and rotwing_eff_sched_var_t::sinr3.

Referenced by eff_scheduling_rotwing_periodic().

Here is the caller graph for this function:

eff_scheduling_rotwing_update_hover_motor_effectiveness()

void eff_scheduling_rotwing_update_hover_motor_effectiveness ( void  )

inline

Definition at line 310 of file eff_scheduling_rotwing.c.

References actuator_state_filt_vect, rotwing_eff_sched_var_t::airspeed, autopilot, bound_or_zero(), rotwing_eff_sched_var_t::cosr, rotwing_eff_sched_param_t::DMdpprz_hover_pitch, rotwing_eff_sched_param_t::DMdpprz_hover_roll, eff_sched_p, eff_sched_var, g1g2, rotwing_eff_sched_param_t::hover_roll_pitch_coef, rotwing_eff_sched_param_t::hover_roll_roll_coef, pprz_autopilot::in_flight, rotwing_eff_sched_var_t::Ixx, rotwing_eff_sched_var_t::Iyy, MAX_PPRZ, rotwing_eff_sched_var_t::pitch_motor_dMdpprz, radio_control, roll_eff_slider, rotwing_eff_sched_var_t::roll_motor_dMdpprz, rotwing_eff_sched_var_t::sinr, RadioControl::values, and rotwing_eff_sched_var_t::wing_rotation_rad.

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_MMOI()

void eff_scheduling_rotwing_update_MMOI ( void  )

inline

Definition at line 283 of file eff_scheduling_rotwing.c.

References rotwing_eff_sched_var_t::cosr2, eff_sched_p, eff_sched_var, rotwing_eff_sched_var_t::Ixx, rotwing_eff_sched_param_t::Ixx_body, rotwing_eff_sched_param_t::Ixx_wing, rotwing_eff_sched_var_t::Iyy, rotwing_eff_sched_param_t::Iyy_body, rotwing_eff_sched_param_t::Iyy_wing, and rotwing_eff_sched_var_t::sinr2.

Referenced by eff_scheduling_rotwing_periodic().

Here is the caller graph for this function:

eff_scheduling_rotwing_update_pusher_effectiveness()

void eff_scheduling_rotwing_update_pusher_effectiveness ( void  )

inline

Definition at line 438 of file eff_scheduling_rotwing.c.

References rotwing_eff_sched_var_t::airspeed, rotwing_eff_sched_var_t::cmd_pusher, eff_sched_p, eff_sched_var, g1g2, rotwing_eff_sched_param_t::k_pusher, rotwing_eff_sched_param_t::k_rpm_pprz_pusher, and rotwing_eff_sched_param_t::m.

Referenced by eff_scheduling_rotwing_periodic().

Here is the caller graph for this function:

eff_scheduling_rotwing_update_rudder_effectiveness()

void eff_scheduling_rotwing_update_rudder_effectiveness ( void  )

inline

Definition at line 399 of file eff_scheduling_rotwing.c.

References rotwing_eff_sched_var_t::airspeed2, rotwing_eff_sched_var_t::cmd_pusher_scaled, rotwing_eff_sched_var_t::cmd_T_mean_scaled, rotwing_eff_sched_var_t::cosr, rotwing_eff_sched_param_t::d_rudder_d_pprz, eff_sched_p, eff_sched_var, g1g2, rotwing_eff_sched_param_t::Izz, and rotwing_eff_sched_param_t::k_rudder.

Referenced by eff_scheduling_rotwing_periodic().

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 268 of file eff_scheduling_rotwing.c.

References rotwing_eff_sched_var_t::cosr, rotwing_eff_sched_var_t::cosr2, eff_sched_var, rotwing_eff_sched_var_t::sinr, rotwing_eff_sched_var_t::sinr2, rotwing_eff_sched_var_t::sinr3, rotwing_eff_sched_var_t::wing_rotation_deg, and rotwing_eff_sched_var_t::wing_rotation_rad.

Referenced by eff_scheduling_rotwing_periodic().

Here is the caller graph for this function:

guidance_indi_get_liftd()

float guidance_indi_get_liftd ( float pitch  UNUSED, float theta  UNUSED  )

inline

Definition at line 468 of file eff_scheduling_rotwing.c.

References eff_scheduling_rotwing_lift_d.

guidance_indi_hybrid_set_wls_settings()

void guidance_indi_hybrid_set_wls_settings ( float  body_v[3], float  roll_angle, float  pitch_angle  )

inline

Definition at line 503 of file eff_scheduling_rotwing.c.

References actuator_state_filt_vect, AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT, eulers_zxy, float_eulers_of_quat_zxy(), rotwing_state_t::fw_min_airspeed, g1g2, guidance_indi_max_bank, GUIDANCE_INDI_MAX_PITCH, GUIDANCE_INDI_MIN_PITCH, guidance_indi_pitch_pref_deg, MAX_PPRZ, rotwing_state_t::meas_skew_angle_deg, ROTWING_QUAD_PREF_PITCH, ROTWING_SKEW_ANGLE_STEP, rotwing_state, rotwing_state_hover_motors_idling(), rotwing_state_hover_motors_running(), rotwing_state_pusher_motor_running(), stateGetAirspeed_f(), stateGetNedToBodyQuat_f(), and Wu_gih_original.

Here is the call graph for this function:

stabilization_indi_set_wls_settings()

void stabilization_indi_set_wls_settings

(

void

)

Function that sets the u_min, u_max and u_pref if function not elsewhere defined.

Definition at line 472 of file eff_scheduling_rotwing.c.

References act_is_servo, act_pref, actuator_state_filt_vect, eff_sched_pusher_time, MAX_PPRZ, rw_flap_offset, WLS_t::u_max, WLS_t::u_min, WLS_t::u_pref, and wls_stab_p.

wing_position_cb()

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

static

Definition at line 205 of file eff_scheduling_rotwing.c.

References eff_sched_var, and rotwing_eff_sched_var_t::wing_rotation_rad.

Referenced by eff_scheduling_rotwing_init().

Here is the caller graph for this function:

Variable Documentation

eff_sched_p

struct rotwing_eff_sched_param_t eff_sched_p

Initial value:

= {
  .Ixx_body                 = ROTWING_EFF_SCHED_IXX_BODY,
  .Iyy_body                 = ROTWING_EFF_SCHED_IYY_BODY,
  .Izz                      = ROTWING_EFF_SCHED_IZZ,
  .Ixx_wing                 = ROTWING_EFF_SCHED_IXX_WING,
  .Iyy_wing                 = ROTWING_EFF_SCHED_IYY_WING,
  .m                        = ROTWING_EFF_SCHED_M,
  .DMdpprz_hover_pitch      = ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_PITCH,
  .DMdpprz_hover_roll       = ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_ROLL,
  .hover_roll_pitch_coef    = ROTWING_EFF_SCHED_HOVER_ROLL_PITCH_COEF,
  .hover_roll_roll_coef     = ROTWING_EFF_SCHED_HOVER_ROLL_ROLL_COEF,
  .k_elevator               = ROTWING_EFF_SCHED_K_ELEVATOR,
  .k_rudder                 = ROTWING_EFF_SCHED_K_RUDDER,
  .k_aileron                = ROTWING_EFF_SCHED_K_AILERON,
  .k_flaperon               = ROTWING_EFF_SCHED_K_FLAPERON,
  .k_pusher                 = ROTWING_EFF_SCHED_K_PUSHER,
  .k_elevator_deflection    = ROTWING_EFF_SCHED_K_ELEVATOR_DEFLECTION,
  .d_rudder_d_pprz          = ROTWING_EFF_SCHED_D_RUDDER_D_PPRZ,
  .k_rpm_pprz_pusher        = ROTWING_EFF_SCHED_K_RPM_PPRZ_PUSHER,
  .k_lift_wing              = ROTWING_EFF_SCHED_K_LIFT_WING,
  .k_lift_fuselage          = ROTWING_EFF_SCHED_K_LIFT_FUSELAGE,
  .k_lift_tail              = ROTWING_EFF_SCHED_K_LIFT_TAIL
}

Definition at line 1 of file eff_scheduling_rotwing.c.

Referenced by eff_scheduling_rotwing_init(), eff_scheduling_rotwing_schedule_liftd(), eff_scheduling_rotwing_update_aileron_effectiveness(), eff_scheduling_rotwing_update_elevator_effectiveness(), eff_scheduling_rotwing_update_flaperon_effectiveness(), eff_scheduling_rotwing_update_hover_motor_effectiveness(), eff_scheduling_rotwing_update_MMOI(), eff_scheduling_rotwing_update_pusher_effectiveness(), and eff_scheduling_rotwing_update_rudder_effectiveness().

eff_sched_pusher_time

float eff_sched_pusher_time = 0.002

Definition at line 173 of file eff_scheduling_rotwing.c.

Referenced by stabilization_indi_set_wls_settings().

eff_sched_var

struct rotwing_eff_sched_var_t eff_sched_var

Definition at line 176 of file eff_scheduling_rotwing.c.

Referenced by eff_scheduling_rotwing_init(), eff_scheduling_rotwing_schedule_liftd(), eff_scheduling_rotwing_update_aileron_effectiveness(), eff_scheduling_rotwing_update_airspeed(), eff_scheduling_rotwing_update_cmd(), eff_scheduling_rotwing_update_elevator_effectiveness(), eff_scheduling_rotwing_update_flaperon_effectiveness(), eff_scheduling_rotwing_update_hover_motor_effectiveness(), eff_scheduling_rotwing_update_MMOI(), eff_scheduling_rotwing_update_pusher_effectiveness(), eff_scheduling_rotwing_update_rudder_effectiveness(), eff_scheduling_rotwing_update_wing_angle(), and wing_position_cb().

eff_scheduling_rotwing_lift_d

float eff_scheduling_rotwing_lift_d = 0.0f

Definition at line 451 of file eff_scheduling_rotwing.c.

Referenced by eff_scheduling_rotwing_schedule_liftd(), and guidance_indi_get_liftd().

roll_eff_slider

float roll_eff_slider = 12.f

Definition at line 174 of file eff_scheduling_rotwing.c.

Referenced by eff_scheduling_rotwing_update_hover_motor_effectiveness().

rw_flap_offset

int32_t rw_flap_offset = 0

Definition at line 152 of file eff_scheduling_rotwing.c.

Referenced by stabilization_indi_set_wls_settings().

wing_position_ev

abi_event wing_position_ev

static

Definition at line 203 of file eff_scheduling_rotwing.c.

Referenced by eff_scheduling_rotwing_init().

Wu_gih_original

const float Wu_gih_original[GUIDANCE_INDI_HYBRID_U] = GUIDANCE_INDI_WLS_WU

static

Definition at line 176 of file eff_scheduling_rotwing.c.

Referenced by guidance_indi_hybrid_set_wls_settings().