diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 3f6bd9de64..0e6e09ee8e 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1372,7 +1372,6 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("iterm_relax_type", "%d", currentPidProfile->iterm_relax_type); BLACKBOX_PRINT_HEADER_LINE("iterm_relax_cutoff", "%d", currentPidProfile->iterm_relax_cutoff); #endif - BLACKBOX_PRINT_HEADER_LINE("vbat_pid_gain", "%d", currentPidProfile->vbatPidCompensation); BLACKBOX_PRINT_HEADER_LINE("pidAtMinThrottle", "%d", currentPidProfile->pidAtMinThrottle); // Betaflight PID controller parameters diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 87f8d98efc..ed29b72b97 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1035,7 +1035,6 @@ const clivalue_t valueTable[] = { { "dterm_lowpass2_hz", VAR_INT16 | PROFILE_VALUE, .config.minmax = { 0, FILTER_FREQUENCY_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lowpass2_hz) }, { "dterm_notch_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, FILTER_FREQUENCY_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_notch_hz) }, { "dterm_notch_cutoff", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, FILTER_FREQUENCY_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_notch_cutoff) }, - { "vbat_pid_gain", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, vbatPidCompensation) }, #if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION) { "vbat_sag_compensation", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 150 }, PG_PID_PROFILE, offsetof(pidProfile_t, vbat_sag_compensation) }, #endif diff --git a/src/main/fc/core.c b/src/main/fc/core.c index ac6ffb3d2e..b0032dadf8 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -1188,7 +1188,7 @@ static FAST_CODE void subTaskMotorUpdate(timeUs_t currentTimeUs) startTime = micros(); } - mixTable(currentTimeUs, currentPidProfile->vbatPidCompensation); + mixTable(currentTimeUs); #ifdef USE_SERVOS // motor outputs are used as sources for servo mixing, so motors must be calculated using mixTable() before servos. diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 722c56b6bf..ab993e561d 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -825,7 +825,7 @@ static void updateDynLpfCutoffs(timeUs_t currentTimeUs, float throttle) } #endif -FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation) +FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs) { // Find min and max throttle based on conditions. Throttle has to be known before mixing calculateThrottleAndCurrentMotorEndpoints(currentTimeUs); @@ -867,9 +867,6 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa scaledAxisPidYaw = -scaledAxisPidYaw; } - // Calculate voltage compensation - const float vbatCompensationFactor = vbatPidCompensation ? calculateVbatPidCompensation() : 1.0f; - // Apply the throttle_limit_percent to scale or limit the throttle based on throttle_limit_type if (currentControlRateProfile->throttle_limit_type != THROTTLE_LIMIT_TYPE_OFF) { throttle = applyThrottleLimit(throttle); @@ -903,8 +900,6 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa scaledAxisPidPitch * activeMixer[i].pitch + scaledAxisPidYaw * activeMixer[i].yaw; - mix *= vbatCompensationFactor; // Add voltage compensation - if (mix > motorMixMax) { motorMixMax = mix; } else if (mix < motorMixMin) { diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 1afab8ce08..e5cf091e2d 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -106,7 +106,7 @@ void mixerInitProfile(void); void mixerConfigureOutput(void); void mixerResetDisarmedMotors(void); -void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation); +void mixTable(timeUs_t currentTimeUs); void stopMotors(void); void writeMotors(void); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index fdee75247e..8fbffdd53e 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -139,7 +139,6 @@ void resetPidProfile(pidProfile_t *pidProfile) .dterm_notch_hz = 0, .dterm_notch_cutoff = 0, .itermWindupPointPercent = 100, - .vbatPidCompensation = 0, .pidAtMinThrottle = PID_STABILISATION_ON, .levelAngleLimit = 55, .feedForwardTransition = 0, diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index b9878cf77a..a13dd6e1f3 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -149,7 +149,6 @@ typedef struct pidProfile_s { uint16_t crash_delay; // ms uint8_t crash_recovery_angle; // degrees uint8_t crash_recovery_rate; // degree/second - uint8_t vbatPidCompensation; // Scale PIDsum to battery voltage uint8_t feedForwardTransition; // Feed forward weight transition uint16_t crash_limit_yaw; // limits yaw errorRate, so crashes don't cause huge throttle increase uint16_t itermLimit; diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 6428e1b6b4..9d1acdc7bd 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1746,7 +1746,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst) sbufWriteU16(dst, 0); sbufWriteU16(dst, 0); // was pidProfile.yaw_p_limit sbufWriteU8(dst, 0); // reserved - sbufWriteU8(dst, currentPidProfile->vbatPidCompensation); + sbufWriteU8(dst, 0); // was vbatPidCompensation sbufWriteU8(dst, currentPidProfile->feedForwardTransition); sbufWriteU8(dst, 0); // was low byte of currentPidProfile->dtermSetpointWeight sbufWriteU8(dst, 0); // reserved @@ -2595,7 +2595,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbufReadU16(src); sbufReadU16(src); // was pidProfile.yaw_p_limit sbufReadU8(src); // reserved - currentPidProfile->vbatPidCompensation = sbufReadU8(src); + sbufReadU8(src); // was vbatPidCompensation currentPidProfile->feedForwardTransition = sbufReadU8(src); sbufReadU8(src); // was low byte of currentPidProfile->dtermSetpointWeight sbufReadU8(src); // reserved diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 0aafeaed71..f4648bf08b 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -460,15 +460,6 @@ void batteryUpdateCurrentMeter(timeUs_t currentTimeUs) } } -float calculateVbatPidCompensation(void) { - float batteryScaler = 1.0f; - if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && batteryCellCount > 0) { - // Up to 33% PID gain. Should be fine for 4,2to 3,3 difference - batteryScaler = constrainf((( (float)batteryConfig()->vbatmaxcellvoltage * batteryCellCount ) / (float) voltageMeter.displayFiltered), 1.0f, 1.33f); - } - return batteryScaler; -} - uint8_t calculateBatteryPercentageRemaining(void) { uint8_t batteryPercentage = 0; diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 943a85621c..f04925a0d1 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -101,7 +101,6 @@ void batteryUpdateAlarms(void); struct rxConfig_s; -float calculateVbatPidCompensation(void); uint8_t calculateBatteryPercentageRemaining(void); bool isBatteryVoltageConfigured(void); uint16_t getBatteryVoltage(void); diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index 0a301eeb8f..cdd574b707 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/src/test/unit/arming_prevention_unittest.cc @@ -1047,7 +1047,7 @@ extern "C" { bool isFirstArmingGyroCalibrationRunning(void) { return false; } void pidController(const pidProfile_t *, timeUs_t) {} void pidStabilisationState(pidStabilisationState_e) {} - void mixTable(timeUs_t , uint8_t) {}; + void mixTable(timeUs_t) {}; void writeMotors(void) {}; void writeServos(void) {}; bool calculateRxChannelsAndUpdateFailsafe(timeUs_t) { return true; } diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc index d6085de65f..0de9872d3c 100644 --- a/src/test/unit/pid_unittest.cc +++ b/src/test/unit/pid_unittest.cc @@ -114,7 +114,6 @@ void setDefaultTestSettings(void) { pidProfile->dterm_notch_cutoff = 160; pidProfile->dterm_filter_type = FILTER_BIQUAD; pidProfile->itermWindupPointPercent = 50; - pidProfile->vbatPidCompensation = 0; pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; pidProfile->levelAngleLimit = 55; pidProfile->feedForwardTransition = 100; diff --git a/src/test/unit/vtx_unittest.cc b/src/test/unit/vtx_unittest.cc index 5cdf19e2aa..8475341848 100644 --- a/src/test/unit/vtx_unittest.cc +++ b/src/test/unit/vtx_unittest.cc @@ -139,7 +139,7 @@ extern "C" { bool isFirstArmingGyroCalibrationRunning(void) { return false; } void pidController(const pidProfile_t *, timeUs_t) {} void pidStabilisationState(pidStabilisationState_e) {} - void mixTable(timeUs_t , uint8_t) {}; + void mixTable(timeUs_t) {}; void writeMotors(void) {}; void writeServos(void) {}; bool calculateRxChannelsAndUpdateFailsafe(timeUs_t) { return true; }