dtc monitor status (#530)

* dtc monitor status * bug in dtc reporting * semicolon
FOME-Tech · Jan 9, 2025 · 4aa624b · 4aa624b
1 parent 101380a
commit 4aa624b
Showing 1 changed file with 21 additions and 4 deletions.
diff --git a/firmware/controllers/can/obd2.cpp b/firmware/controllers/can/obd2.cpp
@@ -102,6 +102,21 @@ static void obdWriteSupportedPids(int PID, int bitOffset, const int16_t *support
 	obdSendPacket(1, PID, 4, value, busIndex);
 }
 
+static void obdStatusQuery(int PID, CanBusIndex busIndex) {
+	static error_codes_set_s localErrorCopy;
+	getErrorCodes(&localErrorCopy);
+
+	CanTxMessage tx(OBD_TEST_RESPONSE, 7, busIndex, false);
+
+	tx[0] = 0x6;
+	tx[1] = 0x41;
+	tx[2] = PID;
+	tx[3] = localErrorCopy.count ? 0x80 : 0x0 + localErrorCopy.count;
+	tx[4] = 0x0;
+	tx[5] = 0x0;
+	tx[6] = 0x0;
+}
+
 static void handleGetDataRequest(const CANRxFrame& rx, CanBusIndex busIndex) {
 	int pid = rx.data8[2];
 	switch (pid) {
@@ -115,7 +130,7 @@ static void handleGetDataRequest(const CANRxFrame& rx, CanBusIndex busIndex) {
 		obdWriteSupportedPids(pid, 41, supportedPids4160, busIndex);
 		break;
 	case PID_MONITOR_STATUS:
-		obdSendPacket(1, pid, 4, 0, busIndex);	// todo: add statuses
+		obdStatusQuery(pid, busIndex);
 		break;
 	case PID_FUEL_SYSTEM_STATUS:
 		// todo: add statuses
@@ -193,19 +208,21 @@ static void handleDtcRequest(int numCodes, ObdCode* dtcCode, CanBusIndex busInde
 	if (numCodes == 0) {
         // No DTCs: Respond with no trouble codes
         CanTxMessage tx(OBD_TEST_RESPONSE, 2, busIndex, false);
-        tx[0] = 0x43;        // Service $03 response
-        tx[1] = 0;           // No DTCs
+		tx[0] = 0x2;
+        tx[1] = 0x43;        // Service $03 response
+        tx[2] = 0x0;           // No DTCs
         return;
     }
 
 	CanTxMessage tx(OBD_TEST_RESPONSE, 2, busIndex, false);
 	int dtcIndex = 0;
     int frameIndex = 0;
+	tx[1] = 0x43;
 
 	while (dtcIndex < numCodes) {
         if (frameIndex == 0) {
             // First frame setup
-            tx[1] = (numCodes * 2) & 0xFF; // Total DTC data length
+            tx[0] = (numCodes * 2) & 0xFF; // Total DTC data length
             int bytesAdded = 0;
 
             for (int i = 0; i < 3 && dtcIndex < numCodes; i++) {