28 #include "generated/airframe.h"
32 #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
33 #error "You need to use WLS control allocation for this module"
36 #ifndef INDI_FUNCTIONS_RC_CHANNEL
37 #error "You need to define an RC channel to switch between simple and advanced scheduling"
41 static float g1g2_forward[INDI_OUTPUTS][INDI_NUM_ACT] = FWD_G1;
51 #ifdef STABILIZATION_INDI_G1
52 static float g1g2_hover[INDI_OUTPUTS][INDI_NUM_ACT] = STABILIZATION_INDI_G1;
55 STABILIZATION_INDI_G1_ROLL,
56 STABILIZATION_INDI_G1_PITCH,
57 STABILIZATION_INDI_G1_YAW,
58 STABILIZATION_INDI_G1_THRUST
62 static float g2_both[INDI_NUM_ACT] = STABILIZATION_INDI_G2;
69 for (i = 0; i < INDI_OUTPUTS; i++) {
70 for (j = 0; j < INDI_NUM_ACT; j++) {
89 for (i = 0; i < INDI_OUTPUTS; i++) {
90 for (j = 0; j < INDI_NUM_ACT; j++) {
103 Bound(pitch_interp, -60.0, -30.0);
104 float ratio = (pitch_interp + 30.0)/(-30.);
107 g1g2[1][0] =
g1g2_hover[1][0]*(1-ratio) + -PITCH_EFF_AT_60/1000*ratio;
108 g1g2[1][1] =
g1g2_hover[1][1]*(1-ratio) + PITCH_EFF_AT_60/1000*ratio;
110 g1g2[2][0] =
g1g2_hover[2][0]*(1-ratio) + -YAW_EFF_AT_60/1000*ratio;
111 g1g2[2][1] =
g1g2_hover[2][1]*(1-ratio) + -YAW_EFF_AT_60/1000*ratio;
114 Bound(airspeed, 0.0, 30.0);
115 float airspeed2 = airspeed*airspeed;
117 float pitch_eff = CE_PITCH_A + CE_PITCH_B*airspeed2;
118 g1g2[1][0] = -pitch_eff/1000;
119 g1g2[1][1] = pitch_eff/1000;
121 float yaw_eff = CE_YAW_A + CE_YAW_B*airspeed2;
122 g1g2[2][0] = -yaw_eff/1000;
123 g1g2[2][1] = -yaw_eff/1000;
128 Bound(
g1g2[0][2], -30.0/1000, -2.0/1000);
129 Bound(
g1g2[0][3], 2.0/1000, 30.0/1000);
132 if(airspeed > 12.0) {
148 #ifdef INDI_THRUST_ON_PITCH_EFF
static void eff_scheduling_periodic_a(void)
static float g1g2_hover[INDI_OUTPUTS][INDI_NUM_ACT]
static float g2_both[INDI_NUM_ACT]
void eff_scheduling_cyfoam_periodic(void)
Periodic function that interpolates between gain sets depending on the scheduling variable.
void eff_scheduling_cyfoam_init(void)
Initialises periodic loop;.
static float g1g2_forward[INDI_OUTPUTS][INDI_NUM_ACT]
static void eff_scheduling_periodic_b(void)
void float_eulers_of_quat_zxy(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch
#define INT32_PERCENTAGE_FRAC
#define FLOAT_OF_BFP(_vbfp, _frac)
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
static float stateGetAirspeed_f(void)
Get airspeed (float).
struct FloatEulers eulers_zxy
state eulers in zxy order
struct RadioControl radio_control
Generic interface for radio control modules.
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
int32_t transition_percentage
Horizontal guidance for rotorcrafts.
float * Bwls[INDI_OUTPUTS]
struct Indi_gains indi_gains
float g1g2[INDI_OUTPUTS][INDI_NUM_ACT]
float actuator_state_filt_vect[INDI_NUM_ACT]
API to get/set the generic vehicle states.
signed char int8_t
Typedef defining 8 bit char type.