support better wbo format (#4000)

* new format

* comment

* pr feedback

* heater duty

* extra

* comment

* test multi-accept logic

* s

* test new format

* this comment made no sense

Co-authored-by: Matthew Kennedy <makenne@microsoft.com>

firmware/controllers/sensors/AemXSeriesLambda.cpp

@@ -3,21 +3,57 @@
 #if EFI_CAN_SUPPORT || EFI_UNIT_TEST
 #include "AemXSeriesLambda.h"
 
+static constexpr uint32_t aem_base    = 0x180;
+static constexpr uint32_t rusefi_base = 0x190;
+
 AemXSeriesWideband::AemXSeriesWideband(uint8_t sensorIndex, SensorType type)
 	: CanSensorBase(
-		0x180 + sensorIndex,	// 0th sensor is 0x180, others sequential above that
+		0,	// ID passed here doesn't matter since we override acceptFrame
 		type,
 		MS2NT(21)	// sensor transmits at 100hz, allow a frame to be missed
 	)
 	, m_sensorIndex(sensorIndex)
 {}
 
-void AemXSeriesWideband::decodeFrame(const CANRxFrame& frame, efitick_t nowNt) {
+bool AemXSeriesWideband::acceptFrame(const CANRxFrame& frame) const {
 	if (frame.DLC != 8) {
-		invalidate();
+		return false;
-		return;
 	}
 	
+	uint32_t id = CAN_ID(frame);
+
+	// 0th sensor is 0x180, 1st sensor is 0x181, etc
+	uint32_t aemXSeriesId = aem_base + m_sensorIndex;
+
+	// 0th sensor is 0x190 and 0x191, 1st sensor is 0x192 and 0x193
+	uint32_t rusefiBaseId = rusefi_base + 2 * m_sensorIndex;
+
+	return 
+		id == aemXSeriesId ||
+		id == rusefiBaseId ||
+		id == rusefiBaseId + 1;
+}
+
+void AemXSeriesWideband::decodeFrame(const CANRxFrame& frame, efitick_t nowNt) {
+	int32_t id = CAN_ID(frame);
+
+	// accept frame has already guaranteed that this message belongs to us
+	// We just have to check if it's AEM or rusEFI
+	if (id < rusefi_base) {
+		decodeAemXSeries(frame, nowNt);
+	} else {
+		// rusEFI custom format
+		if ((id & 0x1) != 0) {
+			// low bit is set, this is the "diag" frame
+			decodeRusefiDiag(frame);
+		} else {
+			// low bit not set, this is standard frame
+			decodeRusefiStandard(frame, nowNt);
+		}
+	}
+}
+
+void AemXSeriesWideband::decodeAemXSeries(const CANRxFrame& frame, efitick_t nowNt) {
 	// reports in 0.0001 lambda per LSB
 	uint16_t lambdaInt = SWAP_UINT16(frame.data16[0]);
 	float lambdaFloat = 0.0001f * lambdaInt;
@@ -63,4 +99,59 @@ void AemXSeriesWideband::decodeFrame(const CANRxFrame& frame, efitick_t nowNt) {
 	setValidValue(lambdaFloat, nowNt);
 }
 
+// TODO: include rusEFI wideband file directly
+namespace wbo
+{
+struct StandardData
+{
+    uint8_t Version;
+    uint8_t Valid;
+
+    uint16_t Lambda;
+    uint16_t TemperatureC;
+
+    uint16_t pad;
+};
+
+struct DiagData
+{
+    uint16_t Esr;
+    uint16_t NernstDc;
+    uint8_t PumpDuty;
+    uint8_t Status;
+
+    uint8_t HeaterDuty;
+    uint8_t pad;
+};
+} // namespace wbo
+
+void AemXSeriesWideband::decodeRusefiStandard(const CANRxFrame& frame, efitick_t nowNt) {
+	auto data = reinterpret_cast<const wbo::StandardData*>(&frame.data8[0]);
+
+	// TODO: enforce version check
+	//bool versionValid = data->Version != RUSEFI_WIDEBAND_VERSION;
+
+	float lambda = 0.0001f * data->Lambda;
+	engine->outputChannels.wbTemperature[m_sensorIndex] = data->TemperatureC;
+
+	bool valid = data->Valid != 0;
+
+	if (valid) {
+		setValidValue(lambda, nowNt);
+	} else {
+		invalidate();
+	}
+}
+
+void AemXSeriesWideband::decodeRusefiDiag(const CANRxFrame& frame) {
+	auto data = reinterpret_cast<const wbo::DiagData*>(&frame.data8[0]);
+
+	engine->outputChannels.wbHeaterDuty[m_sensorIndex] = data->HeaterDuty / 255.0f;
+
+	if (m_sensorIndex == 0 || engineConfiguration->debugMode == DBG_RUSEFI_WIDEBAND) {
+		engine->outputChannels.debugFloatField1 = data->PumpDuty / 255.0f;
+		engine->outputChannels.debugFloatField3 = data->NernstDc / 1000.0f;
+	}
+}
+
 #endif

firmware/controllers/sensors/AemXSeriesLambda.h

@@ -6,9 +6,19 @@ class AemXSeriesWideband final : public CanSensorBase {
 public:
 	AemXSeriesWideband(uint8_t sensorIndex, SensorType type);
 
+	bool acceptFrame(const CANRxFrame& frame) const override;
+
 protected:
+	// Dispatches to one of the three decoders below
 	void decodeFrame(const CANRxFrame& frame, efitick_t nowNt) override;
 
+	// Decode an actual AEM controller, or a rusEFI controller sending AEM format
+	void decodeAemXSeries(const CANRxFrame& frame, efitick_t nowNt);
+
+	// Decode rusEFI custom format
+	void decodeRusefiStandard(const CANRxFrame& frame, efitick_t nowNt);
+	void decodeRusefiDiag(const CANRxFrame& frame);
+
 private:
 	const uint8_t m_sensorIndex;
 };

unit_tests/tests/test_can_wideband.cpp

@@ -2,12 +2,49 @@
 
 #include "AemXSeriesLambda.h"
 
-using ::testing::StrictMock;
+TEST(CanWideband, AcceptFrameId0) {
-using ::testing::_;
+	AemXSeriesWideband dut(0, SensorType::Lambda1);
 
-TEST(CanWideband, DecodeValid) {
+	CANRxFrame frame;
-	Sensor::resetRegistry();
 
+	frame.IDE = false;
+	frame.DLC = 8;
+
+	// Check that the AEM format frame is accepted
+	frame.SID = 0x180;
+	EXPECT_TRUE(dut.acceptFrame(frame));
+
+	// Check that the rusEFI standard data is accepted
+	frame.SID = 0x190;
+	EXPECT_TRUE(dut.acceptFrame(frame));
+
+	// Check that the rusEFI extended data is accepted
+	frame.SID = 0x191;
+	EXPECT_TRUE(dut.acceptFrame(frame));
+}
+
+TEST(CanWideband, AcceptFrameId1) {
+	AemXSeriesWideband dut(1, SensorType::Lambda2);
+
+	CANRxFrame frame;
+
+	frame.IDE = false;
+	frame.DLC = 8;
+
+	// Check that the AEM format frame is accepted
+	frame.SID = 0x181;
+	EXPECT_TRUE(dut.acceptFrame(frame));
+
+	// Check that the rusEFI standard data is accepted
+	frame.SID = 0x192;
+	EXPECT_TRUE(dut.acceptFrame(frame));
+
+	// Check that the rusEFI extended data is accepted
+	frame.SID = 0x193;
+	EXPECT_TRUE(dut.acceptFrame(frame));
+}
+
+TEST(CanWideband, DecodeValidAemFormat) {
 	AemXSeriesWideband dut(0, SensorType::Lambda1);
 	dut.Register();
 
@@ -54,4 +91,48 @@ TEST(CanWideband, DecodeValid) {
 
 	dut.processFrame(frame, getTimeNowNt());
 	EXPECT_FLOAT_EQ(-1, Sensor::get(SensorType::Lambda1).value_or(-1));
+
+	Sensor::resetRegistry();
+}
+
+TEST(CanWideband, DecodeRusefiStandard)
+{
+	EngineTestHelper eth(TEST_ENGINE);
+
+	AemXSeriesWideband dut(0, SensorType::Lambda1);
+	dut.Register();
+
+	CANRxFrame frame;
+	frame.SID = 0x190;
+	frame.IDE = false;
+	frame.DLC = 8;
+
+	// version
+	frame.data8[0] = 0;
+
+	// valid
+	frame.data8[1] = 1;
+
+	// data = 0.7 lambda
+	*reinterpret_cast<uint16_t*>(&frame.data8[2]) = 7000;
+
+	// data = 1234 deg C
+	*reinterpret_cast<uint16_t*>(&frame.data8[4]) = 1234;
+
+	engine->outputChannels.wbTemperature[0] = 0;
+
+	// check not set
+	EXPECT_FLOAT_EQ(-1, Sensor::get(SensorType::Lambda1).value_or(-1));
+
+	// check that lambda updates
+	dut.processFrame(frame, getTimeNowNt());
+	EXPECT_FLOAT_EQ(0.7f, Sensor::get(SensorType::Lambda1).value_or(-1));
+
+	// Check that temperature updates
+	EXPECT_EQ(engine->outputChannels.wbTemperature[0], 1234);
+
+	// Check that valid bit is respected (should be invalid now)
+	frame.data8[1] = 0;
+	dut.processFrame(frame, getTimeNowNt());
+	EXPECT_FLOAT_EQ(-1, Sensor::get(SensorType::Lambda1).value_or(-1));
 }