Initial code for the closed loop algorithm

comms.ino

@@ -134,12 +134,12 @@ void sendValues(int length)
   response[12] = currentStatus.wueCorrection; //Warmup enrichment (%)
   response[13] = (byte)(div(currentStatus.RPM, 100).quot); //rpm / 100 
   response[14] = (byte)(currentStatus.PW / 200); //Pulsewidth 1 multiplied by 10 in ms. Have to convert from uS to mS. 
-  response[15] = currentStatus.TAEamount; //acceleration enrichment (ms)
+  response[15] = currentStatus.TAEamount; //acceleration enrichment (%)
   response[16] = 0x00; //Barometer correction (%)
   response[17] = currentStatus.corrections; //Total GammaE (%)
   response[18] = currentStatus.VE; //Current VE 1 (%)
-  response[19] = configPage1.tpsMin; //Pulsewidth 2 divided by 10 (in ms)
-  response[20] = configPage1.tpsMax; //Current VE 2 (%)
+  response[19] = 0x00; //Pulsewidth 2 (Unused currently)
+  response[20] = 0x00; //Current VE 2 (Unused Currently)
   response[21] = currentStatus.tpsDOT; //TPS DOT
   response[22] = currentStatus.advance;
   response[23] = currentStatus.TPS; // TPS (0% to 100%)

corrections.h

@@ -9,3 +9,4 @@ byte correctionWUE(); //Warmup enrichment
 byte correctionASE(); //After Start Enrichment
 byte correctionAccel(); //Acceleration Enrichment
 byte correctionsFloodClear(); //Check for flood clear on cranking
+byte correctionsAFRClosedLoop(); //Closed loop AFR adjustment

corrections.ino

@@ -18,14 +18,16 @@ byte correctionsTotal()
   byte result; //temporary variable to store the result of each corrections function
   
   //As the 'normal' case will be for each function to return 100, we only perform the division operation if the returned result is not equal to that
-  result = correctionWUE();
-  if (result != 100) { sumCorrections = div((sumCorrections * result), 100).quot; }
+  currentStatus.wueCorrection = correctionWUE();
+  if (currentStatus.wueCorrection != 100) { sumCorrections = div((sumCorrections * currentStatus.wueCorrection), 100).quot; }
   result = correctionASE();
   if (result != 100) { sumCorrections = div((sumCorrections * result), 100).quot; }
-  result = correctionAccel();
-  //if (result != 100) { sumCorrections = div((sumCorrections * result), 100).quot; }
+  currentStatus.TAEamount = correctionAccel();
+  //if (currentStatus.TAEamount != 100) { sumCorrections = div((sumCorrections * currentStatus.TAEamount), 100).quot; }
   result = correctionFloodClear();
   if (result != 100) { sumCorrections = div((sumCorrections * result), 100).quot; }
+  currentStatus.egoCorrection = correctionsAFRClosedLoop();
+  if (currentStatus.egoCorrection != 100) { sumCorrections = div((sumCorrections * currentStatus.egoCorrection), 100).quot; }
   
   return (byte)sumCorrections;
 }
@@ -90,9 +92,8 @@ byte correctionAccel()
   if (currentStatus.tpsDOT > (configPage1.tpsThresh * 10))
   {
     BIT_SET(currentStatus.engine, BIT_ENGINE_ACC); //Mark accleration enrichment as active.
-    currentStatus.TAEamount = table2D_getValue(taeTable, currentStatus.tpsDOT); //Lookup and store the amount of enrichment required
     currentStatus.TAEEndTime = micros() + (configPage1.taeTime * 100); //Set the time in the future where the enrichment will be turned off. taeTime is stored as mS * 10, so multiply it by 100 to get it in uS
-    return 100 + currentStatus.TAEamount;
+    return 100 + table2D_getValue(taeTable, currentStatus.tpsDOT);
   }
   
   //If we reach here then TAE is neither on, nor does it need to be turned on.
@@ -117,3 +118,38 @@ byte correctionFloodClear()
   }
   return 100;
 }
+
+/*
+Lookup the AFR target table and perform either a simple or PID adjustment based on this
+
+Simple (Best suited to narrowband sensors):
+If the O2 sensor reports that the mixture is lean/rich compared to the desired AFR target, it will make a 1% adjustment
+It then waits <egoDelta> number of ignition events and compares O2 against the target table again. If it is still lean/rich then the adjustment is increased to 2%
+This continues until either:
+  a) the O2 reading flips from lean to rich, at which point the adjustment cycle starts again at 1% or
+  b) the adjustment amount increases to <egoLimit> at which point it stays at this level until the O2 state (rich/lean) changes
+ 
+PID (Best suited to wideband sensors):
+ 
+*/
+byte correctionsAFRClosedLoop()
+{
+  if(configPage3.egoAlgorithm == 0) { return 100; } //An egoAlgorithm value of 0 means DISALBLED
+  
+  //Check the ignition count to see whether the next step is required
+  if( (ignitionCount & (configPage3.egoCount - 1)) == 1 ) //This is the equivalent of ( (ignitionCount % configPage3.egoCount) == 0 ) but without the expensive modulus operation. ie It results in True every <egoCount> ignition loops. Note that it only works for power of two vlaues for egoCount
+  {
+    //Check all other requirements for closed loop adjustments
+    if( (currentStatus.coolant > configPage3.egoTemp) && (currentStatus.RPM > (unsigned int)(configPage3.egoRPM * 100)) && (currentStatus.TPS < configPage3.egoTPSMax) && (currentStatus.O2 < configPage3.ego_max) && (currentStatus.O2 > configPage3.ego_min) && (currentStatus.runSecs > configPage3.ego_sdelay) )
+    {
+      //Determine whether the Y axis of the AFR target table tshould be MAP (Speed-Density) or TPS (Alpha-N)
+      byte yValue;
+      if (configPage1.algorithm == 0) { yValue = currentStatus.MAP; }
+      else  { yValue = currentStatus.TPS; }
+      
+      byte afrTarget = get3DTableValue(afrTable, yValue, currentStatus.RPM); //Perform the target lookup
+      
+      
+    }
+  }
+}

scheduler.ino

@@ -263,6 +263,7 @@ ISR(TIMER5_COMPA_vect) //ignitionSchedule1
     {
       ignitionSchedule1.Status = OFF; //Turn off the schedule
       ignitionSchedule1.EndCallback();
+      ignitionCount += 1; //Increment the igintion counter
       TIMSK5 &= ~(1 << OCIE5A); //Turn off this output compare unit (This simply writes 0 to the OCIE3A bit of TIMSK3)
     }
   }
@@ -280,6 +281,7 @@ ISR(TIMER5_COMPB_vect) //ignitionSchedule2
     {
       ignitionSchedule2.Status = OFF; //Turn off the schedule
       ignitionSchedule2.EndCallback();
+      ignitionCount += 1; //Increment the igintion counter
       TIMSK5 &= ~(1 << OCIE5B); //Turn off this output compare unit (This simply writes 0 to the OCIE3A bit of TIMSK3)
     }
   }
@@ -296,8 +298,9 @@ ISR(TIMER5_COMPC_vect) //ignitionSchedule3
     }
     else if (ignitionSchedule3.Status == RUNNING)
     {
-       ignitionSchedule3.EndCallback();
        ignitionSchedule3.Status = OFF; //Turn off the schedule
+       ignitionSchedule3.EndCallback();
+       ignitionCount += 1; //Increment the igintion counter
        TIMSK5 &= ~(1 << OCIE5C); //Turn off this output compare unit (This simply writes 0 to the OCIE3A bit of TIMSK3)
     }
     interrupts();
@@ -315,8 +318,9 @@ ISR(TIMER4_COMPA_vect) //ignitionSchedule4
     }
     else if (ignitionSchedule4.Status == RUNNING)
     {
-       ignitionSchedule4.EndCallback();
        ignitionSchedule4.Status = OFF; //Turn off the schedule
+       ignitionSchedule4.EndCallback();
+       ignitionCount += 1; //Increment the igintion counter
        TIMSK5 &= ~(1 << OCIE4A); //Turn off this output compare unit (This simply writes 0 to the OCIE3A bit of TIMSK3)
     }
     interrupts();

speeduino.ino

@@ -62,6 +62,7 @@ byte coilLOW = LOW;
 
 struct statuses currentStatus;
 volatile int mainLoopCount;
+byte ignitionCount;
 unsigned long secCounter; //The next time to increment 'runSecs' counter.
 int channel1Degrees; //The number of crank degrees until cylinder 1 is at TDC (This is obviously 0 for virtually ALL engines, but there's some weird ones)
 int channel2Degrees; //The number of crank degrees until cylinder 2 (and 5/6/7/8) is at TDC
@@ -183,6 +184,7 @@ void setup()
   #endif
   
   mainLoopCount = 0;
+  ignitionCount = 0;
   
   //Setup other relevant pins
   pinMode(pinMAP, INPUT);