rusefi/firmware/init/sensor/init_tps.cpp

#include "pch.h"

#include "adc_subscription.h"
#include "functional_sensor.h"
#include "redundant_sensor.h"
#include "redundant_ford_tps.h"
#include "proxy_sensor.h"
#include "linear_func.h"
#include "tps.h"
#include "auto_generated_sensor.h"
#include "defaults.h"

struct TpsConfig {
	adc_channel_e channel;
	float closed;
	float open;
	float min;
	float max;
};

class FuncSensPair {
public:
AdcSubscriptionEntry *adc = nullptr;
	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;
		}

		adc = AdcSubscription::SubscribeSensor(m_sens, cfg.channel, /*lowpassCutoffHz*/ 200);

		return m_sens.Register();
	}

	void deinit() {
		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)) {
#if EFI_UNIT_TEST
	printf("Configured NO hardware %s\n", name());
#endif
			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(ObdCode::OBD_TPS_Configuration, "\"%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, POSITION_FULLY_OPEN,
			cfg.min, cfg.max
		);

#if EFI_UNIT_TEST
	printf("Configured YES %s\n", name());
#endif
		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, float secondaryMaximum, const TpsConfig& primary, const TpsConfig& secondary, bool allowIdenticalSensors = false) {
		bool hasFirst = m_pri.init(primary);
		if (!hasFirst) {
			// no input if we have no first channel
			return;
		}

		if (!allowIdenticalSensors) {
			// 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(ObdCode::OBD_TPS_Configuration, "Configuration for redundant pair %s/%s are too similar - did you wire one sensor to both inputs...?", m_pri.name(), m_sec.name());
				return;
			}
		}

		bool hasSecond = m_sec.init(secondary);

        if (engineConfiguration->etbSplit <= 0 || engineConfiguration->etbSplit > MAX_TPS_PPS_DISCREPANCY) {
            engineConfiguration->etbSplit = MAX_TPS_PPS_DISCREPANCY;
        }

		if (isFordTps && fordTps) {
			// we have a secondary
			fordTps->configure(engineConfiguration->etbSplit, secondaryMaximum);
			fordTps->Register();
		} else {
			// not ford TPS
			m_redund.configure(engineConfiguration->etbSplit, !hasSecond);
#if EFI_UNIT_TEST
printf("init m_redund.Register() %s\n", getSensorType(m_redund.type()));
#endif
			m_redund.Register();
		}
	}

	void deinit(bool isFordTps, RedundantFordTps* fordTps) {
		m_pri.deinit();
		m_sec.deinit();

		if (isFordTps && fordTps) {
			fordTps->unregister();
		} else {
			m_redund.unregister();
		}

	}

	void updateUnfilteredRawValues() {
	  engine->outputChannels.rawRawPpsPrimary = m_pri.adc == nullptr ? 0 : m_pri.adc->sensorVolts;
	  engine->outputChannels.rawRawPpsSecondary = m_sec.adc == nullptr ? 0 : m_sec.adc->sensorVolts;
	}

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
static RedundantPair analogTps1(tps1p, tps1s, SensorType::Tps1);
static RedundantPair tps2(tps2p, tps2s, SensorType::Tps2);

SentTps sentTps;

// Used only in case of weird Ford-style ETB TPS
static RedundantFordTps fordTps1(SensorType::Tps1, SensorType::Tps1Primary, SensorType::Tps1Secondary);
static RedundantFordTps fordTps2(SensorType::Tps2, SensorType::Tps2Primary, SensorType::Tps2Secondary);
static RedundantFordTps fordPps(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);

void updateUnfilteredRawPedal() {
  pedal.updateUnfilteredRawValues();
}

// This sensor indicates the driver's throttle intent - Pedal if we have one, TPS if not.
static ProxySensor driverIntent(SensorType::DriverThrottleIntent);

// These sensors are TPS-like, so handle them in here too
static FuncSensPair wastegate(PACK_MULT_VOLTAGE, SensorType::WastegatePosition);
static FuncSensPair idlePos(PACK_MULT_VOLTAGE, SensorType::IdlePosition);

bool isDigitalTps1() {
    return isBrainPinValid(engineConfiguration->sentInputPins[0]) && engineConfiguration->sentEtbType != SentEtbType::NONE;
}

void initTps() {
    criticalAssertVoid(engineConfiguration != nullptr, "null engineConfiguration");
	percent_t minTpsPps = engineConfiguration->tpsErrorDetectionTooLow;
	percent_t maxTpsPps = engineConfiguration->tpsErrorDetectionTooHigh;

	if (!engineConfiguration->consumeObdSensors) {
		bool isFordTps = engineConfiguration->useFordRedundantTps;
		bool isFordPps = engineConfiguration->useFordRedundantPps;

		float tpsSecondaryMaximum = engineConfiguration->tpsSecondaryMaximum;
		if (tpsSecondaryMaximum < 20) {
			// don't allow <20% split point
			tpsSecondaryMaximum = 20;
		}


        if (isDigitalTps1()) {
            sentTps.Register();
        } else {
		    analogTps1.init(isFordTps, &fordTps1, tpsSecondaryMaximum,
    			{ engineConfiguration->tps1_1AdcChannel, (float)engineConfiguration->tpsMin, (float)engineConfiguration->tpsMax, minTpsPps, maxTpsPps },
	    		{ engineConfiguration->tps1_2AdcChannel, (float)engineConfiguration->tps1SecondaryMin, (float)engineConfiguration->tps1SecondaryMax, minTpsPps, maxTpsPps }
		    );
		}

		tps2.init(isFordTps, &fordTps2, tpsSecondaryMaximum,
			{ engineConfiguration->tps2_1AdcChannel, (float)engineConfiguration->tps2Min, (float)engineConfiguration->tps2Max, minTpsPps, maxTpsPps },
			{ engineConfiguration->tps2_2AdcChannel, (float)engineConfiguration->tps2SecondaryMin, (float)engineConfiguration->tps2SecondaryMax, minTpsPps, maxTpsPps }
		);

		float ppsSecondaryMaximum = engineConfiguration->ppsSecondaryMaximum;
		if (ppsSecondaryMaximum < 20) {
			// don't allow <20% split point
			ppsSecondaryMaximum = 20;
		}

	// Pedal sensors
	pedal.init(isFordPps, &fordPps, ppsSecondaryMaximum,
		{ engineConfiguration->throttlePedalPositionAdcChannel, engineConfiguration->throttlePedalUpVoltage, engineConfiguration->throttlePedalWOTVoltage, minTpsPps, maxTpsPps },
		{ engineConfiguration->throttlePedalPositionSecondAdcChannel, engineConfiguration->throttlePedalSecondaryUpVoltage, engineConfiguration->throttlePedalSecondaryWOTVoltage, minTpsPps, maxTpsPps },
		engineConfiguration->allowIdenticalPps
	);

		// TPS-like stuff that isn't actually a TPS
		wastegate.init({ engineConfiguration->wastegatePositionSensor, (float)engineConfiguration->wastegatePositionMin, (float)engineConfiguration->wastegatePositionMax, minTpsPps, maxTpsPps });
		idlePos.init({ engineConfiguration->idlePositionChannel, (float)engineConfiguration->idlePositionMin, (float)engineConfiguration->idlePositionMax, minTpsPps, maxTpsPps });
	}

	// Route the pedal or TPS to driverIntent as appropriate
	if (isAdcChannelValid(engineConfiguration->throttlePedalPositionAdcChannel)) {
		driverIntent.setProxiedSensor(SensorType::AcceleratorPedal);
	} else {
		driverIntent.setProxiedSensor(SensorType::Tps1);
	}

	driverIntent.Register();
}

void deinitTps() {
	bool isFordTps = activeConfiguration.useFordRedundantTps;
	bool isFordPps = activeConfiguration.useFordRedundantPps;

	analogTps1.deinit(isFordTps, &fordTps1);
	tps2.deinit(isFordTps, &fordTps2);
	pedal.deinit(isFordPps, &fordPps);

	sentTps.unregister();

	wastegate.deinit();
	idlePos.deinit();
}