From b6689edc26100e220d8a810d1a1737ace4ead1b7 Mon Sep 17 00:00:00 2001 From: mikeller Date: Tue, 24 Mar 2020 02:54:49 +1300 Subject: [PATCH] Removed VBat PID compensation. --- src/main/blackbox/blackbox.c | 1 - src/main/cli/settings.c | 1 - src/main/fc/core.c | 2 +- src/main/flight/mixer.c | 7 +------ src/main/flight/mixer.h | 2 +- src/main/flight/pid.c | 1 - src/main/flight/pid.h | 1 - src/main/msp/msp.c | 4 ++-- src/main/sensors/battery.c | 9 --------- src/main/sensors/battery.h | 1 - src/test/unit/arming_prevention_unittest.cc | 2 +- src/test/unit/pid_unittest.cc | 1 - src/test/unit/vtx_unittest.cc | 2 +- 13 files changed, 7 insertions(+), 27 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 3fd7cbf63..ef3949695 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1330,7 +1330,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 89d859c57..7fb669747 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1008,7 +1008,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 cb8e2e581..7203c8e25 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -1189,7 +1189,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 77d467afd..adca3c529 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -827,7 +827,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); @@ -869,9 +869,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); @@ -905,8 +902,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 1afab8ce0..e5cf091e2 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 03034767f..42165187b 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 95e65ccb0..676b397fe 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 0017c2da9..0b7dc6b0b 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1745,7 +1745,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 @@ -2594,7 +2594,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 0aafeaed7..f4648bf08 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 943a85621..f04925a0d 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 0a301eeb8..cdd574b70 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 d6085de65..0de9872d3 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 5cdf19e2a..847534184 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; }