Improvements to 5 channel support (Still not 100% right)

scheduler.h

@@ -63,7 +63,7 @@ struct Schedule {
   unsigned int endCompare;
 };
 
-Schedule *timer3Aqueue[4];
+volatile Schedule *timer3Aqueue[4];
 Schedule *timer3Bqueue[4];
 Schedule *timer3Cqueue[4];
 
@@ -86,27 +86,50 @@ Schedule ignitionSchedule8;
 
 Schedule nullSchedule; //This is placed at the end of the queue. It's status will always be set to OFF and hence will never perform any action within an ISR
 
-static inline unsigned int setQueue(Schedule *queue[], Schedule *schedule1, Schedule *schedule2, unsigned int CNT)
+static inline unsigned int setQueue(volatile Schedule *queue[], Schedule *schedule1, Schedule *schedule2, unsigned int CNT)
 {
   //Create an array of all the upcoming targets, relative to the current count on the timer
   unsigned int tmpQueue[4];
-  tmpQueue[0] = schedule1->startCompare - CNT;
-  tmpQueue[1] = schedule1->endCompare - CNT;
-  tmpQueue[2] = schedule2->startCompare - CNT;
-  tmpQueue[3] = schedule2->endCompare - CNT;
 
   //Set the initial queue state. This order matches the tmpQueue order
-  queue[0] = schedule1;
-  queue[1] = schedule1;
-  queue[2] = schedule2;
-  queue[3] = schedule2;
+  if(schedule1->Status == OFF)
+  {
+    queue[0] = schedule2;
+    queue[1] = schedule2;
+    tmpQueue[0] = schedule2->startCompare - CNT;
+    tmpQueue[1] = schedule2->endCompare - CNT;
+  }
+  else
+  {
+    queue[0] = schedule1;
+    queue[1] = schedule1;
+    tmpQueue[0] = schedule1->startCompare - CNT;
+    tmpQueue[1] = schedule1->endCompare - CNT;
+  }
+
+  if(schedule2->Status == OFF)
+  {
+    queue[2] = schedule1;
+    queue[3] = schedule1;
+    tmpQueue[2] = schedule1->startCompare - CNT;
+    tmpQueue[3] = schedule1->endCompare - CNT;   
+  }
+  else
+  {
+    queue[2] = schedule2;
+    queue[3] = schedule2;   
+    tmpQueue[2] = schedule2->startCompare - CNT;
+    tmpQueue[3] = schedule2->endCompare - CNT; 
+  }
+
 
   //Sort the queues. Both queues are kept in sync. 
-  //This implementes a sorting networking based on the Bose-Nelson swap algorithm
-  //See: 
+  //This implementes a sorting networking based on the Bose-Nelson sorting network
+  //See: http://pages.ripco.net/~jgamble/nw.html
   #define SWAP(x,y) if(tmpQueue[y] < tmpQueue[x]) { unsigned int tmp = tmpQueue[x]; tmpQueue[x] = tmpQueue[y]; tmpQueue[y] = tmp; Schedule *tmpS = queue[x]; queue[x] = queue[y]; queue[y] = tmpS; }
   //SWAP(0, 1); //Likely not needed
   //SWAP(2, 3); //Likely not needed
+  SWAP(0, 2);
   SWAP(1, 3);
   SWAP(1, 2);
 
@@ -119,7 +142,7 @@ static inline unsigned int setQueue(Schedule *queue[], Schedule *schedule1, Sche
  * The current item (0) is discarded
  * The final queue slot is set to nullSchedule to indicate that no action should be taken
  */
-static inline unsigned int popQueue(Schedule *queue[])
+static inline unsigned int popQueue(volatile Schedule *queue[])
 {
   queue[0] = queue[1];
   queue[1] = queue[2];

scheduler.ino

@@ -23,6 +23,8 @@ void initialiseSchedulers()
     fuelSchedule2.Status = OFF;
     fuelSchedule3.Status = OFF;
 
+    fuelSchedule5.Status = OFF;
+
     fuelSchedule1.schedulesSet = 0;
     fuelSchedule2.schedulesSet = 0;
     fuelSchedule3.schedulesSet = 0;

speeduino.ino

@@ -1199,7 +1199,7 @@ void loop()
           }
         }
 
-        //if(channel5InjEnabled)
+        if(channel5InjEnabled)
         {
           tempCrankAngle = crankAngle - channel5InjDegrees;
           if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX; }