37#define INDI_SCHEDULING_LOWER_BOUND_G1 0.0001
40#ifndef INDI_SCHEDULING_LOW_AIRSPEED
41#define INDI_SCHEDULING_LOW_AIRSPEED 12.0
45#error "This module works with a Nederdrone with 8 (grouped) actuators"
48#ifndef INDI_SCHEDULING_TRIM_ELEVATOR
49#define INDI_SCHEDULING_TRIM_ELEVATOR 0.0
52#ifndef INDI_SCHEDULING_TRIM_FLAPS
53#define INDI_SCHEDULING_TRIM_FLAPS 0.0
73static float g_forward[4][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL_FWD, STABILIZATION_INDI_G1_PITCH_FWD, STABILIZATION_INDI_G1_YAW_FWD, STABILIZATION_INDI_G1_THRUST_FWD};
75#ifdef STABILIZATION_INDI_G1
76static float g_hover[4][INDI_NUM_ACT] = STABILIZATION_INDI_G1;
78static float g_hover[4][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL, STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW, STABILIZATION_INDI_G1_THRUST};
95 for (
uint8_t i = 0; i < INDI_NUM_ACT; i++) {
144 Bound(ratio,0.0,1.0);
146 float stall_speed = 14.0;
147 float pitch_ratio = 0.0;
150 if ( (
eulers_zxy.theta > -M_PI_4) || (airspeed < stall_speed) ) {
154 pitch_ratio = fabs(1.0 -
eulers_zxy.theta/(-M_PI_4));
163 Bound(pitch_ratio,0.0,1.0);
165 float airspeed_pitch_eff = airspeed;
166 Bound(airspeed_pitch_eff, 8.0, 30.0);
168 for (
uint8_t i = 0; i < INDI_NUM_ACT; i++) {
176 g1g2[1][i] =
g_hover[1][i] * (1.0 - pitch_ratio) +
g_forward[1][i] * pitch_ratio * airspeed_pitch_eff * airspeed_pitch_eff / (16.0*16.0);
179 if( (i ==2) || (i==3)) {
186 float wing_thrust_scaling = 1.0;
188#error "ctfl_eff_scheduling_nederdrone is very specific and only works for one Nederdrone configuration!"
193 Bound(wing_thrust,3000.0,9600.0);
194 wing_thrust_scaling = wing_thrust/9600.0/0.8;
197 g1g2[0][i] *= wing_thrust_scaling;
198 g1g2[1][i] *= wing_thrust_scaling;
199 g1g2[2][i] *= wing_thrust_scaling;
203 float ratio_spec_force = 0.0;
204 float airspeed_spec_force = airspeed;
205 Bound(airspeed_spec_force, 8.0, 20.0);
206 ratio_spec_force = (airspeed_spec_force-8.0) / 12.0;
208 for (
uint8_t i = 0; i < INDI_NUM_ACT; i++) {
213 if( (i ==2) || (i==3)) {
221 float pitch_deg =
eulers_zxy.theta / M_PI * 180.f;
232 if(airspeed > 15.0) {
234 for (i = 0; i < 4; i++) {
237 for (i = 4; i < 8; i++) {
242 for (i = 0; i < 8; i++) {
Core autopilot interface common to all firmwares.
#define INDI_SCHEDULING_LOW_AIRSPEED
float sched_tip_prop_upper_pitch_limit_deg
#define INDI_SCHEDULING_TRIM_ELEVATOR
float indi_Wu_original[INDI_NUM_ACT]
float sched_ratio_tip_props
#define INDI_SCHEDULING_TRIM_FLAPS
void ctrl_eff_scheduling_periodic(void)
Periodic function that interpolates between gain sets depending on the scheduling variable.
void schdule_control_effectiveness(void)
Function that calculates control effectiveness values for the Nederdrone inner loop.
bool sched_tip_props_always_on
float backwing_thrust_eff_scaling
static float g_hover[4][INDI_NUM_ACT]
float sched_tip_prop_lower_pitch_limit_deg
float backwing_pitch_eff_scaling
static float g_forward[4][INDI_NUM_ACT]
void ctrl_eff_scheduling_init(void)
Initialises periodic loop;.
static struct FloatEulers * stateGetNedToBodyEulersZxy_f(void)
Get vehicle body attitude ZXY euler angles (float).
static float stateGetAirspeed_f(void)
Get airspeed (float).
struct FloatEulers eulers_zxy
state eulers in zxy order
Hardware independent API for actuators (servos, motor controllers).
struct RadioControl radio_control
Generic interface for radio control modules.
#define STABILIZATION_INDI_WLS_WU
float act_pref[INDI_NUM_ACT]
float g1g2[INDI_OUTPUTS][INDI_NUM_ACT]
int16_t actuators_pprz[INDI_NUM_ACT+1]
PPRZ command to each actuator Can be used to directly control actuators from the control algorithm if...
API to get/set the generic vehicle states.
int int32_t
Typedef defining 32 bit int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.