diff --git a/flight/modules/Receiver/receiver.c b/flight/modules/Receiver/receiver.c index feccf1b8c..fe3739538 100644 --- a/flight/modules/Receiver/receiver.c +++ b/flight/modules/Receiver/receiver.c @@ -817,7 +817,7 @@ static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel float floatDeadband = ((float)deadband) * 0.01f; // avoid a long tail of non-zero data. if we have deadband, once the filtered result reduces to 1/10th // of deadband revert to 0. We downstream rely on this to know when sticks are centered. - if (floatDeadband > 0.0f && fabsf(inputFiltered[channel]) < floatDeadband / 10.0f) { + if (floatDeadband > 0.0f && fabsf(inputFiltered[channel]) < floatDeadband * 0.1f) { inputFiltered[channel] = 0.0f; } *value = inputFiltered[channel]; diff --git a/flight/modules/Stabilization/inc/stabilization.h b/flight/modules/Stabilization/inc/stabilization.h index 6ba4ef8fc..b1ff2fc13 100644 --- a/flight/modules/Stabilization/inc/stabilization.h +++ b/flight/modules/Stabilization/inc/stabilization.h @@ -45,10 +45,8 @@ typedef struct { StabilizationSettingsData settings; StabilizationBankData stabBank; float gyro_alpha; - float floatThrustPIDScaleCurve[5]; - float floatAcroInsanityFactorRoll; - float floatAcroInsanityFactorPitch; - float floatAcroInsanityFactorYaw; + float floatThrustPIDScaleCurve[STABILIZATIONBANK_THRUSTPIDSCALECURVE_NUMELEM]; + float acroInsanityFactors[STABILIZATIONBANK_ACROINSANITYFACTOR_NUMELEM]; struct { float min_thrust; float max_thrust; diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index 8d307b068..3786b62f4 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -302,15 +302,11 @@ static void stabilizationInnerloopTask() -StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t], StabilizationBankMaximumRateToArray(stabSettings.stabBank.MaximumRate)[t] ); - const float acroFactors[] = { - stabSettings.floatAcroInsanityFactorRoll, - stabSettings.floatAcroInsanityFactorPitch, - stabSettings.floatAcroInsanityFactorYaw - }; + pid_scaler ascaler = create_pid_scaler(t); ascaler.i *= boundf(1.0f - (1.5f * fabsf(stickinput[t])), 0.0f, 1.0f); // this prevents Integral from getting too high while controlled manually float arate = pid_apply_setpoint(&stabSettings.innerPids[t], &ascaler, rate[t], gyro_filtered[t], dT); - float factor = fabsf(stickinput[t]) * acroFactors[t]; + float factor = fabsf(stickinput[t]) * stabSettings.acroInsanityFactors[t]; actuatorDesiredAxis[t] = factor * stickinput[t] + (1.0f - factor) * arate; } break; diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 15baa44a9..ba841b196 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -382,12 +382,12 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) stabSettings.cruiseControl.half_power_delay = stabSettings.settings.CruiseControlPowerDelayComp / 2.0f; stabSettings.cruiseControl.max_power_factor_angle = RAD2DEG(acosf(1.0f / stabSettings.settings.CruiseControlMaxPowerFactor)); - for (int i = 0; i < 5; i++) { + for (int i = 0; i < STABILIZATIONSETTINGSBANK1_THRUSTPIDSCALECURVE_NUMELEM; i++) { stabSettings.floatThrustPIDScaleCurve[i] = (float)(stabSettings.stabBank.ThrustPIDScaleCurve[i]) * 0.01f; } - stabSettings.floatAcroInsanityFactorRoll = (float)(stabSettings.stabBank.AcroInsanityFactor.Roll) * 0.01f; - stabSettings.floatAcroInsanityFactorPitch = (float)(stabSettings.stabBank.AcroInsanityFactor.Pitch) * 0.01f; - stabSettings.floatAcroInsanityFactorYaw = (float)(stabSettings.stabBank.AcroInsanityFactor.Yaw) * 0.01f; + stabSettings.acroInsanityFactors[0] = (float)(stabSettings.stabBank.AcroInsanityFactor.Roll) * 0.01f; + stabSettings.acroInsanityFactors[1] = (float)(stabSettings.stabBank.AcroInsanityFactor.Pitch) * 0.01f; + stabSettings.acroInsanityFactors[2] = (float)(stabSettings.stabBank.AcroInsanityFactor.Yaw) * 0.01f; } /**