rewrite tps init, check if your primary/secondary sensors are too similar (#4231)
* rewrite tps init * staticstaticstaticstaticstaticstaticstatic
This commit is contained in:
Matthew Kennedy
GitHub
parent
33b67809d4
commit
d447f03a3e
|
|
@ -138,7 +138,7 @@ public:
|
|||
* Get a friendly name for the sensor.
|
||||
* For example, CLT, IAT, Throttle Position 2, etc.
|
||||
*/
|
||||
const char* getSensorName() { return getSensorName(m_type); }
|
||||
const char* getSensorName() const { return getSensorName(m_type); }
|
||||
static const char* getSensorName(SensorType type);
|
||||
|
||||
// Retrieve the current reading from the sensor.
|
||||
|
|
@ -170,6 +170,10 @@ public:
|
|||
|
||||
void unregister();
|
||||
|
||||
SensorType type() const {
|
||||
return m_type;
|
||||
}
|
||||
|
||||
protected:
|
||||
// Protected constructor - only subclasses call this
|
||||
explicit Sensor(SensorType type)
|
||||
|
|
|
|||
|
|
@ -8,114 +8,180 @@
|
|||
#include "linear_func.h"
|
||||
#include "tps.h"
|
||||
|
||||
LinearFunc tpsFunc1p(TPS_TS_CONVERSION);
|
||||
LinearFunc tpsFunc1s(TPS_TS_CONVERSION);
|
||||
LinearFunc tpsFunc2p(TPS_TS_CONVERSION);
|
||||
LinearFunc tpsFunc2s(TPS_TS_CONVERSION);
|
||||
struct TpsConfig {
|
||||
adc_channel_e channel;
|
||||
float closed;
|
||||
float open;
|
||||
float min;
|
||||
float max;
|
||||
};
|
||||
|
||||
FunctionalSensor tpsSens1p(SensorType::Tps1Primary, MS2NT(10));
|
||||
FunctionalSensor tpsSens1s(SensorType::Tps1Secondary, MS2NT(10));
|
||||
FunctionalSensor tpsSens2p(SensorType::Tps2Primary, MS2NT(10));
|
||||
FunctionalSensor tpsSens2s(SensorType::Tps2Secondary, MS2NT(10));
|
||||
class FuncSensPair {
|
||||
public:
|
||||
FuncSensPair(float divideInput, SensorType type)
|
||||
: m_func(divideInput)
|
||||
, m_sens(type, MS2NT(10))
|
||||
{
|
||||
m_sens.setFunction(m_func);
|
||||
}
|
||||
|
||||
bool init(const TpsConfig& cfg) {
|
||||
// If the configuration was invalid, don't continue to configure the sensor
|
||||
if (!configure(cfg)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
AdcSubscription::SubscribeSensor(m_sens, cfg.channel, 200);
|
||||
|
||||
return m_sens.Register();
|
||||
}
|
||||
|
||||
void unsubscribe() {
|
||||
AdcSubscription::UnsubscribeSensor(m_sens);
|
||||
}
|
||||
|
||||
SensorType type() const {
|
||||
return m_sens.type();
|
||||
}
|
||||
|
||||
const char* name() const {
|
||||
return m_sens.getSensorName();
|
||||
}
|
||||
|
||||
private:
|
||||
bool configure(const TpsConfig& cfg) {
|
||||
// Only configure if we have a channel
|
||||
if (!isAdcChannelValid(cfg.channel)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
float scaledClosed = cfg.closed / m_func.getDivideInput();
|
||||
float scaledOpen = cfg.open / m_func.getDivideInput();
|
||||
|
||||
float split = absF(scaledOpen - scaledClosed);
|
||||
|
||||
// If the voltage for closed vs. open is very near, something is wrong with your calibration
|
||||
if (split < 0.5f) {
|
||||
firmwareError(OBD_Throttle_Position_Sensor_Circuit_Malfunction, "\"%s\" problem: open %.2f/closed %.2f cal values are too close together. Check your calibration and wiring!", name(),
|
||||
cfg.open,
|
||||
cfg.closed);
|
||||
return false;
|
||||
}
|
||||
|
||||
m_func.configure(
|
||||
cfg.closed, 0,
|
||||
cfg.open, 100,
|
||||
cfg.min, cfg.max
|
||||
);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
LinearFunc m_func;
|
||||
FunctionalSensor m_sens;
|
||||
};
|
||||
|
||||
struct RedundantPair {
|
||||
public:
|
||||
RedundantPair(FuncSensPair& pri, FuncSensPair& sec, SensorType outputType)
|
||||
: m_pri(pri)
|
||||
, m_sec(sec)
|
||||
, m_redund(outputType, m_pri.type(), m_sec.type())
|
||||
{
|
||||
}
|
||||
|
||||
void init(bool isFordTps, RedundantFordTps* fordTps, const TpsConfig& primary, const TpsConfig& secondary) {
|
||||
{
|
||||
// Check that the primary and secondary aren't too close together - if so, the user may have done
|
||||
// an unsafe thing where they wired a single sensor to both inputs. Don't do that!
|
||||
bool hasBothSensors = isAdcChannelValid(primary.channel) && isAdcChannelValid(secondary.channel);
|
||||
bool tooCloseClosed = absF(primary.closed - secondary.closed) < 0.2f;
|
||||
bool tooCloseOpen = absF(primary.open - secondary.open) < 0.2f;
|
||||
|
||||
if (hasBothSensors && tooCloseClosed && tooCloseOpen) {
|
||||
firmwareError(OBD_Throttle_Position_Sensor_Circuit_Malfunction, "Configuration for redundant pair %s/%s are too similar - did you wire one sensor to both inputs...?", m_pri.name(), m_sec.name());
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
m_pri.init(primary);
|
||||
|
||||
bool hasSecond = m_sec.init(secondary);
|
||||
|
||||
if (isFordTps && fordTps) {
|
||||
// we have a secondary
|
||||
fordTps->configure(5.0f, 52.6f);
|
||||
fordTps->Register();
|
||||
} else {
|
||||
// not ford TPS
|
||||
m_redund.configure(5.0f, !hasSecond);
|
||||
m_redund.Register();
|
||||
}
|
||||
}
|
||||
|
||||
void unsubscribe() {
|
||||
m_pri.unsubscribe();
|
||||
m_sec.unsubscribe();
|
||||
}
|
||||
|
||||
private:
|
||||
FuncSensPair& m_pri;
|
||||
FuncSensPair& m_sec;
|
||||
|
||||
RedundantSensor m_redund;
|
||||
};
|
||||
|
||||
static FuncSensPair tps1p(TPS_TS_CONVERSION, SensorType::Tps1Primary);
|
||||
static FuncSensPair tps1s(TPS_TS_CONVERSION, SensorType::Tps1Secondary);
|
||||
static FuncSensPair tps2p(TPS_TS_CONVERSION, SensorType::Tps2Primary);
|
||||
static FuncSensPair tps2s(TPS_TS_CONVERSION, SensorType::Tps2Secondary);
|
||||
|
||||
// Used in case of "normal", non-Ford ETB TPS
|
||||
RedundantSensor tps1(SensorType::Tps1, SensorType::Tps1Primary, SensorType::Tps1Secondary);
|
||||
RedundantSensor tps2(SensorType::Tps2, SensorType::Tps2Primary, SensorType::Tps2Secondary);
|
||||
static RedundantPair tps1(tps1p, tps1s, SensorType::Tps1);
|
||||
static RedundantPair tps2(tps2p, tps2s, SensorType::Tps2);
|
||||
|
||||
// Used only in case of weird Ford-style ETB TPS
|
||||
RedundantFordTps fordTps1(SensorType::Tps1, SensorType::Tps1Primary, SensorType::Tps1Secondary);
|
||||
RedundantFordTps fordTps2(SensorType::Tps2, SensorType::Tps2Primary, SensorType::Tps2Secondary);
|
||||
static RedundantFordTps fordTps1(SensorType::Tps1, SensorType::Tps1Primary, SensorType::Tps1Secondary);
|
||||
static RedundantFordTps fordTps2(SensorType::Tps2, SensorType::Tps2Primary, SensorType::Tps2Secondary);
|
||||
|
||||
LinearFunc pedalFuncPrimary;
|
||||
LinearFunc pedalFuncSecondary;
|
||||
FunctionalSensor pedalSensorPrimary(SensorType::AcceleratorPedalPrimary, MS2NT(10));
|
||||
FunctionalSensor pedalSensorSecondary(SensorType::AcceleratorPedalSecondary, MS2NT(10));
|
||||
|
||||
RedundantSensor pedal(SensorType::AcceleratorPedal, SensorType::AcceleratorPedalPrimary, SensorType::AcceleratorPedalSecondary);
|
||||
// Pedal sensors and redundancy
|
||||
static FuncSensPair pedalPrimary(1, SensorType::AcceleratorPedalPrimary);
|
||||
static FuncSensPair pedalSecondary(1, SensorType::AcceleratorPedalSecondary);
|
||||
static RedundantPair pedal(pedalPrimary, pedalSecondary, SensorType::AcceleratorPedal);
|
||||
|
||||
// This sensor indicates the driver's throttle intent - Pedal if we have one, TPS if not.
|
||||
ProxySensor driverIntent(SensorType::DriverThrottleIntent);
|
||||
static ProxySensor driverIntent(SensorType::DriverThrottleIntent);
|
||||
|
||||
// These sensors are TPS-like, so handle them in here too
|
||||
LinearFunc wastegateFunc(PACK_MULT_VOLTAGE);
|
||||
LinearFunc idlePosFunc(PACK_MULT_VOLTAGE);
|
||||
FunctionalSensor wastegateSens(SensorType::WastegatePosition, MS2NT(10));
|
||||
FunctionalSensor idlePosSens(SensorType::IdlePosition, MS2NT(10));
|
||||
|
||||
static bool configureTps(LinearFunc& func, adc_channel_e channel, float closed, float open, float min, float max, const char* msg) {
|
||||
// Only configure if we have a channel
|
||||
if (!isAdcChannelValid(channel)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
float scaledClosed = closed / func.getDivideInput();
|
||||
float scaledOpen = open / func.getDivideInput();
|
||||
|
||||
float split = absF(scaledOpen - scaledClosed);
|
||||
|
||||
// If the voltage for closed vs. open is very near, something is wrong with your calibration
|
||||
if (split < 0.5f) {
|
||||
firmwareError(OBD_Throttle_Position_Sensor_Circuit_Malfunction, "\"%s\" problem: open %.2f/closed %.2f cal values are too close together. Check your calibration and wiring!", msg,
|
||||
open,
|
||||
closed);
|
||||
return false;
|
||||
}
|
||||
|
||||
func.configure(
|
||||
closed, 0,
|
||||
open, 100,
|
||||
min, max
|
||||
);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool initTpsFunc(LinearFunc& func, FunctionalSensor& sensor, adc_channel_e channel, float closed, float open, float min, float max) {
|
||||
// If the configuration was invalid, don't continue to configure the sensor
|
||||
if (!configureTps(func, channel, closed, open, min, max, sensor.getSensorName())) {
|
||||
return false;
|
||||
}
|
||||
|
||||
sensor.setFunction(func);
|
||||
|
||||
AdcSubscription::SubscribeSensor(sensor, channel, 200);
|
||||
|
||||
return sensor.Register();
|
||||
}
|
||||
|
||||
static void initTpsFuncAndRedund(RedundantSensor& redund, RedundantFordTps* fordTps, bool isFordTps, LinearFunc& func, FunctionalSensor& sensor, adc_channel_e channel, float closed, float open, float min, float max) {
|
||||
bool hasSecond = initTpsFunc(func, sensor, channel, closed, open, min, max);
|
||||
|
||||
if (isFordTps && fordTps) {
|
||||
fordTps->configure(5.0f, 52.6f);
|
||||
fordTps->Register();
|
||||
} else {
|
||||
redund.configure(5.0f, !hasSecond);
|
||||
redund.Register();
|
||||
}
|
||||
}
|
||||
static FuncSensPair wastegate(PACK_MULT_VOLTAGE, SensorType::WastegatePosition);
|
||||
static FuncSensPair idlePos(PACK_MULT_VOLTAGE, SensorType::IdlePosition);
|
||||
|
||||
void initTps() {
|
||||
percent_t min = engineConfiguration->tpsErrorDetectionTooLow;
|
||||
percent_t max = engineConfiguration->tpsErrorDetectionTooHigh;
|
||||
|
||||
if (!engineConfiguration->consumeObdSensors) {
|
||||
// primary TPS sensors
|
||||
initTpsFunc(tpsFunc1p, tpsSens1p, engineConfiguration->tps1_1AdcChannel, engineConfiguration->tpsMin, engineConfiguration->tpsMax, min, max);
|
||||
initTpsFunc(tpsFunc2p, tpsSens2p, engineConfiguration->tps2_1AdcChannel, engineConfiguration->tps2Min, engineConfiguration->tps2Max, min, max);
|
||||
|
||||
// Secondary TPS sensors (and redundant combining)
|
||||
bool isFordTps = engineConfiguration->useFordRedundantTps;
|
||||
initTpsFuncAndRedund(tps1, &fordTps1, isFordTps, tpsFunc1s, tpsSens1s, engineConfiguration->tps1_2AdcChannel, engineConfiguration->tps1SecondaryMin, engineConfiguration->tps1SecondaryMax, min, max);
|
||||
initTpsFuncAndRedund(tps2, &fordTps2, isFordTps, tpsFunc2s, tpsSens2s, engineConfiguration->tps2_2AdcChannel, engineConfiguration->tps2SecondaryMin, engineConfiguration->tps2SecondaryMax, min, max);
|
||||
|
||||
tps1.init(isFordTps, &fordTps1,
|
||||
{ engineConfiguration->tps1_1AdcChannel, (float)engineConfiguration->tpsMin, (float)engineConfiguration->tpsMax, min, max },
|
||||
{ engineConfiguration->tps1_2AdcChannel, (float)engineConfiguration->tps1SecondaryMin, (float)engineConfiguration->tps1SecondaryMax, min, max }
|
||||
);
|
||||
|
||||
tps2.init(isFordTps, &fordTps2,
|
||||
{ engineConfiguration->tps2_1AdcChannel, (float)engineConfiguration->tps2Min, (float)engineConfiguration->tps2Max, min, max },
|
||||
{ engineConfiguration->tps2_2AdcChannel, (float)engineConfiguration->tps2SecondaryMin, (float)engineConfiguration->tps2SecondaryMax, min, max }
|
||||
);
|
||||
|
||||
// Pedal sensors
|
||||
initTpsFunc(pedalFuncPrimary, pedalSensorPrimary, engineConfiguration->throttlePedalPositionAdcChannel, engineConfiguration->throttlePedalUpVoltage, engineConfiguration->throttlePedalWOTVoltage, min, max);
|
||||
initTpsFuncAndRedund(pedal, nullptr, false, pedalFuncSecondary, pedalSensorSecondary, engineConfiguration->throttlePedalPositionSecondAdcChannel, engineConfiguration->throttlePedalSecondaryUpVoltage, engineConfiguration->throttlePedalSecondaryWOTVoltage, min, max);
|
||||
pedal.init(false, nullptr,
|
||||
{ engineConfiguration->throttlePedalPositionAdcChannel, engineConfiguration->throttlePedalUpVoltage, engineConfiguration->throttlePedalWOTVoltage, min, max },
|
||||
{ engineConfiguration->throttlePedalPositionSecondAdcChannel, engineConfiguration->throttlePedalSecondaryUpVoltage, engineConfiguration->throttlePedalSecondaryWOTVoltage, min, max }
|
||||
);
|
||||
|
||||
// TPS-like stuff that isn't actually a TPS
|
||||
initTpsFunc(wastegateFunc, wastegateSens, engineConfiguration->wastegatePositionSensor, engineConfiguration->wastegatePositionMin, engineConfiguration->wastegatePositionMax, min, max);
|
||||
initTpsFunc(idlePosFunc, idlePosSens, engineConfiguration->idlePositionSensor, engineConfiguration->idlePositionMin, engineConfiguration->idlePositionMax, min, max);
|
||||
wastegate.init({ engineConfiguration->wastegatePositionSensor, (float)engineConfiguration->wastegatePositionMin, (float)engineConfiguration->wastegatePositionMax, min, max });
|
||||
idlePos.init({ engineConfiguration->idlePositionSensor, (float)engineConfiguration->idlePositionMin, (float)engineConfiguration->idlePositionMax, min, max });
|
||||
}
|
||||
|
||||
// Route the pedal or TPS to driverIntent as appropriate
|
||||
|
|
@ -129,15 +195,10 @@ void initTps() {
|
|||
}
|
||||
|
||||
void deinitTps() {
|
||||
AdcSubscription::UnsubscribeSensor(tpsSens1p);
|
||||
AdcSubscription::UnsubscribeSensor(tpsSens1s);
|
||||
tps1.unsubscribe();
|
||||
tps2.unsubscribe();
|
||||
pedal.unsubscribe();
|
||||
|
||||
AdcSubscription::UnsubscribeSensor(tpsSens2p);
|
||||
AdcSubscription::UnsubscribeSensor(tpsSens2s);
|
||||
|
||||
AdcSubscription::UnsubscribeSensor(pedalSensorPrimary);
|
||||
AdcSubscription::UnsubscribeSensor(pedalSensorSecondary);
|
||||
|
||||
AdcSubscription::UnsubscribeSensor(wastegateSens);
|
||||
AdcSubscription::UnsubscribeSensor(idlePosSens);
|
||||
wastegate.unsubscribe();
|
||||
idlePos.unsubscribe();
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue