[OBDII] Fixed lambda and flex precision (speeduino#1269)

Faster TPS math
adbancroft · Dec 20, 2024 · 81e7cff · 81e7cff
1 parent 41e54d4
commit 81e7cff
Showing 1 changed file with 12 additions and 14 deletions.
diff --git a/speeduino/comms_CAN.cpp b/speeduino/comms_CAN.cpp
@@ -478,7 +478,7 @@ void obd_response(uint8_t PIDmode, uint8_t requestedPIDlow, uint8_t requestedPID
       case 10:        // PID-0x0A , Fuel Pressure (Gauge) , range is 0 to 765 kPa , formula == A / 3)
         uint16_t temp_fuelpressure;
         // Fuel pressure is in PSI. PSI to kPa is 6.89475729, but that needs to be divided by 3 for OBD2 formula. So 2.298.... 2.3 is close enough, so that in fraction.
-        temp_fuelpressure = (currentStatus.fuelPressure * 23) / 10;
+        temp_fuelpressure = udiv_32_16((23u * currentStatus.fuelPressure), 10);
         outMsg.buf[0] =  0x03;    // sending 3 byte
         outMsg.buf[1] =  0x41;    // 
         outMsg.buf[2] =  0x0A;    // pid code
@@ -550,10 +550,9 @@ void obd_response(uint8_t PIDmode, uint8_t requestedPIDlow, uint8_t requestedPID
       break;
 
       case 17:  // PID-0x11 , 
-        // TPS percentage, range is 0 to 100 percent, formula == 100/256 A 
-        uint16_t temp_tpsPC;
-        temp_tpsPC = currentStatus.TPS;
-        obdcalcA = (temp_tpsPC <<8) / 200;     // (tpsPC *256) /200;
+        // TPS percentage, range is 0 to 100 percent, formula == 100/255 A
+        // Faster math with possible to be off by 1(0.329%)
+        obdcalcA = (327u * currentStatus.TPS) >> 8; //327 * 200 = 0xFF78
         if (obdcalcA > UINT8_MAX){ obdcalcA = UINT8_MAX;}
         outMsg.buf[0] =  0x03;                    // sending 3 bytes
         outMsg.buf[1] =  0x41;                    // Same as query, except that 40h is added to the mode value. So:41h = show current data ,42h = freeze frame ,etc.
@@ -605,10 +604,8 @@ void obd_response(uint8_t PIDmode, uint8_t requestedPIDlow, uint8_t requestedPID
       case 36:      // PID-0x24 O2 sensor2, AB: fuel/air equivalence ratio, CD: voltage ,  Formula == (2/65536)(256A +B) , 8/65536(256C+D) , Range is 0 to <2 and 0 to >8V 
         //uint16_t O2_1e ;
         //int16_t O2_1v ; 
-        obdcalcH16 = configPage2.stoich/10 ;            // configPage2.stoich(is *10 so 14.7 is 147)
-        obdcalcE32 = currentStatus.O2/10;            // afr(is *10 so 25.5 is 255) , needs a 32bit else will overflow
-        obdcalcF32 = (obdcalcE32<<8) / obdcalcH16;      //this is same as (obdcalcE32/256) / obdcalcH16 . this calculates the ratio      
-        obdcalcG16 = (obdcalcF32 *32768)>>8;          
+        obdcalcF32 = currentStatus.O2;            // afr(is *10 so 25.5 is 255) , needs a 32bit else will overflow
+        obdcalcG16 = udiv_32_16((obdcalcF32 * 32768u), configPage2.stoich);
         obdcalcA = highByte(obdcalcG16);
         obdcalcB = lowByte(obdcalcG16);       
 
@@ -630,10 +627,8 @@ void obd_response(uint8_t PIDmode, uint8_t requestedPIDlow, uint8_t requestedPID
       case 37:      //O2 sensor2, AB fuel/air equivalence ratio, CD voltage ,  2/65536(256A +B) ,8/65536(256C+D) , range is 0 to <2 and 0 to >8V
         //uint16_t O2_2e ;
         //int16_t O2_2V ; 
-        obdcalcH16 = configPage2.stoich/10 ;            // configPage2.stoich(is *10 so 14.7 is 147)
-        obdcalcE32 = currentStatus.O2_2/10;            // afr(is *10 so 25.5 is 255) , needs a 32bit else will overflow
-        obdcalcF32 = (obdcalcE32<<8) / obdcalcH16;      //this is same as (obdcalcE32/256) / obdcalcH16 . this calculates the ratio      
-        obdcalcG16 = (obdcalcF32 *32768)>>8;          
+        obdcalcF32 = currentStatus.O2_2;            // afr(is *10 so 25.5 is 255) , needs a 32bit else will overflow
+        obdcalcG16 = udiv_32_16((obdcalcF32 * 32768u), configPage2.stoich);
         obdcalcA = highByte(obdcalcG16);
         obdcalcB = lowByte(obdcalcG16);       
 
@@ -703,10 +698,13 @@ void obd_response(uint8_t PIDmode, uint8_t requestedPIDlow, uint8_t requestedPID
       break;
 
       case 82:        //PID-0x52 Ethanol fuel % , range is 0 to 100% , formula == (100/255)A
+        //Ethanol sensor output can be above 100, add a check to avoid it
+        obdcalcA = (655u * currentStatus.ethanolPct) >> 8; //655 * 100 = 0xFFDC
+        if (obdcalcA > UINT8_MAX){ obdcalcA = UINT8_MAX;}
         outMsg.buf[0] =  0x03;                       // sending 3 byte
         outMsg.buf[1] =  0x41;                       // Same as query, except that 40h is added to the mode value. So:41h = show current data ,42h = freeze frame ,etc. 
         outMsg.buf[2] =  0x52;                       // pid code
-        outMsg.buf[3] =  currentStatus.ethanolPct;   // A
+        outMsg.buf[3] =  obdcalcA;                   // A
         outMsg.buf[4] =  0x00;
         outMsg.buf[5] =  0x00; 
         outMsg.buf[6] =  0x00;