diff --git a/firmware/controllers/sensors/sensor.h b/firmware/controllers/sensors/sensor.h index 0dab8fac5e..744775b2a3 100644 --- a/firmware/controllers/sensors/sensor.h +++ b/firmware/controllers/sensors/sensor.h @@ -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) diff --git a/firmware/init/sensor/init_tps.cpp b/firmware/init/sensor/init_tps.cpp index b444746d66..93fe3f6d5d 100644 --- a/firmware/init/sensor/init_tps.cpp +++ b/firmware/init/sensor/init_tps.cpp @@ -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(); }