mirror of https://github.com/FOME-Tech/fome-fw.git
simpler ETB jam detection #489
rather than try to integrate error over time, simply fault if there's been too much error for too long. Up to +-X error is allowed for any period of time, more is allowed for Y seconds.
This commit is contained in:
parent
d23fe55c89
commit
5c3d417441
|
@ -199,9 +199,6 @@ bool EtbController::init(dc_function_e function, DcMotor *motor, pid_s *pidParam
|
||||||
m_pid.initPidClass(pidParameters);
|
m_pid.initPidClass(pidParameters);
|
||||||
m_pedalMap = pedalMap;
|
m_pedalMap = pedalMap;
|
||||||
|
|
||||||
// Ignore 3% position error before complaining
|
|
||||||
m_errorAccumulator.init(3.0f, etbPeriodSeconds);
|
|
||||||
|
|
||||||
reset();
|
reset();
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
|
@ -209,7 +206,6 @@ bool EtbController::init(dc_function_e function, DcMotor *motor, pid_s *pidParam
|
||||||
|
|
||||||
void EtbController::reset() {
|
void EtbController::reset() {
|
||||||
m_shouldResetPid = true;
|
m_shouldResetPid = true;
|
||||||
etbDutyRateOfChange = etbDutyAverage = 0;
|
|
||||||
m_dutyRocAverage.reset();
|
m_dutyRocAverage.reset();
|
||||||
m_dutyAverage.reset();
|
m_dutyAverage.reset();
|
||||||
etbTpsErrorCounter = 0;
|
etbTpsErrorCounter = 0;
|
||||||
|
@ -482,15 +478,7 @@ expected<percent_t> EtbController::getClosedLoop(percent_t target, percent_t obs
|
||||||
if (m_isAutotune) {
|
if (m_isAutotune) {
|
||||||
return getClosedLoopAutotune(target, observation);
|
return getClosedLoopAutotune(target, observation);
|
||||||
} else {
|
} else {
|
||||||
// Check that we're not over the error limit
|
checkJam(target, observation);
|
||||||
etbIntegralError = m_errorAccumulator.accumulate(target - observation);
|
|
||||||
|
|
||||||
// Allow up to 10 percent-seconds of error
|
|
||||||
if (etbIntegralError > 10.0f) {
|
|
||||||
// TODO: figure out how to handle uncalibrated ETB
|
|
||||||
//getLimpManager()->reportEtbProblem();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Normal case - use PID to compute closed loop part
|
// Normal case - use PID to compute closed loop part
|
||||||
return m_pid.getOutput(target, observation, etbPeriodSeconds);
|
return m_pid.getOutput(target, observation, etbPeriodSeconds);
|
||||||
}
|
}
|
||||||
|
@ -615,33 +603,24 @@ void EtbController::update() {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
auto output = ClosedLoopController::update();
|
ClosedLoopController::update();
|
||||||
|
|
||||||
if (!output) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
checkOutput(output.Value);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void EtbController::checkOutput(percent_t output) {
|
void EtbController::checkJam(percent_t setpoint, percent_t observation) {
|
||||||
etbDutyAverage = m_dutyAverage.average(absF(output));
|
float absError = std::abs(setpoint - observation);
|
||||||
|
|
||||||
etbDutyRateOfChange = m_dutyRocAverage.average(absF(output - prevOutput));
|
auto jamDetectThreshold = engineConfiguration->jamDetectThreshold;
|
||||||
prevOutput = output;
|
|
||||||
|
|
||||||
float integrator = absF(m_pid.getIntegration());
|
if (jamDetectThreshold != 0) {
|
||||||
auto integratorLimit = engineConfiguration->etbJamIntegratorLimit;
|
|
||||||
|
|
||||||
if (integratorLimit != 0) {
|
|
||||||
auto nowNt = getTimeNowNt();
|
auto nowNt = getTimeNowNt();
|
||||||
|
|
||||||
if (integrator > integratorLimit) {
|
if (absError > jamDetectThreshold) {
|
||||||
if (m_jamDetectTimer.hasElapsedSec(engineConfiguration->etbJamTimeout)) {
|
if (m_jamDetectTimer.hasElapsedSec(engineConfiguration->etbJamTimeout)) {
|
||||||
// ETB is jammed!
|
// ETB is jammed!
|
||||||
jamDetected = true;
|
jamDetected = true;
|
||||||
|
|
||||||
// TODO: do something about it!
|
// TODO: do something about it!
|
||||||
|
// getLimpManager()->reportEtbProblem();
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
m_jamDetectTimer.reset(getTimeNowNt());
|
m_jamDetectTimer.reset(getTimeNowNt());
|
||||||
|
|
|
@ -18,8 +18,6 @@ struct_no_prefix electronic_throttle_s
|
||||||
bit etbRevLimitActive
|
bit etbRevLimitActive
|
||||||
bit jamDetected
|
bit jamDetected
|
||||||
|
|
||||||
float etbDutyRateOfChange;"ETB duty rate of change";"per", 1, 0, -0,20, 2
|
|
||||||
float etbDutyAverage;"ETB average duty";"per", 1, 0, -20,50, 2
|
|
||||||
uint16_t etbTpsErrorCounter;"ETB TPS error counter";"count", 1, 0, 0,3, 0
|
uint16_t etbTpsErrorCounter;"ETB TPS error counter";"count", 1, 0, 0,3, 0
|
||||||
uint16_t etbPpsErrorCounter;"ETB pedal error counter";"count", 1, 0, 0,3, 0
|
uint16_t etbPpsErrorCounter;"ETB pedal error counter";"count", 1, 0, 0,3, 0
|
||||||
|
|
||||||
|
|
|
@ -12,7 +12,6 @@
|
||||||
|
|
||||||
#include "sensor.h"
|
#include "sensor.h"
|
||||||
#include "efi_pid.h"
|
#include "efi_pid.h"
|
||||||
#include "error_accumulator.h"
|
|
||||||
#include "electronic_throttle_generated.h"
|
#include "electronic_throttle_generated.h"
|
||||||
#include "exp_average.h"
|
#include "exp_average.h"
|
||||||
|
|
||||||
|
@ -53,9 +52,9 @@ public:
|
||||||
expected<percent_t> getClosedLoop(percent_t setpoint, percent_t observation) override;
|
expected<percent_t> getClosedLoop(percent_t setpoint, percent_t observation) override;
|
||||||
expected<percent_t> getClosedLoopAutotune(percent_t setpoint, percent_t actualThrottlePosition);
|
expected<percent_t> getClosedLoopAutotune(percent_t setpoint, percent_t actualThrottlePosition);
|
||||||
|
|
||||||
void setOutput(expected<percent_t> outputValue) override;
|
void checkJam(percent_t setpoint, percent_t observation);
|
||||||
|
|
||||||
void checkOutput(percent_t output);
|
void setOutput(expected<percent_t> outputValue) override;
|
||||||
|
|
||||||
// Used to inspect the internal PID controller's state
|
// Used to inspect the internal PID controller's state
|
||||||
const pid_state_s& getPidState() const override { return m_pid; };
|
const pid_state_s& getPidState() const override { return m_pid; };
|
||||||
|
@ -90,8 +89,6 @@ private:
|
||||||
DcMotor *m_motor = nullptr;
|
DcMotor *m_motor = nullptr;
|
||||||
Pid m_pid;
|
Pid m_pid;
|
||||||
bool m_shouldResetPid = false;
|
bool m_shouldResetPid = false;
|
||||||
// todo: rename to m_targetErrorAccumulator
|
|
||||||
ErrorAccumulator m_errorAccumulator;
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @return true if OK, false if should be disabled
|
* @return true if OK, false if should be disabled
|
||||||
|
|
|
@ -9,11 +9,9 @@
|
||||||
template <typename TInput, typename TOutput>
|
template <typename TInput, typename TOutput>
|
||||||
class ClosedLoopController {
|
class ClosedLoopController {
|
||||||
public:
|
public:
|
||||||
expected<TOutput> update() {
|
void update() {
|
||||||
expected<TOutput> outputValue = getOutput();
|
expected<TOutput> outputValue = getOutput();
|
||||||
setOutput(outputValue);
|
setOutput(outputValue);
|
||||||
|
|
||||||
return outputValue;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
|
@ -1323,7 +1323,7 @@ float tChargeAirDecrLimit;Maximum allowed rate of decrease allowed for the estim
|
||||||
|
|
||||||
uint16_t autoscale fordInjectorSmallPulseBreakPoint;;"mg", 0.001, 0, 0, 65, 3
|
uint16_t autoscale fordInjectorSmallPulseBreakPoint;;"mg", 0.001, 0, 0, 65, 3
|
||||||
|
|
||||||
uint8_t etbJamIntegratorLimit;;"%", 1, 0, 0, 50, 0
|
uint8_t jamDetectThreshold;;"%", 1, 0, 0, 50, 0
|
||||||
|
|
||||||
! Someday there will be a 6th option for BMW S55 that uses a separate shaft just for HPFP
|
! Someday there will be a 6th option for BMW S55 that uses a separate shaft just for HPFP
|
||||||
#define hpfp_cam_e_enum "NONE", "Intake 1", "Exhaust 1", "Intake 2", "Exhaust 2"
|
#define hpfp_cam_e_enum "NONE", "Intake 1", "Exhaust 1", "Intake 2", "Exhaust 2"
|
||||||
|
|
|
@ -4225,7 +4225,7 @@ dialog = tcuControls, "Transmission Settings"
|
||||||
field = "PWM Frequency", etbFreq
|
field = "PWM Frequency", etbFreq
|
||||||
field = "Minimum ETB position", etbMinimumPosition
|
field = "Minimum ETB position", etbMinimumPosition
|
||||||
field = "Maximum ETB position", etbMaximumPosition
|
field = "Maximum ETB position", etbMaximumPosition
|
||||||
field = "Jam detection integrator max", etbJamIntegratorLimit
|
field = "Jam detection error max", jamDetectThreshold
|
||||||
field = "Jam detection timeout period", etbJamTimeout
|
field = "Jam detection timeout period", etbJamTimeout
|
||||||
field = "Duty Averaging Length", etbExpAverageLength
|
field = "Duty Averaging Length", etbExpAverageLength
|
||||||
field = "Rate of change Averaging Length", etbRocExpAverageLength
|
field = "Rate of change Averaging Length", etbRocExpAverageLength
|
||||||
|
|
|
@ -1,31 +0,0 @@
|
||||||
#include "pch.h"
|
|
||||||
|
|
||||||
#include "error_accumulator.h"
|
|
||||||
|
|
||||||
// todo: shall we rename to StatsIntegrator or SomethingIntegrator assuming we see not "Error Accumulator" usages?
|
|
||||||
// todo: ValueIntegrator maybe?
|
|
||||||
float ErrorAccumulator::accumulate(float error) {
|
|
||||||
// We only care about the absolute value of the error
|
|
||||||
error = absF(error);
|
|
||||||
|
|
||||||
// If m_ignoreError is 5, for example:
|
|
||||||
// 0 error -> bleeds down at 5 per second
|
|
||||||
// 5 error -> integral stays where it is
|
|
||||||
// 10 error -> integral grows at 5 per second
|
|
||||||
float accumulationRate = error - m_ignoreError;
|
|
||||||
|
|
||||||
float newIntegral = accumulationRate * m_dt + m_errorIntegral;
|
|
||||||
|
|
||||||
// Don't allow less than 0 error
|
|
||||||
if (newIntegral < 0) {
|
|
||||||
newIntegral = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
m_errorIntegral = newIntegral;
|
|
||||||
|
|
||||||
return newIntegral;
|
|
||||||
}
|
|
||||||
|
|
||||||
void ErrorAccumulator::reset() {
|
|
||||||
m_errorIntegral = 0;
|
|
||||||
}
|
|
|
@ -1,28 +0,0 @@
|
||||||
#pragma once
|
|
||||||
|
|
||||||
class ErrorAccumulator {
|
|
||||||
public:
|
|
||||||
void init(float ignoreError, float dt) {
|
|
||||||
m_ignoreError = ignoreError;
|
|
||||||
m_dt = dt;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Accumulate the current error, returning the total integrated error
|
|
||||||
float accumulate(float error);
|
|
||||||
|
|
||||||
// Get the current accumulator value
|
|
||||||
float getAccumulator() const {
|
|
||||||
return m_errorIntegral;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Reset the integrator to 0 error.
|
|
||||||
void reset();
|
|
||||||
|
|
||||||
private:
|
|
||||||
// more or less threshold parameter
|
|
||||||
float m_ignoreError = 0;
|
|
||||||
// time parameter
|
|
||||||
float m_dt = 0;
|
|
||||||
// current state
|
|
||||||
float m_errorIntegral = 0;
|
|
||||||
};
|
|
|
@ -8,7 +8,6 @@ UTILSRC_CPP = \
|
||||||
$(UTIL_DIR)/containers/listener_array.cpp \
|
$(UTIL_DIR)/containers/listener_array.cpp \
|
||||||
$(UTIL_DIR)/containers/local_version_holder.cpp \
|
$(UTIL_DIR)/containers/local_version_holder.cpp \
|
||||||
$(UTIL_DIR)/math/biquad.cpp \
|
$(UTIL_DIR)/math/biquad.cpp \
|
||||||
$(UTIL_DIR)/math/error_accumulator.cpp \
|
|
||||||
$(UTIL_DIR)/math/efi_pid.cpp \
|
$(UTIL_DIR)/math/efi_pid.cpp \
|
||||||
$(UTIL_DIR)/math/interpolation.cpp \
|
$(UTIL_DIR)/math/interpolation.cpp \
|
||||||
$(PROJECT_DIR)/util/datalogging.cpp \
|
$(PROJECT_DIR)/util/datalogging.cpp \
|
||||||
|
|
|
@ -1,53 +0,0 @@
|
||||||
#include "pch.h"
|
|
||||||
|
|
||||||
#include "error_accumulator.h"
|
|
||||||
|
|
||||||
TEST(errorAccumulator, ignoreSmallError) {
|
|
||||||
ErrorAccumulator dut;
|
|
||||||
dut.init(5, 0.01);
|
|
||||||
|
|
||||||
for (size_t i = 0; i < 1'000'000; i++) {
|
|
||||||
// An error just below the threshold should never trip
|
|
||||||
ASSERT_EQ(0, dut.accumulate(4));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(errorAccumulator, integrateError) {
|
|
||||||
ErrorAccumulator dut;
|
|
||||||
dut.init(5, 0.01);
|
|
||||||
|
|
||||||
for (size_t i = 0; i < 100; i++) {
|
|
||||||
// error of 1 over the ignore value
|
|
||||||
dut.accumulate(6);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Integral should be 1 * dt * 100 = 1.0
|
|
||||||
ASSERT_NEAR(dut.getAccumulator(), 1, 0.001f);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(errorAccumulator, integrateNegativeError) {
|
|
||||||
ErrorAccumulator dut;
|
|
||||||
dut.init(5, 0.01);
|
|
||||||
|
|
||||||
for (size_t i = 0; i < 100; i++) {
|
|
||||||
// error of 1 over the ignore value, but negative
|
|
||||||
dut.accumulate(-6);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Integral should be 1 * dt * 100 = 1.0
|
|
||||||
ASSERT_NEAR(dut.getAccumulator(), 1, 0.001f);
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(errorAccumulator, integrateErrorBothSigns) {
|
|
||||||
ErrorAccumulator dut;
|
|
||||||
dut.init(5, 0.01);
|
|
||||||
|
|
||||||
for (size_t i = 0; i < 100; i++) {
|
|
||||||
// These should collectively integrate 1 * dt worth of error
|
|
||||||
dut.accumulate(-5.5);
|
|
||||||
dut.accumulate(5.5);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Integral should be 2 * 0.5 * dt * 100 = 1.0
|
|
||||||
ASSERT_NEAR(dut.getAccumulator(), 1, 0.001f);
|
|
||||||
}
|
|
|
@ -1,7 +1,6 @@
|
||||||
|
|
||||||
|
|
||||||
CPPSRC += $(PROJECT_DIR)/../unit_tests/tests/util/test_buffered_writer.cpp \
|
CPPSRC += $(PROJECT_DIR)/../unit_tests/tests/util/test_buffered_writer.cpp \
|
||||||
$(PROJECT_DIR)/../unit_tests/tests/util/test_error_accumulator.cpp \
|
|
||||||
$(PROJECT_DIR)/../unit_tests/tests/util/test_exp_average.cpp \
|
$(PROJECT_DIR)/../unit_tests/tests/util/test_exp_average.cpp \
|
||||||
$(PROJECT_DIR)/../unit_tests/tests/util/test_hash.cpp \
|
$(PROJECT_DIR)/../unit_tests/tests/util/test_hash.cpp \
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue