Expand Up
@@ -71,14 +71,14 @@
#define STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE FALSE
#endif
#ifndef INDI_FILTER_RATES_SECOND_ORDER
#define INDI_FILTER_RATES_SECOND_ORDER FALSE
#ifndef STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
#define STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER FALSE
#endif
// Airspeed [m/s] at which the forward flight throttle limit is used instead of
// the hover throttle limit.
#ifndef INDI_HROTTLE_LIMIT_AIRSPEED_FWD
#define INDI_HROTTLE_LIMIT_AIRSPEED_FWD 8.0
#ifndef STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD
#define STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD 8.0
#endif
#if INDI_OUTPUTS > 4
Expand All
@@ -88,16 +88,15 @@
#endif
#ifdef SetCommandsFromRC
#warning SetAutoCommandsFromRC not used: STAB_INDI overwrites actuators
#warning SetCommandsFromRC not used: STAB_INDI overwrites actuators
#endif
#if !STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
#if INDI_NUM_ACT > WLS_N_U
#error Matrix-WLS_N_U too small: increase WLS_N_U in airframe file
#error Matrix-WLS_N_U too small or not defined: define WLS_N_U >= INDI_NUM_ACT in airframe file
#endif
#if INDI_OUTPUTS > WLS_N_V
#error Matrix-WLS_N_V too small: increase WLS_N_V in airframe file
#error Matrix-WLS_N_V too small or not defined: define WLS_N_U >= INDI_OUTPUTS in airframe file
#endif
#endif
Expand Down
Expand Up
@@ -265,7 +264,7 @@ Butterworth2LowPass estimation_input_lowpass_filters[INDI_NUM_ACT];
Butterworth2LowPass measurement_lowpass_filters [3 ];
Butterworth2LowPass estimation_output_lowpass_filters [3 ];
Butterworth2LowPass acceleration_lowpass_filter ;
#if INDI_FILTER_RATES_SECOND_ORDER
#if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
Butterworth2LowPass rates_filt_so [3 ];
#else
static struct FirstOrderLowPass rates_filt_fo [3 ];
Expand Down
Expand Up
@@ -431,7 +430,7 @@ void init_filters(void)
// Filtering of the accel body z
init_butterworth_2_low_pass (& acceleration_lowpass_filter , tau_est , sample_time , 0.0 );
#if INDI_FILTER_RATES_SECOND_ORDER
#if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_P );
init_butterworth_2_low_pass (& rates_filt_so [0 ], tau , sample_time , 0.0 );
tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_Q );
Expand Down
Expand Up
@@ -554,7 +553,7 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
//Note that due to the delay, the PD controller may need relaxed gains.
struct FloatRates rates_filt ;
#if STABILIZATION_INDI_FILTER_ROLL_RATE
#if INDI_FILTER_RATES_SECOND_ORDER
#if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
rates_filt .p = update_butterworth_2_low_pass (& rates_filt_so [0 ], body_rates -> p );
#else
rates_filt .p = update_first_order_low_pass (& rates_filt_fo [0 ], body_rates -> p );
Expand All
@@ -563,7 +562,7 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
rates_filt .p = body_rates -> p ;
#endif
#if STABILIZATION_INDI_FILTER_PITCH_RATE
#if INDI_FILTER_RATES_SECOND_ORDER
#if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
rates_filt .q = update_butterworth_2_low_pass (& rates_filt_so [1 ], body_rates -> q );
#else
rates_filt .q = update_first_order_low_pass (& rates_filt_fo [1 ], body_rates -> q );
Expand All
@@ -572,7 +571,7 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
rates_filt .q = body_rates -> q ;
#endif
#if STABILIZATION_INDI_FILTER_YAW_RATE
#if INDI_FILTER_RATES_SECOND_ORDER
#if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
rates_filt .r = update_butterworth_2_low_pass (& rates_filt_so [2 ], body_rates -> r );
#else
rates_filt .r = update_first_order_low_pass (& rates_filt_fo [2 ], body_rates -> r );
Expand Down
Expand Up
@@ -729,7 +728,7 @@ void WEAK stabilization_indi_set_wls_settings(float use_increment)
//limit minimum thrust ap can give
if (!act_is_servo [i ]) {
if ((guidance_h .mode == GUIDANCE_H_MODE_HOVER ) || (guidance_h .mode == GUIDANCE_H_MODE_NAV )) {
if (airspeed < INDI_HROTTLE_LIMIT_AIRSPEED_FWD ) {
if (airspeed < STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD ) {
du_min_stab_indi [i ] = GUIDANCE_INDI_MIN_THROTTLE - use_increment * actuator_state_filt_vect [i ];
} else {
du_min_stab_indi [i ] = GUIDANCE_INDI_MIN_THROTTLE_FWD - use_increment * actuator_state_filt_vect [i ];
Expand Down