New secondary scheduling method for fuel schedules

speeduino/globals.h

@@ -158,6 +158,16 @@ volatile byte triggerPri_pin_mask;
 volatile byte *triggerSec_pin_port;
 volatile byte triggerSec_pin_mask;
 
+//These need to be here as they are used in both speeduino.ino and scheduler.ino
+bool channel1InjEnabled = true;
+bool channel2InjEnabled = false;
+bool channel3InjEnabled = false;
+bool channel4InjEnabled = false;
+bool channel5InjEnabled = false;
+
+//This is used across multiple files
+unsigned long revolutionTime; //The time in uS that one revolution would take at current speed (The time tooth 1 was last seen, minus the time it was seen prior to that)
+
 //The status struct contains the current values for all 'live' variables
 //In current version this is 64 bytes
 struct statuses {

speeduino/scheduledIO.h

@@ -29,8 +29,8 @@ inline void endCoil5Charge();
 
 #define openInjector1and4() openInjector1(); openInjector4()
 #define closeInjector1and4() closeInjector1(); closeInjector4()
-#define openInjector2and3() openInjector2(); openInjector2()
+#define openInjector2and3() openInjector2(); openInjector3()
-#define closeInjector2and3() closeInjector2(); closeInjector2()
+#define closeInjector2and3() closeInjector2(); closeInjector3()
 
 //5 cylinder support doubles up injector 3 as being closese to inj 5 (Crank angle)
 #define openInjector3and5() openInjector3(); openInjector5()

speeduino/scheduler.h

@@ -35,6 +35,7 @@ See page 136 of the processors datasheet: http://www.atmel.com/Images/doc2549.pd
   #define FUEL2_COUNTER TCNT3
   #define FUEL3_COUNTER TCNT3
   #define FUEL4_COUNTER TCNT4
+  #define FUEL5_COUNTER TCNT3
 
   #define IGN1_COUNTER  TCNT5
   #define IGN2_COUNTER  TCNT5
@@ -46,6 +47,7 @@ See page 136 of the processors datasheet: http://www.atmel.com/Images/doc2549.pd
   #define FUEL2_COMPARE OCR3B
   #define FUEL3_COMPARE OCR3C
   #define FUEL4_COMPARE OCR4B
+  #define FUEL5_COMPARE OCR3A //Shared with FUEL1
 
   #define IGN1_COMPARE  OCR5A
   #define IGN2_COMPARE  OCR5B
@@ -255,8 +257,12 @@ struct Schedule {
   void (*StartCallback)(); //Start Callback function for schedule
   void (*EndCallback)(); //Start Callback function for schedule
   volatile unsigned long startTime; //The system time (in uS) that the schedule started
-  unsigned int startCompare; //The counter value of the timer when this will start
+  volatile unsigned int startCompare; //The counter value of the timer when this will start
-  unsigned int endCompare;
+  volatile unsigned int endCompare;
+
+  unsigned int nextStartCompare;
+  unsigned int nextEndCompare;
+  volatile bool hasNextSchedule = false;
 };
 
 volatile Schedule *timer3Aqueue[4];

speeduino/scheduler.ino

@@ -87,9 +87,10 @@ void initialiseSchedulers()
   FTM1_SC |= FTM_SC_PS(0b111);
 
   //Setup the channels (See Pg 1014 of K64 DS).
-  //FTM0_C0SC &= ~FTM_CSC_ELSB; //Probably not needed as power on state should be 0
+  //The are probably not needed as power on state should be 0
-  //FTM0_C0SC &= ~FTM_CSC_ELSA; //Probably not needed as power on state should be 0
+  //FTM0_C0SC &= ~FTM_CSC_ELSB;
-  //FTM0_C0SC &= ~FTM_CSC_DMA; //Probably not needed as power on state should be 0
+  //FTM0_C0SC &= ~FTM_CSC_ELSA;
+  //FTM0_C0SC &= ~FTM_CSC_DMA;
   FTM0_C0SC &= ~FTM_CSC_MSB; //According to Pg 965 of the K64 datasheet, this should not be needed as MSB is reset to 0 upon reset, but the channel interrupt fails to fire without it
   FTM0_C0SC |= FTM_CSC_MSA; //Enable Compare mode
   FTM0_C0SC |= FTM_CSC_CHIE; //Enable channel compare interrupt
@@ -162,7 +163,8 @@ void initialiseSchedulers()
 #elif defined(CORE_STM32)
   //see https://github.com/rogerclarkmelbourne/Arduino_STM32/blob/754bc2969921f1ef262bd69e7faca80b19db7524/STM32F1/system/libmaple/include/libmaple/timer.h#L444
   (TIMER1->regs).bas->PSC = (TIMER2->regs).bas->PSC = (TIMER3->regs).bas->PSC = (CYCLES_PER_MICROSECOND << 1) - 1;  //2us resolution
-  //TimerX.setPrescaleFactor(CYCLES_PER_MICROSECOND * 2U); //2us resolution
+  // Alternative 2us resolution:
+  //TimerX.setPrescaleFactor(CYCLES_PER_MICROSECOND * 2U);
 
   Timer2.setMode(TIMER_CH1, TIMER_OUTPUT_COMPARE);
   Timer2.setMode(TIMER_CH2, TIMER_OUTPUT_COMPARE);
@@ -184,9 +186,6 @@ void initialiseSchedulers()
   Timer3.attachInterrupt(3, ignitionSchedule3Interrupt);
   Timer3.attachInterrupt(4, ignitionSchedule4Interrupt);
 
-  //(TIMER2->regs).gen->CCMR1 &= ~TIM_CCMR1_OC1M; //Select channel 1 output Compare and Mode
-  //TIM3->CR1 |= TIM_CR1_CEN
-
 #endif
 
     fuelSchedule1.Status = OFF;
@@ -226,68 +225,76 @@ endCallback: This function is called once the duration time has been reached
 */
 //void setFuelSchedule1(void (*startCallback)(), unsigned long timeout, unsigned long duration, void(*endCallback)())
 void setFuelSchedule1(unsigned long timeout, unsigned long duration)
+{
+  if(fuelSchedule1.Status != RUNNING) //Check that we're not already part way through a schedule
   {
-    if(fuelSchedule1.Status == RUNNING) { return; } //Check that we're not already part way through a schedule
+    //Callbacks no longer used, but retained for now:
-
+    //fuelSchedule1.StartCallback = startCallback;
-    //fuelSchedule1.StartCallback = startCallback; //Name the start callback function
+    //fuelSchedule1.EndCallback = endCallback;
-    //fuelSchedule1.EndCallback = endCallback; //Name the end callback function
     fuelSchedule1.duration = duration;
 
-    /*
+    //The following must be enclosed in the noInterupts block to avoid contention caused if the relevant interrupt fires before the state is fully set
-     * The following must be enclosed in the noInterupts block to avoid contention caused if the relevant interrupts fires before the state is fully set
+    noInterrupts();
-     * We need to calculate the value to reset the timer to (preload) in order to achieve the desired overflow time
+    fuelSchedule1.startCompare = FUEL1_COUNTER + uS_TO_TIMER_COMPARE_SLOW(timeout);
-     * As the timer is ticking every 16uS (Time per Tick = (Prescale)*(1/Frequency))
+    fuelSchedule1.endCompare = fuelSchedule1.startCompare + uS_TO_TIMER_COMPARE_SLOW(duration);
-     * unsigned int absoluteTimeout = TCNT3 + (timeout / 16); //Each tick occurs every 16uS with the 256 prescaler, so divide the timeout by 16 to get ther required number of ticks. Add this to the current tick count to get the target time. This will automatically overflow as required
+    fuelSchedule1.Status = PENDING; //Turn this schedule on
-     */
+    fuelSchedule1.schedulesSet++; //Increment the number of times this schedule has been set
-     noInterrupts();
+    //Schedule 1 shares a timer with schedule 5
-     fuelSchedule1.startCompare = FUEL1_COUNTER + uS_TO_TIMER_COMPARE_SLOW(timeout);
+    //if(channel5InjEnabled) { FUEL1_COMPARE = setQueue(timer3Aqueue, &fuelSchedule1, &fuelSchedule5, FUEL1_COUNTER); }
-     fuelSchedule1.endCompare = fuelSchedule1.startCompare + uS_TO_TIMER_COMPARE_SLOW(duration);
+    //else { timer3Aqueue[0] = &fuelSchedule1; timer3Aqueue[1] = &fuelSchedule1; timer3Aqueue[2] = &fuelSchedule1; timer3Aqueue[3] = &fuelSchedule1; FUEL1_COMPARE = fuelSchedule1.startCompare; }
-     fuelSchedule1.Status = PENDING; //Turn this schedule on
+    //timer3Aqueue[0] = &fuelSchedule1; timer3Aqueue[1] = &fuelSchedule1; timer3Aqueue[2] = &fuelSchedule1; timer3Aqueue[3] = &fuelSchedule1;
-     fuelSchedule1.schedulesSet++; //Increment the number of times this schedule has been set
+    FUEL1_COMPARE = fuelSchedule1.startCompare;
-     /*if(channel5InjEnabled) { FUEL1_COMPARE = setQueue(timer3Aqueue, &fuelSchedule1, &fuelSchedule5, FUEL1_COUNTER); } //Schedule 1 shares a timer with schedule 5
+    interrupts();
-     else { timer3Aqueue[0] = &fuelSchedule1; timer3Aqueue[1] = &fuelSchedule1; timer3Aqueue[2] = &fuelSchedule1; timer3Aqueue[3] = &fuelSchedule1; FUEL1_COMPARE = fuelSchedule1.startCompare; }*/
+    FUEL1_TIMER_ENABLE();
-     timer3Aqueue[0] = &fuelSchedule1; timer3Aqueue[1] = &fuelSchedule1; timer3Aqueue[2] = &fuelSchedule1; timer3Aqueue[3] = &fuelSchedule1; FUEL1_COMPARE = fuelSchedule1.startCompare;
-     interrupts();
-     FUEL1_TIMER_ENABLE();
   }
-void setFuelSchedule2(unsigned long timeout, unsigned long duration)
+  else
   {
-    if(fuelSchedule2.Status == RUNNING) { return; } //Check that we're not already part way through a schedule
+    //If the schedule is already running, we can set the next schedule so it is ready to go
-
+    //This is required in cases of high rpm and high DC where there otherwise would not be enough time to set the schedule
-    //fuelSchedule2.StartCallback = startCallback; //Name the start callback function
+    fuelSchedule1.nextStartCompare = FUEL1_COUNTER + uS_TO_TIMER_COMPARE_SLOW(timeout);
-    //fuelSchedule2.EndCallback = endCallback; //Name the end callback function
+    fuelSchedule1.nextEndCompare = fuelSchedule1.nextStartCompare + uS_TO_TIMER_COMPARE_SLOW(duration);
+    fuelSchedule1.hasNextSchedule = true;
+  }
+}
+void setFuelSchedule2(unsigned long timeout, unsigned long duration)
+{
+  if(fuelSchedule2.Status != RUNNING) //Check that we're not already part way through a schedule
+  {
+    //Callbacks no longer used, but retained for now:
+    //fuelSchedule2.StartCallback = startCallback;
+    //fuelSchedule2.EndCallback = endCallback;
     fuelSchedule2.duration = duration;
 
-    /*
+    //The following must be enclosed in the noInterupts block to avoid contention caused if the relevant interrupt fires before the state is fully set
-     * The following must be enclosed in the noIntterupts block to avoid contention caused if the relevant interrupts fires before the state is fully set
+    noInterrupts();
-     * We need to calculate the value to reset the timer to (preload) in order to achieve the desired overflow time
+    fuelSchedule2.startCompare = FUEL2_COUNTER + uS_TO_TIMER_COMPARE_SLOW(timeout);
-     * As the timer is ticking every 16uS (Time per Tick = (Prescale)*(1/Frequency))
+    fuelSchedule2.endCompare = fuelSchedule2.startCompare + uS_TO_TIMER_COMPARE_SLOW(duration);
-     * unsigned int absoluteTimeout = TCNT3 + (timeout / 16); //Each tick occurs every 16uS with the 256 prescaler, so divide the timeout by 16 to get ther required number of ticks. Add this to the current tick count to get the target time. This will automatically overflow as required
+    FUEL2_COMPARE = fuelSchedule2.startCompare; //Use the B compare unit of timer 3
-     */
+    fuelSchedule2.Status = PENDING; //Turn this schedule on
-     noInterrupts();
+    fuelSchedule2.schedulesSet++; //Increment the number of times this schedule has been set
-     fuelSchedule2.startCompare = FUEL2_COUNTER + uS_TO_TIMER_COMPARE_SLOW(timeout);
+    interrupts();
-     fuelSchedule2.endCompare = fuelSchedule2.startCompare + uS_TO_TIMER_COMPARE_SLOW(duration);
+    FUEL2_TIMER_ENABLE();
-     FUEL2_COMPARE = fuelSchedule2.startCompare; //Use the B copmare unit of timer 3
-     fuelSchedule2.Status = PENDING; //Turn this schedule on
-     fuelSchedule2.schedulesSet++; //Increment the number of times this schedule has been set
-     interrupts();
-     FUEL2_TIMER_ENABLE();
   }
+  else
+  {
+    //If the schedule is already running, we can set the next schedule so it is ready to go
+    //This is required in cases of high rpm and high DC where there otherwise would not be enough time to set the schedule
+    fuelSchedule2.nextStartCompare = FUEL2_COUNTER + uS_TO_TIMER_COMPARE_SLOW(timeout);
+    fuelSchedule2.nextEndCompare = fuelSchedule2.nextStartCompare + uS_TO_TIMER_COMPARE_SLOW(duration);
+    fuelSchedule2.hasNextSchedule = true;
+  }
+}
 //void setFuelSchedule3(void (*startCallback)(), unsigned long timeout, unsigned long duration, void(*endCallback)())
 void setFuelSchedule3(unsigned long timeout, unsigned long duration)
+{
+  if(fuelSchedule3.Status != RUNNING)//Check that we're not already part way through a schedule
   {
-    if(fuelSchedule3.Status == RUNNING) { return; } //Check that we're not already part way through a schedule
+    //Callbacks no longer used, but retained for now:
-
+    //fuelSchedule3.StartCallback = startCallback;
-    //fuelSchedule3.StartCallback = startCallback; //Name the start callback function
+    //fuelSchedule3.EndCallback = endCallback;
-    //fuelSchedule3.EndCallback = endCallback; //Name the end callback function
     fuelSchedule3.duration = duration;
 
-    /*
+    //The following must be enclosed in the noInterupts block to avoid contention caused if the relevant interrupt fires before the state is fully set
-     * The following must be enclosed in the noIntterupts block to avoid contention caused if the relevant interrupts fires before the state is fully set
-     * We need to calculate the value to reset the timer to (preload) in order to achieve the desired overflow time
-     * As the timer is ticking every 16uS (Time per Tick = (Prescale)*(1/Frequency))
-     * unsigned int absoluteTimeout = TCNT3 + (timeout / 16); //Each tick occurs every 16uS with the 256 prescaler, so divide the timeout by 16 to get ther required number of ticks. Add this to the current tick count to get the target time. This will automatically overflow as required
-     */
     noInterrupts();
     fuelSchedule3.startCompare = FUEL3_COUNTER + uS_TO_TIMER_COMPARE_SLOW(timeout);
     fuelSchedule3.endCompare = fuelSchedule3.startCompare + uS_TO_TIMER_COMPARE_SLOW(duration);
@@ -297,21 +304,26 @@ void setFuelSchedule3(unsigned long timeout, unsigned long duration)
     interrupts();
     FUEL3_TIMER_ENABLE();
   }
-//void setFuelSchedule4(void (*startCallback)(), unsigned long timeout, unsigned long duration, void(*endCallback)()) //Uses timer 4 compare B
+  else
-void setFuelSchedule4(unsigned long timeout, unsigned long duration) //Uses timer 4 compare B
   {
-    if(fuelSchedule4.Status == RUNNING) { return; } //Check that we're not already part way through a schedule
+    //If the schedule is already running, we can set the next schedule so it is ready to go
-
+    //This is required in cases of high rpm and high DC where there otherwise would not be enough time to set the schedule
-    //fuelSchedule4.StartCallback = startCallback; //Name the start callback function
+    fuelSchedule3.nextStartCompare = FUEL3_COUNTER + uS_TO_TIMER_COMPARE_SLOW(timeout);
-    //fuelSchedule4.EndCallback = endCallback; //Name the end callback function
+    fuelSchedule3.nextEndCompare = fuelSchedule3.nextStartCompare + uS_TO_TIMER_COMPARE_SLOW(duration);
+    fuelSchedule3.hasNextSchedule = true;
+  }
+}
+//void setFuelSchedule4(void (*startCallback)(), unsigned long timeout, unsigned long duration, void(*endCallback)())
+void setFuelSchedule4(unsigned long timeout, unsigned long duration) //Uses timer 4 compare B
+{
+  if(fuelSchedule4.Status != RUNNING) //Check that we're not already part way through a schedule
+  {
+    //Callbacks no longer used, but retained for now:
+    //fuelSchedule4.StartCallback = startCallback;
+    //fuelSchedule4.EndCallback = endCallback;
     fuelSchedule4.duration = duration;
 
-    /*
+    //The following must be enclosed in the noInterupts block to avoid contention caused if the relevant interrupt fires before the state is fully set
-     * The following must be enclosed in the noIntterupts block to avoid contention caused if the relevant interrupts fires before the state is fully set
-     * We need to calculate the value to reset the timer to (preload) in order to achieve the desired overflow time
-     * As the timer is ticking every 16uS (Time per Tick = (Prescale)*(1/Frequency))
-     * unsigned int absoluteTimeout = TCNT3 + (timeout / 16); //Each tick occurs every 16uS with the 256 prescaler, so divide the timeout by 16 to get ther required number of ticks. Add this to the current tick count to get the target time. This will automatically overflow as required
-     */
     noInterrupts();
     fuelSchedule4.startCompare = FUEL4_COUNTER + uS_TO_TIMER_COMPARE_SLOW(timeout);
     fuelSchedule4.endCompare = fuelSchedule4.startCompare + uS_TO_TIMER_COMPARE_SLOW(duration);
@@ -321,45 +333,62 @@ void setFuelSchedule4(unsigned long timeout, unsigned long duration) //Uses time
     interrupts();
     FUEL4_TIMER_ENABLE();
   }
+  else
+  {
+    //If the schedule is already running, we can set the next schedule so it is ready to go
+    //This is required in cases of high rpm and high DC where there otherwise would not be enough time to set the schedule
+    fuelSchedule4.nextStartCompare = FUEL4_COUNTER + uS_TO_TIMER_COMPARE_SLOW(timeout);
+    fuelSchedule4.nextEndCompare = fuelSchedule4.nextStartCompare + uS_TO_TIMER_COMPARE_SLOW(duration);
+    fuelSchedule4.hasNextSchedule = true;
+  }
+}
 void setFuelSchedule5(void (*startCallback)(), unsigned long timeout, unsigned long duration, void(*endCallback)())
   {
-    if(fuelSchedule5.Status == RUNNING) { return; } //Check that we're not already part way through a schedule
+    if(fuelSchedule5.Status != RUNNING) //Check that we're not already part way through a schedule
+    {
+      fuelSchedule5.StartCallback = startCallback; //Name the start callback function
+      fuelSchedule5.EndCallback = endCallback; //Name the end callback function
+      fuelSchedule5.duration = duration;
 
-    //We need to calculate the value to reset the timer to (preload) in order to achieve the desired overflow time
+      /*
-    //As the timer is ticking every 16uS (Time per Tick = (Prescale)*(1/Frequency))
+       * The following must be enclosed in the noIntterupts block to avoid contention caused if the relevant interrupts fires before the state is fully set
-    //unsigned int absoluteTimeout = TCNT3 + (timeout / 16); //Each tick occurs every 16uS with the 256 prescaler, so divide the timeout by 16 to get ther required number of ticks. Add this to the current tick count to get the target time. This will automatically overflow as required
+       */
-    fuelSchedule5.StartCallback = startCallback; //Name the start callback function
+  #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega2561__)
-    fuelSchedule5.EndCallback = endCallback; //Name the end callback function
+      noInterrupts();
-    fuelSchedule5.duration = duration;
+      fuelSchedule5.startCompare = TCNT3 + (timeout >> 4); //As above, but with bit shift instead of / 16
-
+      fuelSchedule5.endCompare = fuelSchedule5.startCompare + (duration >> 4);
-    /*
+      fuelSchedule5.Status = PENDING; //Turn this schedule on
-     * The following must be enclosed in the noIntterupts block to avoid contention caused if the relevant interrupts fires before the state is fully set
+      fuelSchedule5.schedulesSet++; //Increment the number of times this schedule has been set
-     */
+      OCR3A = setQueue(timer3Aqueue, &fuelSchedule1, &fuelSchedule5, TCNT3); //Schedule 1 shares a timer with schedule 5
-#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega2561__)
+      interrupts();
-    noInterrupts();
+      TIMSK3 |= (1 << OCIE3A); //Turn on the A compare unit (ie turn on the interrupt)
-    fuelSchedule5.startCompare = TCNT3 + (timeout >> 4); //As above, but with bit shift instead of / 16
+  #endif
-    fuelSchedule5.endCompare = fuelSchedule5.startCompare + (duration >> 4);
+    }
-    fuelSchedule5.Status = PENDING; //Turn this schedule on
+    else
-    fuelSchedule5.schedulesSet++; //Increment the number of times this schedule has been set
+    {
-    OCR3A = setQueue(timer3Aqueue, &fuelSchedule1, &fuelSchedule5, TCNT3); //Schedule 1 shares a timer with schedule 5
+      //If the schedule is already running, we can set the next schedule so it is ready to go
-    interrupts();
+      //This is required in cases of high rpm and high DC where there otherwise would not be enough time to set the schedule
-    TIMSK3 |= (1 << OCIE3A); //Turn on the A compare unit (ie turn on the interrupt)
+      fuelSchedule5.nextStartCompare = FUEL5_COUNTER + uS_TO_TIMER_COMPARE_SLOW(timeout);
-#endif
+      fuelSchedule5.nextEndCompare = fuelSchedule5.nextStartCompare + uS_TO_TIMER_COMPARE_SLOW(duration);
+      fuelSchedule5.hasNextSchedule = true;
+    }
   }
 //Ignition schedulers use Timer 5
 void setIgnitionSchedule1(void (*startCallback)(), unsigned long timeout, unsigned long duration, void(*endCallback)())
+{
+  if(ignitionSchedule1.Status != RUNNING) //Check that we're not already part way through a schedule
   {
-    if(ignitionSchedule1.Status == RUNNING) { return; } //Check that we're not already part way through a schedule
-
     ignitionSchedule1.StartCallback = startCallback; //Name the start callback function
     ignitionSchedule1.EndCallback = endCallback; //Name the start callback function
     ignitionSchedule1.duration = duration;
 
-    //As the timer is ticking every 4uS (Time per Tick = (Prescale)*(1/Frequency))
+    //Need to check that the timeout doesn't exceed the overflow
-    if (timeout > MAX_TIMER_PERIOD) { timeout = MAX_TIMER_PERIOD - 1; } // If the timeout is >4x (Each tick represents 4uS) the maximum allowed value of unsigned int (65535), the timer compare value will overflow when appliedcausing erratic behaviour such as erroneous sparking.
+    uint16_t timeout_timer_compare;
+    if (timeout > MAX_TIMER_PERIOD) { timeout_timer_compare = uS_TO_TIMER_COMPARE( (MAX_TIMER_PERIOD - 1) ); } // If the timeout is >4x (Each tick represents 4uS) the maximum allowed value of unsigned int (65535), the timer compare value will overflow when appliedcausing erratic behaviour such as erroneous sparking.
+    else { timeout_timer_compare = uS_TO_TIMER_COMPARE(timeout); } //Normal case
 
     noInterrupts();
-    ignitionSchedule1.startCompare = IGN1_COUNTER + uS_TO_TIMER_COMPARE(timeout); //As there is a tick every 4uS, there are timeout/4 ticks until the interrupt should be triggered ( >>2 divides by 4)
+    ignitionSchedule1.startCompare = IGN1_COUNTER + timeout_timer_compare; //As there is a tick every 4uS, there are timeout/4 ticks until the interrupt should be triggered ( >>2 divides by 4)
     ignitionSchedule1.endCompare = ignitionSchedule1.startCompare + uS_TO_TIMER_COMPARE(duration);
     IGN1_COMPARE = ignitionSchedule1.startCompare;
     ignitionSchedule1.Status = PENDING; //Turn this schedule on
@@ -367,10 +396,11 @@ void setIgnitionSchedule1(void (*startCallback)(), unsigned long timeout, unsign
     interrupts();
     IGN1_TIMER_ENABLE();
   }
+}
 void setIgnitionSchedule2(void (*startCallback)(), unsigned long timeout, unsigned long duration, void(*endCallback)())
+{
+  if(ignitionSchedule2.Status != RUNNING) //Check that we're not already part way through a schedule
   {
-    if(ignitionSchedule2.Status == RUNNING) { return; } //Check that we're not already part way through a schedule
-
     ignitionSchedule2.StartCallback = startCallback; //Name the start callback function
     ignitionSchedule2.EndCallback = endCallback; //Name the start callback function
     ignitionSchedule2.duration = duration;
@@ -387,9 +417,11 @@ void setIgnitionSchedule2(void (*startCallback)(), unsigned long timeout, unsign
     interrupts();
     IGN2_TIMER_ENABLE();
   }
+}
 void setIgnitionSchedule3(void (*startCallback)(), unsigned long timeout, unsigned long duration, void(*endCallback)())
+{
+  if(ignitionSchedule3.Status != RUNNING) //Check that we're not already part way through a schedule
   {
-    if(ignitionSchedule3.Status == RUNNING) { return; } //Check that we're not already part way through a schedule
 
     ignitionSchedule3.StartCallback = startCallback; //Name the start callback function
     ignitionSchedule3.EndCallback = endCallback; //Name the start callback function
@@ -407,9 +439,11 @@ void setIgnitionSchedule3(void (*startCallback)(), unsigned long timeout, unsign
     interrupts();
     IGN3_TIMER_ENABLE();
   }
+}
 void setIgnitionSchedule4(void (*startCallback)(), unsigned long timeout, unsigned long duration, void(*endCallback)())
+{
+  if(ignitionSchedule4.Status != RUNNING) //Check that we're not already part way through a schedule
   {
-    if(ignitionSchedule4.Status == RUNNING) { return; } //Check that we're not already part way through a schedule
 
     ignitionSchedule4.StartCallback = startCallback; //Name the start callback function
     ignitionSchedule4.EndCallback = endCallback; //Name the start callback function
@@ -429,9 +463,11 @@ void setIgnitionSchedule4(void (*startCallback)(), unsigned long timeout, unsign
     interrupts();
     IGN4_TIMER_ENABLE();
   }
+}
 void setIgnitionSchedule5(void (*startCallback)(), unsigned long timeout, unsigned long duration, void(*endCallback)())
+{
+  if(ignitionSchedule5.Status != RUNNING)//Check that we're not already part way through a schedule
   {
-    if(ignitionSchedule5.Status == RUNNING) { return; } //Check that we're not already part way through a schedule
 
     ignitionSchedule5.StartCallback = startCallback; //Name the start callback function
     ignitionSchedule5.EndCallback = endCallback; //Name the start callback function
@@ -451,6 +487,7 @@ void setIgnitionSchedule5(void (*startCallback)(), unsigned long timeout, unsign
     interrupts();
     IGN5_TIMER_ENABLE();
   }
+}
 
 /*******************************************************************************************************************************************************************************************************/
 //This function (All 8 ISR functions that are below) gets called when either the start time or the duration time are reached
@@ -463,24 +500,35 @@ ISR(TIMER3_COMPA_vect, ISR_NOBLOCK) //fuelSchedules 1 and 5
 static inline void fuelSchedule1Interrupt() //Most ARM chips can simply call a function
 #endif
   {
-    if (timer3Aqueue[0]->Status == OFF) { FUEL1_TIMER_DISABLE(); return; } //Safety check. Turn off this output compare unit and return without performing any action
+    if (fuelSchedule1.Status == PENDING) //Check to see if this schedule is turn on
-    if (timer3Aqueue[0]->Status == PENDING) //Check to see if this schedule is turn on
     {
-      //timer3Aqueue[0]->StartCallback();
+      //To use timer queue, change fuelShedule1 to timer3Aqueue[0];
       if (configPage1.injLayout == INJ_SEMISEQUENTIAL) { openInjector1and4(); }
       else { openInjector1(); }
-      timer3Aqueue[0]->Status = RUNNING; //Set the status to be in progress (ie The start callback has been called, but not the end callback)
+      fuelSchedule1.Status = RUNNING; //Set the status to be in progress (ie The start callback has been called, but not the end callback)
-      FUEL1_COMPARE = popQueue(timer3Aqueue);
+      FUEL1_COMPARE = fuelSchedule1.endCompare;
     }
-    else if (timer3Aqueue[0]->Status == RUNNING)
+    else if (fuelSchedule1.Status == RUNNING)
     {
        //timer3Aqueue[0]->EndCallback();
        if (configPage1.injLayout == INJ_SEMISEQUENTIAL) { closeInjector1and4(); }
        else { closeInjector1(); }
-       timer3Aqueue[0]->Status = OFF; //Turn off the schedule
+       fuelSchedule1.Status = OFF; //Turn off the schedule
-       timer3Aqueue[0]->schedulesSet = 0;
+       fuelSchedule1.schedulesSet = 0;
-       FUEL1_COMPARE = popQueue(timer3Aqueue);
+       //FUEL1_COMPARE = fuelSchedule1.endCompare;
+
+       //If there is a next schedule queued up, activate it
+       if(fuelSchedule1.hasNextSchedule)
+       {
+         FUEL1_COMPARE = fuelSchedule1.nextStartCompare;
+         fuelSchedule1.endCompare = fuelSchedule1.nextEndCompare;
+         fuelSchedule1.Status = PENDING;
+         fuelSchedule1.schedulesSet = 1;
+         fuelSchedule1.hasNextSchedule = false;
+       }
+       else { FUEL1_TIMER_DISABLE(); }
     }
+    else if (fuelSchedule1.Status == OFF) { FUEL1_TIMER_DISABLE(); } //Safety check. Turn off this output compare unit and return without performing any action
   }
 
 #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega2561__) //AVR chips use the ISR for this
@@ -504,7 +552,17 @@ static inline void fuelSchedule2Interrupt() //Most ARM chips can simply call a f
        else { closeInjector2(); }
        fuelSchedule2.Status = OFF; //Turn off the schedule
        fuelSchedule2.schedulesSet = 0;
-       FUEL2_TIMER_DISABLE();
+
+       //If there is a next schedule queued up, activate it
+       if(fuelSchedule2.hasNextSchedule)
+       {
+         FUEL2_COMPARE = fuelSchedule2.nextStartCompare;
+         fuelSchedule2.endCompare = fuelSchedule2.nextEndCompare;
+         fuelSchedule2.Status = PENDING;
+         fuelSchedule2.schedulesSet = 1;
+         fuelSchedule2.hasNextSchedule = false;
+       }
+       else { FUEL2_TIMER_DISABLE(); }
     }
   }
 
@@ -531,7 +589,17 @@ static inline void fuelSchedule3Interrupt() //Most ARM chips can simply call a f
        else { closeInjector3and5(); }
        fuelSchedule3.Status = OFF; //Turn off the schedule
        fuelSchedule3.schedulesSet = 0;
-       FUEL3_TIMER_DISABLE();
+
+       //If there is a next schedule queued up, activate it
+       if(fuelSchedule3.hasNextSchedule)
+       {
+         FUEL3_COMPARE = fuelSchedule3.nextStartCompare;
+         fuelSchedule3.endCompare = fuelSchedule3.nextEndCompare;
+         fuelSchedule3.Status = PENDING;
+         fuelSchedule3.schedulesSet = 1;
+         fuelSchedule3.hasNextSchedule = false;
+       }
+       else { FUEL3_TIMER_DISABLE(); }
     }
   }
 
@@ -554,7 +622,17 @@ static inline void fuelSchedule4Interrupt() //Most ARM chips can simply call a f
        closeInjector4();
        fuelSchedule4.Status = OFF; //Turn off the schedule
        fuelSchedule4.schedulesSet = 0;
-       FUEL4_TIMER_DISABLE();
+
+       //If there is a next schedule queued up, activate it
+       if(fuelSchedule4.hasNextSchedule)
+       {
+         FUEL4_COMPARE = fuelSchedule4.nextStartCompare;
+         fuelSchedule4.endCompare = fuelSchedule4.nextEndCompare;
+         fuelSchedule4.Status = PENDING;
+         fuelSchedule4.schedulesSet = 1;
+         fuelSchedule4.hasNextSchedule = false;
+       }
+       else { FUEL4_TIMER_DISABLE(); }
     }
   }
 
@@ -570,7 +648,7 @@ static inline void ignitionSchedule1Interrupt() //Most ARM chips can simply call
       ignitionSchedule1.Status = RUNNING; //Set the status to be in progress (ie The start callback has been called, but not the end callback)
       ignitionSchedule1.startTime = micros();
       ign1LastRev = currentStatus.startRevolutions;
-      IGN1_COMPARE = ignitionSchedule1.endCompare; //OCR5A = TCNT5 + (ignitionSchedule1.duration >> 2); //Divide by 4
+      IGN1_COMPARE = ignitionSchedule1.endCompare;
     }
     else if (ignitionSchedule1.Status == RUNNING)
     {
@@ -642,7 +720,7 @@ static inline void ignitionSchedule4Interrupt() //Most ARM chips can simply call
       ignitionSchedule4.Status = RUNNING; //Set the status to be in progress (ie The start callback has been called, but not the end callback)
       ignitionSchedule4.startTime = micros();
       ign4LastRev = currentStatus.startRevolutions;
-      IGN4_COMPARE = ignitionSchedule4.endCompare; //OCR4A = TCNT4 + (ignitionSchedule4.duration >> 4); //Divide by 16
+      IGN4_COMPARE = ignitionSchedule4.endCompare;
     }
     else if (ignitionSchedule4.Status == RUNNING)
     {
@@ -683,15 +761,24 @@ static inline void ignitionSchedule5Interrupt() //Most ARM chips can simply call
 #if defined(CORE_TEENSY)
 void ftm0_isr(void)
 {
+  //Use separate variables for each test to ensure conversion to bool
+  bool interrupt1 = (FTM0_C0SC & FTM_CSC_CHF);
+  bool interrupt2 = (FTM0_C1SC & FTM_CSC_CHF);
+  bool interrupt3 = (FTM0_C2SC & FTM_CSC_CHF);
+  bool interrupt4 = (FTM0_C3SC & FTM_CSC_CHF);
+  bool interrupt5 = (FTM0_C4SC & FTM_CSC_CHF);
+  bool interrupt6 = (FTM0_C5SC & FTM_CSC_CHF);
+  bool interrupt7 = (FTM0_C6SC & FTM_CSC_CHF);
+  bool interrupt8 = (FTM0_C7SC & FTM_CSC_CHF);
 
-  if(FTM0_C0SC & FTM_CSC_CHF) { FTM0_C0SC &= ~FTM_CSC_CHF; fuelSchedule1Interrupt(); }
+  if(interrupt1) { FTM0_C0SC &= ~FTM_CSC_CHF; fuelSchedule1Interrupt(); }
-  else if(FTM0_C1SC & FTM_CSC_CHF) { FTM0_C1SC &= ~FTM_CSC_CHF; fuelSchedule2Interrupt(); }
+  else if(interrupt2) { FTM0_C1SC &= ~FTM_CSC_CHF; fuelSchedule2Interrupt(); }
-  else if(FTM0_C2SC & FTM_CSC_CHF) { FTM0_C2SC &= ~FTM_CSC_CHF; fuelSchedule3Interrupt(); }
+  else if(interrupt3) { FTM0_C2SC &= ~FTM_CSC_CHF; fuelSchedule3Interrupt(); }
-  else if(FTM0_C3SC & FTM_CSC_CHF) { FTM0_C3SC &= ~FTM_CSC_CHF; fuelSchedule4Interrupt(); }
+  else if(interrupt4) { FTM0_C3SC &= ~FTM_CSC_CHF; fuelSchedule4Interrupt(); }
-  else if(FTM0_C4SC & FTM_CSC_CHF) { FTM0_C4SC &= ~FTM_CSC_CHF; ignitionSchedule1Interrupt(); }
+  else if(interrupt5) { FTM0_C4SC &= ~FTM_CSC_CHF; ignitionSchedule1Interrupt(); }
-  else if(FTM0_C5SC & FTM_CSC_CHF) { FTM0_C5SC &= ~FTM_CSC_CHF; ignitionSchedule2Interrupt(); }
+  else if(interrupt6) { FTM0_C5SC &= ~FTM_CSC_CHF; ignitionSchedule2Interrupt(); }
-  else if(FTM0_C6SC & FTM_CSC_CHF) { FTM0_C6SC &= ~FTM_CSC_CHF; ignitionSchedule3Interrupt(); }
+  else if(interrupt7) { FTM0_C6SC &= ~FTM_CSC_CHF; ignitionSchedule3Interrupt(); }
-  else if(FTM0_C7SC & FTM_CSC_CHF) { FTM0_C7SC &= ~FTM_CSC_CHF; ignitionSchedule4Interrupt(); }
+  else if(interrupt8) { FTM0_C7SC &= ~FTM_CSC_CHF; ignitionSchedule4Interrupt(); }
 
 }
 #endif

speeduino/speeduino.ino

@@ -114,12 +114,6 @@ int channel3InjDegrees; //The number of crank degrees until cylinder 3 (and 5/6/
 int channel4InjDegrees; //The number of crank degrees until cylinder 4 (and 5/6/7/8) is at TDC
 int channel5InjDegrees; //The number of crank degrees until cylinder 5 is at TDC
 
-bool channel1InjEnabled = true;
-bool channel2InjEnabled = false;
-bool channel3InjEnabled = false;
-bool channel4InjEnabled = false;
-bool channel5InjEnabled = false;
-
 //These are the functions the get called to begin and end the ignition coil charging. They are required for the various spark output modes
 void (*ign1StartFunction)();
 void (*ign1EndFunction)();
@@ -1365,7 +1359,7 @@ void loop()
 
       if (fuelOn && currentStatus.PW1 > 0 && !BIT_CHECK(currentStatus.squirt, BIT_SQUIRT_BOOSTCUT))
       {
-        if (injector1StartAngle <= crankAngle && fuelSchedule1.schedulesSet == 0) { injector1StartAngle += CRANK_ANGLE_MAX_INJ; }
+        if ( (injector1StartAngle <= crankAngle) && (fuelSchedule1.Status == RUNNING) ) { injector1StartAngle += CRANK_ANGLE_MAX_INJ; }
         if (injector1StartAngle > crankAngle)
         {
           setFuelSchedule1(
@@ -1391,7 +1385,7 @@ void loop()
           if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX_INJ; }
           tempStartAngle = injector2StartAngle - channel2InjDegrees;
           if ( tempStartAngle < 0) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
-          if (tempStartAngle <= tempCrankAngle && fuelSchedule2.schedulesSet == 0) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
+          if ( (tempStartAngle <= tempCrankAngle) && (fuelSchedule1.Status == RUNNING) ) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
           if ( tempStartAngle > tempCrankAngle )
           {
             setFuelSchedule2(
@@ -1407,7 +1401,7 @@ void loop()
           if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX_INJ; }
           tempStartAngle = injector3StartAngle - channel3InjDegrees;
           if ( tempStartAngle < 0) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
-          if (tempStartAngle <= tempCrankAngle && fuelSchedule3.schedulesSet == 0) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
+          if ( (tempStartAngle <= tempCrankAngle) && (fuelSchedule3.Status == RUNNING) ) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
           if ( tempStartAngle > tempCrankAngle )
           {
             setFuelSchedule3(
@@ -1423,7 +1417,7 @@ void loop()
           if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX_INJ; }
           tempStartAngle = injector4StartAngle - channel4InjDegrees;
           if ( tempStartAngle < 0) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
-          if (tempStartAngle <= tempCrankAngle && fuelSchedule4.schedulesSet == 0) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
+          if ( (tempStartAngle <= tempCrankAngle) && (fuelSchedule4.Status == RUNNING) ) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
           if ( tempStartAngle > tempCrankAngle )
           {
             setFuelSchedule4(