From c925905c95bc98f2629032d54e03ff9ed4e565e2 Mon Sep 17 00:00:00 2001
From: "Vitor Moreno B. Sales" <vitor_boss@yahoo.com.br>
Date: Fri, 2 Jun 2017 18:09:36 -0300
Subject: [PATCH 01/10] Initial STM32F4 series support

---
 speeduino/auxiliaries.ino |  71 +++--
 speeduino/cancomms.h      |  15 +-
 speeduino/cancomms.ino    | 269 ++++++-------------
 speeduino/comms.h         |   4 +-
 speeduino/comms.ino       | 526 +++++++++++++++++++-------------------
 speeduino/corrections.h   |   3 +-
 speeduino/corrections.ino |   8 +-
 speeduino/globals.h       |  11 +-
 speeduino/maths.h         |   4 +-
 speeduino/maths.ino       |  30 ++-
 speeduino/scheduler.ino   |   5 +-
 speeduino/sensors.ino     |  28 +-
 speeduino/speeduino.ino   |  59 ++---
 speeduino/storage.h       |  28 +-
 speeduino/storage.ino     | 510 ++++++++++++++++++++++++++++++++++++
 speeduino/table.ino       |  42 ++-
 speeduino/updates.ino     |  52 ++--
 speeduino/utils.ino       |  35 ++-
 18 files changed, 1086 insertions(+), 614 deletions(-)

diff --git a/speeduino/auxiliaries.ino b/speeduino/auxiliaries.ino
index 306a5663..a0e560fb 100644
--- a/speeduino/auxiliaries.ino
+++ b/speeduino/auxiliaries.ino
@@ -3,7 +3,6 @@ Speeduino - Simple engine management for the Arduino Mega 2560 platform
 Copyright (C) Josh Stewart
 A full copy of the license may be found in the projects root directory
 */
-//integerPID boostPID(&currentStatus.MAP, &boost_pwm_target_value, &boost_cl_target_boost, configPage3.boostKP, configPage3.boostKI, configPage3.boostKD, DIRECT); //This is the PID object if that algorithm is used. Needs to be global as it maintains state outside of each function call
 integerPID boostPID(&MAPx100, &boost_pwm_target_value, &boostTargetx100, configPage3.boostKP, configPage3.boostKI, configPage3.boostKD, DIRECT); //This is the PID object if that algorithm is used. Needs to be global as it maintains state outside of each function call
 
 /*
@@ -11,21 +10,21 @@ Fan control
 */
 void initialiseFan()
 {
-  if(configPage4.fanInv) { fanHIGH = LOW, fanLOW = HIGH; }
-  else { fanHIGH = HIGH, fanLOW = LOW; }
+  if( configPage4.fanInv == 1 ) { fanHIGH = LOW; fanLOW = HIGH; }
+  else { fanHIGH = HIGH; fanLOW = LOW; }
   digitalWrite(pinFan, fanLOW);         //Initiallise program with the fan in the off state
   currentStatus.fanOn = false;
 }
 
 void fanControl()
 {
-  if(configPage4.fanEnable)
+  if( configPage4.fanEnable == 1 )
   {
     int onTemp = (int)configPage4.fanSP - CALIBRATION_TEMPERATURE_OFFSET;
     int offTemp = onTemp - configPage4.fanHyster;
 
-    if (!currentStatus.fanOn && currentStatus.coolant >= onTemp) { digitalWrite(pinFan,fanHIGH); currentStatus.fanOn = true; }
-    if (currentStatus.fanOn && currentStatus.coolant <= offTemp) { digitalWrite(pinFan, fanLOW); currentStatus.fanOn = false; }
+    if ( (!currentStatus.fanOn) && (currentStatus.coolant >= onTemp) ) { digitalWrite(pinFan,fanHIGH); currentStatus.fanOn = true; }
+    if ( (currentStatus.fanOn) && (currentStatus.coolant <= offTemp) ) { digitalWrite(pinFan, fanLOW); currentStatus.fanOn = false; }
   }
 }
 
@@ -45,7 +44,7 @@ void initialiseAuxPWM()
 
   boost_pwm_max_count = 1000000L / (16 * configPage3.boostFreq * 2); //Converts the frequency in Hz to the number of ticks (at 16uS) it takes to complete 1 cycle. The x2 is there because the frequency is stored at half value (in a byte) to allow freqneucies up to 511Hz
   vvt_pwm_max_count = 1000000L / (16 * configPage3.vvtFreq * 2); //Converts the frequency in Hz to the number of ticks (at 16uS) it takes to complete 1 cycle
-  //TIMSK1 |= (1 << OCIE1A); //Turn on the A compare unit (ie turn on the interrupt)  //Shouldn't be needed with closed loop as its turned on below
+  //TIMSK1 |= (1 << OCIE1A); <---- Not required as compare A is turned on when needed by boost control
   TIMSK1 |= (1 << OCIE1B); //Turn on the B compare unit (ie turn on the interrupt)
 
   boostPID.SetOutputLimits(percentage(configPage1.boostMinDuty, boost_pwm_max_count) , percentage(configPage1.boostMaxDuty, boost_pwm_max_count));
@@ -58,29 +57,44 @@ void initialiseAuxPWM()
 
 void boostControl()
 {
-  if(configPage3.boostEnabled)
+  if( configPage3.boostEnabled==1 )
   {
-    if(currentStatus.MAP < 100) { TIMSK1 &= ~(1 << OCIE1A); digitalWrite(pinBoost, LOW); return; } //Set duty to 0 and turn off timer compare
-    MAPx100 = currentStatus.MAP * 100;
-
-    boost_cl_target_boost = get3DTableValue(&boostTable, currentStatus.TPS, currentStatus.RPM) * 2; //Boost target table is in kpa and divided by 2
-    //If flex fuel is enabled, there can be an adder to the boost target based on ethanol content
-    if(configPage1.flexEnabled)
+    if(currentStatus.MAP >= 100)
     {
-      int16_t boostAdder = (((int16_t)configPage1.flexBoostHigh - (int16_t)configPage1.flexBoostLow) * currentStatus.ethanolPct) / 100;
-      boostAdder = boostAdder + configPage1.flexBoostLow; //Required in case flexBoostLow is less than 0
-      boost_cl_target_boost = boost_cl_target_boost + boostAdder;
+      MAPx100 = currentStatus.MAP * 100;
+
+      boost_cl_target_boost = get3DTableValue(&boostTable, currentStatus.TPS, currentStatus.RPM) * 2; //Boost target table is in kpa and divided by 2
+      //If flex fuel is enabled, there can be an adder to the boost target based on ethanol content
+      if( configPage1.flexEnabled == 1 )
+      {
+        int16_t boostAdder = (((int16_t)configPage1.flexBoostHigh - (int16_t)configPage1.flexBoostLow) * currentStatus.ethanolPct) / 100;
+        boostAdder = boostAdder + configPage1.flexBoostLow; //Required in case flexBoostLow is less than 0
+        boost_cl_target_boost = boost_cl_target_boost + boostAdder;
+      }
+
+      boostTargetx100 = boost_cl_target_boost  * 100;
+      currentStatus.boostTarget = boost_cl_target_boost >> 1; //Boost target is sent as a byte value to TS and so is divided by 2
+      if(currentStatus.boostTarget > 0)
+      {
+        if( (boostCounter & 31) == 1) { boostPID.SetTunings(configPage3.boostKP, configPage3.boostKI, configPage3.boostKD); } //This only needs to be run very infrequently, once every 32 calls to boostControl(). This is approx. once per second
+
+        boostPID.Compute();
+        currentStatus.boostDuty = (unsigned long)(boost_pwm_target_value * 100UL) / boost_pwm_max_count;
+        TIMSK1 |= (1 << OCIE1A); //Turn on the compare unit (ie turn on the interrupt)
+      }
+      else
+      {
+        //If boost target is 0, turn everything off
+        TIMSK1 &= ~(1 << OCIE1A); //Turn off timer
+        digitalWrite(pinBoost, LOW);
+      }
+    }
+    else
+    {
+      //Boost control does nothing if kPa below 100
+      TIMSK1 &= ~(1 << OCIE1A); //Turn off timer
+      digitalWrite(pinBoost, LOW); //Make sure solenoid is off (0% duty)
     }
-
-    boostTargetx100 = boost_cl_target_boost  * 100;
-    currentStatus.boostTarget = boost_cl_target_boost >> 1; //Boost target is sent as a byte value to TS and so is divided by 2
-    if(currentStatus.boostTarget == 0) { TIMSK1 &= ~(1 << OCIE1A); digitalWrite(pinBoost, LOW); return; } //Set duty to 0 and turn off timer compare if the target is 0
-
-    if( (boostCounter & 31) == 1) { boostPID.SetTunings(configPage3.boostKP, configPage3.boostKI, configPage3.boostKD); } //This only needs to be run very infrequently, once every 32 calls to boostControl(). This is approx. once per second
-
-    boostPID.Compute();
-    currentStatus.boostDuty = (unsigned long)(boost_pwm_target_value * 100UL) / boost_pwm_max_count;
-    TIMSK1 |= (1 << OCIE1A); //Turn on the compare unit (ie turn on the interrupt)
   }
   else { TIMSK1 &= ~(1 << OCIE1A); } // Disable timer channel
 
@@ -89,7 +103,7 @@ void boostControl()
 
 void vvtControl()
 {
-  if(configPage3.vvtEnabled)
+  if( configPage3.vvtEnabled == 1 )
   {
     byte vvtDuty = get3DTableValue(&vvtTable, currentStatus.TPS, currentStatus.RPM);
     vvt_pwm_target_value = percentage(vvtDuty, vvt_pwm_max_count);
@@ -142,3 +156,4 @@ void boostControl() { }
 void vvtControl() { }
 
 #endif
+
diff --git a/speeduino/cancomms.h b/speeduino/cancomms.h
index d5dafcc2..eba8135b 100644
--- a/speeduino/cancomms.h
+++ b/speeduino/cancomms.h
@@ -6,13 +6,22 @@
 uint8_t currentcanCommand;
 uint8_t currentCanPage = 1;//Not the same as the speeduino config page numbers
 uint8_t nCanretry = 0;      //no of retrys
-uint8_t cancmdfail = 0;     //command fail yes/no 
-uint8_t canlisten = 0;      
+uint8_t cancmdfail = 0;     //command fail yes/no
+uint8_t canlisten = 0;
 uint8_t Lbuffer[8];         //8 byte buffer to store incomng can data
 uint8_t Gdata[9];
 uint8_t Glow, Ghigh;
-  
+
+#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+  HardwareSerial &CANSerial = Serial3;
+#elif defined(CORE_STM32)
+  HardwareSerial &CANSerial = Serial2;
+#elif defined(CORE_TEENSY)
+  HardwareSerial &CANSerial = Serial2;
+#endif
+
 void canCommand();//This is the heart of the Command Line Interpeter.  All that needed to be done was to make it human readable.
 void sendCancommand(uint8_t cmdtype , uint16_t canadddress, uint8_t candata1, uint8_t candata2, uint16_t paramgroup);
 
 #endif // CANCOMMS_H
+
diff --git a/speeduino/cancomms.ino b/speeduino/cancomms.ino
index 18ad551a..f972b7a6 100644
--- a/speeduino/cancomms.ino
+++ b/speeduino/cancomms.ino
@@ -11,20 +11,9 @@ It parses the command and calls the relevant function
 sendcancommand is called when a comman d is to be sent via serial3 to the Can interface
 */
 
-//#include "cancomms.h"
-//#include "globals.h"
-//#include "storage.h"
-
 void canCommand()
 {
-#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)  
-  currentcanCommand = Serial3.read();
-#elif defined(CORE_STM32)
-  currentcanCommand = Serial2.read();
-#elif defined(CORE_TEENSY)
-  currentcanCommand = Serial2.read();
-#else return;  
-#endif  
+  currentcanCommand = CANSerial.read();
 
   switch (currentcanCommand)
   {
@@ -34,32 +23,15 @@ void canCommand()
 
     case 'G': // this is the reply command sent by the Can interface
         //uint8_t Gdata;
-#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)       
-        while (Serial3.available() == 0) { }
-        cancmdfail = Serial3.read();
-#elif defined(CORE_STM32)
-        while (Serial2.available() == 0) { }
-        cancmdfail = Serial2.read();
-#elif defined(CORE_TEENSY)
-        while (Serial2.available() == 0) { }
-        cancmdfail = Serial2.read();
-#else return;        
-#endif
+        while (CANSerial.available() == 0) { }
+        cancmdfail = CANSerial.read();
+
         if (cancmdfail != 0)
         {
           for (byte Gx = 0; Gx < 8; Gx++) //read all 8 bytes of data
             {
-#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)             
-              while (Serial3.available() == 0) { }
-              Gdata[Gx] = Serial3.read();
-#elif defined(CORE_STM32)
-              while (Serial2.available() == 0) { }
-              Gdata[Gx] = Serial2.read();
-#elif defined(CORE_TEENSY)
-              while (Serial2.available() == 0) { }
-              Gdata[Gx] = Serial2.read();
-#else return;              
-#endif
+              while (CANSerial.available() == 0) { }
+              Gdata[Gx] = CANSerial.read();
             }
 
           Glow = Gdata[(configPage10.caninput_param_start_byte[currentStatus.current_caninchannel])];
@@ -72,150 +44,91 @@ void canCommand()
              }
           else
              {
-              Ghigh = 0;     
+              Ghigh = 0;
              }
-                
-          currentStatus.canin[currentStatus.current_caninchannel] = word(Ghigh, Glow);   
+
+          currentStatus.canin[currentStatus.current_caninchannel] = word(Ghigh, Glow);
         }
-        
-        else{}  //continue as command request failed and/or data/device was not available  
-        
+
+        else{}  //continue as command request failed and/or data/device was not available
+
         if (currentStatus.current_caninchannel <= 6)     // if channel is 0-7
            {
             currentStatus.current_caninchannel++;       //inc to next channel
            }
-        else  
+        else
            {
             currentStatus.current_caninchannel = 0;      //reset to start
-           }   
-             
+           }
+
         break;
 
     case 'L':
         uint8_t Llength;
-#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
-        while (Serial3.available() == 0) { }
-        canlisten = Serial3.read();
-#elif defined(CORE_STM32)
-        while (Serial2.available() == 0) { }
-        canlisten = Serial2.read();
-#elif defined(CORE_TEENSY)
-        while (Serial2.available() == 0) { }
-        canlisten = Serial2.read();
-#else return;        
-#endif
+        while (CANSerial.available() == 0) { }
+        canlisten = CANSerial.read();
+
         if (canlisten == 0)
          {
           //command request failed and/or data/device was not available
           break;
          }
-#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)         
-       while (Serial3.available() == 0) { }
-       Llength= Serial3.read();              // next the number of bytes expected value
-#elif defined(CORE_STM32)
-       while (Serial2.available() == 0) { }
-       Llength= Serial2.read();              // next the number of bytes expected value
-#elif defined(CORE_TEENSY)       
-       while (Serial2.available() == 0) { }
-       Llength= Serial2.read();              // next the number of bytes expected value
-#else return;       
-#endif
+
+         while (CANSerial.available() == 0) { }
+         Llength= CANSerial.read();              // next the number of bytes expected value
+
          for (uint8_t Lcount = 0; Lcount <Llength ;Lcount++)
-                {
-#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)                  
-                  while (Serial3.available() == 0){}
-                  // receive all x bytes into "Lbuffer"
-                  Lbuffer[Lcount] = Serial3.read();
-#elif defined(CORE_STM32)
-                  while (Serial2.available() == 0){}
-                  // receive all x bytes into "Lbuffer"
-                  Lbuffer[Lcount] = Serial2.read();
-#elif defined(CORE_TEENSY)
-                  while (Serial2.available() == 0){}
-                  // receive all x bytes into "Lbuffer"
-                  Lbuffer[Lcount] = Serial2.read();
-#else return;                  
-#endif
-                }
-       break;
-       
+         {
+           while (CANSerial.available() == 0){}
+           // receive all x bytes into "Lbuffer"
+           Lbuffer[Lcount] = CANSerial.read();
+         }
+         break;
+
     case 'r': //New format for the optimised OutputChannels
       byte cmd;
-      byte tsCanId_sent;
-#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) //ATmega2561 does not have Serial3      
-      if (Serial3.available() < 6) { return; }
-      tsCanId_sent = Serial3.read(); //Read the $tsCanId
-      cmd = Serial3.read();
-#elif defined(CORE_STM32)
-      if (Serial2.available() < 6) { return; }
-      tsCanId_sent = Serial2.read(); //Read the $tsCanId
-      cmd = Serial2.read();
-#elif defined(CORE_TEENSY)
-      if (Serial2.available() < 6) { return; }
-      tsCanId_sent = Serial2.read(); //Read the $tsCanId
-      cmd = Serial2.read();
-#else return;      
-#endif
-      uint16_t offset, length;
-      if(cmd == 0x30) //Send output channels command 0x30 is 48dec
+
+      if (CANSerial.available() >= 6)
       {
-        byte tmp;
-#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) //ATmega2561 does not have Serial3
-        tmp = Serial3.read();
-        offset = word(Serial3.read(), tmp);
-        tmp = Serial3.read();
-        length = word(Serial3.read(), tmp);
-#elif defined(CORE_STM32)
-        tmp = Serial2.read();
-        offset = word(Serial2.read(), tmp);
-        tmp = Serial2.read();
-        length = word(Serial2.read(), tmp);
-#elif defined(CORE_TEENSY)
-        tmp = Serial2.read();
-        offset = word(Serial2.read(), tmp);
-        tmp = Serial2.read();
-        length = word(Serial2.read(), tmp);
-#else return;        
-#endif        
-        sendValues(offset, length, 3);
-      }
-      else
-      {
-        //No other r/ commands should be called
+        CANSerial.read(); //Read the $tsCanId
+        cmd = CANSerial.read();
+
+        uint16_t offset, length;
+        if(cmd == 0x30) //Send output channels command 0x30 is 48dec
+        {
+          byte tmp;
+          tmp = CANSerial.read();
+          offset = word(CANSerial.read(), tmp);
+          tmp = CANSerial.read();
+          length = word(CANSerial.read(), tmp);
+          sendValues(offset, length, 3);
+        }
+        else
+        {
+          //No other r/ commands should be called
+        }
       }
       break;
-      
+
     case 'S': // send code version
-       for (unsigned int sig = 0; sig < sizeof(displaySignature) - 1; sig++){
-#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) //ATmega2561 does not have Serial3        
-           Serial3.write(displaySignature[sig]);
-#elif defined(CORE_STM32) 
-          Serial2.write(displaySignature[sig]);
-#elif defined(CORE_TEENSY)
-          Serial2.write(displaySignature[sig]);
-#else return;
-#endif          
+       for (unsigned int sig = 0; sig < sizeof(displaySignature) - 1; sig++)
+       {
+         CANSerial.write(displaySignature[sig]);
        }
        //Serial3.print("speeduino 201609-dev");
        break;
 
     case 'Q': // send code version
-       for (unsigned int revn = 0; revn < sizeof( TSfirmwareVersion) - 1; revn++){
-#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) //ATmega2561 does not have Serial3
-           Serial3.write( TSfirmwareVersion[revn]);
-#elif defined(CORE_STM32) 
-           Serial2.write( TSfirmwareVersion[revn]);          
-#elif defined(CORE_TEENSY)
-           Serial2.write( TSfirmwareVersion[revn]);
-#else return;
-#endif           
+       for (unsigned int revn = 0; revn < sizeof( TSfirmwareVersion) - 1; revn++)
+       {
+         CANSerial.write( TSfirmwareVersion[revn]);
        }
        //Serial3.print("speeduino 201609-dev");
        break;
 
     case 'Z': //dev use
        break;
-       
+
     default:
        break;
   }
@@ -227,65 +140,29 @@ void sendCancommand(uint8_t cmdtype, uint16_t canaddress, uint8_t candata1, uint
     switch (cmdtype)
     {
       case 0:
-#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) //ATmega2561 does not have Serial3       
-        Serial3.print("G");
-        Serial3.write(canaddress);  //tscanid of speeduino device
-        Serial3.write(candata1);    // table id
-        Serial3.write(candata2);    //table memory offset
-#elif defined(CORE_STM32)
-        Serial2.print("G");
-        Serial2.write(canaddress);  //tscanid of speeduino device
-        Serial2.write(candata1);    // table id
-        Serial2.write(candata2);    //table memory offset
-#elif defined(CORE_TEENSY)
-        Serial2.print("G");
-        Serial2.write(canaddress);  //tscanid of speeduino device
-        Serial2.write(candata1);    // table id
-        Serial2.write(candata2);    //table memory offset
-#else return;
-#endif        
-     break;
+        CANSerial.print("G");
+        CANSerial.write(canaddress);  //tscanid of speeduino device
+        CANSerial.write(candata1);    // table id
+        CANSerial.write(candata2);    //table memory offset
+        break;
 
-     case 1:                      //send request to listen for a can message
-#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) //ATmega2561 does not have Serial3 
-        Serial3.print("L");
-        Serial3.write(canaddress);  //11 bit canaddress of device to listen for
-#elif defined(CORE_STM32)
-        Serial2.print("L");
-        Serial2.write(canaddress);  //11 bit canaddress of device to listen for
-#elif defined(CORE_TEENSY)        
-        Serial2.print("L");
-        Serial2.write(canaddress);  //11 bit canaddress of device to listen for
-
-#else return;
-#endif
-     break;
+      case 1:                      //send request to listen for a can message
+        CANSerial.print("L");
+        CANSerial.write(canaddress);  //11 bit canaddress of device to listen for
+        break;
 
      case 2:
-#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) //ATmega2561 does not have Serial3     
-        Serial3.print("R");                         //send "R" to request data from the parmagroup whos value is sent next
-        Serial3.write( lowByte(paramgroup) );       //send lsb first
-        Serial3.write( lowByte(paramgroup >> 8) );
-#elif defined(CORE_STM32)
-        Serial2.print("R");                         //send "R" to request data from the parmagroup whos value is sent next
-        Serial2.write( lowByte(paramgroup) );       //send lsb first
-        Serial2.write( lowByte(paramgroup >> 8) );
-#elif defined(CORE_TEENSY)
-        Serial2.print("R");                         //send "R" to request data from the parmagroup whos value is sent next
-        Serial2.write( lowByte(paramgroup) );       //send lsb first
-        Serial2.write( lowByte(paramgroup >> 8) );
-#else return;
-#endif
-     break;
+        CANSerial.print("R");                         //send "R" to request data from the parmagroup whos value is sent next
+        CANSerial.write( lowByte(paramgroup) );       //send lsb first
+        CANSerial.write( lowByte(paramgroup >> 8) );
+        break;
 
      case 3:
         //send to truecan send routine
-#if defined(CORE_STM32)
+        break;
 
-#elif defined(CORE_TEENSY)
-#else return;
-#endif
-     break;
+     default:
+        break;
     }
 }
-
+
diff --git a/speeduino/comms.h b/speeduino/comms.h
index bd18276d..24f5100b 100644
--- a/speeduino/comms.h
+++ b/speeduino/comms.h
@@ -15,7 +15,7 @@
 #define packetSize   57//41
 
 byte currentPage = 1;//Not the same as the speeduino config page numbers
-boolean isMap = true;
+bool isMap = true;
 unsigned long requestCount = 0; //The number of times the A command has been issued
 byte currentCommand;
 bool cmdPending = false;
@@ -49,4 +49,4 @@ void sendToothLog(bool useChar);
 void testComm();
 void commandButtons();
 
-#endif // COMMS_H
+#endif // COMMS_H
diff --git a/speeduino/comms.ino b/speeduino/comms.ino
index 9ed67d44..7b50cd67 100644
--- a/speeduino/comms.ino
+++ b/speeduino/comms.ino
@@ -36,13 +36,15 @@ void command()
     case 'E': // receive command button commands
       cmdPending = true;
 
-      if(Serial.available() < 2) { return; }
-      cmdGroup = Serial.read();
-      cmdValue = Serial.read();
-      cmdCombined = word(cmdGroup, cmdValue);
-      if (currentStatus.RPM == 0) { commandButtons(); }
+      if(Serial.available() >= 2)
+      {
+        cmdGroup = Serial.read();
+        cmdValue = Serial.read();
+        cmdCombined = word(cmdGroup, cmdValue);
+        if (currentStatus.RPM == 0) { commandButtons(); }
 
-      cmdPending = false;
+        cmdPending = false;
+      }
       break;
 
     case 'L': // List the contents of current page in human readable form
@@ -57,18 +59,20 @@ void command()
       //A 2nd byte of data is required after the 'P' specifying the new page number.
       cmdPending = true;
 
-      if (Serial.available() == 0) { return; }
-      currentPage = Serial.read();
-      if (currentPage >= '0') {//This converts the ascii number char into binary
-        currentPage -= '0';
+      if (Serial.available() > 0)
+      {
+        currentPage = Serial.read();
+        if (currentPage >= '0') {//This converts the ascii number char into binary
+          currentPage -= '0';
+        }
+        if ( (currentPage == veMapPage) || (currentPage == ignMapPage) || (currentPage == afrMapPage) ) { // Detecting if the current page is a table/map
+          isMap = true;
+        }
+        else {
+          isMap = false;
+        }
+        cmdPending = false;
       }
-      if (currentPage == veMapPage || currentPage == ignMapPage || currentPage == afrMapPage) { // Detecting if the current page is a table/map
-        isMap = true;
-      }
-      else {
-        isMap = false;
-      }
-      cmdPending = false;
       break;
 
     case 'F': // send serial protocol version
@@ -90,24 +94,31 @@ void command()
 
     case 'W': // receive new VE obr constant at 'W'+<offset>+<newbyte>
       cmdPending = true;
+
       int valueOffset; //cannot use offset as a variable name, it is a reserved word for several teensy libraries
 
       if (isMap)
       {
-        if(Serial.available()< 3) { return; } // 1 additional byte is required on the MAP pages which are larger than 255 bytes
-        byte offset1, offset2;
-        offset1 = Serial.read();
-        offset2 = Serial.read();
-        valueOffset = word(offset2, offset1);
+        if(Serial.available() >= 3) // 1 additional byte is required on the MAP pages which are larger than 255 bytes
+        {
+          byte offset1, offset2;
+          offset1 = Serial.read();
+          offset2 = Serial.read();
+          valueOffset = word(offset2, offset1);
+          receiveValue(valueOffset, Serial.read());
+          cmdPending = false;
+        }
       }
       else
       {
-        if(Serial.available()< 2) { return; }
-        valueOffset = Serial.read();
+        if(Serial.available() >= 2)
+        {
+          valueOffset = Serial.read();
+          receiveValue(valueOffset, Serial.read());
+          cmdPending = false;
+        }
       }
 
-      receiveValue(valueOffset, Serial.read());
-      cmdPending = false;
       break;
 
     case 't': // receive new Calibration info. Command structure: "t", <tble_idx> <data array>. This is an MS2/Extra command, NOT part of MS1 spec
@@ -124,12 +135,6 @@ void command()
       break;
 
     case 'Z': //Totally non-standard testing function. Will be removed once calibration testing is completed. This function takes 1.5kb of program space! :S
-      digitalWrite(pinInjector1, HIGH);
-      digitalWrite(pinInjector2, HIGH);
-      delay(20);
-      digitalWrite(pinInjector1, LOW);
-      digitalWrite(pinInjector2, LOW);
-      return;
       Serial.println(F("Coolant"));
       for (int x = 0; x < CALIBRATION_TABLE_SIZE; x++)
       {
@@ -172,25 +177,27 @@ void command()
     case 'r': //New format for the optimised OutputChannels
       cmdPending = true;
       byte cmd;
-      if (Serial.available() < 6) { return; }
-      tsCanId = Serial.read(); //Read the $tsCanId
-      cmd = Serial.read(); // read the command
+      if (Serial.available() >= 6)
+      {
+        tsCanId = Serial.read(); //Read the $tsCanId
+        cmd = Serial.read(); // read the command
 
-      uint16_t offset, length;
-      if(cmd == 0x30) //Send output channels command 0x30 is 48dec
-      {
-        byte tmp;
-        tmp = Serial.read();
-        offset = word(Serial.read(), tmp);
-        tmp = Serial.read();
-        length = word(Serial.read(), tmp);
-        sendValues(offset, length, 0);
+        uint16_t offset, length;
+        if(cmd == 0x30) //Send output channels command 0x30 is 48dec
+        {
+          byte tmp;
+          tmp = Serial.read();
+          offset = word(Serial.read(), tmp);
+          tmp = Serial.read();
+          length = word(Serial.read(), tmp);
+          sendValues(offset, length, 0);
+        }
+        else
+        {
+          //No other r/ commands should be called
+        }
+        cmdPending = false;
       }
-      else
-      {
-        //No other r/ commands should be called
-      }
-      cmdPending = false;
       break;
 
 
@@ -240,7 +247,6 @@ This function returns the current values of a fixed group of variables
 void sendValues(uint16_t offset, uint16_t packetLength, byte portNum)
 {
   byte fullStatus[packetSize];
-  byte response[packetLength];
 
   if (portNum == 3)
   {
@@ -343,20 +349,10 @@ void sendValues(uint16_t offset, uint16_t packetLength, byte portNum)
 
   for(byte x=0; x<packetLength; x++)
   {
-    response[x] = fullStatus[offset+x];
+    if (portNum == 0) { Serial.write(fullStatus[offset+x]); }
+    else if (portNum == 3) { CANSerial.write(fullStatus[offset+x]); }
   }
 
-//cli();
-  if (portNum == 0) { Serial.write(response, (size_t)packetLength); }
-  #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) //ATmega2561 does not have Serial3
-    else if (portNum == 3) { Serial3.write(response, (size_t)packetLength); }
-  #elif defined(CORE_STM32)
-    else if (portNum == 3) { Serial2.write(response, (size_t)packetLength); }
-  #elif defined(CORE_TEENSY)
-    else if (portNum == 3) { Serial2.write(response, (size_t)packetLength); }
-  #endif
-//sei();
-  return;
 }
 
 void receiveValue(int valueOffset, byte newValue)
@@ -369,8 +365,7 @@ void receiveValue(int valueOffset, byte newValue)
     case veMapPage:
       if (valueOffset < 256) //New value is part of the fuel map
       {
-        fuelTable.values[15 - valueOffset / 16][valueOffset % 16] = newValue;
-        return;
+        fuelTable.values[15 - (valueOffset / 16)][valueOffset % 16] = newValue;
       }
       else
       {
@@ -383,10 +378,9 @@ void receiveValue(int valueOffset, byte newValue)
         else
         {
           //Y Axis
-          valueOffset = 15 - (valueOffset - 272); //Need to do a translation to flip the order (Due to us using (0,0) in the top left rather than bottom right
-          fuelTable.axisY[valueOffset] = (int)(newValue) * TABLE_LOAD_MULTIPLIER;
+          int tempOffset = 15 - (valueOffset - 272); //Need to do a translation to flip the order (Due to us using (0,0) in the top left rather than bottom right
+          fuelTable.axisY[tempOffset] = (int)(newValue) * TABLE_LOAD_MULTIPLIER;
         }
-        return;
       }
       break;
 
@@ -402,8 +396,7 @@ void receiveValue(int valueOffset, byte newValue)
     case ignMapPage: //Ignition settings page (Page 2)
       if (valueOffset < 256) //New value is part of the ignition map
       {
-        ignitionTable.values[15 - valueOffset / 16][valueOffset % 16] = newValue;
-        return;
+        ignitionTable.values[15 - (valueOffset / 16)][valueOffset % 16] = newValue;
       }
       else
       {
@@ -416,11 +409,11 @@ void receiveValue(int valueOffset, byte newValue)
         else
         {
           //Y Axis
-          valueOffset = 15 - (valueOffset - 272); //Need to do a translation to flip the order
-          ignitionTable.axisY[valueOffset] = (int)(newValue) * TABLE_LOAD_MULTIPLIER;
+          int tempOffset = 15 - (valueOffset - 272); //Need to do a translation to flip the order
+          ignitionTable.axisY[tempOffset] = (int)(newValue) * TABLE_LOAD_MULTIPLIER;
         }
-        return;
       }
+      break;
 
     case ignSetPage:
       pnt_configPage = &configPage2;
@@ -434,8 +427,7 @@ void receiveValue(int valueOffset, byte newValue)
     case afrMapPage: //Air/Fuel ratio target settings page
       if (valueOffset < 256) //New value is part of the afr map
       {
-        afrTable.values[15 - valueOffset / 16][valueOffset % 16] = newValue;
-        return;
+        afrTable.values[15 - (valueOffset / 16)][valueOffset % 16] = newValue;
       }
       else
       {
@@ -448,12 +440,12 @@ void receiveValue(int valueOffset, byte newValue)
         else
         {
           //Y Axis
-          valueOffset = 15 - (valueOffset - 272); //Need to do a translation to flip the order
-          afrTable.axisY[valueOffset] = int(newValue) * TABLE_LOAD_MULTIPLIER;
+          int tempOffset = 15 - (valueOffset - 272); //Need to do a translation to flip the order
+          afrTable.axisY[tempOffset] = int(newValue) * TABLE_LOAD_MULTIPLIER;
 
         }
-        return;
       }
+      break;
 
     case afrSetPage:
       pnt_configPage = &configPage3;
@@ -472,57 +464,56 @@ void receiveValue(int valueOffset, byte newValue)
         *((byte *)pnt_configPage + (byte)valueOffset) = newValue;
       }
       break;
+
     case boostvvtPage: //Boost and VVT maps (8x8)
       if (valueOffset < 64) //New value is part of the boost map
       {
-        boostTable.values[7 - valueOffset / 8][valueOffset % 8] = newValue;
-        return;
+        boostTable.values[7 - (valueOffset / 8)][valueOffset % 8] = newValue;
       }
       else if (valueOffset < 72) //New value is on the X (RPM) axis of the boost table
       {
         boostTable.axisX[(valueOffset - 64)] = int(newValue) * TABLE_RPM_MULTIPLIER; //The RPM values sent by TunerStudio are divided by 100, need to multiply it back by 100 to make it correct (TABLE_RPM_MULTIPLIER)
-        return;
       }
       else if (valueOffset < 80) //New value is on the Y (TPS) axis of the boost table
       {
         boostTable.axisY[(7 - (valueOffset - 72))] = int(newValue); //TABLE_LOAD_MULTIPLIER is NOT used for boost as it is TPS based (0-100)
-        return;
       }
       else if (valueOffset < 144) //New value is part of the vvt map
       {
-        valueOffset = valueOffset - 80;
-        vvtTable.values[7 - valueOffset / 8][valueOffset % 8] = newValue;
-        return;
+        int tempOffset = valueOffset - 80;
+        vvtTable.values[7 - (tempOffset / 8)][tempOffset % 8] = newValue;
       }
       else if (valueOffset < 152) //New value is on the X (RPM) axis of the vvt table
       {
-        valueOffset = valueOffset - 144;
-        vvtTable.axisX[valueOffset] = int(newValue) * TABLE_RPM_MULTIPLIER; //The RPM values sent by TunerStudio are divided by 100, need to multiply it back by 100 to make it correct (TABLE_RPM_MULTIPLIER)
-        return;
+        int tempOffset = valueOffset - 144;
+        vvtTable.axisX[tempOffset] = int(newValue) * TABLE_RPM_MULTIPLIER; //The RPM values sent by TunerStudio are divided by 100, need to multiply it back by 100 to make it correct (TABLE_RPM_MULTIPLIER)
       }
       else //New value is on the Y (Load) axis of the vvt table
       {
-        valueOffset = valueOffset - 152;
-        vvtTable.axisY[(7 - valueOffset)] = int(newValue); //TABLE_LOAD_MULTIPLIER is NOT used for vvt as it is TPS based (0-100)
-        return;
+        int tempOffset = valueOffset - 152;
+        vvtTable.axisY[(7 - tempOffset)] = int(newValue); //TABLE_LOAD_MULTIPLIER is NOT used for vvt as it is TPS based (0-100)
       }
-    case seqFuelPage:
-      if (valueOffset < 36) { trim1Table.values[5 - valueOffset / 6][valueOffset % 6] = newValue; } //Trim1 values
-      else if (valueOffset < 42) { trim1Table.axisX[(valueOffset - 36)] = int(newValue) * TABLE_RPM_MULTIPLIER; } //New value is on the X (RPM) axis of the trim1 table. The RPM values sent by TunerStudio are divided by 100, need to multiply it back by 100 to make it correct (TABLE_RPM_MULTIPLIER)
-      else if (valueOffset < 48) { trim1Table.axisY[(5 - (valueOffset - 42))] = int(newValue) * TABLE_LOAD_MULTIPLIER; } //New value is on the Y (TPS) axis of the boost table
-      //Trim table 2
-      else if (valueOffset < 84) { valueOffset = valueOffset - 48; trim2Table.values[5 - valueOffset / 6][valueOffset % 6] = newValue; } //New value is part of the trim2 map
-      else if (valueOffset < 90) { valueOffset = valueOffset - 84; trim2Table.axisX[valueOffset] = int(newValue) * TABLE_RPM_MULTIPLIER; } //New value is on the X (RPM) axis of the table. //The RPM values sent by TunerStudio are divided by 100, need to multiply it back by 100 to make it correct (TABLE_RPM_MULTIPLIER)
-      else if (valueOffset < 96) { valueOffset = valueOffset - 90; trim2Table.axisY[(5 - valueOffset)] = int(newValue) * TABLE_LOAD_MULTIPLIER; } //New value is on the Y (Load) axis of the table
-      //Trim table 3
-      else if (valueOffset < 132) { valueOffset = valueOffset - 96; trim3Table.values[5 - valueOffset / 6][valueOffset % 6] = newValue; } //New value is part of the trim2 map
-      else if (valueOffset < 138) { valueOffset = valueOffset - 132; trim3Table.axisX[valueOffset] = int(newValue) * TABLE_RPM_MULTIPLIER; } //New value is on the X (RPM) axis of the table. //The RPM values sent by TunerStudio are divided by 100, need to multiply it back by 100 to make it correct (TABLE_RPM_MULTIPLIER)
-      else if (valueOffset < 144) { valueOffset = valueOffset - 138; trim3Table.axisY[(5 - valueOffset)] = int(newValue) * TABLE_LOAD_MULTIPLIER; } //New value is on the Y (Load) axis of the table
-      //Trim table 4
-      else if (valueOffset < 180) { valueOffset = valueOffset - 144; trim4Table.values[5 - valueOffset / 6][valueOffset % 6] = newValue; } //New value is part of the trim2 map
-      else if (valueOffset < 186) { valueOffset = valueOffset - 180; trim4Table.axisX[valueOffset] = int(newValue) * TABLE_RPM_MULTIPLIER; } //New value is on the X (RPM) axis of the table. //The RPM values sent by TunerStudio are divided by 100, need to multiply it back by 100 to make it correct (TABLE_RPM_MULTIPLIER)
-      else if (valueOffset < 192) { valueOffset = valueOffset - 186; trim4Table.axisY[(5 - valueOffset)] = int(newValue) * TABLE_LOAD_MULTIPLIER; } //New value is on the Y (Load) axis of the table
+      break;
 
+    case seqFuelPage:
+      {
+        int tempOffset;
+        if (valueOffset < 36) { trim1Table.values[5 - (valueOffset / 6)][valueOffset % 6] = newValue; } //Trim1 values
+        else if (valueOffset < 42) { trim1Table.axisX[(valueOffset - 36)] = int(newValue) * TABLE_RPM_MULTIPLIER; } //New value is on the X (RPM) axis of the trim1 table. The RPM values sent by TunerStudio are divided by 100, need to multiply it back by 100 to make it correct (TABLE_RPM_MULTIPLIER)
+        else if (valueOffset < 48) { trim1Table.axisY[(5 - (valueOffset - 42))] = int(newValue) * TABLE_LOAD_MULTIPLIER; } //New value is on the Y (TPS) axis of the boost table
+        //Trim table 2
+        else if (valueOffset < 84) { tempOffset = valueOffset - 48; trim2Table.values[5 - (tempOffset / 6)][tempOffset % 6] = newValue; } //New value is part of the trim2 map
+        else if (valueOffset < 90) { tempOffset = valueOffset - 84; trim2Table.axisX[tempOffset] = int(newValue) * TABLE_RPM_MULTIPLIER; } //New value is on the X (RPM) axis of the table. The RPM values sent by TunerStudio are divided by 100, need to multiply it back by 100 to make it correct (TABLE_RPM_MULTIPLIER)
+        else if (valueOffset < 96) { tempOffset = valueOffset - 90; trim2Table.axisY[(5 - tempOffset)] = int(newValue) * TABLE_LOAD_MULTIPLIER; } //New value is on the Y (Load) axis of the table
+        //Trim table 3
+        else if (valueOffset < 132) { tempOffset = valueOffset - 96; trim3Table.values[5 - (tempOffset / 6)][tempOffset % 6] = newValue; } //New value is part of the trim2 map
+        else if (valueOffset < 138) { tempOffset = valueOffset - 132; trim3Table.axisX[tempOffset] = int(newValue) * TABLE_RPM_MULTIPLIER; } //New value is on the X (RPM) axis of the table. The RPM values sent by TunerStudio are divided by 100, need to multiply it back by 100 to make it correct (TABLE_RPM_MULTIPLIER)
+        else if (valueOffset < 144) { tempOffset = valueOffset - 138; trim3Table.axisY[(5 - tempOffset)] = int(newValue) * TABLE_LOAD_MULTIPLIER; } //New value is on the Y (Load) axis of the table
+        //Trim table 4
+        else if (valueOffset < 180) { tempOffset = valueOffset - 144; trim4Table.values[5 - (tempOffset / 6)][tempOffset % 6] = newValue; } //New value is part of the trim2 map
+        else if (valueOffset < 186) { tempOffset = valueOffset - 180; trim4Table.axisX[tempOffset] = int(newValue) * TABLE_RPM_MULTIPLIER; } //New value is on the X (RPM) axis of the table. The RPM values sent by TunerStudio are divided by 100, need to multiply it back by 100 to make it correct (TABLE_RPM_MULTIPLIER)
+        else if (valueOffset < 192) { tempOffset = valueOffset - 186; trim4Table.axisY[(5 - tempOffset)] = int(newValue) * TABLE_LOAD_MULTIPLIER; } //New value is on the Y (Load) axis of the table
+      }
       break;
 
     case canbusPage:
@@ -550,6 +541,7 @@ void sendPage(bool useChar)
   void* pnt_configPage;
   struct table3D currentTable;
   byte currentTitleIndex = 0;// This corresponds to the count up to the first char of a string in pageTitles
+  bool sendComplete = false; //Used to track whether all send operations are complete
 
   switch (currentPage)
   {
@@ -565,12 +557,13 @@ void sendPage(bool useChar)
         // currentTitleIndex = 27;
         if (useChar)
         {
+          uint16_t* pnt16_configPage;
           // To Display Values from Config Page 1
           // When casting to the __FlashStringHelper type Serial.println uses the same subroutine as when using the F macro
           Serial.println((const __FlashStringHelper *)&pageTitles[27]);//27 is the index to the first char in the second sting in pageTitles
           // The following loop displays in human readable form of all byte values in config page 1 up to but not including the first array.
           // incrementing void pointers is cumbersome. Thus we have "pnt_configPage = (byte *)pnt_configPage + 1"
-          for (pnt_configPage = &configPage1; pnt_configPage < &configPage1.wueValues[0]; pnt_configPage = (byte *)pnt_configPage + 1) Serial.println(*((byte *)pnt_configPage));
+          for (pnt_configPage = &configPage1; pnt_configPage < &configPage1.wueValues[0]; pnt_configPage = (byte *)pnt_configPage + 1) { Serial.println(*((byte *)pnt_configPage)); }
           for (byte x = 10; x; x--)// The x between the ';' has the same representation as the "x != 0" test or comparision
           {
             Serial.print(configPage1.wueValues[10 - x]);// This displays the values horizantially on the screen
@@ -581,15 +574,16 @@ void sendPage(bool useChar)
             Serial.println(*((byte *)pnt_configPage));// This displays all the byte values between the last array up to but not including the first unsigned int on config page 1
           }
           // The following loop displays four unsigned ints
-          for (pnt_configPage = &configPage1.inj1Ang; pnt_configPage < (unsigned int *)&configPage1.inj4Ang + 1; pnt_configPage = (unsigned int *)pnt_configPage + 1) Serial.println(*((unsigned int *)pnt_configPage));
+          for (pnt16_configPage = (uint16_t *)&configPage1.inj1Ang; pnt16_configPage < (uint16_t*)&configPage1.inj4Ang + 1; pnt16_configPage = (uint16_t*)pnt16_configPage + 1)
+          { Serial.println(*((uint16_t *)pnt16_configPage)); }
           // Following loop displays byte values between the unsigned ints
-          for (pnt_configPage = (unsigned int *)&configPage1.inj4Ang + 1; pnt_configPage < &configPage1.mapMax; pnt_configPage = (byte *)pnt_configPage + 1) Serial.println(*((byte *)pnt_configPage));
+          for (pnt_configPage = (uint16_t *)&configPage1.inj4Ang + 1; pnt_configPage < &configPage1.mapMax; pnt_configPage = (byte *)pnt_configPage + 1) { Serial.println(*((byte *)pnt_configPage)); }
           Serial.println(configPage1.mapMax);
           // Following loop displays remaining byte values of the page
-          for (pnt_configPage = (unsigned int *)&configPage1.mapMax + 1; pnt_configPage < (byte *)&configPage1 + page_size; pnt_configPage = (byte *)pnt_configPage + 1) Serial.println(*((byte *)pnt_configPage));
-          return;
+          for (pnt_configPage = (uint16_t *)&configPage1.mapMax + 1; pnt_configPage < (byte *)&configPage1 + page_size; pnt_configPage = (byte *)pnt_configPage + 1) { Serial.println(*((byte *)pnt_configPage)); }
+          sendComplete = true;
         }
-        else pnt_configPage = &configPage1; //Create a pointer to Page 1 in memory
+        else { pnt_configPage = &configPage1; } //Create a pointer to Page 1 in memory
         break;
       }
 
@@ -609,7 +603,7 @@ void sendPage(bool useChar)
           Serial.println((const __FlashStringHelper *)&pageTitles[56]);
           Serial.println(configPage2.triggerAngle);// configPsge2.triggerAngle is an int so just display it without complication
           // Following loop displays byte values after that first int up to but not including the first array in config page 2
-          for (pnt_configPage = (int *)&configPage2 + 1; pnt_configPage < &configPage2.taeBins[0]; pnt_configPage = (byte *)pnt_configPage + 1) Serial.println(*((byte *)pnt_configPage));
+          for (pnt_configPage = (int *)&configPage2 + 1; pnt_configPage < &configPage2.taeBins[0]; pnt_configPage = (byte *)pnt_configPage + 1) { Serial.println(*((byte *)pnt_configPage)); }
           for (byte y = 2; y; y--)// Displaying two equal sized arrays
           {
             byte * currentVar;// A placeholder for each array
@@ -620,9 +614,9 @@ void sendPage(bool useChar)
               currentVar = configPage2.taeValues;
             }
 
-            for (byte x = 4; x; x--)
+            for (byte j = 4; j; j--)
             {
-              Serial.print(currentVar[4 - x]);
+              Serial.print(currentVar[4 - j]);
               Serial.print(' ');
             }
             Serial.println();
@@ -644,9 +638,9 @@ void sendPage(bool useChar)
           {
             Serial.println(*((byte *)pnt_configPage));// Displaying remaining byte values of the page
           }
-          return;
+          sendComplete = true;
         }
-        else pnt_configPage = &configPage2; //Create a pointer to Page 2 in memory
+        else { pnt_configPage = &configPage2; } //Create a pointer to Page 2 in memory
         break;
       }
 
@@ -684,8 +678,9 @@ void sendPage(bool useChar)
           for (byte y = 2; y; y--)// and again
           {
             byte* currentVar;
-            if (y == 2) currentVar = configPage3.airDenBins;
-            else currentVar = configPage3.airDenRates;
+            if (y == 2) { currentVar = configPage3.airDenBins; }
+            else { currentVar = configPage3.airDenRates; }
+
             for (byte x = 9; x; x--)
             {
               Serial.print(currentVar[9 - x]);
@@ -698,9 +693,9 @@ void sendPage(bool useChar)
           {
             Serial.println(*((byte *)pnt_configPage));
           }
-          return;
+          sendComplete = true;
         }
-        else pnt_configPage = &configPage3; //Create a pointer to Page 3 in memory
+        else { pnt_configPage = &configPage3; } //Create a pointer to Page 3 in memory
         break;
       }
 
@@ -747,10 +742,10 @@ void sendPage(bool useChar)
             Serial.println();
           }
           // Following loop is for remaining byte value of page
-          for (pnt_configPage = (byte *)&configPage4.iacCrankBins[3] + 1; pnt_configPage < (byte *)&configPage4 + page_size; pnt_configPage = (byte *)pnt_configPage + 1) Serial.println(*((byte *)pnt_configPage));
-          return;
+          for (pnt_configPage = (byte *)&configPage4.iacCrankBins[3] + 1; pnt_configPage < (byte *)&configPage4 + page_size; pnt_configPage = (byte *)pnt_configPage + 1) { Serial.println(*((byte *)pnt_configPage)); }
+          sendComplete = true;
         }
-        else pnt_configPage = &configPage4; //Create a pointer to Page 4 in memory
+        else { pnt_configPage = &configPage4; } //Create a pointer to Page 4 in memory
         break;
       }
 
@@ -767,15 +762,15 @@ void sendPage(bool useChar)
           byte response[160]; //Bit hacky, but the size is: (8x8 + 8 + 8) + (8x8 + 8 + 8) = 160
 
           //Boost table
-          for (int x = 0; x < 64; x++) { response[x] = boostTable.values[7 - x / 8][x % 8]; }
+          for (int x = 0; x < 64; x++) { response[x] = boostTable.values[7 - (x / 8)][x % 8]; }
           for (int x = 64; x < 72; x++) { response[x] = byte(boostTable.axisX[(x - 64)] / TABLE_RPM_MULTIPLIER); }
           for (int y = 72; y < 80; y++) { response[y] = byte(boostTable.axisY[7 - (y - 72)]); }
           //VVT table
-          for (int x = 0; x < 64; x++) { response[x + 80] = vvtTable.values[7 - x / 8][x % 8]; }
+          for (int x = 0; x < 64; x++) { response[x + 80] = vvtTable.values[7 - (x / 8)][x % 8]; }
           for (int x = 64; x < 72; x++) { response[x + 80] = byte(vvtTable.axisX[(x - 64)] / TABLE_RPM_MULTIPLIER); }
           for (int y = 72; y < 80; y++) { response[y + 80] = byte(vvtTable.axisY[7 - (y - 72)]); }
           Serial.write((byte *)&response, sizeof(response));
-          return;
+          sendComplete = true;
         }
         break;
       }
@@ -813,7 +808,7 @@ void sendPage(bool useChar)
             }
             Serial.println("");
           }
-            return;
+          sendComplete = true;
           //Do.... Something?
         }
         else
@@ -822,23 +817,23 @@ void sendPage(bool useChar)
           byte response[192]; //Bit hacky, but the size is: (6x6 + 6 + 6) * 4 = 192
 
           //trim1 table
-          for (int x = 0; x < 36; x++) { response[x] = trim1Table.values[5 - x / 6][x % 6]; }
+          for (int x = 0; x < 36; x++) { response[x] = trim1Table.values[5 - (x / 6)][x % 6]; }
           for (int x = 36; x < 42; x++) { response[x] = byte(trim1Table.axisX[(x - 36)] / TABLE_RPM_MULTIPLIER); }
           for (int y = 42; y < 48; y++) { response[y] = byte(trim1Table.axisY[5 - (y - 42)] / TABLE_LOAD_MULTIPLIER); }
           //trim2 table
-          for (int x = 0; x < 36; x++) { response[x + 48] = trim2Table.values[5 - x / 6][x % 6]; }
+          for (int x = 0; x < 36; x++) { response[x + 48] = trim2Table.values[5 - (x / 6)][x % 6]; }
           for (int x = 36; x < 42; x++) { response[x + 48] = byte(trim2Table.axisX[(x - 36)] / TABLE_RPM_MULTIPLIER); }
           for (int y = 42; y < 48; y++) { response[y + 48] = byte(trim2Table.axisY[5 - (y - 42)] / TABLE_LOAD_MULTIPLIER); }
           //trim3 table
-          for (int x = 0; x < 36; x++) { response[x + 96] = trim3Table.values[5 - x / 6][x % 6]; }
+          for (int x = 0; x < 36; x++) { response[x + 96] = trim3Table.values[5 - (x / 6)][x % 6]; }
           for (int x = 36; x < 42; x++) { response[x + 96] = byte(trim3Table.axisX[(x - 36)] / TABLE_RPM_MULTIPLIER); }
           for (int y = 42; y < 48; y++) { response[y + 96] = byte(trim3Table.axisY[5 - (y - 42)] / TABLE_LOAD_MULTIPLIER); }
           //trim4 table
-          for (int x = 0; x < 36; x++) { response[x + 144] = trim4Table.values[5 - x / 6][x % 6]; }
+          for (int x = 0; x < 36; x++) { response[x + 144] = trim4Table.values[5 - (x / 6)][x % 6]; }
           for (int x = 36; x < 42; x++) { response[x + 144] = byte(trim4Table.axisX[(x - 36)] / TABLE_RPM_MULTIPLIER); }
           for (int y = 42; y < 48; y++) { response[y + 144] = byte(trim4Table.axisY[5 - (y - 42)] / TABLE_LOAD_MULTIPLIER); }
           Serial.write((byte *)&response, sizeof(response));
-          return;
+          sendComplete = true;
         }
         break;
       }
@@ -854,126 +849,129 @@ void sendPage(bool useChar)
           {
             Serial.println(*((byte *)pnt_configPage));// Displaying byte values of config page 3 up to but not including the first array
           }
-          return;
+          sendComplete = true;
         }
-        else pnt_configPage = &configPage10; //Create a pointer to Page 10 in memory
+        else { pnt_configPage = &configPage10; } //Create a pointer to Page 10 in memory
         break;
       }
 
     default:
       {
         Serial.println(F("\nPage has not been implemented yet. Change to another page."));
-        return;
+        sendComplete = true;
         break;
       }
   }
-  if (isMap)
+  if(!sendComplete)
   {
-    if (useChar)
+    if (isMap)
     {
-      do //This is a do while loop that kicks in for the boostvvtPage
+      if (useChar)
       {
-        const char spaceChar = ' ';
-        /*while(pageTitles[currentTitleIndex])
+        do //This is a do while loop that kicks in for the boostvvtPage
         {
-         Serial.print(pageTitles[currentTitleIndex]);
-         currentTitleIndex++;
-        }*/
-        Serial.println((const __FlashStringHelper *)&pageTitles[currentTitleIndex]);// F macro hack
-        Serial.println();
-        for (int y = 0; y < currentTable.ySize; y++)
-        {
-          byte axisY = byte(currentTable.axisY[y]);
-          if (axisY < 100)
+          const char spaceChar = ' ';
+          /*while(pageTitles[currentTitleIndex])
           {
-            Serial.write(spaceChar);
-            if (axisY < 10)
+           Serial.print(pageTitles[currentTitleIndex]);
+           currentTitleIndex++;
+          }*/
+          Serial.println((const __FlashStringHelper *)&pageTitles[currentTitleIndex]);// F macro hack
+          Serial.println();
+          for (int y = 0; y < currentTable.ySize; y++)
+          {
+            byte axisY = byte(currentTable.axisY[y]);
+            if (axisY < 100)
             {
               Serial.write(spaceChar);
-            }
-          }
-          Serial.print(axisY);// Vertical Bins
-          Serial.write(spaceChar);
-          for (int x = 0; x < currentTable.xSize; x++)
-          {
-            byte value = currentTable.values[y][x];
-            if (value < 100)
-            {
-              Serial.write(spaceChar);
-              if (value < 10)
+              if (axisY < 10)
               {
                 Serial.write(spaceChar);
               }
             }
-            Serial.print(value);
+            Serial.print(axisY);// Vertical Bins
+            Serial.write(spaceChar);
+            for (int i = 0; i < currentTable.xSize; i++)
+            {
+              byte value = currentTable.values[y][i];
+              if (value < 100)
+              {
+                Serial.write(spaceChar);
+                if (value < 10)
+                {
+                  Serial.write(spaceChar);
+                }
+              }
+              Serial.print(value);
+              Serial.write(spaceChar);
+            }
+            Serial.println();
+          }
+          Serial.print(F("    "));
+          for (int x = 0; x < currentTable.xSize; x++)// Horizontal bins
+          {
+            byte axisX = byte(currentTable.axisX[x] / 100);
+            if (axisX < 100)
+            {
+              Serial.write(spaceChar);
+              if (axisX < 10)
+              {
+                Serial.write(spaceChar);
+              }
+            }
+            Serial.print(axisX);
             Serial.write(spaceChar);
           }
           Serial.println();
-        }
-        Serial.print(F("    "));
-        for (int x = 0; x < currentTable.xSize; x++)// Horizontal bins
-        {
-          byte axisX = byte(currentTable.axisX[x] / 100);
-          if (axisX < 100)
+          if(currentTitleIndex == 121) //Check to see if on boostTable
           {
-            Serial.write(spaceChar);
-            if (axisX < 10)
-            {
-              Serial.write(spaceChar);
-            }
+            currentTitleIndex = 132; //Change over to vvtTable mid display
+            currentTable = vvtTable;
           }
-          Serial.print(axisX);
-          Serial.write(spaceChar);
-        }
-        Serial.println();
-        if(currentTitleIndex == 121) //Check to see if on boostTable
-        {
-          currentTitleIndex = 132; //Change over to vvtTable mid display
-          currentTable = vvtTable;
-        }
-        else currentTitleIndex = 0;
-      }while(currentTitleIndex == 132); //Should never loop unless going to display vvtTable
+          else currentTitleIndex = 0;
+        }while(currentTitleIndex == 132); //Should never loop unless going to display vvtTable
+      }
+      else
+      {
+        //Need to perform a translation of the values[yaxis][xaxis] into the MS expected format
+        //MS format has origin (0,0) in the bottom left corner, we use the top left for efficiency reasons
+        byte response[MAP_PAGE_SIZE];
+
+        for (int x = 0; x < 256; x++) { response[x] = currentTable.values[15 - (x / 16)][x % 16]; } //This is slightly non-intuitive, but essentially just flips the table vertically (IE top line becomes the bottom line etc). Columns are unchanged. Every 16 loops, manually call loop() to avoid potential misses
+        //loop();
+        for (int x = 256; x < 272; x++) { response[x] = byte(currentTable.axisX[(x - 256)] / TABLE_RPM_MULTIPLIER); }  //RPM Bins for VE table (Need to be dvidied by 100)
+        //loop();
+        for (int y = 272; y < 288; y++) { response[y] = byte(currentTable.axisY[15 - (y - 272)] / TABLE_LOAD_MULTIPLIER); } //MAP or TPS bins for VE table
+        //loop();
+        Serial.write((byte *)&response, sizeof(response));
+      }
     }
     else
     {
-      //Need to perform a translation of the values[yaxis][xaxis] into the MS expected format
-      //MS format has origin (0,0) in the bottom left corner, we use the top left for efficiency reasons
-      byte response[map_page_size];
+      /*if(useChar)
+      {
+       while(pageTitles[currentTitleIndex])
+       {
+        Serial.print(pageTitles[currentTitleIndex]);
+        currentTitleIndex++;
+       }
+       Serial.println();
+       for(byte x=0;x<page_size;x++) Serial.println(*((byte *)pnt_configPage + x));
+      }
+      else
+      {*/
+      //All other bytes can simply be copied from the config table
+      //byte response[npage_size[currentPage]];
+      for (byte x = 0; x < npage_size[currentPage]; x++)
+      {
+        //response[x] = *((byte *)pnt_configPage + x);
+        Serial.write(*((byte *)pnt_configPage + x)); //Each byte is simply the location in memory of the configPage + the offset + the variable number (x)
+      }
 
-      for (int x = 0; x < 256; x++) { response[x] = currentTable.values[15 - x / 16][x % 16]; } //This is slightly non-intuitive, but essentially just flips the table vertically (IE top line becomes the bottom line etc). Columns are unchanged. Every 16 loops, manually call loop() to avoid potential misses
-      //loop();
-      for (int x = 256; x < 272; x++) { response[x] = byte(currentTable.axisX[(x - 256)] / TABLE_RPM_MULTIPLIER); }  //RPM Bins for VE table (Need to be dvidied by 100)
-      //loop();
-      for (int y = 272; y < 288; y++) { response[y] = byte(currentTable.axisY[15 - (y - 272)] / TABLE_LOAD_MULTIPLIER); } //MAP or TPS bins for VE table
-      //loop();
-      Serial.write((byte *)&response, sizeof(response));
-    }
-  }
-  else
-  {
-    /*if(useChar)
-    {
-     while(pageTitles[currentTitleIndex])
-     {
-      Serial.print(pageTitles[currentTitleIndex]);
-      currentTitleIndex++;
-     }
-     Serial.println();
-     for(byte x=0;x<page_size;x++) Serial.println(*((byte *)pnt_configPage + x));
-    }
-    else
-    {*/
-    //All other bytes can simply be copied from the config table
-    byte response[npage_size[currentPage]];
-    for (byte x = 0; x < npage_size[currentPage]; x++)
-    {
-      response[x] = *((byte *)pnt_configPage + x); //Each byte is simply the location in memory of the configPage + the offset + the variable number (x)
-    }
-
-    Serial.write((byte *)&response, npage_size[currentPage]);
-    // }
-  }
-  return;
+      //Serial.write((byte *)&response, npage_size[currentPage]);
+      // }
+    } //isMap
+  } //sendComplete
 }
 
 /*
@@ -1012,9 +1010,7 @@ void receiveCalibration(byte tableID)
       break;
 
     default:
-      return; //Should never get here, but if we do, just fail back to main loop
-      //pnt_TargetTable = (table2D *)&o2CalibrationTable;
-      //break;
+      break; //Should never get here, but if we do, just fail back to main loop
   }
 
   //1024 value pairs are sent. We have to receive them all, but only use every second one (We only store 512 calibratino table entries to save on EEPROM space)
@@ -1085,33 +1081,33 @@ Send 256 tooth log entries
 void sendToothLog(bool useChar)
 {
   //We need TOOTH_LOG_SIZE number of records to send to TunerStudio. If there aren't that many in the buffer then we just return and wait for the next call
-  if (toothHistoryIndex < TOOTH_LOG_SIZE) {
-    return;  //This should no longer ever occur since the flagging system was put in place
-  }
-  unsigned int tempToothHistory[TOOTH_LOG_BUFFER]; //Create a temporary array that will contain a copy of what is in the main toothHistory array
-
-  //Copy the working history into the temporary buffer array. This is done so that, if the history loops whilst the values are being sent over serial, it doesn't affect the values
-  memcpy( (void*)tempToothHistory, (void*)toothHistory, sizeof(tempToothHistory) );
-  toothHistoryIndex = 0; //Reset the history index
-
-  //Loop only needs to go to half the buffer size
-  if (useChar)
+  if (toothHistoryIndex >= TOOTH_LOG_SIZE) //Sanity check. Flagging system means this should always be true
   {
-    for (int x = 0; x < TOOTH_LOG_SIZE; x++)
+    unsigned int tempToothHistory[TOOTH_LOG_BUFFER]; //Create a temporary array that will contain a copy of what is in the main toothHistory array
+
+    //Copy the working history into the temporary buffer array. This is done so that, if the history loops whilst the values are being sent over serial, it doesn't affect the values
+    memcpy( (void*)tempToothHistory, (void*)toothHistory, sizeof(tempToothHistory) );
+    toothHistoryIndex = 0; //Reset the history index
+
+    //Loop only needs to go to half the buffer size
+    if (useChar)
     {
-      Serial.println(tempToothHistory[x]);
+      for (int x = 0; x < TOOTH_LOG_SIZE; x++)
+      {
+        Serial.println(tempToothHistory[x]);
+      }
     }
-  }
-  else
-  {
-    for (int x = 0; x < TOOTH_LOG_SIZE; x++)
+    else
     {
-      Serial.write(highByte(tempToothHistory[x]));
-      Serial.write(lowByte(tempToothHistory[x]));
+      for (int x = 0; x < TOOTH_LOG_SIZE; x++)
+      {
+        Serial.write(highByte(tempToothHistory[x]));
+        Serial.write(lowByte(tempToothHistory[x]));
+      }
+      BIT_CLEAR(currentStatus.squirt, BIT_SQUIRT_TOOTHLOG1READY);
     }
-    BIT_CLEAR(currentStatus.squirt, BIT_SQUIRT_TOOTHLOG1READY);
+    toothLogRead = true;
   }
-  toothLogRead = true;
 }
 
 void testComm()
@@ -1141,10 +1137,10 @@ void commandButtons()
       BIT_SET(currentStatus.testOutputs, 1);
       break;
     case 513: // cmd group is for injector1 on actions
-      if(BIT_CHECK(currentStatus.testOutputs, 1)){digitalWrite(pinInjector1, HIGH);}
+      if( BIT_CHECK(currentStatus.testOutputs, 1) == 1 ){ digitalWrite(pinInjector1, HIGH); }
       break;
     case 514: // cmd group is for injector1 off actions
-      if(BIT_CHECK(currentStatus.testOutputs, 1)){digitalWrite(pinInjector1, LOW);}
+      if( BIT_CHECK(currentStatus.testOutputs, 1) == 1 ){digitalWrite(pinInjector1, LOW);}
       break;
     case 515: // cmd group is for injector1 50% dc actions
       //for (byte dcloop = 0; dcloop < 11; dcloop++)
@@ -1156,67 +1152,69 @@ void commandButtons()
       //}
       break;
     case 516: // cmd group is for injector2 on actions
-        if(BIT_CHECK(currentStatus.testOutputs, 1)){digitalWrite(pinInjector2, HIGH);}
+        if( BIT_CHECK(currentStatus.testOutputs, 1) == 1){ digitalWrite(pinInjector2, HIGH); }
       break;
     case 517: // cmd group is for injector2 off actions
-        if(BIT_CHECK(currentStatus.testOutputs, 1)){digitalWrite(pinInjector2, LOW);}
+        if( BIT_CHECK(currentStatus.testOutputs, 1) == 1 ){ digitalWrite(pinInjector2, LOW); }
       break;
     case 518: // cmd group is for injector2 50%dc actions
 
       break;
     case 519: // cmd group is for injector3 on actions
-        if(BIT_CHECK(currentStatus.testOutputs, 1)){digitalWrite(pinInjector3, HIGH);}
+        if( BIT_CHECK(currentStatus.testOutputs, 1) == 1 ) { digitalWrite(pinInjector3, HIGH); }
       break;
     case 520: // cmd group is for injector3 off actions
-        if(BIT_CHECK(currentStatus.testOutputs, 1)){digitalWrite(pinInjector3, LOW);}
+        if( BIT_CHECK(currentStatus.testOutputs, 1) == 1 ) { digitalWrite(pinInjector3, LOW); }
       break;
     case 521: // cmd group is for injector3 50%dc actions
 
       break;
     case 522: // cmd group is for injector4 on actions
-        if(BIT_CHECK(currentStatus.testOutputs, 1)){digitalWrite(pinInjector4, HIGH);}
+        if( BIT_CHECK(currentStatus.testOutputs, 1) == 1 ){ digitalWrite(pinInjector4, HIGH); }
       break;
     case 523: // cmd group is for injector4 off actions
-        if(BIT_CHECK(currentStatus.testOutputs, 1)){digitalWrite(pinInjector4, LOW);}
+        if( BIT_CHECK(currentStatus.testOutputs, 1) == 1 ){ digitalWrite(pinInjector4, LOW); }
       break;
     case 524: // cmd group is for injector4 50% dc actions
 
       break;
     case 769: // cmd group is for spark1 on actions
-        if(BIT_CHECK(currentStatus.testOutputs, 1)){digitalWrite(pinCoil1, HIGH);}
+        if( BIT_CHECK(currentStatus.testOutputs, 1) == 1 ) { digitalWrite(pinCoil1, HIGH); }
       break;
     case 770: // cmd group is for spark1 off actions
-        if(BIT_CHECK(currentStatus.testOutputs, 1)){digitalWrite(pinCoil1, LOW);}
+        if( BIT_CHECK(currentStatus.testOutputs, 1) == 1 ) { digitalWrite(pinCoil1, LOW); }
       break;
     case 771: // cmd group is for spark1 50%dc actions
 
       break;
     case 772: // cmd group is for spark2 on actions
-        if(BIT_CHECK(currentStatus.testOutputs, 1)){digitalWrite(pinCoil2, HIGH);}
+        if( BIT_CHECK(currentStatus.testOutputs, 1) == 1 ) { digitalWrite(pinCoil2, HIGH); }
       break;
     case 773: // cmd group is for spark2 off actions
-        if(BIT_CHECK(currentStatus.testOutputs, 1)){digitalWrite(pinCoil2, LOW);}
+        if( BIT_CHECK(currentStatus.testOutputs, 1) == 1 ) { digitalWrite(pinCoil2, LOW); }
       break;
     case 774: // cmd group is for spark2 50%dc actions
 
       break;
     case 775: // cmd group is for spark3 on actions
-        if(BIT_CHECK(currentStatus.testOutputs, 1)){digitalWrite(pinCoil3, HIGH);}
+        if( BIT_CHECK(currentStatus.testOutputs, 1) == 1 ) { digitalWrite(pinCoil3, HIGH); }
       break;
     case 776: // cmd group is for spark3 off actions
-        if(BIT_CHECK(currentStatus.testOutputs, 1)){digitalWrite(pinCoil3, LOW);}
+        if( BIT_CHECK(currentStatus.testOutputs, 1) == 1 ) { digitalWrite(pinCoil3, LOW); }
       break;
     case 777: // cmd group is for spark3 50%dc actions
 
       break;
     case 778: // cmd group is for spark4 on actions
-        if(BIT_CHECK(currentStatus.testOutputs, 1)){digitalWrite(pinCoil4, HIGH);}
+        if( BIT_CHECK(currentStatus.testOutputs, 1) == 1 ) { digitalWrite(pinCoil4, HIGH); }
       break;
     case 779: // cmd group is for spark4 off actions
-        if(BIT_CHECK(currentStatus.testOutputs, 1)){digitalWrite(pinCoil4, LOW);}
+        if( BIT_CHECK(currentStatus.testOutputs, 1) == 1 ) { digitalWrite(pinCoil4, LOW); }
       break;
     case 780: // cmd group is for spark4 50%dc actions
 
+    default:
       break;
   }
-}
+}
+
diff --git a/speeduino/corrections.h b/speeduino/corrections.h
index 5a28e50d..7da66cf6 100644
--- a/speeduino/corrections.h
+++ b/speeduino/corrections.h
@@ -20,6 +20,7 @@ static inline byte correctionIATDensity(); //Inlet temp density correction
 static inline byte correctionLaunch(); //Launch control correction
 static inline bool correctionDFCO(); //Decelleration fuel cutoff
 
+int SetIgnStartAngle(int IgnDegrees);
 int8_t correctionsIgn(int8_t advance);
 static inline int8_t correctionFixedTiming(int8_t);
 static inline int8_t correctionCrankingFixedTiming(int8_t);
@@ -31,4 +32,4 @@ static inline int8_t correctionSoftFlatShift(int8_t);
 
 uint16_t correctionsDwell(uint16_t dwell);
 
-#endif // CORRECTIONS_H
+#endif // CORRECTIONS_H
diff --git a/speeduino/corrections.ino b/speeduino/corrections.ino
index 48c798be..71552e66 100644
--- a/speeduino/corrections.ino
+++ b/speeduino/corrections.ino
@@ -313,6 +313,12 @@ static inline byte correctionAFRClosedLoop()
 }
 
 //******************************** IGNITION ADVANCE CORRECTIONS ********************************
+int SetIgnStartAngle(int IgnDegrees)
+{
+  IgnDegrees += CRANK_ANGLE_MAX_IGN - currentStatus.advance - dwellAngle;
+  if(IgnDegrees > CRANK_ANGLE_MAX_IGN) { return IgnDegrees - CRANK_ANGLE_MAX_IGN; }
+  else { return IgnDegrees; }
+}
 
 int8_t correctionsIgn(int8_t advance)
 {
@@ -419,4 +425,4 @@ uint16_t correctionsDwell(uint16_t dwell)
     dwell = (revolutionTime / pulsesPerRevolution) - (configPage2.sparkDur * 100);
   }
   return dwell;
-}
+}
diff --git a/speeduino/globals.h b/speeduino/globals.h
index 45bd1084..002b5443 100644
--- a/speeduino/globals.h
+++ b/speeduino/globals.h
@@ -11,8 +11,13 @@
   #define BOARD_NR_GPIO_PINS 34
 #elif defined(STM32_MCU_SERIES) || defined(_VARIANT_ARDUINO_STM32_)
   #define CORE_STM32
-  #define LED_BUILTIN 33
-
+  #if defined (ARDUINO_ARCH_STM32F1)
+    #define LED_BUILTIN 33
+  #elif defined (ARDUINO_ARCH_STM32F4)
+    #define LED_BUILTIN PA6
+  #endif
+  
+  extern "C" char* sbrk(int incr); //Used to freeRam
   inline unsigned char  digitalPinToInterrupt(unsigned char Interrupt_pin) { return Interrupt_pin; } //This isn't included in the stm32duino libs (yet)
   #define portOutputRegister(port) (volatile byte *)( &(port->regs->ODR) ) //These are defined in STM32F1/variants/generic_stm32f103c/variant.h but return a non byte* value
   #define portInputRegister(port) (volatile byte *)( &(port->regs->IDR) ) //These are defined in STM32F1/variants/generic_stm32f103c/variant.h but return a non byte* value
@@ -109,7 +114,7 @@ const byte data_structure_version = 2; //This identifies the data structure when
 const byte page_size = 64;
 const int npage_size[11] = {0,288,64,288,64,288,64,64,160,192,128};
 //const byte page10_size = 128;
-const int map_page_size = 288;
+#define MAP_PAGE_SIZE 288
 
 struct table3D fuelTable; //16x16 fuel map
 struct table3D ignitionTable; //16x16 ignition map
diff --git a/speeduino/maths.h b/speeduino/maths.h
index 7daa0e7b..c3fad7bd 100644
--- a/speeduino/maths.h
+++ b/speeduino/maths.h
@@ -1,7 +1,7 @@
 #ifndef MATH_H
 #define MATH_H
 
-int fastMap1023toX(unsigned long, int);
+int fastResize(unsigned int, int);
 unsigned long percentage(byte, unsigned long);
 
-#endif
+#endif
diff --git a/speeduino/maths.ino b/speeduino/maths.ino
index 4c142533..08d00954 100644
--- a/speeduino/maths.ino
+++ b/speeduino/maths.ino
@@ -13,9 +13,13 @@ int fastMap(unsigned long x, int in_min, int in_max, int out_min, int out_max)
 //This is a common case because it means converting from a standard 10-bit analog input to a byte or 10-bit analog into 0-511 (Eg the temperature readings)
 //int fastMap1023toX(unsigned long x, int in_min, int in_max, int out_min, int out_max)
 //removed ununsed variables, in_min and out_min is aways 0, in_max is aways 1023
-int fastMap1023toX(unsigned long x, int out_max)
+int fastResize(unsigned int x, int out_max)
 {
- return (x * out_max) >> 10;
+  #if defined(CORE_STM32)
+    return (x * out_max) >> 12;
+  #else
+    return (x * out_max) >> 10;
+  #endif
 }
 
 /*
@@ -64,16 +68,18 @@ int divs100(long n) {
 // return q + (r > 99);
 }
 
-//Unsigned divide by 100
 unsigned long divu100(unsigned long n) {
-  //return (n / 100); // No difference with this on/off
- unsigned long q, r;
- q = (n >> 1) + (n >> 3) + (n >> 6) - (n >> 10) +
- (n >> 12) + (n >> 13) - (n >> 16);
- q = q + (q >> 20);
- q = q >> 6;
- r = n - q*100;
- return q + ((r + 28) >> 7);
+  #if defined(CORE_STM32)
+    return (n / 100); // No difference with this on/off
+  #else
+    unsigned long q, r;
+    q = (n >> 1) + (n >> 3) + (n >> 6) - (n >> 10) +
+    (n >> 12) + (n >> 13) - (n >> 16);
+    q = q + (q >> 20);
+    q = q >> 6;
+    r = n - q*100;
+    return q + ((r + 28) >> 7);
+  #endif  
 // return q + (r > 99);
 }
 
@@ -94,4 +100,4 @@ inline long powint(int factor, unsigned int exponent)
    while (exponent--)
       product *= factor;
    return product;
-}
+}
diff --git a/speeduino/scheduler.ino b/speeduino/scheduler.ino
index 21ace743..878869a4 100644
--- a/speeduino/scheduler.ino
+++ b/speeduino/scheduler.ino
@@ -173,6 +173,7 @@ void initialiseSchedulers()
   Timer3.setMode(TIMER_CH2, TIMER_OUTPUT_COMPARE);
   Timer3.setMode(TIMER_CH3, TIMER_OUTPUT_COMPARE);
   Timer3.setMode(TIMER_CH4, TIMER_OUTPUT_COMPARE);
+  Timer1.setMode(TIMER_CH1, TIMER_OUTPUT_COMPARE); 
 
   Timer2.attachInterrupt(1, fuelSchedule1Interrupt);
   Timer2.attachInterrupt(2, fuelSchedule2Interrupt);
@@ -183,9 +184,7 @@ void initialiseSchedulers()
   Timer3.attachInterrupt(2, ignitionSchedule2Interrupt);
   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
+  Timer1.attachInterrupt(1, ignitionSchedule5Interrupt);
 
 #endif
 
diff --git a/speeduino/sensors.ino b/speeduino/sensors.ino
index d38e614e..be5032f9 100644
--- a/speeduino/sensors.ino
+++ b/speeduino/sensors.ino
@@ -62,7 +62,7 @@ void instanteneousMAPReading()
 
   currentStatus.mapADC = ADC_FILTER(tempReading, ADCFILTER_MAP, currentStatus.mapADC); //Very weak filter
 
-  currentStatus.MAP = fastMap1023toX(currentStatus.mapADC, configPage1.mapMax); //Get the current MAP value
+  currentStatus.MAP = fastResize(currentStatus.mapADC, configPage1.mapMax); //Get the current MAP value
 
 
 }
@@ -107,7 +107,7 @@ void readMAP()
         if (MAPrunningValue == 0 || MAPcount == 0) {  instanteneousMAPReading(); return; }
 
         currentStatus.mapADC = ldiv(MAPrunningValue, MAPcount).quot;
-        currentStatus.MAP = fastMap1023toX(currentStatus.mapADC, configPage1.mapMax); //Get the current MAP value
+        currentStatus.MAP = fastResize(currentStatus.mapADC, configPage1.mapMax); //Get the current MAP value
         MAPcurRev = currentStatus.startRevolutions; //Reset the current rev count
         MAPrunningValue = 0;
         MAPcount = 0;
@@ -137,7 +137,7 @@ void readMAP()
       {
         //Reaching here means that the last cylce has completed and the MAP value should be calculated
         currentStatus.mapADC = MAPrunningValue;
-        currentStatus.MAP = fastMap1023toX(currentStatus.mapADC, configPage1.mapMax); //Get the current MAP value
+        currentStatus.MAP = fastResize(currentStatus.mapADC, configPage1.mapMax); //Get the current MAP value
         MAPcurRev = currentStatus.startRevolutions; //Reset the current rev count
         MAPrunningValue = 1023; //Reset the latest value so the next reading will always be lower
       }
@@ -150,10 +150,10 @@ void readTPS()
   currentStatus.TPSlast = currentStatus.TPS;
   currentStatus.TPSlast_time = currentStatus.TPS_time;
   #if defined(ANALOG_ISR)
-    byte tempTPS = fastMap1023toX(AnChannel[pinTPS-A0], 255); //Get the current raw TPS ADC value and map it into a byte
+    byte tempTPS = fastResize(AnChannel[pinTPS-A0], 255); //Get the current raw TPS ADC value and map it into a byte
   #else
     analogRead(pinTPS);
-    byte tempTPS = fastMap1023toX(analogRead(pinTPS), 255); //Get the current raw TPS ADC value and map it into a byte
+    byte tempTPS = fastResize(analogRead(pinTPS), 255); //Get the current raw TPS ADC value and map it into a byte
   #endif
   currentStatus.tpsADC = ADC_FILTER(tempTPS, ADCFILTER_TPS, currentStatus.tpsADC);
   //Check that the ADC values fall within the min and max ranges (Should always be the case, but noise can cause these to fluctuate outside the defined range).
@@ -167,10 +167,10 @@ void readTPS()
 void readCLT()
 {
   #if defined(ANALOG_ISR)
-    tempReading = fastMap1023toX(AnChannel[pinCLT-A0], 511); //Get the current raw CLT value
+    tempReading = fastResize(AnChannel[pinCLT-A0], 511); //Get the current raw CLT value
   #else
     tempReading = analogRead(pinCLT);
-    tempReading = fastMap1023toX(analogRead(pinCLT), 511); //Get the current raw CLT value
+    tempReading = fastResize(analogRead(pinCLT), 511); //Get the current raw CLT value
   #endif
   currentStatus.cltADC = ADC_FILTER(tempReading, ADCFILTER_CLT, currentStatus.cltADC);
   currentStatus.coolant = cltCalibrationTable[currentStatus.cltADC] - CALIBRATION_TEMPERATURE_OFFSET; //Temperature calibration values are stored as positive bytes. We subtract 40 from them to allow for negative temperatures
@@ -179,10 +179,10 @@ void readCLT()
 void readIAT()
 {
   #if defined(ANALOG_ISR)
-    tempReading = fastMap1023toX(AnChannel[pinIAT-A0], 511); //Get the current raw IAT value
+    tempReading = fastResize(AnChannel[pinIAT-A0], 511); //Get the current raw IAT value
   #else
     tempReading = analogRead(pinIAT);
-    tempReading = fastMap1023toX(analogRead(pinIAT), 511); //Get the current raw IAT value
+    tempReading = fastResize(analogRead(pinIAT), 511); //Get the current raw IAT value
   #endif
   currentStatus.iatADC = ADC_FILTER(tempReading, ADCFILTER_IAT, currentStatus.iatADC);
   currentStatus.IAT = iatCalibrationTable[currentStatus.iatADC] - CALIBRATION_TEMPERATURE_OFFSET;
@@ -191,10 +191,10 @@ void readIAT()
 void readO2()
 {
   #if defined(ANALOG_ISR)
-    tempReading = fastMap1023toX(AnChannel[pinO2-A0], 511); //Get the current O2 value.
+    tempReading = fastResize(AnChannel[pinO2-A0], 511); //Get the current O2 value.
   #else
     tempReading = analogRead(pinO2);
-    tempReading = fastMap1023toX(analogRead(pinO2), 511); //Get the current O2 value.
+    tempReading = fastResize(analogRead(pinO2), 511); //Get the current O2 value.
   #endif
   currentStatus.O2ADC = ADC_FILTER(tempReading, ADCFILTER_O2, currentStatus.O2ADC);
   currentStatus.O2 = o2CalibrationTable[currentStatus.O2ADC];
@@ -209,10 +209,10 @@ void readO2()
 void readBat()
 {
   #if defined(ANALOG_ISR)
-    tempReading = fastMap1023toX(AnChannel[pinBat-A0], 245); //Get the current raw Battery value. Permissible values are from 0v to 24.5v (245)
+    tempReading = fastResize(AnChannel[pinBat-A0], 245); //Get the current raw Battery value. Permissible values are from 0v to 24.5v (245)
   #else
     tempReading = analogRead(pinBat);
-    tempReading = fastMap1023toX(analogRead(pinBat), 245); //Get the current raw Battery value. Permissible values are from 0v to 24.5v (245)
+    tempReading = fastResize(analogRead(pinBat), 245); //Get the current raw Battery value. Permissible values are from 0v to 24.5v (245)
   #endif
   currentStatus.battery10 = ADC_FILTER(tempReading, ADCFILTER_BAT, currentStatus.battery10);
 }
@@ -224,4 +224,4 @@ void readBat()
 void flexPulse()
  {
    ++flexCounter;
- }
+ }
diff --git a/speeduino/speeduino.ino b/speeduino/speeduino.ino
index 5c457b97..69d1a5d0 100644
--- a/speeduino/speeduino.ino
+++ b/speeduino/speeduino.ino
@@ -133,6 +133,7 @@ void (*ign5StartFunction)();
 void (*ign5EndFunction)();
 
 int timePerDegree;
+int dwellAngle;
 byte degreesPerLoop; //The number of crank degrees that pass for each mainloop of the program
 volatile bool fpPrimed = false; //Tracks whether or not the fuel pump priming has been completed yet
 
@@ -152,7 +153,14 @@ void setup()
   table3D_setSize(&trim3Table, 6);
   table3D_setSize(&trim4Table, 6);
 
-  loadConfig();
+  #if defined(CORE_STM32)
+    pinMode(CS, OUTPUT);
+    digitalWrite(CS, HIGH); // disable
+    Spi.begin();
+    loadConfig_STM();
+  #else  
+    loadConfig();
+  #endif
   doUpdates(); //Check if any data items need updating (Occurs ith firmware updates)
 
   Serial.begin(115200);
@@ -1269,7 +1277,7 @@ void loop()
       else { currentStatus.dwell =  (configPage2.dwellRun * 100); }
       currentStatus.dwell = correctionsDwell(currentStatus.dwell);
 
-      int dwellAngle = (div(currentStatus.dwell, timePerDegree).quot ); //Convert the dwell time to dwell angle based on the current engine speed
+      dwellAngle = (div(currentStatus.dwell, timePerDegree).quot ); //Convert the dwell time to dwell angle based on the current engine speed
 
       //Calculate start angle for each channel
       //1 cylinder (Everyone gets this)
@@ -1281,61 +1289,45 @@ void loop()
       {
         //2 cylinders
         case 2:
-          ignition2StartAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance - dwellAngle;
-          if(ignition2StartAngle > CRANK_ANGLE_MAX_IGN) {ignition2StartAngle -= CRANK_ANGLE_MAX_IGN;}
+          ignition2StartAngle = SetIgnStartAngle(channel2IgnDegrees);
           break;
         //3 cylinders
         case 3:
-          ignition2StartAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance - dwellAngle;
-          if(ignition2StartAngle > CRANK_ANGLE_MAX_IGN) {ignition2StartAngle -= CRANK_ANGLE_MAX_IGN;}
+          ignition2StartAngle = SetIgnStartAngle(channel2IgnDegrees);
           ignition3StartAngle = channel3IgnDegrees + 360 - currentStatus.advance - dwellAngle;
           if(ignition3StartAngle > CRANK_ANGLE_MAX_IGN) {ignition3StartAngle -= CRANK_ANGLE_MAX_IGN;}
           break;
         //4 cylinders
         case 4:
-          ignition2StartAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance - dwellAngle;
-          if(ignition2StartAngle > CRANK_ANGLE_MAX_IGN) {ignition2StartAngle -= CRANK_ANGLE_MAX_IGN;}
+          ignition2StartAngle = SetIgnStartAngle(channel2IgnDegrees);
           if(ignition2StartAngle < 0) {ignition2StartAngle += CRANK_ANGLE_MAX_IGN;}
 
           if(configPage2.sparkMode == IGN_MODE_SEQUENTIAL)
           {
-            ignition3StartAngle = channel3IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance - dwellAngle;
-            if(ignition3StartAngle > CRANK_ANGLE_MAX_IGN) {ignition3StartAngle -= CRANK_ANGLE_MAX_IGN;}
-            ignition4StartAngle = channel4IgnDegrees + CRANK_ANGLE_MAX - currentStatus.advance - dwellAngle;
-            if(ignition4StartAngle > CRANK_ANGLE_MAX_IGN) {ignition4StartAngle -= CRANK_ANGLE_MAX_IGN;}
+            ignition3StartAngle = SetIgnStartAngle(channel3IgnDegrees);
+            ignition4StartAngle = SetIgnStartAngle(channel4IgnDegrees);
           }
           break;
         //5 cylinders
         case 5:
-          ignition2StartAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance - dwellAngle;
-          if(ignition2StartAngle > CRANK_ANGLE_MAX_IGN) {ignition2StartAngle -= CRANK_ANGLE_MAX_IGN;}
+          ignition2StartAngle = SetIgnStartAngle(channel2IgnDegrees);
           if(ignition2StartAngle < 0) {ignition2StartAngle += CRANK_ANGLE_MAX_IGN;}
 
-          ignition3StartAngle = channel3IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance - dwellAngle;
-          if(ignition3StartAngle > CRANK_ANGLE_MAX_IGN) {ignition3StartAngle -= CRANK_ANGLE_MAX_IGN;}
-
-          ignition4StartAngle = channel4IgnDegrees + CRANK_ANGLE_MAX - currentStatus.advance - dwellAngle;
-          if(ignition4StartAngle > CRANK_ANGLE_MAX_IGN) {ignition4StartAngle -= CRANK_ANGLE_MAX_IGN;}
-
-          ignition5StartAngle = channel5IgnDegrees + CRANK_ANGLE_MAX - currentStatus.advance - dwellAngle;
-          if(ignition5StartAngle > CRANK_ANGLE_MAX_IGN) {ignition5StartAngle -= CRANK_ANGLE_MAX_IGN;}
+          ignition3StartAngle = SetIgnStartAngle(channel3IgnDegrees);
+          ignition4StartAngle = SetIgnStartAngle(channel4IgnDegrees);
+          ignition5StartAngle = SetIgnStartAngle(channel5IgnDegrees);
 
           break;
         //6 cylinders
         case 6:
-          ignition2StartAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance - dwellAngle;
-          if(ignition2StartAngle > CRANK_ANGLE_MAX_IGN) {ignition2StartAngle -= CRANK_ANGLE_MAX_IGN;}
-          ignition3StartAngle = channel3IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance - dwellAngle;
-          if(ignition3StartAngle > CRANK_ANGLE_MAX_IGN) {ignition3StartAngle -= CRANK_ANGLE_MAX_IGN;}
+          ignition2StartAngle = SetIgnStartAngle(channel2IgnDegrees);
+          ignition3StartAngle = SetIgnStartAngle(channel3IgnDegrees);
           break;
         //8 cylinders
         case 8:
-          ignition2StartAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance - dwellAngle;
-          if(ignition2StartAngle > CRANK_ANGLE_MAX_IGN) {ignition2StartAngle -= CRANK_ANGLE_MAX_IGN;}
-          ignition3StartAngle = channel3IgnDegrees + CRANK_ANGLE_MAX - currentStatus.advance - dwellAngle;
-          if(ignition3StartAngle > CRANK_ANGLE_MAX_IGN) {ignition3StartAngle -= CRANK_ANGLE_MAX_IGN;}
-          ignition4StartAngle = channel4IgnDegrees + CRANK_ANGLE_MAX - currentStatus.advance - dwellAngle;
-          if(ignition4StartAngle > CRANK_ANGLE_MAX_IGN) {ignition4StartAngle -= CRANK_ANGLE_MAX_IGN;}
+          ignition2StartAngle = SetIgnStartAngle(channel2IgnDegrees);
+          ignition3StartAngle = SetIgnStartAngle(channel3IgnDegrees);
+          ignition4StartAngle = SetIgnStartAngle(channel4IgnDegrees);
           break;
 
         //Will hit the default case on 1 cylinder or >8 cylinders. Do nothing in these cases
@@ -1586,4 +1578,5 @@ void loop()
         }
       } //Ignition schedules on
     } //Has sync and RPM
-} //loop()
+} //loop()
+
diff --git a/speeduino/storage.h b/speeduino/storage.h
index b848a148..80275a81 100644
--- a/speeduino/storage.h
+++ b/speeduino/storage.h
@@ -1,11 +1,37 @@
 #ifndef STORAGE_H
 #define STORAGE_H
+#if defined(CORE_STM32)
+  #include <SPI.h>
+#endif
 
 void writeConfig();
 void loadConfig();
 void loadCalibration();
 void writeCalibration();
 
+#if defined(CORE_STM32)
+  // Based on code by Heather Dewey-Hagborg
+  // http://arduino.cc/en/Tutorial/SPIEEPROM
+
+  #define PAGE_SIZE 256 //W25Q16
+  #define SPI_MODE 0
+  #define CS 10 // chip select
+
+  // EEPROM opcodes
+  #define WREN 6 //Write Enable
+  #define WRDI 4 //Write Disable
+  #define RDSR 5 //Read Status Register-1
+  #define WRSR 1 //Write Status Register
+  #define READ 3 //Read Data
+  #define WRITE 2 //Page Program
+  //STM32F103 clones
+  #if defined (ARDUINO_ARCH_STM32F1)
+    SPIClass Spi(2); //STM32F103 series share SPI1 with analogic channels, use 2 instead.
+  #else
+    SPIClass Spi(1); //STM32F407 eeprom is on SPI1 port
+  #endif
+#endif
+
 /*
 Current layout of EEPROM data (Version 3) is as follows (All sizes are in bytes):
 |---------------------------------------------------|
@@ -122,4 +148,4 @@ Current layout of EEPROM data (Version 3) is as follows (All sizes are in bytes)
 #define EEPROM_CALIBRATION_IAT 3071
 #define EEPROM_CALIBRATION_CLT 3583
 
-#endif // STORAGE_H
+#endif // STORAGE_H
diff --git a/speeduino/storage.ino b/speeduino/storage.ino
index f7fa9421..974845e5 100644
--- a/speeduino/storage.ino
+++ b/speeduino/storage.ino
@@ -530,3 +530,513 @@ void writeCalibration()
   }
 
 }
+
+byte EEPROM_read(int EEPROM_address)
+{
+  //READ EEPROM
+  digitalWrite(CS, LOW); // EEPROM enable
+  Spi.send(READ); //transmit read opcode
+  Spi.send((uint8)(EEPROM_address>>8)); //send MSByte address first
+  Spi.send((uint8)(EEPROM_address)); //send LSByte address
+  int data = Spi.send(0xFF); //get data byte
+  digitalWrite(CS, HIGH); // EEPROM disable
+  return data;
+}
+
+void EEPROM_write(int EEPROM_address, byte data)
+{
+  //WRITE EEPROM
+  digitalWrite(CS, LOW); // EEPROM enable
+  Spi.send(WREN); // EEPROM write enable instruction
+  digitalWrite(CS, HIGH); // EEPROM disable
+  delay(2);
+  digitalWrite(CS, LOW); // EEPROM enable
+  Spi.send(WRITE); // EEPROM write instruction
+  Spi.send((uint8)(EEPROM_address>>8)); //send MSByte address first
+  Spi.send((uint8)(EEPROM_address)); //send LSByte address
+  Spi.send(data);
+  digitalWrite(CS, HIGH); // EEPROM disable
+}
+
+void EEPROM_update(int EEPROM_address, byte data)
+{
+  byte result = EEPROM_read(EEPROM_address);
+  if(result != data) { EEPROM_write(EEPROM_address, data); }
+}
+
+void writeConfig_STM()
+{
+  /*
+  We only ever write to the EEPROM where the new value is different from the currently stored byte
+  This is due to the limited write life of the EEPROM (Approximately 100,000 writes)
+  */
+
+  int offset;
+  //Create a pointer to the config page
+  byte* pnt_configPage;
+
+  //if(EEPROM.read(0) != data_structure_version) { EEPROM.write(,data_structure_version); }   //Write the data structure version
+  EEPROM_update(0, data_structure_version); //Write the data structure version
+  
+  /*---------------------------------------------------
+  | Fuel table (See storage.h for data layout) - Page 1
+  | 16x16 table itself + the 16 values along each of the axis
+  -----------------------------------------------------*/
+  EEPROM_update(EEPROM_CONFIG1_XSIZE, fuelTable.xSize); //Write the VE Tables RPM dimension size
+  EEPROM_update(EEPROM_CONFIG1_YSIZE, fuelTable.ySize); //Write the VE Tables MAP/TPS dimension size
+
+  for(int x=EEPROM_CONFIG1_MAP; x<EEPROM_CONFIG1_XBINS; x++)
+  {
+    EEPROM_update(x, fuelTable.values[15-offset/16][offset%16]); //Write the 16x16 map
+    offset++;
+  }
+  //RPM bins
+  for(int x=EEPROM_CONFIG1_XBINS; x<EEPROM_CONFIG1_YBINS; x++)
+  {
+    offset = x - EEPROM_CONFIG1_XBINS;
+    EEPROM_update(x, byte(fuelTable.axisX[offset]/TABLE_RPM_MULTIPLIER)); //RPM bins are divided by 100 and converted to a byte
+  }
+  //TPS/MAP bins
+  for(int x=EEPROM_CONFIG1_YBINS; x<EEPROM_CONFIG2_START; x++)
+  {
+    offset = x - EEPROM_CONFIG1_YBINS;
+    EEPROM_update(x, fuelTable.axisY[offset] / TABLE_LOAD_MULTIPLIER); //Table load is divided by 2 (Allows for MAP up to 511)
+  }
+  //That concludes the writing of the VE table
+  //*********************************************************************************************************************************************************************************
+
+  /*---------------------------------------------------
+  | Config page 2 (See storage.h for data layout)
+  | 64 byte long config table
+  -----------------------------------------------------*/
+  pnt_configPage = (byte *)&configPage1; //Create a pointer to Page 2 in memory
+  for(int x=EEPROM_CONFIG2_START; x<EEPROM_CONFIG2_END; x++)
+  {
+    EEPROM_update(x, *(pnt_configPage + byte(x - EEPROM_CONFIG2_START)));
+  }
+  //*********************************************************************************************************************************************************************************
+
+  /*---------------------------------------------------
+  | Ignition table (See storage.h for data layout) - Page 1
+  | 16x16 table itself + the 16 values along each of the axis
+  -----------------------------------------------------*/
+  //Begin writing the Ignition table, basically the same thing as above
+  EEPROM_update(EEPROM_CONFIG3_XSIZE, ignitionTable.xSize); //Write the ignition Table RPM dimension size
+  EEPROM_update(EEPROM_CONFIG3_YSIZE, ignitionTable.ySize); //Write the ignition Table MAP/TPS dimension size
+
+  for(int x=EEPROM_CONFIG3_MAP; x<EEPROM_CONFIG3_XBINS; x++)
+  {
+    offset = x - EEPROM_CONFIG3_MAP;
+    EEPROM_update(x, ignitionTable.values[15-offset/16][offset%16]); //Write the 16x16 map with translation
+  }
+  //RPM bins
+  for(int x=EEPROM_CONFIG3_XBINS; x<EEPROM_CONFIG3_YBINS; x++)
+  {
+    offset = x - EEPROM_CONFIG3_XBINS;
+    EEPROM_update(x, byte(ignitionTable.axisX[offset]/TABLE_RPM_MULTIPLIER)); //RPM bins are divided by 100 and converted to a byte
+  }
+  //TPS/MAP bins
+  for(int x=EEPROM_CONFIG3_YBINS; x<EEPROM_CONFIG4_START; x++)
+  {
+    offset = x - EEPROM_CONFIG3_YBINS;
+    EEPROM_update(x, ignitionTable.axisY[offset]/TABLE_LOAD_MULTIPLIER); //Table load is divided by 2 (Allows for MAP up to 511)
+  }
+  //That concludes the writing of the IGN table
+  //*********************************************************************************************************************************************************************************
+
+  /*---------------------------------------------------
+  | Config page 2 (See storage.h for data layout)
+  | 64 byte long config table
+  -----------------------------------------------------*/
+  pnt_configPage = (byte *)&configPage2; //Create a pointer to Page 2 in memory
+  for(int x=EEPROM_CONFIG4_START; x<EEPROM_CONFIG4_END; x++)
+  {
+    EEPROM_update(x, *(pnt_configPage + byte(x - EEPROM_CONFIG4_START)));
+  }
+  //*********************************************************************************************************************************************************************************
+
+  /*---------------------------------------------------
+  | AFR table (See storage.h for data layout) - Page 5
+  | 16x16 table itself + the 16 values along each of the axis
+  -----------------------------------------------------*/
+  //Begin writing the Ignition table, basically the same thing as above
+  EEPROM_update(EEPROM_CONFIG5_XSIZE, afrTable.xSize); //Write the ignition Table RPM dimension size
+  EEPROM_update(EEPROM_CONFIG5_YSIZE, afrTable.ySize); //Write the ignition Table MAP/TPS dimension size
+
+  for(int x=EEPROM_CONFIG5_MAP; x<EEPROM_CONFIG5_XBINS; x++)
+  {
+    offset = x - EEPROM_CONFIG5_MAP;
+    EEPROM_update(x, afrTable.values[15-offset/16][offset%16]); //Write the 16x16 map
+  }
+  //RPM bins
+  for(int x=EEPROM_CONFIG5_XBINS; x<EEPROM_CONFIG5_YBINS; x++)
+  {
+    offset = x - EEPROM_CONFIG5_XBINS;
+    EEPROM_update(x, byte(afrTable.axisX[offset]/TABLE_RPM_MULTIPLIER)); //RPM bins are divided by 100 and converted to a byte
+  }
+  //TPS/MAP bins
+  for(int x=EEPROM_CONFIG5_YBINS; x<EEPROM_CONFIG6_START; x++)
+  {
+    offset = x - EEPROM_CONFIG5_YBINS;
+    EEPROM_update(x, afrTable.axisY[offset]/TABLE_LOAD_MULTIPLIER); //Table load is divided by 2 (Allows for MAP up to 511)
+  }
+  //That concludes the writing of the AFR table
+  //*********************************************************************************************************************************************************************************
+
+  /*---------------------------------------------------
+  | Config page 3 (See storage.h for data layout)
+  | 64 byte long config table
+  -----------------------------------------------------*/
+  pnt_configPage = (byte *)&configPage3; //Create a pointer to Page 3 in memory
+  for(int x=EEPROM_CONFIG6_START; x<EEPROM_CONFIG6_END; x++)
+  {
+    EEPROM_update(x, *(pnt_configPage + byte(x - EEPROM_CONFIG6_START)));
+  }
+  //*********************************************************************************************************************************************************************************
+
+  /*---------------------------------------------------
+  | Config page 4 (See storage.h for data layout)
+  | 64 byte long config table
+  -----------------------------------------------------*/
+  pnt_configPage = (byte *)&configPage4; //Create a pointer to Page 4 in memory
+  //The next 128 bytes can simply be pulled straight from the configTable
+  for(int x=EEPROM_CONFIG7_START; x<EEPROM_CONFIG7_END; x++)
+  {
+    EEPROM_update(x, *(pnt_configPage + byte(x - EEPROM_CONFIG7_START)));
+  }
+
+  /*---------------------------------------------------
+  | Boost and vvt tables (See storage.h for data layout) - Page 8
+  | 8x8 table itself + the 8 values along each of the axis
+  -----------------------------------------------------*/
+  //Begin writing the 2 tables, basically the same thing as above but we're doing these 2 together (2 tables per page instead of 1)
+  EEPROM_update(EEPROM_CONFIG8_XSIZE1, boostTable.xSize); //Write the boost Table RPM dimension size
+  EEPROM_update(EEPROM_CONFIG8_YSIZE1, boostTable.ySize); //Write the boost Table MAP/TPS dimension size
+  EEPROM_update(EEPROM_CONFIG8_XSIZE2, vvtTable.xSize); //Write the vvt Table RPM dimension size
+  EEPROM_update(EEPROM_CONFIG8_YSIZE2, vvtTable.ySize); //Write the vvt Table MAP/TPS dimension size
+
+  int y = EEPROM_CONFIG8_MAP2; //We do the 2 maps together in the same loop
+  for(int x=EEPROM_CONFIG8_MAP1; x<EEPROM_CONFIG8_XBINS1; x++)
+  {
+    offset = x - EEPROM_CONFIG8_MAP1;
+    EEPROM_update(x, boostTable.values[7-offset/8][offset%8]); //Write the 8x8 map
+    offset = y - EEPROM_CONFIG8_MAP2;
+    EEPROM_update(y, vvtTable.values[7-offset/8][offset%8]); //Write the 8x8 map
+    y++;
+  }
+  //RPM bins
+  y = EEPROM_CONFIG8_XBINS2;
+  for(int x=EEPROM_CONFIG8_XBINS1; x<EEPROM_CONFIG8_YBINS1; x++)
+  {
+    offset = x - EEPROM_CONFIG8_XBINS1;
+    EEPROM_update(x, byte(boostTable.axisX[offset]/TABLE_RPM_MULTIPLIER)); //RPM bins are divided by 100 and converted to a byte
+    offset = y - EEPROM_CONFIG8_XBINS2;
+    EEPROM_update(x, byte(vvtTable.axisX[offset]/TABLE_RPM_MULTIPLIER)); //RPM bins are divided by 100 and converted to a byte
+    y++;
+  }
+  //TPS/MAP bins
+  y=EEPROM_CONFIG8_YBINS2;
+  for(int x=EEPROM_CONFIG8_YBINS1; x<EEPROM_CONFIG8_XSIZE2; x++)
+  {
+    offset = x - EEPROM_CONFIG8_YBINS1;
+    EEPROM_update(x, boostTable.axisY[offset]); //TABLE_LOAD_MULTIPLIER is NOT used for boost as it is TPS based (0-100)
+    offset = y - EEPROM_CONFIG8_YBINS2;
+    EEPROM_update(y, vvtTable.axisY[offset]); //TABLE_LOAD_MULTIPLIER is NOT used for VVT as it is TPS based (0-100)
+    y++;
+  }
+
+  /*---------------------------------------------------
+  | Fuel trim tables (See storage.h for data layout) - Page 9
+  | 6x6 tables itself + the 6 values along each of the axis
+  -----------------------------------------------------*/
+  //Begin writing the 2 tables, basically the same thing as above but we're doing these 2 together (2 tables per page instead of 1)
+  EEPROM_update(EEPROM_CONFIG9_XSIZE1,trim1Table.xSize); //Write the boost Table RPM dimension size
+  EEPROM_update(EEPROM_CONFIG9_YSIZE1,trim1Table.ySize); //Write the boost Table MAP/TPS dimension size
+  EEPROM_update(EEPROM_CONFIG9_XSIZE2,trim2Table.xSize); //Write the boost Table RPM dimension size
+  EEPROM_update(EEPROM_CONFIG9_YSIZE2,trim2Table.ySize); //Write the boost Table MAP/TPS dimension size
+  EEPROM_update(EEPROM_CONFIG9_XSIZE3,trim3Table.xSize); //Write the boost Table RPM dimension size
+  EEPROM_update(EEPROM_CONFIG9_YSIZE3,trim3Table.ySize); //Write the boost Table MAP/TPS dimension size
+  EEPROM_update(EEPROM_CONFIG9_XSIZE4,trim4Table.xSize); //Write the boost Table RPM dimension size
+  EEPROM_update(EEPROM_CONFIG9_YSIZE4,trim4Table.ySize); //Write the boost Table MAP/TPS dimension size
+
+  y = EEPROM_CONFIG9_MAP2; //We do the 4 maps together in the same loop
+  int z = EEPROM_CONFIG9_MAP3; //We do the 4 maps together in the same loop
+  int i = EEPROM_CONFIG9_MAP4; //We do the 4 maps together in the same loop
+  for(int x=EEPROM_CONFIG9_MAP1; x<EEPROM_CONFIG9_XBINS1; x++)
+  {
+    offset = x - EEPROM_CONFIG9_MAP1;
+    EEPROM_update(x, trim1Table.values[5-offset/6][offset%6]);  //Write the 6x6 map
+    offset = y - EEPROM_CONFIG9_MAP2;
+    EEPROM_update(y, trim2Table.values[5-offset/6][offset%6]);  //Write the 6x6 map
+    offset = z - EEPROM_CONFIG9_MAP3;
+    EEPROM_update(z, trim3Table.values[5-offset/6][offset%6]);  //Write the 6x6 map
+    offset = i - EEPROM_CONFIG9_MAP4;
+    EEPROM_update(i, trim4Table.values[5-offset/6][offset%6]);  //Write the 6x6 map
+    y++;
+    z++;
+    i++;
+  }
+  //RPM bins
+  y = EEPROM_CONFIG9_XBINS2;
+  z = EEPROM_CONFIG9_XBINS3;
+  i = EEPROM_CONFIG9_XBINS4;
+  for(int x=EEPROM_CONFIG9_XBINS1; x<EEPROM_CONFIG9_YBINS1; x++)
+  {
+    offset = x - EEPROM_CONFIG9_XBINS1;
+    EEPROM_update(x, byte(trim1Table.axisX[offset]/TABLE_RPM_MULTIPLIER)); //RPM bins are divided by 100 and converted to a byte
+    offset = y - EEPROM_CONFIG9_XBINS2;
+    EEPROM_update(y, byte(trim2Table.axisX[offset]/TABLE_RPM_MULTIPLIER)); //RPM bins are divided by 100 and converted to a byte
+    offset = z - EEPROM_CONFIG9_XBINS3;
+    EEPROM_update(z, byte(trim3Table.axisX[offset]/TABLE_RPM_MULTIPLIER)); //RPM bins are divided by 100 and converted to a byte
+    offset = i - EEPROM_CONFIG9_XBINS4;
+    EEPROM_update(i, byte(trim4Table.axisX[offset]/TABLE_RPM_MULTIPLIER)); //RPM bins are divided by 100 and converted to a byte
+    y++;
+    z++;
+    i++;
+  }
+  //TPS/MAP bins
+  y=EEPROM_CONFIG9_YBINS2;
+  z=EEPROM_CONFIG9_YBINS3;
+  i=EEPROM_CONFIG9_YBINS4;
+  for(int x=EEPROM_CONFIG9_YBINS1; x<EEPROM_CONFIG9_XSIZE2; x++)
+  {
+    offset = x - EEPROM_CONFIG9_YBINS1;
+    EEPROM_update(x, trim1Table.axisY[offset]/TABLE_LOAD_MULTIPLIER); //Table load is divided by 2 (Allows for MAP up to 511)
+    offset = y - EEPROM_CONFIG9_YBINS2;
+    EEPROM_update(y, trim2Table.axisY[offset]/TABLE_LOAD_MULTIPLIER); //Table load is divided by 2 (Allows for MAP up to 511)
+    offset = z - EEPROM_CONFIG9_YBINS3;
+    EEPROM_update(z, trim3Table.axisY[offset]/TABLE_LOAD_MULTIPLIER); //Table load is divided by 2 (Allows for MAP up to 511)
+    offset = i - EEPROM_CONFIG9_YBINS4;
+    EEPROM_update(i, trim4Table.axisY[offset]/TABLE_LOAD_MULTIPLIER); //Table load is divided by 2 (Allows for MAP up to 511)
+    y++;
+    z++;
+    i++;
+  }
+  //*********************************************************************************************************************************************************************************
+
+  /*---------------------------------------------------
+  | Config page 10 (See storage.h for data layout)
+  | 128 byte long config table
+  -----------------------------------------------------*/
+  pnt_configPage = (byte *)&configPage10; //Create a pointer to Page 10 in memory
+  for(int x=EEPROM_CONFIG10_START; x<EEPROM_CONFIG10_END; x++)
+  {
+    EEPROM_update(x, *(pnt_configPage + byte(x - EEPROM_CONFIG10_START)));
+  }
+  //*********************************************************************************************************************************************************************************
+
+}
+
+void loadConfig_STM()
+{
+  int offset;
+  //Create a pointer to the config page
+  byte* pnt_configPage;
+
+  //Fuel table (See storage.h for data layout)
+  //fuelTable.xSize = EEPROM.read(EEPROM_CONFIG1_XSIZE); //Read the VE Tables RPM dimension size
+  //fuelTable.ySize = EEPROM.read(EEPROM_CONFIG1_YSIZE); //Read the VE Tables MAP/TPS dimension size
+  for(int x=EEPROM_CONFIG1_MAP; x<EEPROM_CONFIG1_XBINS; x++)
+  {
+    offset = x - EEPROM_CONFIG1_MAP;
+    fuelTable.values[15-offset/16][offset%16] = EEPROM_read(x); //Read the 8x8 map
+  }
+  //RPM bins
+  for(int x=EEPROM_CONFIG1_XBINS; x<EEPROM_CONFIG1_YBINS; x++)
+  {
+    offset = x - EEPROM_CONFIG1_XBINS;
+    fuelTable.axisX[offset] = (EEPROM_read(x) * TABLE_RPM_MULTIPLIER); //RPM bins are divided by 100 when stored. Multiply them back now
+  }
+  //TPS/MAP bins
+  for(int x=EEPROM_CONFIG1_YBINS; x<EEPROM_CONFIG2_START; x++)
+  {
+    offset = x - EEPROM_CONFIG1_YBINS;
+    fuelTable.axisY[offset] = EEPROM_read(x) * TABLE_LOAD_MULTIPLIER;
+  }
+
+  pnt_configPage = (byte *)&configPage1; //Create a pointer to Page 1 in memory
+  for(int x=EEPROM_CONFIG2_START; x<EEPROM_CONFIG2_END; x++)
+  {
+    *(pnt_configPage + byte(x - EEPROM_CONFIG2_START)) = EEPROM_read(x);
+  }
+  //That concludes the reading of the VE table
+
+  //*********************************************************************************************************************************************************************************
+  //IGNITION CONFIG PAGE (2)
+
+  //Begin writing the Ignition table, basically the same thing as above
+  //ignitionTable.xSize = EEPROM_read(EEPROM_CONFIG2_XSIZE); //Read the ignition Table RPM dimension size (Currently not supproted)
+  //ignitionTable.ySize = EEPROM_read(EEPROM_CONFIG2_YSIZE); //Read the ignition Table MAP/TPS dimension size (Currently not supproted)
+
+  for(int x=EEPROM_CONFIG3_MAP; x<EEPROM_CONFIG3_XBINS; x++)
+  {
+    offset = x - EEPROM_CONFIG3_MAP;
+    ignitionTable.values[15-offset/16][offset%16] = EEPROM_read(x); //Read the 8x8 map
+  }
+  //RPM bins
+  for(int x=EEPROM_CONFIG3_XBINS; x<EEPROM_CONFIG3_YBINS; x++)
+  {
+    offset = x - EEPROM_CONFIG3_XBINS;
+    ignitionTable.axisX[offset] = (EEPROM_read(x) * TABLE_RPM_MULTIPLIER); //RPM bins are divided by 100 when stored. Multiply them back now
+  }
+  //TPS/MAP bins
+  for(int x=EEPROM_CONFIG3_YBINS; x<EEPROM_CONFIG4_START; x++)
+  {
+    offset = x - EEPROM_CONFIG3_YBINS;
+    ignitionTable.axisY[offset] = EEPROM_read(x) * TABLE_LOAD_MULTIPLIER; //Table load is divided by 2 (Allows for MAP up to 511)
+  }
+
+  pnt_configPage = (byte *)&configPage2; //Create a pointer to Page 2 in memory
+  for(int x=EEPROM_CONFIG4_START; x<EEPROM_CONFIG4_END; x++)
+  {
+    *(pnt_configPage + byte(x - EEPROM_CONFIG4_START)) = EEPROM_read(x);
+  }
+
+  //*********************************************************************************************************************************************************************************
+  //AFR TARGET CONFIG PAGE (3)
+
+  //Begin writing the Ignition table, basically the same thing as above
+  //ignitionTable.xSize = EEPROM_read(EEPROM_CONFIG2_XSIZE); //Read the ignition Table RPM dimension size (Currently not supproted)
+  //ignitionTable.ySize = EEPROM_read(EEPROM_CONFIG2_YSIZE); //Read the ignition Table MAP/TPS dimension size (Currently not supproted)
+
+  for(int x=EEPROM_CONFIG5_MAP; x<EEPROM_CONFIG5_XBINS; x++)
+  {
+    offset = x - EEPROM_CONFIG5_MAP;
+    afrTable.values[15-offset/16][offset%16] = EEPROM_read(x); //Read the 16x16 map
+  }
+  //RPM bins
+  for(int x=EEPROM_CONFIG5_XBINS; x<EEPROM_CONFIG5_YBINS; x++)
+  {
+    offset = x - EEPROM_CONFIG5_XBINS;
+    afrTable.axisX[offset] = (EEPROM_read(x) * TABLE_RPM_MULTIPLIER); //RPM bins are divided by 100 when stored. Multiply them back now
+  }
+  //TPS/MAP bins
+  for(int x=EEPROM_CONFIG5_YBINS; x<EEPROM_CONFIG6_START; x++)
+  {
+    offset = x - EEPROM_CONFIG5_YBINS;
+    afrTable.axisY[offset] = EEPROM_read(x) * TABLE_LOAD_MULTIPLIER; //Table load is divided by 2 (Allows for MAP up to 511)
+  }
+
+  pnt_configPage = (byte *)&configPage3; //Create a pointer to Page 2 in memory
+  for(int x=EEPROM_CONFIG6_START; x<EEPROM_CONFIG6_END; x++)
+  {
+    *(pnt_configPage + byte(x - EEPROM_CONFIG6_START)) = EEPROM_read(x);
+  }
+
+  //*********************************************************************************************************************************************************************************
+
+  //CONFIG PAGE (4)                                                                                                                                           //############
+  pnt_configPage = (byte *)&configPage4; //Create a pointer to Page 3 in memory
+  //Begin writing the Ignition table, basically the same thing as above
+  //ignitionTable.xSize = EEPROM_read(EEPROM_CONFIG2_XSIZE); //Read the ignition Table RPM dimension size (Currently not supproted)
+  //ignitionTable.ySize = EEPROM_read(EEPROM_CONFIG2_YSIZE); //Read the ignition Table MAP/TPS dimension size (Currently not supproted)
+
+  //The next 64 bytes can simply be pulled straight from the configTable
+  for(int x=EEPROM_CONFIG7_START; x<EEPROM_CONFIG7_END; x++)
+  {
+    *(pnt_configPage + byte(x - EEPROM_CONFIG7_START)) = EEPROM_read(x);
+  }
+
+  //*********************************************************************************************************************************************************************************
+  // Boost and vvt tables load
+  int y = EEPROM_CONFIG8_MAP2;
+  for(int x=EEPROM_CONFIG8_MAP1; x<EEPROM_CONFIG8_XBINS1; x++)
+  {
+    offset = x - EEPROM_CONFIG8_MAP1;
+    boostTable.values[7-offset/8][offset%8] = EEPROM_read(x); //Read the 8x8 map
+    offset = y - EEPROM_CONFIG8_MAP2;
+    vvtTable.values[7-offset/8][offset%8] = EEPROM_read(y); //Read the 8x8 map
+    y++;
+  }
+
+  //RPM bins
+  y = EEPROM_CONFIG8_XBINS2;
+  for(int x=EEPROM_CONFIG8_XBINS1; x<EEPROM_CONFIG8_YBINS1; x++)
+  {
+    offset = x - EEPROM_CONFIG8_XBINS1;
+    boostTable.axisX[offset] = (EEPROM_read(x) * TABLE_RPM_MULTIPLIER); //RPM bins are divided by 100 when stored. Multiply them back now
+    offset = y - EEPROM_CONFIG8_XBINS2;
+    vvtTable.axisX[offset] = (EEPROM_read(y) * TABLE_RPM_MULTIPLIER); //RPM bins are divided by 100 when stored. Multiply them back now
+    y++;
+  }
+
+  //TPS/MAP bins
+  y = EEPROM_CONFIG8_YBINS2;
+  for(int x=EEPROM_CONFIG8_YBINS1; x<EEPROM_CONFIG8_XSIZE2; x++)
+  {
+    offset = x - EEPROM_CONFIG8_YBINS1;
+    boostTable.axisY[offset] = EEPROM_read(x); //TABLE_LOAD_MULTIPLIER is NOT used for boost as it is TPS based (0-100)
+    offset = y - EEPROM_CONFIG8_YBINS2;
+    vvtTable.axisY[offset] = EEPROM_read(y); //TABLE_LOAD_MULTIPLIER is NOT used for VVT as it is TPS based (0-100)
+    y++;
+  }
+
+  //*********************************************************************************************************************************************************************************
+  // Fuel trim tables load
+  y = EEPROM_CONFIG9_MAP2;
+  int z = EEPROM_CONFIG9_MAP3;
+  int i = EEPROM_CONFIG9_MAP4;
+  for(int x=EEPROM_CONFIG9_MAP1; x<EEPROM_CONFIG9_XBINS1; x++)
+  {
+    offset = x - EEPROM_CONFIG9_MAP1;
+    trim1Table.values[5-offset/6][offset%6] = EEPROM_read(x); //Read the 6x6 map
+    offset = y - EEPROM_CONFIG9_MAP2;
+    trim2Table.values[5-offset/6][offset%6] = EEPROM_read(y); //Read the 6x6 map
+    offset = z - EEPROM_CONFIG9_MAP3;
+    trim3Table.values[5-offset/6][offset%6] = EEPROM_read(z); //Read the 6x6 map
+    offset = i - EEPROM_CONFIG9_MAP4;
+    trim4Table.values[5-offset/6][offset%6] = EEPROM_read(i); //Read the 6x6 map
+    y++;
+    z++;
+    i++;
+  }
+
+  //RPM bins
+  y = EEPROM_CONFIG9_XBINS2;
+  z = EEPROM_CONFIG9_XBINS3;
+  i = EEPROM_CONFIG9_XBINS4;
+  for(int x=EEPROM_CONFIG9_XBINS1; x<EEPROM_CONFIG9_YBINS1; x++)
+  {
+    offset = x - EEPROM_CONFIG9_XBINS1;
+    trim1Table.axisX[offset] = (EEPROM_read(x) * TABLE_RPM_MULTIPLIER); //RPM bins are divided by 100 when stored. Multiply them back now
+    offset = y - EEPROM_CONFIG9_XBINS2;
+    trim2Table.axisX[offset] = (EEPROM_read(y) * TABLE_RPM_MULTIPLIER); //RPM bins are divided by 100 when stored. Multiply them back now
+    offset = z - EEPROM_CONFIG9_XBINS3;
+    trim3Table.axisX[offset] = (EEPROM_read(z) * TABLE_RPM_MULTIPLIER); //RPM bins are divided by 100 when stored. Multiply them back now
+    offset = i - EEPROM_CONFIG9_XBINS4;
+    trim4Table.axisX[offset] = (EEPROM_read(i) * TABLE_RPM_MULTIPLIER); //RPM bins are divided by 100 when stored. Multiply them back now
+    y++;
+    z++;
+    i++;
+  }
+
+  //TPS/MAP bins
+  y = EEPROM_CONFIG9_YBINS2;
+  z = EEPROM_CONFIG9_YBINS3;
+  i = EEPROM_CONFIG9_YBINS4;
+  for(int x=EEPROM_CONFIG9_YBINS1; x<EEPROM_CONFIG9_XSIZE2; x++)
+  {
+    offset = x - EEPROM_CONFIG9_YBINS1;
+    trim1Table.axisY[offset] = EEPROM_read(x) * TABLE_LOAD_MULTIPLIER; //Table load is divided by 2 (Allows for MAP up to 511)
+    offset = y - EEPROM_CONFIG9_YBINS2;
+    trim2Table.axisY[offset] = EEPROM_read(y) * TABLE_LOAD_MULTIPLIER; //Table load is divided by 2 (Allows for MAP up to 511)
+    offset = z - EEPROM_CONFIG9_YBINS3;
+    trim3Table.axisY[offset] = EEPROM_read(z) * TABLE_LOAD_MULTIPLIER; //Table load is divided by 2 (Allows for MAP up to 511)
+    offset = i - EEPROM_CONFIG9_YBINS4;
+    trim4Table.axisY[offset] = EEPROM_read(i) * TABLE_LOAD_MULTIPLIER; //Table load is divided by 2 (Allows for MAP up to 511)
+    y++;
+    z++;
+    i++;
+  }
+  //*********************************************************************************************************************************************************************************
+  //canbus control page load
+    pnt_configPage = (byte *)&configPage10; //Create a pointer to Page 10 in memory
+  for(int x=EEPROM_CONFIG10_START; x<EEPROM_CONFIG10_END; x++)
+  {
+    *(pnt_configPage + byte(x - EEPROM_CONFIG10_START)) = EEPROM_read(x);
+  }
+
+  //*********************************************************************************************************************************************************************************
+
+}
+
diff --git a/speeduino/table.ino b/speeduino/table.ino
index 9a9e57b9..1862fc0d 100644
--- a/speeduino/table.ino
+++ b/speeduino/table.ino
@@ -358,7 +358,7 @@ int get3DTableValue(struct table3D *fromTable, int Y, int X)
     int D = fromTable->values[yMax][xMax];
 
     //Check that all values aren't just the same (This regularly happens with things like the fuel trim maps)
-    if(A == B && A == C && A == D) { return A; }
+    if(A == D) { return A; } //yMin = yMax and xMin = xMax
    
     //Create some normalised position values
     //These are essentially percentages (between 0 and 1) of where the desired value falls between the nearest bins on each axis
@@ -379,9 +379,13 @@ int get3DTableValue(struct table3D *fromTable, int Y, int X)
     int r = (p * q) >> 8;
     return ( (A * m) + (B * n) + (C * o) + (D * r) ) >> 8;
 }
+
+
 /* Executed a benchmark on all options and this is the results
- * Stadard:226224 91 |FP Math:32240 91.89 |Clean code:34056 91, Number of loops:2500
- * 
+ * @Mega: Stadard:226224 91 |FP Math:32240 91.89 |Clean code:34056 91, Number of loops:2500
+ * @STM32: Stadard:1624  91 |FP Math:85180 91.89 |Clean code:1001 91 , Number of loops:2500
+ * @STM32 Stadard:13000  loops/S |Clean code:15700 loops/S @8000RPM ~20.8% faster
+
 //This function pulls a value from a 3D table given a target for X and Y coordinates.
 //It performs a 2D linear interpolation as descibred in: http://www.megamanual.com/v22manual/ve_tuner.pdf
 float get3DTableValueF(struct table3D *fromTable, int Y, int X)
@@ -466,47 +470,38 @@ int get3DTableValueS(struct table3D *fromTable, int Y, int X)
   int yMaxValue, yMinValue;
   int xMaxValue, xMinValue;
 
-  if(fromTable->lastXMin==0) {fromTable->lastXMin=fromTable->xSize-1;}
+  if(fromTable->lastXMin == 0) {fromTable->lastXMin = fromTable->xSize-1;}
   else {xMin = fromTable->lastXMin;}
-  if(fromTable->lastYMin==0) {fromTable->lastYMin=fromTable->ySize-1;}
+  if(fromTable->lastYMin == 0) {fromTable->lastYMin = fromTable->ySize-1;}
   else {yMin = fromTable->lastYMin;}
 
-  if(xMin>fromTable->xSize-1)
-  { 
-    fromTable->lastXMin = fromTable->xSize-1;
-    xMin = fromTable->xSize-1;
-  }  
-  if(yMin>fromTable->ySize-1)
-  { 
-    fromTable->lastYMin = fromTable->ySize-1;
-    yMin = fromTable->ySize-1;
-  }    
-    
   do  //RPM axis
   {
-    if(X>=fromTable->axisX[xMin]) {break;}
+    if(X >= fromTable->axisX[xMin]) {break;}
     xMin--;
   }while(1);
-  fromTable->lastXMin = xMin + 1;
+  
   do  //MAP axis
   {
-    if(Y<=fromTable->axisY[yMin]) {break;}
+    if(Y <= fromTable->axisY[yMin]) {break;}
     yMin--;
   }while(1);
-  fromTable->lastYMin = yMin + 1;
   
   xMax = xMin + 1;
   yMax = yMin + 1;
-  if (xMax>fromTable->xSize-1)  //Overflow protection
+  if (xMax > fromTable->xSize-1)  //Overflow protection
   {
     xMax = fromTable->xSize-1;
     xMin = xMax - 1;
   }
-  if (yMax>fromTable->ySize-1)  //Overflow protection
+  fromTable->lastXMin = xMin + 1;
+
+  if (yMax > fromTable->ySize-1)  //Overflow protection
   {
     yMax = fromTable->ySize-1;
     yMin = yMax - 1;
   }
+  fromTable->lastYMin = yMin + 1;
 
   yMaxValue = fromTable->axisY[yMax];
   yMinValue = fromTable->axisY[yMin];
@@ -517,6 +512,7 @@ int get3DTableValueS(struct table3D *fromTable, int Y, int X)
   int B = fromTable->values[yMin][xMax];
   int C = fromTable->values[yMax][xMin];
   int D = fromTable->values[yMax][xMax];
+  if(A == D) { return A; }
 
   p = ((long)(X - xMinValue) << 8) / (xMaxValue - xMinValue); //(RPM - RPM[1])/(RPM[2]- RPM[1])
   q = 256 - (((long)(Y - yMaxValue) << 8) / (yMinValue - yMaxValue)); //(MAP - MAP[2])/(MAP[2]- MAP[1])
@@ -527,4 +523,4 @@ int get3DTableValueS(struct table3D *fromTable, int Y, int X)
   int r = (p * q) >> 8;
   return ( (A * m) + (B * n) + (C * o) + (D * r) ) >> 8; 
 }
-*/
+*/
diff --git a/speeduino/updates.ino b/speeduino/updates.ino
index cbcff403..ef92caeb 100644
--- a/speeduino/updates.ino
+++ b/speeduino/updates.ino
@@ -10,23 +10,45 @@ void doUpdates()
 {
   #define CURRENT_DATA_VERSION    3
 
-  //May 2017 firmware introduced a -40 offset on the ignition table. Update that table to +40
-  if(EEPROM.read(EEPROM_DATA_VERSION) == 2)
-  {
-    for(int x=0; x<16; x++)
+  #if defined(CORE_AVR) | defined(CORE_TEENSY)
+    //May 2017 firmware introduced a -40 offset on the ignition table. Update that table to +40
+    if(EEPROM.read(EEPROM_DATA_VERSION) == 2)
     {
-      for(int y=0; y<16; y++)
+      for(int x=0; x<16; x++)
       {
-        ignitionTable.values[x][y] = ignitionTable.values[x][y] + 40;
+        for(int y=0; y<16; y++)
+        {
+          ignitionTable.values[x][y] = ignitionTable.values[x][y] + 40;
+        }
       }
+      writeConfig();
+      EEPROM.write(EEPROM_DATA_VERSION, 3);
     }
-    writeConfig();
-    EEPROM.write(EEPROM_DATA_VERSION, 3);
-  }
 
-  //Final check is always for 255 and 0 (Brand new arduino)
-  if(EEPROM.read(EEPROM_DATA_VERSION) == 0 || EEPROM.read(EEPROM_DATA_VERSION) == 255)
-  {
-    EEPROM.write(EEPROM_DATA_VERSION, CURRENT_DATA_VERSION);
-  }
-}
+    //Final check is always for 255 and 0 (Brand new arduino)
+    if(EEPROM.read(EEPROM_DATA_VERSION) == 0 || EEPROM.read(EEPROM_DATA_VERSION) == 255)
+    {
+      EEPROM.write(EEPROM_DATA_VERSION, CURRENT_DATA_VERSION);
+    }
+  #elif defined(CORE_STM)
+    //May 2017 firmware introduced a -40 offset on the ignition table. Update that table to +40
+    if(EEPROM_read(EEPROM_DATA_VERSION) == 2)
+    {
+      for(int x=0; x<16; x++)
+      {
+        for(int y=0; y<16; y++)
+        {
+          ignitionTable.values[x][y] = ignitionTable.values[x][y] + 40;
+        }
+      }
+      writeConfig_STM();
+      EEPROM_write(EEPROM_DATA_VERSION, 3);
+    }
+
+    //Final check is always for 255 and 0 (Brand new arduino)
+    if(EEPROM_read(EEPROM_DATA_VERSION) == 0 || EEPROM_read(EEPROM_DATA_VERSION) == 255)
+    {
+      EEPROM_write(EEPROM_DATA_VERSION, CURRENT_DATA_VERSION);
+    }
+  #endif
+}
diff --git a/speeduino/utils.ino b/speeduino/utils.ino
index 2c7d8e38..e4894526 100644
--- a/speeduino/utils.ino
+++ b/speeduino/utils.ino
@@ -31,8 +31,8 @@ int freeRam ()
   // The difference is the free, available ram.
   return (uint16_t)stackTop - heapTop;
 #elif defined(CORE_STM32)
-  //Figure this out some_time
-  return 0;
+  char top = 't';
+  return &top - reinterpret_cast<char*>(sbrk(0));
 #endif
 }
 
@@ -51,13 +51,22 @@ void setPinMapping(byte boardID)
     #define A7  boardADCPins[7]
     #define A8  boardADCPins[8]
     #define A9  boardADCPins[9]
-    //STM32F1 have only 9 12bit adc
-    #define A10  boardADCPins[0]
-    #define A11  boardADCPins[1]
-    #define A12  boardADCPins[2]
-    #define A13  boardADCPins[3]
-    #define A14  boardADCPins[4]
-    #define A15  boardADCPins[5]
+    #if defined (ARDUINO_ARCH_STM32F4)
+      #define A10  boardADCPins[10]
+      #define A11  boardADCPins[11]
+      #define A12  boardADCPins[12]
+      #define A13  boardADCPins[13]
+      #define A14  boardADCPins[14]
+      #define A15  boardADCPins[15]
+    #else
+      //STM32F1 have only 9 12bit adc
+      #define A10  boardADCPins[0]
+      #define A11  boardADCPins[1]
+      #define A12  boardADCPins[2]
+      #define A13  boardADCPins[3]
+      #define A14  boardADCPins[4]
+      #define A15  boardADCPins[5]
+    #endif
   #endif
 
   switch (boardID)
@@ -185,8 +194,8 @@ void setPinMapping(byte boardID)
         pinStepperStep = 13; //Step pin for DRV8825 driver
         pinStepperEnable = 14; //Enable pin for DRV8825
         pinDisplayReset = 2; // OLED reset pin
-        pinFan = 1; //Pin for the fan output
-        pinFuelPump = 0; //Fuel pump output
+        pinFan = 0; //Pin for the fan output
+        pinFuelPump = 1; //Fuel pump output
         pinTachOut = 31; //Tacho output pin
         //external interrupt enabled pins
         pinFlex = 32; // Flex sensor (Must be external interrupt enabled)
@@ -261,8 +270,8 @@ void setPinMapping(byte boardID)
         pinStepperStep = 13; //Step pin for DRV8825 driver
         pinStepperEnable = 14; //Enable pin for DRV8825
         pinDisplayReset = 2; // OLED reset pin
-        pinFan = 1; //Pin for the fan output
-        pinFuelPump = 0; //Fuel pump output
+        pinFan = 0; //Pin for the fan output
+        pinFuelPump = 1; //Fuel pump output
         pinTachOut = 31; //Tacho output pin
         //external interrupt enabled pins
         pinFlex = 32; // Flex sensor (Must be external interrupt enabled)

From e9db1a2f78be2770c0a26717f1753939eb54f42d Mon Sep 17 00:00:00 2001
From: "Vitor Moreno B. Sales" <vitor_boss@yahoo.com.br>
Date: Wed, 2 Aug 2017 22:46:48 -0300
Subject: [PATCH 02/10] New update to STM32

Using timer bits instead (mainLoopCount & 31) comparators.
Few changes to support another library on STM32
---
 speeduino/auxiliaries.ino |    1 -
 speeduino/cancomms.h      |    6 +-
 speeduino/cancomms.ino    |    2 +-
 speeduino/comms.h         |    2 +-
 speeduino/comms.ino       |   69 +-
 speeduino/corrections.h   |    3 +-
 speeduino/corrections.ino |  309 +++++----
 speeduino/decoders.h      |   13 +-
 speeduino/decoders.ino    | 1373 ++++++++++++++++++++++---------------
 speeduino/errors.ino      |   26 +-
 speeduino/globals.h       |   38 +-
 speeduino/idle.h          |    4 +-
 speeduino/idle.ino        |  135 ++--
 speeduino/maths.h         |    2 +-
 speeduino/maths.ino       |   98 +--
 speeduino/scheduledIO.h   |   31 +-
 speeduino/scheduledIO.ino |   57 +-
 speeduino/sensors.h       |    2 +-
 speeduino/sensors.ino     |  122 ++--
 speeduino/speeduino.ino   |  233 +++----
 speeduino/storage.h       |   26 -
 speeduino/storage.ino     |  561 +--------------
 speeduino/table.ino       |  533 +++++++-------
 speeduino/timers.ino      |   21 +-
 speeduino/updates.ino     |   52 +-
 speeduino/utils.h         |    4 +-
 speeduino/utils.ino       |  122 ++--
 27 files changed, 1799 insertions(+), 2046 deletions(-)

diff --git a/speeduino/auxiliaries.ino b/speeduino/auxiliaries.ino
index a0e560fb..45438685 100644
--- a/speeduino/auxiliaries.ino
+++ b/speeduino/auxiliaries.ino
@@ -156,4 +156,3 @@ void boostControl() { }
 void vvtControl() { }
 
 #endif
-
diff --git a/speeduino/cancomms.h b/speeduino/cancomms.h
index eba8135b..0d31a4b5 100644
--- a/speeduino/cancomms.h
+++ b/speeduino/cancomms.h
@@ -15,7 +15,8 @@ uint8_t Glow, Ghigh;
 #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
   HardwareSerial &CANSerial = Serial3;
 #elif defined(CORE_STM32)
-  HardwareSerial &CANSerial = Serial2;
+  SerialUART &CANSerial = Serial2;
+  //HardwareSerial &CANSerial = Serial2;
 #elif defined(CORE_TEENSY)
   HardwareSerial &CANSerial = Serial2;
 #endif
@@ -23,5 +24,4 @@ uint8_t Glow, Ghigh;
 void canCommand();//This is the heart of the Command Line Interpeter.  All that needed to be done was to make it human readable.
 void sendCancommand(uint8_t cmdtype , uint16_t canadddress, uint8_t candata1, uint8_t candata2, uint16_t paramgroup);
 
-#endif // CANCOMMS_H
-
+#endif // CANCOMMS_H
diff --git a/speeduino/cancomms.ino b/speeduino/cancomms.ino
index f972b7a6..cac44fdc 100644
--- a/speeduino/cancomms.ino
+++ b/speeduino/cancomms.ino
@@ -101,6 +101,7 @@ void canCommand()
           offset = word(CANSerial.read(), tmp);
           tmp = CANSerial.read();
           length = word(CANSerial.read(), tmp);
+
           sendValues(offset, length, 3);
         }
         else
@@ -165,4 +166,3 @@ void sendCancommand(uint8_t cmdtype, uint16_t canaddress, uint8_t candata1, uint
         break;
     }
 }
-
diff --git a/speeduino/comms.h b/speeduino/comms.h
index 24f5100b..81e1016e 100644
--- a/speeduino/comms.h
+++ b/speeduino/comms.h
@@ -49,4 +49,4 @@ void sendToothLog(bool useChar);
 void testComm();
 void commandButtons();
 
-#endif // COMMS_H
+#endif // COMMS_H
diff --git a/speeduino/comms.ino b/speeduino/comms.ino
index 7b50cd67..b5b0128c 100644
--- a/speeduino/comms.ino
+++ b/speeduino/comms.ino
@@ -15,7 +15,7 @@ A detailed description of each call can be found at: http://www.msextra.com/doc/
 
 void command()
 {
-  if (!cmdPending) { currentCommand = Serial.read(); }
+  if (cmdPending == 0) { currentCommand = Serial.read(); }
 
   switch (currentCommand)
   {
@@ -23,8 +23,7 @@ void command()
     case 'A': // send x bytes of realtime values
       sendValues(0, packetSize, 0);   //send values to serial0
       break;
-
-
+      
     case 'B': // Burn current values to eeprom
       writeConfig();
       break;
@@ -80,12 +79,12 @@ void command()
       break;
 
     case 'S': // send code version
-      Serial.print("Speeduino 2017.05-dev");
+      Serial.print(displaySignature);Serial.print(".");Serial.print(TSfirmwareVersion);
       currentStatus.secl = 0; //This is required in TS3 due to its stricter timings
       break;
 
     case 'Q': // send code version
-      Serial.print("speeduino 201705-dev");
+      Serial.print(displaySignature);Serial.print(TSfirmwareVersion);
      break;
 
     case 'V': // send VE table and constants in binary
@@ -182,15 +181,15 @@ void command()
         tsCanId = Serial.read(); //Read the $tsCanId
         cmd = Serial.read(); // read the command
 
-        uint16_t offset, length;
+        uint16_t valueOffset, length;
         if(cmd == 0x30) //Send output channels command 0x30 is 48dec
         {
           byte tmp;
           tmp = Serial.read();
-          offset = word(Serial.read(), tmp);
+          valueOffset = word(Serial.read(), tmp);
           tmp = Serial.read();
           length = word(Serial.read(), tmp);
-          sendValues(offset, length, 0);
+          sendValues(valueOffset, length, 0);
         }
         else
         {
@@ -347,11 +346,12 @@ void sendValues(uint16_t offset, uint16_t packetLength, byte portNum)
   fullStatus[55] = lowByte(currentStatus.canin[7]);
   fullStatus[56] = highByte(currentStatus.canin[7]);
 
-  for(byte x=0; x<packetLength; x++)
-  {
-    if (portNum == 0) { Serial.write(fullStatus[offset+x]); }
-    else if (portNum == 3) { CANSerial.write(fullStatus[offset+x]); }
-  }
+  Serial.write((byte *)&fullStatus+offset, packetLength);
+  //for(byte x=0; x<packetLength; x++)
+  //{
+  //  if (portNum == 0) { Serial.write(fullStatus[offset+x]); }
+  //  else if (portNum == 3) { CANSerial.write(fullStatus[offset+x]); }
+  //}
 
 }
 
@@ -1016,7 +1016,7 @@ void receiveCalibration(byte tableID)
   //1024 value pairs are sent. We have to receive them all, but only use every second one (We only store 512 calibratino table entries to save on EEPROM space)
   //The values are sent as 2 byte ints, but we convert them to single bytes. Any values over 255 are capped at 255.
   int tempValue;
-  byte tempBuffer[2];
+  int tempBuffer;
   bool every2nd = true;
   int x;
   int counter = 0;
@@ -1034,10 +1034,10 @@ void receiveCalibration(byte tableID)
     else
     {
       while ( Serial.available() < 2 ) {}
-      tempBuffer[0] = Serial.read();
-      tempBuffer[1] = Serial.read();
+      tempBuffer = Serial.read();
+      tempBuffer |= Serial.read() << 8;
 
-      tempValue = div(int(word(tempBuffer[1], tempBuffer[0])), DIVISION_FACTOR).quot; //Read 2 bytes, convert to word (an unsigned int), convert to signed int. These values come through * 10 from Tuner Studio
+      tempValue = div(int(tempBuffer), DIVISION_FACTOR).quot; //Read 2 bytes, convert to word (an unsigned int), convert to signed int. These values come through * 10 from Tuner Studio
       tempValue = ((tempValue - 32) * 5) / 9; //Convert from F to C
     }
     tempValue = tempValue + OFFSET;
@@ -1137,10 +1137,10 @@ void commandButtons()
       BIT_SET(currentStatus.testOutputs, 1);
       break;
     case 513: // cmd group is for injector1 on actions
-      if( BIT_CHECK(currentStatus.testOutputs, 1) == 1 ){ digitalWrite(pinInjector1, HIGH); }
+      if( BIT_CHECK(currentStatus.testOutputs, 1) ){ digitalWrite(pinInjector1, HIGH); }
       break;
     case 514: // cmd group is for injector1 off actions
-      if( BIT_CHECK(currentStatus.testOutputs, 1) == 1 ){digitalWrite(pinInjector1, LOW);}
+      if( BIT_CHECK(currentStatus.testOutputs, 1) ){digitalWrite(pinInjector1, LOW);}
       break;
     case 515: // cmd group is for injector1 50% dc actions
       //for (byte dcloop = 0; dcloop < 11; dcloop++)
@@ -1152,69 +1152,68 @@ void commandButtons()
       //}
       break;
     case 516: // cmd group is for injector2 on actions
-        if( BIT_CHECK(currentStatus.testOutputs, 1) == 1){ digitalWrite(pinInjector2, HIGH); }
+        if( BIT_CHECK(currentStatus.testOutputs, 1) ){ digitalWrite(pinInjector2, HIGH); }
       break;
     case 517: // cmd group is for injector2 off actions
-        if( BIT_CHECK(currentStatus.testOutputs, 1) == 1 ){ digitalWrite(pinInjector2, LOW); }
+        if( BIT_CHECK(currentStatus.testOutputs, 1) ){ digitalWrite(pinInjector2, LOW); }
       break;
     case 518: // cmd group is for injector2 50%dc actions
 
       break;
     case 519: // cmd group is for injector3 on actions
-        if( BIT_CHECK(currentStatus.testOutputs, 1) == 1 ) { digitalWrite(pinInjector3, HIGH); }
+        if( BIT_CHECK(currentStatus.testOutputs, 1) ) { digitalWrite(pinInjector3, HIGH); }
       break;
     case 520: // cmd group is for injector3 off actions
-        if( BIT_CHECK(currentStatus.testOutputs, 1) == 1 ) { digitalWrite(pinInjector3, LOW); }
+        if( BIT_CHECK(currentStatus.testOutputs, 1) ) { digitalWrite(pinInjector3, LOW); }
       break;
     case 521: // cmd group is for injector3 50%dc actions
 
       break;
     case 522: // cmd group is for injector4 on actions
-        if( BIT_CHECK(currentStatus.testOutputs, 1) == 1 ){ digitalWrite(pinInjector4, HIGH); }
+        if( BIT_CHECK(currentStatus.testOutputs, 1) ){ digitalWrite(pinInjector4, HIGH); }
       break;
     case 523: // cmd group is for injector4 off actions
-        if( BIT_CHECK(currentStatus.testOutputs, 1) == 1 ){ digitalWrite(pinInjector4, LOW); }
+        if( BIT_CHECK(currentStatus.testOutputs, 1) ){ digitalWrite(pinInjector4, LOW); }
       break;
     case 524: // cmd group is for injector4 50% dc actions
 
       break;
     case 769: // cmd group is for spark1 on actions
-        if( BIT_CHECK(currentStatus.testOutputs, 1) == 1 ) { digitalWrite(pinCoil1, HIGH); }
+        if( BIT_CHECK(currentStatus.testOutputs, 1) ) { digitalWrite(pinCoil1, HIGH); }
       break;
     case 770: // cmd group is for spark1 off actions
-        if( BIT_CHECK(currentStatus.testOutputs, 1) == 1 ) { digitalWrite(pinCoil1, LOW); }
+        if( BIT_CHECK(currentStatus.testOutputs, 1) ) { digitalWrite(pinCoil1, LOW); }
       break;
     case 771: // cmd group is for spark1 50%dc actions
 
       break;
     case 772: // cmd group is for spark2 on actions
-        if( BIT_CHECK(currentStatus.testOutputs, 1) == 1 ) { digitalWrite(pinCoil2, HIGH); }
+        if( BIT_CHECK(currentStatus.testOutputs, 1) ) { digitalWrite(pinCoil2, HIGH); }
       break;
     case 773: // cmd group is for spark2 off actions
-        if( BIT_CHECK(currentStatus.testOutputs, 1) == 1 ) { digitalWrite(pinCoil2, LOW); }
+        if( BIT_CHECK(currentStatus.testOutputs, 1) ) { digitalWrite(pinCoil2, LOW); }
       break;
     case 774: // cmd group is for spark2 50%dc actions
 
       break;
     case 775: // cmd group is for spark3 on actions
-        if( BIT_CHECK(currentStatus.testOutputs, 1) == 1 ) { digitalWrite(pinCoil3, HIGH); }
+        if( BIT_CHECK(currentStatus.testOutputs, 1) ) { digitalWrite(pinCoil3, HIGH); }
       break;
     case 776: // cmd group is for spark3 off actions
-        if( BIT_CHECK(currentStatus.testOutputs, 1) == 1 ) { digitalWrite(pinCoil3, LOW); }
+        if( BIT_CHECK(currentStatus.testOutputs, 1) ) { digitalWrite(pinCoil3, LOW); }
       break;
     case 777: // cmd group is for spark3 50%dc actions
 
       break;
     case 778: // cmd group is for spark4 on actions
-        if( BIT_CHECK(currentStatus.testOutputs, 1) == 1 ) { digitalWrite(pinCoil4, HIGH); }
+        if( BIT_CHECK(currentStatus.testOutputs, 1) ) { digitalWrite(pinCoil4, HIGH); }
       break;
     case 779: // cmd group is for spark4 off actions
-        if( BIT_CHECK(currentStatus.testOutputs, 1) == 1 ) { digitalWrite(pinCoil4, LOW); }
+        if( BIT_CHECK(currentStatus.testOutputs, 1) ) { digitalWrite(pinCoil4, LOW); }
       break;
     case 780: // cmd group is for spark4 50%dc actions
 
     default:
       break;
   }
-}
-
+}
diff --git a/speeduino/corrections.h b/speeduino/corrections.h
index 7da66cf6..5a28e50d 100644
--- a/speeduino/corrections.h
+++ b/speeduino/corrections.h
@@ -20,7 +20,6 @@ static inline byte correctionIATDensity(); //Inlet temp density correction
 static inline byte correctionLaunch(); //Launch control correction
 static inline bool correctionDFCO(); //Decelleration fuel cutoff
 
-int SetIgnStartAngle(int IgnDegrees);
 int8_t correctionsIgn(int8_t advance);
 static inline int8_t correctionFixedTiming(int8_t);
 static inline int8_t correctionCrankingFixedTiming(int8_t);
@@ -32,4 +31,4 @@ static inline int8_t correctionSoftFlatShift(int8_t);
 
 uint16_t correctionsDwell(uint16_t dwell);
 
-#endif // CORRECTIONS_H
+#endif // CORRECTIONS_H
diff --git a/speeduino/corrections.ino b/speeduino/corrections.ino
index 71552e66..1a47d740 100644
--- a/speeduino/corrections.ino
+++ b/speeduino/corrections.ino
@@ -74,7 +74,7 @@ byte correctionsFuel()
   if (currentStatus.launchCorrection != 100) { sumCorrections = (sumCorrections * currentStatus.launchCorrection); activeCorrections++; }
 
   bitWrite(currentStatus.squirt, BIT_SQUIRT_DFCO, correctionDFCO());
-  if ( bitRead(currentStatus.squirt, BIT_SQUIRT_DFCO) ) { sumCorrections = 0; }
+  if ( bitRead(currentStatus.squirt, BIT_SQUIRT_DFCO) == 1 ) { sumCorrections = 0; }
 
   sumCorrections = sumCorrections / powint(100,activeCorrections);
 
@@ -88,10 +88,21 @@ Uses a 2D enrichment table (WUETable) where the X axis is engine temp and the Y
 */
 static inline byte correctionWUE()
 {
+  byte WUEValue;
   //Possibly reduce the frequency this runs at (Costs about 50 loops per second)
-  if (currentStatus.coolant > (WUETable.axisX[9] - CALIBRATION_TEMPERATURE_OFFSET)) { BIT_CLEAR(currentStatus.engine, BIT_ENGINE_WARMUP); return 100; } //This prevents us doing the 2D lookup if we're already up to temp
-  BIT_SET(currentStatus.engine, BIT_ENGINE_WARMUP);
-  return table2D_getValue(&WUETable, currentStatus.coolant + CALIBRATION_TEMPERATURE_OFFSET);
+  if (currentStatus.coolant > (WUETable.axisX[9] - CALIBRATION_TEMPERATURE_OFFSET))
+  {
+    //This prevents us doing the 2D lookup if we're already up to temp
+    BIT_CLEAR(currentStatus.engine, BIT_ENGINE_WARMUP);
+    WUEValue = 100;
+  }
+  else
+  {
+    BIT_SET(currentStatus.engine, BIT_ENGINE_WARMUP);
+    WUEValue = table2D_getValue(&WUETable, currentStatus.coolant + CALIBRATION_TEMPERATURE_OFFSET);
+  }
+
+  return WUEValue;
 }
 
 /*
@@ -100,8 +111,9 @@ Additional fuel % to be added when the engine is cranking
 */
 static inline byte correctionCranking()
 {
-  if ( BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) ) { return 100 + configPage1.crankingPct; }
-  else { return 100; }
+  byte crankingValue = 100;
+  if ( BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) ) { crankingValue = 100 + configPage1.crankingPct; }
+  return crankingValue;
 }
 
 /*
@@ -111,17 +123,21 @@ where an additional amount of fuel is added (Over and above the WUE amount)
 */
 static inline byte correctionASE()
 {
+  byte ASEValue;
   //Two checks are requiredL:
   //1) Is the negine run time less than the configured ase time
   //2) Make sure we're not still cranking
   if ( (currentStatus.runSecs < configPage1.aseCount) && !(BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK)) )
   {
     BIT_SET(currentStatus.engine, BIT_ENGINE_ASE); //Mark ASE as active.
-    return 100 + configPage1.asePct;
+    ASEValue = 100 + configPage1.asePct;
   }
-
-  BIT_CLEAR(currentStatus.engine, BIT_ENGINE_ASE); //Mark ASE as inactive.
-  return 100;
+  else
+  {
+    BIT_CLEAR(currentStatus.engine, BIT_ENGINE_ASE); //Mark ASE as inactive.
+    ASEValue = 100;
+  }
+  return ASEValue;
 }
 
 /*
@@ -131,6 +147,7 @@ When the enrichment is turned on, it runs at that amount for a fixed period of t
 */
 static inline byte correctionAccel()
 {
+  byte accelValue = 100;
   //First, check whether the accel. enrichment is already running
   if( BIT_CHECK(currentStatus.engine, BIT_ENGINE_ACC) )
   {
@@ -140,29 +157,37 @@ static inline byte correctionAccel()
       //Time to turn enrichment off
       BIT_CLEAR(currentStatus.engine, BIT_ENGINE_ACC);
       currentStatus.TAEamount = 0;
-      return 100;
+      accelValue = 100;
+    }
+    else
+    {
+      //Enrichment still needs to keep running. Simply return the total TAE amount
+      accelValue = currentStatus.TAEamount;
     }
-    //Enrichment still needs to keep running. Simply return the total TAE amount
-    return currentStatus.TAEamount;
   }
-
-  //Check for deceleration (Deceleration adjustment not yet supported)
-  if (currentStatus.TPS < currentStatus.TPSlast) { return 100; }
-
-  //If TAE isn't currently turned on, need to check whether it needs to be turned on
-  int rateOfChange = ldiv(1000000, (currentStatus.TPS_time - currentStatus.TPSlast_time)).quot * (currentStatus.TPS - currentStatus.TPSlast); //This is the % per second that the TPS has moved
-  //currentStatus.tpsDOT = divs10(rateOfChange); //The TAE bins are divided by 10 in order to allow them to be stored in a byte.
-  currentStatus.tpsDOT = rateOfChange / 10;
-
-  if (rateOfChange > configPage1.tpsThresh)
+  else
   {
-    BIT_SET(currentStatus.engine, BIT_ENGINE_ACC); //Mark accleration enrichment as active.
-    currentStatus.TAEEndTime = micros() + ((unsigned long)configPage1.taeTime * 10000); //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 + table2D_getValue(&taeTable, currentStatus.tpsDOT);
+    //Check for deceleration (Deceleration adjustment not yet supported)
+    if (currentStatus.TPS < currentStatus.TPSlast)
+    {
+      accelValue = 100;
+    }
+    else
+    {
+      //If TAE isn't currently turned on, need to check whether it needs to be turned on
+      int rateOfChange = ldiv(1000000, (currentStatus.TPS_time - currentStatus.TPSlast_time)).quot * (currentStatus.TPS - currentStatus.TPSlast); //This is the % per second that the TPS has moved
+      currentStatus.tpsDOT = rateOfChange / 10; //The TAE bins are divided by 10 in order to allow them to be stored in a byte. Faster as this than divu10
+
+      if (rateOfChange > configPage1.tpsThresh)
+      {
+        BIT_SET(currentStatus.engine, BIT_ENGINE_ACC); //Mark accleration enrichment as active.
+        currentStatus.TAEEndTime = micros() + ((unsigned long)configPage1.taeTime * 10000); //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
+        accelValue = 100 + table2D_getValue(&taeTable, currentStatus.tpsDOT);
+      }
+    }
   }
 
-  //If we reach here then TAE is neither on, nor does it need to be turned on.
-  return 100;
+  return accelValue;
 }
 
 /*
@@ -172,16 +197,17 @@ This function always returns either 100 or 0
 
 static inline byte correctionFloodClear()
 {
-  if(BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK))
+  byte floodValue = 100;
+  if( BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) )
   {
     //Engine is currently cranking, check what the TPS is
     if(currentStatus.TPS >= configPage2.floodClear)
     {
       //Engine is cranking and TPS is above threshold. Cut all fuel
-      return 0;
+      floodValue = 0;
     }
   }
-  return 100;
+  return floodValue;
 }
 
 /*
@@ -190,8 +216,11 @@ Uses a 2D enrichment table (WUETable) where the X axis is engine temp and the Y
 */
 static inline byte correctionBatVoltage()
 {
-  if (currentStatus.battery10 > (injectorVCorrectionTable.axisX[5])) { return injectorVCorrectionTable.values[injectorVCorrectionTable.xSize-1]; } //This prevents us doing the 2D lookup if the voltage is above maximum
-  return table2D_getValue(&injectorVCorrectionTable, currentStatus.battery10);
+  byte batValue = 100;
+  if (currentStatus.battery10 > (injectorVCorrectionTable.axisX[5])) { batValue = injectorVCorrectionTable.values[injectorVCorrectionTable.xSize-1]; } //This prevents us doing the 2D lookup if the voltage is above maximum
+  else { batValue = table2D_getValue(&injectorVCorrectionTable, currentStatus.battery10); }
+
+  return batValue;
 }
 
 /*
@@ -200,8 +229,11 @@ This corrects for changes in air density from movement of the temperature
 */
 static inline byte correctionIATDensity()
 {
-  if ( (currentStatus.IAT + CALIBRATION_TEMPERATURE_OFFSET) > (IATDensityCorrectionTable.axisX[8])) { return IATDensityCorrectionTable.values[IATDensityCorrectionTable.xSize-1]; } //This prevents us doing the 2D lookup if the intake temp is above maximum
-  return table2D_getValue(&IATDensityCorrectionTable, currentStatus.IAT + CALIBRATION_TEMPERATURE_OFFSET); //currentStatus.IAT is the actual temperature, values in IATDensityCorrectionTable.axisX are temp+offset
+  byte IATValue = 100;
+  if ( (currentStatus.IAT + CALIBRATION_TEMPERATURE_OFFSET) > (IATDensityCorrectionTable.axisX[8])) { IATValue = IATDensityCorrectionTable.values[IATDensityCorrectionTable.xSize-1]; } //This prevents us doing the 2D lookup if the intake temp is above maximum
+  else { IATValue = table2D_getValue(&IATDensityCorrectionTable, currentStatus.IAT + CALIBRATION_TEMPERATURE_OFFSET); }//currentStatus.IAT is the actual temperature, values in IATDensityCorrectionTable.axisX are temp+offset
+
+  return IATValue;
 }
 
 /*
@@ -210,8 +242,10 @@ This simple check applies the extra fuel if we're currently launching
 */
 static inline byte correctionLaunch()
 {
-  if(currentStatus.launchingHard || currentStatus.launchingSoft) { return (100 + configPage3.lnchFuelAdd); }
-  else { return 100; }
+  byte launchValue = 100;
+  if(currentStatus.launchingHard || currentStatus.launchingSoft) { launchValue = (100 + configPage3.lnchFuelAdd); }
+
+  return launchValue;
 }
 
 /*
@@ -219,9 +253,13 @@ static inline byte correctionLaunch()
  */
 static inline bool correctionDFCO()
 {
-  if ( !configPage2.dfcoEnabled ) { return false; } //If the DFCO option isn't turned on, always return false (off)
-  if ( bitRead(currentStatus.squirt, BIT_SQUIRT_DFCO) ) { return ( currentStatus.RPM > ( configPage2.dfcoRPM * 10) ) && ( currentStatus.TPS < configPage2.dfcoTPSThresh ); }
-  else { return ( currentStatus.RPM > (unsigned int)( (configPage2.dfcoRPM * 10) + configPage2.dfcoHyster) ) && ( currentStatus.TPS < configPage2.dfcoTPSThresh ); }
+  bool DFCOValue = false;
+  if ( configPage2.dfcoEnabled == 1 )
+  {
+    if ( bitRead(currentStatus.squirt, BIT_SQUIRT_DFCO) == 1 ) { DFCOValue = ( currentStatus.RPM > ( configPage2.dfcoRPM * 10) ) && ( currentStatus.TPS < configPage2.dfcoTPSThresh ); }
+    else { DFCOValue = ( currentStatus.RPM > (unsigned int)( (configPage2.dfcoRPM * 10) + configPage2.dfcoHyster) ) && ( currentStatus.TPS < configPage2.dfcoTPSThresh ); }
+  }
+  return DFCOValue;
 }
 
 /*
@@ -230,9 +268,13 @@ static inline bool correctionDFCO()
 */
 static inline byte correctionFlex()
 {
-  if(!configPage1.flexEnabled) { return 100; } //Check for flex being enabled
-  byte flexRange = configPage1.flexFuelHigh - configPage1.flexFuelLow;
-  return percentage(currentStatus.ethanolPct, flexRange) + 100;
+  byte flexValue = 100;
+  if(configPage1.flexEnabled == 1)
+  {
+    byte flexRange = configPage1.flexFuelHigh - configPage1.flexFuelLow;
+    flexValue = percentage(currentStatus.ethanolPct, flexRange) + 100;
+  }
+  return flexValue;
 }
 
 /*
@@ -251,78 +293,76 @@ PID (Best suited to wideband sensors):
 
 static inline byte correctionAFRClosedLoop()
 {
-  if( (configPage3.egoType == 0)) { return 100; } //egoType of 0 means no O2 sensor
-
-  currentStatus.afrTarget = currentStatus.O2; //Catch all incase the below doesn't run. This prevents the Include AFR option from doing crazy things if the AFR target conditions aren't met. This value is changed again below if all conditions are met.
-
-  //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
+  byte AFRValue = 100;
+  if( configPage3.egoType > 0 ) //egoType of 0 means no O2 sensor
   {
-    //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; }
-    currentStatus.afrTarget = get3DTableValue(&afrTable, yValue, currentStatus.RPM); //Perform the target lookup
+    currentStatus.afrTarget = currentStatus.O2; //Catch all incase the below doesn't run. This prevents the Include AFR option from doing crazy things if the AFR target conditions aren't met. This value is changed again below if all conditions are met.
 
-    //Check all other requirements for closed loop adjustments
-    if( (currentStatus.coolant > (int)(configPage3.egoTemp - CALIBRATION_TEMPERATURE_OFFSET)) && (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) )
+    //Check the ignition count to see whether the next step is required
+    //This if statement 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
+    //if( (ignitionCount & (configPage3.egoCount - 1)) == 1 )
     {
-      //Check which algorithm is used, simple or PID
-      if (configPage3.egoAlgorithm == 0)
+      //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; }
+      currentStatus.afrTarget = get3DTableValue(&afrTable, yValue, currentStatus.RPM); //Perform the target lookup
+
+      //Check all other requirements for closed loop adjustments
+      if( (currentStatus.coolant > (int)(configPage3.egoTemp - CALIBRATION_TEMPERATURE_OFFSET)) && (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) )
       {
-        //*************************************************************************************************************************************
-        //Simple algorithm
-        if(currentStatus.O2 > currentStatus.afrTarget)
+        //Check which algorithm is used, simple or PID
+        if (configPage3.egoAlgorithm == 0)
         {
-          //Running lean
-          if(currentStatus.egoCorrection < (100 + configPage3.egoLimit) ) //Fueling adjustment must be at most the egoLimit amount (up or down)
+          //*************************************************************************************************************************************
+          //Simple algorithm
+          if(currentStatus.O2 > currentStatus.afrTarget)
           {
-            if(currentStatus.egoCorrection >= 100) { return (currentStatus.egoCorrection + 1); } //Increase the fueling by 1%
-            else { return 100; } //This means that the last reading had been rich, so simply return back to no adjustment (100%)
+            //Running lean
+            if(currentStatus.egoCorrection < (100 + configPage3.egoLimit) ) //Fueling adjustment must be at most the egoLimit amount (up or down)
+            {
+              if(currentStatus.egoCorrection >= 100) { AFRValue = (currentStatus.egoCorrection + 1); } //Increase the fueling by 1%
+              else { AFRValue = 100; } //This means that the last reading had been rich, so simply return back to no adjustment (100%)
+            }
+            else { AFRValue = currentStatus.egoCorrection; } //Means we're at the maximum adjustment amount, so simply return then again
           }
-          else { return currentStatus.egoCorrection; } //Means we're at the maximum adjustment amount, so simply return then again
+          else
+            //Running Rich
+            if(currentStatus.egoCorrection > (100 - configPage3.egoLimit) ) //Fueling adjustment must be at most the egoLimit amount (up or down)
+            {
+              if(currentStatus.egoCorrection <= 100) { AFRValue = (currentStatus.egoCorrection - 1); } //Increase the fueling by 1%
+              else { AFRValue = 100; } //This means that the last reading had been lean, so simply return back to no adjustment (100%)
+            }
+            else { AFRValue = currentStatus.egoCorrection; } //Means we're at the maximum adjustment amount, so simply return then again
         }
-        else
-          //Running Rich
-          if(currentStatus.egoCorrection > (100 - configPage3.egoLimit) ) //Fueling adjustment must be at most the egoLimit amount (up or down)
-          {
-            if(currentStatus.egoCorrection <= 100) { return (currentStatus.egoCorrection - 1); } //Increase the fueling by 1%
-            else { return 100; } //This means that the last reading had been lean, so simply return back to no adjustment (100%)
-          }
-          else { return currentStatus.egoCorrection; } //Means we're at the maximum adjustment amount, so simply return then again
-      }
-      else if(configPage3.egoAlgorithm == 2)
-      {
-        //*************************************************************************************************************************************
-        //PID algorithm
-        egoPID.SetOutputLimits((long)(-configPage3.egoLimit), (long)(configPage3.egoLimit)); //Set the limits again, just incase the user has changed them since the last loop. Note that these are sent to the PID library as (Eg:) -15 and +15
-        egoPID.SetTunings(configPage3.egoKP, configPage3.egoKI, configPage3.egoKD); //Set the PID values again, just incase the user has changed them since the last loop
-        PID_O2 = (long)(currentStatus.O2);
-        PID_AFRTarget = (long)(currentStatus.afrTarget);
+        else if(configPage3.egoAlgorithm == 2)
+        {
+          //*************************************************************************************************************************************
+          //PID algorithm
+          egoPID.SetOutputLimits((long)(-configPage3.egoLimit), (long)(configPage3.egoLimit)); //Set the limits again, just incase the user has changed them since the last loop. Note that these are sent to the PID library as (Eg:) -15 and +15
+          egoPID.SetTunings(configPage3.egoKP, configPage3.egoKI, configPage3.egoKD); //Set the PID values again, just incase the user has changed them since the last loop
+          PID_O2 = (long)(currentStatus.O2);
+          PID_AFRTarget = (long)(currentStatus.afrTarget);
 
-        egoPID.Compute();
-        //currentStatus.egoCorrection = 100 + PID_output;
-        return (100 + PID_output);
-      }
-      else { return 100; } // Occurs if the egoAlgorithm is set to 0 (No Correction)
+          egoPID.Compute();
+          //currentStatus.egoCorrection = 100 + PID_output;
+          AFRValue = 100 + PID_output;
+        }
+        else { AFRValue = 100; } // Occurs if the egoAlgorithm is set to 0 (No Correction)
 
-    }
-  }
+      } //Multi variable check
+    } //ignitionCount
+  } //egoType
 
-  return 100; //Catch all (Includes when AFR target = current AFR
+  return AFRValue; //Catch all (Includes when AFR target = current AFR
 }
 
 //******************************** IGNITION ADVANCE CORRECTIONS ********************************
-int SetIgnStartAngle(int IgnDegrees)
-{
-  IgnDegrees += CRANK_ANGLE_MAX_IGN - currentStatus.advance - dwellAngle;
-  if(IgnDegrees > CRANK_ANGLE_MAX_IGN) { return IgnDegrees - CRANK_ANGLE_MAX_IGN; }
-  else { return IgnDegrees; }
-}
 
-int8_t correctionsIgn(int8_t advance)
+int8_t correctionsIgn(int8_t base_advance)
 {
-  advance = correctionFlexTiming(advance);
+  int8_t advance;
+  advance = correctionFlexTiming(base_advance);
   advance = correctionIATretard(advance);
   advance = correctionSoftRevLimit(advance);
   advance = correctionSoftLaunch(advance);
@@ -337,83 +377,100 @@ int8_t correctionsIgn(int8_t advance)
 
 static inline int8_t correctionFixedTiming(int8_t advance)
 {
-  if (configPage2.FixAng != 0) { return configPage2.FixAng; } //Check whether the user has set a fixed timing angle
-  return advance;
+  byte ignFixValue = advance;
+  if (configPage2.FixAng != 0) { ignFixValue = configPage2.FixAng; } //Check whether the user has set a fixed timing angle
+  return ignFixValue;
 }
 
 static inline int8_t correctionCrankingFixedTiming(int8_t advance)
 {
-  if ( BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) ) { return configPage2.CrankAng; } //Use the fixed cranking ignition angle
-  return advance;
+  byte ignCrankFixValue = advance;
+  if ( BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) ) { ignCrankFixValue = configPage2.CrankAng; } //Use the fixed cranking ignition angle
+  return ignCrankFixValue;
 }
 
 static inline int8_t correctionFlexTiming(int8_t advance)
 {
-  if(!configPage1.flexEnabled) { return advance; } //Check for flex being enabled
-  byte flexRange = configPage1.flexAdvHigh - configPage1.flexAdvLow;
+  byte ignFlexValue = advance;
+  if( configPage1.flexEnabled == 1 ) //Check for flex being enabled
+  {
+    byte flexRange = configPage1.flexAdvHigh - configPage1.flexAdvLow;
 
-  if (currentStatus.ethanolPct != 0) { currentStatus.flexIgnCorrection = percentage(currentStatus.ethanolPct, flexRange); }
-  else { currentStatus.flexIgnCorrection = 0; }
+    if (currentStatus.ethanolPct != 0) { currentStatus.flexIgnCorrection = percentage(currentStatus.ethanolPct, flexRange); }
+    else { currentStatus.flexIgnCorrection = 0; }
 
-  return advance + currentStatus.flexIgnCorrection;
+    ignFlexValue = advance + currentStatus.flexIgnCorrection;
+  }
+  return ignFlexValue;
 }
 
 static inline int8_t correctionIATretard(int8_t advance)
 {
+  byte ignIATValue = advance;
   //Adjust the advance based on IAT. If the adjustment amount is greater than the current advance, just set advance to 0
   int8_t advanceIATadjust = table2D_getValue(&IATRetardTable, currentStatus.IAT);
   int tempAdvance = (advance - advanceIATadjust);
-  if (tempAdvance >= -OFFSET_IGNITION) { return  tempAdvance; }
-  else { return -OFFSET_IGNITION; }
+  if (tempAdvance >= -OFFSET_IGNITION) { ignIATValue = tempAdvance; }
+  else { ignIATValue = -OFFSET_IGNITION; }
+
+  return ignIATValue;
 }
 
 static inline int8_t correctionSoftRevLimit(int8_t advance)
 {
+  byte ignSoftRevValue = advance;
   BIT_CLEAR(currentStatus.spark, BIT_SPARK_SFTLIM);
-  if (currentStatus.RPM > ((unsigned int)(configPage2.SoftRevLim) * 100) ) { BIT_SET(currentStatus.spark, BIT_SPARK_SFTLIM); return configPage2.SoftLimRetard;  } //Softcut RPM limit (If we're above softcut limit, delay timing by configured number of degrees)
-  return advance;
+  if (currentStatus.RPM > ((unsigned int)(configPage2.SoftRevLim) * 100) ) { BIT_SET(currentStatus.spark, BIT_SPARK_SFTLIM); ignSoftRevValue = configPage2.SoftLimRetard;  } //Softcut RPM limit (If we're above softcut limit, delay timing by configured number of degrees)
+
+  return ignSoftRevValue;
 }
 
 static inline int8_t correctionSoftLaunch(int8_t advance)
 {
+  byte ignSoftLaunchValue = advance;
   //SoftCut rev limit for 2-step launch control.
   if (configPage3.launchEnabled && clutchTrigger && (currentStatus.clutchEngagedRPM < ((unsigned int)(configPage3.flatSArm) * 100)) && (currentStatus.RPM > ((unsigned int)(configPage3.lnchSoftLim) * 100)) )
   {
     currentStatus.launchingSoft = true;
     BIT_SET(currentStatus.spark, BIT_SPARK_SLAUNCH);
-    return configPage3.lnchRetard;
+    ignSoftLaunchValue = configPage3.lnchRetard;
+  }
+  else
+  {
+    currentStatus.launchingSoft = false;
+    BIT_CLEAR(currentStatus.spark, BIT_SPARK_SLAUNCH);
   }
 
-  currentStatus.launchingSoft = false;
-  BIT_CLEAR(currentStatus.spark, BIT_SPARK_SLAUNCH);
-  return advance;
+  return ignSoftLaunchValue;
 }
 
 static inline int8_t correctionSoftFlatShift(int8_t  advance)
 {
+  byte ignSoftFlatValue = advance;
+
   if(configPage3.flatSEnable && clutchTrigger && (currentStatus.clutchEngagedRPM > ((unsigned int)(configPage3.flatSArm) * 100)) && (currentStatus.RPM > (currentStatus.clutchEngagedRPM-configPage3.flatSSoftWin) ) )
   {
     BIT_SET(currentStatus.spark2, BIT_SPARK2_FLATSS);
-    return configPage3.flatSRetard;
+    ignSoftFlatValue = configPage3.flatSRetard;
   }
+  else { BIT_CLEAR(currentStatus.spark2, BIT_SPARK2_FLATSS); }
 
-  BIT_CLEAR(currentStatus.spark2, BIT_SPARK2_FLATSS);
-  return advance;
+  return ignSoftFlatValue;
 }
 
 //******************************** DWELL CORRECTIONS ********************************
 uint16_t correctionsDwell(uint16_t dwell)
 {
-
+  uint16_t tempDwell = dwell;
   //Pull battery voltage based dwell correction and apply if needed
   currentStatus.dwellCorrection = table2D_getValue(&dwellVCorrectionTable, currentStatus.battery10);
-  if (currentStatus.dwellCorrection != 100) { dwell = divs100(dwell) * currentStatus.dwellCorrection; }
+  if (currentStatus.dwellCorrection != 100) { tempDwell = divs100(dwell) * currentStatus.dwellCorrection; }
 
   //Dwell limiter
-  uint16_t dwellPerRevolution = dwell + (uint16_t)(configPage2.sparkDur * 100); //Spark duration is in mS*10. Multiple it by 100 to get spark duration in uS
+  uint16_t dwellPerRevolution = tempDwell + (uint16_t)(configPage2.sparkDur * 100); //Spark duration is in mS*10. Multiple it by 100 to get spark duration in uS
   int8_t pulsesPerRevolution = 1;
   //Single channel spark mode is the only time there will be more than 1 pulse per revolution on any given output
-  if(configPage2.sparkMode == IGN_MODE_SINGLE && configPage1.nCylinders > 1) //No point in running this for 1 cylinder engines
+  if( (configPage2.sparkMode == IGN_MODE_SINGLE) && (configPage1.nCylinders > 1) ) //No point in running this for 1 cylinder engines
   {
     pulsesPerRevolution = (configPage1.nCylinders >> 1);
     dwellPerRevolution = dwellPerRevolution * pulsesPerRevolution;
@@ -422,7 +479,7 @@ uint16_t correctionsDwell(uint16_t dwell)
   if(dwellPerRevolution > revolutionTime)
   {
     //Possibly need some method of reducing spark duration here as well, but this is a start
-    dwell = (revolutionTime / pulsesPerRevolution) - (configPage2.sparkDur * 100);
+    tempDwell = (revolutionTime / pulsesPerRevolution) - (configPage2.sparkDur * 100);
   }
-  return dwell;
-}
+  return tempDwell;
+}
diff --git a/speeduino/decoders.h b/speeduino/decoders.h
index 03096b00..91fda7e0 100644
--- a/speeduino/decoders.h
+++ b/speeduino/decoders.h
@@ -4,26 +4,26 @@
 #include <limits.h>
 
 #if defined(CORE_AVR)
-  #define READ_PRI_TRIGGER() ((*triggerPri_pin_port & triggerPri_pin_mask) ? HIGH : LOW)
-  #define READ_SEC_TRIGGER() ((*triggerSec_pin_port & triggerSec_pin_mask) ? HIGH : LOW)
+  #define READ_PRI_TRIGGER() ((*triggerPri_pin_port & triggerPri_pin_mask) ? true : false)
+  #define READ_SEC_TRIGGER() ((*triggerSec_pin_port & triggerSec_pin_mask) ? true : false)
 #elif defined(CORE_TEENSY) || defined(CORE_STM32)
   #define READ_PRI_TRIGGER() digitalRead(pinTrigger)
   #define READ_SEC_TRIGGER() digitalRead(pinTrigger2)
 #endif
 
 static inline void addToothLogEntry(unsigned long);
-static inline int stdGetRPM();
+static inline uint16_t stdGetRPM();
 static inline void setFilter(unsigned long);
 static inline int crankingGetRPM(byte);
 void triggerSetup_missingTooth();
 void triggerPri_missingTooth();
 void triggerSec_missingTooth();
-int getRPM_missingTooth();
+uint16_t getRPM_missingTooth();
 int getCrankAngle_missingTooth(int timePerDegree);
 void triggerSetup_DualWheel();
 void triggerPri_DualWheel();
 void triggerSec_DualWheel();
-int getRPM_DualWheel();
+uint16_t getRPM_DualWheel();
 int getCrankAngle_DualWheel(int timePerDegree);
 
 unsigned long MAX_STALL_TIME = 500000UL; //The maximum time (in uS) that the system will continue to function before the engine is considered stalled/stopped. This is unique to each decoder, depending on the number of teeth etc. 500000 (half a second) is used as the default value, most decoders will be much less.
@@ -54,10 +54,9 @@ volatile unsigned long secondaryLastToothTime1 = 0; //The time (micros()) that t
 
 volatile int triggerActualTeeth;
 volatile unsigned long triggerFilterTime; // The shortest time (in uS) that pulses will be accepted (Used for debounce filtering)
-unsigned int triggerSecFilterTime; // The shortest time (in uS) that pulses will be accepted (Used for debounce filtering) for the secondary input
+unsigned long triggerSecFilterTime; // The shortest time (in uS) that pulses will be accepted (Used for debounce filtering) for the secondary input
 unsigned int triggerSecFilterTime_duration; // The shortest valid time (in uS) pulse DURATION
 volatile int triggerToothAngle; //The number of crank degrees that elapse per tooth
-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)
 bool secondDerivEnabled; //The use of the 2nd derivative calculation is limited to certain decoders. This is set to either true or false in each decoders setup routine
 bool decoderIsSequential; //Whether or not the decoder supports sequential operation
 byte checkSyncToothCount; //How many teeth must've been seen on this revolution before we try to confirm sync (Useful for missing tooth type decoders)
diff --git a/speeduino/decoders.ino b/speeduino/decoders.ino
index 2743726c..b48759a4 100644
--- a/speeduino/decoders.ino
+++ b/speeduino/decoders.ino
@@ -42,16 +42,28 @@ static inline void addToothLogEntry(unsigned long toothTime)
 /*
 As nearly all the decoders use a common method of determining RPM (The time the last full revolution took)
 A common function is simpler
+degreesOver is the number of crank degrees between tooth #1s. Some patterns have a tooth #1 every crank rev, others are every cam rev.
 */
-static inline int stdGetRPM()
+//static inline uint16_t stdGetRPM(uin16_t degreesOver)
+static inline uint16_t stdGetRPM()
 {
-  if( !currentStatus.hasSync ) { return 0; } //Safety check
-  if( BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) && currentStatus.startRevolutions == 0) { return 0; } //Prevents crazy RPM spike when there has been less than 1 full revolution
-  noInterrupts();
-  revolutionTime = (toothOneTime - toothOneMinusOneTime); //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)
-  interrupts();
-  uint16_t tempRPM = (US_IN_MINUTE / revolutionTime); //Calc RPM based on last full revolution time (Faster as /)
-  if(tempRPM >= MAX_RPM) { return currentStatus.RPM; } //Sanity check
+  uint16_t tempRPM = 0;
+
+  if( currentStatus.hasSync == true )
+  {
+    if( (BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK)) && (currentStatus.startRevolutions == 0) ) { tempRPM = 0; } //Prevents crazy RPM spike when there has been less than 1 full revolution
+    else
+    {
+      noInterrupts();
+      revolutionTime = (toothOneTime - toothOneMinusOneTime); //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)
+      interrupts();
+      //if(degreesOver == 720) { revolutionTime / 2; }
+      tempRPM = (US_IN_MINUTE / revolutionTime); //Calc RPM based on last full revolution time (Faster as /)
+      if(tempRPM >= MAX_RPM) { tempRPM = currentStatus.RPM; } //Sanity check
+    }
+  }
+  else { tempRPM = 0; }
+
   return tempRPM;
 }
 
@@ -75,12 +87,15 @@ It takes an argument of the full (COMPLETE) number of teeth per revolution. For
 */
 static inline int crankingGetRPM(byte totalTeeth)
 {
-  if(toothLastToothTime == 0 || toothLastMinusOneToothTime == 0) { return 0; } //Startup check
-  noInterrupts();
-  revolutionTime = (toothLastToothTime - toothLastMinusOneToothTime) * totalTeeth;
-  interrupts();
-  uint16_t tempRPM = (US_IN_MINUTE / revolutionTime);
-  if(tempRPM >= MAX_RPM) { return currentStatus.RPM; } //Sanity check
+  uint16_t tempRPM = 0;
+  if( (toothLastToothTime > 0) && (toothLastMinusOneToothTime > 0) )
+  {
+    noInterrupts();
+    revolutionTime = (toothLastToothTime - toothLastMinusOneToothTime) * totalTeeth;
+    interrupts();
+    tempRPM = (US_IN_MINUTE / revolutionTime);
+    if(tempRPM >= MAX_RPM) { tempRPM = currentStatus.RPM; } //Sanity check
+  }
   return tempRPM;
 }
 
@@ -92,7 +107,7 @@ Note: This does not currently support dual wheel (ie missing tooth + single toot
 void triggerSetup_missingTooth()
 {
   triggerToothAngle = 360 / configPage2.triggerTeeth; //The number of degrees that passes from tooth to tooth
-  if(configPage2.TrigSpeed) { triggerToothAngle = 720 / configPage2.triggerTeeth; } //Account for cam speed missing tooth
+  if(configPage2.TrigSpeed == 1) { triggerToothAngle = 720 / configPage2.triggerTeeth; } //Account for cam speed missing tooth
   triggerActualTeeth = configPage2.triggerTeeth - configPage2.triggerMissingTeeth; //The number of physical teeth on the wheel. Doing this here saves us a calculation each time in the interrupt
   triggerFilterTime = (int)(1000000 / (MAX_RPM / 60 * configPage2.triggerTeeth)); //Trigger filter time is the shortest possible time (in uS) that there can be between crank teeth (ie at max RPM). Any pulses that occur faster than this time will be disgarded as noise
   secondDerivEnabled = false;
@@ -108,38 +123,46 @@ void triggerPri_missingTooth()
 
    curTime = micros();
    curGap = curTime - toothLastToothTime;
-   if ( curGap < triggerFilterTime ) { return; } //Debounce check. Pulses should never be less than triggerFilterTime, so if they are it means a false trigger. (A 36-1 wheel at 8000pm will have triggers approx. every 200uS)
-   toothCurrentCount++; //Increment the tooth counter
-
-   addToothLogEntry(curGap);
-
-   //if(toothCurrentCount > checkSyncToothCount || !currentStatus.hasSync)
+   if ( curGap >= triggerFilterTime ) //Pulses should never be less than triggerFilterTime, so if they are it means a false trigger. (A 36-1 wheel at 8000pm will have triggers approx. every 200uS)
    {
-     //Begin the missing tooth detection
-     //If the time between the current tooth and the last is greater than 1.5x the time between the last tooth and the tooth before that, we make the assertion that we must be at the first tooth after the gap
-     if(configPage2.triggerMissingTeeth == 1) { targetGap = (3 * (toothLastToothTime - toothLastMinusOneToothTime)) >> 1; } //Multiply by 1.5 (Checks for a gap 1.5x greater than the last one) (Uses bitshift to multiply by 3 then divide by 2. Much faster than multiplying by 1.5)
-     else { targetGap = ((toothLastToothTime - toothLastMinusOneToothTime)) * 2; } //Multiply by 2 (Checks for a gap 2x greater than the last one)
+     toothCurrentCount++; //Increment the tooth counter
 
-     if ( curGap > targetGap || toothCurrentCount > triggerActualTeeth)
+     addToothLogEntry(curGap);
+
+     //if(toothCurrentCount > checkSyncToothCount || currentStatus.hasSync == false)
      {
-       if(toothCurrentCount < (triggerActualTeeth) && currentStatus.hasSync) { currentStatus.hasSync = false; return; } //This occurs when we're at tooth #1, but haven't seen all the other teeth. This indicates a signal issue so we flag lost sync so this will attempt to resync on the next revolution.
-       toothCurrentCount = 1;
-       revolutionOne = !revolutionOne; //Flip sequential revolution tracker
-       toothOneMinusOneTime = toothOneTime;
-       toothOneTime = curTime;
-       currentStatus.hasSync = true;
-       currentStatus.startRevolutions++; //Counter
-       triggerFilterTime = 0; //This is used to prevent a condition where serious intermitent signals (Eg someone furiously plugging the sensor wire in and out) can leave the filter in an unrecoverable state
-     }
-     else
-     {
-       //Filter can only be recalc'd for the regular teeth, not the missing one.
-       setFilter(curGap);
+       //Begin the missing tooth detection
+       //If the time between the current tooth and the last is greater than 1.5x the time between the last tooth and the tooth before that, we make the assertion that we must be at the first tooth after the gap
+       if(configPage2.triggerMissingTeeth == 1) { targetGap = (3 * (toothLastToothTime - toothLastMinusOneToothTime)) >> 1; } //Multiply by 1.5 (Checks for a gap 1.5x greater than the last one) (Uses bitshift to multiply by 3 then divide by 2. Much faster than multiplying by 1.5)
+       else { targetGap = ((toothLastToothTime - toothLastMinusOneToothTime)) * 2; } //Multiply by 2 (Checks for a gap 2x greater than the last one)
+
+       if ( (curGap > targetGap) || (toothCurrentCount > triggerActualTeeth) )
+       {
+         if(toothCurrentCount < (triggerActualTeeth) && (currentStatus.hasSync == true) ) { currentStatus.hasSync = false; } //This occurs when we're at tooth #1, but haven't seen all the other teeth. This indicates a signal issue so we flag lost sync so this will attempt to resync on the next revolution.
+         else
+         {
+           toothCurrentCount = 1;
+           revolutionOne = !revolutionOne; //Flip sequential revolution tracker
+           toothOneMinusOneTime = toothOneTime;
+           toothOneTime = curTime;
+           currentStatus.hasSync = true;
+           currentStatus.startRevolutions++; //Counter
+           triggerFilterTime = 0; //This is used to prevent a condition where serious intermitent signals (Eg someone furiously plugging the sensor wire in and out) can leave the filter in an unrecoverable state
+           toothLastMinusOneToothTime = toothLastToothTime;
+           toothLastToothTime = curTime;
+         }
+       }
+       else
+       {
+         //Filter can only be recalc'd for the regular teeth, not the missing one.
+         setFilter(curGap);
+         toothLastMinusOneToothTime = toothLastToothTime;
+         toothLastToothTime = curTime;
+       }
      }
+
+
    }
-
-   toothLastMinusOneToothTime = toothLastToothTime;
-   toothLastToothTime = curTime;
 }
 
 void triggerSec_missingTooth()
@@ -148,15 +171,20 @@ void triggerSec_missingTooth()
   revolutionOne = 1;
 }
 
-int getRPM_missingTooth()
+uint16_t getRPM_missingTooth()
 {
+  uint16_t tempRPM = 0;
   if( currentStatus.RPM < (unsigned int)(configPage2.crankRPM * 100) )
   {
-    if(configPage2.TrigSpeed) { crankingGetRPM(configPage2.triggerTeeth/2); } //Account for cam speed
-    else { return crankingGetRPM(configPage2.triggerTeeth); }
+    if(configPage2.TrigSpeed == 1) { crankingGetRPM(configPage2.triggerTeeth/2); } //Account for cam speed
+    else { tempRPM = crankingGetRPM(configPage2.triggerTeeth); }
   }
-  if(configPage2.TrigSpeed) { return (stdGetRPM() * 2); } //Account for cam speed
-  return stdGetRPM();
+  else
+  {
+    if(configPage2.TrigSpeed == 1) { tempRPM = (stdGetRPM() * 2); } //Account for cam speed
+    else { tempRPM = stdGetRPM(); }
+  }
+  return tempRPM;
 }
 
 int getCrankAngle_missingTooth(int timePerDegree)
@@ -172,7 +200,7 @@ int getCrankAngle_missingTooth(int timePerDegree)
     tempRevolutionOne = revolutionOne;
     interrupts();
 
-    int crankAngle = (tempToothCurrentCount - 1) * triggerToothAngle + configPage2.triggerAngle; //Number of teeth that have passed since tooth 1, multiplied by the angle each tooth represents, plus the angle that tooth 1 is ATDC. This gives accuracy only to the nearest tooth.
+    int crankAngle = ((tempToothCurrentCount - 1) * triggerToothAngle) + configPage2.triggerAngle; //Number of teeth that have passed since tooth 1, multiplied by the angle each tooth represents, plus the angle that tooth 1 is ATDC. This gives accuracy only to the nearest tooth.
     //Estimate the number of degrees travelled since the last tooth}
     long elapsedTime = (micros() - tempToothLastToothTime);
     //crankAngle += DIV_ROUND_CLOSEST(elapsedTime, timePerDegree);
@@ -210,25 +238,28 @@ void triggerPri_DualWheel()
 {
    curTime = micros();
    curGap = curTime - toothLastToothTime;
-   if ( curGap < triggerFilterTime ) { return; } //Pulses should never be less than triggerFilterTime, so if they are it means a false trigger.
-   toothCurrentCount++; //Increment the tooth counter
-   addToothLogEntry(curGap);
-
-   toothLastMinusOneToothTime = toothLastToothTime;
-   toothLastToothTime = curTime;
-
-   if ( !currentStatus.hasSync ) { return; }
-
-   if ( toothCurrentCount == 1 || toothCurrentCount > configPage2.triggerTeeth )
+   if ( curGap >= triggerFilterTime )
    {
-     toothCurrentCount = 1;
-     revolutionOne = !revolutionOne; //Flip sequential revolution tracker
-     toothOneMinusOneTime = toothOneTime;
-     toothOneTime = curTime;
-     currentStatus.startRevolutions++; //Counter
-   }
+     toothCurrentCount++; //Increment the tooth counter
+     addToothLogEntry(curGap);
 
-   setFilter(curGap); //Recalc the new filter value
+     toothLastMinusOneToothTime = toothLastToothTime;
+     toothLastToothTime = curTime;
+
+     if ( currentStatus.hasSync == true )
+     {
+       if ( (toothCurrentCount == 1) || (toothCurrentCount > configPage2.triggerTeeth) )
+       {
+         toothCurrentCount = 1;
+         revolutionOne = !revolutionOne; //Flip sequential revolution tracker
+         toothOneMinusOneTime = toothOneTime;
+         toothOneTime = curTime;
+         currentStatus.startRevolutions++; //Counter
+       }
+
+       setFilter(curGap); //Recalc the new filter value
+     }
+   } //TRigger filter
 
 
 }
@@ -237,28 +268,34 @@ void triggerSec_DualWheel()
 {
   curTime2 = micros();
   curGap2 = curTime2 - toothLastSecToothTime;
-  if ( curGap2 < triggerSecFilterTime ) { return; }
-  toothLastSecToothTime = curTime2;
-  triggerSecFilterTime = curGap2 >> 2; //Set filter at 25% of the current speed
-
-  if(!currentStatus.hasSync)
+  if ( curGap2 >= triggerSecFilterTime )
   {
-    toothLastToothTime = micros();
-    toothLastMinusOneToothTime = (toothOneTime - 6000000) / configPage2.triggerTeeth; //Fixes RPM at 10rpm until a full revolution has taken place
-    toothCurrentCount = configPage2.triggerTeeth;
+    toothLastSecToothTime = curTime2;
+    triggerSecFilterTime = curGap2 >> 2; //Set filter at 25% of the current speed
 
-    currentStatus.hasSync = true;
-  }
-  else if (configPage2.useResync) { toothCurrentCount = configPage2.triggerTeeth; }
+    if(currentStatus.hasSync == false)
+    {
+      toothLastToothTime = micros();
+      toothLastMinusOneToothTime = (toothOneTime - 6000000) / configPage2.triggerTeeth; //Fixes RPM at 10rpm until a full revolution has taken place
+      toothCurrentCount = configPage2.triggerTeeth;
 
-  revolutionOne = 1; //Sequential revolution reset
+      currentStatus.hasSync = true;
+    }
+    else if (configPage2.useResync == 1) { toothCurrentCount = configPage2.triggerTeeth; }
+
+    revolutionOne = 1; //Sequential revolution reset
+  } //Trigger filter
 }
 
-int getRPM_DualWheel()
+uint16_t getRPM_DualWheel()
 {
-  if( !currentStatus.hasSync) { return 0; }
-  if(currentStatus.RPM < configPage2.crankRPM) { return crankingGetRPM(configPage2.triggerTeeth); }
-  return stdGetRPM();
+  uint16_t tempRPM = 0;
+  if( currentStatus.hasSync == true )
+  {
+    if(currentStatus.RPM < configPage2.crankRPM) { tempRPM = crankingGetRPM(configPage2.triggerTeeth); }
+    else { tempRPM = stdGetRPM(); }
+  }
+  return tempRPM;
 }
 
 int getCrankAngle_DualWheel(int timePerDegree)
@@ -277,7 +314,7 @@ int getCrankAngle_DualWheel(int timePerDegree)
     //Handle case where the secondary tooth was the last one seen
     if(tempToothCurrentCount == 0) { tempToothCurrentCount = configPage2.triggerTeeth; }
 
-    int crankAngle = (tempToothCurrentCount - 1) * triggerToothAngle + configPage2.triggerAngle; //Number of teeth that have passed since tooth 1, multiplied by the angle each tooth represents, plus the angle that tooth 1 is ATDC. This gives accuracy only to the nearest tooth.
+    int crankAngle = ((tempToothCurrentCount - 1) * triggerToothAngle) + configPage2.triggerAngle; //Number of teeth that have passed since tooth 1, multiplied by the angle each tooth represents, plus the angle that tooth 1 is ATDC. This gives accuracy only to the nearest tooth.
     //Estimate the number of degrees travelled since the last tooth}
     long elapsedTime = micros() - tempToothLastToothTime;
     if(elapsedTime < SHRT_MAX ) { crankAngle += div((int)elapsedTime, timePerDegree).quot; } //This option is much faster, but only available for smaller values of elapsedTime
@@ -303,7 +340,6 @@ void triggerSetup_BasicDistributor()
 {
   triggerActualTeeth = configPage1.nCylinders;
   if(triggerActualTeeth == 0) { triggerActualTeeth = 1; }
-  //triggerToothAngle = 360 / triggerActualTeeth; //The number of degrees that passes from tooth to tooth
   triggerToothAngle = 720 / triggerActualTeeth; //The number of degrees that passes from tooth to tooth
   triggerFilterTime = 60000000L / MAX_RPM / configPage1.nCylinders; // Minimum time required between teeth
   triggerFilterTime = triggerFilterTime / 2; //Safety margin
@@ -319,37 +355,38 @@ void triggerPri_BasicDistributor()
 {
   curTime = micros();
   curGap = curTime - toothLastToothTime;
-  if ( curGap < triggerFilterTime ) { return; } //Noise rejection check. Pulses should never be less than triggerFilterTime
-
-  if( toothCurrentCount == triggerActualTeeth || !currentStatus.hasSync ) //Check if we're back to the beginning of a revolution
+  if ( curGap >= triggerFilterTime )
   {
-     toothCurrentCount = 1; //Reset the counter
-     toothOneMinusOneTime = toothOneTime;
-     toothOneTime = curTime;
-     currentStatus.hasSync = true;
-     currentStatus.startRevolutions++; //Counter
-  }
-  else
-  {
-    toothCurrentCount++; //Increment the tooth counter
-  }
+    if( (toothCurrentCount == triggerActualTeeth) || (currentStatus.hasSync == false) ) //Check if we're back to the beginning of a revolution
+    {
+       toothCurrentCount = 1; //Reset the counter
+       toothOneMinusOneTime = toothOneTime;
+       toothOneTime = curTime;
+       currentStatus.hasSync = true;
+       currentStatus.startRevolutions++; //Counter
+    }
+    else
+    {
+      toothCurrentCount++; //Increment the tooth counter
+    }
 
-  setFilter(curGap); //Recalc the new filter value
-  addToothLogEntry(curGap);
+    setFilter(curGap); //Recalc the new filter value
+    addToothLogEntry(curGap);
 
-  if ( configPage2.ignCranklock && BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) )
-  {
-    endCoil1Charge();
-    endCoil2Charge();
-    endCoil3Charge();
-    endCoil4Charge();
-  }
+    if ( configPage2.ignCranklock && BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) )
+    {
+      endCoil1Charge();
+      endCoil2Charge();
+      endCoil3Charge();
+      endCoil4Charge();
+    }
 
-  toothLastMinusOneToothTime = toothLastToothTime;
-  toothLastToothTime = curTime;
+    toothLastMinusOneToothTime = toothLastToothTime;
+    toothLastToothTime = curTime;
+  } //Trigger filter
 }
 void triggerSec_BasicDistributor() { return; } //Not required
-int getRPM_BasicDistributor()
+uint16_t getRPM_BasicDistributor()
 {
   uint16_t tempRPM;
   if( currentStatus.RPM < (unsigned int)(configPage2.crankRPM * 100) )
@@ -371,11 +408,7 @@ int getCrankAngle_BasicDistributor(int timePerDegree)
     tempToothLastToothTime = toothLastToothTime;
     interrupts();
 
-    //int crankAngle = (tempToothCurrentCount - 1) * triggerToothAngle + configPage2.triggerAngle; //Number of teeth that have passed since tooth 1, multiplied by the angle each tooth represents, plus the angle that tooth 1 is ATDC. This gives accuracy only to the nearest tooth.
-    //crankAngle += ldiv( (micros() - tempToothLastToothTime), timePerDegree).quot; //Estimate the number of degrees travelled since the last tooth
-
-
-    int crankAngle = (tempToothCurrentCount - 1) * triggerToothAngle + configPage2.triggerAngle; //Number of teeth that have passed since tooth 1, multiplied by the angle each tooth represents, plus the angle that tooth 1 is ATDC. This gives accuracy only to the nearest tooth.
+    int crankAngle = ((tempToothCurrentCount - 1) * triggerToothAngle) + configPage2.triggerAngle; //Number of teeth that have passed since tooth 1, multiplied by the angle each tooth represents, plus the angle that tooth 1 is ATDC. This gives accuracy only to the nearest tooth.
     //Estimate the number of degrees travelled since the last tooth}
     long elapsedTime = micros() - tempToothLastToothTime;
     if(elapsedTime < SHRT_MAX ) { crankAngle += div((int)elapsedTime, timePerDegree).quot; } //This option is much faster, but only available for smaller values of elapsedTime
@@ -416,6 +449,9 @@ void triggerPri_GM7X()
      toothCurrentCount = 1;
      toothOneMinusOneTime = toothOneTime;
      toothOneTime = curTime;
+
+     toothLastMinusOneToothTime = toothLastToothTime;
+     toothLastToothTime = curTime;
    }
    else
    {
@@ -425,15 +461,16 @@ void triggerPri_GM7X()
        toothCurrentCount = 3;
        currentStatus.hasSync = true;
        currentStatus.startRevolutions++; //Counter
-       return; //We return here so that the tooth times below don't get set (The magical 3rd tooth should not be considered for any calculations that use those times)
+     }
+     else
+     {
+       toothLastMinusOneToothTime = toothLastToothTime;
+       toothLastToothTime = curTime;
      }
    }
-
-   toothLastMinusOneToothTime = toothLastToothTime;
-   toothLastToothTime = curTime;
 }
 void triggerSec_GM7X() { return; } //Not required
-int getRPM_GM7X()
+uint16_t getRPM_GM7X()
 {
    return stdGetRPM();
 }
@@ -452,7 +489,7 @@ int getCrankAngle_GM7X(int timePerDegree)
     int crankAngle;
     if( tempToothCurrentCount < 3 )
     {
-      crankAngle = (tempToothCurrentCount - 1) * triggerToothAngle + 42; //Number of teeth that have passed since tooth 1, multiplied by the angle each tooth represents, plus the angle that tooth 1 is ATDC. This gives accuracy only to the nearest tooth.
+      crankAngle = ((tempToothCurrentCount - 1) * triggerToothAngle) + 42; //Number of teeth that have passed since tooth 1, multiplied by the angle each tooth represents, plus the angle that tooth 1 is ATDC. This gives accuracy only to the nearest tooth.
     }
     else if( tempToothCurrentCount == 3 )
     {
@@ -460,7 +497,7 @@ int getCrankAngle_GM7X(int timePerDegree)
     }
     else
     {
-      crankAngle = (tempToothCurrentCount - 2) * triggerToothAngle + 42; //Number of teeth that have passed since tooth 1, multiplied by the angle each tooth represents, plus the angle that tooth 1 is ATDC. This gives accuracy only to the nearest tooth.
+      crankAngle = ((tempToothCurrentCount - 2) * triggerToothAngle) + 42; //Number of teeth that have passed since tooth 1, multiplied by the angle each tooth represents, plus the angle that tooth 1 is ATDC. This gives accuracy only to the nearest tooth.
     }
 
     //Estimate the number of degrees travelled since the last tooth}
@@ -496,15 +533,6 @@ void triggerSetup_4G63()
   if(configPage1.nCylinders == 6)
   {
     // 70 / 50 for 6 cylinder applications
-    /*
-    toothAngles[0] = 355; //Rising edge of tooth #1
-    toothAngles[1] = 65; //
-    toothAngles[2] = 115; //
-    toothAngles[3] = 185; //Should be #1
-    toothAngles[4] = 235; //
-    toothAngles[5] = 305; //
-    */
-
     toothAngles[0] = 185; //
     toothAngles[1] = 235; //
     toothAngles[2] = 305; //
@@ -546,70 +574,72 @@ void triggerPri_4G63()
 {
   curTime = micros();
   curGap = curTime - toothLastToothTime;
-  if ( curGap < triggerFilterTime ) { return; } //Filter check. Pulses should never be less than triggerFilterTime
-
-  addToothLogEntry(curGap);
-  triggerFilterTime = curGap >> 2; //This only applies during non-sync conditions. If there is sync then triggerFilterTime gets changed again below with a better value.
-
-  toothLastMinusOneToothTime = toothLastToothTime;
-  toothLastToothTime = curTime;
-
-  toothCurrentCount++;
-  if(toothCurrentCount == 1 || toothCurrentCount > configPage1.nCylinders) //Trigger is on CHANGE, hence 4 pulses = 1 crank rev
+  if ( curGap >= triggerFilterTime )
   {
-     toothCurrentCount = 1; //Reset the counter
-     toothOneMinusOneTime = toothOneTime;
-     toothOneTime = curTime;
-     currentStatus.hasSync = true;
-     currentStatus.startRevolutions++; //Counter
-     //if ((startRevolutions & 15) == 1) { currentStatus.hasSync = false; } //Every 64 revolutions, force a resync with the cam
-  }
-  else if (!currentStatus.hasSync) { return; }
+    addToothLogEntry(curGap);
+    triggerFilterTime = curGap >> 2; //This only applies during non-sync conditions. If there is sync then triggerFilterTime gets changed again below with a better value.
 
-  if ( BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) && configPage2.ignCranklock)
-  {
-    if( toothCurrentCount == 1 ) { endCoil1Charge(); }
-    else if( toothCurrentCount == 3 ) { endCoil2Charge(); }
-    else if( toothCurrentCount == 5 ) { endCoil3Charge(); }
-  }
+    toothLastMinusOneToothTime = toothLastToothTime;
+    toothLastToothTime = curTime;
 
-  //Whilst this is an uneven tooth pattern, if the specific angle between the last 2 teeth is specified, 1st deriv prediction can be used
-  if(configPage2.triggerFilter == 1 || currentStatus.RPM < 1400)
-  {
-    //Lite filter
-    if(toothCurrentCount == 1 || toothCurrentCount == 3 || toothCurrentCount == 5)
+    toothCurrentCount++;
+    if( (toothCurrentCount == 1) || (toothCurrentCount > configPage1.nCylinders) ) //Trigger is on CHANGE, hence 4 pulses = 1 crank rev
     {
-      triggerToothAngle = 70;
-      triggerFilterTime = curGap; //Trigger filter is set to whatever time it took to do 70 degrees (Next trigger is 110 degrees away)
-      if(configPage1.nCylinders == 6)
-      {
-        triggerFilterTime = (curGap >> 2); //Trigger filter is set to (70/4)=17.5=17 degrees (Next trigger is 50 degrees away).
-      }
+       toothCurrentCount = 1; //Reset the counter
+       toothOneMinusOneTime = toothOneTime;
+       toothOneTime = curTime;
+       currentStatus.hasSync = true;
+       currentStatus.startRevolutions++; //Counter
     }
-    else
+
+    if (currentStatus.hasSync == true)
     {
-      triggerToothAngle = 110;
-      triggerFilterTime = (curGap * 3) >> 3; //Trigger filter is set to (110*3)/8=41.25=41 degrees (Next trigger is 70 degrees away).
-      if(configPage1.nCylinders == 6)
+      if ( BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) && configPage2.ignCranklock)
       {
-        triggerToothAngle = 50;
-        triggerFilterTime = (curGap * 3) >> 2; //Trigger filter is set to (50*3)/4=37.5=37 degrees (Next trigger is 70 degrees away).
+        if( toothCurrentCount == 1 ) { endCoil1Charge(); }
+        else if( toothCurrentCount == 3 ) { endCoil2Charge(); }
+        else if( toothCurrentCount == 5 ) { endCoil3Charge(); }
       }
-    }
-  }
-  else if(configPage2.triggerFilter == 2)
-  {
-    //Medium filter level
-    if(toothCurrentCount == 1 || toothCurrentCount == 3 || toothCurrentCount == 5) { triggerToothAngle = 70; triggerFilterTime = (curGap * 5) >> 2 ; } //87.5 degrees with a target of 110
-    else { triggerToothAngle = 110; triggerFilterTime = (curGap >> 1); } //55 degrees with a target of 70
-  }
-  else if (configPage2.triggerFilter == 3)
-  {
-    //Aggressive filter level
-    if(toothCurrentCount == 1 || toothCurrentCount == 3 || toothCurrentCount == 5) { triggerToothAngle = 70; triggerFilterTime = (curGap * 11) >> 3 ; } //96.26 degrees with a target of 110
-    else { triggerToothAngle = 110; triggerFilterTime = (curGap * 9) >> 5; } //61.87 degrees with a target of 70
-  }
-  else { triggerFilterTime = 0; } //trigger filter is turned off.
+
+      //Whilst this is an uneven tooth pattern, if the specific angle between the last 2 teeth is specified, 1st deriv prediction can be used
+      if( (configPage2.triggerFilter == 1) || (currentStatus.RPM < 1400) )
+      {
+        //Lite filter
+        if( (toothCurrentCount == 1) || (toothCurrentCount == 3) || (toothCurrentCount == 5) )
+        {
+          triggerToothAngle = 70;
+          triggerFilterTime = curGap; //Trigger filter is set to whatever time it took to do 70 degrees (Next trigger is 110 degrees away)
+          if(configPage1.nCylinders == 6)
+          {
+            triggerFilterTime = (curGap >> 2); //Trigger filter is set to (70/4)=17.5=17 degrees (Next trigger is 50 degrees away).
+          }
+        }
+        else
+        {
+          triggerToothAngle = 110;
+          triggerFilterTime = (curGap * 3) >> 3; //Trigger filter is set to (110*3)/8=41.25=41 degrees (Next trigger is 70 degrees away).
+          if(configPage1.nCylinders == 6)
+          {
+            triggerToothAngle = 50;
+            triggerFilterTime = (curGap * 3) >> 2; //Trigger filter is set to (50*3)/4=37.5=37 degrees (Next trigger is 70 degrees away).
+          }
+        }
+      }
+      else if(configPage2.triggerFilter == 2)
+      {
+        //Medium filter level
+        if( (toothCurrentCount == 1) || (toothCurrentCount == 3) || (toothCurrentCount == 5) ) { triggerToothAngle = 70; triggerFilterTime = (curGap * 5) >> 2 ; } //87.5 degrees with a target of 110
+        else { triggerToothAngle = 110; triggerFilterTime = (curGap >> 1); } //55 degrees with a target of 70
+      }
+      else if (configPage2.triggerFilter == 3)
+      {
+        //Aggressive filter level
+        if( (toothCurrentCount == 1) || (toothCurrentCount == 3) || (toothCurrentCount == 5) ) { triggerToothAngle = 70; triggerFilterTime = (curGap * 11) >> 3 ; } //96.26 degrees with a target of 110
+        else { triggerToothAngle = 110; triggerFilterTime = (curGap * 9) >> 5; } //61.87 degrees with a target of 70
+      }
+      else { triggerFilterTime = 0; } //trigger filter is turned off.
+    } //Has sync
+  } //Filter time
 
 }
 void triggerSec_4G63()
@@ -617,102 +647,102 @@ void triggerSec_4G63()
   //byte crankState = READ_PRI_TRIGGER();
   //First filter is a duration based one to ensure the pulse was of sufficient length (time)
   //if(READ_SEC_TRIGGER()) { secondaryLastToothTime1 = micros(); return; }
-  if(currentStatus.hasSync)
+  if(currentStatus.hasSync == true)
   {
-  //if ( (micros() - secondaryLastToothTime1) < triggerSecFilterTime_duration ) { return; } //1166 is the time taken to cross 70 degrees at 10k rpm
+  //1166 is the time taken to cross 70 degrees at 10k rpm
+  //if ( (micros() - secondaryLastToothTime1) < triggerSecFilterTime_duration ) { return; }
   //triggerSecFilterTime_duration = (micros() - secondaryLastToothTime1) >> 1;
   }
 
 
   curTime2 = micros();
   curGap2 = curTime2 - toothLastSecToothTime;
-  if ( curGap2 < triggerSecFilterTime ) { return; }
-  toothLastSecToothTime = curTime2;
-
-  triggerSecFilterTime = curGap2 >> 1; //Basic 50% filter for the secondary reading
-  //triggerSecFilterTime = (curGap2 * 9) >> 5; //62.5%
-  //triggerSecFilterTime = (curGap2 * 6) >> 3; //75%
-
-  if(BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) || !currentStatus.hasSync)
+  if ( curGap2 >= triggerSecFilterTime )
   {
-    triggerFilterTime = 1500; //If this is removed, can have trouble getting sync again after the engine is turned off (but ECU not reset).
-    if(READ_PRI_TRIGGER())// && (crankState == digitalRead(pinTrigger)))
+    toothLastSecToothTime = curTime2;
+
+    triggerSecFilterTime = curGap2 >> 1; //Basic 50% filter for the secondary reading
+    //More aggressive options:
+    //62.5%:
+    //triggerSecFilterTime = (curGap2 * 9) >> 5;
+    //75%:
+    //triggerSecFilterTime = (curGap2 * 6) >> 3;
+
+    if(BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) || (currentStatus.hasSync == false) )
     {
-      toothCurrentCount = 4; //If the crank trigger is currently HIGH, it means we're on tooth #1
+      triggerFilterTime = 1500; //If this is removed, can have trouble getting sync again after the engine is turned off (but ECU not reset).
+      if(READ_PRI_TRIGGER() == true)// && (crankState == digitalRead(pinTrigger)))
+      {
+        toothCurrentCount = 4; //If the crank trigger is currently HIGH, it means we're on tooth #1
+      }
     }
-}
 
-  //if ( (micros() - secondaryLastToothTime1) < triggerSecFilterTime_duration && configPage2.useResync )
-  if ( configPage2.useResync )
-  {
-    triggerSecFilterTime_duration = (micros() - secondaryLastToothTime1) >> 1;
-    if(READ_PRI_TRIGGER())// && (crankState == digitalRead(pinTrigger)))
+    //if ( (micros() - secondaryLastToothTime1) < triggerSecFilterTime_duration && configPage2.useResync )
+    if ( configPage2.useResync == 1 )
     {
-      toothCurrentCount = 4; //If the crank trigger is currently HIGH, it means we're on tooth #1
-
-      /* High-res mode
-      toothCurrentCount = 7; //If the crank trigger is currently HIGH, it means we're on the falling edge of the narrow crank tooth
-      toothLastMinusOneToothTime = toothLastToothTime;
-      toothLastToothTime = curTime;
-      */
-    }
-  }
-
-  return;
+      triggerSecFilterTime_duration = (micros() - secondaryLastToothTime1) >> 1;
+      if(READ_PRI_TRIGGER() == true)// && (crankState == digitalRead(pinTrigger)))
+      {
+        toothCurrentCount = 4; //If the crank trigger is currently HIGH, it means we're on tooth #1
+      }
+    } // Use resync
+  } //Trigger filter
 }
 
 
-int getRPM_4G63()
+uint16_t getRPM_4G63()
 {
+  uint16_t tempRPM = 0;
   //During cranking, RPM is calculated 4 times per revolution, once for each rising/falling of the crank signal.
   //Because these signals aren't even (Alternating 110 and 70 degrees), this needs a special function
-  if(!currentStatus.hasSync) { return 0; }
-  if( currentStatus.RPM < (unsigned int)(configPage2.crankRPM * 100) )
+  if(currentStatus.hasSync == true)
   {
-    //if(currentStatus.startRevolutions < 2) { return 0; } //Need at least 2 full revolutions to prevent crazy initial rpm value
-    int tempToothAngle;
-    unsigned long toothTime;
-    if(toothLastToothTime == 0 || toothLastMinusOneToothTime == 0) { return 0; }
-
-    noInterrupts();
-    tempToothAngle = triggerToothAngle;
-    /* High-res mode
-    if(toothCurrentCount == 1) { tempToothAngle = 70; }
-    else { tempToothAngle = toothAngles[toothCurrentCount-1] - toothAngles[toothCurrentCount-2]; }
-    */
-    //revolutionTime = (toothOneTime - toothOneMinusOneTime); //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)
-    toothTime = (toothLastToothTime - toothLastMinusOneToothTime); //Note that trigger tooth angle changes between 70 and 110 depending on the last tooth that was seen
-    interrupts();
-    toothTime = toothTime * 36;
-    int tempRPM = ((unsigned long)tempToothAngle * 6000000UL) / toothTime;
-    return tempRPM;
+    if( currentStatus.RPM < (unsigned int)(configPage2.crankRPM * 100) )
+    {
+      int tempToothAngle;
+      unsigned long toothTime;
+      if( (toothLastToothTime == 0) || (toothLastMinusOneToothTime == 0) ) { tempRPM = 0; }
+      else
+      {
+        noInterrupts();
+        tempToothAngle = triggerToothAngle;
+        toothTime = (toothLastToothTime - toothLastMinusOneToothTime); //Note that trigger tooth angle changes between 70 and 110 depending on the last tooth that was seen
+        interrupts();
+        toothTime = toothTime * 36;
+        tempRPM = ((unsigned long)tempToothAngle * 6000000UL) / toothTime;
+      }
+    }
+    else { tempRPM = stdGetRPM(); }
   }
-  else { return stdGetRPM(); }
+
+  return tempRPM;
 }
 
 int getCrankAngle_4G63(int timePerDegree)
 {
-    if(!currentStatus.hasSync) { return 0;}
-    //This is the current angle ATDC the engine is at. This is the last known position based on what tooth was last 'seen'. It is only accurate to the resolution of the trigger wheel (Eg 36-1 is 10 degrees)
-    unsigned long tempToothLastToothTime;
-    int tempToothCurrentCount;
-    //Grab some variables that are used in the trigger code and assign them to temp variables.
-    noInterrupts();
-    tempToothCurrentCount = toothCurrentCount;
-    tempToothLastToothTime = toothLastToothTime;
-    interrupts();
+    int crankAngle = 0;
+    if(currentStatus.hasSync == true)
+    {
+      //This is the current angle ATDC the engine is at. This is the last known position based on what tooth was last 'seen'. It is only accurate to the resolution of the trigger wheel (Eg 36-1 is 10 degrees)
+      unsigned long tempToothLastToothTime;
+      int tempToothCurrentCount;
+      //Grab some variables that are used in the trigger code and assign them to temp variables.
+      noInterrupts();
+      tempToothCurrentCount = toothCurrentCount;
+      tempToothLastToothTime = toothLastToothTime;
+      interrupts();
 
-    int crankAngle = toothAngles[(tempToothCurrentCount - 1)] + configPage2.triggerAngle; //Perform a lookup of the fixed toothAngles array to find what the angle of the last tooth passed was.
-    //Estimate the number of degrees travelled since the last tooth}
+      crankAngle = toothAngles[(tempToothCurrentCount - 1)] + configPage2.triggerAngle; //Perform a lookup of the fixed toothAngles array to find what the angle of the last tooth passed was.
+      //Estimate the number of degrees travelled since the last tooth}
 
-    unsigned long elapsedTime = micros() - tempToothLastToothTime;
-    if(elapsedTime < SHRT_MAX ) { crankAngle += div((int)elapsedTime, timePerDegree).quot; } //This option is much faster, but only available for smaller values of elapsedTime
-    else { crankAngle += ldiv(elapsedTime, timePerDegree).quot; }
-
-    if (crankAngle >= 720) { crankAngle -= 720; }
-    if (crankAngle > CRANK_ANGLE_MAX) { crankAngle -= CRANK_ANGLE_MAX; }
-    if (crankAngle < 0) { crankAngle += 360; }
+      unsigned long elapsedTime = micros() - tempToothLastToothTime;
+      if(elapsedTime < SHRT_MAX ) { crankAngle += div((int)elapsedTime, timePerDegree).quot; } //This option is much faster, but only available for smaller values of elapsedTime
+      else { crankAngle += ldiv(elapsedTime, timePerDegree).quot; }
 
+      if (crankAngle >= 720) { crankAngle -= 720; }
+      if (crankAngle > CRANK_ANGLE_MAX) { crankAngle -= CRANK_ANGLE_MAX; }
+      if (crankAngle < 0) { crankAngle += 360; }
+    }
     return crankAngle;
 }
 
@@ -759,34 +789,38 @@ void triggerSetup_24X()
 
 void triggerPri_24X()
 {
-  if(toothCurrentCount == 25) { currentStatus.hasSync = false; return; } //Indicates sync has not been achieved (Still waiting for 1 revolution of the crank to take place)
-  curTime = micros();
-  curGap = curTime - toothLastToothTime;
-
-  if(toothCurrentCount == 0)
-  {
-     toothCurrentCount = 1; //Reset the counter
-     toothOneMinusOneTime = toothOneTime;
-     toothOneTime = curTime;
-     currentStatus.hasSync = true;
-     currentStatus.startRevolutions++; //Counter
-  }
+  if(toothCurrentCount == 25) { currentStatus.hasSync = false; } //Indicates sync has not been achieved (Still waiting for 1 revolution of the crank to take place)
   else
   {
-    toothCurrentCount++; //Increment the tooth counter
+    curTime = micros();
+    curGap = curTime - toothLastToothTime;
+
+    if(toothCurrentCount == 0)
+    {
+       toothCurrentCount = 1; //Reset the counter
+       toothOneMinusOneTime = toothOneTime;
+       toothOneTime = curTime;
+       currentStatus.hasSync = true;
+       currentStatus.startRevolutions++; //Counter
+    }
+    else
+    {
+      toothCurrentCount++; //Increment the tooth counter
+    }
+
+    addToothLogEntry(curGap);
+
+    toothLastToothTime = curTime;
   }
-
-   addToothLogEntry(curGap);
-
-   toothLastToothTime = curTime;
 }
+
 void triggerSec_24X()
 {
   toothCurrentCount = 0; //All we need to do is reset the tooth count back to zero, indicating that we're at the beginning of a new revolution
   return;
 }
 
-int getRPM_24X()
+uint16_t getRPM_24X()
 {
    return stdGetRPM();
 }
@@ -848,30 +882,34 @@ void triggerSetup_Jeep2000()
 
 void triggerPri_Jeep2000()
 {
-  if(toothCurrentCount == 13) { currentStatus.hasSync = false; return; } //Indicates sync has not been achieved (Still waiting for 1 revolution of the crank to take place)
-  curTime = micros();
-  curGap = curTime - toothLastToothTime;
-  if ( curGap < triggerFilterTime ) { return; } //Noise rejection check. Pulses should never be less than triggerFilterTime
-
-  if(toothCurrentCount == 0)
-  {
-     toothCurrentCount = 1; //Reset the counter
-     toothOneMinusOneTime = toothOneTime;
-     toothOneTime = curTime;
-     currentStatus.hasSync = true;
-     currentStatus.startRevolutions++; //Counter
-  }
+  if(toothCurrentCount == 13) { currentStatus.hasSync = false; } //Indicates sync has not been achieved (Still waiting for 1 revolution of the crank to take place)
   else
   {
-    toothCurrentCount++; //Increment the tooth counter
-  }
+    curTime = micros();
+    curGap = curTime - toothLastToothTime;
+    if ( curGap >= triggerFilterTime )
+    {
+      if(toothCurrentCount == 0)
+      {
+         toothCurrentCount = 1; //Reset the counter
+         toothOneMinusOneTime = toothOneTime;
+         toothOneTime = curTime;
+         currentStatus.hasSync = true;
+         currentStatus.startRevolutions++; //Counter
+      }
+      else
+      {
+        toothCurrentCount++; //Increment the tooth counter
+      }
 
-  setFilter(curGap); //Recalc the new filter value
+      setFilter(curGap); //Recalc the new filter value
 
-  addToothLogEntry(curGap);
+      addToothLogEntry(curGap);
 
-  toothLastMinusOneToothTime = toothLastToothTime;
-  toothLastToothTime = curTime;
+      toothLastMinusOneToothTime = toothLastToothTime;
+      toothLastToothTime = curTime;
+    } //Trigger filter
+  } //Sync check
 }
 void triggerSec_Jeep2000()
 {
@@ -879,7 +917,7 @@ void triggerSec_Jeep2000()
   return;
 }
 
-int getRPM_Jeep2000()
+uint16_t getRPM_Jeep2000()
 {
    return stdGetRPM();
 }
@@ -930,30 +968,38 @@ void triggerPri_Audi135()
 {
    curTime = micros();
    curGap = curTime - toothSystemLastToothTime;
-   if ( curGap < triggerFilterTime ) { return; }
-   toothSystemCount++;
-
-   if ( !currentStatus.hasSync ) { toothLastToothTime = curTime; return; }
-   if ( toothSystemCount < 3 ) { return; } //We only proceed for every third tooth
-
-   addToothLogEntry(curGap);
-   toothSystemLastToothTime = curTime;
-   toothSystemCount = 0;
-   toothCurrentCount++; //Increment the tooth counter
-
-   if ( toothCurrentCount == 1 || toothCurrentCount > 45 )
+   if ( curGap > triggerFilterTime )
    {
-     toothCurrentCount = 1;
-     toothOneMinusOneTime = toothOneTime;
-     toothOneTime = curTime;
-     revolutionOne = !revolutionOne;
-     currentStatus.startRevolutions++; //Counter
-   }
+     toothSystemCount++;
 
-   setFilter(curGap); //Recalc the new filter value
+     if ( currentStatus.hasSync == false ) { toothLastToothTime = curTime; }
+     else
+     {
+       if ( toothSystemCount >= 3 )
+       {
+         //We only proceed for every third tooth
 
-   toothLastMinusOneToothTime = toothLastToothTime;
-   toothLastToothTime = curTime;
+         addToothLogEntry(curGap);
+         toothSystemLastToothTime = curTime;
+         toothSystemCount = 0;
+         toothCurrentCount++; //Increment the tooth counter
+
+         if ( (toothCurrentCount == 1) || (toothCurrentCount > 45) )
+         {
+           toothCurrentCount = 1;
+           toothOneMinusOneTime = toothOneTime;
+           toothOneTime = curTime;
+           revolutionOne = !revolutionOne;
+           currentStatus.startRevolutions++; //Counter
+         }
+
+         setFilter(curGap); //Recalc the new filter value
+
+         toothLastMinusOneToothTime = toothLastToothTime;
+         toothLastToothTime = curTime;
+       } //3rd tooth check
+     } // Sync check
+   } // Trigger filter
 }
 
 void triggerSec_Audi135()
@@ -965,18 +1011,18 @@ void triggerSec_Audi135()
   toothLastSecToothTime = curTime2;
   */
 
-  if( !currentStatus.hasSync )
+  if( currentStatus.hasSync == false )
   {
     toothCurrentCount = 0;
     currentStatus.hasSync = true;
     toothSystemCount = 3; //Need to set this to 3 so that the next primary tooth is counted
   }
-  else if (configPage2.useResync) { toothCurrentCount = 0; toothSystemCount = 3; }
-  else if ( currentStatus.startRevolutions < 100 && toothCurrentCount != 45 ) { toothCurrentCount = 0; }
+  else if (configPage2.useResync == 1) { toothCurrentCount = 0; toothSystemCount = 3; }
+  else if ( (currentStatus.startRevolutions < 100) && (toothCurrentCount != 45) ) { toothCurrentCount = 0; }
   revolutionOne = 1; //Sequential revolution reset
 }
 
-int getRPM_Audi135()
+uint16_t getRPM_Audi135()
 {
    return stdGetRPM();
 }
@@ -997,7 +1043,7 @@ int getCrankAngle_Audi135(int timePerDegree)
     //Handle case where the secondary tooth was the last one seen
     if(tempToothCurrentCount == 0) { tempToothCurrentCount = 45; }
 
-    int crankAngle = (tempToothCurrentCount - 1) * triggerToothAngle + configPage2.triggerAngle; //Number of teeth that have passed since tooth 1, multiplied by the angle each tooth represents, plus the angle that tooth 1 is ATDC. This gives accuracy only to the nearest tooth.
+    int crankAngle = ((tempToothCurrentCount - 1) * triggerToothAngle) + configPage2.triggerAngle; //Number of teeth that have passed since tooth 1, multiplied by the angle each tooth represents, plus the angle that tooth 1 is ATDC. This gives accuracy only to the nearest tooth.
     //Estimate the number of degrees travelled since the last tooth}
     long elapsedTime = micros() - tempToothLastToothTime;
     if(elapsedTime < SHRT_MAX ) { crankAngle += div((int)elapsedTime, timePerDegree).quot; } //This option is much faster, but only available for smaller values of elapsedTime
@@ -1036,16 +1082,18 @@ void triggerPri_HondaD17()
    addToothLogEntry(curGap);
 
    //
-   if( toothCurrentCount == 13 && currentStatus.hasSync)
+   if( (toothCurrentCount == 13) && (currentStatus.hasSync == true) )
    {
      toothCurrentCount = 0;
-     return;
    }
-   else if( toothCurrentCount == 1 && currentStatus.hasSync)
+   else if( (toothCurrentCount == 1) && (currentStatus.hasSync == true) )
    {
      toothOneMinusOneTime = toothOneTime;
      toothOneTime = curTime;
      currentStatus.startRevolutions++; //Counter
+
+     toothLastMinusOneToothTime = toothLastToothTime;
+     toothLastToothTime = curTime;
    }
    else
    {
@@ -1055,15 +1103,18 @@ void triggerPri_HondaD17()
      {
        toothCurrentCount = 0;
        currentStatus.hasSync = true;
-       return; //We return here so that the tooth times below don't get set (The magical 13th tooth should not be considered for any calculations that use those times)
+     }
+     else
+     {
+       //The tooth times below don't get set on tooth 13(The magical 13th tooth should not be considered for any calculations that use those times)
+       toothLastMinusOneToothTime = toothLastToothTime;
+       toothLastToothTime = curTime;
      }
    }
 
-   toothLastMinusOneToothTime = toothLastToothTime;
-   toothLastToothTime = curTime;
 }
 void triggerSec_HondaD17() { return; } //The 4+1 signal on the cam is yet to be supported
-int getRPM_HondaD17()
+uint16_t getRPM_HondaD17()
 {
    return stdGetRPM();
 }
@@ -1082,11 +1133,11 @@ int getCrankAngle_HondaD17(int timePerDegree)
     int crankAngle;
     if( tempToothCurrentCount == 0 )
     {
-      crankAngle = 11 * triggerToothAngle + configPage2.triggerAngle; //if temptoothCurrentCount is 0, the last tooth seen was the 13th one. Based on this, ignore the 13th tooth and use the 12th one as the last reference.
+      crankAngle = (11 * triggerToothAngle) + configPage2.triggerAngle; //if temptoothCurrentCount is 0, the last tooth seen was the 13th one. Based on this, ignore the 13th tooth and use the 12th one as the last reference.
     }
     else
     {
-      crankAngle = (tempToothCurrentCount - 1) * triggerToothAngle + configPage2.triggerAngle; //Number of teeth that have passed since tooth 1, multiplied by the angle each tooth represents, plus the angle that tooth 1 is ATDC. This gives accuracy only to the nearest tooth.
+      crankAngle = ((tempToothCurrentCount - 1) * triggerToothAngle) + configPage2.triggerAngle; //Number of teeth that have passed since tooth 1, multiplied by the angle each tooth represents, plus the angle that tooth 1 is ATDC. This gives accuracy only to the nearest tooth.
     }
 
     //Estimate the number of degrees travelled since the last tooth}
@@ -1131,59 +1182,63 @@ void triggerPri_Miata9905()
 {
   curTime = micros();
   curGap = curTime - toothLastToothTime;
-  if ( curGap < triggerFilterTime ) { return; } //Debounce check. Pulses should never be less than triggerFilterTime
-
-  toothCurrentCount++;
-  if(toothCurrentCount == 1 || toothCurrentCount == 5) //Trigger is on CHANGE, hence 4 pulses = 1 crank rev
+  if ( curGap >= triggerFilterTime )
   {
-     toothCurrentCount = 1; //Reset the counter
-     toothOneMinusOneTime = toothOneTime;
-     toothOneTime = curTime;
-     currentStatus.hasSync = true;
-     currentStatus.startRevolutions++; //Counter
-     //if ((startRevolutions & 15) == 1) { currentStatus.hasSync = false; } //Every 64 revolutions, force a resync with the cam
-  }
-  else if (!currentStatus.hasSync) { return; }
+    toothCurrentCount++;
+    if( (toothCurrentCount == 1) || (toothCurrentCount == 5) ) //Trigger is on CHANGE, hence 4 pulses = 1 crank rev
+    {
+       toothCurrentCount = 1; //Reset the counter
+       toothOneMinusOneTime = toothOneTime;
+       toothOneTime = curTime;
+       currentStatus.hasSync = true;
+       currentStatus.startRevolutions++; //Counter
+    }
 
-  addToothLogEntry(curGap);
+    if (currentStatus.hasSync == true)
+    {
+      addToothLogEntry(curGap);
 
-  //Whilst this is an uneven tooth pattern, if the specific angle between the last 2 teeth is specified, 1st deriv prediction can be used
-  if(toothCurrentCount == 1 || toothCurrentCount == 3) { triggerToothAngle = 70; triggerFilterTime = curGap; } //Trigger filter is set to whatever time it took to do 70 degrees (Next trigger is 110 degrees away)
-  else { triggerToothAngle = 110; triggerFilterTime = (curGap * 3) >> 3; } //Trigger filter is set to (110*3)/8=41.25=41 degrees (Next trigger is 70 degrees away).
+      //Whilst this is an uneven tooth pattern, if the specific angle between the last 2 teeth is specified, 1st deriv prediction can be used
+      if( (toothCurrentCount == 1) || (toothCurrentCount == 3) ) { triggerToothAngle = 70; triggerFilterTime = curGap; } //Trigger filter is set to whatever time it took to do 70 degrees (Next trigger is 110 degrees away)
+      else { triggerToothAngle = 110; triggerFilterTime = (curGap * 3) >> 3; } //Trigger filter is set to (110*3)/8=41.25=41 degrees (Next trigger is 70 degrees away).
 
-  curGap = curGap >> 1;
+      curGap = curGap >> 1;
 
-  toothLastMinusOneToothTime = toothLastToothTime;
-  toothLastToothTime = curTime;
+      toothLastMinusOneToothTime = toothLastToothTime;
+      toothLastToothTime = curTime;
+    } //Has sync
+  } //Trigger filter
 }
+
 void triggerSec_Miata9905()
 {
   curTime2 = micros();
   curGap2 = curTime2 - toothLastSecToothTime;
-  if ( curGap2 < triggerSecFilterTime ) { return; }
-  toothLastSecToothTime = curTime2;
-  lastGap = curGap2;
-
-  if(BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) || !currentStatus.hasSync)
+  if ( curGap2 >= triggerSecFilterTime )
   {
-    triggerFilterTime = 1500;
-    //Check the status of the crank trigger
-    targetGap = (lastGap) >> 1; //The target gap is set at half the last tooth gap
-    if ( curGap < targetGap) //If the gap between this tooth and the last one is less than half of the previous gap, then we are very likely at the extra (3rd) tooth on the cam). This tooth is located at 421 crank degrees (aka 61 degrees) and therefore the last crank tooth seen was number 1 (At 350 degrees)
+    toothLastSecToothTime = curTime2;
+    lastGap = curGap2;
+
+    if( (BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK)) || (currentStatus.hasSync == false) )
     {
-      toothCurrentCount = 1;
-      currentStatus.hasSync = true;
+      triggerFilterTime = 1500;
+      //Check the status of the crank trigger
+      targetGap = (lastGap) >> 1; //The target gap is set at half the last tooth gap
+      if ( curGap < targetGap) //If the gap between this tooth and the last one is less than half of the previous gap, then we are very likely at the extra (3rd) tooth on the cam). This tooth is located at 421 crank degrees (aka 61 degrees) and therefore the last crank tooth seen was number 1 (At 350 degrees)
+      {
+        toothCurrentCount = 1;
+        currentStatus.hasSync = true;
+      }
     }
   }
-  //else { triggerFilterTime = 1500; } //reset filter time (ugly)
-  return;
 }
 
 
-int getRPM_Miata9905()
+uint16_t getRPM_Miata9905()
 {
   //During cranking, RPM is calculated 4 times per revolution, once for each tooth on the crank signal.
   //Because these signals aren't even (Alternating 110 and 70 degrees), this needs a special function
+  uint16_t tempRPM = 0;
   if(currentStatus.RPM < configPage2.crankRPM)
   {
     int tempToothAngle;
@@ -1192,33 +1247,38 @@ int getRPM_Miata9905()
     revolutionTime = (toothLastToothTime - toothLastMinusOneToothTime); //Note that trigger tooth angle changes between 70 and 110 depending on the last tooth that was seen
     interrupts();
     revolutionTime = revolutionTime * 36;
-    return (tempToothAngle * 60000000L) / revolutionTime;
+    tempRPM = (tempToothAngle * 60000000L) / revolutionTime;
   }
-  else { return stdGetRPM(); }
+  else { tempRPM = stdGetRPM(); }
+
+  return tempRPM;
 }
 
 int getCrankAngle_Miata9905(int timePerDegree)
 {
-    if(!currentStatus.hasSync) { return 0;}
-    //This is the current angle ATDC the engine is at. This is the last known position based on what tooth was last 'seen'. It is only accurate to the resolution of the trigger wheel (Eg 36-1 is 10 degrees)
-    unsigned long tempToothLastToothTime;
-    int tempToothCurrentCount;
-    //Grab some variables that are used in the trigger code and assign them to temp variables.
-    noInterrupts();
-    tempToothCurrentCount = toothCurrentCount;
-    tempToothLastToothTime = toothLastToothTime;
-    interrupts();
+    int crankAngle = 0;
+    if(currentStatus.hasSync == true)
+    {
+      //This is the current angle ATDC the engine is at. This is the last known position based on what tooth was last 'seen'. It is only accurate to the resolution of the trigger wheel (Eg 36-1 is 10 degrees)
+      unsigned long tempToothLastToothTime;
+      int tempToothCurrentCount;
+      //Grab some variables that are used in the trigger code and assign them to temp variables.
+      noInterrupts();
+      tempToothCurrentCount = toothCurrentCount;
+      tempToothLastToothTime = toothLastToothTime;
+      interrupts();
 
-    int crankAngle = toothAngles[(tempToothCurrentCount - 1)] + configPage2.triggerAngle; //Perform a lookup of the fixed toothAngles array to find what the angle of the last tooth passed was.
-    //Estimate the number of degrees travelled since the last tooth}
+      crankAngle = toothAngles[(tempToothCurrentCount - 1)] + configPage2.triggerAngle; //Perform a lookup of the fixed toothAngles array to find what the angle of the last tooth passed was.
+      //Estimate the number of degrees travelled since the last tooth}
 
-    long elapsedTime = micros() - tempToothLastToothTime;
-    if(elapsedTime < SHRT_MAX ) { crankAngle += div((int)elapsedTime, timePerDegree).quot; } //This option is much faster, but only available for smaller values of elapsedTime
-    else { crankAngle += ldiv(elapsedTime, timePerDegree).quot; }
+      long elapsedTime = micros() - tempToothLastToothTime;
+      if(elapsedTime < SHRT_MAX ) { crankAngle += div((int)elapsedTime, timePerDegree).quot; } //This option is much faster, but only available for smaller values of elapsedTime
+      else { crankAngle += ldiv(elapsedTime, timePerDegree).quot; }
 
-    if (crankAngle >= 720) { crankAngle -= 720; }
-    if (crankAngle > CRANK_ANGLE_MAX) { crankAngle -= CRANK_ANGLE_MAX; }
-    if (crankAngle < 0) { crankAngle += 360; }
+      if (crankAngle >= 720) { crankAngle -= 720; }
+      if (crankAngle > CRANK_ANGLE_MAX) { crankAngle -= CRANK_ANGLE_MAX; }
+      if (crankAngle < 0) { crankAngle += 360; }
+    }
 
     return crankAngle;
 }
@@ -1252,36 +1312,39 @@ void triggerPri_MazdaAU()
 {
   curTime = micros();
   curGap = curTime - toothLastToothTime;
-  if ( curGap < triggerFilterTime ) { return; } //Filter check. Pulses should never be less than triggerFilterTime
-
-  addToothLogEntry(curGap);
-
-  toothCurrentCount++;
-  if(toothCurrentCount == 1 || toothCurrentCount == 5) //Trigger is on CHANGE, hence 4 pulses = 1 crank rev
+  if ( curGap >= triggerFilterTime )
   {
-     toothCurrentCount = 1; //Reset the counter
-     toothOneMinusOneTime = toothOneTime;
-     toothOneTime = curTime;
-     currentStatus.hasSync = true;
-     currentStatus.startRevolutions++; //Counter
-     //if ((startRevolutions & 15) == 1) { currentStatus.hasSync = false; } //Every 64 revolutions, force a resync with the cam. For testing only!
-  }
-  else if (!currentStatus.hasSync) { return; }
+    addToothLogEntry(curGap);
 
-  // Locked cranking timing is available, fixed at 12* BTDC
-  if ( BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) && configPage2.ignCranklock )
-  {
-    if( toothCurrentCount == 1 ) { endCoil1Charge(); }
-    else if( toothCurrentCount == 3 ) { endCoil2Charge(); }
-  }
+    toothCurrentCount++;
+    if( (toothCurrentCount == 1) || (toothCurrentCount == 5) ) //Trigger is on CHANGE, hence 4 pulses = 1 crank rev
+    {
+       toothCurrentCount = 1; //Reset the counter
+       toothOneMinusOneTime = toothOneTime;
+       toothOneTime = curTime;
+       currentStatus.hasSync = true;
+       currentStatus.startRevolutions++; //Counter
+    }
 
-  //Whilst this is an uneven tooth pattern, if the specific angle between the last 2 teeth is specified, 1st deriv prediction can be used
-  if(toothCurrentCount == 1 || toothCurrentCount == 3) { triggerToothAngle = 72; triggerFilterTime = curGap; } //Trigger filter is set to whatever time it took to do 72 degrees (Next trigger is 108 degrees away)
-  else { triggerToothAngle = 108; triggerFilterTime = (curGap * 3) >> 3; } //Trigger filter is set to (108*3)/8=40 degrees (Next trigger is 70 degrees away).
+    if (currentStatus.hasSync == true)
+    {
+      // Locked cranking timing is available, fixed at 12* BTDC
+      if ( BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) && configPage2.ignCranklock )
+      {
+        if( toothCurrentCount == 1 ) { endCoil1Charge(); }
+        else if( toothCurrentCount == 3 ) { endCoil2Charge(); }
+      }
 
-  toothLastMinusOneToothTime = toothLastToothTime;
-  toothLastToothTime = curTime;
+      //Whilst this is an uneven tooth pattern, if the specific angle between the last 2 teeth is specified, 1st deriv prediction can be used
+      if( (toothCurrentCount == 1) || (toothCurrentCount == 3) ) { triggerToothAngle = 72; triggerFilterTime = curGap; } //Trigger filter is set to whatever time it took to do 72 degrees (Next trigger is 108 degrees away)
+      else { triggerToothAngle = 108; triggerFilterTime = (curGap * 3) >> 3; } //Trigger filter is set to (108*3)/8=40 degrees (Next trigger is 70 degrees away).
+
+      toothLastMinusOneToothTime = toothLastToothTime;
+      toothLastToothTime = curTime;
+    } //Has sync
+  } //Filter time
 }
+
 void triggerSec_MazdaAU()
 {
   curTime2 = micros();
@@ -1290,8 +1353,8 @@ void triggerSec_MazdaAU()
   //if ( curGap2 < triggerSecFilterTime ) { return; }
   toothLastSecToothTime = curTime2;
 
-  //if(BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) || !currentStatus.hasSync) //Not sure if the cranking check is needed here
-  if(!currentStatus.hasSync)
+  //if(BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) || currentStatus.hasSync == false)
+  if(currentStatus.hasSync == false)
   {
     //we find sync by looking for the 2 teeth that are close together. The next crank tooth after that is the one we're looking for.
     //For the sake of this decoder, the lone cam tooth will be designated #1
@@ -1316,47 +1379,54 @@ void triggerSec_MazdaAU()
 }
 
 
-int getRPM_MazdaAU()
+uint16_t getRPM_MazdaAU()
 {
-  if (!currentStatus.hasSync) { return 0; }
+  uint16_t tempRPM = 0;
 
-  //During cranking, RPM is calculated 4 times per revolution, once for each tooth on the crank signal.
-  //Because these signals aren't even (Alternating 108 and 72 degrees), this needs a special function
-  if(currentStatus.RPM < configPage2.crankRPM)
+  if (currentStatus.hasSync == true)
   {
-    int tempToothAngle;
-    noInterrupts();
-    tempToothAngle = triggerToothAngle;
-    revolutionTime = (toothLastToothTime - toothLastMinusOneToothTime); //Note that trigger tooth angle changes between 72 and 108 depending on the last tooth that was seen
-    interrupts();
-    revolutionTime = revolutionTime * 36;
-    return (tempToothAngle * 60000000L) / revolutionTime;
+    //During cranking, RPM is calculated 4 times per revolution, once for each tooth on the crank signal.
+    //Because these signals aren't even (Alternating 108 and 72 degrees), this needs a special function
+    if(currentStatus.RPM < configPage2.crankRPM)
+    {
+      int tempToothAngle;
+      noInterrupts();
+      tempToothAngle = triggerToothAngle;
+      revolutionTime = (toothLastToothTime - toothLastMinusOneToothTime); //Note that trigger tooth angle changes between 72 and 108 depending on the last tooth that was seen
+      interrupts();
+      revolutionTime = revolutionTime * 36;
+      tempRPM = (tempToothAngle * 60000000L) / revolutionTime;
+    }
+    else { tempRPM = stdGetRPM(); }
   }
-  else { return stdGetRPM(); }
+  return tempRPM;
 }
 
 int getCrankAngle_MazdaAU(int timePerDegree)
 {
-    if(!currentStatus.hasSync) { return 0;}
-    //This is the current angle ATDC the engine is at. This is the last known position based on what tooth was last 'seen'. It is only accurate to the resolution of the trigger wheel (Eg 36-1 is 10 degrees)
-    unsigned long tempToothLastToothTime;
-    int tempToothCurrentCount;
-    //Grab some variables that are used in the trigger code and assign them to temp variables.
-    noInterrupts();
-    tempToothCurrentCount = toothCurrentCount;
-    tempToothLastToothTime = toothLastToothTime;
-    interrupts();
+    int crankAngle = 0;
+    if(currentStatus.hasSync == true)
+    {
+      //This is the current angle ATDC the engine is at. This is the last known position based on what tooth was last 'seen'. It is only accurate to the resolution of the trigger wheel (Eg 36-1 is 10 degrees)
+      unsigned long tempToothLastToothTime;
+      int tempToothCurrentCount;
+      //Grab some variables that are used in the trigger code and assign them to temp variables.
+      noInterrupts();
+      tempToothCurrentCount = toothCurrentCount;
+      tempToothLastToothTime = toothLastToothTime;
+      interrupts();
 
-    int crankAngle = toothAngles[(tempToothCurrentCount - 1)] + configPage2.triggerAngle; //Perform a lookup of the fixed toothAngles array to find what the angle of the last tooth passed was.
-    //Estimate the number of degrees travelled since the last tooth}
+      crankAngle = toothAngles[(tempToothCurrentCount - 1)] + configPage2.triggerAngle; //Perform a lookup of the fixed toothAngles array to find what the angle of the last tooth passed was.
+      //Estimate the number of degrees travelled since the last tooth}
 
-    long elapsedTime = micros() - tempToothLastToothTime;
-    if(elapsedTime < SHRT_MAX ) { crankAngle += div((int)elapsedTime, timePerDegree).quot; } //This option is much faster, but only available for smaller values of elapsedTime
-    else { crankAngle += ldiv(elapsedTime, timePerDegree).quot; }
+      long elapsedTime = micros() - tempToothLastToothTime;
+      if(elapsedTime < SHRT_MAX ) { crankAngle += div((int)elapsedTime, timePerDegree).quot; } //This option is much faster, but only available for smaller values of elapsedTime
+      else { crankAngle += ldiv(elapsedTime, timePerDegree).quot; }
 
-    if (crankAngle >= 720) { crankAngle -= 720; }
-    if (crankAngle > CRANK_ANGLE_MAX) { crankAngle -= CRANK_ANGLE_MAX; }
-    if (crankAngle < 0) { crankAngle += 360; }
+      if (crankAngle >= 720) { crankAngle -= 720; }
+      if (crankAngle > CRANK_ANGLE_MAX) { crankAngle -= CRANK_ANGLE_MAX; }
+      if (crankAngle < 0) { crankAngle += 360; }
+    }
 
     return crankAngle;
 }
@@ -1388,11 +1458,15 @@ void triggerSec_non360()
   //This is not used, the trigger is identical to the dual wheel one, so that is used instead.
 }
 
-int getRPM_non360()
+uint16_t getRPM_non360()
 {
-  if( !currentStatus.hasSync || toothCurrentCount == 0 ) { return 0; }
-  if(currentStatus.RPM < configPage2.crankRPM) { return crankingGetRPM(configPage2.triggerTeeth); }
-  return stdGetRPM();
+  uint16_t tempRPM = 0;
+  if( (currentStatus.hasSync == true) && (toothCurrentCount != 0) )
+  {
+    if(currentStatus.RPM < configPage2.crankRPM) { tempRPM = crankingGetRPM(configPage2.triggerTeeth); }
+    else { tempRPM = stdGetRPM(); }
+  }
+  return tempRPM;
 }
 
 int getCrankAngle_non360(int timePerDegree)
@@ -1447,14 +1521,14 @@ void triggerPri_Nissan360()
 {
    curTime = micros();
    //curGap = curTime - toothLastToothTime;
-   //if ( curGap < triggerFilterTime ) { return; } //Pulses should never be less than triggerFilterTime, so if they are it means a false trigger.
+   //if ( curGap < triggerFilterTime ) { return; }
    toothCurrentCount++; //Increment the tooth counter
    //addToothLogEntry(curGap); Disable tooth logging on this decoder due to overhead
 
    toothLastMinusOneToothTime = toothLastToothTime;
    toothLastToothTime = curTime;
 
-   //if ( !currentStatus.hasSync ) { return; }
+   //if ( currentStatus.hasSync == false ) { return; }
 
    if ( toothCurrentCount == 181) //1 complete crank revolution
    {
@@ -1470,7 +1544,8 @@ void triggerPri_Nissan360()
      currentStatus.startRevolutions++; //Counter
    }
 
-   //setFilter(curGap); //Recalc the new filter value
+   //Recalc the new filter value
+   //setFilter(curGap);
 }
 
 void triggerSec_Nissan360()
@@ -1479,113 +1554,116 @@ void triggerSec_Nissan360()
   curGap2 = curTime2 - toothLastSecToothTime;
   //if ( curGap2 < triggerSecFilterTime ) { return; }
   toothLastSecToothTime = curTime2;
-  //triggerSecFilterTime = curGap2 >> 2; //Set filter at 25% of the current speed
+  //OPTIONAL: Set filter at 25% of the current speed
+  //triggerSecFilterTime = curGap2 >> 2;
 
 
   //Calculate number of primary teeth that this window has been active for
-  if(secondaryToothCount == 0) { secondaryToothCount = toothCurrentCount; return; } //This occurs on the first rotation upon powerup
-  if(READ_SEC_TRIGGER() == LOW) { secondaryToothCount = toothCurrentCount; return; } //This represents the start of a secondary window
-
-  //If we reach here, we are at the end of a secondary window
-  byte secondaryDuration = toothCurrentCount - secondaryToothCount; //How many primary teeth have passed during the duration of this secondary window
-
-  if(!currentStatus.hasSync)
-  {
-    if(configPage1.nCylinders == 4)
-    {
-      if(secondaryDuration >= 15 || secondaryDuration <= 17) //Duration of window = 16 primary teeth
-      {
-        toothCurrentCount = 16; //End of first window (The longest) occurs 16 teeth after TDC
-        currentStatus.hasSync = true;
-      }
-      else if(secondaryDuration >= 11 || secondaryDuration <= 13) //Duration of window = 12 primary teeth
-      {
-        toothCurrentCount = 102; //End of second window is after 90+12 primary teeth
-        currentStatus.hasSync = true;
-      }
-      else if(secondaryDuration >= 7 || secondaryDuration <= 9) //Duration of window = 8 primary teeth
-      {
-        toothCurrentCount = 188; //End of third window is after 90+90+8 primary teeth
-        currentStatus.hasSync = true;
-      }
-      else if(secondaryDuration >= 3 || secondaryDuration <= 5) //Duration of window = 4 primary teeth
-      {
-        toothCurrentCount = 274; //End of fourth window is after 90+90+90+4 primary teeth
-        currentStatus.hasSync = true;
-      }
-    }
-    else if(configPage1.nCylinders == 6)
-    {
-      if(secondaryDuration >= 23 || secondaryDuration <= 25) //Duration of window = 16 primary teeth
-      {
-        toothCurrentCount = 24; //End of first window (The longest) occurs 24 teeth after TDC
-        currentStatus.hasSync = true;
-      }
-      else if(secondaryDuration >= 19 || secondaryDuration <= 21) //Duration of window = 12 primary teeth
-      {
-        toothCurrentCount = 84; //End of second window is after 60+20 primary teeth
-        currentStatus.hasSync = true;
-      }
-      else if(secondaryDuration >= 15 || secondaryDuration <= 17) //Duration of window = 16 primary teeth
-      {
-        toothCurrentCount = 136; //End of third window is after 60+60+16 primary teeth
-        currentStatus.hasSync = true;
-      }
-      else if(secondaryDuration >= 11 || secondaryDuration <= 13) //Duration of window = 12 primary teeth
-      {
-        toothCurrentCount = 192; //End of fourth window is after 60+60+60+12 primary teeth
-        currentStatus.hasSync = true;
-      }
-      else if(secondaryDuration >= 7 || secondaryDuration <= 9) //Duration of window = 8 primary teeth
-      {
-        toothCurrentCount = 248; //End of fifth window is after 60+60+60+60+8 primary teeth
-        currentStatus.hasSync = true;
-      }
-      else if(secondaryDuration >= 3 || secondaryDuration <= 5) //Duration of window = 4 primary teeth
-      {
-        toothCurrentCount = 304; //End of sixth window is after 60+60+60+60+60+4 primary teeth
-        currentStatus.hasSync = true;
-      }
-    }
-    else { currentStatus.hasSync = false; return ;} //This should really never happen
-  }
+  if( (secondaryToothCount == 0) || (READ_SEC_TRIGGER() == LOW) ) { secondaryToothCount = toothCurrentCount; } //This occurs on the first rotation upon powerup OR the start of a secondary window
   else
   {
-    //Already have sync, but do a verify every 720 degrees.
-    if(configPage1.nCylinders == 4)
-    {
-      if(secondaryDuration >= 15) //Duration of window = 16 primary teeth
-      {
-        toothCurrentCount = 16; //End of first window (The longest) occurs 16 teeth after TDC
-      }
-    }
-    else if(configPage1.nCylinders == 6)
-    {
-      if(secondaryDuration >= 23) //Duration of window = 24 primary teeth
-      {
-        toothCurrentCount = 24; //End of first window (The longest) occurs 24 teeth after TDC
-      }
-    }
-  }
+    //If we reach here, we are at the end of a secondary window
+    byte secondaryDuration = toothCurrentCount - secondaryToothCount; //How many primary teeth have passed during the duration of this secondary window
 
+    if(currentStatus.hasSync == false)
+    {
+      if(configPage1.nCylinders == 4)
+      {
+        if( (secondaryDuration >= 15) || (secondaryDuration <= 17) ) //Duration of window = 16 primary teeth
+        {
+          toothCurrentCount = 16; //End of first window (The longest) occurs 16 teeth after TDC
+          currentStatus.hasSync = true;
+        }
+        else if( (secondaryDuration >= 11) || (secondaryDuration <= 13) ) //Duration of window = 12 primary teeth
+        {
+          toothCurrentCount = 102; //End of second window is after 90+12 primary teeth
+          currentStatus.hasSync = true;
+        }
+        else if( (secondaryDuration >= 7) || (secondaryDuration <= 9) ) //Duration of window = 8 primary teeth
+        {
+          toothCurrentCount = 188; //End of third window is after 90+90+8 primary teeth
+          currentStatus.hasSync = true;
+        }
+        else if( (secondaryDuration >= 3) || (secondaryDuration <= 5) ) //Duration of window = 4 primary teeth
+        {
+          toothCurrentCount = 274; //End of fourth window is after 90+90+90+4 primary teeth
+          currentStatus.hasSync = true;
+        }
+      }
+      else if(configPage1.nCylinders == 6)
+      {
+        if( (secondaryDuration >= 23) || (secondaryDuration <= 25) ) //Duration of window = 16 primary teeth
+        {
+          toothCurrentCount = 24; //End of first window (The longest) occurs 24 teeth after TDC
+          currentStatus.hasSync = true;
+        }
+        else if( (secondaryDuration >= 19) || (secondaryDuration <= 21) ) //Duration of window = 12 primary teeth
+        {
+          toothCurrentCount = 84; //End of second window is after 60+20 primary teeth
+          currentStatus.hasSync = true;
+        }
+        else if( (secondaryDuration >= 15) || (secondaryDuration <= 17) ) //Duration of window = 16 primary teeth
+        {
+          toothCurrentCount = 136; //End of third window is after 60+60+16 primary teeth
+          currentStatus.hasSync = true;
+        }
+        else if( (secondaryDuration >= 11) || (secondaryDuration <= 13) ) //Duration of window = 12 primary teeth
+        {
+          toothCurrentCount = 192; //End of fourth window is after 60+60+60+12 primary teeth
+          currentStatus.hasSync = true;
+        }
+        else if( (secondaryDuration >= 7) || (secondaryDuration <= 9) ) //Duration of window = 8 primary teeth
+        {
+          toothCurrentCount = 248; //End of fifth window is after 60+60+60+60+8 primary teeth
+          currentStatus.hasSync = true;
+        }
+        else if( (secondaryDuration >= 3) || (secondaryDuration <= 5) ) //Duration of window = 4 primary teeth
+        {
+          toothCurrentCount = 304; //End of sixth window is after 60+60+60+60+60+4 primary teeth
+          currentStatus.hasSync = true;
+        }
+      }
+      else { currentStatus.hasSync = false; } //This should really never happen
+    }
+    else
+    {
+      //Already have sync, but do a verify every 720 degrees.
+      if(configPage1.nCylinders == 4)
+      {
+        if(secondaryDuration >= 15) //Duration of window = 16 primary teeth
+        {
+          toothCurrentCount = 16; //End of first window (The longest) occurs 16 teeth after TDC
+        }
+      }
+      else if(configPage1.nCylinders == 6)
+      {
+        if(secondaryDuration >= 23) //Duration of window = 24 primary teeth
+        {
+          toothCurrentCount = 24; //End of first window (The longest) occurs 24 teeth after TDC
+        }
+      } //Cylinder count
+    } //Has sync
+  } //First getting sync or not
 }
 
-int getRPM_Nissan360()
+uint16_t getRPM_Nissan360()
 {
-  //if(currentStatus.RPM < configPage2.crankRPM) { return crankingGetRPM(configPage2.triggerTeeth); }
   return stdGetRPM();
 }
 
 int getCrankAngle_Nissan360(int timePerDegree)
 {
   //As each tooth represents 2 crank degrees, we only need to determine whether we're more or less than halfway between teeth to know whether to add another 1 degrees
+  int crankAngle = 0;
   unsigned long halfTooth = (toothLastToothTime - toothLastMinusOneToothTime) >> 1;
   if ( (micros() - toothLastToothTime) > halfTooth)
   {
     //Means we're over halfway to the next tooth, so add on 1 degree
-    return (toothCurrentCount * triggerToothAngle) + 1;
+    crankAngle = (toothCurrentCount * triggerToothAngle) + 1;
   }
-  return (toothCurrentCount * triggerToothAngle);
+  else { crankAngle = (toothCurrentCount * triggerToothAngle); }
+
+  return crankAngle;
 }
 
 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
@@ -1623,14 +1701,14 @@ void triggerPri_Subaru67()
 {
    curTime = micros();
    //curGap = curTime - toothLastToothTime;
-   //if ( curGap < triggerFilterTime ) { return; } //Pulses should never be less than triggerFilterTime, so if they are it means a false trigger.
+   //if ( curGap < triggerFilterTime ) { return; }
    toothCurrentCount++; //Increment the tooth counter
    addToothLogEntry(curGap);
 
    toothLastMinusOneToothTime = toothLastToothTime;
    toothLastToothTime = curTime;
 
-   if ( !currentStatus.hasSync || configPage2.useResync)
+   if ( (currentStatus.hasSync == false) || configPage2.useResync)
    {
      //Sync is determined by counting the number of cam teeth that have passed between the crank teeth
      switch(secondaryToothCount)
@@ -1662,24 +1740,28 @@ void triggerPri_Subaru67()
 
      }
    }
-   if ( !currentStatus.hasSync ) { return; } //Check again if we have sync and return if not
 
-   //Locked timing during cranking. This is fixed at 10* BTDC.
-   if ( BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) && configPage2.ignCranklock)
+   //Check sync again
+   if ( currentStatus.hasSync == true )
    {
-     if( toothCurrentCount == 1 || toothCurrentCount == 7 ) { endCoil1Charge(); endCoil3Charge(); }
-     else if( toothCurrentCount == 4 || toothCurrentCount == 10 ) { endCoil2Charge(); endCoil4Charge(); }
+     //Locked timing during cranking. This is fixed at 10* BTDC.
+     if ( BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) && configPage2.ignCranklock)
+     {
+       if( (toothCurrentCount == 1) || (toothCurrentCount == 7) ) { endCoil1Charge(); endCoil3Charge(); }
+       else if( (toothCurrentCount == 4) || (toothCurrentCount == 10) ) { endCoil2Charge(); endCoil4Charge(); }
+     }
+
+     if ( toothCurrentCount > 12 ) //2 complete crank revolutions
+     {
+       toothCurrentCount = 1;
+       toothOneMinusOneTime = toothOneTime;
+       toothOneTime = curTime;
+       currentStatus.startRevolutions++; //Counter
+     }
    }
 
-   if ( toothCurrentCount > 12 ) //2 complete crank revolutions
-   {
-     toothCurrentCount = 1;
-     toothOneMinusOneTime = toothOneTime;
-     toothOneTime = curTime;
-     currentStatus.startRevolutions++; //Counter
-   }
-
-   //setFilter(curGap); //Recalc the new filter value
+   //Recalc the new filter value
+   //setFilter(curGap);
 }
 
 void triggerSec_Subaru67()
@@ -1688,11 +1770,12 @@ void triggerSec_Subaru67()
   curGap2 = curTime2 - toothLastSecToothTime;
   //if ( curGap2 < triggerSecFilterTime ) { return; }
   toothLastSecToothTime = curTime2;
-  //triggerSecFilterTime = curGap2 >> 2; //Set filter at 25% of the current speed
+  //OPTIONAL Set filter at 25% of the current speed
+  //triggerSecFilterTime = curGap2 >> 2;
   secondaryToothCount++;
 }
 
-int getRPM_Subaru67()
+uint16_t getRPM_Subaru67()
 {
   //if(currentStatus.RPM < configPage2.crankRPM) { return crankingGetRPM(configPage2.triggerTeeth); }
   return stdGetRPM();
@@ -1700,26 +1783,186 @@ int getRPM_Subaru67()
 
 int getCrankAngle_Subaru67(int timePerDegree)
 {
-  if(!currentStatus.hasSync) { return 0; }
-  //This is the current angle ATDC the engine is at. This is the last known position based on what tooth was last 'seen'. It is only accurate to the resolution of the trigger wheel (Eg 36-1 is 10 degrees)
-  unsigned long tempToothLastToothTime;
-  int tempToothCurrentCount;
-  //Grab some variables that are used in the trigger code and assign them to temp variables.
-  noInterrupts();
-  tempToothCurrentCount = toothCurrentCount;
-  tempToothLastToothTime = toothLastToothTime;
-  interrupts();
+  int crankAngle = 0;
+  if( currentStatus.hasSync == true )
+  {
+    //This is the current angle ATDC the engine is at. This is the last known position based on what tooth was last 'seen'. It is only accurate to the resolution of the trigger wheel (Eg 36-1 is 10 degrees)
+    unsigned long tempToothLastToothTime;
+    int tempToothCurrentCount;
+    //Grab some variables that are used in the trigger code and assign them to temp variables.
+    noInterrupts();
+    tempToothCurrentCount = toothCurrentCount;
+    tempToothLastToothTime = toothLastToothTime;
+    interrupts();
 
-  int crankAngle = toothAngles[(tempToothCurrentCount - 1)] + configPage2.triggerAngle; //Perform a lookup of the fixed toothAngles array to find what the angle of the last tooth passed was.
-  //Estimate the number of degrees travelled since the last tooth}
+    crankAngle = toothAngles[(tempToothCurrentCount - 1)] + configPage2.triggerAngle; //Perform a lookup of the fixed toothAngles array to find what the angle of the last tooth passed was.
+    //Estimate the number of degrees travelled since the last tooth}
 
-  unsigned long elapsedTime = micros() - tempToothLastToothTime;
-  if(elapsedTime < SHRT_MAX ) { crankAngle += div((int)elapsedTime, timePerDegree).quot; } //This option is much faster, but only available for smaller values of elapsedTime
-  else { crankAngle += ldiv(elapsedTime, timePerDegree).quot; }
+    unsigned long elapsedTime = micros() - tempToothLastToothTime;
+    if(elapsedTime < SHRT_MAX ) { crankAngle += div((int)elapsedTime, timePerDegree).quot; } //This option is much faster, but only available for smaller values of elapsedTime
+    else { crankAngle += ldiv(elapsedTime, timePerDegree).quot; }
 
-  if (crankAngle >= 720) { crankAngle -= 720; }
-  if (crankAngle > CRANK_ANGLE_MAX) { crankAngle -= CRANK_ANGLE_MAX; }
-  if (crankAngle < 0) { crankAngle += 360; }
+    if (crankAngle >= 720) { crankAngle -= 720; }
+    if (crankAngle > CRANK_ANGLE_MAX) { crankAngle -= CRANK_ANGLE_MAX; }
+    if (crankAngle < 0) { crankAngle += 360; }
+  }
 
   return crankAngle;
 }
+
+/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
+Name: Daihatsu +1 trigger for 3 and 4 cylinder engines
+Desc: Tooth equal to the number of cylinders are evenly spaced on the cam. No position sensing (Distributor is retained) so crank angle is a made up figure based purely on the first teeth to be seen
+Note: This is a very simple decoder. See http://www.megamanual.com/ms2/GM_7pinHEI.htm
+*/
+void triggerSetup_Daihatsu()
+{
+  triggerActualTeeth = configPage1.nCylinders + 1;
+  triggerToothAngle = 720 / triggerActualTeeth; //The number of degrees that passes from tooth to tooth
+  triggerFilterTime = 60000000L / MAX_RPM / configPage1.nCylinders; // Minimum time required between teeth
+  triggerFilterTime = triggerFilterTime / 2; //Safety margin
+  secondDerivEnabled = false;
+  decoderIsSequential = false;
+
+  MAX_STALL_TIME = (1851UL * triggerToothAngle)*4;//Minimum 90rpm. (1851uS is the time per degree at 90rpm). This uses 90rpm rather than 50rpm due to the potentially very high stall time on a 4 cylinder if we wait that long.
+
+  if(configPage1.nCylinders == 3)
+  {
+    toothAngles[0] = 0; //tooth #1
+    toothAngles[1] = 30; //tooth #2 (Extra tooth)
+    toothAngles[2] = 240; //tooth #3
+    toothAngles[3] = 480; //tooth #4
+  }
+  else
+  {
+    //Should be 4 cylinders here
+    toothAngles[0] = 0; //tooth #1
+    toothAngles[1] = 30; //tooth #2 (Extra tooth)
+    toothAngles[2] = 180; //tooth #3
+    toothAngles[3] = 360; //tooth #4
+    toothAngles[4] = 540; //tooth #5
+  }
+}
+
+void triggerPri_Daihatsu()
+{
+  curTime = micros();
+  curGap = curTime - toothLastToothTime;
+
+  //if ( curGap >= triggerFilterTime )
+  {
+    toothSystemCount++;
+
+    if (currentStatus.hasSync == true)
+    {
+      if( (toothCurrentCount == triggerActualTeeth) ) //Check if we're back to the beginning of a revolution
+      {
+         toothCurrentCount = 1; //Reset the counter
+         toothOneMinusOneTime = toothOneTime;
+         toothOneTime = curTime;
+         currentStatus.hasSync = true;
+         currentStatus.startRevolutions++; //Counter
+
+         //Need to set a special filter time for the next tooth
+         triggerFilterTime = 20; //Fix this later
+      }
+      else
+      {
+        toothCurrentCount++; //Increment the tooth counter
+        setFilter(curGap); //Recalc the new filter value
+      }
+
+      //addToothLogEntry(curGap);
+
+      if ( configPage2.ignCranklock && BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) )
+      {
+        //This locks the cranking timing to 0 degrees BTDC (All the triggers allow for)
+        if(toothCurrentCount == 1) { endCoil1Charge(); }
+        else if(toothCurrentCount == 2) { endCoil2Charge(); }
+        else if(toothCurrentCount == 3) { endCoil3Charge(); }
+        else if(toothCurrentCount == 4) { endCoil4Charge(); }
+      }
+    }
+    else //NO SYNC
+    {
+      //
+      if(toothSystemCount >= 3) //Need to have seen at least 3 teeth to determine SYNC
+      {
+        unsigned long targetTime;
+        //We need to try and find the extra tooth (#2) which is located 30 degrees after tooth #1
+        //Aim for tooth times less than about 60 degrees
+        if(configPage1.nCylinders == 3)
+        {
+          targetTime = (toothLastToothTime -  toothLastMinusOneToothTime) >> 2; //Teeth are 240 degrees apart for 3 cylinder. 240/3 = 60
+        }
+        else
+        {
+          targetTime = ((toothLastToothTime -  toothLastMinusOneToothTime) * 3) >> 4; //Teeth are 180 degrees apart for 4 cylinder. (180*3)/8 = 67
+        }
+        if(curGap < targetTime)
+        {
+          //Means we're on the extra tooth here
+          toothCurrentCount = 2; //Reset the counter
+          currentStatus.hasSync = true;
+          triggerFilterTime = targetTime; //Lazy, but it works
+        }
+      }
+    }
+
+    toothLastMinusOneToothTime = toothLastToothTime;
+    toothLastToothTime = curTime;
+  } //Trigger filter
+}
+void triggerSec_Daihatsu() { return; } //Not required (Should never be called in the first place)
+
+uint16_t getRPM_Daihatsu()
+{
+  uint16_t tempRPM = 0;
+  if( currentStatus.RPM < (unsigned int)(configPage2.crankRPM * 100) && false) //Disable special cranking processing for now
+  {
+    //Cn't use standard cranking RPM functin due to extra tooth
+    if( currentStatus.hasSync == true )
+    {
+      if(toothCurrentCount == 2) { tempRPM = currentStatus.RPM; }
+      else if (toothCurrentCount == 3) { tempRPM = currentStatus.RPM; }
+      else
+      {
+        noInterrupts();
+        revolutionTime = (toothLastToothTime - toothLastMinusOneToothTime) * (triggerActualTeeth-1);
+        interrupts();
+        tempRPM = (US_IN_MINUTE / revolutionTime);
+        if(tempRPM >= MAX_RPM) { tempRPM = currentStatus.RPM; } //Sanity check
+      } //is tooth #2
+    }
+    else { tempRPM = 0; } //No sync
+  }
+  else
+  { tempRPM = stdGetRPM() << 1; } //Multiply RPM by 2 due to tracking over 720 degrees now rather than 360
+
+  return tempRPM;
+
+}
+int getCrankAngle_Daihatsu(int timePerDegree)
+{
+    //This is the current angle ATDC the engine is at. This is the last known position based on what tooth was last 'seen'. It is only accurate to the resolution of the trigger wheel (Eg 36-1 is 10 degrees)
+    unsigned long tempToothLastToothTime;
+    int tempToothCurrentCount;
+    int crankAngle;
+    //Grab some variables that are used in the trigger code and assign them to temp variables.
+    noInterrupts();
+    tempToothCurrentCount = toothCurrentCount;
+    tempToothLastToothTime = toothLastToothTime;
+    interrupts();
+
+    crankAngle = toothAngles[tempToothCurrentCount-1] + configPage2.triggerAngle; //Crank angle of the last tooth seen
+    //Estimate the number of degrees travelled since the last tooth}
+    long elapsedTime = micros() - tempToothLastToothTime;
+    if(elapsedTime < SHRT_MAX ) { crankAngle += div((int)elapsedTime, timePerDegree).quot; } //This option is much faster, but only available for smaller values of elapsedTime
+    else { crankAngle += ldiv(elapsedTime, timePerDegree).quot; }
+
+    if (crankAngle >= 720) { crankAngle -= 720; }
+    if (crankAngle > CRANK_ANGLE_MAX) { crankAngle -= CRANK_ANGLE_MAX; }
+    if (crankAngle < 0) { crankAngle += CRANK_ANGLE_MAX; }
+
+    return crankAngle;
+}
diff --git a/speeduino/errors.ino b/speeduino/errors.ino
index a2bb50ca..903e1de7 100644
--- a/speeduino/errors.ino
+++ b/speeduino/errors.ino
@@ -17,32 +17,32 @@ byte setError(byte errorID)
     errorCodes[errorCount] = errorID;
     errorCount++;
     if(errorCount == 1) { BIT_SET(currentStatus.spark, BIT_SPARK_ERROR); } //Enable the error indicator
-    return errorCount;
   }
-  return 0;
+  return errorCount;
 }
 
 void clearError(byte errorID)
 {
-  byte clearedError;
+  byte clearedError = 255;
 
   if (errorID == errorCodes[0]) { clearedError = 0; }
   else if(errorID == errorCodes[1]) { clearedError = 1; }
   else if(errorID == errorCodes[2]) { clearedError = 2; }
   else if(errorID == errorCodes[3]) { clearedError = 3; }
-  else return; //Occurs when the error we're being asked to clear is not currently one of the active errors
 
-  errorCodes[clearedError] = ERR_NONE;
-  //Clear the required error and move any from above it 'down' in the error array
-  for (byte x=clearedError; x < (errorCount-1); x++)
+  if(clearedError < MAX_ERRORS)
   {
-    errorCodes[x] = errorCodes[x+1];
-    errorCodes[x+1] = ERR_NONE;
-  }
+    errorCodes[clearedError] = ERR_NONE;
+    //Clear the required error and move any from above it 'down' in the error array
+    for (byte x=clearedError; x < (errorCount-1); x++)
+    {
+      errorCodes[x] = errorCodes[x+1];
+      errorCodes[x+1] = ERR_NONE;
+    }
 
-  errorCount--;
-  if(errorCount == 0) { BIT_CLEAR(currentStatus.spark, BIT_SPARK_ERROR); } //Enable the error indicator
-  
+    errorCount--;
+    if(errorCount == 0) { BIT_CLEAR(currentStatus.spark, BIT_SPARK_ERROR); } //Enable the error indicator
+  }
 }
 
 byte getNextError()
diff --git a/speeduino/globals.h b/speeduino/globals.h
index 002b5443..257ed8d8 100644
--- a/speeduino/globals.h
+++ b/speeduino/globals.h
@@ -9,18 +9,20 @@
   #define CORE_AVR
 #elif defined(CORE_TEENSY)
   #define BOARD_NR_GPIO_PINS 34
-#elif defined(STM32_MCU_SERIES) || defined(_VARIANT_ARDUINO_STM32_)
+#elif defined(STM32_MCU_SERIES) || defined(ARDUINO_ARCH_STM32) || defined(__STM32F1__) || defined(STM32F4) || defined(STM32)
   #define CORE_STM32
-  #if defined (ARDUINO_ARCH_STM32F1)
+  #if defined (STM32F1) || defined(__STM32F1__)
+    #define BOARD_NR_GPIO_PINS 34
     #define LED_BUILTIN 33
-  #elif defined (ARDUINO_ARCH_STM32F4)
-    #define LED_BUILTIN PA6
+  #elif defined(ARDUINO_BLACK_F407VE)
+    #define BOARD_NR_GPIO_PINS 78
+    #define LED_BUILTIN PA7
   #endif
   
   extern "C" char* sbrk(int incr); //Used to freeRam
   inline unsigned char  digitalPinToInterrupt(unsigned char Interrupt_pin) { return Interrupt_pin; } //This isn't included in the stm32duino libs (yet)
-  #define portOutputRegister(port) (volatile byte *)( &(port->regs->ODR) ) //These are defined in STM32F1/variants/generic_stm32f103c/variant.h but return a non byte* value
-  #define portInputRegister(port) (volatile byte *)( &(port->regs->IDR) ) //These are defined in STM32F1/variants/generic_stm32f103c/variant.h but return a non byte* value
+  #define portOutputRegister(port) (volatile byte *)( &(port->ODR) )
+  #define portInputRegister(port) (volatile byte *)( &(port->IDR) )
 #else
   #error Incorrect board selected. Please select the correct board (Usually Mega 2560) and upload again
 #endif
@@ -28,7 +30,7 @@
 //Handy bitsetting macros
 #define BIT_SET(a,b) ((a) |= (1<<(b)))
 #define BIT_CLEAR(a,b) ((a) &= ~(1<<(b)))
-#define BIT_CHECK(var,pos) ((var) & (1<<(pos)))
+#define BIT_CHECK(var,pos) !!((var) & (1<<(pos)))
 
 #define MS_IN_MINUTE 60000
 #define US_IN_MINUTE 60000000
@@ -107,8 +109,8 @@
 const byte signature = 20;
 
 //const char signature[] = "speeduino";
-const char displaySignature[] = "speeduino 201609-dev";
-const char TSfirmwareVersion[] = "Speeduino 2016.09";
+const char displaySignature[] = "Speeduino 2017";
+const char TSfirmwareVersion[] = "07-dev";
 
 const byte data_structure_version = 2; //This identifies the data structure when reading / writing.
 const byte page_size = 64;
@@ -163,11 +165,21 @@ 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 {
-  volatile boolean hasSync;
-  unsigned int RPM;
+  volatile bool hasSync;
+  uint16_t RPM;
   long longRPM;
   int mapADC;
   long MAP; //Has to be a long for PID calcs (Boost control)
@@ -220,7 +232,7 @@ struct statuses {
   volatile unsigned int loopsPerSecond;
   boolean launchingSoft; //True when in launch control soft limit mode
   boolean launchingHard; //True when in launch control hard limit mode
-  int freeRAM;
+  unsigned int freeRAM;
   unsigned int clutchEngagedRPM;
   bool flatShiftingHard;
   volatile byte startRevolutions; //A counter for how many revolutions have been completed since sync was achieved.
@@ -245,7 +257,7 @@ struct statuses currentStatus; //The global status object
 //This mostly covers off variables that are required for fuel
 struct config1 {
 
-  int8_t flexBoostLow;
+  int8_t flexBoostLow; //Must be signed to allow for negatives
   byte flexBoostHigh;
   byte asePct;  //Afterstart enrichment (%)
   byte aseCount; //Afterstart enrichment cycles. This is the number of ignition cycles that the afterstart enrichment % lasts for
diff --git a/speeduino/idle.h b/speeduino/idle.h
index 234eea06..929a7755 100644
--- a/speeduino/idle.h
+++ b/speeduino/idle.h
@@ -25,7 +25,7 @@ struct StepperIdle
   volatile unsigned long stepStartTime; //The time the curren
 };
 
-#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega2561__)
+#if defined(CORE_AVR)
   #define IDLE_COUNTER TCNT4
   #define IDLE_COMPARE OCR4C
 
@@ -46,7 +46,7 @@ struct StepperIdle
   #define IDLE_COMPARE 0
 
   #define IDLE_TIMER_ENABLE()
-  #define IDLE_TIMER_DISABLE() 
+  #define IDLE_TIMER_DISABLE()
 
 #endif
 
diff --git a/speeduino/idle.ino b/speeduino/idle.ino
index 28bb9563..af41d507 100644
--- a/speeduino/idle.ino
+++ b/speeduino/idle.ino
@@ -19,12 +19,12 @@ void initialiseIdle()
 {
   //By default, turn off the PWM interrupt (It gets turned on below if needed)
   IDLE_TIMER_DISABLE();
-  #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega2561__) //AVR chips use the ISR for this
+  #if defined(CORE_AVR) //AVR chips use the ISR for this
     //No timer work required for AVRs. Timer is shared with the schedules and setup in there.
 
   #elif defined (CORE_TEENSY)
 
-    if(configPage4.iacAlgorithm == IAC_ALGORITHM_PWM_OL || configPage4.iacAlgorithm == IAC_ALGORITHM_PWM_CL)
+    if( (configPage4.iacAlgorithm == IAC_ALGORITHM_PWM_OL) || (configPage4.iacAlgorithm == IAC_ALGORITHM_PWM_CL) )
     {
     //FlexTimer 2 is used for idle
     FTM2_MODE |= FTM_MODE_WPDIS; // Write Protection Disable
@@ -154,7 +154,6 @@ void initialiseIdle()
       iacCrankStepsTable.axisX = configPage4.iacCrankBins;
       iacStepTime = configPage4.iacStepTime * 1000;
 
-      //homeStepper(); //Returns the stepper to the 'home' position
       completedHomeSteps = 0;
       idleStepper.curIdleStep = 0;
       idleStepper.stepperStatus = SOFF;
@@ -181,6 +180,10 @@ void initialiseIdle()
       idlePID.SetTunings(configPage3.idleKP, configPage3.idleKI, configPage3.idleKD);
       idlePID.SetMode(AUTOMATIC); //Turn PID on
       break;
+
+    default:
+      //Well this just shouldn't happen
+      break;
   }
   idleInitComplete = configPage4.iacAlgorithm; //Sets which idle method was initialised
   currentStatus.idleLoad = 0;
@@ -242,48 +245,53 @@ void idleControl()
 
     case IAC_ALGORITHM_STEP_OL:    //Case 4 is open loop stepper control
       //First thing to check is whether there is currently a step going on and if so, whether it needs to be turned off
-      if( checkForStepping() ) { return; } //If this is true it means there's either a step taking place or
-      if( !isStepperHomed() ) { return; } //Check whether homing is completed yet.
-
-      //Check for cranking pulsewidth
-      if( BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) )
+      if( isStepperHomed() && !checkForStepping() ) //Check that homing is complete and that there's not currently a step already taking place
       {
-        //Currently cranking. Use the cranking table
-        idleStepper.targetIdleStep = table2D_getValue(&iacCrankStepsTable, (currentStatus.coolant + CALIBRATION_TEMPERATURE_OFFSET)) * 3; //All temps are offset by 40 degrees. Step counts are divided by 3 in TS. Multiply back out here
-        doStep();
-      }
-      else if( (currentStatus.coolant + CALIBRATION_TEMPERATURE_OFFSET) < iacStepTable.axisX[IDLE_TABLE_SIZE-1])
-      {
-        //Standard running
-        if ((mainLoopCount & 255) == 1)
+        //Check for cranking pulsewidth
+        if( BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) )
         {
-          //Only do a lookup of the required value around 4 times per second. Any more than this can create too much jitter and require a hyster value that is too high
-          idleStepper.targetIdleStep = table2D_getValue(&iacStepTable, (currentStatus.coolant + CALIBRATION_TEMPERATURE_OFFSET)) * 3; //All temps are offset by 40 degrees. Step counts are divided by 3 in TS. Multiply back out here
-          iacStepTime = configPage4.iacStepTime * 1000;
+          //Currently cranking. Use the cranking table
+          idleStepper.targetIdleStep = table2D_getValue(&iacCrankStepsTable, (currentStatus.coolant + CALIBRATION_TEMPERATURE_OFFSET)) * 3; //All temps are offset by 40 degrees. Step counts are divided by 3 in TS. Multiply back out here
+          doStep();
         }
-        doStep();
+        else if( (currentStatus.coolant + CALIBRATION_TEMPERATURE_OFFSET) < iacStepTable.axisX[IDLE_TABLE_SIZE-1])
+        {
+          //Standard running
+          if ((mainLoopCount & 255) == 1)
+          {
+            //Only do a lookup of the required value around 4 times per second. Any more than this can create too much jitter and require a hyster value that is too high
+            idleStepper.targetIdleStep = table2D_getValue(&iacStepTable, (currentStatus.coolant + CALIBRATION_TEMPERATURE_OFFSET)) * 3; //All temps are offset by 40 degrees. Step counts are divided by 3 in TS. Multiply back out here
+            iacStepTime = configPage4.iacStepTime * 1000;
+          }
+          doStep();
+        }
+        currentStatus.idleLoad = idleStepper.curIdleStep >> 1; //Current step count (Divided by 2 for byte)
       }
-      currentStatus.idleLoad = idleStepper.curIdleStep >> 1; //Current step count (Divided by 2 for byte)
       break;
 
     case IAC_ALGORITHM_STEP_CL://Case 5 is closed loop stepper control
       //First thing to check is whether there is currently a step going on and if so, whether it needs to be turned off
-      if( checkForStepping() ) { return; } //If this is true it means there's either a step taking place or
-      if( !isStepperHomed() ) { return; } //Check whether homing is completed yet.
-      if( (idleCounter & 31) == 1)
+      if( isStepperHomed() && !checkForStepping() ) //Check that homing is complete and that there's not currently a step already taking place
       {
-        //This only needs to be run very infrequently, once every 32 calls to idleControl(). This is approx. once per second
-        idlePID.SetTunings(configPage3.idleKP, configPage3.idleKI, configPage3.idleKD);
-        iacStepTime = configPage4.iacStepTime * 1000;
+        if( (idleCounter & 31) == 1)
+        {
+          //This only needs to be run very infrequently, once every 32 calls to idleControl(). This is approx. once per second
+          idlePID.SetTunings(configPage3.idleKP, configPage3.idleKI, configPage3.idleKD);
+          iacStepTime = configPage4.iacStepTime * 1000;
+        }
+
+        idle_cl_target_rpm = table2D_getValue(&iacClosedLoopTable, currentStatus.coolant + CALIBRATION_TEMPERATURE_OFFSET) * 10; //All temps are offset by 40 degrees
+        idlePID.Compute();
+        idleStepper.targetIdleStep = idle_pid_target_value;
+
+        doStep();
+        currentStatus.idleLoad = idleStepper.curIdleStep >> 1; //Current step count (Divided by 2 for byte)
+        idleCounter++;
       }
+      break;
 
-      idle_cl_target_rpm = table2D_getValue(&iacClosedLoopTable, currentStatus.coolant + CALIBRATION_TEMPERATURE_OFFSET) * 10; //All temps are offset by 40 degrees
-      idlePID.Compute();
-      idleStepper.targetIdleStep = idle_pid_target_value;
-
-      doStep();
-      currentStatus.idleLoad = idleStepper.curIdleStep >> 1; //Current step count (Divided by 2 for byte)
-      idleCounter++;
+    default:
+      //There really should be a valid idle type
       break;
   }
 }
@@ -296,6 +304,7 @@ False: If the motor has not yet been homed. Will also perform another homing ste
 */
 static inline byte isStepperHomed()
 {
+  bool isHomed = true; //As it's the most common scenario, default value is true
   if( completedHomeSteps < (configPage4.iacStepHome * 3) ) //Home steps are divided by 3 from TS
   {
     digitalWrite(pinStepperDir, STEPPER_BACKWARD); //Sets stepper direction to backwards
@@ -304,9 +313,9 @@ static inline byte isStepperHomed()
     idleStepper.stepperStatus = STEPPING;
     completedHomeSteps++;
     idleOn = true;
-    return false;
+    isHomed = false;
   }
-  return true;
+  return isHomed;
 }
 
 /*
@@ -317,7 +326,8 @@ False: If the motor is ready for another step
 */
 static inline byte checkForStepping()
 {
-  if(idleStepper.stepperStatus == STEPPING || idleStepper.stepperStatus == COOLING)
+  bool isStepping = false;
+  if( (idleStepper.stepperStatus == STEPPING) || (idleStepper.stepperStatus == COOLING) )
   {
     if(micros() > (idleStepper.stepStartTime + iacStepTime) )
     {
@@ -327,7 +337,7 @@ static inline byte checkForStepping()
         digitalWrite(pinStepperStep, LOW); //Turn off the step
         idleStepper.stepStartTime = micros();
         idleStepper.stepperStatus = COOLING; //'Cooling' is the time the stepper needs to sit in LOW state before the next step can be made
-        return true;
+        isStepping = true;
       }
       else
       {
@@ -339,10 +349,10 @@ static inline byte checkForStepping()
     else
     {
       //Means we're in a step, but it doesn't need to turn off yet. No further action at this time
-      return true;
+      isStepping = true;
     }
   }
-  return false;
+  return isStepping;
 }
 
 /*
@@ -350,36 +360,31 @@ Performs a step
 */
 static inline void doStep()
 {
-  if ( idleStepper.targetIdleStep > (idleStepper.curIdleStep - configPage4.iacStepHyster) && idleStepper.targetIdleStep < (idleStepper.curIdleStep + configPage4.iacStepHyster) ) { return; } //Hysteris check
-  else if(idleStepper.targetIdleStep < idleStepper.curIdleStep) { digitalWrite(pinStepperDir, STEPPER_BACKWARD); idleStepper.curIdleStep--; }//Sets stepper direction to backwards
-  else if (idleStepper.targetIdleStep > idleStepper.curIdleStep) { digitalWrite(pinStepperDir, STEPPER_FORWARD); idleStepper.curIdleStep++; }//Sets stepper direction to forwards
+  if ( (idleStepper.targetIdleStep <= (idleStepper.curIdleStep - configPage4.iacStepHyster)) || (idleStepper.targetIdleStep >= (idleStepper.curIdleStep + configPage4.iacStepHyster)) ) //Hysteris check
+  {
+    if(idleStepper.targetIdleStep < idleStepper.curIdleStep) { digitalWrite(pinStepperDir, STEPPER_BACKWARD); idleStepper.curIdleStep--; }//Sets stepper direction to backwards
+    else if (idleStepper.targetIdleStep > idleStepper.curIdleStep) { digitalWrite(pinStepperDir, STEPPER_FORWARD); idleStepper.curIdleStep++; }//Sets stepper direction to forwards
 
-  digitalWrite(pinStepperEnable, LOW); //Enable the DRV8825
-  digitalWrite(pinStepperStep, HIGH);
-  idleStepper.stepStartTime = micros();
-  idleStepper.stepperStatus = STEPPING;
-  idleOn = true;
+    digitalWrite(pinStepperEnable, LOW); //Enable the DRV8825
+    digitalWrite(pinStepperStep, HIGH);
+    idleStepper.stepStartTime = micros();
+    idleStepper.stepperStatus = STEPPING;
+    idleOn = true;
+  }
 }
 
 //This function simply turns off the idle PWM and sets the pin low
 static inline void disableIdle()
 {
-  if(configPage4.iacAlgorithm == IAC_ALGORITHM_PWM_CL || configPage4.iacAlgorithm == IAC_ALGORITHM_PWM_OL)
+  if( (configPage4.iacAlgorithm == IAC_ALGORITHM_PWM_CL) || (configPage4.iacAlgorithm == IAC_ALGORITHM_PWM_OL) )
   {
     IDLE_TIMER_DISABLE();
     digitalWrite(pinIdle1, LOW);
   }
-  else if (configPage4.iacAlgorithm == IAC_ALGORITHM_STEP_CL || configPage4.iacAlgorithm == IAC_ALGORITHM_STEP_OL)
+  else if ( (configPage4.iacAlgorithm == IAC_ALGORITHM_STEP_CL) || (configPage4.iacAlgorithm == IAC_ALGORITHM_STEP_OL) )
   {
     digitalWrite(pinStepperEnable, HIGH); //Disable the DRV8825
     idleStepper.targetIdleStep = idleStepper.curIdleStep; //Don't try to move anymore
-    //The below appears to be causing issues, so for now this will simply halt the stepper entirely rather than taking it back to step 1
-    /*
-    idleStepper.targetIdleStep = 1; //Home the stepper
-    if( checkForStepping() ) { return; } //If this is true it means there's either a step taking place or
-    if( !isStepperHomed() ) { return; } //Check whether homing is completed yet.
-    doStep();
-    */
   }
 }
 
@@ -387,22 +392,22 @@ static inline void disableIdle()
 //Typically this is enabling the PWM interrupt
 static inline void enableIdle()
 {
-  if(configPage4.iacAlgorithm == IAC_ALGORITHM_PWM_CL || configPage4.iacAlgorithm == IAC_ALGORITHM_PWM_OL)
+  if( (configPage4.iacAlgorithm == IAC_ALGORITHM_PWM_CL) || (configPage4.iacAlgorithm == IAC_ALGORITHM_PWM_OL) )
   {
     IDLE_TIMER_ENABLE();
   }
-  else if (configPage4.iacAlgorithm == IAC_ALGORITHM_STEP_CL || configPage4.iacAlgorithm == IAC_ALGORITHM_STEP_OL)
+  else if ( (configPage4.iacAlgorithm == IAC_ALGORITHM_STEP_CL) || (configPage4.iacAlgorithm == IAC_ALGORITHM_STEP_OL) )
   {
 
   }
 }
 
-#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega2561__) //AVR chips use the ISR for this
+#if defined(CORE_AVR) //AVR chips use the ISR for this
 ISR(TIMER4_COMPC_vect)
 #elif defined (CORE_TEENSY) || defined (CORE_STM32)
 static inline void idleInterrupt() //Most ARM chips can simply call a function
 #endif
-#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega2561__) || defined (CORE_TEENSY)
+#if defined(CORE_AVR) || defined (CORE_TEENSY)
 {
   if (idle_pwm_state)
   {
@@ -410,13 +415,13 @@ static inline void idleInterrupt() //Most ARM chips can simply call a function
     {
       //Normal direction
       *idle_pin_port &= ~(idle_pin_mask);  // Switch pin to low (1 pin mode)
-      if(configPage4.iacChannels) { *idle2_pin_port |= (idle2_pin_mask); } //If 2 idle channels are in use, flip idle2 to be the opposite of idle1
+      if(configPage4.iacChannels == 1) { *idle2_pin_port |= (idle2_pin_mask); } //If 2 idle channels are in use, flip idle2 to be the opposite of idle1
     }
     else
     {
       //Reversed direction
       *idle_pin_port |= (idle_pin_mask);  // Switch pin high
-      if(configPage4.iacChannels) { *idle2_pin_port &= ~(idle2_pin_mask); } //If 2 idle channels are in use, flip idle2 to be the opposite of idle1
+      if(configPage4.iacChannels == 1) { *idle2_pin_port &= ~(idle2_pin_mask); } //If 2 idle channels are in use, flip idle2 to be the opposite of idle1
     }
     IDLE_COMPARE = IDLE_COUNTER + (idle_pwm_max_count - idle_pwm_cur_value);
     idle_pwm_state = false;
@@ -427,13 +432,13 @@ static inline void idleInterrupt() //Most ARM chips can simply call a function
     {
       //Normal direction
       *idle_pin_port |= (idle_pin_mask);  // Switch pin high
-      if(configPage4.iacChannels) { *idle2_pin_port &= ~(idle2_pin_mask); } //If 2 idle channels are in use, flip idle2 to be the opposite of idle1
+      if(configPage4.iacChannels == 1) { *idle2_pin_port &= ~(idle2_pin_mask); } //If 2 idle channels are in use, flip idle2 to be the opposite of idle1
     }
     else
     {
       //Reversed direction
       *idle_pin_port &= ~(idle_pin_mask);  // Switch pin to low (1 pin mode)
-      if(configPage4.iacChannels) { *idle2_pin_port |= (idle2_pin_mask); } //If 2 idle channels are in use, flip idle2 to be the opposite of idle1
+      if(configPage4.iacChannels == 1) { *idle2_pin_port |= (idle2_pin_mask); } //If 2 idle channels are in use, flip idle2 to be the opposite of idle1
     }
     IDLE_COMPARE = IDLE_COUNTER + idle_pwm_target_value;
     idle_pwm_cur_value = idle_pwm_target_value;
diff --git a/speeduino/maths.h b/speeduino/maths.h
index c3fad7bd..dee4e611 100644
--- a/speeduino/maths.h
+++ b/speeduino/maths.h
@@ -1,7 +1,7 @@
 #ifndef MATH_H
 #define MATH_H
 
-int fastResize(unsigned int, int);
+int fastResize(int, int);
 unsigned long percentage(byte, unsigned long);
 
 #endif
diff --git a/speeduino/maths.ino b/speeduino/maths.ino
index 08d00954..be1716b1 100644
--- a/speeduino/maths.ino
+++ b/speeduino/maths.ino
@@ -5,70 +5,75 @@
 //Replace the standard arduino map() function to use the div function instead
 int fastMap(unsigned long x, int in_min, int in_max, int out_min, int out_max)
 {
-  return ldiv( ((x - in_min) * (out_max - out_min)) , (in_max - in_min) ).quot + out_min;
+  unsigned long a = (x - (unsigned long)in_min);
+  int b = (out_max - out_min);
+  int c = (in_max - in_min);
+  int d = (ldiv( (a * (long)b) , (long)c ).quot);
+  return d + out_min;
+  //return ldiv( ((x - in_min) * (out_max - out_min)) , (in_max - in_min) ).quot + out_min;
   //return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
 }
 
-//This is a dedicated function that specifically handles the case of mapping 0-1023 values into a 0 to X range 
+//This is a dedicated function that specifically handles the case of mapping 0-1023 values into a 0 to X range
 //This is a common case because it means converting from a standard 10-bit analog input to a byte or 10-bit analog into 0-511 (Eg the temperature readings)
 //int fastMap1023toX(unsigned long x, int in_min, int in_max, int out_min, int out_max)
 //removed ununsed variables, in_min and out_min is aways 0, in_max is aways 1023
-int fastResize(unsigned int x, int out_max)
-{
-  #if defined(CORE_STM32)
-    return (x * out_max) >> 12;
-  #else
-    return (x * out_max) >> 10;
-  #endif
-}
-
+#if defined(CORE_STM32)
+  #define fastResize(x, out_max) ( ((unsigned long)x * out_max) >> 12)
+#else  
+  #define fastResize(x, out_max) ( ((unsigned long)x * out_max) >> 10)
+#endif
 /*
 The following are all fast versions of specific divisions
 Ref: http://www.hackersdelight.org/divcMore.pdf
 */
 
 //Unsigned divide by 10
-unsigned int divu10(unsigned int n) {
- unsigned long q, r;
- q = (n >> 1) + (n >> 2);
- q = q + (q >> 4);
- q = q + (q >> 8);
- q = q + (q >> 16);
- q = q >> 3;
- r = n - q*10;
- return q + ((r + 6) >> 4);
-// return q + (r > 9);
+unsigned int divu10(unsigned int n)
+{
+  unsigned long q, r;
+  q = (n >> 1) + (n >> 2);
+  q = q + (q >> 4);
+  q = q + (q >> 8);
+  q = q + (q >> 16);
+  q = q >> 3;
+  r = n - (q * 10);
+  return q + ((r + 6) >> 4);
 }
 
 //Signed divide by 10
-int divs10(long n) {
- long q, r;
- n = n + (n>>31 & 9);
- q = (n >> 1) + (n >> 2);
- q = q + (q >> 4);
- q = q + (q >> 8);
- q = q + (q >> 16);
- q = q >> 3;
- r = n - q*10;
- return q + ((r + 6) >> 4);
-// return q + (r > 9);
+int divs10(long n)
+{
+  long q, r, p;
+  p = n + ( (n>>31) & 9);
+  q = (p >> 1) + (p >> 2);
+  q = q + (q >> 4);
+  q = q + (q >> 8);
+  q = q + (q >> 16);
+  q = q >> 3;
+  r = p - (q * 10);
+  return q + ((r + 6) >> 4);
 }
 
 //Signed divide by 100
-int divs100(long n) {
+int divs100(long n)
+{
   return (n / 100); // Amazingly, gcc is producing a better /divide by 100 function than this
- long q, r;
- n = n + (n>>31 & 99);
- q = (n >> 1) + (n >> 3) + (n >> 6) - (n >> 10) +
- (n >> 12) + (n >> 13) - (n >> 16);
- q = q + (q >> 20);
- q = q >> 6;
- r = n - q*100;
- return q + ((r + 28) >> 7);
-// return q + (r > 99);
+  /*
+  long q, r;
+  n = n + (n>>31 & 99);
+  q = (n >> 1) + (n >> 3) + (n >> 6) - (n >> 10) +
+  (n >> 12) + (n >> 13) - (n >> 16);
+  q = q + (q >> 20);
+  q = q >> 6;
+  r = n - q*100;
+  return q + ((r + 28) >> 7);
+  */
 }
 
-unsigned long divu100(unsigned long n) {
+//Unsigned divide by 100
+unsigned long divu100(unsigned long n)
+{
   #if defined(CORE_STM32)
     return (n / 100); // No difference with this on/off
   #else
@@ -80,11 +85,10 @@ unsigned long divu100(unsigned long n) {
     r = n - q*100;
     return q + ((r + 28) >> 7);
   #endif  
-// return q + (r > 99);
 }
 
 //Return x percent of y
-//This is a relatively fast approximation of a percentage value. 
+//This is a relatively fast approximation of a percentage value.
 unsigned long percentage(byte x, unsigned long y)
 {
   return (y * x) / 100; //For some reason this is faster
@@ -97,7 +101,7 @@ unsigned long percentage(byte x, unsigned long y)
 inline long powint(int factor, unsigned int exponent)
 {
    long product = 1;
-   while (exponent--)
-      product *= factor;
+   unsigned int counter = exponent;
+   while ( (counter--) > 0) { product *= factor; }
    return product;
 }
diff --git a/speeduino/scheduledIO.h b/speeduino/scheduledIO.h
index 28c66b8e..a6ba3049 100644
--- a/speeduino/scheduledIO.h
+++ b/speeduino/scheduledIO.h
@@ -1,30 +1,39 @@
 #ifndef SCHEDULEDIO_H
 #define SCHEDULEDIO_H
 
-inline void openInjector1();
-inline void closeInjector1();
 inline void beginCoil1Charge();
 inline void endCoil1Charge();
 
-inline void openInjector2();
-inline void closeInjector2();
 inline void beginCoil2Charge();
 inline void endCoil2Charge();
 
-inline void openInjector3();
-inline void closeInjector3();
 inline void beginCoil3Charge();
 inline void endCoil3Charge();
 
-inline void openInjector4();
-inline void closeInjector4();
 inline void beginCoil4Charge();
 inline void endCoil4Charge();
 
-inline void openInjector5();
-inline void closeInjector5();
 inline void beginCoil5Charge();
 inline void endCoil5Charge();
 
-#endif
+#define openInjector1() *inj1_pin_port |= (inj1_pin_mask); BIT_SET(currentStatus.squirt, BIT_SQUIRT_INJ1)
+#define closeInjector1() *inj1_pin_port &= ~(inj1_pin_mask);  BIT_CLEAR(currentStatus.squirt, BIT_SQUIRT_INJ1)
+#define openInjector2() *inj2_pin_port |= (inj2_pin_mask); BIT_SET(currentStatus.squirt, BIT_SQUIRT_INJ2)
+#define closeInjector2() *inj2_pin_port &= ~(inj2_pin_mask);  BIT_CLEAR(currentStatus.squirt, BIT_SQUIRT_INJ2)
+#define openInjector3() *inj3_pin_port |= (inj3_pin_mask); BIT_SET(currentStatus.squirt, BIT_SQUIRT_INJ3)
+#define closeInjector3() *inj3_pin_port &= ~(inj3_pin_mask);  BIT_CLEAR(currentStatus.squirt, BIT_SQUIRT_INJ3)
+#define openInjector4() *inj4_pin_port |= (inj4_pin_mask); BIT_SET(currentStatus.squirt, BIT_SQUIRT_INJ4)
+#define closeInjector4() *inj4_pin_port &= ~(inj4_pin_mask);  BIT_CLEAR(currentStatus.squirt, BIT_SQUIRT_INJ4)
+#define openInjector5() *inj5_pin_port |= (inj5_pin_mask)
+#define closeInjector5() *inj5_pin_port &= ~(inj5_pin_mask)
 
+#define openInjector1and4() openInjector1(); openInjector4()
+#define closeInjector1and4() closeInjector1(); closeInjector4()
+#define openInjector2and3() openInjector2(); openInjector3()
+#define closeInjector2and3() closeInjector2(); closeInjector3()
+
+//5 cylinder support doubles up injector 3 as being closese to inj 5 (Crank angle)
+#define openInjector3and5() openInjector3(); openInjector5()
+#define closeInjector3and5() closeInjector3(); closeInjector5()
+
+#endif
diff --git a/speeduino/scheduledIO.ino b/speeduino/scheduledIO.ino
index 8263f2b5..4fc28559 100644
--- a/speeduino/scheduledIO.ino
+++ b/speeduino/scheduledIO.ino
@@ -1,79 +1,25 @@
 
-
-//These functions simply trigger the injector/coil driver off or on.
-//NOTE: squirt status is changed as per http://www.msextra.com/doc/ms1extra/COM_RS232.htm#Acmd
-/*
-#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
-  //For the AVR chips, use the faster bit flipping method of switching pins
-  void ignitionSetter(byte *port, bool startCharge)
-  {
-    if(
-  }
-
-  void openInjector1() { *inj1_pin_port |= (inj1_pin_mask); ; BIT_SET(currentStatus.squirt, BIT_SQUIRT_INJ1); }
-  void closeInjector1() { *inj1_pin_port &= ~(inj1_pin_mask);  BIT_CLEAR(currentStatus.squirt, BIT_SQUIRT_INJ1); }
-  void beginCoil1Charge() { *ign1_pin_port |= (ign1_pin_mask); BIT_SET(currentStatus.spark, 0); digitalWrite(pinTachOut, LOW); }
-  void endCoil1Charge() { *ign1_pin_port &= ~(ign1_pin_mask); BIT_CLEAR(currentStatus.spark, 0); }
-
-  void openInjector2() { *inj2_pin_port |= (inj2_pin_mask); ; BIT_SET(currentStatus.squirt, BIT_SQUIRT_INJ2); }
-  void closeInjector2() { *inj2_pin_port &= ~(inj2_pin_mask);  BIT_CLEAR(currentStatus.squirt, BIT_SQUIRT_INJ2); }
-  void beginCoil2Charge() { *ign2_pin_port |= (ign2_pin_mask); BIT_SET(currentStatus.spark, 1); digitalWrite(pinTachOut, LOW); }
-  void endCoil2Charge() { *ign2_pin_port &= ~(ign2_pin_mask); BIT_CLEAR(currentStatus.spark, 1);}
-
-  void openInjector3() { *inj3_pin_port |= (inj3_pin_mask); ; BIT_SET(currentStatus.squirt, BIT_SQUIRT_INJ3); }
-  void closeInjector3() { *inj3_pin_port &= ~(inj3_pin_mask);  BIT_CLEAR(currentStatus.squirt, BIT_SQUIRT_INJ3); }
-  void beginCoil3Charge() { *ign3_pin_port |= (ign3_pin_mask); BIT_SET(currentStatus.spark, 2); digitalWrite(pinTachOut, LOW); }
-  void endCoil3Charge() { *ign3_pin_port &= ~(ign3_pin_mask); BIT_CLEAR(currentStatus.spark, 2);}
-
-  void openInjector4() { *inj4_pin_port |= (inj4_pin_mask); ; BIT_SET(currentStatus.squirt, BIT_SQUIRT_INJ4); }
-  void closeInjector4() { *inj4_pin_port &= ~(inj4_pin_mask);  BIT_CLEAR(currentStatus.squirt, BIT_SQUIRT_INJ4); }
-  void beginCoil4Charge() { *ign4_pin_port |= (ign4_pin_mask); BIT_SET(currentStatus.spark, 3); digitalWrite(pinTachOut, LOW); }
-  void endCoil4Charge() { *ign4_pin_port &= ~(ign4_pin_mask); BIT_CLEAR(currentStatus.spark, 3);}
-
-#else
-*/
-
 volatile bool tachoAlt = true;
 #define TACH_PULSE_HIGH() *tach_pin_port |= (tach_pin_mask)
-#define TACH_PULSE_LOW() if( (configPage1.tachoDiv == 0) || tachoAlt ) *tach_pin_port &= ~(tach_pin_mask); tachoAlt = !tachoAlt
+#define TACH_PULSE_LOW() if( (configPage1.tachoDiv == 0) || tachoAlt ) { *tach_pin_port &= ~(tach_pin_mask); tachoAlt = !tachoAlt; }
 
-  inline void openInjector1() { digitalWrite(pinInjector1, HIGH); BIT_SET(currentStatus.squirt, BIT_SQUIRT_INJ1); }
-  inline void closeInjector1() { digitalWrite(pinInjector1, LOW); BIT_CLEAR(currentStatus.squirt, BIT_SQUIRT_INJ1); }
   inline void beginCoil1Charge() { digitalWrite(pinCoil1, coilHIGH); TACH_PULSE_LOW(); }
   inline void endCoil1Charge() { digitalWrite(pinCoil1, coilLOW); TACH_PULSE_HIGH(); }
 
-  inline void openInjector2() { digitalWrite(pinInjector2, HIGH); BIT_SET(currentStatus.squirt, BIT_SQUIRT_INJ2); } //Sets the relevant pin HIGH and changes the current status bit for injector 2 (2nd bit of currentStatus.squirt)
-  inline void closeInjector2() { digitalWrite(pinInjector2, LOW); BIT_CLEAR(currentStatus.squirt, BIT_SQUIRT_INJ2); }
   inline void beginCoil2Charge() { digitalWrite(pinCoil2, coilHIGH); TACH_PULSE_LOW(); }
   inline void endCoil2Charge() { digitalWrite(pinCoil2, coilLOW); TACH_PULSE_HIGH(); }
 
-  inline void openInjector3() { digitalWrite(pinInjector3, HIGH); BIT_SET(currentStatus.squirt, BIT_SQUIRT_INJ3); } //Sets the relevant pin HIGH and changes the current status bit for injector 3 (3rd bit of currentStatus.squirt)
-  inline void closeInjector3() { digitalWrite(pinInjector3, LOW); BIT_CLEAR(currentStatus.squirt, BIT_SQUIRT_INJ3); }
   inline void beginCoil3Charge() { digitalWrite(pinCoil3, coilHIGH); TACH_PULSE_LOW(); }
   inline void endCoil3Charge() { digitalWrite(pinCoil3, coilLOW); TACH_PULSE_HIGH(); }
 
-  inline void openInjector4() { digitalWrite(pinInjector4, HIGH); BIT_SET(currentStatus.squirt, BIT_SQUIRT_INJ4); } //Sets the relevant pin HIGH and changes the current status bit for injector 4 (4th bit of currentStatus.squirt)
-  inline void closeInjector4() { digitalWrite(pinInjector4, LOW); BIT_CLEAR(currentStatus.squirt, BIT_SQUIRT_INJ4); }
   inline void beginCoil4Charge() { digitalWrite(pinCoil4, coilHIGH); TACH_PULSE_LOW(); }
   inline void endCoil4Charge() { digitalWrite(pinCoil4, coilLOW); TACH_PULSE_HIGH(); }
 
-  inline void openInjector5() { digitalWrite(pinInjector5, HIGH); }
-  inline void closeInjector5() { digitalWrite(pinInjector5, LOW); }
   inline void beginCoil5Charge() { digitalWrite(pinCoil5, coilHIGH); TACH_PULSE_LOW(); }
   inline void endCoil5Charge() { digitalWrite(pinCoil5, coilLOW); TACH_PULSE_HIGH(); }
 
-//#endif
 
 
-//Combination functions for semi-sequential injection
-void openInjector1and4() { digitalWrite(pinInjector1, HIGH); digitalWrite(pinInjector4, HIGH); BIT_SET(currentStatus.squirt, 0); }
-void closeInjector1and4() { digitalWrite(pinInjector1, LOW); digitalWrite(pinInjector4, LOW);BIT_CLEAR(currentStatus.squirt, 0); }
-void openInjector2and3() { digitalWrite(pinInjector2, HIGH); digitalWrite(pinInjector3, HIGH); BIT_SET(currentStatus.squirt, 1); }
-void closeInjector2and3() { digitalWrite(pinInjector2, LOW); digitalWrite(pinInjector3, LOW); BIT_CLEAR(currentStatus.squirt, 1); }
-//Below functions are used for 5 cylinder support
-void openInjector3and5() { digitalWrite(pinInjector3, HIGH); digitalWrite(pinInjector5, HIGH); BIT_SET(currentStatus.squirt, 0); }
-void closeInjector3and5() { digitalWrite(pinInjector3, LOW); digitalWrite(pinInjector5, LOW);BIT_CLEAR(currentStatus.squirt, 0); }
-
 //As above but for ignition (Wasted COP mode)
 void beginCoil1and3Charge() { digitalWrite(pinCoil1, coilHIGH); digitalWrite(pinCoil3, coilHIGH); digitalWrite(pinTachOut, LOW); }
 void endCoil1and3Charge() { digitalWrite(pinCoil1, coilLOW); digitalWrite(pinCoil3, coilLOW); }
@@ -81,4 +27,3 @@ void beginCoil2and4Charge() { digitalWrite(pinCoil2, coilHIGH); digitalWrite(pin
 void endCoil2and4Charge() { digitalWrite(pinCoil2, coilLOW); digitalWrite(pinCoil4, coilLOW); }
 
 void nullCallback() { return; }
-
diff --git a/speeduino/sensors.h b/speeduino/sensors.h
index a7498efe..63610724 100644
--- a/speeduino/sensors.h
+++ b/speeduino/sensors.h
@@ -86,7 +86,7 @@ ISR(ADC_vect)
   #if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__)
     if (nChannel==7) { ADMUX = 0x40; }
   #elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
-    if(ADCSRB & 0x08) { nChannel += 8; }  //8 to 15
+    if( (ADCSRB & 0x08) > 0) { nChannel += 8; }  //8 to 15
     if(nChannel == 15)
     {
       ADMUX = 0x40; //channel 0
diff --git a/speeduino/sensors.ino b/speeduino/sensors.ino
index be5032f9..bdde8740 100644
--- a/speeduino/sensors.ino
+++ b/speeduino/sensors.ino
@@ -50,14 +50,14 @@ MAPcount = 0;
 void instanteneousMAPReading()
 {
   //Instantaneous MAP readings
-  #if defined(ANALOG_ISR)
+  #if defined(ANALOG_ISR_MAP)
     tempReading = AnChannel[pinMAP-A0];
   #else
     tempReading = analogRead(pinMAP);
     tempReading = analogRead(pinMAP);
   #endif
   //Error checking
-  if(tempReading >= VALID_MAP_MAX || tempReading <= VALID_MAP_MIN) { mapErrorCount += 1; }
+  if( (tempReading >= VALID_MAP_MAX) || (tempReading <= VALID_MAP_MIN) ) { mapErrorCount += 1; }
   else { mapErrorCount = 0; }
 
   currentStatus.mapADC = ADC_FILTER(tempReading, ADCFILTER_MAP, currentStatus.mapADC); //Very weak filter
@@ -80,68 +80,78 @@ void readMAP()
     case 1:
       //Average of a cycle
 
-      if (currentStatus.RPM < 1 || !currentStatus.hasSync) {  instanteneousMAPReading(); return; } //If the engine isn't running, fall back to instantaneous reads
-
-      if( (MAPcurRev == currentStatus.startRevolutions) || (MAPcurRev == currentStatus.startRevolutions+1) ) //2 revolutions are looked at for 4 stroke. 2 stroke not currently catered for.
+      if ( (currentStatus.RPM > 0) && (currentStatus.hasSync == true) ) //If the engine isn't running, fall back to instantaneous reads
       {
-        #if defined(ANALOG_ISR)
-          tempReading = AnChannel[pinMAP-A0];
-        #else
-          tempReading = analogRead(pinMAP);
-          tempReading = analogRead(pinMAP);
-        #endif
-
-        //Error check
-        if(tempReading < VALID_MAP_MAX && tempReading > VALID_MAP_MIN)
+        if( (MAPcurRev == currentStatus.startRevolutions) || (MAPcurRev == (currentStatus.startRevolutions+1)) ) //2 revolutions are looked at for 4 stroke. 2 stroke not currently catered for.
         {
-          MAPrunningValue = MAPrunningValue + tempReading; //Add the current reading onto the total
-          MAPcount++;
+          #if defined(ANALOG_ISR_MAP)
+            tempReading = AnChannel[pinMAP-A0];
+          #else
+            tempReading = analogRead(pinMAP);
+            tempReading = analogRead(pinMAP);
+          #endif
+
+          //Error check
+          if( (tempReading < VALID_MAP_MAX) && (tempReading > VALID_MAP_MIN) )
+          {
+            MAPrunningValue = MAPrunningValue + (unsigned long)tempReading; //Add the current reading onto the total
+            MAPcount++;
+          }
+          else { mapErrorCount += 1; }
+        }
+        else
+        {
+          //Reaching here means that the last cylce has completed and the MAP value should be calculated
+          //Sanity check
+          if( (MAPrunningValue != 0) && (MAPcount != 0) )
+          {
+            currentStatus.mapADC = ldiv(MAPrunningValue, MAPcount).quot;
+            currentStatus.MAP = fastResize(currentStatus.mapADC, configPage1.mapMax); //Get the current MAP value
+            MAPcurRev = currentStatus.startRevolutions; //Reset the current rev count
+            MAPrunningValue = 0;
+            MAPcount = 0;
+          }
+          else { instanteneousMAPReading(); }
         }
-        else { mapErrorCount += 1; }
-      }
-      else
-      {
-        //Reaching here means that the last cylce has completed and the MAP value should be calculated
-
-        //Sanity check
-        if (MAPrunningValue == 0 || MAPcount == 0) {  instanteneousMAPReading(); return; }
-
-        currentStatus.mapADC = ldiv(MAPrunningValue, MAPcount).quot;
-        currentStatus.MAP = fastResize(currentStatus.mapADC, configPage1.mapMax); //Get the current MAP value
-        MAPcurRev = currentStatus.startRevolutions; //Reset the current rev count
-        MAPrunningValue = 0;
-        MAPcount = 0;
       }
+      else {  instanteneousMAPReading(); }
       break;
 
     case 2:
       //Minimum reading in a cycle
-      if (currentStatus.RPM < 1) {  instanteneousMAPReading(); return; } //If the engine isn't running, fall back to instantaneous reads
-
-      if( (MAPcurRev == currentStatus.startRevolutions) || (MAPcurRev == currentStatus.startRevolutions+1) ) //2 revolutions are looked at for 4 stroke. 2 stroke not currently catered for.
+      if (currentStatus.RPM > 0 ) //If the engine isn't running, fall back to instantaneous reads
       {
-        #if defined(ANALOG_ISR)
-          tempReading = AnChannel[pinMAP-A0];
-        #else
-          tempReading = analogRead(pinMAP);
-          tempReading = analogRead(pinMAP);
-        #endif
-        //Error check
-        if(tempReading < VALID_MAP_MAX && tempReading > VALID_MAP_MIN)
+        if( (MAPcurRev == currentStatus.startRevolutions) || (MAPcurRev == (currentStatus.startRevolutions+1)) ) //2 revolutions are looked at for 4 stroke. 2 stroke not currently catered for.
         {
-          if( tempReading < MAPrunningValue) { MAPrunningValue = tempReading; } //Check whether the current reading is lower than the running minimum
+          #if defined(ANALOG_ISR_MAP)
+            tempReading = AnChannel[pinMAP-A0];
+          #else
+            tempReading = analogRead(pinMAP);
+            tempReading = analogRead(pinMAP);
+          #endif
+          //Error check
+          if( (tempReading < VALID_MAP_MAX) && (tempReading > VALID_MAP_MIN) )
+          {
+            if( (unsigned long)tempReading < MAPrunningValue ) { MAPrunningValue = (unsigned long)tempReading; } //Check whether the current reading is lower than the running minimum
+          }
+          else { mapErrorCount += 1; }
+        }
+        else
+        {
+          //Reaching here means that the last cylce has completed and the MAP value should be calculated
+          currentStatus.mapADC = MAPrunningValue;
+          currentStatus.MAP = fastResize(currentStatus.mapADC, configPage1.mapMax); //Get the current MAP value
+          MAPcurRev = currentStatus.startRevolutions; //Reset the current rev count
+          MAPrunningValue = 1023; //Reset the latest value so the next reading will always be lower
         }
-        else { mapErrorCount += 1; }
-      }
-      else
-      {
-        //Reaching here means that the last cylce has completed and the MAP value should be calculated
-        currentStatus.mapADC = MAPrunningValue;
-        currentStatus.MAP = fastResize(currentStatus.mapADC, configPage1.mapMax); //Get the current MAP value
-        MAPcurRev = currentStatus.startRevolutions; //Reset the current rev count
-        MAPrunningValue = 1023; //Reset the latest value so the next reading will always be lower
       }
+      else { instanteneousMAPReading(); }
       break;
+
+    default:
+    //Instantaneous MAP readings (Just in case)
+    instanteneousMAPReading();
+    break;
   }
 }
 
@@ -200,11 +210,11 @@ void readO2()
   currentStatus.O2 = o2CalibrationTable[currentStatus.O2ADC];
 }
 
-/* Second O2 currently disabled as its not being used
-  currentStatus.O2_2ADC = map(analogRead(pinO2_2), 0, 1023, 0, 511); //Get the current O2 value.
-  currentStatus.O2_2ADC = ADC_FILTER(tempReading, ADCFILTER_O2, currentStatus.O2_2ADC);
-  currentStatus.O2_2 = o2CalibrationTable[currentStatus.O2_2ADC];
-*/
+  //Second O2 currently disabled as its not being used
+  //Get the current O2 value.
+  //currentStatus.O2_2ADC = map(analogRead(pinO2_2), 0, 1023, 0, 511);
+  //currentStatus.O2_2ADC = ADC_FILTER(tempReading, ADCFILTER_O2, currentStatus.O2_2ADC);
+  //currentStatus.O2_2 = o2CalibrationTable[currentStatus.O2_2ADC];
 
 void readBat()
 {
diff --git a/speeduino/speeduino.ino b/speeduino/speeduino.ino
index 69d1a5d0..6be82ea5 100644
--- a/speeduino/speeduino.ino
+++ b/speeduino/speeduino.ino
@@ -68,7 +68,7 @@ bool fuelPumpOn = false; //The current status of the fuel pump
 
 void (*trigger)(); //Pointer for the trigger function (Gets pointed to the relevant decoder)
 void (*triggerSecondary)(); //Pointer for the secondary trigger function (Gets pointed to the relevant decoder)
-int (*getRPM)(); //Pointer to the getRPM function (Gets pointed to the relevant decoder)
+uint16_t (*getRPM)(); //Pointer to the getRPM function (Gets pointed to the relevant decoder)
 int (*getCrankAngle)(int); //Pointer to the getCrank Angle function (Gets pointed to the relevant decoder)
 
 byte cltCalibrationTable[CALIBRATION_TABLE_SIZE];
@@ -86,8 +86,6 @@ unsigned long previousLoopTime; //The time the previous loop started (uS)
 
 int CRANK_ANGLE_MAX = 720;
 int CRANK_ANGLE_MAX_IGN = 360, CRANK_ANGLE_MAX_INJ = 360; // The number of crank degrees that the system track over. 360 for wasted / timed batch and 720 for sequential
-//bool useSequentialFuel; // Whether sequential fueling is to be used (1 squirt per cycle)
-//bool useSequentialIgnition; // Whether sequential ignition is used (1 spark per cycle)
 
 static byte coilHIGH = HIGH;
 static byte coilLOW = LOW;
@@ -114,12 +112,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)();
@@ -133,9 +125,9 @@ void (*ign5StartFunction)();
 void (*ign5EndFunction)();
 
 int timePerDegree;
-int dwellAngle;
 byte degreesPerLoop; //The number of crank degrees that pass for each mainloop of the program
 volatile bool fpPrimed = false; //Tracks whether or not the fuel pump priming has been completed yet
+bool initialisationComplete = false; //Tracks whether the setup() functino has run completely
 
 void setup()
 {
@@ -152,18 +144,15 @@ void setup()
   table3D_setSize(&trim2Table, 6);
   table3D_setSize(&trim3Table, 6);
   table3D_setSize(&trim4Table, 6);
+  Serial.begin(115200);
 
   #if defined(CORE_STM32)
-    pinMode(CS, OUTPUT);
-    digitalWrite(CS, HIGH); // disable
-    Spi.begin();
-    loadConfig_STM();
-  #else  
-    loadConfig();
+    EEPROM.init();
   #endif
+  loadConfig();
   doUpdates(); //Check if any data items need updating (Occurs ith firmware updates)
 
-  Serial.begin(115200);
+  //Serial.begin(115200);
 #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) //ATmega2561 does not have Serial3
   if (configPage10.enable_canbus == 1) { Serial3.begin(115200); }
 #elif defined(CORE_STM32)
@@ -172,14 +161,16 @@ void setup()
   {
     //enable local can interface
   }
-#elif defined(CORE_TEESNY)
+#elif defined(CORE_TEENSY)
   if (configPage10.enable_canbus == 1) { Serial2.begin(115200); }
   else if (configPage10.enable_canbus == 2)
   {
+    //Teensy onboard CAN not used currently
     //enable local can interface
-    FlexCAN CANbus0(2500000, 0);   //setup can interface to 250k
-    static CAN_message_t txmsg,rxmsg;
-    CANbus0.begin();
+    //setup can interface to 250k
+    //FlexCAN CANbus0(2500000, 0);
+    //static CAN_message_t txmsg,rxmsg;
+    //CANbus0.begin();
   }
 #endif
 
@@ -306,6 +297,8 @@ void setup()
         triggerInterrupt = 3; break;
       case 21:
         triggerInterrupt = 2; break;
+      default:
+        triggerInterrupt = 0; break; //This should NEVER happen
     }
   #else
     triggerInterrupt = pinTrigger;
@@ -326,6 +319,8 @@ void setup()
         triggerInterrupt2 = 3; break;
       case 21:
         triggerInterrupt2 = 2; break;
+      default:
+        triggerInterrupt2 = 0; break; //This should NEVER happen
     }
   #else
     triggerInterrupt2 = pinTrigger2;
@@ -512,6 +507,17 @@ void setup()
             attachInterrupt(triggerInterrupt2, triggerSec_Subaru67, FALLING);
             break;
 
+    case 14:
+            triggerSetup_Daihatsu();
+            trigger = triggerPri_Daihatsu;
+            getRPM = getRPM_Daihatsu;
+            getCrankAngle = getCrankAngle_Daihatsu;
+
+            if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
+            else { attachInterrupt(triggerInterrupt, trigger, FALLING); }
+            //No secondary input required for this pattern
+            break;
+
     default:
       trigger = triggerPri_missingTooth;
       getRPM = getRPM_missingTooth;
@@ -541,6 +547,7 @@ void setup()
 
       channel1InjEnabled = true;
       break;
+
     case 2:
       channel1IgnDegrees = 0;
 
@@ -561,13 +568,24 @@ void setup()
       channel1InjEnabled = true;
       channel2InjEnabled = true;
       break;
+
     case 3:
       channel1IgnDegrees = 0;
 
       if (configPage1.engineType == EVEN_FIRE )
       {
-        channel2IgnDegrees = 120;
-        channel3IgnDegrees = 240;
+        if(configPage2.sparkMode == IGN_MODE_SEQUENTIAL)
+        {
+          channel2IgnDegrees = 240;
+          channel3IgnDegrees = 480;
+
+          CRANK_ANGLE_MAX_IGN = 720;
+        }
+        else
+        {
+          channel2IgnDegrees = 120;
+          channel3IgnDegrees = 240;
+        }
       }
       else
       {
@@ -579,8 +597,8 @@ void setup()
       if(configPage1.injLayout == INJ_SEMISEQUENTIAL  || configPage1.injLayout == INJ_PAIRED)
       {
         channel1InjDegrees = 0;
-        channel2InjDegrees = channel2IgnDegrees;
-        channel3InjDegrees = channel3IgnDegrees;
+        channel2InjDegrees = 120;
+        channel3InjDegrees = 240;
       }
       else if (configPage1.injLayout == INJ_SEQUENTIAL)
       {
@@ -688,18 +706,12 @@ void setup()
       break;
     case 6:
       channel1IgnDegrees = 0;
+      channel1InjDegrees = 0;
       channel2IgnDegrees = 120;
+      channel2InjDegrees = 120;
       channel3IgnDegrees = 240;
+      channel3InjDegrees = 240;
 
-      //For alternatiing injection, the squirt occurs at different times for each channel
-      /*
-      if(configPage1.injLayout == INJ_SEMISEQUENTIAL || configPage1.injLayout == INJ_SEQUENTIAL || configPage1.injLayout == INJ_PAIRED) //No full sequential for more than 4 cylinders
-      {
-        channel1InjDegrees = 0;
-        channel2InjDegrees = 120;
-        channel3InjDegrees = 240;
-      }
-      */
       if (!configPage1.injTiming) { channel1InjDegrees = channel2InjDegrees = channel3InjDegrees = 0; } //For simultaneous, all squirts happen at the same time
 
       configPage1.injLayout = 0; //This is a failsafe. We can never run semi-sequential with more than 4 cylinders
@@ -709,21 +721,11 @@ void setup()
       channel3InjEnabled = true;
       break;
     case 8:
-      channel1IgnDegrees = 0;
-      channel2IgnDegrees = 90;
-      channel3IgnDegrees = 180;
-      channel4IgnDegrees = 270;
+      channel1IgnDegrees = channel1InjDegrees = 0;
+      channel2IgnDegrees = channel2InjDegrees = 90;
+      channel3IgnDegrees = channel3InjDegrees = 180;
+      channel4IgnDegrees = channel4InjDegrees = 270;
 
-      //For alternatiing injection, the squirt occurs at different times for each channel
-      /*
-      if(configPage1.injLayout == INJ_SEMISEQUENTIAL || configPage1.injTiming == INJ_SEQUENTIAL) //No full sequential for more than 4 cylinders
-      {
-        channel1InjDegrees = 0;
-        channel2InjDegrees = 90;
-        channel3InjDegrees = 180;
-        channel4InjDegrees = 270;
-      }
-      */
       if (!configPage1.injTiming)  { channel1InjDegrees = channel2InjDegrees = channel3InjDegrees = channel4InjDegrees = 0; } //For simultaneous, all squirts happen at the same time
 
       configPage1.injLayout = 0; //This is a failsafe. We can never run semi-sequential with more than 4 cylinders
@@ -832,8 +834,12 @@ void setup()
   digitalWrite(pinFuelPump, HIGH);
   fuelPumpOn = true;
   //Perform the priming pulses. Set these to run at an arbitrary time in the future (100us). The prime pulse value is in ms*10, so need to multiple by 100 to get to uS
-  setFuelSchedule1(openInjector1and4, 100, (unsigned long)(configPage1.primePulse * 100), closeInjector1and4);
-  setFuelSchedule2(openInjector2and3, 100, (unsigned long)(configPage1.primePulse * 100), closeInjector2and3);
+  setFuelSchedule1(100, (unsigned long)(configPage1.primePulse * 100));
+  setFuelSchedule2(100, (unsigned long)(configPage1.primePulse * 100));
+  setFuelSchedule3(100, (unsigned long)(configPage1.primePulse * 100));
+  setFuelSchedule4(100, (unsigned long)(configPage1.primePulse * 100));
+
+  initialisationComplete = true;
 }
 
 void loop()
@@ -901,7 +907,8 @@ void loop()
           }
 #endif
 
-    // if (configPage1.displayType && (mainLoopCount & 255) == 1) { updateDisplay();} //Displays currently disabled
+    //Displays currently disabled
+    // if (configPage1.displayType && (mainLoopCount & 255) == 1) { updateDisplay();}
 
     previousLoopTime = currentLoopTime;
     currentLoopTime = micros();
@@ -922,6 +929,7 @@ void loop()
       currentStatus.runSecs = 0; //Reset the counter for number of seconds running.
       secCounter = 0; //Reset our seconds counter.
       currentStatus.startRevolutions = 0;
+      toothSystemCount = 0;
       MAPcurRev = 0;
       MAPcount = 0;
       currentStatus.rpmDOT = 0;
@@ -1103,15 +1111,6 @@ void loop()
       }
 
       currentStatus.advance = correctionsIgn(currentStatus.advance);
-      /*
-      //Check for fixed ignition angles
-      if (configPage2.FixAng != 0) { currentStatus.advance = configPage2.FixAng; } //Check whether the user has set a fixed timing angle
-      if ( BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) ) { currentStatus.advance = configPage2.CrankAng; } //Use the fixed cranking ignition angle
-      //Adjust the advance based on IAT. If the adjustment amount is greater than the current advance, just set advance to 0
-      byte advanceIATadjust = table2D_getValue(&IATRetardTable, currentStatus.IAT);
-      if (advanceIATadjust <= currentStatus.advance) { currentStatus.advance -= advanceIATadjust; }
-      else { currentStatus.advance = 0; }
-      */
 
       int injector1StartAngle = 0;
       int injector2StartAngle = 0;
@@ -1158,7 +1157,7 @@ void loop()
 
           long toothDeltaV = (1000000L * angle2 / toothHistory[toothHistoryIndex]) - (1000000L * angle1 / toothHistory[toothHistoryIndex-1]);
           long toothDeltaT = toothHistory[toothHistoryIndex];
-          //long timeToLastTooth = micros() - toothLastToothTime; //Cannot be unsigned
+          //long timeToLastTooth = micros() - toothLastToothTime;
 
           rpmDelta = (toothDeltaV << 10) / (6 * toothDeltaT);
         }
@@ -1277,7 +1276,7 @@ void loop()
       else { currentStatus.dwell =  (configPage2.dwellRun * 100); }
       currentStatus.dwell = correctionsDwell(currentStatus.dwell);
 
-      dwellAngle = (div(currentStatus.dwell, timePerDegree).quot ); //Convert the dwell time to dwell angle based on the current engine speed
+      int dwellAngle = (div(currentStatus.dwell, timePerDegree).quot ); //Convert the dwell time to dwell angle based on the current engine speed
 
       //Calculate start angle for each channel
       //1 cylinder (Everyone gets this)
@@ -1289,45 +1288,61 @@ void loop()
       {
         //2 cylinders
         case 2:
-          ignition2StartAngle = SetIgnStartAngle(channel2IgnDegrees);
+          ignition2StartAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance - dwellAngle;
+          if(ignition2StartAngle > CRANK_ANGLE_MAX_IGN) {ignition2StartAngle -= CRANK_ANGLE_MAX_IGN;}
           break;
         //3 cylinders
         case 3:
-          ignition2StartAngle = SetIgnStartAngle(channel2IgnDegrees);
+          ignition2StartAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance - dwellAngle;
+          if(ignition2StartAngle > CRANK_ANGLE_MAX_IGN) {ignition2StartAngle -= CRANK_ANGLE_MAX_IGN;}
           ignition3StartAngle = channel3IgnDegrees + 360 - currentStatus.advance - dwellAngle;
           if(ignition3StartAngle > CRANK_ANGLE_MAX_IGN) {ignition3StartAngle -= CRANK_ANGLE_MAX_IGN;}
           break;
         //4 cylinders
         case 4:
-          ignition2StartAngle = SetIgnStartAngle(channel2IgnDegrees);
+          ignition2StartAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance - dwellAngle;
+          if(ignition2StartAngle > CRANK_ANGLE_MAX_IGN) {ignition2StartAngle -= CRANK_ANGLE_MAX_IGN;}
           if(ignition2StartAngle < 0) {ignition2StartAngle += CRANK_ANGLE_MAX_IGN;}
 
           if(configPage2.sparkMode == IGN_MODE_SEQUENTIAL)
           {
-            ignition3StartAngle = SetIgnStartAngle(channel3IgnDegrees);
-            ignition4StartAngle = SetIgnStartAngle(channel4IgnDegrees);
+            ignition3StartAngle = channel3IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance - dwellAngle;
+            if(ignition3StartAngle > CRANK_ANGLE_MAX_IGN) {ignition3StartAngle -= CRANK_ANGLE_MAX_IGN;}
+            ignition4StartAngle = channel4IgnDegrees + CRANK_ANGLE_MAX - currentStatus.advance - dwellAngle;
+            if(ignition4StartAngle > CRANK_ANGLE_MAX_IGN) {ignition4StartAngle -= CRANK_ANGLE_MAX_IGN;}
           }
           break;
         //5 cylinders
         case 5:
-          ignition2StartAngle = SetIgnStartAngle(channel2IgnDegrees);
+          ignition2StartAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance - dwellAngle;
+          if(ignition2StartAngle > CRANK_ANGLE_MAX_IGN) {ignition2StartAngle -= CRANK_ANGLE_MAX_IGN;}
           if(ignition2StartAngle < 0) {ignition2StartAngle += CRANK_ANGLE_MAX_IGN;}
 
-          ignition3StartAngle = SetIgnStartAngle(channel3IgnDegrees);
-          ignition4StartAngle = SetIgnStartAngle(channel4IgnDegrees);
-          ignition5StartAngle = SetIgnStartAngle(channel5IgnDegrees);
+          ignition3StartAngle = channel3IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance - dwellAngle;
+          if(ignition3StartAngle > CRANK_ANGLE_MAX_IGN) {ignition3StartAngle -= CRANK_ANGLE_MAX_IGN;}
+
+          ignition4StartAngle = channel4IgnDegrees + CRANK_ANGLE_MAX - currentStatus.advance - dwellAngle;
+          if(ignition4StartAngle > CRANK_ANGLE_MAX_IGN) {ignition4StartAngle -= CRANK_ANGLE_MAX_IGN;}
+
+          ignition5StartAngle = channel5IgnDegrees + CRANK_ANGLE_MAX - currentStatus.advance - dwellAngle;
+          if(ignition5StartAngle > CRANK_ANGLE_MAX_IGN) {ignition5StartAngle -= CRANK_ANGLE_MAX_IGN;}
 
           break;
         //6 cylinders
         case 6:
-          ignition2StartAngle = SetIgnStartAngle(channel2IgnDegrees);
-          ignition3StartAngle = SetIgnStartAngle(channel3IgnDegrees);
+          ignition2StartAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance - dwellAngle;
+          if(ignition2StartAngle > CRANK_ANGLE_MAX_IGN) {ignition2StartAngle -= CRANK_ANGLE_MAX_IGN;}
+          ignition3StartAngle = channel3IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance - dwellAngle;
+          if(ignition3StartAngle > CRANK_ANGLE_MAX_IGN) {ignition3StartAngle -= CRANK_ANGLE_MAX_IGN;}
           break;
         //8 cylinders
         case 8:
-          ignition2StartAngle = SetIgnStartAngle(channel2IgnDegrees);
-          ignition3StartAngle = SetIgnStartAngle(channel3IgnDegrees);
-          ignition4StartAngle = SetIgnStartAngle(channel4IgnDegrees);
+          ignition2StartAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance - dwellAngle;
+          if(ignition2StartAngle > CRANK_ANGLE_MAX_IGN) {ignition2StartAngle -= CRANK_ANGLE_MAX_IGN;}
+          ignition3StartAngle = channel3IgnDegrees + CRANK_ANGLE_MAX - currentStatus.advance - dwellAngle;
+          if(ignition3StartAngle > CRANK_ANGLE_MAX_IGN) {ignition3StartAngle -= CRANK_ANGLE_MAX_IGN;}
+          ignition4StartAngle = channel4IgnDegrees + CRANK_ANGLE_MAX - currentStatus.advance - dwellAngle;
+          if(ignition4StartAngle > CRANK_ANGLE_MAX_IGN) {ignition4StartAngle -= CRANK_ANGLE_MAX_IGN;}
           break;
 
         //Will hit the default case on 1 cylinder or >8 cylinders. Do nothing in these cases
@@ -1347,25 +1362,13 @@ 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)
         {
-          if (configPage1.injLayout == INJ_SEMISEQUENTIAL)
-          {
-            setFuelSchedule1(openInjector1and4,
-                      ((unsigned long)(injector1StartAngle - crankAngle) * (unsigned long)timePerDegree),
-                      (unsigned long)currentStatus.PW1,
-                      closeInjector1and4
-                      );
-          }
-          else
-          {
-            setFuelSchedule1(openInjector1,
-                      ((unsigned long)(injector1StartAngle - crankAngle) * (unsigned long)timePerDegree),
-                      (unsigned long)currentStatus.PW1,
-                      closeInjector1
-                      );
-          }
+          setFuelSchedule1(
+                    ((unsigned long)(injector1StartAngle - crankAngle) * (unsigned long)timePerDegree),
+                    (unsigned long)currentStatus.PW1
+                    );
         }
 
         /*-----------------------------------------------------------------------------------------
@@ -1385,25 +1388,13 @@ 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) && (fuelSchedule2.Status == RUNNING) ) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
           if ( tempStartAngle > tempCrankAngle )
           {
-            if (configPage1.injLayout == 1)
-            {
-              setFuelSchedule2(openInjector2and3,
-                        ((unsigned long)(tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree),
-                        (unsigned long)currentStatus.PW2,
-                        closeInjector2and3
-                        );
-            }
-            else
-            {
-              setFuelSchedule2(openInjector2,
-                        ((unsigned long)(tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree),
-                        (unsigned long)currentStatus.PW2,
-                        closeInjector2
-                        );
-            }
+            setFuelSchedule2(
+                      ((unsigned long)(tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree),
+                      (unsigned long)currentStatus.PW2
+                      );
           }
         }
 
@@ -1413,13 +1404,12 @@ 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(openInjector3,
+            setFuelSchedule3(
                       ((unsigned long)(tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree),
-                      (unsigned long)currentStatus.PW3,
-                      closeInjector3
+                      (unsigned long)currentStatus.PW3
                       );
           }
         }
@@ -1430,13 +1420,12 @@ 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(openInjector4,
+            setFuelSchedule4(
                       ((unsigned long)(tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree),
-                      (unsigned long)currentStatus.PW4,
-                      closeInjector4
+                      (unsigned long)currentStatus.PW4
                       );
           }
         }
@@ -1451,10 +1440,15 @@ void loop()
           if ( tempStartAngle > tempCrankAngle )
           {
             //Note the hacky use of fuel schedule 3 below
+            /*
             setFuelSchedule3(openInjector3and5,
                       ((unsigned long)(tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree),
                       (unsigned long)currentStatus.PW1,
                       closeInjector3and5
+                    );*/
+            setFuelSchedule3(
+                      ((unsigned long)(tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree),
+                      (unsigned long)currentStatus.PW1
                       );
           }
         }
@@ -1578,5 +1572,4 @@ void loop()
         }
       } //Ignition schedules on
     } //Has sync and RPM
-} //loop()
-
+} //loop()
diff --git a/speeduino/storage.h b/speeduino/storage.h
index 80275a81..93df9c5b 100644
--- a/speeduino/storage.h
+++ b/speeduino/storage.h
@@ -1,37 +1,11 @@
 #ifndef STORAGE_H
 #define STORAGE_H
-#if defined(CORE_STM32)
-  #include <SPI.h>
-#endif
 
 void writeConfig();
 void loadConfig();
 void loadCalibration();
 void writeCalibration();
 
-#if defined(CORE_STM32)
-  // Based on code by Heather Dewey-Hagborg
-  // http://arduino.cc/en/Tutorial/SPIEEPROM
-
-  #define PAGE_SIZE 256 //W25Q16
-  #define SPI_MODE 0
-  #define CS 10 // chip select
-
-  // EEPROM opcodes
-  #define WREN 6 //Write Enable
-  #define WRDI 4 //Write Disable
-  #define RDSR 5 //Read Status Register-1
-  #define WRSR 1 //Write Status Register
-  #define READ 3 //Read Data
-  #define WRITE 2 //Page Program
-  //STM32F103 clones
-  #if defined (ARDUINO_ARCH_STM32F1)
-    SPIClass Spi(2); //STM32F103 series share SPI1 with analogic channels, use 2 instead.
-  #else
-    SPIClass Spi(1); //STM32F407 eeprom is on SPI1 port
-  #endif
-#endif
-
 /*
 Current layout of EEPROM data (Version 3) is as follows (All sizes are in bytes):
 |---------------------------------------------------|
diff --git a/speeduino/storage.ino b/speeduino/storage.ino
index 974845e5..7db0ea66 100644
--- a/speeduino/storage.ino
+++ b/speeduino/storage.ino
@@ -33,14 +33,14 @@ void writeConfig()
   for(int x=EEPROM_CONFIG1_MAP; x<EEPROM_CONFIG1_XBINS; x++)
   {
     offset = x - EEPROM_CONFIG1_MAP;
-    if(EEPROM.read(x) != fuelTable.values[15-offset/16][offset%16]) { EEPROM.write(x, fuelTable.values[15-offset/16][offset%16]); }  //Write the 16x16 map
+    if( EEPROM.read(x) != (fuelTable.values[15-(offset/16)][offset%16]) ) { EEPROM.write(x, fuelTable.values[15-(offset/16)][offset%16]); }  //Write the 16x16 map
   }
 
   //RPM bins
   for(int x=EEPROM_CONFIG1_XBINS; x<EEPROM_CONFIG1_YBINS; x++)
   {
     offset = x - EEPROM_CONFIG1_XBINS;
-    if(EEPROM.read(x) != byte(fuelTable.axisX[offset]/TABLE_RPM_MULTIPLIER)) { EEPROM.write(x, byte(fuelTable.axisX[offset]/TABLE_RPM_MULTIPLIER)); } //RPM bins are divided by 100 and converted to a byte
+    if( EEPROM.read(x) != (byte(fuelTable.axisX[offset]/TABLE_RPM_MULTIPLIER)) ) { EEPROM.write(x, byte(fuelTable.axisX[offset]/TABLE_RPM_MULTIPLIER)); } //RPM bins are divided by 100 and converted to a byte
   }
   //TPS/MAP bins
   for(int x=EEPROM_CONFIG1_YBINS; x<EEPROM_CONFIG2_START; x++)
@@ -74,7 +74,7 @@ void writeConfig()
   for(int x=EEPROM_CONFIG3_MAP; x<EEPROM_CONFIG3_XBINS; x++)
   {
     offset = x - EEPROM_CONFIG3_MAP;
-    if(EEPROM.read(x) != ignitionTable.values[15-offset/16][offset%16]) { EEPROM.write(x, ignitionTable.values[15-offset/16][offset%16]); }  //Write the 16x16 map with translation
+    if(EEPROM.read(x) != (ignitionTable.values[15-(offset/16)][offset%16]) ) { EEPROM.write(x, ignitionTable.values[15-(offset/16)][offset%16]); }  //Write the 16x16 map with translation
   }
   //RPM bins
   for(int x=EEPROM_CONFIG3_XBINS; x<EEPROM_CONFIG3_YBINS; x++)
@@ -114,7 +114,7 @@ void writeConfig()
   for(int x=EEPROM_CONFIG5_MAP; x<EEPROM_CONFIG5_XBINS; x++)
   {
     offset = x - EEPROM_CONFIG5_MAP;
-    if(EEPROM.read(x) != afrTable.values[15-offset/16][offset%16]) { EEPROM.write(x, afrTable.values[15-offset/16][offset%16]); }  //Write the 16x16 map
+    if(EEPROM.read(x) != (afrTable.values[15-(offset/16)][offset%16]) ) { EEPROM.write(x, afrTable.values[15-(offset/16)][offset%16]); }  //Write the 16x16 map
   }
   //RPM bins
   for(int x=EEPROM_CONFIG5_XBINS; x<EEPROM_CONFIG5_YBINS; x++)
@@ -167,9 +167,9 @@ void writeConfig()
   for(int x=EEPROM_CONFIG8_MAP1; x<EEPROM_CONFIG8_XBINS1; x++)
   {
     offset = x - EEPROM_CONFIG8_MAP1;
-    if(EEPROM.read(x) != boostTable.values[7-offset/8][offset%8]) { EEPROM.write(x, boostTable.values[7-offset/8][offset%8]); }  //Write the 8x8 map
+    if(EEPROM.read(x) != (boostTable.values[7-(offset/8)][offset%8]) ) { EEPROM.write(x, boostTable.values[7-(offset/8)][offset%8]); }  //Write the 8x8 map
     offset = y - EEPROM_CONFIG8_MAP2;
-    if(EEPROM.read(y) != vvtTable.values[7-offset/8][offset%8]) { EEPROM.write(y, vvtTable.values[7-offset/8][offset%8]); }  //Write the 8x8 map
+    if(EEPROM.read(y) != (vvtTable.values[7-(offset/8)][offset%8]) ) { EEPROM.write(y, vvtTable.values[7-(offset/8)][offset%8]); }  //Write the 8x8 map
     y++;
   }
   //RPM bins
@@ -213,13 +213,13 @@ void writeConfig()
   for(int x=EEPROM_CONFIG9_MAP1; x<EEPROM_CONFIG9_XBINS1; x++)
   {
     offset = x - EEPROM_CONFIG9_MAP1;
-    EEPROM.update(x, trim1Table.values[5-offset/6][offset%6]);  //Write the 6x6 map
+    EEPROM.update(x, (trim1Table.values[5-(offset/6)][offset%6]) );  //Write the 6x6 map
     offset = y - EEPROM_CONFIG9_MAP2;
-    EEPROM.update(y, trim2Table.values[5-offset/6][offset%6]);  //Write the 6x6 map
+    EEPROM.update(y, trim2Table.values[5-(offset/6)][offset%6]);  //Write the 6x6 map
     offset = z - EEPROM_CONFIG9_MAP3;
-    EEPROM.update(z, trim3Table.values[5-offset/6][offset%6]);  //Write the 6x6 map
+    EEPROM.update(z, trim3Table.values[5-(offset/6)][offset%6]);  //Write the 6x6 map
     offset = i - EEPROM_CONFIG9_MAP4;
-    EEPROM.update(i, trim4Table.values[5-offset/6][offset%6]);  //Write the 6x6 map
+    EEPROM.update(i, trim4Table.values[5-(offset/6)][offset%6]);  //Write the 6x6 map
     y++;
     z++;
     i++;
@@ -283,12 +283,10 @@ void loadConfig()
 
 
   //Fuel table (See storage.h for data layout)
-  //fuelTable.xSize = EEPROM.read(EEPROM_CONFIG1_XSIZE); //Read the VE Tables RPM dimension size
-  //fuelTable.ySize = EEPROM.read(EEPROM_CONFIG1_YSIZE); //Read the VE Tables MAP/TPS dimension size
   for(int x=EEPROM_CONFIG1_MAP; x<EEPROM_CONFIG1_XBINS; x++)
   {
     offset = x - EEPROM_CONFIG1_MAP;
-    fuelTable.values[15-offset/16][offset%16] = EEPROM.read(x); //Read the 8x8 map
+    fuelTable.values[15-(offset/16)][offset%16] = EEPROM.read(x); //Read the 8x8 map
   }
   //RPM bins
   for(int x=EEPROM_CONFIG1_XBINS; x<EEPROM_CONFIG1_YBINS; x++)
@@ -314,13 +312,10 @@ void loadConfig()
   //IGNITION CONFIG PAGE (2)
 
   //Begin writing the Ignition table, basically the same thing as above
-  //ignitionTable.xSize = EEPROM.read(EEPROM_CONFIG2_XSIZE); //Read the ignition Table RPM dimension size (Currently not supproted)
-  //ignitionTable.ySize = EEPROM.read(EEPROM_CONFIG2_YSIZE); //Read the ignition Table MAP/TPS dimension size (Currently not supproted)
-
   for(int x=EEPROM_CONFIG3_MAP; x<EEPROM_CONFIG3_XBINS; x++)
   {
     offset = x - EEPROM_CONFIG3_MAP;
-    ignitionTable.values[15-offset/16][offset%16] = EEPROM.read(x); //Read the 8x8 map
+    ignitionTable.values[15-(offset/16)][offset%16] = EEPROM.read(x); //Read the 8x8 map
   }
   //RPM bins
   for(int x=EEPROM_CONFIG3_XBINS; x<EEPROM_CONFIG3_YBINS; x++)
@@ -345,13 +340,10 @@ void loadConfig()
   //AFR TARGET CONFIG PAGE (3)
 
   //Begin writing the Ignition table, basically the same thing as above
-  //ignitionTable.xSize = EEPROM.read(EEPROM_CONFIG2_XSIZE); //Read the ignition Table RPM dimension size (Currently not supproted)
-  //ignitionTable.ySize = EEPROM.read(EEPROM_CONFIG2_YSIZE); //Read the ignition Table MAP/TPS dimension size (Currently not supproted)
-
   for(int x=EEPROM_CONFIG5_MAP; x<EEPROM_CONFIG5_XBINS; x++)
   {
     offset = x - EEPROM_CONFIG5_MAP;
-    afrTable.values[15-offset/16][offset%16] = EEPROM.read(x); //Read the 16x16 map
+    afrTable.values[15-(offset/16)][offset%16] = EEPROM.read(x); //Read the 16x16 map
   }
   //RPM bins
   for(int x=EEPROM_CONFIG5_XBINS; x<EEPROM_CONFIG5_YBINS; x++)
@@ -374,13 +366,10 @@ void loadConfig()
 
   //*********************************************************************************************************************************************************************************
 
-  //CONFIG PAGE (4)                                                                                                                                           //############
+  //CONFIG PAGE (4)
   pnt_configPage = (byte *)&configPage4; //Create a pointer to Page 3 in memory
   //Begin writing the Ignition table, basically the same thing as above
-  //ignitionTable.xSize = EEPROM.read(EEPROM_CONFIG2_XSIZE); //Read the ignition Table RPM dimension size (Currently not supproted)
-  //ignitionTable.ySize = EEPROM.read(EEPROM_CONFIG2_YSIZE); //Read the ignition Table MAP/TPS dimension size (Currently not supproted)
-
-  //The next 64 bytes can simply be pulled straight from the configTable
+  //The first 64 bytes can simply be pulled straight from the configTable
   for(int x=EEPROM_CONFIG7_START; x<EEPROM_CONFIG7_END; x++)
   {
     *(pnt_configPage + byte(x - EEPROM_CONFIG7_START)) = EEPROM.read(x);
@@ -392,9 +381,9 @@ void loadConfig()
   for(int x=EEPROM_CONFIG8_MAP1; x<EEPROM_CONFIG8_XBINS1; x++)
   {
     offset = x - EEPROM_CONFIG8_MAP1;
-    boostTable.values[7-offset/8][offset%8] = EEPROM.read(x); //Read the 8x8 map
+    boostTable.values[7-(offset/8)][offset%8] = EEPROM.read(x); //Read the 8x8 map
     offset = y - EEPROM_CONFIG8_MAP2;
-    vvtTable.values[7-offset/8][offset%8] = EEPROM.read(y); //Read the 8x8 map
+    vvtTable.values[7-(offset/8)][offset%8] = EEPROM.read(y); //Read the 8x8 map
     y++;
   }
 
@@ -428,13 +417,13 @@ void loadConfig()
   for(int x=EEPROM_CONFIG9_MAP1; x<EEPROM_CONFIG9_XBINS1; x++)
   {
     offset = x - EEPROM_CONFIG9_MAP1;
-    trim1Table.values[5-offset/6][offset%6] = EEPROM.read(x); //Read the 6x6 map
+    trim1Table.values[5-(offset/6)][offset%6] = EEPROM.read(x); //Read the 6x6 map
     offset = y - EEPROM_CONFIG9_MAP2;
-    trim2Table.values[5-offset/6][offset%6] = EEPROM.read(y); //Read the 6x6 map
+    trim2Table.values[5-(offset/6)][offset%6] = EEPROM.read(y); //Read the 6x6 map
     offset = z - EEPROM_CONFIG9_MAP3;
-    trim3Table.values[5-offset/6][offset%6] = EEPROM.read(z); //Read the 6x6 map
+    trim3Table.values[5-(offset/6)][offset%6] = EEPROM.read(z); //Read the 6x6 map
     offset = i - EEPROM_CONFIG9_MAP4;
-    trim4Table.values[5-offset/6][offset%6] = EEPROM.read(i); //Read the 6x6 map
+    trim4Table.values[5-(offset/6)][offset%6] = EEPROM.read(i); //Read the 6x6 map
     y++;
     z++;
     i++;
@@ -531,512 +520,4 @@ void writeCalibration()
 
 }
 
-byte EEPROM_read(int EEPROM_address)
-{
-  //READ EEPROM
-  digitalWrite(CS, LOW); // EEPROM enable
-  Spi.send(READ); //transmit read opcode
-  Spi.send((uint8)(EEPROM_address>>8)); //send MSByte address first
-  Spi.send((uint8)(EEPROM_address)); //send LSByte address
-  int data = Spi.send(0xFF); //get data byte
-  digitalWrite(CS, HIGH); // EEPROM disable
-  return data;
-}
-
-void EEPROM_write(int EEPROM_address, byte data)
-{
-  //WRITE EEPROM
-  digitalWrite(CS, LOW); // EEPROM enable
-  Spi.send(WREN); // EEPROM write enable instruction
-  digitalWrite(CS, HIGH); // EEPROM disable
-  delay(2);
-  digitalWrite(CS, LOW); // EEPROM enable
-  Spi.send(WRITE); // EEPROM write instruction
-  Spi.send((uint8)(EEPROM_address>>8)); //send MSByte address first
-  Spi.send((uint8)(EEPROM_address)); //send LSByte address
-  Spi.send(data);
-  digitalWrite(CS, HIGH); // EEPROM disable
-}
-
-void EEPROM_update(int EEPROM_address, byte data)
-{
-  byte result = EEPROM_read(EEPROM_address);
-  if(result != data) { EEPROM_write(EEPROM_address, data); }
-}
-
-void writeConfig_STM()
-{
-  /*
-  We only ever write to the EEPROM where the new value is different from the currently stored byte
-  This is due to the limited write life of the EEPROM (Approximately 100,000 writes)
-  */
-
-  int offset;
-  //Create a pointer to the config page
-  byte* pnt_configPage;
-
-  //if(EEPROM.read(0) != data_structure_version) { EEPROM.write(,data_structure_version); }   //Write the data structure version
-  EEPROM_update(0, data_structure_version); //Write the data structure version
-  
-  /*---------------------------------------------------
-  | Fuel table (See storage.h for data layout) - Page 1
-  | 16x16 table itself + the 16 values along each of the axis
-  -----------------------------------------------------*/
-  EEPROM_update(EEPROM_CONFIG1_XSIZE, fuelTable.xSize); //Write the VE Tables RPM dimension size
-  EEPROM_update(EEPROM_CONFIG1_YSIZE, fuelTable.ySize); //Write the VE Tables MAP/TPS dimension size
-
-  for(int x=EEPROM_CONFIG1_MAP; x<EEPROM_CONFIG1_XBINS; x++)
-  {
-    EEPROM_update(x, fuelTable.values[15-offset/16][offset%16]); //Write the 16x16 map
-    offset++;
-  }
-  //RPM bins
-  for(int x=EEPROM_CONFIG1_XBINS; x<EEPROM_CONFIG1_YBINS; x++)
-  {
-    offset = x - EEPROM_CONFIG1_XBINS;
-    EEPROM_update(x, byte(fuelTable.axisX[offset]/TABLE_RPM_MULTIPLIER)); //RPM bins are divided by 100 and converted to a byte
-  }
-  //TPS/MAP bins
-  for(int x=EEPROM_CONFIG1_YBINS; x<EEPROM_CONFIG2_START; x++)
-  {
-    offset = x - EEPROM_CONFIG1_YBINS;
-    EEPROM_update(x, fuelTable.axisY[offset] / TABLE_LOAD_MULTIPLIER); //Table load is divided by 2 (Allows for MAP up to 511)
-  }
-  //That concludes the writing of the VE table
-  //*********************************************************************************************************************************************************************************
-
-  /*---------------------------------------------------
-  | Config page 2 (See storage.h for data layout)
-  | 64 byte long config table
-  -----------------------------------------------------*/
-  pnt_configPage = (byte *)&configPage1; //Create a pointer to Page 2 in memory
-  for(int x=EEPROM_CONFIG2_START; x<EEPROM_CONFIG2_END; x++)
-  {
-    EEPROM_update(x, *(pnt_configPage + byte(x - EEPROM_CONFIG2_START)));
-  }
-  //*********************************************************************************************************************************************************************************
-
-  /*---------------------------------------------------
-  | Ignition table (See storage.h for data layout) - Page 1
-  | 16x16 table itself + the 16 values along each of the axis
-  -----------------------------------------------------*/
-  //Begin writing the Ignition table, basically the same thing as above
-  EEPROM_update(EEPROM_CONFIG3_XSIZE, ignitionTable.xSize); //Write the ignition Table RPM dimension size
-  EEPROM_update(EEPROM_CONFIG3_YSIZE, ignitionTable.ySize); //Write the ignition Table MAP/TPS dimension size
-
-  for(int x=EEPROM_CONFIG3_MAP; x<EEPROM_CONFIG3_XBINS; x++)
-  {
-    offset = x - EEPROM_CONFIG3_MAP;
-    EEPROM_update(x, ignitionTable.values[15-offset/16][offset%16]); //Write the 16x16 map with translation
-  }
-  //RPM bins
-  for(int x=EEPROM_CONFIG3_XBINS; x<EEPROM_CONFIG3_YBINS; x++)
-  {
-    offset = x - EEPROM_CONFIG3_XBINS;
-    EEPROM_update(x, byte(ignitionTable.axisX[offset]/TABLE_RPM_MULTIPLIER)); //RPM bins are divided by 100 and converted to a byte
-  }
-  //TPS/MAP bins
-  for(int x=EEPROM_CONFIG3_YBINS; x<EEPROM_CONFIG4_START; x++)
-  {
-    offset = x - EEPROM_CONFIG3_YBINS;
-    EEPROM_update(x, ignitionTable.axisY[offset]/TABLE_LOAD_MULTIPLIER); //Table load is divided by 2 (Allows for MAP up to 511)
-  }
-  //That concludes the writing of the IGN table
-  //*********************************************************************************************************************************************************************************
-
-  /*---------------------------------------------------
-  | Config page 2 (See storage.h for data layout)
-  | 64 byte long config table
-  -----------------------------------------------------*/
-  pnt_configPage = (byte *)&configPage2; //Create a pointer to Page 2 in memory
-  for(int x=EEPROM_CONFIG4_START; x<EEPROM_CONFIG4_END; x++)
-  {
-    EEPROM_update(x, *(pnt_configPage + byte(x - EEPROM_CONFIG4_START)));
-  }
-  //*********************************************************************************************************************************************************************************
-
-  /*---------------------------------------------------
-  | AFR table (See storage.h for data layout) - Page 5
-  | 16x16 table itself + the 16 values along each of the axis
-  -----------------------------------------------------*/
-  //Begin writing the Ignition table, basically the same thing as above
-  EEPROM_update(EEPROM_CONFIG5_XSIZE, afrTable.xSize); //Write the ignition Table RPM dimension size
-  EEPROM_update(EEPROM_CONFIG5_YSIZE, afrTable.ySize); //Write the ignition Table MAP/TPS dimension size
-
-  for(int x=EEPROM_CONFIG5_MAP; x<EEPROM_CONFIG5_XBINS; x++)
-  {
-    offset = x - EEPROM_CONFIG5_MAP;
-    EEPROM_update(x, afrTable.values[15-offset/16][offset%16]); //Write the 16x16 map
-  }
-  //RPM bins
-  for(int x=EEPROM_CONFIG5_XBINS; x<EEPROM_CONFIG5_YBINS; x++)
-  {
-    offset = x - EEPROM_CONFIG5_XBINS;
-    EEPROM_update(x, byte(afrTable.axisX[offset]/TABLE_RPM_MULTIPLIER)); //RPM bins are divided by 100 and converted to a byte
-  }
-  //TPS/MAP bins
-  for(int x=EEPROM_CONFIG5_YBINS; x<EEPROM_CONFIG6_START; x++)
-  {
-    offset = x - EEPROM_CONFIG5_YBINS;
-    EEPROM_update(x, afrTable.axisY[offset]/TABLE_LOAD_MULTIPLIER); //Table load is divided by 2 (Allows for MAP up to 511)
-  }
-  //That concludes the writing of the AFR table
-  //*********************************************************************************************************************************************************************************
-
-  /*---------------------------------------------------
-  | Config page 3 (See storage.h for data layout)
-  | 64 byte long config table
-  -----------------------------------------------------*/
-  pnt_configPage = (byte *)&configPage3; //Create a pointer to Page 3 in memory
-  for(int x=EEPROM_CONFIG6_START; x<EEPROM_CONFIG6_END; x++)
-  {
-    EEPROM_update(x, *(pnt_configPage + byte(x - EEPROM_CONFIG6_START)));
-  }
-  //*********************************************************************************************************************************************************************************
-
-  /*---------------------------------------------------
-  | Config page 4 (See storage.h for data layout)
-  | 64 byte long config table
-  -----------------------------------------------------*/
-  pnt_configPage = (byte *)&configPage4; //Create a pointer to Page 4 in memory
-  //The next 128 bytes can simply be pulled straight from the configTable
-  for(int x=EEPROM_CONFIG7_START; x<EEPROM_CONFIG7_END; x++)
-  {
-    EEPROM_update(x, *(pnt_configPage + byte(x - EEPROM_CONFIG7_START)));
-  }
-
-  /*---------------------------------------------------
-  | Boost and vvt tables (See storage.h for data layout) - Page 8
-  | 8x8 table itself + the 8 values along each of the axis
-  -----------------------------------------------------*/
-  //Begin writing the 2 tables, basically the same thing as above but we're doing these 2 together (2 tables per page instead of 1)
-  EEPROM_update(EEPROM_CONFIG8_XSIZE1, boostTable.xSize); //Write the boost Table RPM dimension size
-  EEPROM_update(EEPROM_CONFIG8_YSIZE1, boostTable.ySize); //Write the boost Table MAP/TPS dimension size
-  EEPROM_update(EEPROM_CONFIG8_XSIZE2, vvtTable.xSize); //Write the vvt Table RPM dimension size
-  EEPROM_update(EEPROM_CONFIG8_YSIZE2, vvtTable.ySize); //Write the vvt Table MAP/TPS dimension size
-
-  int y = EEPROM_CONFIG8_MAP2; //We do the 2 maps together in the same loop
-  for(int x=EEPROM_CONFIG8_MAP1; x<EEPROM_CONFIG8_XBINS1; x++)
-  {
-    offset = x - EEPROM_CONFIG8_MAP1;
-    EEPROM_update(x, boostTable.values[7-offset/8][offset%8]); //Write the 8x8 map
-    offset = y - EEPROM_CONFIG8_MAP2;
-    EEPROM_update(y, vvtTable.values[7-offset/8][offset%8]); //Write the 8x8 map
-    y++;
-  }
-  //RPM bins
-  y = EEPROM_CONFIG8_XBINS2;
-  for(int x=EEPROM_CONFIG8_XBINS1; x<EEPROM_CONFIG8_YBINS1; x++)
-  {
-    offset = x - EEPROM_CONFIG8_XBINS1;
-    EEPROM_update(x, byte(boostTable.axisX[offset]/TABLE_RPM_MULTIPLIER)); //RPM bins are divided by 100 and converted to a byte
-    offset = y - EEPROM_CONFIG8_XBINS2;
-    EEPROM_update(x, byte(vvtTable.axisX[offset]/TABLE_RPM_MULTIPLIER)); //RPM bins are divided by 100 and converted to a byte
-    y++;
-  }
-  //TPS/MAP bins
-  y=EEPROM_CONFIG8_YBINS2;
-  for(int x=EEPROM_CONFIG8_YBINS1; x<EEPROM_CONFIG8_XSIZE2; x++)
-  {
-    offset = x - EEPROM_CONFIG8_YBINS1;
-    EEPROM_update(x, boostTable.axisY[offset]); //TABLE_LOAD_MULTIPLIER is NOT used for boost as it is TPS based (0-100)
-    offset = y - EEPROM_CONFIG8_YBINS2;
-    EEPROM_update(y, vvtTable.axisY[offset]); //TABLE_LOAD_MULTIPLIER is NOT used for VVT as it is TPS based (0-100)
-    y++;
-  }
-
-  /*---------------------------------------------------
-  | Fuel trim tables (See storage.h for data layout) - Page 9
-  | 6x6 tables itself + the 6 values along each of the axis
-  -----------------------------------------------------*/
-  //Begin writing the 2 tables, basically the same thing as above but we're doing these 2 together (2 tables per page instead of 1)
-  EEPROM_update(EEPROM_CONFIG9_XSIZE1,trim1Table.xSize); //Write the boost Table RPM dimension size
-  EEPROM_update(EEPROM_CONFIG9_YSIZE1,trim1Table.ySize); //Write the boost Table MAP/TPS dimension size
-  EEPROM_update(EEPROM_CONFIG9_XSIZE2,trim2Table.xSize); //Write the boost Table RPM dimension size
-  EEPROM_update(EEPROM_CONFIG9_YSIZE2,trim2Table.ySize); //Write the boost Table MAP/TPS dimension size
-  EEPROM_update(EEPROM_CONFIG9_XSIZE3,trim3Table.xSize); //Write the boost Table RPM dimension size
-  EEPROM_update(EEPROM_CONFIG9_YSIZE3,trim3Table.ySize); //Write the boost Table MAP/TPS dimension size
-  EEPROM_update(EEPROM_CONFIG9_XSIZE4,trim4Table.xSize); //Write the boost Table RPM dimension size
-  EEPROM_update(EEPROM_CONFIG9_YSIZE4,trim4Table.ySize); //Write the boost Table MAP/TPS dimension size
-
-  y = EEPROM_CONFIG9_MAP2; //We do the 4 maps together in the same loop
-  int z = EEPROM_CONFIG9_MAP3; //We do the 4 maps together in the same loop
-  int i = EEPROM_CONFIG9_MAP4; //We do the 4 maps together in the same loop
-  for(int x=EEPROM_CONFIG9_MAP1; x<EEPROM_CONFIG9_XBINS1; x++)
-  {
-    offset = x - EEPROM_CONFIG9_MAP1;
-    EEPROM_update(x, trim1Table.values[5-offset/6][offset%6]);  //Write the 6x6 map
-    offset = y - EEPROM_CONFIG9_MAP2;
-    EEPROM_update(y, trim2Table.values[5-offset/6][offset%6]);  //Write the 6x6 map
-    offset = z - EEPROM_CONFIG9_MAP3;
-    EEPROM_update(z, trim3Table.values[5-offset/6][offset%6]);  //Write the 6x6 map
-    offset = i - EEPROM_CONFIG9_MAP4;
-    EEPROM_update(i, trim4Table.values[5-offset/6][offset%6]);  //Write the 6x6 map
-    y++;
-    z++;
-    i++;
-  }
-  //RPM bins
-  y = EEPROM_CONFIG9_XBINS2;
-  z = EEPROM_CONFIG9_XBINS3;
-  i = EEPROM_CONFIG9_XBINS4;
-  for(int x=EEPROM_CONFIG9_XBINS1; x<EEPROM_CONFIG9_YBINS1; x++)
-  {
-    offset = x - EEPROM_CONFIG9_XBINS1;
-    EEPROM_update(x, byte(trim1Table.axisX[offset]/TABLE_RPM_MULTIPLIER)); //RPM bins are divided by 100 and converted to a byte
-    offset = y - EEPROM_CONFIG9_XBINS2;
-    EEPROM_update(y, byte(trim2Table.axisX[offset]/TABLE_RPM_MULTIPLIER)); //RPM bins are divided by 100 and converted to a byte
-    offset = z - EEPROM_CONFIG9_XBINS3;
-    EEPROM_update(z, byte(trim3Table.axisX[offset]/TABLE_RPM_MULTIPLIER)); //RPM bins are divided by 100 and converted to a byte
-    offset = i - EEPROM_CONFIG9_XBINS4;
-    EEPROM_update(i, byte(trim4Table.axisX[offset]/TABLE_RPM_MULTIPLIER)); //RPM bins are divided by 100 and converted to a byte
-    y++;
-    z++;
-    i++;
-  }
-  //TPS/MAP bins
-  y=EEPROM_CONFIG9_YBINS2;
-  z=EEPROM_CONFIG9_YBINS3;
-  i=EEPROM_CONFIG9_YBINS4;
-  for(int x=EEPROM_CONFIG9_YBINS1; x<EEPROM_CONFIG9_XSIZE2; x++)
-  {
-    offset = x - EEPROM_CONFIG9_YBINS1;
-    EEPROM_update(x, trim1Table.axisY[offset]/TABLE_LOAD_MULTIPLIER); //Table load is divided by 2 (Allows for MAP up to 511)
-    offset = y - EEPROM_CONFIG9_YBINS2;
-    EEPROM_update(y, trim2Table.axisY[offset]/TABLE_LOAD_MULTIPLIER); //Table load is divided by 2 (Allows for MAP up to 511)
-    offset = z - EEPROM_CONFIG9_YBINS3;
-    EEPROM_update(z, trim3Table.axisY[offset]/TABLE_LOAD_MULTIPLIER); //Table load is divided by 2 (Allows for MAP up to 511)
-    offset = i - EEPROM_CONFIG9_YBINS4;
-    EEPROM_update(i, trim4Table.axisY[offset]/TABLE_LOAD_MULTIPLIER); //Table load is divided by 2 (Allows for MAP up to 511)
-    y++;
-    z++;
-    i++;
-  }
-  //*********************************************************************************************************************************************************************************
-
-  /*---------------------------------------------------
-  | Config page 10 (See storage.h for data layout)
-  | 128 byte long config table
-  -----------------------------------------------------*/
-  pnt_configPage = (byte *)&configPage10; //Create a pointer to Page 10 in memory
-  for(int x=EEPROM_CONFIG10_START; x<EEPROM_CONFIG10_END; x++)
-  {
-    EEPROM_update(x, *(pnt_configPage + byte(x - EEPROM_CONFIG10_START)));
-  }
-  //*********************************************************************************************************************************************************************************
-
-}
-
-void loadConfig_STM()
-{
-  int offset;
-  //Create a pointer to the config page
-  byte* pnt_configPage;
-
-  //Fuel table (See storage.h for data layout)
-  //fuelTable.xSize = EEPROM.read(EEPROM_CONFIG1_XSIZE); //Read the VE Tables RPM dimension size
-  //fuelTable.ySize = EEPROM.read(EEPROM_CONFIG1_YSIZE); //Read the VE Tables MAP/TPS dimension size
-  for(int x=EEPROM_CONFIG1_MAP; x<EEPROM_CONFIG1_XBINS; x++)
-  {
-    offset = x - EEPROM_CONFIG1_MAP;
-    fuelTable.values[15-offset/16][offset%16] = EEPROM_read(x); //Read the 8x8 map
-  }
-  //RPM bins
-  for(int x=EEPROM_CONFIG1_XBINS; x<EEPROM_CONFIG1_YBINS; x++)
-  {
-    offset = x - EEPROM_CONFIG1_XBINS;
-    fuelTable.axisX[offset] = (EEPROM_read(x) * TABLE_RPM_MULTIPLIER); //RPM bins are divided by 100 when stored. Multiply them back now
-  }
-  //TPS/MAP bins
-  for(int x=EEPROM_CONFIG1_YBINS; x<EEPROM_CONFIG2_START; x++)
-  {
-    offset = x - EEPROM_CONFIG1_YBINS;
-    fuelTable.axisY[offset] = EEPROM_read(x) * TABLE_LOAD_MULTIPLIER;
-  }
-
-  pnt_configPage = (byte *)&configPage1; //Create a pointer to Page 1 in memory
-  for(int x=EEPROM_CONFIG2_START; x<EEPROM_CONFIG2_END; x++)
-  {
-    *(pnt_configPage + byte(x - EEPROM_CONFIG2_START)) = EEPROM_read(x);
-  }
-  //That concludes the reading of the VE table
-
-  //*********************************************************************************************************************************************************************************
-  //IGNITION CONFIG PAGE (2)
-
-  //Begin writing the Ignition table, basically the same thing as above
-  //ignitionTable.xSize = EEPROM_read(EEPROM_CONFIG2_XSIZE); //Read the ignition Table RPM dimension size (Currently not supproted)
-  //ignitionTable.ySize = EEPROM_read(EEPROM_CONFIG2_YSIZE); //Read the ignition Table MAP/TPS dimension size (Currently not supproted)
-
-  for(int x=EEPROM_CONFIG3_MAP; x<EEPROM_CONFIG3_XBINS; x++)
-  {
-    offset = x - EEPROM_CONFIG3_MAP;
-    ignitionTable.values[15-offset/16][offset%16] = EEPROM_read(x); //Read the 8x8 map
-  }
-  //RPM bins
-  for(int x=EEPROM_CONFIG3_XBINS; x<EEPROM_CONFIG3_YBINS; x++)
-  {
-    offset = x - EEPROM_CONFIG3_XBINS;
-    ignitionTable.axisX[offset] = (EEPROM_read(x) * TABLE_RPM_MULTIPLIER); //RPM bins are divided by 100 when stored. Multiply them back now
-  }
-  //TPS/MAP bins
-  for(int x=EEPROM_CONFIG3_YBINS; x<EEPROM_CONFIG4_START; x++)
-  {
-    offset = x - EEPROM_CONFIG3_YBINS;
-    ignitionTable.axisY[offset] = EEPROM_read(x) * TABLE_LOAD_MULTIPLIER; //Table load is divided by 2 (Allows for MAP up to 511)
-  }
-
-  pnt_configPage = (byte *)&configPage2; //Create a pointer to Page 2 in memory
-  for(int x=EEPROM_CONFIG4_START; x<EEPROM_CONFIG4_END; x++)
-  {
-    *(pnt_configPage + byte(x - EEPROM_CONFIG4_START)) = EEPROM_read(x);
-  }
-
-  //*********************************************************************************************************************************************************************************
-  //AFR TARGET CONFIG PAGE (3)
-
-  //Begin writing the Ignition table, basically the same thing as above
-  //ignitionTable.xSize = EEPROM_read(EEPROM_CONFIG2_XSIZE); //Read the ignition Table RPM dimension size (Currently not supproted)
-  //ignitionTable.ySize = EEPROM_read(EEPROM_CONFIG2_YSIZE); //Read the ignition Table MAP/TPS dimension size (Currently not supproted)
-
-  for(int x=EEPROM_CONFIG5_MAP; x<EEPROM_CONFIG5_XBINS; x++)
-  {
-    offset = x - EEPROM_CONFIG5_MAP;
-    afrTable.values[15-offset/16][offset%16] = EEPROM_read(x); //Read the 16x16 map
-  }
-  //RPM bins
-  for(int x=EEPROM_CONFIG5_XBINS; x<EEPROM_CONFIG5_YBINS; x++)
-  {
-    offset = x - EEPROM_CONFIG5_XBINS;
-    afrTable.axisX[offset] = (EEPROM_read(x) * TABLE_RPM_MULTIPLIER); //RPM bins are divided by 100 when stored. Multiply them back now
-  }
-  //TPS/MAP bins
-  for(int x=EEPROM_CONFIG5_YBINS; x<EEPROM_CONFIG6_START; x++)
-  {
-    offset = x - EEPROM_CONFIG5_YBINS;
-    afrTable.axisY[offset] = EEPROM_read(x) * TABLE_LOAD_MULTIPLIER; //Table load is divided by 2 (Allows for MAP up to 511)
-  }
-
-  pnt_configPage = (byte *)&configPage3; //Create a pointer to Page 2 in memory
-  for(int x=EEPROM_CONFIG6_START; x<EEPROM_CONFIG6_END; x++)
-  {
-    *(pnt_configPage + byte(x - EEPROM_CONFIG6_START)) = EEPROM_read(x);
-  }
-
-  //*********************************************************************************************************************************************************************************
-
-  //CONFIG PAGE (4)                                                                                                                                           //############
-  pnt_configPage = (byte *)&configPage4; //Create a pointer to Page 3 in memory
-  //Begin writing the Ignition table, basically the same thing as above
-  //ignitionTable.xSize = EEPROM_read(EEPROM_CONFIG2_XSIZE); //Read the ignition Table RPM dimension size (Currently not supproted)
-  //ignitionTable.ySize = EEPROM_read(EEPROM_CONFIG2_YSIZE); //Read the ignition Table MAP/TPS dimension size (Currently not supproted)
-
-  //The next 64 bytes can simply be pulled straight from the configTable
-  for(int x=EEPROM_CONFIG7_START; x<EEPROM_CONFIG7_END; x++)
-  {
-    *(pnt_configPage + byte(x - EEPROM_CONFIG7_START)) = EEPROM_read(x);
-  }
-
-  //*********************************************************************************************************************************************************************************
-  // Boost and vvt tables load
-  int y = EEPROM_CONFIG8_MAP2;
-  for(int x=EEPROM_CONFIG8_MAP1; x<EEPROM_CONFIG8_XBINS1; x++)
-  {
-    offset = x - EEPROM_CONFIG8_MAP1;
-    boostTable.values[7-offset/8][offset%8] = EEPROM_read(x); //Read the 8x8 map
-    offset = y - EEPROM_CONFIG8_MAP2;
-    vvtTable.values[7-offset/8][offset%8] = EEPROM_read(y); //Read the 8x8 map
-    y++;
-  }
-
-  //RPM bins
-  y = EEPROM_CONFIG8_XBINS2;
-  for(int x=EEPROM_CONFIG8_XBINS1; x<EEPROM_CONFIG8_YBINS1; x++)
-  {
-    offset = x - EEPROM_CONFIG8_XBINS1;
-    boostTable.axisX[offset] = (EEPROM_read(x) * TABLE_RPM_MULTIPLIER); //RPM bins are divided by 100 when stored. Multiply them back now
-    offset = y - EEPROM_CONFIG8_XBINS2;
-    vvtTable.axisX[offset] = (EEPROM_read(y) * TABLE_RPM_MULTIPLIER); //RPM bins are divided by 100 when stored. Multiply them back now
-    y++;
-  }
-
-  //TPS/MAP bins
-  y = EEPROM_CONFIG8_YBINS2;
-  for(int x=EEPROM_CONFIG8_YBINS1; x<EEPROM_CONFIG8_XSIZE2; x++)
-  {
-    offset = x - EEPROM_CONFIG8_YBINS1;
-    boostTable.axisY[offset] = EEPROM_read(x); //TABLE_LOAD_MULTIPLIER is NOT used for boost as it is TPS based (0-100)
-    offset = y - EEPROM_CONFIG8_YBINS2;
-    vvtTable.axisY[offset] = EEPROM_read(y); //TABLE_LOAD_MULTIPLIER is NOT used for VVT as it is TPS based (0-100)
-    y++;
-  }
-
-  //*********************************************************************************************************************************************************************************
-  // Fuel trim tables load
-  y = EEPROM_CONFIG9_MAP2;
-  int z = EEPROM_CONFIG9_MAP3;
-  int i = EEPROM_CONFIG9_MAP4;
-  for(int x=EEPROM_CONFIG9_MAP1; x<EEPROM_CONFIG9_XBINS1; x++)
-  {
-    offset = x - EEPROM_CONFIG9_MAP1;
-    trim1Table.values[5-offset/6][offset%6] = EEPROM_read(x); //Read the 6x6 map
-    offset = y - EEPROM_CONFIG9_MAP2;
-    trim2Table.values[5-offset/6][offset%6] = EEPROM_read(y); //Read the 6x6 map
-    offset = z - EEPROM_CONFIG9_MAP3;
-    trim3Table.values[5-offset/6][offset%6] = EEPROM_read(z); //Read the 6x6 map
-    offset = i - EEPROM_CONFIG9_MAP4;
-    trim4Table.values[5-offset/6][offset%6] = EEPROM_read(i); //Read the 6x6 map
-    y++;
-    z++;
-    i++;
-  }
-
-  //RPM bins
-  y = EEPROM_CONFIG9_XBINS2;
-  z = EEPROM_CONFIG9_XBINS3;
-  i = EEPROM_CONFIG9_XBINS4;
-  for(int x=EEPROM_CONFIG9_XBINS1; x<EEPROM_CONFIG9_YBINS1; x++)
-  {
-    offset = x - EEPROM_CONFIG9_XBINS1;
-    trim1Table.axisX[offset] = (EEPROM_read(x) * TABLE_RPM_MULTIPLIER); //RPM bins are divided by 100 when stored. Multiply them back now
-    offset = y - EEPROM_CONFIG9_XBINS2;
-    trim2Table.axisX[offset] = (EEPROM_read(y) * TABLE_RPM_MULTIPLIER); //RPM bins are divided by 100 when stored. Multiply them back now
-    offset = z - EEPROM_CONFIG9_XBINS3;
-    trim3Table.axisX[offset] = (EEPROM_read(z) * TABLE_RPM_MULTIPLIER); //RPM bins are divided by 100 when stored. Multiply them back now
-    offset = i - EEPROM_CONFIG9_XBINS4;
-    trim4Table.axisX[offset] = (EEPROM_read(i) * TABLE_RPM_MULTIPLIER); //RPM bins are divided by 100 when stored. Multiply them back now
-    y++;
-    z++;
-    i++;
-  }
-
-  //TPS/MAP bins
-  y = EEPROM_CONFIG9_YBINS2;
-  z = EEPROM_CONFIG9_YBINS3;
-  i = EEPROM_CONFIG9_YBINS4;
-  for(int x=EEPROM_CONFIG9_YBINS1; x<EEPROM_CONFIG9_XSIZE2; x++)
-  {
-    offset = x - EEPROM_CONFIG9_YBINS1;
-    trim1Table.axisY[offset] = EEPROM_read(x) * TABLE_LOAD_MULTIPLIER; //Table load is divided by 2 (Allows for MAP up to 511)
-    offset = y - EEPROM_CONFIG9_YBINS2;
-    trim2Table.axisY[offset] = EEPROM_read(y) * TABLE_LOAD_MULTIPLIER; //Table load is divided by 2 (Allows for MAP up to 511)
-    offset = z - EEPROM_CONFIG9_YBINS3;
-    trim3Table.axisY[offset] = EEPROM_read(z) * TABLE_LOAD_MULTIPLIER; //Table load is divided by 2 (Allows for MAP up to 511)
-    offset = i - EEPROM_CONFIG9_YBINS4;
-    trim4Table.axisY[offset] = EEPROM_read(i) * TABLE_LOAD_MULTIPLIER; //Table load is divided by 2 (Allows for MAP up to 511)
-    y++;
-    z++;
-    i++;
-  }
-  //*********************************************************************************************************************************************************************************
-  //canbus control page load
-    pnt_configPage = (byte *)&configPage10; //Create a pointer to Page 10 in memory
-  for(int x=EEPROM_CONFIG10_START; x<EEPROM_CONFIG10_END; x++)
-  {
-    *(pnt_configPage + byte(x - EEPROM_CONFIG10_START)) = EEPROM_read(x);
-  }
-
-  //*********************************************************************************************************************************************************************************
-
-}
 
diff --git a/speeduino/table.ino b/speeduino/table.ino
index 1862fc0d..0c019ff6 100644
--- a/speeduino/table.ino
+++ b/speeduino/table.ino
@@ -12,42 +12,53 @@ Note that this may clear some of the existing values of the table
 
 void table2D_setSize(struct table2D* targetTable, byte newSize)
 {
-  //2D tables can contain either bytes or ints, depending on the value of the valueSize field
-  if(targetTable->valueSize == SIZE_BYTE)
+  //Table resize is ONLY permitted during system initialisation.
+  //if(initialisationComplete == false)
   {
-    targetTable->values = (byte *)realloc(targetTable->values, newSize * sizeof(byte));
-    targetTable->axisX = (byte *)realloc(targetTable->axisX, newSize * sizeof(byte));
-    targetTable->xSize = newSize;
-  }
-  else
-  {
-    targetTable->values16 = (int *)realloc(targetTable->values16, newSize * sizeof(int));
-    targetTable->axisX16 = (int *)realloc(targetTable->axisX16, newSize * sizeof(int));
-    targetTable->xSize = newSize;
-  }
+    //2D tables can contain either bytes or ints, depending on the value of the valueSize field
+    if(targetTable->valueSize == SIZE_BYTE)
+    {
+      targetTable->values = (byte *)realloc(targetTable->values, newSize * sizeof(byte));
+      targetTable->axisX = (byte *)realloc(targetTable->axisX, newSize * sizeof(byte));
+      targetTable->xSize = newSize;
+    }
+    else
+    {
+      targetTable->values16 = (int *)realloc(targetTable->values16, newSize * sizeof(int));
+      targetTable->axisX16 = (int *)realloc(targetTable->axisX16, newSize * sizeof(int));
+      targetTable->xSize = newSize;
+    } //Byte or int
+  } //initialisationComplete
 }
 
 
 void table3D_setSize(struct table3D *targetTable, byte newSize)
 {
+  if(initialisationComplete == false)
+  {
     targetTable->values = (byte **)malloc(newSize * sizeof(byte*));
     for(byte i = 0; i < newSize; i++) { targetTable->values[i] = (byte *)malloc(newSize * sizeof(byte)); }
-      
+
     targetTable->axisX = (int *)malloc(newSize * sizeof(int));
     targetTable->axisY = (int *)malloc(newSize * sizeof(int));
     targetTable->xSize = newSize;
     targetTable->ySize = newSize;
+  } //initialisationComplete
 }
 
 /*
 This function simply pulls a 1D linear interpolated (ie averaged) value from a 2D table
 ie: Given a value on the X axis, it returns a Y value that coresponds to the point on the curve between the nearest two defined X values
 
-This function must take into account whether a table contains 8-bit or 16-bit values. 
+This function must take into account whether a table contains 8-bit or 16-bit values.
 Unfortunately this means many of the lines are duplicated depending on this
 */
-int table2D_getValue(struct table2D *fromTable, int X)
+int table2D_getValue(struct table2D *fromTable, int X_in)
 {
+    int returnValue;
+    bool valueFound = false;
+
+    int X = X_in;
 
     int xMinValue, xMaxValue;
     if (fromTable->valueSize == SIZE_BYTE)
@@ -64,7 +75,7 @@ int table2D_getValue(struct table2D *fromTable, int X)
     }
     int xMin = 0;
     int xMax = 0;
-    
+
     //If the requested X value is greater/small than the maximum/minimum bin, reset X to be that value
     if(X > xMaxValue) { X = xMaxValue; }
     if(X < xMinValue) { X = xMinValue; }
@@ -72,8 +83,8 @@ int table2D_getValue(struct table2D *fromTable, int X)
 
     if (fromTable->valueSize == SIZE_BYTE)
     {
-       //Byte version
-       
+       //**** Byte version ****
+
        //1st check is whether we're still in the same X bin as last time
        if ( (X <= fromTable->axisX[fromTable->lastXMax]) && (X > fromTable->axisX[fromTable->lastXMin]) )
        {
@@ -84,28 +95,36 @@ int table2D_getValue(struct table2D *fromTable, int X)
        }
        else
        {
-          //
+          //If we're not in the same bin, loop through to find where we are
           for (int x = fromTable->xSize-1; x >= 0; x--)
           {
               //Checks the case where the X value is exactly what was requested
               if ( (X == fromTable->axisX[x]) || (x == 0) )
               {
-                return fromTable->values[x]; //Simply return the coresponding value
+                returnValue = fromTable->values[x]; //Simply return the coresponding value
+                valueFound = true;
               }
-              //Normal case
-              if ( (X <= fromTable->axisX[x]) && (X > fromTable->axisX[x-1]) )
+              else
               {
-                xMaxValue = fromTable->axisX[x];
-                xMinValue = fromTable->axisX[x-1];
-                fromTable->lastXMax = xMax = x;
-                fromTable->lastXMin = xMin = x-1;
-                break;
-              }   
+                //Normal case
+                if ( (X <= fromTable->axisX[x]) && (X > fromTable->axisX[x-1]) )
+                {
+                  xMaxValue = fromTable->axisX[x];
+                  xMinValue = fromTable->axisX[x-1];
+                  xMax = x;
+                  fromTable->lastXMax = xMax;
+                  xMin = x-1;
+                  fromTable->lastXMin = xMin;
+                  break;
+                }
+              }
           }
        }
     }
     else
     {
+      // **** INT VERSION ****
+
        //1st check is whether we're still in the same X bin as last time
        if ( (X <= fromTable->axisX16[fromTable->lastXMax]) && (X > fromTable->axisX16[fromTable->lastXMin]) )
        {
@@ -116,118 +135,91 @@ int table2D_getValue(struct table2D *fromTable, int X)
        }
        else
        {
-          //
+          //If we're not in the same bin, loop through to find where we are
           for (int x = fromTable->xSize-1; x >= 0; x--)
           {
               //Checks the case where the X value is exactly what was requested
               if ( (X == fromTable->axisX16[x]) || (x == 0) )
               {
-                return fromTable->values16[x]; //Simply return the coresponding value
+                returnValue = fromTable->values16[x]; //Simply return the coresponding value
+                valueFound = true;
               }
-              //Normal case
-              if ( (X <= fromTable->axisX16[x]) && (X > fromTable->axisX16[x-1]) )
+              else
               {
-                xMaxValue = fromTable->axisX16[x];
-                xMinValue = fromTable->axisX16[x-1];
-                fromTable->lastXMax = xMax = x;
-                fromTable->lastXMin = xMin = x-1;
-                break;
-              }    
-          }
+                //Normal case
+                if ( (X <= fromTable->axisX16[x]) && (X > fromTable->axisX16[x-1]) )
+                {
+                  xMaxValue = fromTable->axisX16[x];
+                  xMinValue = fromTable->axisX16[x-1];
+                  xMax = x;
+                  fromTable->lastXMax = xMax;
+                  xMin = x-1;
+                  fromTable->lastXMin = xMin;
+                  break;
+                } //Found in loop
+              } //Exact hit
+          } //For loop
        }
-      
+
     }
 
-/*
-    for (int x = fromTable->xSize-1; x >= 0; x--)
+    if (valueFound == false)
     {
-       if (fromTable->valueSize == SIZE_BYTE)
-       {
-          //Byte version
-          //Checks the case where the X value is exactly what was requested
-          if ( (X == fromTable->axisX[x]) || (x == 0) )
-          {
-            return fromTable->values[x]; //Simply return the coresponding value
-          }
-          //Normal case
-          if ( (X <= fromTable->axisX[x]) && (X > fromTable->axisX[x-1]) )
-          {
-            xMaxValue = fromTable->axisX[x];
-            xMinValue = fromTable->axisX[x-1];
-            xMax = x;
-            xMin = x-1;
-            break;
-          }
-       }
-       else
-       {
+      unsigned int m = X - xMinValue;
+      unsigned int n = xMaxValue - xMinValue;
+
+      //Float version
+      /*
+      int yVal = (m / n) * (abs(fromTable.values[xMax] - fromTable.values[xMin]));
+      */
+
+      //Non-Float version
+      int yVal;
+      if (fromTable->valueSize == SIZE_BYTE)
+      {
+         //Byte version
+         yVal = ((long)(m << 6) / n) * (abs(fromTable->values[xMax] - fromTable->values[xMin]));
+         yVal = (yVal >> 6);
+
+         if (fromTable->values[xMax] > fromTable->values[xMin]) { yVal = fromTable->values[xMin] + yVal; }
+         else { yVal = fromTable->values[xMin] - yVal; }
+      }
+      else
+      {
          //int version
-         if ( (X == fromTable->axisX16[x]) || (x == 0) )
-          {
-            return fromTable->values16[x]; //Simply return the coresponding value
-          }
-          //Normal case
-          if ( (X <= fromTable->axisX16[x]) && (X > fromTable->axisX16[x-1]) )
-          {
-            xMaxValue = fromTable->axisX16[x];
-            xMinValue = fromTable->axisX16[x-1];
-            xMax = x;
-            xMin = x-1;
-            break;
-          }
-       }
+         yVal = ((long)(m << 6) / n) * (abs(fromTable->values16[xMax] - fromTable->values16[xMin]));
+         yVal = (yVal >> 6);
+
+         if (fromTable->values[xMax] > fromTable->values16[xMin]) { yVal = fromTable->values16[xMin] + yVal; }
+         else { yVal = fromTable->values16[xMin] - yVal; }
+      }
+      returnValue = yVal;
     }
-    */
-    
-    unsigned int m = X - xMinValue;
-    unsigned int n = xMaxValue - xMinValue;
-    
-    //Float version
-    /*
-    int yVal = (m / n) * (abs(fromTable.values[xMax] - fromTable.values[xMin]));
-    */
-    
-    //Non-Float version
-    int yVal;
-    if (fromTable->valueSize == SIZE_BYTE)
-    {
-       //Byte version
-       yVal = ((long)(m << 6) / n) * (abs(fromTable->values[xMax] - fromTable->values[xMin]));
-       yVal = (yVal >> 6);
-        
-       if (fromTable->values[xMax] > fromTable->values[xMin]) { yVal = fromTable->values[xMin] + yVal; }
-       else { yVal = fromTable->values[xMin] - yVal; }
-    }
-    else
-    {
-       //int version
-       yVal = ((long)(m << 6) / n) * (abs(fromTable->values16[xMax] - fromTable->values16[xMin]));
-       yVal = (yVal >> 6);
-        
-       if (fromTable->values[xMax] > fromTable->values16[xMin]) { yVal = fromTable->values16[xMin] + yVal; }
-       else { yVal = fromTable->values16[xMin] - yVal; }
-    }
-    
-    return yVal;
+
+    return returnValue;
 }
 
 
 //This function pulls a value from a 3D table given a target for X and Y coordinates.
 //It performs a 2D linear interpolation as descibred in: http://www.megamanual.com/v22manual/ve_tuner.pdf
-int get3DTableValue(struct table3D *fromTable, int Y, int X)
+int get3DTableValue(struct table3D *fromTable, int Y_in, int X_in)
   {
+    int X = X_in;
+    int Y = Y_in;
+
+    int tableResult = 0;
     //Loop through the X axis bins for the min/max pair
-    //Note: For the X axis specifically, rather than looping from tableAxisX[0] up to tableAxisX[max], we start at tableAxisX[Max] and go down. 
+    //Note: For the X axis specifically, rather than looping from tableAxisX[0] up to tableAxisX[max], we start at tableAxisX[Max] and go down.
     //      This is because the important tables (fuel and injection) will have the highest RPM at the top of the X axis, so starting there will mean the best case occurs when the RPM is highest (And hence the CPU is needed most)
     int xMinValue = fromTable->axisX[0];
     int xMaxValue = fromTable->axisX[fromTable->xSize-1];
     byte xMin = 0;
     byte xMax = 0;
-    
+
     //If the requested X value is greater/small than the maximum/minimum bin, reset X to be that value
     if(X > xMaxValue) { X = xMaxValue; }
     if(X < xMinValue) { X = xMinValue; }
-    
+
     //1st check is whether we're still in the same X bin as last time
     if ( (X <= fromTable->axisX[fromTable->lastXMax]) && (X > fromTable->axisX[fromTable->lastXMin]) )
     {
@@ -239,18 +231,22 @@ int get3DTableValue(struct table3D *fromTable, int Y, int X)
     //2nd check is whether we're in the next RPM bin (To the right)
     else if ( ((fromTable->lastXMax + 1) < fromTable->xSize ) && (X <= fromTable->axisX[fromTable->lastXMax +1 ]) && (X > fromTable->axisX[fromTable->lastXMin + 1]) ) //First make sure we're not already at the last X bin
     {
-      fromTable->lastXMax = xMax = fromTable->lastXMax + 1;
-      fromTable->lastXMin = xMin = fromTable->lastXMin + 1;
+      xMax = fromTable->lastXMax + 1;
+      fromTable->lastXMax = xMax;
+      xMin = fromTable->lastXMin + 1;
+      fromTable->lastXMin = xMin;
       xMaxValue = fromTable->axisX[fromTable->lastXMax];
       xMinValue = fromTable->axisX[fromTable->lastXMin];
     }
     //3rd check is to look at the previous bin (to the left)
     else if ( (fromTable->lastXMin > 0 ) && (X <= fromTable->axisX[fromTable->lastXMax - 1]) && (X > fromTable->axisX[fromTable->lastXMin - 1]) ) //First make sure we're not already at the first X bin
     {
-      fromTable->lastXMax = xMax = fromTable->lastXMax - 1;
-      fromTable->lastXMin = xMin = fromTable->lastXMin - 1;
+      xMax = fromTable->lastXMax - 1;
+      fromTable->lastXMax = xMax;
+      xMin = fromTable->lastXMin - 1;
+      fromTable->lastXMin = xMin;
       xMaxValue = fromTable->axisX[fromTable->lastXMax];
-      xMinValue = fromTable->axisX[fromTable->lastXMin];      
+      xMinValue = fromTable->axisX[fromTable->lastXMin];
     }
     else
     //If it's not caught by one of the above scenarios, give up and just run the loop
@@ -262,8 +258,10 @@ int get3DTableValue(struct table3D *fromTable, int Y, int X)
         {
           xMaxValue = fromTable->axisX[x];
           xMinValue = fromTable->axisX[x];
-          fromTable->lastXMax = xMax = x;
-          fromTable->lastXMin = xMin = x;
+          xMax = x;
+          fromTable->lastXMax = xMax;
+          xMin = x;
+          fromTable->lastXMin = xMin;
           break;
         }
         //Normal case
@@ -271,23 +269,25 @@ int get3DTableValue(struct table3D *fromTable, int Y, int X)
         {
           xMaxValue = fromTable->axisX[x];
           xMinValue = fromTable->axisX[x-1];
-          fromTable->lastXMax = xMax = x;
-          fromTable->lastXMin = xMin = x-1;
+          xMax = x;
+          fromTable->lastXMax = xMax;
+          xMin = x-1;
+          fromTable->lastXMin = xMin;
           break;
-        }   
+        }
       }
     }
-    
+
     //Loop through the Y axis bins for the min/max pair
     int yMaxValue = fromTable->axisY[0];
     int yMinValue = fromTable->axisY[fromTable->ySize-1];
     byte yMin = 0;
     byte yMax = 0;
-    
+
     //If the requested Y value is greater/small than the maximum/minimum bin, reset Y to be that value
     if(Y > yMaxValue) { Y = yMaxValue; }
     if(Y < yMinValue) { Y = yMinValue; }
-    
+
     //1st check is whether we're still in the same Y bin as last time
     if ( (Y >= fromTable->axisY[fromTable->lastYMax]) && (Y < fromTable->axisY[fromTable->lastYMin]) )
     {
@@ -299,23 +299,27 @@ int get3DTableValue(struct table3D *fromTable, int Y, int X)
     //2nd check is whether we're in the next MAP/TPS bin (Next one up)
     else if ( (fromTable->lastYMin > 0 ) && (Y <= fromTable->axisY[fromTable->lastYMin - 1 ]) && (Y > fromTable->axisY[fromTable->lastYMax - 1]) ) //First make sure we're not already at the top Y bin
     {
-      fromTable->lastYMax = yMax = fromTable->lastYMax - 1;
-      fromTable->lastYMin = yMin = fromTable->lastYMin - 1;
+      yMax = fromTable->lastYMax - 1;
+      fromTable->lastYMax = yMax;
+      yMin = fromTable->lastYMin - 1;
+      fromTable->lastYMin = yMin;
       yMaxValue = fromTable->axisY[fromTable->lastYMax];
       yMinValue = fromTable->axisY[fromTable->lastYMin];
     }
     //3rd check is to look at the previous bin (Next one down)
     else if ( ((fromTable->lastYMax + 1) < fromTable->ySize) && (Y <= fromTable->axisY[fromTable->lastYMin + 1]) && (Y > fromTable->axisY[fromTable->lastYMax + 1]) ) //First make sure we're not already at the bottom Y bin
     {
-      fromTable->lastYMax = yMax = fromTable->lastYMax + 1;
-      fromTable->lastYMin = yMin = fromTable->lastYMin + 1;
+      yMax = fromTable->lastYMax + 1;
+      fromTable->lastYMax = yMax;
+      yMin = fromTable->lastYMin + 1;
+      fromTable->lastYMin = yMin;
       yMaxValue = fromTable->axisY[fromTable->lastYMax];
-      yMinValue = fromTable->axisY[fromTable->lastYMin];      
+      yMinValue = fromTable->axisY[fromTable->lastYMin];
     }
     else
     //If it's not caught by one of the above scenarios, give up and just run the loop
     {
-      
+
       for (byte y = fromTable->ySize-1; y >= 0; y--)
       {
         //Checks the case where the Y value is exactly what was requested
@@ -323,8 +327,10 @@ int get3DTableValue(struct table3D *fromTable, int Y, int X)
         {
           yMaxValue = fromTable->axisY[y];
           yMinValue = fromTable->axisY[y];
-          fromTable->lastYMax = yMax = y;
-          fromTable->lastYMin = yMin = y;
+          yMax = y;
+          fromTable->lastYMax = yMax;
+          yMin = y;
+          fromTable->lastYMin = yMin;
           break;
         }
         //Normal case
@@ -332,25 +338,27 @@ int get3DTableValue(struct table3D *fromTable, int Y, int X)
         {
           yMaxValue = fromTable->axisY[y];
           yMinValue = fromTable->axisY[y-1];
-          fromTable->lastYMax = yMax = y;
-          fromTable->lastYMin = yMin = y-1;
+          yMax = y;
+          fromTable->lastYMax = yMax;
+          yMin = y-1;
+          fromTable->lastYMin = yMin;
           break;
-        }   
+        }
       }
     }
-        
-    
-    /* 
+
+
+    /*
     At this point we have the 4 corners of the map where the interpolated value will fall in
     Eg: (yMin,xMin)  (yMin,xMax)
-    
+
         (yMax,xMin)  (yMax,xMax)
-        
+
     In the following calculation the table values are referred to by the following variables:
               A          B
-              
+
               C          D
-    
+
     */
     int A = fromTable->values[yMin][xMin];
     int B = fromTable->values[yMin][xMax];
@@ -358,122 +366,51 @@ int get3DTableValue(struct table3D *fromTable, int Y, int X)
     int D = fromTable->values[yMax][xMax];
 
     //Check that all values aren't just the same (This regularly happens with things like the fuel trim maps)
-    if(A == D) { return A; } //yMin = yMax and xMin = xMax
-   
-    //Create some normalised position values
-    //These are essentially percentages (between 0 and 1) of where the desired value falls between the nearest bins on each axis
-    
-    
-    //Initial check incase the values were hit straight on
-    long p;
-    if (xMaxValue == xMinValue) { p = ((long)(X - xMinValue) << 8); }  //This only occurs if the requested X value was equal to one of the X axis bins
-    else { p = ((long)(X - xMinValue) << 8) / (xMaxValue - xMinValue); } //This is the standard case
-    
-    long q;
-    if (yMaxValue == yMinValue) { q = ((long)(Y - yMinValue) << 8); }
-    else { q = 256 - (((long)(Y - yMaxValue) << 8) / (yMinValue - yMaxValue)); }
+    if( (A == D) ) { tableResult = A; }
+    else
+    {
+      //Create some normalised position values
+      //These are essentially percentages (between 0 and 1) of where the desired value falls between the nearest bins on each axis
 
-    int m = ((256-p) * (256-q)) >> 8;
-    int n = (p * (256-q)) >> 8;
-    int o = ((256-p) * q) >> 8;
-    int r = (p * q) >> 8;
-    return ( (A * m) + (B * n) + (C * o) + (D * r) ) >> 8;
+
+      //Initial check incase the values were hit straight on
+      long p = (long)X - xMinValue;
+      if (xMaxValue == xMinValue) { p = (p << 8); }  //This only occurs if the requested X value was equal to one of the X axis bins
+      else { p = ( (p << 8) / (xMaxValue - xMinValue) ); } //This is the standard case
+
+      long q = (long)Y - yMinValue;
+      if (yMaxValue == yMinValue) { q = (q << 8); }
+      else { q = 256 - ( (q << 8) / (yMinValue - yMaxValue) ); }
+
+      int m = ((256-p) * (256-q)) >> 8;
+      int n = (p * (256-q)) >> 8;
+      int o = ((256-p) * q) >> 8;
+      int r = (p * q) >> 8;
+      tableResult = ( (A * m) + (B * n) + (C * o) + (D * r) ) >> 8;
+    }
+    return tableResult;
 }
 
-
 /* Executed a benchmark on all options and this is the results
- * @Mega: Stadard:226224 91 |FP Math:32240 91.89 |Clean code:34056 91, Number of loops:2500
- * @STM32: Stadard:1624  91 |FP Math:85180 91.89 |Clean code:1001 91 , Number of loops:2500
- * @STM32 Stadard:13000  loops/S |Clean code:15700 loops/S @8000RPM ~20.8% faster
+ * @Mega:   Stadard:463028 92 |FP Math:850392 92.57 |Clean code:618308 92  , Number of loops:5000
+ * @STM32F1: Stadard:21449 92 |FP Math:125707 92.57  |Clean code:11634 92  , Number of loops:5000
+ * @STM32F4: Stadard:7123 92  |FP Math:16503 92.57  |Clean code:4618 92    , Number of loops:5000
+ * @STM32F1: Stadard:13000  loops/S |Clean code:15700 loops/S @8000RPM ~20.8% faster
 
 //This function pulls a value from a 3D table given a target for X and Y coordinates.
 //It performs a 2D linear interpolation as descibred in: http://www.megamanual.com/v22manual/ve_tuner.pdf
 float get3DTableValueF(struct table3D *fromTable, int Y, int X)
 {
-  float m, n, o ,p, q, r;
   byte xMin, xMax;
   byte yMin, yMax;
   int yMaxValue, yMinValue;
   int xMaxValue, xMinValue;
+  float tableResult = 0;
 
-  if(fromTable->lastXMin==0) {fromTable->lastXMin = fromTable->xSize-1;}
-  else {xMin = fromTable->lastXMin;}
-  if(fromTable->lastYMin==0) {fromTable->lastYMin = fromTable->ySize-1;}
-  else {yMin = fromTable->lastYMin;}
-  //yMin = fromTable->lastYMin;
-
-  if(xMin>fromTable->xSize-1)
-  { 
-    fromTable->lastXMin = fromTable->xSize-1;
-    xMin = fromTable->xSize-1;
-  }  
-  if(yMin>fromTable->ySize-1)
-  { 
-    fromTable->lastYMin = fromTable->ySize-1;
-    yMin = fromTable->ySize-1;
-  }    
-  
-  do  //RPM axis
-  {
-    if(X>=fromTable->axisX[xMin]) {break;}
-    xMin--;
-  }while(1);
-  fromTable->lastXMin = xMin + 1;
-  do  //MAP axis
-  {
-    if(Y<=fromTable->axisY[yMin]) {break;}
-    yMin--;
-  }while(1);
-  fromTable->lastYMin = yMin + 1;
-  
-  xMax = xMin + 1;
-  yMax = yMin + 1;
-  if (xMax>fromTable->xSize-1)  //Overflow protection
-  {
-    xMax = fromTable->xSize-1;
-    xMin = xMax - 1;
-  }
-  if (yMax>fromTable->ySize-1)  //Overflow protection
-  {
-    yMax = fromTable->ySize-1;
-    yMin = yMax - 1;
-  }
-
-  yMaxValue = fromTable->axisY[yMax];
-  yMinValue = fromTable->axisY[yMin];
-  xMaxValue = fromTable->axisX[xMax];
-  xMinValue = fromTable->axisX[xMin]; 
-      
-  int A = fromTable->values[yMin][xMin];
-  int B = fromTable->values[yMin][xMax];
-  int C = fromTable->values[yMax][xMin];
-  int D = fromTable->values[yMax][xMax];
-     
-  p = float(X - xMinValue) / (xMaxValue - xMinValue); //(RPM - RPM[1])/(RPM[2]- RPM[1])
-  q = float(Y - yMinValue) / (yMaxValue - yMinValue); //(MAP - MAP[1])/(MAP[2]- MAP[1])
-
-  m = (1.0-p) * (1.0-q);
-  n = p * (1-q);
-  o = (1-p) * q;
-  r = p * q;
-  
-  return ( (A * m) + (B * n) + (C * o) + (D * r) );    
-}
-
-//This function pulls a value from a 3D table given a target for X and Y coordinates.
-//It performs a 2D linear interpolation as descibred in: http://www.megamanual.com/v22manual/ve_tuner.pdf
-int get3DTableValueS(struct table3D *fromTable, int Y, int X)
-{
-  byte xMin, xMax;
-  byte yMin, yMax;
-  long p, q;
-  int yMaxValue, yMinValue;
-  int xMaxValue, xMinValue;
-
-  if(fromTable->lastXMin == 0) {fromTable->lastXMin = fromTable->xSize-1;}
-  else {xMin = fromTable->lastXMin;}
-  if(fromTable->lastYMin == 0) {fromTable->lastYMin = fromTable->ySize-1;}
-  else {yMin = fromTable->lastYMin;}
+  xMin = fromTable->lastXMin;
+  yMin = fromTable->lastYMin;
+  if(xMin == 0) { xMin = fromTable->xSize-1; }
+  if(yMin == 0) { yMin = fromTable->ySize-1; }
 
   do  //RPM axis
   {
@@ -503,24 +440,98 @@ int get3DTableValueS(struct table3D *fromTable, int Y, int X)
   }
   fromTable->lastYMin = yMin + 1;
 
-  yMaxValue = fromTable->axisY[yMax];
-  yMinValue = fromTable->axisY[yMin];
-  xMaxValue = fromTable->axisX[xMax];
-  xMinValue = fromTable->axisX[xMin]; 
-  
   int A = fromTable->values[yMin][xMin];
-  int B = fromTable->values[yMin][xMax];
-  int C = fromTable->values[yMax][xMin];
   int D = fromTable->values[yMax][xMax];
-  if(A == D) { return A; }
+  if(A == D) { tableResult = A; }
+  else
+  {
+    int B = fromTable->values[yMin][xMax];
+    int C = fromTable->values[yMax][xMin];
 
-  p = ((long)(X - xMinValue) << 8) / (xMaxValue - xMinValue); //(RPM - RPM[1])/(RPM[2]- RPM[1])
-  q = 256 - (((long)(Y - yMaxValue) << 8) / (yMinValue - yMaxValue)); //(MAP - MAP[2])/(MAP[2]- MAP[1])
+    yMaxValue = fromTable->axisY[yMax];
+    yMinValue = fromTable->axisY[yMin];
+    xMaxValue = fromTable->axisX[xMax];
+    xMinValue = fromTable->axisX[xMin]; 
+     
+    float m, n, o ,p, q, r;
+    p = float(X - xMinValue) / (xMaxValue - xMinValue); //(RPM - RPM[1])/(RPM[2]- RPM[1])
+    q = float(Y - yMinValue) / (yMaxValue - yMinValue); //(MAP - MAP[1])/(MAP[2]- MAP[1])
 
-  int m = ((256-p) * (256-q)) >> 8;
-  int n = (p * (256-q)) >> 8;
-  int o = ((256-p) * q) >> 8;
-  int r = (p * q) >> 8;
-  return ( (A * m) + (B * n) + (C * o) + (D * r) ) >> 8; 
+    m = (1.0-p) * (1.0-q);
+    n = p * (1-q);
+    o = (1-p) * q;
+    r = p * q;
+    tableResult = ( (A * m) + (B * n) + (C * o) + (D * r) );
+  }
+  return tableResult;
 }
-*/
+
+//This function pulls a value from a 3D table given a target for X and Y coordinates.
+//It performs a 2D linear interpolation as descibred in: http://www.megamanual.com/v22manual/ve_tuner.pdf
+int get3DTableValueS(struct table3D *fromTable, int Y, int X)
+{
+  byte xMin, xMax;
+  byte yMin, yMax;
+  long p, q;
+  int yMaxValue, yMinValue;
+  int xMaxValue, xMinValue;
+  int tableResult = 0;
+
+  xMin = fromTable->lastXMin;
+  yMin = fromTable->lastYMin;
+  if(xMin == 0) { xMin = fromTable->xSize-1; }
+  if(yMin == 0) { yMin = fromTable->ySize-1; }
+
+  do  //RPM axis
+  {
+    if(X >= fromTable->axisX[xMin]) {break;}
+    xMin--;
+  }while(1);
+  
+  do  //MAP axis
+  {
+    if(Y <= fromTable->axisY[yMin]) {break;}
+    yMin--;
+  }while(1);
+  
+  xMax = xMin + 1;
+  yMax = yMin + 1;
+  if (xMax > fromTable->xSize-1)  //Overflow protection
+  {
+    xMax = fromTable->xSize-1;
+    xMin = xMax - 1;
+  }
+  fromTable->lastXMin = xMin + 1;
+
+  if (yMax > fromTable->ySize-1)  //Overflow protection
+  {
+    yMax = fromTable->ySize-1;
+    yMin = yMax - 1;
+  }
+  fromTable->lastYMin = yMin + 1;
+
+  int A = fromTable->values[yMin][xMin];
+  int D = fromTable->values[yMax][xMax];
+  if(A == D) { tableResult = A; }
+  else
+  {
+    int B = fromTable->values[yMin][xMax];
+    int C = fromTable->values[yMax][xMin];
+
+    yMaxValue = fromTable->axisY[yMax];
+    yMinValue = fromTable->axisY[yMin];
+    xMaxValue = fromTable->axisX[xMax];
+    xMinValue = fromTable->axisX[xMin]; 
+
+    p = ((long)(X - xMinValue) << 8) / (xMaxValue - xMinValue); //(RPM - RPM[1])/(RPM[2]- RPM[1])
+    q = 256 - (((long)(Y - yMaxValue) << 8) / (yMinValue - yMaxValue)); //(MAP - MAP[2])/(MAP[2]- MAP[1])
+
+    int m = ((256-p) * (256-q)) >> 8;
+    int n = (p * (256-q)) >> 8;
+    int o = ((256-p) * q) >> 8;
+    int r = (p * q) >> 8;
+    tableResult = ( (A * m) + (B * n) + (C * o) + (D * r) ) >> 8; 
+  }
+  return tableResult;  
+}
+*/
diff --git a/speeduino/timers.ino b/speeduino/timers.ino
index 198cad59..35455d1c 100644
--- a/speeduino/timers.ino
+++ b/speeduino/timers.ino
@@ -32,7 +32,8 @@ void initialiseTimers()
    TCCR2B &= ~(1<<CS21);             // Clear bit
 
    //Enable the watchdog timer for 2 second resets (Good reference: https://tushev.org/articles/arduino/5/arduino-and-watchdog-timer)
-   //wdt_enable(WDTO_2S); //Boooooooooo WDT is currently broken on Mega 2560 bootloaders :(
+   //Boooooooooo WDT is currently broken on Mega 2560 bootloaders :(
+   //wdt_enable(WDTO_2S);
 
 #elif defined (CORE_TEENSY)
    //Uses the PIT timer on Teensy.
@@ -41,8 +42,9 @@ void initialiseTimers()
 #elif defined(CORE_STM32)
   Timer4.setPeriod(1000);  // Set up period
   // Set up an interrupt
-  Timer4.setMode(TIMER_CH1, TIMER_OUTPUT_COMPARE);
+  Timer4.setMode(1, TIMER_OUTPUT_COMPARE);
   Timer4.attachInterrupt(1, oneMSInterval);
+  Timer4.resume();
 #endif
 
   dwellLimit_uS = (1000 * configPage2.dwellLimit);
@@ -70,11 +72,11 @@ void oneMSInterval() //Most ARM chips can simply call a function
   targetOverdwellTime = micros() - dwellLimit_uS; //Set a target time in the past that all coil charging must have begun after. If the coil charge began before this time, it's been running too long
   //Check first whether each spark output is currently on. Only check it's dwell time if it is
 
-  if(ignitionSchedule1.Status == RUNNING) { if(ignitionSchedule1.startTime < targetOverdwellTime && configPage2.useDwellLim) { endCoil1Charge(); } }
-  if(ignitionSchedule2.Status == RUNNING) { if(ignitionSchedule2.startTime < targetOverdwellTime && configPage2.useDwellLim) { endCoil2Charge(); } }
-  if(ignitionSchedule3.Status == RUNNING) { if(ignitionSchedule3.startTime < targetOverdwellTime && configPage2.useDwellLim) { endCoil3Charge(); } }
-  if(ignitionSchedule4.Status == RUNNING) { if(ignitionSchedule4.startTime < targetOverdwellTime && configPage2.useDwellLim) { endCoil4Charge(); } }
-  if(ignitionSchedule5.Status == RUNNING) { if(ignitionSchedule5.startTime < targetOverdwellTime && configPage2.useDwellLim) { endCoil5Charge(); } }
+  if(ignitionSchedule1.Status == RUNNING) { if( (ignitionSchedule1.startTime < targetOverdwellTime) && (configPage2.useDwellLim) ) { endCoil1Charge(); } }
+  if(ignitionSchedule2.Status == RUNNING) { if( (ignitionSchedule2.startTime < targetOverdwellTime) && (configPage2.useDwellLim) ) { endCoil2Charge(); } }
+  if(ignitionSchedule3.Status == RUNNING) { if( (ignitionSchedule3.startTime < targetOverdwellTime) && (configPage2.useDwellLim) ) { endCoil3Charge(); } }
+  if(ignitionSchedule4.Status == RUNNING) { if( (ignitionSchedule4.startTime < targetOverdwellTime) && (configPage2.useDwellLim) ) { endCoil4Charge(); } }
+  if(ignitionSchedule5.Status == RUNNING) { if( (ignitionSchedule5.startTime < targetOverdwellTime) && (configPage2.useDwellLim) ) { endCoil5Charge(); } }
 
   //Loop executed every 100ms loop
   //Anything inside this if statement will run every 100ms.
@@ -92,7 +94,8 @@ void oneMSInterval() //Most ARM chips can simply call a function
   {
     loop250ms = 0; //Reset Counter.
     #if defined(CORE_AVR)
-      //wdt_reset(); //Reset watchdog timer
+      //Reset watchdog timer (Not active currently)
+      //wdt_reset();
     #endif
   }
 
@@ -137,7 +140,7 @@ void oneMSInterval() //Most ARM chips can simply call a function
     }
     //**************************************************************************************************************************************************
     //Set the flex reading (if enabled). The flexCounter is updated with every pulse from the sensor. If cleared once per second, we get a frequency reading
-    if(configPage1.flexEnabled)
+    if(configPage1.flexEnabled == true)
     {
       if(flexCounter < 50)
       {
diff --git a/speeduino/updates.ino b/speeduino/updates.ino
index ef92caeb..b8044392 100644
--- a/speeduino/updates.ino
+++ b/speeduino/updates.ino
@@ -10,45 +10,25 @@ void doUpdates()
 {
   #define CURRENT_DATA_VERSION    3
 
-  #if defined(CORE_AVR) | defined(CORE_TEENSY)
-    //May 2017 firmware introduced a -40 offset on the ignition table. Update that table to +40
-    if(EEPROM.read(EEPROM_DATA_VERSION) == 2)
+  //May 2017 firmware introduced a -40 offset on the ignition table. Update that table to +40
+  if(EEPROM.read(EEPROM_DATA_VERSION) == 2)
+  {
+    for(int x=0; x<16; x++)
     {
-      for(int x=0; x<16; x++)
+      for(int y=0; y<16; y++)
       {
-        for(int y=0; y<16; y++)
-        {
-          ignitionTable.values[x][y] = ignitionTable.values[x][y] + 40;
-        }
+        ignitionTable.values[x][y] = ignitionTable.values[x][y] + 40;
       }
-      writeConfig();
-      EEPROM.write(EEPROM_DATA_VERSION, 3);
     }
+    writeConfig();
+    EEPROM.write(EEPROM_DATA_VERSION, 3);
+  }
 
-    //Final check is always for 255 and 0 (Brand new arduino)
-    if(EEPROM.read(EEPROM_DATA_VERSION) == 0 || EEPROM.read(EEPROM_DATA_VERSION) == 255)
-    {
-      EEPROM.write(EEPROM_DATA_VERSION, CURRENT_DATA_VERSION);
-    }
-  #elif defined(CORE_STM)
-    //May 2017 firmware introduced a -40 offset on the ignition table. Update that table to +40
-    if(EEPROM_read(EEPROM_DATA_VERSION) == 2)
-    {
-      for(int x=0; x<16; x++)
-      {
-        for(int y=0; y<16; y++)
-        {
-          ignitionTable.values[x][y] = ignitionTable.values[x][y] + 40;
-        }
-      }
-      writeConfig_STM();
-      EEPROM_write(EEPROM_DATA_VERSION, 3);
-    }
+  //Final check is always for 255 and 0 (Brand new arduino)
+  if( (EEPROM.read(EEPROM_DATA_VERSION) == 0) || (EEPROM.read(EEPROM_DATA_VERSION) == 255) )
+  {
+    configPage10.true_address = 0x200;
+    EEPROM.write(EEPROM_DATA_VERSION, CURRENT_DATA_VERSION);
 
-    //Final check is always for 255 and 0 (Brand new arduino)
-    if(EEPROM_read(EEPROM_DATA_VERSION) == 0 || EEPROM_read(EEPROM_DATA_VERSION) == 255)
-    {
-      EEPROM_write(EEPROM_DATA_VERSION, CURRENT_DATA_VERSION);
-    }
-  #endif
-}
+  }
+}
diff --git a/speeduino/utils.h b/speeduino/utils.h
index 308a51c1..b258be60 100644
--- a/speeduino/utils.h
+++ b/speeduino/utils.h
@@ -6,10 +6,10 @@ These are some utility functions and variables used through the main code
 
 #include <Arduino.h>
 
-int freeRam ();
+unsigned int freeRam ();
 void setPinMapping(byte boardID);
 unsigned int PW();
 unsigned int PW_SD();
 unsigned int PW_AN();
 
-#endif // UTILS_H
+#endif // UTILS_H
diff --git a/speeduino/utils.ino b/speeduino/utils.ino
index e4894526..5ffb43d8 100644
--- a/speeduino/utils.ino
+++ b/speeduino/utils.ino
@@ -10,7 +10,7 @@
 */
 #include "utils.h"
 
-int freeRam ()
+unsigned int freeRam ()
 {
 #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
   extern int __heap_start, *__brkval;
@@ -41,31 +41,41 @@ void setPinMapping(byte boardID)
   //This is dumb, but it'll do for now to get things compiling
   #if defined(CORE_STM32)
     //STM32F1/variants/.../board.cpp
-    #define A0  boardADCPins[0]
-    #define A1  boardADCPins[1]
-    #define A2  boardADCPins[2]
-    #define A3  boardADCPins[3]
-    #define A4  boardADCPins[4]
-    #define A5  boardADCPins[5]
-    #define A6  boardADCPins[6]
-    #define A7  boardADCPins[7]
-    #define A8  boardADCPins[8]
-    #define A9  boardADCPins[9]
-    #if defined (ARDUINO_ARCH_STM32F4)
-      #define A10  boardADCPins[10]
-      #define A11  boardADCPins[11]
-      #define A12  boardADCPins[12]
-      #define A13  boardADCPins[13]
-      #define A14  boardADCPins[14]
-      #define A15  boardADCPins[15]
+    #if defined (STM32F4)
+      #define A0  PA0
+      #define A1  PA1
+      #define A2  PA2
+      #define A3  PA3
+      #define A4  PA4
+      #define A5  PA5
+      #define A6  PA6
+      #define A7  PA7
+      #define A8  PB0
+      #define A9  PB1
+      #define A10  PC0
+      #define A11  PC1
+      #define A12  PC2
+      #define A13  PC3
+      #define A14  PC4
+      #define A15  PC5
     #else
+      #define A0  PB0
+      #define A1  PA7
+      #define A2  PA6
+      #define A3  PA5
+      #define A4  PA4
+      #define A5  PA3
+      #define A6  PA2
+      #define A7  PA1
+      #define A8  PA0
       //STM32F1 have only 9 12bit adc
-      #define A10  boardADCPins[0]
-      #define A11  boardADCPins[1]
-      #define A12  boardADCPins[2]
-      #define A13  boardADCPins[3]
-      #define A14  boardADCPins[4]
-      #define A15  boardADCPins[5]
+      #define A9  PB0
+      #define A10  PA7
+      #define A11  PA6
+      #define A12  PA5
+      #define A13  PA4
+      #define A14  PA3
+      #define A15  PA2
     #endif
   #endif
 
@@ -166,6 +176,7 @@ void setPinMapping(byte boardID)
       #if defined(CORE_TEENSY)
         pinTrigger = 23;
         pinStepperDir = 33;
+        pinStepperStep = 34;
         pinCoil1 = 31;
         pinTachOut = 28;
         pinFan = 27;
@@ -194,8 +205,8 @@ void setPinMapping(byte boardID)
         pinStepperStep = 13; //Step pin for DRV8825 driver
         pinStepperEnable = 14; //Enable pin for DRV8825
         pinDisplayReset = 2; // OLED reset pin
-        pinFan = 0; //Pin for the fan output
-        pinFuelPump = 1; //Fuel pump output
+        pinFan = 1; //Pin for the fan output
+        pinFuelPump = 0; //Fuel pump output
         pinTachOut = 31; //Tacho output pin
         //external interrupt enabled pins
         pinFlex = 32; // Flex sensor (Must be external interrupt enabled)
@@ -242,12 +253,41 @@ void setPinMapping(byte boardID)
         pinTrigger = 23;
         pinTrigger2 = 35;
         pinStepperDir = 33;
+        pinStepperStep = 34;
         pinCoil1 = 31;
         pinTachOut = 28;
         pinFan = 27;
         pinCoil4 = 29;
         pinCoil3 = 30;
         pinO2 = A22;
+      #elif defined(STM32F4)
+        pinInjector1 = PE11; //Output pin injector 1 is on
+        pinInjector2 = PE12; //Output pin injector 2 is on
+        pinInjector3 = PE13; //Output pin injector 3 is on
+        pinInjector4 = PE14; //Output pin injector 4 is on
+        pinInjector5 = PE15; //Output pin injector 4 is on
+        pinCoil1 = PB10; //Pin for coil 1
+        pinCoil2 = PB11; //Pin for coil 2
+        pinCoil3 = PB12; //Pin for coil 3
+        pinCoil4 = PB13; //Pin for coil 4
+        pinCoil5 = PB14; //Pin for coil 5
+        pinTPS = PA0; //TPS input pin
+        pinMAP = PA1; //MAP sensor pin
+        pinIAT = PA2; //IAT sensor pin
+        pinCLT = PA3; //CLS sensor pin
+        pinO2 = PA4; //O2 Sensor pin
+        pinBat = PA5; //Battery reference voltage pin
+        pinStepperDir = PD8; //Direction pin  for DRV8825 driver
+        pinStepperStep = PB15; //Step pin for DRV8825 driver
+        pinStepperEnable = PD9; //Enable pin for DRV8825
+        pinDisplayReset = PE1; // OLED reset pin
+        pinFan = PE2; //Pin for the fan output
+        pinFuelPump = PA6; //Fuel pump output
+        pinTachOut = PA7; //Tacho output pin
+        //external interrupt enabled pins
+        pinFlex = PC4; // Flex sensor (Must be external interrupt enabled)
+        pinTrigger = PC5; //The CAS pin
+        pinTrigger2 = PC6; //The Cam Sensor pin
       #elif defined(CORE_STM32)
         //http://docs.leaflabs.com/static.leaflabs.com/pub/leaflabs/maple-docs/0.0.12/hardware/maple-mini.html#master-pin-map
         //pins 23, 24 and 33 couldn't be used
@@ -270,8 +310,8 @@ void setPinMapping(byte boardID)
         pinStepperStep = 13; //Step pin for DRV8825 driver
         pinStepperEnable = 14; //Enable pin for DRV8825
         pinDisplayReset = 2; // OLED reset pin
-        pinFan = 0; //Pin for the fan output
-        pinFuelPump = 1; //Fuel pump output
+        pinFan = 1; //Pin for the fan output
+        pinFuelPump = 0; //Fuel pump output
         pinTachOut = 31; //Tacho output pin
         //external interrupt enabled pins
         pinFlex = 32; // Flex sensor (Must be external interrupt enabled)
@@ -286,7 +326,7 @@ void setPinMapping(byte boardID)
       pinInjector2 = 10; //Output pin injector 2 is on
       pinInjector3 = 9; //Output pin injector 3 is on
       pinInjector4 = 8; //Output pin injector 4 is on
-      pinInjector5 = 12; //Output pin injector 5 is on
+      pinInjector5 = 14; //Output pin injector 5 is on
       pinCoil1 = 39; //Pin for coil 1
       pinCoil2 = 41; //Pin for coil 2
       pinCoil3 = 35; //Pin for coil 3
@@ -508,23 +548,13 @@ void setPinMapping(byte boardID)
   tach_pin_mask = digitalPinToBitMask(pinTachOut);
 
   //And for inputs
-  #if defined(CORE_STM32)
-    pinMode(pinMAP, INPUT_ANALOG);
-    pinMode(pinO2, INPUT_ANALOG);
-    pinMode(pinO2_2, INPUT_ANALOG);
-    pinMode(pinTPS, INPUT_ANALOG);
-    pinMode(pinIAT, INPUT_ANALOG);
-    pinMode(pinCLT, INPUT_ANALOG);
-    pinMode(pinBat, INPUT_ANALOG);
-  #else
-    pinMode(pinMAP, INPUT);
-    pinMode(pinO2, INPUT);
-    pinMode(pinO2_2, INPUT);
-    pinMode(pinTPS, INPUT);
-    pinMode(pinIAT, INPUT);
-    pinMode(pinCLT, INPUT);
-    pinMode(pinBat, INPUT);
-  #endif
+  pinMode(pinMAP, INPUT);
+  pinMode(pinO2, INPUT);
+  pinMode(pinO2_2, INPUT);
+  pinMode(pinTPS, INPUT);
+  pinMode(pinIAT, INPUT);
+  pinMode(pinCLT, INPUT);
+  pinMode(pinBat, INPUT);
   pinMode(pinTrigger, INPUT);
   pinMode(pinTrigger2, INPUT);
   pinMode(pinTrigger3, INPUT);

From d9f38f82de23086ee19fe9b89f9a612ff755f972 Mon Sep 17 00:00:00 2001
From: "Vitor Moreno B. Sales" <vitor_boss@yahoo.com.br>
Date: Wed, 2 Aug 2017 22:51:07 -0300
Subject: [PATCH 03/10] Fixing last comit

---
 speeduino/auxiliaries.h   |  33 +++
 speeduino/auxiliaries.ino |  92 ++++++---
 speeduino/cancomms.h      |   2 +-
 speeduino/cancomms.ino    |  71 +++----
 speeduino/comms.h         |   5 +-
 speeduino/comms.ino       | 167 ++++++++-------
 speeduino/corrections.ino |  16 +-
 speeduino/decoders.h      |   7 +
 speeduino/decoders.ino    | 181 +++++++++++++++-
 speeduino/globals.h       | 157 +++++++++-----
 speeduino/maths.h         |   4 +-
 speeduino/maths.ino       |  30 ++-
 speeduino/scheduler.h     |  96 +++++----
 speeduino/scheduler.ino   | 420 ++++++++++++++++++++++++--------------
 speeduino/sensors.h       |   6 +-
 speeduino/sensors.ino     |  57 ++++--
 speeduino/speeduino.ino   | 230 ++++++++++++---------
 speeduino/storage.h       |   7 +-
 speeduino/storage.ino     |  23 ++-
 speeduino/table.h         |   3 +-
 speeduino/table.ino       | 216 ++++++++++++--------
 speeduino/timers.h        |   6 +-
 speeduino/timers.ino      |  24 ++-
 speeduino/updates.ino     |  15 +-
 speeduino/utils.h         |  44 +++-
 speeduino/utils.ino       | 162 +++++----------
 26 files changed, 1333 insertions(+), 741 deletions(-)

diff --git a/speeduino/auxiliaries.h b/speeduino/auxiliaries.h
index 8d535704..5ce8d246 100644
--- a/speeduino/auxiliaries.h
+++ b/speeduino/auxiliaries.h
@@ -6,6 +6,39 @@ void boostControl();
 void vvtControl();
 void initialiseFan();
 
+#if defined(CORE_AVR)
+  #define ENABLE_BOOST_TIMER()  TIMSK1 |= (1 << OCIE1A)
+  #define DISABLE_BOOST_TIMER() TIMSK1 &= ~(1 << OCIE1A)
+  #define ENABLE_VVT_TIMER()    TIMSK1 |= (1 << OCIE1B)
+  #define DISABLE_VVT_TIMER()   TIMSK1 &= ~(1 << OCIE1B)
+
+  #define BOOST_TIMER_COMPARE   OCR1A
+  #define BOOST_TIMER_COUNTER   TCNT1
+  #define VVT_TIMER_COMPARE     OCR1B
+  #define VVT_TIMER_COUNTER     TCNT1
+
+#elif defined(CORE_TEENSY)
+  #define ENABLE_BOOST_TIMER()  FTM1_C0SC |= FTM_CSC_CHIE
+  #define DISABLE_BOOST_TIMER() FTM1_C0SC &= ~FTM_CSC_CHIE
+
+  #define ENABLE_VVT_TIMER()    FTM1_C1SC |= FTM_CSC_CHIE
+  #define DISABLE_VVT_TIMER()   FTM1_C1SC &= ~FTM_CSC_CHIE
+
+  #define BOOST_TIMER_COMPARE   FTM1_C0V
+  #define BOOST_TIMER_COUNTER   FTM1_CNT
+  #define VVT_TIMER_COMPARE     FTM1_C1V
+  #define VVT_TIMER_COUNTER     FTM1_CNT
+
+#elif defined(CORE_STM32)
+
+#endif
+
+#define BOOST_PIN_LOW()  *boost_pin_port &= ~(boost_pin_mask)
+#define BOOST_PIN_HIGH() *boost_pin_port |= (boost_pin_mask)
+#define VVT_PIN_LOW()    *vvt_pin_port &= ~(vvt_pin_mask)
+#define VVT_PIN_HIGH()   *vvt_pin_port |= (vvt_pin_mask)
+
+
 volatile byte *boost_pin_port;
 volatile byte boost_pin_mask;
 volatile byte *vvt_pin_port;
diff --git a/speeduino/auxiliaries.ino b/speeduino/auxiliaries.ino
index 45438685..88805e40 100644
--- a/speeduino/auxiliaries.ino
+++ b/speeduino/auxiliaries.ino
@@ -3,7 +3,8 @@ Speeduino - Simple engine management for the Arduino Mega 2560 platform
 Copyright (C) Josh Stewart
 A full copy of the license may be found in the projects root directory
 */
-integerPID boostPID(&MAPx100, &boost_pwm_target_value, &boostTargetx100, configPage3.boostKP, configPage3.boostKI, configPage3.boostKD, DIRECT); //This is the PID object if that algorithm is used. Needs to be global as it maintains state outside of each function call
+//integerPID boostPID(&MAPx100, &boost_pwm_target_value, &boostTargetx100, configPage3.boostKP, configPage3.boostKI, configPage3.boostKD, DIRECT); //This is the PID object if that algorithm is used. Needs to be global as it maintains state outside of each function call
+integerPIDnew boostPID(&currentStatus.MAP, &boost_pwm_target_value, &boost_cl_target_boost, configPage3.boostKP, configPage3.boostKI, configPage3.boostKD, DIRECT); //This is the PID object if that algorithm is used. Needs to be global as it maintains state outside of each function call
 
 /*
 Fan control
@@ -28,14 +29,18 @@ void fanControl()
   }
 }
 
-#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+#if defined(CORE_AVR) || defined(CORE_TEENSY)
 void initialiseAuxPWM()
 {
-  TCCR1B = 0x00;          //Disbale Timer1 while we set it up
-  TCNT1  = 0;             //Reset Timer Count
-  TIFR1  = 0x00;          //Timer1 INT Flag Reg: Clear Timer Overflow Flag
-  TCCR1A = 0x00;          //Timer1 Control Reg A: Wave Gen Mode normal (Simply counts up from 0 to 65535 (16-bit int)
-  TCCR1B = (1 << CS12);   //Timer1 Control Reg B: Timer Prescaler set to 256. 1 tick = 16uS. Refer to http://www.instructables.com/files/orig/F3T/TIKL/H3WSA4V7/F3TTIKLH3WSA4V7.jpg
+  #if defined(CORE_AVR)
+    TCCR1B = 0x00;          //Disbale Timer1 while we set it up
+    TCNT1  = 0;             //Reset Timer Count
+    TIFR1  = 0x00;          //Timer1 INT Flag Reg: Clear Timer Overflow Flag
+    TCCR1A = 0x00;          //Timer1 Control Reg A: Wave Gen Mode normal (Simply counts up from 0 to 65535 (16-bit int)
+    TCCR1B = (1 << CS12);   //Timer1 Control Reg B: Timer Prescaler set to 256. 1 tick = 16uS. Refer to http://www.instructables.com/files/orig/F3T/TIKL/H3WSA4V7/F3TTIKLH3WSA4V7.jpg
+  #elif defined(CORE_TEENSY)
+    //REALLY NEED TO DO THIS!
+  #endif
 
   boost_pin_port = portOutputRegister(digitalPinToPort(pinBoost));
   boost_pin_mask = digitalPinToBitMask(pinBoost);
@@ -45,7 +50,7 @@ void initialiseAuxPWM()
   boost_pwm_max_count = 1000000L / (16 * configPage3.boostFreq * 2); //Converts the frequency in Hz to the number of ticks (at 16uS) it takes to complete 1 cycle. The x2 is there because the frequency is stored at half value (in a byte) to allow freqneucies up to 511Hz
   vvt_pwm_max_count = 1000000L / (16 * configPage3.vvtFreq * 2); //Converts the frequency in Hz to the number of ticks (at 16uS) it takes to complete 1 cycle
   //TIMSK1 |= (1 << OCIE1A); <---- Not required as compare A is turned on when needed by boost control
-  TIMSK1 |= (1 << OCIE1B); //Turn on the B compare unit (ie turn on the interrupt)
+  ENABLE_VVT_TIMER(); //Turn on the B compare unit (ie turn on the interrupt)
 
   boostPID.SetOutputLimits(percentage(configPage1.boostMinDuty, boost_pwm_max_count) , percentage(configPage1.boostMaxDuty, boost_pwm_max_count));
   boostPID.SetTunings(configPage3.boostKP, configPage3.boostKI, configPage3.boostKD);
@@ -63,7 +68,8 @@ void boostControl()
     {
       MAPx100 = currentStatus.MAP * 100;
 
-      boost_cl_target_boost = get3DTableValue(&boostTable, currentStatus.TPS, currentStatus.RPM) * 2; //Boost target table is in kpa and divided by 2
+      if( (boostCounter & 3) == 1) { boost_cl_target_boost = get3DTableValue(&boostTable, currentStatus.TPS, currentStatus.RPM) * 2; } //Boost target table is in kpa and divided by 2
+
       //If flex fuel is enabled, there can be an adder to the boost target based on ethanol content
       if( configPage1.flexEnabled == 1 )
       {
@@ -80,23 +86,25 @@ void boostControl()
 
         boostPID.Compute();
         currentStatus.boostDuty = (unsigned long)(boost_pwm_target_value * 100UL) / boost_pwm_max_count;
-        TIMSK1 |= (1 << OCIE1A); //Turn on the compare unit (ie turn on the interrupt)
+        if(currentStatus.boostDuty == 0) { DISABLE_BOOST_TIMER(); BOOST_PIN_LOW(); } //If boost duty is 0, shut everything down
+        else { ENABLE_BOOST_TIMER(); } //Turn on the compare unit (ie turn on the interrupt) if boost duty >0
+
       }
       else
       {
         //If boost target is 0, turn everything off
-        TIMSK1 &= ~(1 << OCIE1A); //Turn off timer
-        digitalWrite(pinBoost, LOW);
+        DISABLE_BOOST_TIMER(); //Turn off timer
+        BOOST_PIN_LOW();
       }
     }
     else
     {
       //Boost control does nothing if kPa below 100
-      TIMSK1 &= ~(1 << OCIE1A); //Turn off timer
-      digitalWrite(pinBoost, LOW); //Make sure solenoid is off (0% duty)
+      DISABLE_BOOST_TIMER(); //Turn off timer
+      BOOST_PIN_LOW(); //Make sure solenoid is off (0% duty)
     }
   }
-  else { TIMSK1 &= ~(1 << OCIE1A); } // Disable timer channel
+  else { DISABLE_BOOST_TIMER(); } // Disable timer channel
 
   boostCounter++;
 }
@@ -106,51 +114,73 @@ void vvtControl()
   if( configPage3.vvtEnabled == 1 )
   {
     byte vvtDuty = get3DTableValue(&vvtTable, currentStatus.TPS, currentStatus.RPM);
-    vvt_pwm_target_value = percentage(vvtDuty, vvt_pwm_max_count);
+    if(vvtDuty == 0)
+    {
+      //Make sure solenoid is off (0% duty)
+      VVT_PIN_LOW();
+      DISABLE_VVT_TIMER();
+    }
+    else if (vvtDuty >= 100)
+    {
+      //Make sure solenoid is on (100% duty)
+      VVT_PIN_HIGH();
+      DISABLE_VVT_TIMER();
+    }
+    else
+    {
+      vvt_pwm_target_value = percentage(vvtDuty, vvt_pwm_max_count);
+      ENABLE_VVT_TIMER();
+    }
   }
-#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
-  else { TIMSK1 &= ~(1 << OCIE1B); } // Disable timer channel
-#endif
+  else { DISABLE_VVT_TIMER(); } // Disable timer channel
 }
 
 //The interrupt to control the Boost PWM
-ISR(TIMER1_COMPA_vect)
+#if defined(CORE_AVR)
+  ISR(TIMER1_COMPA_vect)
+#elif defined (CORE_TEENSY)
+  static inline void boostInterrupt() //Most ARM chips can simply call a function
+#endif
 {
   if (boost_pwm_state)
   {
-    *boost_pin_port &= ~(boost_pin_mask);  // Switch pin to low
-    OCR1A = TCNT1 + (boost_pwm_max_count - boost_pwm_cur_value);
+    BOOST_PIN_LOW();  // Switch pin to low
+    BOOST_TIMER_COMPARE = BOOST_TIMER_COUNTER + (boost_pwm_max_count - boost_pwm_cur_value);
     boost_pwm_state = false;
   }
   else
   {
-    *boost_pin_port |= (boost_pin_mask);  // Switch pin high
-    OCR1A = TCNT1 + boost_pwm_target_value;
+    BOOST_PIN_HIGH();  // Switch pin high
+    BOOST_TIMER_COMPARE = BOOST_TIMER_COUNTER + boost_pwm_target_value;
     boost_pwm_cur_value = boost_pwm_target_value;
     boost_pwm_state = true;
   }
 }
 
 //The interrupt to control the VVT PWM
-ISR(TIMER1_COMPB_vect)
+#if defined(CORE_AVR)
+  ISR(TIMER1_COMPB_vect)
+#elif defined (CORE_TEENSY)
+  static inline void vvtInterrupt() //Most ARM chips can simply call a function
+#endif
 {
   if (vvt_pwm_state)
   {
-    *vvt_pin_port &= ~(vvt_pin_mask);  // Switch pin to low
-    OCR1B = TCNT1 + (vvt_pwm_max_count - vvt_pwm_cur_value);
+    VVT_PIN_LOW();  // Switch pin to low
+    VVT_TIMER_COMPARE = VVT_TIMER_COUNTER + (vvt_pwm_max_count - vvt_pwm_cur_value);
     vvt_pwm_state = false;
   }
   else
   {
-    *vvt_pin_port |= (vvt_pin_mask);  // Switch pin high
-    OCR1B = TCNT1 + vvt_pwm_target_value;
+    VVT_PIN_HIGH();  // Switch pin high
+    VVT_TIMER_COMPARE = VVT_TIMER_COUNTER + vvt_pwm_target_value;
     vvt_pwm_cur_value = vvt_pwm_target_value;
     vvt_pwm_state = true;
   }
 }
 
-#elif defined (CORE_TEENSY) || defined(CORE_STM32)
-//YET TO BE IMPLEMENTED ON TEENSY
+#elif defined(CORE_STM32)
+//YET TO BE IMPLEMENTED ON STM32
 void initialiseAuxPWM() { }
 void boostControl() { }
 void vvtControl() { }
diff --git a/speeduino/cancomms.h b/speeduino/cancomms.h
index 0d31a4b5..38104469 100644
--- a/speeduino/cancomms.h
+++ b/speeduino/cancomms.h
@@ -15,8 +15,8 @@ uint8_t Glow, Ghigh;
 #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
   HardwareSerial &CANSerial = Serial3;
 #elif defined(CORE_STM32)
-  SerialUART &CANSerial = Serial2;
   //HardwareSerial &CANSerial = Serial2;
+  SerialUART &CANSerial = Serial2;
 #elif defined(CORE_TEENSY)
   HardwareSerial &CANSerial = Serial2;
 #endif
diff --git a/speeduino/cancomms.ino b/speeduino/cancomms.ino
index cac44fdc..f686c1ec 100644
--- a/speeduino/cancomms.ino
+++ b/speeduino/cancomms.ino
@@ -18,41 +18,41 @@ void canCommand()
   switch (currentcanCommand)
   {
     case 'A': // sends the bytes of realtime values
-        sendValues(0, packetSize,3); //send values to serial3
+        sendValues(0, packetSize,0x30,3); //send values to serial3
         break;
 
     case 'G': // this is the reply command sent by the Can interface
-        //uint8_t Gdata;
-        while (CANSerial.available() == 0) { }
+       byte destcaninchannel;
+      if (CANSerial.available() >= 10)
+      {
         cancmdfail = CANSerial.read();
-
+        destcaninchannel = CANSerial.read();
         if (cancmdfail != 0)
-        {
-          for (byte Gx = 0; Gx < 8; Gx++) //read all 8 bytes of data
-            {
-              while (CANSerial.available() == 0) { }
-              Gdata[Gx] = CANSerial.read();
-            }
-
-          Glow = Gdata[(configPage10.caninput_param_start_byte[currentStatus.current_caninchannel])];
-          if (configPage10.caninput_param_num_bytes[currentStatus.current_caninchannel] == 2)
-             {
-              if ((configPage10.caninput_param_start_byte[currentStatus.current_caninchannel]) != 7)   //you cant have a 2 byte value starting at byte 7(8 on the list)
-                 {
-                  Ghigh = Gdata[((configPage10.caninput_param_start_byte[currentStatus.current_caninchannel])+1)];
-                 }
-             }
+           {                                 // read all 8 bytes of data.
+            for (byte Gx = 0; Gx < 8; Gx++) // first two are the can address the data is from. next two are the can address the data is for.then next 1 or two bytes of data
+              {
+                Gdata[Gx] = CANSerial.read();
+              }
+            Glow = Gdata[(configPage10.caninput_param_start_byte[destcaninchannel]&7)];
+            if ((BIT_CHECK(configPage10.caninput_param_num_bytes,destcaninchannel)))  //if true then num bytes is 2
+               {
+                if ((configPage10.caninput_param_start_byte[destcaninchannel]&7) < 8)   //you cant have a 2 byte value starting at byte 7(8 on the list)
+                   {
+                    Ghigh = Gdata[((configPage10.caninput_param_start_byte[destcaninchannel]&7)+1)];
+                   }
+            else{Ghigh = 0;}
+               }
           else
-             {
-              Ghigh = 0;
-             }
+               {
+                 Ghigh = 0;
+               }
 
-          currentStatus.canin[currentStatus.current_caninchannel] = word(Ghigh, Glow);
+          currentStatus.canin[destcaninchannel] = (Ghigh<<8) | Glow;
         }
 
         else{}  //continue as command request failed and/or data/device was not available
 
-        if (currentStatus.current_caninchannel <= 6)     // if channel is 0-7
+        if (currentStatus.current_caninchannel < 15)     // if channel is < 15 then we can do another one
            {
             currentStatus.current_caninchannel++;       //inc to next channel
            }
@@ -60,7 +60,7 @@ void canCommand()
            {
             currentStatus.current_caninchannel = 0;      //reset to start
            }
-
+      }
         break;
 
     case 'L':
@@ -86,23 +86,22 @@ void canCommand()
          break;
 
     case 'r': //New format for the optimised OutputChannels
-      byte cmd;
-
+      byte Cmd;
       if (CANSerial.available() >= 6)
       {
         CANSerial.read(); //Read the $tsCanId
-        cmd = CANSerial.read();
+        Cmd = CANSerial.read();
 
         uint16_t offset, length;
-        if(cmd == 0x30) //Send output channels command 0x30 is 48dec
+        if( (Cmd == 0x30) || ( (Cmd >= 0x40) && (Cmd <0x50) ) ) //Send output channels command 0x30 is 48dec, 0x40(64dec)-0x4F(79dec) are external can request
         {
           byte tmp;
           tmp = CANSerial.read();
           offset = word(CANSerial.read(), tmp);
           tmp = CANSerial.read();
           length = word(CANSerial.read(), tmp);
-
-          sendValues(offset, length, 3);
+          sendValues(offset, length,Cmd, 3);
+//Serial.print(Cmd);
         }
         else
         {
@@ -152,14 +151,16 @@ void sendCancommand(uint8_t cmdtype, uint16_t canaddress, uint8_t candata1, uint
         CANSerial.write(canaddress);  //11 bit canaddress of device to listen for
         break;
 
-     case 2:
-        CANSerial.print("R");                         //send "R" to request data from the parmagroup whos value is sent next
-        CANSerial.write( lowByte(paramgroup) );       //send lsb first
-        CANSerial.write( lowByte(paramgroup >> 8) );
+     case 2:                                          // requests via serial3
+        CANSerial.print("R");                         //send "R" to request data from the parmagroup can address whos value is sent next
+        CANSerial.write(candata1);                    //the currentStatus.current_caninchannel
+        CANSerial.write(lowByte(paramgroup) );       //send lsb first
+        CANSerial.write(highByte(paramgroup) );
         break;
 
      case 3:
         //send to truecan send routine
+        //canaddress == speeduino canid, candata1 == canin channel dest, paramgroup == can address  to request from
         break;
 
      default:
diff --git a/speeduino/comms.h b/speeduino/comms.h
index 81e1016e..8b191f42 100644
--- a/speeduino/comms.h
+++ b/speeduino/comms.h
@@ -11,8 +11,9 @@
 #define boostvvtPage 8
 #define seqFuelPage  9
 #define canbusPage   10//Config Page 10
+#define warmupPage   11 //Config Page 11
 
-#define packetSize   57//41
+#define packetSize   73
 
 byte currentPage = 1;//Not the same as the speeduino config page numbers
 bool isMap = true;
@@ -40,7 +41,7 @@ const char pageTitles[] PROGMEM //This is being stored in the avr flash instead
   };
 
 void command();//This is the heart of the Command Line Interpeter.  All that needed to be done was to make it human readable.
-void sendValues(uint16_t offset, uint16_t packetlength, byte portnum);
+void sendValues(uint16_t offset, uint16_t packetlength,byte cmd, byte portnum);
 void receiveValue(int offset, byte newValue);
 void saveConfig();
 void sendPage(bool useChar);
diff --git a/speeduino/comms.ino b/speeduino/comms.ino
index b5b0128c..04d82ed2 100644
--- a/speeduino/comms.ino
+++ b/speeduino/comms.ino
@@ -15,15 +15,16 @@ A detailed description of each call can be found at: http://www.msextra.com/doc/
 
 void command()
 {
-  if (cmdPending == 0) { currentCommand = Serial.read(); }
+  if (!cmdPending) { currentCommand = Serial.read(); }
 
   switch (currentCommand)
   {
 
     case 'A': // send x bytes of realtime values
-      sendValues(0, packetSize, 0);   //send values to serial0
+      sendValues(0, packetSize,0x30, 0);   //send values to serial0
       break;
-      
+
+
     case 'B': // Burn current values to eeprom
       writeConfig();
       break;
@@ -79,12 +80,12 @@ void command()
       break;
 
     case 'S': // send code version
-      Serial.print(displaySignature);Serial.print(".");Serial.print(TSfirmwareVersion);
+      Serial.print("Speeduino 2017.07-dev");
       currentStatus.secl = 0; //This is required in TS3 due to its stricter timings
       break;
 
     case 'Q': // send code version
-      Serial.print(displaySignature);Serial.print(TSfirmwareVersion);
+      Serial.print("speeduino 201707-dev");
      break;
 
     case 'V': // send VE table and constants in binary
@@ -181,15 +182,15 @@ void command()
         tsCanId = Serial.read(); //Read the $tsCanId
         cmd = Serial.read(); // read the command
 
-        uint16_t valueOffset, length;
+        uint16_t offset, length;
         if(cmd == 0x30) //Send output channels command 0x30 is 48dec
         {
           byte tmp;
           tmp = Serial.read();
-          valueOffset = word(Serial.read(), tmp);
+          offset = word(Serial.read(), tmp);
           tmp = Serial.read();
           length = word(Serial.read(), tmp);
-          sendValues(valueOffset, length, 0);
+          sendValues(offset, length,cmd, 0);
         }
         else
         {
@@ -243,33 +244,23 @@ void command()
 This function returns the current values of a fixed group of variables
 */
 //void sendValues(int packetlength, byte portNum)
-void sendValues(uint16_t offset, uint16_t packetLength, byte portNum)
+void sendValues(uint16_t offset, uint16_t packetLength, byte cmd, byte portNum)
 {
   byte fullStatus[packetSize];
 
   if (portNum == 3)
   {
     //CAN serial
-    #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) //ATmega2561 does not have Serial3
+    #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)|| defined(CORE_STM32) || defined (CORE_TEENSY) //ATmega2561 does not have Serial3
       if (offset == 0)
         {
-          Serial3.write("A");         //confirm cmd type
+          CANSerial.write("A");         //confirm cmd type
         }
       else
         {
-          Serial3.write("r");         //confirm cmd type
+      CANSerial.write("r");         //confirm cmd type
+      CANSerial.write(cmd);
         }
-      Serial3.write(packetLength);      //confirm no of byte to be sent
-    #elif defined(CORE_STM32) || defined (CORE_TEENSY)
-      if (offset == 0)
-        {
-          Serial2.write("A");         //confirm cmd type
-        }
-      else
-        {
-          Serial2.write("r");         //confirm cmd type
-        }
-      Serial2.write(packetLength);      //confirm no of byte to be sent
     #endif
   }
   else
@@ -284,19 +275,19 @@ void sendValues(uint16_t offset, uint16_t packetLength, byte portNum)
   fullStatus[1] = currentStatus.squirt; //Squirt Bitfield
   fullStatus[2] = currentStatus.engine; //Engine Status Bitfield
   fullStatus[3] = (byte)(divu100(currentStatus.dwell)); //Dwell in ms * 10
-  fullStatus[4] = (byte)(currentStatus.MAP >> 1); //map value is divided by 2
-  fullStatus[5] = (byte)(currentStatus.IAT + CALIBRATION_TEMPERATURE_OFFSET); //mat
-  fullStatus[6] = (byte)(currentStatus.coolant + CALIBRATION_TEMPERATURE_OFFSET); //Coolant ADC
-  fullStatus[7] = currentStatus.tpsADC; //TPS (Raw 0-255)
-  fullStatus[8] = currentStatus.battery10; //battery voltage
-  fullStatus[9] = currentStatus.O2; //O2
-  fullStatus[10] = currentStatus.egoCorrection; //Exhaust gas correction (%)
-  fullStatus[11] = currentStatus.iatCorrection; //Air temperature Correction (%)
-  fullStatus[12] = currentStatus.wueCorrection; //Warmup enrichment (%)
-  fullStatus[13] = lowByte(currentStatus.RPM); //rpm HB
-  fullStatus[14] = highByte(currentStatus.RPM); //rpm LB
-  fullStatus[15] = currentStatus.TAEamount; //acceleration enrichment (%)
-  fullStatus[16] = currentStatus.baro; //Barometer value
+  fullStatus[4] = lowByte(currentStatus.MAP); //2 bytes for MAP
+  fullStatus[5] = highByte(currentStatus.MAP);
+  fullStatus[6] = (byte)(currentStatus.IAT + CALIBRATION_TEMPERATURE_OFFSET); //mat
+  fullStatus[7] = (byte)(currentStatus.coolant + CALIBRATION_TEMPERATURE_OFFSET); //Coolant ADC
+  fullStatus[8] = currentStatus.batCorrection; //Battery voltage correction (%)
+  fullStatus[9] = currentStatus.battery10; //battery voltage
+  fullStatus[10] = currentStatus.O2; //O2
+  fullStatus[11] = currentStatus.egoCorrection; //Exhaust gas correction (%)
+  fullStatus[12] = currentStatus.iatCorrection; //Air temperature Correction (%)
+  fullStatus[13] = currentStatus.wueCorrection; //Warmup enrichment (%)
+  fullStatus[14] = lowByte(currentStatus.RPM); //rpm HB
+  fullStatus[15] = highByte(currentStatus.RPM); //rpm LB
+  fullStatus[16] = currentStatus.TAEamount; //acceleration enrichment (%)
   fullStatus[17] = currentStatus.corrections; //Total GammaE (%)
   fullStatus[18] = currentStatus.VE; //Current VE 1 (%)
   fullStatus[19] = currentStatus.afrTarget;
@@ -313,9 +304,9 @@ void sendValues(uint16_t offset, uint16_t packetLength, byte portNum)
   fullStatus[26] = lowByte(currentStatus.freeRAM); //(byte)((currentStatus.loopsPerSecond >> 8) & 0xFF);
   fullStatus[27] = highByte(currentStatus.freeRAM);
 
-  fullStatus[28] = currentStatus.batCorrection; //Battery voltage correction (%)
-  fullStatus[29] = currentStatus.spark; //Spark related bitfield
-  fullStatus[30] = currentStatus.O2_2; //O2
+  fullStatus[28] = currentStatus.boostTarget;
+  fullStatus[29] = currentStatus.boostDuty;
+  fullStatus[30] = currentStatus.spark; //Spark related bitfield
 
   //rpmDOT must be sent as a signed integer
   fullStatus[31] = lowByte(currentStatus.rpmDOT);
@@ -325,10 +316,13 @@ void sendValues(uint16_t offset, uint16_t packetLength, byte portNum)
   fullStatus[34] = currentStatus.flexCorrection; //Flex fuel correction (% above or below 100)
   fullStatus[35] = currentStatus.flexIgnCorrection; //Ignition correction (Increased degrees of advance) for flex fuel
   fullStatus[36] = getNextError();
-  fullStatus[37] = currentStatus.boostTarget;
-  fullStatus[38] = currentStatus.boostDuty;
-  fullStatus[39] = currentStatus.idleLoad;
-  fullStatus[40] = currentStatus.testOutputs;
+
+  fullStatus[37] = currentStatus.idleLoad;
+  fullStatus[38] = currentStatus.testOutputs;
+
+  fullStatus[39] = currentStatus.O2_2; //O2
+  fullStatus[40] = currentStatus.baro; //Barometer value
+
   fullStatus[41] = lowByte(currentStatus.canin[0]);
   fullStatus[42] = highByte(currentStatus.canin[0]);
   fullStatus[43] = lowByte(currentStatus.canin[1]);
@@ -345,13 +339,28 @@ void sendValues(uint16_t offset, uint16_t packetLength, byte portNum)
   fullStatus[54] = highByte(currentStatus.canin[6]);
   fullStatus[55] = lowByte(currentStatus.canin[7]);
   fullStatus[56] = highByte(currentStatus.canin[7]);
+  fullStatus[57] = lowByte(currentStatus.canin[8]);
+  fullStatus[58] = highByte(currentStatus.canin[8]);
+  fullStatus[59] = lowByte(currentStatus.canin[9]);
+  fullStatus[60] = highByte(currentStatus.canin[9]);
+  fullStatus[61] = lowByte(currentStatus.canin[10]);
+  fullStatus[62] = highByte(currentStatus.canin[10]);
+  fullStatus[63] = lowByte(currentStatus.canin[11]);
+  fullStatus[64] = highByte(currentStatus.canin[11]);
+  fullStatus[65] = lowByte(currentStatus.canin[12]);
+  fullStatus[66] = highByte(currentStatus.canin[12]);
+  fullStatus[67] = lowByte(currentStatus.canin[13]);
+  fullStatus[68] = highByte(currentStatus.canin[13]);
+  fullStatus[69] = lowByte(currentStatus.canin[14]);
+  fullStatus[70] = highByte(currentStatus.canin[14]);
+  fullStatus[71] = lowByte(currentStatus.canin[15]);
+  fullStatus[72] = highByte(currentStatus.canin[15]);
 
-  Serial.write((byte *)&fullStatus+offset, packetLength);
-  //for(byte x=0; x<packetLength; x++)
-  //{
-  //  if (portNum == 0) { Serial.write(fullStatus[offset+x]); }
-  //  else if (portNum == 3) { CANSerial.write(fullStatus[offset+x]); }
-  //}
+  for(byte x=0; x<packetLength; x++)
+  {
+    if (portNum == 0) { Serial.write(fullStatus[offset+x]); }
+    else if (portNum == 3){ CANSerial.write(fullStatus[offset+x]); }
+  }
 
 }
 
@@ -525,6 +534,15 @@ void receiveValue(int valueOffset, byte newValue)
       }
       break;
 
+    case warmupPage: //Idle Air Control settings page (Page 4)
+      pnt_configPage = &configPage11;
+      //For some reason, TunerStudio is sending offsets greater than the maximum page size. I'm not sure if it's their bug or mine, but the fix is to only update the config page if the offset is less than the maximum size
+      if (valueOffset < page_size)
+      {
+        *((byte *)pnt_configPage + (byte)valueOffset) = newValue;
+      }
+      break;
+
     default:
       break;
   }
@@ -538,22 +556,19 @@ useChar - If true, all values are send as chars, this is for the serial command
 */
 void sendPage(bool useChar)
 {
-  void* pnt_configPage;
-  struct table3D currentTable;
+  void* pnt_configPage = &configPage1; //Default value is for safety only. Will be changed below if needed.
+  struct table3D currentTable = fuelTable; //Default value is for safety only. Will be changed below if needed.
   byte currentTitleIndex = 0;// This corresponds to the count up to the first char of a string in pageTitles
   bool sendComplete = false; //Used to track whether all send operations are complete
 
   switch (currentPage)
   {
     case veMapPage:
-      {
         currentTitleIndex = 0;
         currentTable = fuelTable;
         break;
-      }
 
     case veSetPage:
-      {
         // currentTitleIndex = 27;
         if (useChar)
         {
@@ -585,17 +600,13 @@ void sendPage(bool useChar)
         }
         else { pnt_configPage = &configPage1; } //Create a pointer to Page 1 in memory
         break;
-      }
 
     case ignMapPage:
-      {
         currentTitleIndex = 42;// the index to the first char of the third string in pageTitles
         currentTable = ignitionTable;
         break;
-      }
 
     case ignSetPage:
-      {
         //currentTitleIndex = 56;
         if (useChar)
         {
@@ -642,17 +653,13 @@ void sendPage(bool useChar)
         }
         else { pnt_configPage = &configPage2; } //Create a pointer to Page 2 in memory
         break;
-      }
 
     case afrMapPage:
-      {
         currentTitleIndex = 71;//Array index to next string
         currentTable = afrTable;
         break;
-      }
 
     case afrSetPage:
-      {
         //currentTitleIndex = 91;
         if (useChar)
         {
@@ -697,10 +704,8 @@ void sendPage(bool useChar)
         }
         else { pnt_configPage = &configPage3; } //Create a pointer to Page 3 in memory
         break;
-      }
 
     case iacPage:
-      {
         //currentTitleIndex = 106;
         //To Display Values from Config Page 4
         if (useChar)
@@ -747,10 +752,8 @@ void sendPage(bool useChar)
         }
         else { pnt_configPage = &configPage4; } //Create a pointer to Page 4 in memory
         break;
-      }
 
     case boostvvtPage:
-      {
         if(useChar)
         {
           currentTable = boostTable;
@@ -773,9 +776,8 @@ void sendPage(bool useChar)
           sendComplete = true;
         }
         break;
-      }
+
     case seqFuelPage:
-      {
         if(useChar)
         {
           currentTable = trim1Table;
@@ -836,10 +838,8 @@ void sendPage(bool useChar)
           sendComplete = true;
         }
         break;
-      }
 
     case canbusPage:
-      {
         //currentTitleIndex = 141;
         if (useChar)
         {
@@ -853,14 +853,22 @@ void sendPage(bool useChar)
         }
         else { pnt_configPage = &configPage10; } //Create a pointer to Page 10 in memory
         break;
-      }
+
+    case warmupPage:
+        if (useChar)
+        {
+          sendComplete = true;
+        }
+        else { pnt_configPage = &configPage11; } //Create a pointer to Page 4 in memory
+        break;
 
     default:
-      {
         Serial.println(F("\nPage has not been implemented yet. Change to another page."));
+        //Just set default Values to avoid warnings
+        pnt_configPage = &configPage11;
+        currentTable = fuelTable;
         sendComplete = true;
         break;
-      }
   }
   if(!sendComplete)
   {
@@ -1010,13 +1018,18 @@ void receiveCalibration(byte tableID)
       break;
 
     default:
+      OFFSET = 0;
+      pnt_TargetTable = (byte *)&o2CalibrationTable;
+      DIVISION_FACTOR = 1;
+      BYTES_PER_VALUE = 1;
+      EEPROM_START = EEPROM_CALIBRATION_O2;
       break; //Should never get here, but if we do, just fail back to main loop
   }
 
   //1024 value pairs are sent. We have to receive them all, but only use every second one (We only store 512 calibratino table entries to save on EEPROM space)
   //The values are sent as 2 byte ints, but we convert them to single bytes. Any values over 255 are capped at 255.
   int tempValue;
-  int tempBuffer;
+  byte tempBuffer[2];
   bool every2nd = true;
   int x;
   int counter = 0;
@@ -1034,10 +1047,10 @@ void receiveCalibration(byte tableID)
     else
     {
       while ( Serial.available() < 2 ) {}
-      tempBuffer = Serial.read();
-      tempBuffer |= Serial.read() << 8;
+      tempBuffer[0] = Serial.read();
+      tempBuffer[1] = Serial.read();
 
-      tempValue = div(int(tempBuffer), DIVISION_FACTOR).quot; //Read 2 bytes, convert to word (an unsigned int), convert to signed int. These values come through * 10 from Tuner Studio
+      tempValue = div(int(word(tempBuffer[1], tempBuffer[0])), DIVISION_FACTOR).quot; //Read 2 bytes, convert to word (an unsigned int), convert to signed int. These values come through * 10 from Tuner Studio
       tempValue = ((tempValue - 32) * 5) / 9; //Convert from F to C
     }
     tempValue = tempValue + OFFSET;
diff --git a/speeduino/corrections.ino b/speeduino/corrections.ino
index 1a47d740..e9bcf88c 100644
--- a/speeduino/corrections.ino
+++ b/speeduino/corrections.ino
@@ -112,7 +112,11 @@ Additional fuel % to be added when the engine is cranking
 static inline byte correctionCranking()
 {
   byte crankingValue = 100;
-  if ( BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) ) { crankingValue = 100 + configPage1.crankingPct; }
+  //if ( BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) ) { crankingValue = 100 + configPage1.crankingPct; }
+  if ( BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) )
+  { 
+    crankingValue = table2D_getValue(&crankingEnrichTable, currentStatus.coolant + CALIBRATION_TEMPERATURE_OFFSET);
+  }
   return crankingValue;
 }
 
@@ -152,12 +156,13 @@ static inline byte correctionAccel()
   if( BIT_CHECK(currentStatus.engine, BIT_ENGINE_ACC) )
   {
     //If it is currently running, check whether it should still be running or whether it's reached it's end time
-    if( currentLoopTime >= currentStatus.TAEEndTime )
+    if( micros() >= currentStatus.TAEEndTime )
     {
       //Time to turn enrichment off
       BIT_CLEAR(currentStatus.engine, BIT_ENGINE_ACC);
       currentStatus.TAEamount = 0;
       accelValue = 100;
+      currentStatus.tpsDOT = 0;
     }
     else
     {
@@ -167,15 +172,18 @@ static inline byte correctionAccel()
   }
   else
   {
+    int8_t TPS_change = (currentStatus.TPS - currentStatus.TPSlast);
     //Check for deceleration (Deceleration adjustment not yet supported)
-    if (currentStatus.TPS < currentStatus.TPSlast)
+    //Also check for only very small movement (Movement less than or equal to 2% is ignored). This not only means we can skip the lookup, but helps reduce false triggering around 0-2% throttle openings
+    if (TPS_change <= 2)
     {
       accelValue = 100;
+      currentStatus.tpsDOT = 0;
     }
     else
     {
       //If TAE isn't currently turned on, need to check whether it needs to be turned on
-      int rateOfChange = ldiv(1000000, (currentStatus.TPS_time - currentStatus.TPSlast_time)).quot * (currentStatus.TPS - currentStatus.TPSlast); //This is the % per second that the TPS has moved
+      int rateOfChange = ldiv(1000000, (currentStatus.TPS_time - currentStatus.TPSlast_time)).quot * TPS_change; //This is the % per second that the TPS has moved
       currentStatus.tpsDOT = rateOfChange / 10; //The TAE bins are divided by 10 in order to allow them to be stored in a byte. Faster as this than divu10
 
       if (rateOfChange > configPage1.tpsThresh)
diff --git a/speeduino/decoders.h b/speeduino/decoders.h
index 91fda7e0..8d5ac583 100644
--- a/speeduino/decoders.h
+++ b/speeduino/decoders.h
@@ -15,6 +15,8 @@ static inline void addToothLogEntry(unsigned long);
 static inline uint16_t stdGetRPM();
 static inline void setFilter(unsigned long);
 static inline int crankingGetRPM(byte);
+static inline void doPerToothTiming(uint16_t crankAngle);
+
 void triggerSetup_missingTooth();
 void triggerPri_missingTooth();
 void triggerSec_missingTooth();
@@ -61,6 +63,11 @@ bool secondDerivEnabled; //The use of the 2nd derivative calculation is limited
 bool decoderIsSequential; //Whether or not the decoder supports sequential operation
 byte checkSyncToothCount; //How many teeth must've been seen on this revolution before we try to confirm sync (Useful for missing tooth type decoders)
 
+int16_t ignition1EndTooth = 0;
+int16_t ignition2EndTooth = 0;
+int16_t ignition3EndTooth = 0;
+int16_t ignition4EndTooth = 0;
+
 int toothAngles[24]; //An array for storing fixed tooth angles. Currently sized at 24 for the GM 24X decoder, but may grow later if there are other decoders that use this style
 
 //Used for identifying long and short pulses on the 4G63 (And possibly other) trigger patterns
diff --git a/speeduino/decoders.ino b/speeduino/decoders.ino
index b48759a4..ad683954 100644
--- a/speeduino/decoders.ino
+++ b/speeduino/decoders.ino
@@ -94,11 +94,24 @@ static inline int crankingGetRPM(byte totalTeeth)
     revolutionTime = (toothLastToothTime - toothLastMinusOneToothTime) * totalTeeth;
     interrupts();
     tempRPM = (US_IN_MINUTE / revolutionTime);
-    if(tempRPM >= MAX_RPM) { tempRPM = currentStatus.RPM; } //Sanity check
+    if( tempRPM >= MAX_RPM ) { tempRPM = currentStatus.RPM; } //Sanity check. This can prevent spiking caused by noise on individual teeth. The new RPM should never be above 4x the cranking setting value (Remembering that this function is only called is the current RPM is less than the cranking setting)
   }
   return tempRPM;
 }
 
+/*
+On decoders that are enabled for per tooth based timing adjustments, this function performs the timer compare changes on the schedules themselves
+For each ignition channel, a check is made whether we're at the relevant tooth and whether that ignition schedule is currently running
+Only if both these conditions are met will the schedule be updated with the latest timing information.
+*/
+static inline void doPerToothTiming(uint16_t crankAngle)
+{
+  if ( (toothCurrentCount == ignition1EndTooth) && (ignitionSchedule1.Status == RUNNING) ) { IGN1_COMPARE = IGN1_COUNTER + uS_TO_TIMER_COMPARE( (ignition1EndAngle - crankAngle) * timePerDegree ); }
+  else if ( (toothCurrentCount == ignition2EndTooth) && (ignitionSchedule2.Status == RUNNING) ) { IGN2_COMPARE = IGN2_COUNTER + uS_TO_TIMER_COMPARE( (ignition2EndAngle - crankAngle) * timePerDegree ); }
+  else if ( (toothCurrentCount == ignition3EndTooth) && (ignitionSchedule3.Status == RUNNING) ) { IGN3_COMPARE = IGN3_COUNTER + uS_TO_TIMER_COMPARE( (ignition3EndAngle - crankAngle) * timePerDegree ); }
+  else if ( (toothCurrentCount == ignition4EndTooth) && (ignitionSchedule4.Status == RUNNING) ) { IGN4_COMPARE = IGN4_COUNTER + uS_TO_TIMER_COMPARE( (ignition4EndAngle - crankAngle) * timePerDegree ); }
+}
+
 /*
 Name: Missing tooth wheel
 Desc: A multi-tooth wheel with one of more 'missing' teeth. The first tooth after the missing one is considered number 1 and isthe basis for the trigger angle
@@ -161,7 +174,12 @@ void triggerPri_missingTooth()
        }
      }
 
-
+     //EXPERIMENTAL!
+     if(configPage1.perToothIgn == true)
+     {
+       uint16_t crankAngle = ( (toothCurrentCount-1) * triggerToothAngle ) + configPage2.triggerAngle;
+       doPerToothTiming(crankAngle);
+     }
    }
 }
 
@@ -174,7 +192,7 @@ void triggerSec_missingTooth()
 uint16_t getRPM_missingTooth()
 {
   uint16_t tempRPM = 0;
-  if( currentStatus.RPM < (unsigned int)(configPage2.crankRPM * 100) )
+  if( (currentStatus.RPM < (unsigned int)(configPage2.crankRPM * 100)) && (toothCurrentCount != 1) ) //Can't do per tooth RPM if we're at tooth #1 as the missing tooth messes the calculation
   {
     if(configPage2.TrigSpeed == 1) { crankingGetRPM(configPage2.triggerTeeth/2); } //Account for cam speed
     else { tempRPM = crankingGetRPM(configPage2.triggerTeeth); }
@@ -217,6 +235,32 @@ int getCrankAngle_missingTooth(int timePerDegree)
     return crankAngle;
 }
 
+void triggerSetEndTeeth_missingTooth()
+{
+
+  ignition1EndTooth = ( (ignition1EndAngle - configPage2.triggerAngle) / triggerToothAngle ) - 1;
+  if(ignition1EndTooth > configPage2.triggerTeeth) { ignition1EndTooth -= configPage2.triggerTeeth; }
+  if(ignition1EndTooth <= 0) { ignition1EndTooth -= configPage2.triggerTeeth; }
+  if(ignition1EndTooth > triggerActualTeeth) { ignition1EndTooth = triggerActualTeeth; }
+
+  ignition2EndTooth = ( (ignition2EndAngle - configPage2.triggerAngle) / triggerToothAngle ) - 1;
+  if(ignition2EndTooth > configPage2.triggerTeeth) { ignition2EndTooth -= configPage2.triggerTeeth; }
+  if(ignition2EndTooth <= 0) { ignition2EndTooth -= configPage2.triggerTeeth; }
+  if(ignition2EndTooth > triggerActualTeeth) { ignition2EndTooth = triggerActualTeeth; }
+
+  ignition3EndTooth = ( (ignition3EndAngle - configPage2.triggerAngle) / triggerToothAngle ) - 1;
+  if(ignition3EndTooth > configPage2.triggerTeeth) { ignition3EndTooth -= configPage2.triggerTeeth; }
+  if(ignition3EndTooth <= 0) { ignition3EndTooth -= configPage2.triggerTeeth; }
+  if(ignition3EndTooth > triggerActualTeeth) { ignition3EndTooth = triggerActualTeeth; }
+
+  ignition4EndTooth = ( (ignition4EndAngle - configPage2.triggerAngle) / triggerToothAngle ) - 1;
+  if(ignition4EndTooth > configPage2.triggerTeeth) { ignition4EndTooth -= configPage2.triggerTeeth; }
+  if(ignition4EndTooth <= 0) { ignition4EndTooth -= configPage2.triggerTeeth; }
+  if(ignition4EndTooth > triggerActualTeeth) { ignition4EndTooth = triggerActualTeeth; }
+
+
+}
+
 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 Name: Dual wheel
 Desc: 2 wheels located either both on the crank or with the primary on the crank and the secondary on the cam.
@@ -225,6 +269,7 @@ Note: There can be no missing teeth on the primary wheel
 void triggerSetup_DualWheel()
 {
   triggerToothAngle = 360 / configPage2.triggerTeeth; //The number of degrees that passes from tooth to tooth
+  if(configPage2.TrigSpeed == 1) { triggerToothAngle = 720 / configPage2.triggerTeeth; } //Account for cam speed missing tooth
   toothCurrentCount = 255; //Default value
   triggerFilterTime = (int)(1000000 / (MAX_RPM / 60 * configPage2.triggerTeeth)); //Trigger filter time is the shortest possible time (in uS) that there can be between crank teeth (ie at max RPM). Any pulses that occur faster than this time will be disgarded as noise
   triggerSecFilterTime = (int)(1000000 / (MAX_RPM / 60 * 2)) / 2; //Same as above, but fixed at 2 teeth on the secondary input and divided by 2 (for cam speed)
@@ -259,6 +304,13 @@ void triggerPri_DualWheel()
 
        setFilter(curGap); //Recalc the new filter value
      }
+
+     //EXPERIMENTAL!
+     if(configPage1.perToothIgn == true)
+     {
+       uint16_t crankAngle = ( (toothCurrentCount-1) * triggerToothAngle ) + configPage2.triggerAngle;
+       doPerToothTiming(crankAngle);
+     }
    } //TRigger filter
 
 
@@ -330,6 +382,29 @@ int getCrankAngle_DualWheel(int timePerDegree)
     return crankAngle;
 }
 
+void triggerSetEndTeeth_DualWheel()
+{
+  ignition1EndTooth = ( (ignition1EndAngle - configPage2.triggerAngle) / triggerToothAngle ) - 1;
+  if(ignition1EndTooth > configPage2.triggerTeeth) { ignition1EndTooth -= configPage2.triggerTeeth; }
+  if(ignition1EndTooth <= 0) { ignition1EndTooth -= configPage2.triggerTeeth; }
+  if(ignition1EndTooth > triggerActualTeeth) { ignition1EndTooth = triggerActualTeeth; }
+
+  ignition2EndTooth = ( (ignition2EndAngle - configPage2.triggerAngle) / triggerToothAngle ) - 1;
+  if(ignition2EndTooth > configPage2.triggerTeeth) { ignition2EndTooth -= configPage2.triggerTeeth; }
+  if(ignition2EndTooth <= 0) { ignition2EndTooth -= configPage2.triggerTeeth; }
+  if(ignition2EndTooth > triggerActualTeeth) { ignition2EndTooth = triggerActualTeeth; }
+
+  ignition3EndTooth = ( (ignition3EndAngle - configPage2.triggerAngle) / triggerToothAngle ) - 1;
+  if(ignition3EndTooth > configPage2.triggerTeeth) { ignition3EndTooth -= configPage2.triggerTeeth; }
+  if(ignition3EndTooth <= 0) { ignition3EndTooth -= configPage2.triggerTeeth; }
+  if(ignition3EndTooth > triggerActualTeeth) { ignition3EndTooth = triggerActualTeeth; }
+
+  ignition4EndTooth = ( (ignition4EndAngle - configPage2.triggerAngle) / triggerToothAngle ) - 1;
+  if(ignition4EndTooth > configPage2.triggerTeeth) { ignition4EndTooth -= configPage2.triggerTeeth; }
+  if(ignition4EndTooth <= 0) { ignition4EndTooth -= configPage2.triggerTeeth; }
+  if(ignition4EndTooth > triggerActualTeeth) { ignition4EndTooth = triggerActualTeeth; }
+}
+
 
 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 Name: Basic Distributor
@@ -343,8 +418,10 @@ void triggerSetup_BasicDistributor()
   triggerToothAngle = 720 / triggerActualTeeth; //The number of degrees that passes from tooth to tooth
   triggerFilterTime = 60000000L / MAX_RPM / configPage1.nCylinders; // Minimum time required between teeth
   triggerFilterTime = triggerFilterTime / 2; //Safety margin
+  triggerFilterTime = 0;
   secondDerivEnabled = false;
   decoderIsSequential = false;
+  toothCurrentCount = 0; //Default value
 
   if(configPage1.nCylinders <= 4) { MAX_STALL_TIME = (1851UL * triggerToothAngle); }//Minimum 90rpm. (1851uS is the time per degree at 90rpm). This uses 90rpm rather than 50rpm due to the potentially very high stall time on a 4 cylinder if we wait that long.
   else { MAX_STALL_TIME = (3200UL * triggerToothAngle); } //Minimum 50rpm. (3200uS is the time per degree at 50rpm).
@@ -355,7 +432,7 @@ void triggerPri_BasicDistributor()
 {
   curTime = micros();
   curGap = curTime - toothLastToothTime;
-  if ( curGap >= triggerFilterTime )
+  if ( (curGap >= triggerFilterTime) )
   {
     if( (toothCurrentCount == triggerActualTeeth) || (currentStatus.hasSync == false) ) //Check if we're back to the beginning of a revolution
     {
@@ -381,6 +458,20 @@ void triggerPri_BasicDistributor()
       endCoil4Charge();
     }
 
+    if(configPage1.perToothIgn == true)
+    {
+      uint16_t crankAngle = ( (toothCurrentCount-1) * triggerToothAngle ) + configPage2.triggerAngle;
+      if ( (toothCurrentCount == ignition1EndTooth) && (ignitionSchedule1.Status == RUNNING) )
+      {
+        IGN1_COMPARE = IGN1_COUNTER + uS_TO_TIMER_COMPARE( (ignition1EndAngle - crankAngle) * timePerDegree );
+        //IGN1_COMPARE = IGN1_COUNTER + uS_TO_TIMER_COMPARE( (ignition1EndAngle - crankAngle)*my_timePerDegree - micros_compensation );
+
+      }
+      else if ( (toothCurrentCount == ignition2EndTooth) && (ignitionSchedule2.Status == RUNNING) ) { IGN2_COMPARE = IGN2_COUNTER + uS_TO_TIMER_COMPARE( (ignition2EndAngle - crankAngle) * timePerDegree ); }
+      else if ( (toothCurrentCount == ignition3EndTooth) && (ignitionSchedule3.Status == RUNNING) ) { IGN3_COMPARE = IGN3_COUNTER + uS_TO_TIMER_COMPARE( (ignition3EndAngle - crankAngle) * timePerDegree ); }
+      else if ( (toothCurrentCount == ignition4EndTooth) && (ignitionSchedule4.Status == RUNNING) ) { IGN4_COMPARE = IGN4_COUNTER + uS_TO_TIMER_COMPARE( (ignition4EndAngle - crankAngle) * timePerDegree ); }
+    }
+
     toothLastMinusOneToothTime = toothLastToothTime;
     toothLastToothTime = curTime;
   } //Trigger filter
@@ -392,7 +483,10 @@ uint16_t getRPM_BasicDistributor()
   if( currentStatus.RPM < (unsigned int)(configPage2.crankRPM * 100) )
   { tempRPM = crankingGetRPM(triggerActualTeeth >> 1); } //crankGetRPM uses teeth per 360 degrees. As triggerActualTeeh is total teeth in 720 degrees, we divide the tooth count by 2
   else
-  { tempRPM = stdGetRPM() << 1; } //Multiply RPM by 2 due to tracking over 720 degrees now rather than 360
+  {
+    tempRPM = stdGetRPM() << 1;
+    revolutionTime = revolutionTime >> 1; //Revolution time has to be divided by 2 as otherwise it would be over 720 degrees (triggerActualTeeth = nCylinders)
+  } //Multiply RPM by 2 due to tracking over 720 degrees now rather than 360
 
   return tempRPM;
 
@@ -421,6 +515,14 @@ int getCrankAngle_BasicDistributor(int timePerDegree)
     return crankAngle;
 }
 
+void triggerSetEndTeeth_BasicDistributor()
+{
+  ignition1EndTooth = ( (ignition1EndAngle - configPage2.triggerAngle) / triggerToothAngle ) - 1;
+  if(ignition1EndTooth > configPage2.triggerTeeth) { ignition1EndTooth -= configPage2.triggerTeeth; }
+  if(ignition1EndTooth <= 0) { ignition1EndTooth -= configPage2.triggerTeeth; }
+  if(ignition1EndTooth > triggerActualTeeth) { ignition1EndTooth = triggerActualTeeth; }
+}
+
 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 Name: GM7X
 Desc: GM 7X trigger wheel. It has six equally spaced teeth and a seventh tooth for cylinder identification.
@@ -512,6 +614,11 @@ int getCrankAngle_GM7X(int timePerDegree)
     return crankAngle;
 }
 
+void triggerSetEndTeeth_GM7X()
+{
+
+}
+
 
 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 Name: Mitsubishi 4G63 / NA/NB Miata + MX-5 / 4/2
@@ -574,7 +681,7 @@ void triggerPri_4G63()
 {
   curTime = micros();
   curGap = curTime - toothLastToothTime;
-  if ( curGap >= triggerFilterTime )
+  if ( (curGap >= triggerFilterTime) || (currentStatus.startRevolutions == 0) )
   {
     addToothLogEntry(curGap);
     triggerFilterTime = curGap >> 2; //This only applies during non-sync conditions. If there is sync then triggerFilterTime gets changed again below with a better value.
@@ -657,7 +764,7 @@ void triggerSec_4G63()
 
   curTime2 = micros();
   curGap2 = curTime2 - toothLastSecToothTime;
-  if ( curGap2 >= triggerSecFilterTime )
+  if ( (curGap2 >= triggerSecFilterTime) || (currentStatus.startRevolutions == 0) )
   {
     toothLastSecToothTime = curTime2;
 
@@ -746,6 +853,11 @@ int getCrankAngle_4G63(int timePerDegree)
     return crankAngle;
 }
 
+void triggerSetEndTeeth_4G63()
+{
+
+}
+
 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 Name: GM
 Desc: TBA
@@ -851,6 +963,11 @@ int getCrankAngle_24X(int timePerDegree)
     return crankAngle;
 }
 
+void triggerSetEndTeeth_24X()
+{
+
+}
+
 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 Name: Jeep 2000
 Desc: For '91 to 2000 6 cylinder Jeep engines
@@ -947,7 +1064,12 @@ int getCrankAngle_Jeep2000(int timePerDegree)
     return crankAngle;
 }
 
-/*
+void triggerSetEndTeeth_Jeep2000()
+{
+
+}
+
+/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 Name: Audi 135
 Desc: 135 teeth on the crank and 1 tooth on the cam.
 Note: This is very similar to the dual wheel decoder, however due to the 135 teeth not dividing evenly into 360, only every 3rd crank tooth is used in calculating the crank angle. This effectively makes it a 45 tooth dual wheel setup
@@ -1059,6 +1181,11 @@ int getCrankAngle_Audi135(int timePerDegree)
     return crankAngle;
 }
 
+void triggerSetEndTeeth_Audi135()
+{
+
+}
+
 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 Name: Honda D17
 Desc:
@@ -1152,6 +1279,11 @@ int getCrankAngle_HondaD17(int timePerDegree)
     return crankAngle;
 }
 
+void triggerSetEndTeeth_HondaD17()
+{
+
+}
+
 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 Name: Miata '99 to '05
 Desc: TBA (See: http://forum.diyefi.org/viewtopic.php?f=56&t=1077)
@@ -1233,7 +1365,6 @@ void triggerSec_Miata9905()
   }
 }
 
-
 uint16_t getRPM_Miata9905()
 {
   //During cranking, RPM is calculated 4 times per revolution, once for each tooth on the crank signal.
@@ -1283,6 +1414,11 @@ int getCrankAngle_Miata9905(int timePerDegree)
     return crankAngle;
 }
 
+void triggerSetEndTeeth_Miata9905()
+{
+
+}
+
 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 Name: Mazda AU version
 Desc:
@@ -1431,6 +1567,11 @@ int getCrankAngle_MazdaAU(int timePerDegree)
     return crankAngle;
 }
 
+void triggerSetEndTeeth_MazdaAU()
+{
+
+}
+
 /*
 Name: Non-360 Dual wheel
 Desc: 2 wheels located either both on the crank or with the primary on the crank and the secondary on the cam.
@@ -1499,6 +1640,11 @@ int getCrankAngle_non360(int timePerDegree)
     return crankAngle;
 }
 
+void triggerSetEndTeeth_Non360()
+{
+
+}
+
 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 Name: Nissan 360 tooth with cam
 Desc:
@@ -1666,8 +1812,13 @@ int getCrankAngle_Nissan360(int timePerDegree)
   return crankAngle;
 }
 
+void triggerSetEndTeeth_Nissan360()
+{
+
+}
+
 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
-Name: Nissan 360 tooth with cam
+Name: Subary 6/7
 Desc:
 Note:
 */
@@ -1810,6 +1961,11 @@ int getCrankAngle_Subaru67(int timePerDegree)
   return crankAngle;
 }
 
+void triggerSetEndTeeth_Subaru67()
+{
+
+}
+
 /* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 Name: Daihatsu +1 trigger for 3 and 4 cylinder engines
 Desc: Tooth equal to the number of cylinders are evenly spaced on the cam. No position sensing (Distributor is retained) so crank angle is a made up figure based purely on the first teeth to be seen
@@ -1966,3 +2122,8 @@ int getCrankAngle_Daihatsu(int timePerDegree)
 
     return crankAngle;
 }
+
+void triggerSetEndTeeth_Daihatsu()
+{
+
+}
diff --git a/speeduino/globals.h b/speeduino/globals.h
index 257ed8d8..98a400ec 100644
--- a/speeduino/globals.h
+++ b/speeduino/globals.h
@@ -18,11 +18,16 @@
     #define BOARD_NR_GPIO_PINS 78
     #define LED_BUILTIN PA7
   #endif
-  
+
   extern "C" char* sbrk(int incr); //Used to freeRam
   inline unsigned char  digitalPinToInterrupt(unsigned char Interrupt_pin) { return Interrupt_pin; } //This isn't included in the stm32duino libs (yet)
-  #define portOutputRegister(port) (volatile byte *)( &(port->ODR) )
-  #define portInputRegister(port) (volatile byte *)( &(port->IDR) )
+  #if defined(ARDUINO_ARCH_STM32)
+    #define portOutputRegister(port) (volatile byte *)( &(port->ODR) )
+    #define portInputRegister(port) (volatile byte *)( &(port->IDR) )
+  #else
+    #define portOutputRegister(port) (volatile byte *)( &(port->regs->ODR) ) //These are defined in STM32F1/variants/generic_stm32f103c/variant.h but return a non byte* value
+    #define portInputRegister(port) (volatile byte *)( &(port->regs->IDR) ) //These are defined in STM32F1/variants/generic_stm32f103c/variant.h but return a non byte* value
+  #endif
 #else
   #error Incorrect board selected. Please select the correct board (Usually Mega 2560) and upload again
 #endif
@@ -74,6 +79,12 @@
 #define BIT_SPARK2_UNUSED7        6
 #define BIT_SPARK2_UNUSED8        7
 
+#define BIT_TIMER_1HZ             0
+#define BIT_TIMER_4HZ             1
+#define BIT_TIMER_10HZ            2
+#define BIT_TIMER_15HZ            3
+#define BIT_TIMER_30HZ            4
+
 #define VALID_MAP_MAX 1022 //The largest ADC value that is valid for the MAP sensor
 #define VALID_MAP_MIN 2 //The smallest ADC value that is valid for the MAP sensor
 
@@ -106,16 +117,19 @@
 
 #define SERIAL_BUFFER_THRESHOLD 32 // When the serial buffer is filled to greater than this threshold value, the serial processing operations will be performed more urgently in order to avoid it overflowing. Serial buffer is 64 bytes long, so the threshold is set at half this as a reasonable figure
 
+#define FUEL_PUMP_ON() *pump_pin_port |= (pump_pin_mask)
+#define FUEL_PUMP_OFF() *tach_pin_port &= ~(tach_pin_mask)
+
 const byte signature = 20;
 
 //const char signature[] = "speeduino";
-const char displaySignature[] = "Speeduino 2017";
-const char TSfirmwareVersion[] = "07-dev";
+const char displaySignature[] = "speeduino 201609-dev";
+const char TSfirmwareVersion[] = "Speeduino 2016.09";
 
 const byte data_structure_version = 2; //This identifies the data structure when reading / writing.
 const byte page_size = 64;
-const int npage_size[11] = {0,288,64,288,64,288,64,64,160,192,128};
-//const byte page10_size = 128;
+const int npage_size[12] = {0,288,64,288,64,288,64,64,160,192,128,192};
+//const byte page11_size = 128;
 #define MAP_PAGE_SIZE 288
 
 struct table3D fuelTable; //16x16 fuel map
@@ -129,6 +143,7 @@ struct table3D trim3Table; //6x6 Fuel trim 3 map
 struct table3D trim4Table; //6x6 Fuel trim 4 map
 struct table2D taeTable; //4 bin TPS Acceleration Enrichment map (2D)
 struct table2D WUETable; //10 bin Warm Up Enrichment map (2D)
+struct table2D crankingEnrichTable; //4 bin cranking Enrichment map (2D)
 struct table2D dwellVCorrectionTable; //6 bin dwell voltage correction (2D)
 struct table2D injectorVCorrectionTable; //6 bin injector voltage correction (2D)
 struct table2D IATDensityCorrectionTable; //9 bin inlet air temperature density correction (2D)
@@ -159,6 +174,8 @@ volatile byte ign5_pin_mask;
 
 volatile byte *tach_pin_port;
 volatile byte tach_pin_mask;
+volatile byte *pump_pin_port;
+volatile byte pump_pin_mask;
 
 volatile byte *triggerPri_pin_port;
 volatile byte triggerPri_pin_mask;
@@ -172,9 +189,17 @@ bool channel3InjEnabled = false;
 bool channel4InjEnabled = false;
 bool channel5InjEnabled = false;
 
+int ignition1EndAngle = 0;
+int ignition2EndAngle = 0;
+int ignition3EndAngle = 0;
+int ignition4EndAngle = 0;
+
 //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)
 
+volatile byte TIMER_mask;
+volatile byte LOOP_TIMER;
+
 //The status struct contains the current values for all 'live' variables
 //In current version this is 64 bytes
 struct statuses {
@@ -182,8 +207,9 @@ struct statuses {
   uint16_t RPM;
   long longRPM;
   int mapADC;
+  int baroADC;
   long MAP; //Has to be a long for PID calcs (Boost control)
-  byte baro; //Barometric pressure is simply the inital MAP reading, taken before the engine is running
+  byte baro; //Barometric pressure is simply the inital MAP reading, taken before the engine is running. Alternatively, can be taken from an external sensor
   byte TPS; //The current TPS reading (0% - 100%)
   byte TPSlast; //The previous TPS reading
   unsigned long TPS_time; //The time the TPS sample was taken
@@ -232,7 +258,7 @@ struct statuses {
   volatile unsigned int loopsPerSecond;
   boolean launchingSoft; //True when in launch control soft limit mode
   boolean launchingHard; //True when in launch control hard limit mode
-  unsigned int freeRAM;
+  uint16_t freeRAM;
   unsigned int clutchEngagedRPM;
   bool flatShiftingHard;
   volatile byte startRevolutions; //A counter for how many revolutions have been completed since sync was achieved.
@@ -241,7 +267,7 @@ struct statuses {
   bool testActive;
   byte boostDuty;
   byte idleLoad; //Either the current steps or current duty cycle for the idle control.
-  int canin[9];   //16bit raw value of selected canin data for channel 1-8
+  uint16_t canin[16];   //16bit raw value of selected canin data for channel 0-15
   uint8_t current_caninchannel = 0; //start off at channel 0
 
   //Helpful bitwise operations:
@@ -314,7 +340,7 @@ struct config1 {
   byte algorithm : 1; //"Speed Density", "Alpha-N"
   byte baroCorr : 1;
   byte injLayout : 2;
-  byte unused2_38g : 1;
+  byte perToothIgn : 1;
   byte unused2_38h : 1;
 
   byte primePulse;
@@ -325,7 +351,7 @@ struct config1 {
   byte boostMaxDuty;
   byte tpsMin;
   byte tpsMax;
-  byte mapMin;
+  int8_t mapMin; //Must be signed
   uint16_t mapMax;
   byte fpPrime; //Time (In seconds) that the fuel pump should be primed for on power up
   byte stoich;
@@ -433,7 +459,8 @@ struct config3 {
   byte egoRPM; //RPM must be above this for closed loop to function
   byte egoTPSMax; //TPS must be below this for closed loop to function
   byte vvtPin : 6;
-  byte unused6_13 : 2;
+  byte useExtBaro : 1;
+  byte unused6_13f : 1;
   byte boostPin : 6;
   byte unused6_14 : 2;
   byte voltageCorrectionBins[6]; //X axis bins for voltage correction tables
@@ -466,7 +493,7 @@ struct config3 {
   byte lnchPullRes : 2;
   byte fuelTrimEnabled : 1;
   byte flatSEnable : 1;
-  byte unused60 : 4;
+  byte baroPin : 4;
   byte flatSSoftWin;
   byte flatSRetard;
   byte flatSArm;
@@ -517,45 +544,18 @@ struct config4 {
 struct config10 {
   byte enable_canbus:2;
   byte enable_candata_in:1;
-  byte caninput_sel[8];
-  uint16_t caninput_param_group[8];
-  uint8_t caninput_param_start_byte[8];
-  byte caninput_param_num_bytes[8];
-  byte unused10_41;
-  byte unused10_42;
-  byte unused10_43;
-  byte unused10_44;
-  byte unused10_45;
-  byte unused10_46;
-  byte unused10_47;
-  byte unused10_48;
-  byte unused10_49;
+  uint16_t caninput_sel;                    //bit status on/off if input is enabled
+  uint16_t caninput_param_group[16];        //u16 [15] array holding can address of input
+  uint8_t caninput_param_start_byte[16];     //u08 [15] array holds the start byte number(value of 0-7)
+  uint16_t caninput_param_num_bytes;     //u16 bit status of the number of bytes length 1 or 2
+  byte unused10_53;
+  byte unused10_54;
   byte enable_candata_out : 1;
   byte canoutput_sel[8];
   uint16_t canoutput_param_group[8];
   uint8_t canoutput_param_start_byte[8];
   byte canoutput_param_num_bytes[8];
-  byte unused10_76;
-  byte unused10_77;
-  byte unused10_78;
-  byte unused10_79;
-  byte unused10_80;
-  byte unused10_81;
-  byte unused10_82;
-  byte unused10_83;
-  byte unused10_84;
-  byte unused10_85;
-  byte unused10_86;
-  byte unused10_87;
-  byte unused10_88;
-  byte unused10_89;
-  byte unused10_90;
-  byte unused10_91;
-  byte unused10_92;
-  byte unused10_93;
-  byte unused10_94;
-  byte unused10_95;
-  byte unused10_96;
+
   byte unused10_97;
   byte unused10_98;
   byte unused10_99;
@@ -590,6 +590,64 @@ struct config10 {
   } __attribute__((__packed__)); //The 32 bit systems require all structs to be fully packed
 #endif
 
+/*
+Page 11 - No specific purpose. Created initially for the cranking enrich curve
+192 bytes long
+See ini file for further info (Config Page 11 in the ini)
+*/
+struct config11 {
+  byte crankingEnrichBins[4];
+  byte crankingEnrichValues[4];
+  byte unused11_8;
+  byte unused11_9;
+  byte unused11_10;
+  byte unused11_11;
+  byte unused11_12;
+  byte unused11_13;
+  byte unused11_14;
+  byte unused11_15;
+  byte unused11_16;
+  byte unused11_17;
+  byte unused11_18;
+  byte unused11_19;
+  byte unused11_20;
+  byte unused10_21;
+  byte unused11_22;
+  byte unused11_23;
+  byte unused11_24;
+  byte unused11_25;
+  byte unused11_26;
+  byte unused11_27;
+  byte unused11_28;
+  byte unused11_29;
+  byte unused11_107;
+  byte unused11_108;
+  byte unused11_109;
+  byte unused11_110;
+  byte unused11_111;
+  byte unused11_112;
+  byte unused11_113;
+  byte unused11_114;
+  byte unused11_115;
+  byte unused11_116;
+  byte unused11_117;
+  byte unused11_118;
+  byte unused11_119;
+  byte unused11_120;
+  byte unused11_121;
+  byte unused11_122;
+  byte unused11_123;
+  byte unused11_124;
+  byte unused11_125;
+  byte unused11_126;
+  byte unused11_127;
+  byte unused11_128_192[64];
+#if defined(CORE_AVR)
+  };
+#else
+  } __attribute__((__packed__)); //The 32 bit systems require all structs to be fully packed
+#endif
+
 
 byte pinInjector1; //Output pin injector 1
 byte pinInjector2; //Output pin injector 2
@@ -648,6 +706,7 @@ byte pinStepperEnable; //Turning the DRV8825 driver on/off
 byte pinLaunch;
 byte pinIgnBypass; //The pin used for an ignition bypass (Optional)
 byte pinFlex; //Pin with the flex sensor attached
+byte pinBaro; //Pin that an external barometric pressure sensor is attached to (If used)
 
 // global variables // from speeduino.ino
 extern struct statuses currentStatus; // from speeduino.ino
@@ -656,10 +715,12 @@ extern struct table3D ignitionTable; //16x16 ignition map
 extern struct table3D afrTable; //16x16 afr target map
 extern struct table2D taeTable; //4 bin TPS Acceleration Enrichment map (2D)
 extern struct table2D WUETable; //10 bin Warm Up Enrichment map (2D)
+extern struct table2D crankingEnrichTable; //4 bin cranking Enrichment map (2D)
 extern struct config1 configPage1;
 extern struct config2 configPage2;
 extern struct config3 configPage3;
 extern struct config10 configPage10;
+extern struct config11 configPage11;
 extern unsigned long currentLoopTime; //The time the current loop started (uS)
 extern unsigned long previousLoopTime; //The time the previous loop started (uS)
 extern byte ignitionCount;
diff --git a/speeduino/maths.h b/speeduino/maths.h
index dee4e611..55e154b9 100644
--- a/speeduino/maths.h
+++ b/speeduino/maths.h
@@ -1,7 +1,7 @@
 #ifndef MATH_H
 #define MATH_H
 
-int fastResize(int, int);
+int fastMap1023toX(int, int);
 unsigned long percentage(byte, unsigned long);
 
-#endif
+#endif
diff --git a/speeduino/maths.ino b/speeduino/maths.ino
index be1716b1..6a1033ec 100644
--- a/speeduino/maths.ino
+++ b/speeduino/maths.ino
@@ -18,11 +18,10 @@ int fastMap(unsigned long x, int in_min, int in_max, int out_min, int out_max)
 //This is a common case because it means converting from a standard 10-bit analog input to a byte or 10-bit analog into 0-511 (Eg the temperature readings)
 //int fastMap1023toX(unsigned long x, int in_min, int in_max, int out_min, int out_max)
 //removed ununsed variables, in_min and out_min is aways 0, in_max is aways 1023
-#if defined(CORE_STM32)
-  #define fastResize(x, out_max) ( ((unsigned long)x * out_max) >> 12)
-#else  
-  #define fastResize(x, out_max) ( ((unsigned long)x * out_max) >> 10)
-#endif
+#define fastMap1023toX(x, out_max) ( ((unsigned long)x * out_max) >> 10)
+//This is a new version that allows for out_min
+#define fastMap10Bit(x, out_min, out_max) ( ( ((unsigned long)x * (out_max-out_min)) >> 10 ) + out_min)
+
 /*
 The following are all fast versions of specific divisions
 Ref: http://www.hackersdelight.org/divcMore.pdf
@@ -74,17 +73,14 @@ int divs100(long n)
 //Unsigned divide by 100
 unsigned long divu100(unsigned long n)
 {
-  #if defined(CORE_STM32)
-    return (n / 100); // No difference with this on/off
-  #else
-    unsigned long q, r;
-    q = (n >> 1) + (n >> 3) + (n >> 6) - (n >> 10) +
-    (n >> 12) + (n >> 13) - (n >> 16);
-    q = q + (q >> 20);
-    q = q >> 6;
-    r = n - q*100;
-    return q + ((r + 28) >> 7);
-  #endif  
+  //return (n / 100);
+  unsigned long q, r;
+  q = (n >> 1) + (n >> 3) + (n >> 6) - (n >> 10) +
+  (n >> 12) + (n >> 13) - (n >> 16);
+  q = q + (q >> 20);
+  q = q >> 6;
+  r = n - (q * 100);
+  return q + ((r + 28) >> 7);
 }
 
 //Return x percent of y
@@ -104,4 +100,4 @@ inline long powint(int factor, unsigned int exponent)
    unsigned int counter = exponent;
    while ( (counter--) > 0) { product *= factor; }
    return product;
-}
+}
diff --git a/speeduino/scheduler.h b/speeduino/scheduler.h
index b8a67720..41f7d563 100644
--- a/speeduino/scheduler.h
+++ b/speeduino/scheduler.h
@@ -26,7 +26,7 @@ See page 136 of the processors datasheet: http://www.atmel.com/Images/doc2549.pd
 #define SCHEDULER_H
 
 
-#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega2561__)
+#if defined(CORE_AVR)
   #include <avr/interrupt.h>
   #include <avr/io.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
@@ -160,58 +162,59 @@ See page 136 of the processors datasheet: http://www.atmel.com/Images/doc2549.pd
   #define uS_TO_TIMER_COMPARE_SLOW(uS) ((uS * 15) >> 5)
 
 #elif defined(CORE_STM32)
+  #include "HardwareTimer.h"
   //Placeholders ONLY!
-
+  //https://visualgdb.com/tutorials/arm/stm32/timers/hal/
   //https://github.com/rogerclarkmelbourne/Arduino_STM32/blob/master/STM32F4/cores/maple/libmaple/timer.h#L51
   #define MAX_TIMER_PERIOD 131070 //The longest period of time (in uS) that the timer can permit (IN this case it is 65535 * 2, as each timer tick is 2uS)
   #define uS_TO_TIMER_COMPARE(uS) (uS >> 1) //Converts a given number of uS into the required number of timer ticks until that time has passed.
   #define uS_TO_TIMER_COMPARE_SLOW(uS) (uS >> 1) //Converts a given number of uS into the required number of timer ticks until that time has passed.
 
-  #define FUEL1_COUNTER (TIMER2->regs).gen->CNT
-  #define FUEL2_COUNTER (TIMER2->regs).gen->CNT
-  #define FUEL3_COUNTER (TIMER2->regs).gen->CNT
-  #define FUEL4_COUNTER (TIMER2->regs).gen->CNT
+  #define FUEL1_COUNTER (TIM2)->CNT
+  #define FUEL2_COUNTER (TIM2)->CNT
+  #define FUEL3_COUNTER (TIM2)->CNT
+  #define FUEL4_COUNTER (TIM2)->CNT
+  #define FUEL5_COUNTER (TIM2)->CNT
 
-  #define IGN1_COUNTER  (TIMER3->regs).gen->CNT
-  #define IGN2_COUNTER  (TIMER3->regs).gen->CNT
-  #define IGN3_COUNTER  (TIMER3->regs).gen->CNT
-  #define IGN4_COUNTER  (TIMER3->regs).gen->CNT
-  #define IGN5_COUNTER  (TIMER1->regs).gen->CNT
+  #define IGN1_COUNTER  (TIM3)->CNT
+  #define IGN2_COUNTER  (TIM3)->CNT
+  #define IGN3_COUNTER  (TIM3)->CNT
+  #define IGN4_COUNTER  (TIM3)->CNT
+  #define IGN5_COUNTER  (TIM3)->CNT
 
-  #define FUEL1_COMPARE (TIMER2->regs).gen->CCR1
-  #define FUEL2_COMPARE (TIMER2->regs).gen->CCR2
-  #define FUEL3_COMPARE (TIMER2->regs).gen->CCR3
-  #define FUEL4_COMPARE (TIMER2->regs).gen->CCR4
+  #define FUEL1_COMPARE (TIM2)->CCR1
+  #define FUEL2_COMPARE (TIM2)->CCR2
+  #define FUEL3_COMPARE (TIM2)->CCR3
+  #define FUEL4_COMPARE (TIM2)->CCR4
 
-  #define IGN1_COMPARE (TIMER3->regs).gen->CCR1
-  #define IGN2_COMPARE (TIMER3->regs).gen->CCR2
-  #define IGN3_COMPARE (TIMER3->regs).gen->CCR3
-  #define IGN4_COMPARE (TIMER3->regs).gen->CCR4
-  #define IGN5_COMPARE (TIMER1->regs).gen->CCR1
+  #define IGN1_COMPARE (TIM3)->CCR1
+  #define IGN2_COMPARE (TIM3)->CCR2
+  #define IGN3_COMPARE (TIM3)->CCR3
+  #define IGN4_COMPARE (TIM3)->CCR4
+  #define IGN5_COMPARE (TIM3)->CCR1
 
   //https://github.com/rogerclarkmelbourne/Arduino_STM32/blob/754bc2969921f1ef262bd69e7faca80b19db7524/STM32F1/system/libmaple/include/libmaple/timer.h#L444
-  #define FUEL1_TIMER_ENABLE() (TIMER2->regs).gen->CCER |= TIMER_CCER_CC1E
-  #define FUEL2_TIMER_ENABLE() (TIMER2->regs).gen->CCER |= TIMER_CCER_CC2E
-  #define FUEL3_TIMER_ENABLE() (TIMER2->regs).gen->CCER |= TIMER_CCER_CC3E
-  #define FUEL4_TIMER_ENABLE() (TIMER2->regs).gen->CCER |= TIMER_CCER_CC4E
+  #define FUEL1_TIMER_ENABLE() (TIM2)->CCER |= TIM_CCER_CC1E
+  #define FUEL2_TIMER_ENABLE() (TIM2)->CCER |= TIM_CCER_CC2E
+  #define FUEL3_TIMER_ENABLE() (TIM2)->CCER |= TIM_CCER_CC3E
+  #define FUEL4_TIMER_ENABLE() (TIM2)->CCER |= TIM_CCER_CC4E
 
-  #define IGN1_TIMER_ENABLE() (TIMER3->regs).gen->CCER |= TIMER_CCER_CC1E
-  #define IGN2_TIMER_ENABLE() (TIMER3->regs).gen->CCER |= TIMER_CCER_CC2E
-  #define IGN3_TIMER_ENABLE() (TIMER3->regs).gen->CCER |= TIMER_CCER_CC3E
-  #define IGN4_TIMER_ENABLE() (TIMER3->regs).gen->CCER |= TIMER_CCER_CC4E
-  #define IGN5_TIMER_ENABLE() (TIMER1->regs).gen->CCER |= TIMER_CCER_CC1E
+  #define IGN1_TIMER_ENABLE() (TIM3)->CCER |= TIM_CCER_CC1E
+  #define IGN2_TIMER_ENABLE() (TIM3)->CCER |= TIM_CCER_CC2E
+  #define IGN3_TIMER_ENABLE() (TIM3)->CCER |= TIM_CCER_CC3E
+  #define IGN4_TIMER_ENABLE() (TIM3)->CCER |= TIM_CCER_CC4E
+  #define IGN5_TIMER_ENABLE() (TIM1)->CCER |= TIM_CCER_CC1E
 
-  #define FUEL1_TIMER_DISABLE() (TIMER2->regs).gen->CCER &= ~TIMER_CCER_CC1E
-  #define FUEL2_TIMER_DISABLE() (TIMER2->regs).gen->CCER &= ~TIMER_CCER_CC2E
-  #define FUEL3_TIMER_DISABLE() (TIMER2->regs).gen->CCER &= ~TIMER_CCER_CC3E
-  #define FUEL4_TIMER_DISABLE() (TIMER2->regs).gen->CCER &= ~TIMER_CCER_CC4E
-
-  #define IGN1_TIMER_DISABLE() (TIMER3->regs).gen->CCER &= ~TIMER_CCER_CC1E
-  #define IGN2_TIMER_DISABLE() (TIMER3->regs).gen->CCER &= ~TIMER_CCER_CC2E
-  #define IGN3_TIMER_DISABLE() (TIMER3->regs).gen->CCER &= ~TIMER_CCER_CC3E
-  #define IGN4_TIMER_DISABLE() (TIMER3->regs).gen->CCER &= ~TIMER_CCER_CC4E
-  #define IGN5_TIMER_DISABLE() (TIMER1->regs).gen->CCER &= ~TIMER_CCER_CC1E
+  #define FUEL1_TIMER_DISABLE() (TIM2)->CCER &= ~TIM_CCER_CC1E
+  #define FUEL2_TIMER_DISABLE() (TIM2)->CCER &= ~TIM_CCER_CC2E
+  #define FUEL3_TIMER_DISABLE() (TIM2)->CCER &= ~TIM_CCER_CC3E
+  #define FUEL4_TIMER_DISABLE() (TIM2)->CCER &= ~TIM_CCER_CC4E
 
+  #define IGN1_TIMER_DISABLE() (TIM3)->CCER &= ~TIM_CCER_CC1E
+  #define IGN2_TIMER_DISABLE() (TIM3)->CCER &= ~TIM_CCER_CC2E
+  #define IGN3_TIMER_DISABLE() (TIM3)->CCER &= ~TIM_CCER_CC3E
+  #define IGN4_TIMER_DISABLE() (TIM3)->CCER &= ~TIM_CCER_CC4E
+  #define IGN5_TIMER_DISABLE() (TIM1)->CCER &= ~TIM_CCER_CC1E
 #endif
 
 void initialiseSchedulers();
@@ -255,8 +258,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
-  unsigned int endCompare;
+  volatile unsigned int startCompare; //The counter value of the timer when this will start
+  volatile unsigned int endCompare;
+
+  unsigned int nextStartCompare;
+  unsigned int nextEndCompare;
+  volatile bool hasNextSchedule = false;
 };
 
 volatile Schedule *timer3Aqueue[4];
@@ -345,8 +352,11 @@ static inline unsigned int popQueue(volatile Schedule *queue[])
   queue[2] = queue[3];
   queue[3] = &nullSchedule;
 
-  if( queue[0]->Status == PENDING ) { return queue[0]->startCompare; }
-  else { return queue[0]->endCompare; }
+  unsigned int returnCompare;
+  if( queue[0]->Status == PENDING ) { returnCompare = queue[0]->startCompare; }
+  else { returnCompare = queue[0]->endCompare; }
+
+  return returnCompare;
 }
 
 
diff --git a/speeduino/scheduler.ino b/speeduino/scheduler.ino
index 878869a4..5919e2de 100644
--- a/speeduino/scheduler.ino
+++ b/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
-  //FTM0_C0SC &= ~FTM_CSC_ELSA; //Probably not needed as power on state should be 0
-  //FTM0_C0SC &= ~FTM_CSC_DMA; //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_ELSB;
+  //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
@@ -161,19 +162,20 @@ 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
+  Timer1.setPrescaleFactor(((Timer1.getBaseFrequency()  / 1000000) << 1) -1);  //2us resolution
+  Timer2.setPrescaleFactor(((Timer2.getBaseFrequency()  / 1000000) << 1) -1);  //2us resolution
+  Timer3.setPrescaleFactor(((Timer3.getBaseFrequency()  / 1000000) << 1) -1);  //2us resolution
 
-  Timer2.setMode(TIMER_CH1, TIMER_OUTPUT_COMPARE);
-  Timer2.setMode(TIMER_CH2, TIMER_OUTPUT_COMPARE);
-  Timer2.setMode(TIMER_CH3, TIMER_OUTPUT_COMPARE);
-  Timer2.setMode(TIMER_CH4, TIMER_OUTPUT_COMPARE);
+  Timer2.setMode(1, TIMER_OUTPUT_COMPARE);
+  Timer2.setMode(2, TIMER_OUTPUT_COMPARE);
+  Timer2.setMode(3, TIMER_OUTPUT_COMPARE);
+  Timer2.setMode(4, TIMER_OUTPUT_COMPARE);
 
-  Timer3.setMode(TIMER_CH1, TIMER_OUTPUT_COMPARE);
-  Timer3.setMode(TIMER_CH2, TIMER_OUTPUT_COMPARE);
-  Timer3.setMode(TIMER_CH3, TIMER_OUTPUT_COMPARE);
-  Timer3.setMode(TIMER_CH4, TIMER_OUTPUT_COMPARE);
-  Timer1.setMode(TIMER_CH1, TIMER_OUTPUT_COMPARE); 
+  Timer3.setMode(1, TIMER_OUTPUT_COMPARE);
+  Timer3.setMode(2, TIMER_OUTPUT_COMPARE);
+  Timer3.setMode(3, TIMER_OUTPUT_COMPARE);
+  Timer3.setMode(4, TIMER_OUTPUT_COMPARE);
+  Timer1.setMode(1, TIMER_OUTPUT_COMPARE); 
 
   Timer2.attachInterrupt(1, fuelSchedule1Interrupt);
   Timer2.attachInterrupt(2, fuelSchedule2Interrupt);
@@ -186,6 +188,9 @@ void initialiseSchedulers()
   Timer3.attachInterrupt(4, ignitionSchedule4Interrupt);
   Timer1.attachInterrupt(1, ignitionSchedule5Interrupt);
 
+  Timer1.resume();
+  Timer2.resume();
+  Timer3.resume();
 #endif
 
     fuelSchedule1.Status = OFF;
@@ -223,68 +228,78 @@ timeout: The number of uS in the future that the startCallback should be trigger
 duration: The number of uS after startCallback is called before endCallback is called
 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(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
-
-    fuelSchedule1.StartCallback = startCallback; //Name the start callback function
-    fuelSchedule1.EndCallback = endCallback; //Name the end callback function
+    //Callbacks no longer used, but retained for now:
+    //fuelSchedule1.StartCallback = startCallback;
+    //fuelSchedule1.EndCallback = endCallback;
     fuelSchedule1.duration = duration;
 
-    /*
-     * The following must be enclosed in the noInterupts 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();
-     fuelSchedule1.startCompare = FUEL1_COUNTER + uS_TO_TIMER_COMPARE_SLOW(timeout);
-     fuelSchedule1.endCompare = fuelSchedule1.startCompare + uS_TO_TIMER_COMPARE_SLOW(duration);
-     fuelSchedule1.Status = PENDING; //Turn this schedule on
-     fuelSchedule1.schedulesSet++; //Increment the number of times this schedule has been set
-     /*if(channel5InjEnabled) { FUEL1_COMPARE = setQueue(timer3Aqueue, &fuelSchedule1, &fuelSchedule5, FUEL1_COUNTER); } //Schedule 1 shares a timer with schedule 5
-     else { timer3Aqueue[0] = &fuelSchedule1; timer3Aqueue[1] = &fuelSchedule1; timer3Aqueue[2] = &fuelSchedule1; timer3Aqueue[3] = &fuelSchedule1; FUEL1_COMPARE = fuelSchedule1.startCompare; }*/
-     timer3Aqueue[0] = &fuelSchedule1; timer3Aqueue[1] = &fuelSchedule1; timer3Aqueue[2] = &fuelSchedule1; timer3Aqueue[3] = &fuelSchedule1; FUEL1_COMPARE = fuelSchedule1.startCompare;
-     interrupts();
-     FUEL1_TIMER_ENABLE();
+    //The following must be enclosed in the noInterupts block to avoid contention caused if the relevant interrupt fires before the state is fully set
+    noInterrupts();
+    fuelSchedule1.startCompare = FUEL1_COUNTER + uS_TO_TIMER_COMPARE_SLOW(timeout);
+    fuelSchedule1.endCompare = fuelSchedule1.startCompare + uS_TO_TIMER_COMPARE_SLOW(duration);
+    fuelSchedule1.Status = PENDING; //Turn this schedule on
+    fuelSchedule1.schedulesSet++; //Increment the number of times this schedule has been set
+    //Schedule 1 shares a timer with schedule 5
+    //if(channel5InjEnabled) { FUEL1_COMPARE = setQueue(timer3Aqueue, &fuelSchedule1, &fuelSchedule5, FUEL1_COUNTER); }
+    //else { timer3Aqueue[0] = &fuelSchedule1; timer3Aqueue[1] = &fuelSchedule1; timer3Aqueue[2] = &fuelSchedule1; timer3Aqueue[3] = &fuelSchedule1; FUEL1_COMPARE = fuelSchedule1.startCompare; }
+    //timer3Aqueue[0] = &fuelSchedule1; timer3Aqueue[1] = &fuelSchedule1; timer3Aqueue[2] = &fuelSchedule1; timer3Aqueue[3] = &fuelSchedule1;
+    FUEL1_COMPARE = fuelSchedule1.startCompare;
+    interrupts();
+    FUEL1_TIMER_ENABLE();
   }
-void setFuelSchedule2(void (*startCallback)(), unsigned long timeout, unsigned long duration, void(*endCallback)())
+  else
   {
-    if(fuelSchedule2.Status == RUNNING) { return; } //Check that we're not already part way through a schedule
-
-    fuelSchedule2.StartCallback = startCallback; //Name the start callback function
-    fuelSchedule2.EndCallback = endCallback; //Name the end callback function
+    //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
+    fuelSchedule1.nextStartCompare = FUEL1_COUNTER + uS_TO_TIMER_COMPARE_SLOW(timeout);
+    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 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();
-     fuelSchedule2.startCompare = FUEL2_COUNTER + uS_TO_TIMER_COMPARE_SLOW(timeout);
-     fuelSchedule2.endCompare = fuelSchedule2.startCompare + uS_TO_TIMER_COMPARE_SLOW(duration);
-     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();
+    //The following must be enclosed in the noInterupts block to avoid contention caused if the relevant interrupt fires before the state is fully set
+    noInterrupts();
+    fuelSchedule2.startCompare = FUEL2_COUNTER + uS_TO_TIMER_COMPARE_SLOW(timeout);
+    fuelSchedule2.endCompare = fuelSchedule2.startCompare + uS_TO_TIMER_COMPARE_SLOW(duration);
+    FUEL2_COMPARE = fuelSchedule2.startCompare; //Use the B compare 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();
   }
-void setFuelSchedule3(void (*startCallback)(), unsigned long timeout, unsigned long duration, void(*endCallback)())
+  else
   {
-    if(fuelSchedule3.Status == RUNNING) { return; } //Check that we're not already part way through a schedule
-
-    fuelSchedule3.StartCallback = startCallback; //Name the start callback function
-    fuelSchedule3.EndCallback = endCallback; //Name the end callback function
+    //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
+  {
+    //Callbacks no longer used, but retained for now:
+    //fuelSchedule3.StartCallback = startCallback;
+    //fuelSchedule3.EndCallback = endCallback;
     fuelSchedule3.duration = duration;
 
-    /*
-     * 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
-     */
+    //The following must be enclosed in the noInterupts block to avoid contention caused if the relevant interrupt fires before the state is fully set
     noInterrupts();
     fuelSchedule3.startCompare = FUEL3_COUNTER + uS_TO_TIMER_COMPARE_SLOW(timeout);
     fuelSchedule3.endCompare = fuelSchedule3.startCompare + uS_TO_TIMER_COMPARE_SLOW(duration);
@@ -294,20 +309,26 @@ void setFuelSchedule3(void (*startCallback)(), unsigned long timeout, unsigned l
     interrupts();
     FUEL3_TIMER_ENABLE();
   }
-void setFuelSchedule4(void (*startCallback)(), unsigned long timeout, unsigned long duration, void(*endCallback)()) //Uses timer 4 compare B
+  else
   {
-    if(fuelSchedule4.Status == RUNNING) { return; } //Check that we're not already part way through a schedule
-
-    fuelSchedule4.StartCallback = startCallback; //Name the start callback function
-    fuelSchedule4.EndCallback = endCallback; //Name the end callback function
+    //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
+    fuelSchedule3.nextStartCompare = FUEL3_COUNTER + uS_TO_TIMER_COMPARE_SLOW(timeout);
+    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 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
-     */
+    //The following must be enclosed in the noInterupts block to avoid contention caused if the relevant interrupt fires before the state is fully set
     noInterrupts();
     fuelSchedule4.startCompare = FUEL4_COUNTER + uS_TO_TIMER_COMPARE_SLOW(timeout);
     fuelSchedule4.endCompare = fuelSchedule4.startCompare + uS_TO_TIMER_COMPARE_SLOW(duration);
@@ -317,45 +338,62 @@ void setFuelSchedule4(void (*startCallback)(), unsigned long timeout, unsigned l
     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))
-    //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
-    fuelSchedule5.EndCallback = endCallback; //Name the end callback function
-    fuelSchedule5.duration = duration;
-
-    /*
-     * The following must be enclosed in the noIntterupts block to avoid contention caused if the relevant interrupts fires before the state is fully set
-     */
-#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega2561__)
-    noInterrupts();
-    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
-    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
-    interrupts();
-    TIMSK3 |= (1 << OCIE3A); //Turn on the A compare unit (ie turn on the interrupt)
-#endif
+      /*
+       * The following must be enclosed in the noIntterupts block to avoid contention caused if the relevant interrupts fires before the state is fully set
+       */
+  #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega2561__)
+      noInterrupts();
+      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
+      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
+      interrupts();
+      TIMSK3 |= (1 << OCIE3A); //Turn on the A compare unit (ie turn on the interrupt)
+  #endif
+    }
+    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
+      fuelSchedule5.nextStartCompare = FUEL5_COUNTER + uS_TO_TIMER_COMPARE_SLOW(timeout);
+      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))
-    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.
+    //Need to check that the timeout doesn't exceed the overflow
+    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
@@ -363,19 +401,22 @@ 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;
 
-    //As the timer is ticking every 4uS (Time per Tick = (Prescale)*(1/Frequency))
-    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.
+    //Need to check that the timeout doesn't exceed the overflow
+    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();
-    ignitionSchedule2.startCompare = IGN2_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)
+    ignitionSchedule2.startCompare = IGN2_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)
     ignitionSchedule2.endCompare = ignitionSchedule2.startCompare + uS_TO_TIMER_COMPARE(duration);
     IGN2_COMPARE = ignitionSchedule2.startCompare;
     ignitionSchedule2.Status = PENDING; //Turn this schedule on
@@ -383,19 +424,23 @@ 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
     ignitionSchedule3.duration = duration;
 
-    //The timer is ticking every 4uS (Time per Tick = (Prescale)*(1/Frequency))
-    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.
+    //Need to check that the timeout doesn't exceed the overflow
+    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();
-    ignitionSchedule3.startCompare = IGN3_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)
+    ignitionSchedule3.startCompare = IGN3_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)
     ignitionSchedule3.endCompare = ignitionSchedule3.startCompare + uS_TO_TIMER_COMPARE(duration);
     IGN3_COMPARE = ignitionSchedule3.startCompare;
     ignitionSchedule3.Status = PENDING; //Turn this schedule on
@@ -403,21 +448,23 @@ 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
     ignitionSchedule4.duration = duration;
 
-    //We need to calculate the value to reset the timer to (preload) in order to achieve the desired overflow time
-    //The timer is ticking every 16uS (Time per Tick = (Prescale)*(1/Frequency))
-    //Note this is different to the other ignition timers
-    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.
+    //Need to check that the timeout doesn't exceed the overflow
+    uint16_t timeout_timer_compare;
+    if (timeout > MAX_TIMER_PERIOD) { timeout_timer_compare = uS_TO_TIMER_COMPARE_SLOW( (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_SLOW(timeout); } //Normal case
 
     noInterrupts();
-    ignitionSchedule4.startCompare = IGN4_COUNTER + uS_TO_TIMER_COMPARE_SLOW(timeout);
+    ignitionSchedule4.startCompare = IGN4_COUNTER + timeout_timer_compare;
     ignitionSchedule4.endCompare = ignitionSchedule4.startCompare + uS_TO_TIMER_COMPARE_SLOW(duration);
     IGN4_COMPARE = ignitionSchedule4.startCompare;
     ignitionSchedule4.Status = PENDING; //Turn this schedule on
@@ -425,21 +472,23 @@ 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
     ignitionSchedule5.duration = duration;
 
-    //We need to calculate the value to reset the timer to (preload) in order to achieve the desired overflow time
-    //The timer is ticking every 16uS (Time per Tick = (Prescale)*(1/Frequency))
-    //Note this is different to the other ignition timers
-    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.
+    //Need to check that the timeout doesn't exceed the overflow
+    uint16_t timeout_timer_compare;
+    if (timeout > MAX_TIMER_PERIOD) { timeout_timer_compare = uS_TO_TIMER_COMPARE_SLOW( (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_SLOW(timeout); } //Normal case
 
     noInterrupts();
-    ignitionSchedule5.startCompare = IGN5_COUNTER + uS_TO_TIMER_COMPARE_SLOW(timeout);
+    ignitionSchedule5.startCompare = IGN5_COUNTER + timeout_timer_compare;
     ignitionSchedule5.endCompare = ignitionSchedule5.startCompare + uS_TO_TIMER_COMPARE_SLOW(duration);
     IGN5_COMPARE = ignitionSchedule5.startCompare;
     ignitionSchedule5.Status = PENDING; //Turn this schedule on
@@ -447,6 +496,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
@@ -459,20 +509,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 (timer3Aqueue[0]->Status == PENDING) //Check to see if this schedule is turn on
+    if (fuelSchedule1.Status == PENDING) //Check to see if this schedule is turn on
     {
-      timer3Aqueue[0]->StartCallback();
-      timer3Aqueue[0]->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);
+      //To use timer queue, change fuelShedule1 to timer3Aqueue[0];
+      if (configPage1.injLayout == INJ_SEMISEQUENTIAL) { openInjector1and4(); }
+      else { openInjector1(); }
+      fuelSchedule1.Status = RUNNING; //Set the status to be in progress (ie The start callback has been called, but not the end callback)
+      FUEL1_COMPARE = fuelSchedule1.endCompare;
     }
-    else if (timer3Aqueue[0]->Status == RUNNING)
+    else if (fuelSchedule1.Status == RUNNING)
     {
-       timer3Aqueue[0]->EndCallback();
-       timer3Aqueue[0]->Status = OFF; //Turn off the schedule
-       timer3Aqueue[0]->schedulesSet = 0;
-       FUEL1_COMPARE = popQueue(timer3Aqueue);
+       //timer3Aqueue[0]->EndCallback();
+       if (configPage1.injLayout == INJ_SEMISEQUENTIAL) { closeInjector1and4(); }
+       else { closeInjector1(); }
+       fuelSchedule1.Status = OFF; //Turn off the schedule
+       fuelSchedule1.schedulesSet = 0;
+       //FUEL1_COMPARE = fuelSchedule1.endCompare;
+
+       //If there is a next schedule queued up, activate it
+       if(fuelSchedule1.hasNextSchedule == true)
+       {
+         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
@@ -483,16 +548,30 @@ static inline void fuelSchedule2Interrupt() //Most ARM chips can simply call a f
   {
     if (fuelSchedule2.Status == PENDING) //Check to see if this schedule is turn on
     {
-      fuelSchedule2.StartCallback();
+      //fuelSchedule2.StartCallback();
+      if (configPage1.injLayout == INJ_SEMISEQUENTIAL) { openInjector2and3(); }
+      else { openInjector2(); }
       fuelSchedule2.Status = RUNNING; //Set the status to be in progress (ie The start callback has been called, but not the end callback)
       FUEL2_COMPARE = fuelSchedule2.endCompare;
     }
     else if (fuelSchedule2.Status == RUNNING)
     {
-       fuelSchedule2.EndCallback();
+       //fuelSchedule2.EndCallback();
+       if (configPage1.injLayout == INJ_SEMISEQUENTIAL) { closeInjector2and3(); }
+       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 == true)
+       {
+         FUEL2_COMPARE = fuelSchedule2.nextStartCompare;
+         fuelSchedule2.endCompare = fuelSchedule2.nextEndCompare;
+         fuelSchedule2.Status = PENDING;
+         fuelSchedule2.schedulesSet = 1;
+         fuelSchedule2.hasNextSchedule = false;
+       }
+       else { FUEL2_TIMER_DISABLE(); }
     }
   }
 
@@ -504,16 +583,32 @@ static inline void fuelSchedule3Interrupt() //Most ARM chips can simply call a f
   {
     if (fuelSchedule3.Status == PENDING) //Check to see if this schedule is turn on
     {
-      fuelSchedule3.StartCallback();
+      //fuelSchedule3.StartCallback();
+      //Hack for 5 cylinder
+      if(channel5InjEnabled) { openInjector3and5(); }
+      else { openInjector3(); }
       fuelSchedule3.Status = RUNNING; //Set the status to be in progress (ie The start callback has been called, but not the end callback)
       FUEL3_COMPARE = fuelSchedule3.endCompare;
     }
     else if (fuelSchedule3.Status == RUNNING)
     {
-       fuelSchedule3.EndCallback();
+       //fuelSchedule3.EndCallback();
+       //Hack for 5 cylinder
+       if(channel5InjEnabled) { closeInjector3and5(); }
+       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 == true)
+       {
+         FUEL3_COMPARE = fuelSchedule3.nextStartCompare;
+         fuelSchedule3.endCompare = fuelSchedule3.nextEndCompare;
+         fuelSchedule3.Status = PENDING;
+         fuelSchedule3.schedulesSet = 1;
+         fuelSchedule3.hasNextSchedule = false;
+       }
+       else { FUEL3_TIMER_DISABLE(); }
     }
   }
 
@@ -525,16 +620,28 @@ static inline void fuelSchedule4Interrupt() //Most ARM chips can simply call a f
   {
     if (fuelSchedule4.Status == PENDING) //Check to see if this schedule is turn on
     {
-      fuelSchedule4.StartCallback();
+      //fuelSchedule4.StartCallback();
+      openInjector4();
       fuelSchedule4.Status = RUNNING; //Set the status to be in progress (ie The start callback has been called, but not the end callback)
       FUEL4_COMPARE = fuelSchedule4.endCompare;
     }
     else if (fuelSchedule4.Status == RUNNING)
     {
-       fuelSchedule4.EndCallback();
+       //fuelSchedule4.EndCallback();
+       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 == true)
+       {
+         FUEL4_COMPARE = fuelSchedule4.nextStartCompare;
+         fuelSchedule4.endCompare = fuelSchedule4.nextEndCompare;
+         fuelSchedule4.Status = PENDING;
+         fuelSchedule4.schedulesSet = 1;
+         fuelSchedule4.hasNextSchedule = false;
+       }
+       else { FUEL4_TIMER_DISABLE(); }
     }
   }
 
@@ -550,7 +657,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)
     {
@@ -622,7 +729,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)
     {
@@ -663,15 +770,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(); }
-  else if(FTM0_C1SC & FTM_CSC_CHF) { FTM0_C1SC &= ~FTM_CSC_CHF; fuelSchedule2Interrupt(); }
-  else if(FTM0_C2SC & FTM_CSC_CHF) { FTM0_C2SC &= ~FTM_CSC_CHF; fuelSchedule3Interrupt(); }
-  else if(FTM0_C3SC & FTM_CSC_CHF) { FTM0_C3SC &= ~FTM_CSC_CHF; fuelSchedule4Interrupt(); }
-  else if(FTM0_C4SC & FTM_CSC_CHF) { FTM0_C4SC &= ~FTM_CSC_CHF; ignitionSchedule1Interrupt(); }
-  else if(FTM0_C5SC & FTM_CSC_CHF) { FTM0_C5SC &= ~FTM_CSC_CHF; ignitionSchedule2Interrupt(); }
-  else if(FTM0_C6SC & FTM_CSC_CHF) { FTM0_C6SC &= ~FTM_CSC_CHF; ignitionSchedule3Interrupt(); }
-  else if(FTM0_C7SC & FTM_CSC_CHF) { FTM0_C7SC &= ~FTM_CSC_CHF; ignitionSchedule4Interrupt(); }
+  if(interrupt1) { FTM0_C0SC &= ~FTM_CSC_CHF; fuelSchedule1Interrupt(); }
+  else if(interrupt2) { FTM0_C1SC &= ~FTM_CSC_CHF; fuelSchedule2Interrupt(); }
+  else if(interrupt3) { FTM0_C2SC &= ~FTM_CSC_CHF; fuelSchedule3Interrupt(); }
+  else if(interrupt4) { FTM0_C3SC &= ~FTM_CSC_CHF; fuelSchedule4Interrupt(); }
+  else if(interrupt5) { FTM0_C4SC &= ~FTM_CSC_CHF; ignitionSchedule1Interrupt(); }
+  else if(interrupt6) { FTM0_C5SC &= ~FTM_CSC_CHF; ignitionSchedule2Interrupt(); }
+  else if(interrupt7) { FTM0_C6SC &= ~FTM_CSC_CHF; ignitionSchedule3Interrupt(); }
+  else if(interrupt8) { FTM0_C7SC &= ~FTM_CSC_CHF; ignitionSchedule4Interrupt(); }
 
 }
 #endif
diff --git a/speeduino/sensors.h b/speeduino/sensors.h
index 63610724..c8294a2f 100644
--- a/speeduino/sensors.h
+++ b/speeduino/sensors.h
@@ -12,6 +12,7 @@
 #define ADCFILTER_O2   128
 #define ADCFILTER_BAT  128
 #define ADCFILTER_MAP   20 //This is only used on Instantaneous MAP readings and is intentionally very weak to allow for faster response
+#define ADCFILTER_BARO  64
 
 #define BARO_MIN      87
 #define BARO_MAX      108
@@ -38,10 +39,9 @@ byte MAPcurRev; //Tracks which revolution we're sampling on
 
 void instanteneousMAPReading();
 void readMAP();
+void readBaro();
 void flexPulse();
 
-unsigned int tempReading;
-
 #if defined(ANALOG_ISR)
 //Analog ISR interrupt routine
 /*
@@ -105,4 +105,4 @@ ISR(ADC_vect)
 }
 #endif
 
-#endif // SENSORS_H
+#endif // SENSORS_H
diff --git a/speeduino/sensors.ino b/speeduino/sensors.ino
index bdde8740..ce41fd0c 100644
--- a/speeduino/sensors.ino
+++ b/speeduino/sensors.ino
@@ -49,6 +49,7 @@ MAPcount = 0;
 
 void instanteneousMAPReading()
 {
+  unsigned int tempReading;
   //Instantaneous MAP readings
   #if defined(ANALOG_ISR_MAP)
     tempReading = AnChannel[pinMAP-A0];
@@ -62,13 +63,14 @@ void instanteneousMAPReading()
 
   currentStatus.mapADC = ADC_FILTER(tempReading, ADCFILTER_MAP, currentStatus.mapADC); //Very weak filter
 
-  currentStatus.MAP = fastResize(currentStatus.mapADC, configPage1.mapMax); //Get the current MAP value
-
+  currentStatus.MAP = fastMap10Bit(currentStatus.mapADC, configPage1.mapMin, configPage1.mapMax); //Get the current MAP value
+  if(currentStatus.MAP < 0) { currentStatus.MAP = 0; } //Sanity check
 
 }
 
 void readMAP()
 {
+  unsigned int tempReading;
   //MAP Sampling system
   switch(configPage1.mapSample)
   {
@@ -106,7 +108,8 @@ void readMAP()
           if( (MAPrunningValue != 0) && (MAPcount != 0) )
           {
             currentStatus.mapADC = ldiv(MAPrunningValue, MAPcount).quot;
-            currentStatus.MAP = fastResize(currentStatus.mapADC, configPage1.mapMax); //Get the current MAP value
+            currentStatus.MAP = fastMap10Bit(currentStatus.mapADC, configPage1.mapMin, configPage1.mapMax); //Get the current MAP value
+            if(currentStatus.MAP < 0) { currentStatus.MAP = 0; } //Sanity check
             MAPcurRev = currentStatus.startRevolutions; //Reset the current rev count
             MAPrunningValue = 0;
             MAPcount = 0;
@@ -140,7 +143,8 @@ void readMAP()
         {
           //Reaching here means that the last cylce has completed and the MAP value should be calculated
           currentStatus.mapADC = MAPrunningValue;
-          currentStatus.MAP = fastResize(currentStatus.mapADC, configPage1.mapMax); //Get the current MAP value
+          currentStatus.MAP = fastMap10Bit(currentStatus.mapADC, configPage1.mapMin, configPage1.mapMax); //Get the current MAP value
+          if(currentStatus.MAP < 0) { currentStatus.MAP = 0; } //Sanity check
           MAPcurRev = currentStatus.startRevolutions; //Reset the current rev count
           MAPrunningValue = 1023; //Reset the latest value so the next reading will always be lower
         }
@@ -160,10 +164,10 @@ void readTPS()
   currentStatus.TPSlast = currentStatus.TPS;
   currentStatus.TPSlast_time = currentStatus.TPS_time;
   #if defined(ANALOG_ISR)
-    byte tempTPS = fastResize(AnChannel[pinTPS-A0], 255); //Get the current raw TPS ADC value and map it into a byte
+    byte tempTPS = fastMap1023toX(AnChannel[pinTPS-A0], 255); //Get the current raw TPS ADC value and map it into a byte
   #else
     analogRead(pinTPS);
-    byte tempTPS = fastResize(analogRead(pinTPS), 255); //Get the current raw TPS ADC value and map it into a byte
+    byte tempTPS = fastMap1023toX(analogRead(pinTPS), 255); //Get the current raw TPS ADC value and map it into a byte
   #endif
   currentStatus.tpsADC = ADC_FILTER(tempTPS, ADCFILTER_TPS, currentStatus.tpsADC);
   //Check that the ADC values fall within the min and max ranges (Should always be the case, but noise can cause these to fluctuate outside the defined range).
@@ -176,11 +180,12 @@ void readTPS()
 
 void readCLT()
 {
+  unsigned int tempReading;
   #if defined(ANALOG_ISR)
-    tempReading = fastResize(AnChannel[pinCLT-A0], 511); //Get the current raw CLT value
+    tempReading = fastMap1023toX(AnChannel[pinCLT-A0], 511); //Get the current raw CLT value
   #else
     tempReading = analogRead(pinCLT);
-    tempReading = fastResize(analogRead(pinCLT), 511); //Get the current raw CLT value
+    tempReading = fastMap1023toX(analogRead(pinCLT), 511); //Get the current raw CLT value
   #endif
   currentStatus.cltADC = ADC_FILTER(tempReading, ADCFILTER_CLT, currentStatus.cltADC);
   currentStatus.coolant = cltCalibrationTable[currentStatus.cltADC] - CALIBRATION_TEMPERATURE_OFFSET; //Temperature calibration values are stored as positive bytes. We subtract 40 from them to allow for negative temperatures
@@ -188,23 +193,44 @@ void readCLT()
 
 void readIAT()
 {
+  unsigned int tempReading;
   #if defined(ANALOG_ISR)
-    tempReading = fastResize(AnChannel[pinIAT-A0], 511); //Get the current raw IAT value
+    tempReading = fastMap1023toX(AnChannel[pinIAT-A0], 511); //Get the current raw IAT value
   #else
     tempReading = analogRead(pinIAT);
-    tempReading = fastResize(analogRead(pinIAT), 511); //Get the current raw IAT value
+    tempReading = fastMap1023toX(analogRead(pinIAT), 511); //Get the current raw IAT value
   #endif
   currentStatus.iatADC = ADC_FILTER(tempReading, ADCFILTER_IAT, currentStatus.iatADC);
   currentStatus.IAT = iatCalibrationTable[currentStatus.iatADC] - CALIBRATION_TEMPERATURE_OFFSET;
 }
 
+void readBaro()
+{
+  if ( configPage3.useExtBaro != 0 )
+  {
+    int tempReading;
+    // readings
+    #if defined(ANALOG_ISR_MAP)
+      tempReading = AnChannel[pinBaro-A0];
+    #else
+      tempReading = analogRead(pinBaro);
+      tempReading = analogRead(pinBaro);
+    #endif
+
+    currentStatus.baroADC = ADC_FILTER(tempReading, ADCFILTER_BARO, currentStatus.baroADC); //Very weak filter
+
+    currentStatus.baro = fastMap1023toX(currentStatus.baroADC, configPage1.mapMax); //Get the current MAP value
+  }
+}
+
 void readO2()
 {
+  unsigned int tempReading;
   #if defined(ANALOG_ISR)
-    tempReading = fastResize(AnChannel[pinO2-A0], 511); //Get the current O2 value.
+    tempReading = fastMap1023toX(AnChannel[pinO2-A0], 511); //Get the current O2 value.
   #else
     tempReading = analogRead(pinO2);
-    tempReading = fastResize(analogRead(pinO2), 511); //Get the current O2 value.
+    tempReading = fastMap1023toX(analogRead(pinO2), 511); //Get the current O2 value.
   #endif
   currentStatus.O2ADC = ADC_FILTER(tempReading, ADCFILTER_O2, currentStatus.O2ADC);
   currentStatus.O2 = o2CalibrationTable[currentStatus.O2ADC];
@@ -218,11 +244,12 @@ void readO2()
 
 void readBat()
 {
+  unsigned int tempReading;
   #if defined(ANALOG_ISR)
-    tempReading = fastResize(AnChannel[pinBat-A0], 245); //Get the current raw Battery value. Permissible values are from 0v to 24.5v (245)
+    tempReading = fastMap1023toX(AnChannel[pinBat-A0], 245); //Get the current raw Battery value. Permissible values are from 0v to 24.5v (245)
   #else
     tempReading = analogRead(pinBat);
-    tempReading = fastResize(analogRead(pinBat), 245); //Get the current raw Battery value. Permissible values are from 0v to 24.5v (245)
+    tempReading = fastMap1023toX(analogRead(pinBat), 245); //Get the current raw Battery value. Permissible values are from 0v to 24.5v (245)
   #endif
   currentStatus.battery10 = ADC_FILTER(tempReading, ADCFILTER_BAT, currentStatus.battery10);
 }
@@ -234,4 +261,4 @@ void readBat()
 void flexPulse()
  {
    ++flexCounter;
- }
+ }
diff --git a/speeduino/speeduino.ino b/speeduino/speeduino.ino
index 6be82ea5..c07d9039 100644
--- a/speeduino/speeduino.ino
+++ b/speeduino/speeduino.ino
@@ -54,6 +54,7 @@ struct config2 configPage2;
 struct config3 configPage3;
 struct config4 configPage4;
 struct config10 configPage10;
+struct config11 configPage11;
 
 int req_fuel_uS, inj_opentime_uS;
 
@@ -70,6 +71,7 @@ void (*trigger)(); //Pointer for the trigger function (Gets pointed to the relev
 void (*triggerSecondary)(); //Pointer for the secondary trigger function (Gets pointed to the relevant decoder)
 uint16_t (*getRPM)(); //Pointer to the getRPM function (Gets pointed to the relevant decoder)
 int (*getCrankAngle)(int); //Pointer to the getCrank Angle function (Gets pointed to the relevant decoder)
+void (*triggerSetEndTeeth)(); //Pointer to the triggerSetEndTeeth function of each decoder
 
 byte cltCalibrationTable[CALIBRATION_TABLE_SIZE];
 byte iatCalibrationTable[CALIBRATION_TABLE_SIZE];
@@ -85,7 +87,8 @@ unsigned long currentLoopTime; //The time the current loop started (uS)
 unsigned long previousLoopTime; //The time the previous loop started (uS)
 
 int CRANK_ANGLE_MAX = 720;
-int CRANK_ANGLE_MAX_IGN = 360, CRANK_ANGLE_MAX_INJ = 360; // The number of crank degrees that the system track over. 360 for wasted / timed batch and 720 for sequential
+int CRANK_ANGLE_MAX_IGN = 360;
+int CRANK_ANGLE_MAX_INJ = 360; // The number of crank degrees that the system track over. 360 for wasted / timed batch and 720 for sequential
 
 static byte coilHIGH = HIGH;
 static byte coilLOW = LOW;
@@ -97,6 +100,7 @@ byte deltaToothCount = 0; //The last tooth that was used with the deltaV calc
 int rpmDelta;
 byte ignitionCount;
 uint16_t fixedCrankingOverride = 0;
+int16_t lastAdvance; //Stores the previous advance figure to track changes.
 bool clutchTrigger;
 bool previousClutchTrigger;
 
@@ -152,7 +156,6 @@ void setup()
   loadConfig();
   doUpdates(); //Check if any data items need updating (Occurs ith firmware updates)
 
-  //Serial.begin(115200);
 #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) //ATmega2561 does not have Serial3
   if (configPage10.enable_canbus == 1) { Serial3.begin(115200); }
 #elif defined(CORE_STM32)
@@ -183,6 +186,10 @@ void setup()
   WUETable.xSize = 10;
   WUETable.values = configPage1.wueValues;
   WUETable.axisX = configPage2.wueBins;
+  crankingEnrichTable.valueSize = SIZE_BYTE;
+  crankingEnrichTable.xSize = 4;
+  crankingEnrichTable.values = configPage11.crankingEnrichValues;
+  crankingEnrichTable.axisX = configPage11.crankingEnrichBins;
 
   dwellVCorrectionTable.valueSize = SIZE_BYTE;
   dwellVCorrectionTable.xSize = 6;
@@ -229,22 +236,31 @@ void setup()
 
   //Lookup the current MAP reading for barometric pressure
   readMAP();
-  /*
-   * The highest sea-level pressure on Earth occurs in Siberia, where the Siberian High often attains a sea-level pressure above 105 kPa;
-   * with record highs close to 108.5 kPa.
-   * The lowest measurable sea-level pressure is found at the centers of tropical cyclones and tornadoes, with a record low of 87 kPa;
-   */
-  if ((currentStatus.MAP >= BARO_MIN) && (currentStatus.MAP <= BARO_MAX)) //Check if engine isn't running
+  //barometric reading can be taken from either an external sensor if enabled, or simply by using the initial MAP value
+  if ( configPage3.useExtBaro != 0 )
   {
-    currentStatus.baro = currentStatus.MAP;
+    readBaro();
     EEPROM.update(EEPROM_LAST_BARO, currentStatus.baro);
   }
   else
   {
-    //Attempt to use the last known good baro reading from EEPROM
-    if ((EEPROM.read(EEPROM_LAST_BARO) >= BARO_MIN) && (EEPROM.read(EEPROM_LAST_BARO) <= BARO_MAX)) //Make sure it's not invalid (Possible on first run etc)
-    { currentStatus.baro = EEPROM.read(EEPROM_LAST_BARO); } //last baro correction
-    else { currentStatus.baro = 100; } //Final fall back position.
+    /*
+     * The highest sea-level pressure on Earth occurs in Siberia, where the Siberian High often attains a sea-level pressure above 105 kPa;
+     * with record highs close to 108.5 kPa.
+     * The lowest measurable sea-level pressure is found at the centers of tropical cyclones and tornadoes, with a record low of 87 kPa;
+     */
+    if ((currentStatus.MAP >= BARO_MIN) && (currentStatus.MAP <= BARO_MAX)) //Check if engine isn't running
+    {
+      currentStatus.baro = currentStatus.MAP;
+      EEPROM.update(EEPROM_LAST_BARO, currentStatus.baro);
+    }
+    else
+    {
+      //Attempt to use the last known good baro reading from EEPROM
+      if ((EEPROM.read(EEPROM_LAST_BARO) >= BARO_MIN) && (EEPROM.read(EEPROM_LAST_BARO) <= BARO_MAX)) //Make sure it's not invalid (Possible on first run etc)
+      { currentStatus.baro = EEPROM.read(EEPROM_LAST_BARO); } //last baro correction
+      else { currentStatus.baro = 100; } //Final fall back position.
+    }
   }
 
   //Perform all initialisations
@@ -341,6 +357,7 @@ void setup()
       triggerSecondary = triggerSec_missingTooth;
       getRPM = getRPM_missingTooth;
       getCrankAngle = getCrankAngle_missingTooth;
+      triggerSetEndTeeth = triggerSetEndTeeth_missingTooth;
 
       if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
       else { attachInterrupt(triggerInterrupt, trigger, FALLING); }
@@ -354,6 +371,7 @@ void setup()
       trigger = triggerPri_BasicDistributor;
       getRPM = getRPM_BasicDistributor;
       getCrankAngle = getCrankAngle_BasicDistributor;
+      triggerSetEndTeeth = triggerSetEndTeeth_BasicDistributor;
 
       if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
       else { attachInterrupt(triggerInterrupt, trigger, FALLING); }
@@ -364,6 +382,7 @@ void setup()
       trigger = triggerPri_DualWheel;
       getRPM = getRPM_DualWheel;
       getCrankAngle = getCrankAngle_DualWheel;
+      triggerSetEndTeeth = triggerSetEndTeeth_DualWheel;
 
       if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
       else { attachInterrupt(triggerInterrupt, trigger, FALLING); }
@@ -376,6 +395,7 @@ void setup()
       trigger = triggerPri_GM7X;
       getRPM = getRPM_GM7X;
       getCrankAngle = getCrankAngle_GM7X;
+      triggerSetEndTeeth = triggerSetEndTeeth_GM7X;
 
       if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
       else { attachInterrupt(triggerInterrupt, trigger, FALLING); }
@@ -386,6 +406,7 @@ void setup()
       trigger = triggerPri_4G63;
       getRPM = getRPM_4G63;
       getCrankAngle = getCrankAngle_4G63;
+      triggerSetEndTeeth = triggerSetEndTeeth_4G63;
 
       //These may both need to change, not sure
       if(configPage2.TrigEdge == 0)
@@ -405,6 +426,7 @@ void setup()
       trigger = triggerPri_24X;
       getRPM = getRPM_24X;
       getCrankAngle = getCrankAngle_24X;
+      triggerSetEndTeeth = triggerSetEndTeeth_24X;
 
       if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
       else { attachInterrupt(triggerInterrupt, trigger, FALLING); } // Primary trigger connects to
@@ -416,6 +438,7 @@ void setup()
       trigger = triggerPri_Jeep2000;
       getRPM = getRPM_Jeep2000;
       getCrankAngle = getCrankAngle_Jeep2000;
+      triggerSetEndTeeth = triggerSetEndTeeth_Jeep2000;
 
       if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
       else { attachInterrupt(triggerInterrupt, trigger, FALLING); } // Primary trigger connects to
@@ -427,6 +450,7 @@ void setup()
       trigger = triggerPri_Audi135;
       getRPM = getRPM_Audi135;
       getCrankAngle = getCrankAngle_Audi135;
+      triggerSetEndTeeth = triggerSetEndTeeth_Audi135;
 
       if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
       else { attachInterrupt(triggerInterrupt, trigger, FALLING); }
@@ -438,6 +462,7 @@ void setup()
       trigger = triggerPri_HondaD17;
       getRPM = getRPM_HondaD17;
       getCrankAngle = getCrankAngle_HondaD17;
+      triggerSetEndTeeth = triggerSetEndTeeth_HondaD17;
 
       if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
       else { attachInterrupt(triggerInterrupt, trigger, FALLING); } // Primary trigger connects to
@@ -449,6 +474,7 @@ void setup()
       trigger = triggerPri_Miata9905;
       getRPM = getRPM_Miata9905;
       getCrankAngle = getCrankAngle_Miata9905;
+      triggerSetEndTeeth = triggerSetEndTeeth_Miata9905;
 
       //These may both need to change, not sure
       if(configPage2.TrigEdge == 0)
@@ -468,6 +494,7 @@ void setup()
       trigger = triggerPri_MazdaAU;
       getRPM = getRPM_MazdaAU;
       getCrankAngle = getCrankAngle_MazdaAU;
+      triggerSetEndTeeth = triggerSetEndTeeth_MazdaAU;
 
       if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
       else { attachInterrupt(triggerInterrupt, trigger, FALLING); } // Primary trigger connects to
@@ -479,6 +506,7 @@ void setup()
       trigger = triggerPri_DualWheel; //Is identical to the dual wheel decoder, so that is used. Same goes for the secondary below
       getRPM = getRPM_non360;
       getCrankAngle = getCrankAngle_non360;
+      triggerSetEndTeeth = triggerSetEndTeeth_Non360;
 
       if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
       else { attachInterrupt(triggerInterrupt, trigger, FALLING); }
@@ -490,6 +518,7 @@ void setup()
         trigger = triggerPri_Nissan360;
         getRPM = getRPM_Nissan360;
         getCrankAngle = getCrankAngle_Nissan360;
+        triggerSetEndTeeth = triggerSetEndTeeth_Nissan360;
 
         if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
         else { attachInterrupt(triggerInterrupt, trigger, FALLING); }
@@ -501,6 +530,7 @@ void setup()
             trigger = triggerPri_Subaru67;
             getRPM = getRPM_Subaru67;
             getCrankAngle = getCrankAngle_Subaru67;
+            triggerSetEndTeeth = triggerSetEndTeeth_Subaru67;
 
             if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
             else { attachInterrupt(triggerInterrupt, trigger, FALLING); }
@@ -512,6 +542,7 @@ void setup()
             trigger = triggerPri_Daihatsu;
             getRPM = getRPM_Daihatsu;
             getCrankAngle = getCrankAngle_Daihatsu;
+            triggerSetEndTeeth = triggerSetEndTeeth_Daihatsu;
 
             if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
             else { attachInterrupt(triggerInterrupt, trigger, FALLING); }
@@ -845,10 +876,12 @@ void setup()
 void loop()
 {
       mainLoopCount++;
+      LOOP_TIMER = TIMER_mask;
       //Check for any requets from serial. Serial operations are checked under 2 scenarios:
       // 1) Every 64 loops (64 Is more than fast enough for TunerStudio). This function is equivalent to ((loopCount % 64) == 1) but is considerably faster due to not using the mod or division operations
       // 2) If the amount of data in the serial buffer is greater than a set threhold (See globals.h). This is to avoid serial buffer overflow when large amounts of data is being sent
-      if ( ((mainLoopCount & 31) == 1) or (Serial.available() > SERIAL_BUFFER_THRESHOLD) )
+      //if ( ((mainLoopCount & 31) == 1) or (Serial.available() > SERIAL_BUFFER_THRESHOLD) )
+      if( BIT_CHECK(LOOP_TIMER, BIT_TIMER_15HZ) or (Serial.available() > SERIAL_BUFFER_THRESHOLD) )
       {
         if (Serial.available() > 0)
         {
@@ -856,56 +889,28 @@ void loop()
         }
       }
 
-#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) //ATmega2561 does not have Serial3
-      //if serial3 interface is enabled then check for serial3 requests.
-      if (configPage10.enable_canbus == 1)
-          {
-            if ( ((mainLoopCount & 31) == 1) or (Serial3.available() > SERIAL_BUFFER_THRESHOLD) )
-                {
-                  if (Serial3.available() > 0)
-                    {
-                    canCommand();
-                    }
-                }
-          }
-
-#elif defined(CORE_STM32)
-      //if can or secondary serial interface is enabled then check for requests.
-      if (configPage10.enable_canbus == 1)  //secondary serial interface enabled
-          {
-            if ( ((mainLoopCount & 31) == 1) or (Serial2.available() > SERIAL_BUFFER_THRESHOLD) )
-                {
-                  if (Serial2.available() > 0)
-                    {
-                    canCommand();
-                    }
-                }
-          }
-      else if (configPage10.enable_canbus == 2) // can module enabled
-          {
-            //check local can module
-          }
-#elif defined(CORE_TEENSY)
-      //if can or secondary serial interface is enabled then check for requests.
-      if (configPage10.enable_canbus == 1)  //secondary serial interface enabled
-          {
-            if ( ((mainLoopCount & 31) == 1) or (Serial2.available() > SERIAL_BUFFER_THRESHOLD) )
-                {
-                  if (Serial2.available() > 0)
-                    {
-                    canCommand();
-                    }
-                }
-          }
-      else if (configPage10.enable_canbus == 2) // can module enabled
-          {
-            //check local can module
-            // if ( ((mainLoopCount & 31) == 1) or (CANbus0.available())
-            //    {
-            //      CANbus0.read(rx_msg);
-            //    }
-          }
-#endif
+    //if serial3 interface is enabled then check for serial3 requests.
+    if (configPage10.enable_canbus == 1)
+    {
+      //if ( ((mainLoopCount & 31) == 1) or (CANSerial.available() > SERIAL_BUFFER_THRESHOLD) )
+      if( BIT_CHECK(LOOP_TIMER, BIT_TIMER_15HZ) or (CANSerial.available() > SERIAL_BUFFER_THRESHOLD) )
+      {
+        if (CANSerial.available() > 0)
+        {
+          canCommand();
+        }
+      }
+    }
+  #if defined(CORE_TEENSY) || defined(CORE_STM32)
+    else if (configPage10.enable_canbus == 2) // can module enabled
+    {
+      //check local can module
+      // if ( ((mainLoopCount & 31) == 1) or (CANbus0.available())
+      //    {
+      //      CANbus0.read(rx_msg);
+      //    }
+    }
+  #endif
 
     //Displays currently disabled
     // if (configPage1.displayType && (mainLoopCount & 255) == 1) { updateDisplay();}
@@ -916,7 +921,8 @@ void loop()
     if ( (timeToLastTooth < MAX_STALL_TIME) || (toothLastToothTime > currentLoopTime) ) //Check how long ago the last tooth was seen compared to now. If it was more than half a second ago then the engine is probably stopped. toothLastToothTime can be greater than currentLoopTime if a pulse occurs between getting the lastest time and doing the comparison
     {
       currentStatus.RPM = currentStatus.longRPM = getRPM(); //Long RPM is included here
-      if(fuelPumpOn == false) { digitalWrite(pinFuelPump, HIGH); fuelPumpOn = true; } //Check if the fuel pump is on and turn it on if it isn't.
+      //if(fuelPumpOn == false) { digitalWrite(pinFuelPump, HIGH); fuelPumpOn = true; } //Check if the fuel pump is on and turn it on if it isn't.
+      FUEL_PUMP_ON();
     }
     else
     {
@@ -952,8 +958,9 @@ void loop()
     //-----------------------------------------------------------------------------------------------------
     readMAP();
 
-    if ((mainLoopCount & 31) == 1) //Every 32 loops
+    if (BIT_CHECK(LOOP_TIMER, BIT_TIMER_15HZ)) //Every 32 loops
     {
+      BIT_CLEAR(TIMER_mask, BIT_TIMER_15HZ);
       readTPS(); //TPS reading to be performed every 32 loops (any faster and it can upset the TPSdot sampling time)
 
       //Check for launching/flat shift (clutch) can be done around here too
@@ -996,14 +1003,19 @@ void loop()
 
       //And check whether the tooth log buffer is ready
       if(toothHistoryIndex > TOOTH_LOG_SIZE) { BIT_SET(currentStatus.squirt, BIT_SQUIRT_TOOTHLOG1READY); }
+
+      //Most boost tends to run at about 30Hz, so placing it here ensures a new target time is fetched frequently enough
+      boostControl();
     }
-    if( (mainLoopCount & 63) == 1) //Every 64 loops
+    if( BIT_CHECK(LOOP_TIMER, BIT_TIMER_30HZ)) //Every 64 loops
     {
-      boostControl(); //Most boost tends to run at about 30Hz, so placing it here ensures a new target time is fetched frequently enough
+      //Nothing here currently
+      BIT_CLEAR(TIMER_mask, BIT_TIMER_30HZ);
     }
     //The IAT and CLT readings can be done less frequently. This still runs about 4 times per second
-    if ((mainLoopCount & 255) == 1) //Every 256 loops
+    if (BIT_CHECK(LOOP_TIMER, BIT_TIMER_4HZ)) //Every 256 loops
     {
+       BIT_CLEAR(TIMER_mask, BIT_TIMER_4HZ);
        readCLT();
        readIAT();
        readO2();
@@ -1014,15 +1026,15 @@ void loop()
           {
             if (configPage10.enable_candata_in)
               {
-                if (configPage10.caninput_sel[currentStatus.current_caninchannel])  //if current input channel is enabled
+                if (BIT_CHECK(configPage10.caninput_sel,currentStatus.current_caninchannel))  //if current input channel bit is enabled
                   {
-                    sendCancommand(2,0,0,0,configPage10.caninput_param_group[currentStatus.current_caninchannel]);    //send an R command for data from paramgroup[currentStatus.current_caninchannel]
+                    sendCancommand(2,0,currentStatus.current_caninchannel,0,((configPage10.caninput_param_group[currentStatus.current_caninchannel]&2047)+256));    //send an R command for data from paramgroup[currentStatus.current_caninchannel]
                   }
                 else
                   {
-                    if (currentStatus.current_caninchannel <= 6)
+                    if (currentStatus.current_caninchannel < 15)
                         {
-                          currentStatus.current_caninchannel++;   //step to next input channel if under 9
+                          currentStatus.current_caninchannel++;   //step to next input channel if under 15
                         }
                     else
                         {
@@ -1035,26 +1047,26 @@ void loop()
       //if serial3io is enabled then check for serial3 requests.
             if (configPage10.enable_candata_in)
               {
-                if (configPage10.caninput_sel[currentStatus.current_caninchannel])  //if current input channel is enabled
+                if (BIT_CHECK(configPage10.caninput_sel,currentStatus.current_caninchannel))  //if current input channel is enabled
                   {
                     if (configPage10.enable_canbus == 1)  //can via secondary serial
                     {
-                      sendCancommand(2,0,0,0,configPage10.caninput_param_group[currentStatus.current_caninchannel]);    //send an R command for data from paramgroup[currentStatus.current_caninchannel]
+                      sendCancommand(2,0,currentStatus.current_caninchannel,0,((configPage10.caninput_param_group[currentStatus.current_caninchannel]&2047)+256));    //send an R command for data from paramgroup[currentStatus.current_caninchannel]
                     }
                     else if (configPage10.enable_canbus == 2) // can via internal can module
                     {
-                      sendCancommand(3,configPage10.speeduino_tsCanId,0,0,configPage10.caninput_param_group[currentStatus.current_caninchannel]);    //send via localcanbus the command for data from paramgroup[currentStatus.current_caninchannel]
+                      sendCancommand(3,configPage10.speeduino_tsCanId,currentStatus.current_caninchannel,0,configPage10.caninput_param_group[currentStatus.current_caninchannel]);    //send via localcanbus the command for data from paramgroup[currentStatus.current_caninchannel]
                     }
                   }
                 else
                   {
-                    if (currentStatus.current_caninchannel <= 6)
+                    if (currentStatus.current_caninchannel < 15)
                         {
-                          currentStatus.current_caninchannel++;   //step to next input channel if under 9
+                          currentStatus.current_caninchannel++;   //step to next input channel if under 15
                         }
                     else
                         {
-                          currentStatus.current_caninchannel = 0;   //reset input channel back to 1
+                          currentStatus.current_caninchannel = 0;   //reset input channel back to 0
                         }
                   }
               }
@@ -1063,6 +1075,13 @@ void loop()
        vvtControl();
        idleControl(); //Perform any idle related actions. Even at higher frequencies, running 4x per second is sufficient.
     }
+    if (BIT_CHECK(LOOP_TIMER, BIT_TIMER_1HZ)) //Every 1024 loops (Approx. 1 per second)
+    {
+      //Approx. once per second
+      readBaro();
+      BIT_CLEAR(TIMER_mask, BIT_TIMER_1HZ);
+    }
+
     if(configPage4.iacAlgorithm == IAC_ALGORITHM_STEP_OL || configPage4.iacAlgorithm == IAC_ALGORITHM_STEP_CL) { idleControl(); } //Run idlecontrol every loop for stepper idle.
 
     //Always check for sync
@@ -1070,6 +1089,7 @@ void loop()
     if (currentStatus.hasSync && (currentStatus.RPM > 0))
     {
         if(currentStatus.startRevolutions >= configPage2.StgCycles)  { ignitionOn = true; fuelOn = true;} //Enable the fuel and ignition, assuming staging revolutions are complete
+        else { ignitionOn = false; fuelOn = false;}
         //If it is, check is we're running or cranking
         if(currentStatus.RPM > ((unsigned int)configPage2.crankRPM * 100)) //Crank RPM stored in byte as RPM / 100
         {
@@ -1094,7 +1114,7 @@ void loop()
       //Begin the fuel calculation
       //Calculate an injector pulsewidth from the VE
       currentStatus.corrections = correctionsFuel();
-      //currentStatus.corrections = 100;
+      lastAdvance = currentStatus.advance; //Store the previous advance value
       if (configPage1.algorithm == 0) //Check which fuelling algorithm is being used
       {
         //Speed Density
@@ -1280,7 +1300,8 @@ void loop()
 
       //Calculate start angle for each channel
       //1 cylinder (Everyone gets this)
-      ignition1StartAngle = CRANK_ANGLE_MAX_IGN - currentStatus.advance - dwellAngle; // 360 - desired advance angle - number of degrees the dwell will take
+      ignition1EndAngle = CRANK_ANGLE_MAX_IGN - currentStatus.advance;
+      ignition1StartAngle = ignition1EndAngle - dwellAngle; // 360 - desired advance angle - number of degrees the dwell will take
       if(ignition1StartAngle < 0) {ignition1StartAngle += CRANK_ANGLE_MAX_IGN;}
 
       //This test for more cylinders and do the same thing
@@ -1288,40 +1309,50 @@ void loop()
       {
         //2 cylinders
         case 2:
-          ignition2StartAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance - dwellAngle;
+          ignition2EndAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
+          ignition2StartAngle = ignition2EndAngle - dwellAngle;
           if(ignition2StartAngle > CRANK_ANGLE_MAX_IGN) {ignition2StartAngle -= CRANK_ANGLE_MAX_IGN;}
           break;
         //3 cylinders
         case 3:
-          ignition2StartAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance - dwellAngle;
+          ignition2EndAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
+          ignition2StartAngle = ignition2EndAngle - dwellAngle;
           if(ignition2StartAngle > CRANK_ANGLE_MAX_IGN) {ignition2StartAngle -= CRANK_ANGLE_MAX_IGN;}
+          ignition3EndAngle = channel3IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
           ignition3StartAngle = channel3IgnDegrees + 360 - currentStatus.advance - dwellAngle;
           if(ignition3StartAngle > CRANK_ANGLE_MAX_IGN) {ignition3StartAngle -= CRANK_ANGLE_MAX_IGN;}
           break;
         //4 cylinders
         case 4:
-          ignition2StartAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance - dwellAngle;
+          ignition2EndAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
+          ignition2StartAngle = ignition2EndAngle - dwellAngle;
           if(ignition2StartAngle > CRANK_ANGLE_MAX_IGN) {ignition2StartAngle -= CRANK_ANGLE_MAX_IGN;}
           if(ignition2StartAngle < 0) {ignition2StartAngle += CRANK_ANGLE_MAX_IGN;}
 
           if(configPage2.sparkMode == IGN_MODE_SEQUENTIAL)
           {
-            ignition3StartAngle = channel3IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance - dwellAngle;
+            ignition3EndAngle = channel3IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
+            ignition3StartAngle = ignition3EndAngle - dwellAngle;
             if(ignition3StartAngle > CRANK_ANGLE_MAX_IGN) {ignition3StartAngle -= CRANK_ANGLE_MAX_IGN;}
-            ignition4StartAngle = channel4IgnDegrees + CRANK_ANGLE_MAX - currentStatus.advance - dwellAngle;
+
+            ignition4EndAngle = channel4IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
+            ignition4StartAngle = ignition4EndAngle - dwellAngle;
             if(ignition4StartAngle > CRANK_ANGLE_MAX_IGN) {ignition4StartAngle -= CRANK_ANGLE_MAX_IGN;}
           }
           break;
         //5 cylinders
         case 5:
-          ignition2StartAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance - dwellAngle;
+          ignition2EndAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
+          ignition2StartAngle = ignition2EndAngle - dwellAngle;
           if(ignition2StartAngle > CRANK_ANGLE_MAX_IGN) {ignition2StartAngle -= CRANK_ANGLE_MAX_IGN;}
           if(ignition2StartAngle < 0) {ignition2StartAngle += CRANK_ANGLE_MAX_IGN;}
 
-          ignition3StartAngle = channel3IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance - dwellAngle;
+          ignition3EndAngle = channel3IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
+          ignition3StartAngle = ignition3EndAngle - dwellAngle;
           if(ignition3StartAngle > CRANK_ANGLE_MAX_IGN) {ignition3StartAngle -= CRANK_ANGLE_MAX_IGN;}
 
-          ignition4StartAngle = channel4IgnDegrees + CRANK_ANGLE_MAX - currentStatus.advance - dwellAngle;
+          ignition4EndAngle = channel4IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
+          ignition4StartAngle = ignition4EndAngle - dwellAngle;
           if(ignition4StartAngle > CRANK_ANGLE_MAX_IGN) {ignition4StartAngle -= CRANK_ANGLE_MAX_IGN;}
 
           ignition5StartAngle = channel5IgnDegrees + CRANK_ANGLE_MAX - currentStatus.advance - dwellAngle;
@@ -1330,18 +1361,26 @@ void loop()
           break;
         //6 cylinders
         case 6:
-          ignition2StartAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance - dwellAngle;
+          ignition2EndAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
+          ignition2StartAngle = ignition2EndAngle - dwellAngle;
           if(ignition2StartAngle > CRANK_ANGLE_MAX_IGN) {ignition2StartAngle -= CRANK_ANGLE_MAX_IGN;}
-          ignition3StartAngle = channel3IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance - dwellAngle;
+
+          ignition3EndAngle = channel3IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
+          ignition3StartAngle = ignition3EndAngle - dwellAngle;
           if(ignition3StartAngle > CRANK_ANGLE_MAX_IGN) {ignition3StartAngle -= CRANK_ANGLE_MAX_IGN;}
           break;
         //8 cylinders
         case 8:
-          ignition2StartAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance - dwellAngle;
+          ignition2EndAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
+          ignition2StartAngle = ignition2EndAngle - dwellAngle;
           if(ignition2StartAngle > CRANK_ANGLE_MAX_IGN) {ignition2StartAngle -= CRANK_ANGLE_MAX_IGN;}
-          ignition3StartAngle = channel3IgnDegrees + CRANK_ANGLE_MAX - currentStatus.advance - dwellAngle;
+
+          ignition3EndAngle = channel3IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
+          ignition3StartAngle = ignition3EndAngle - dwellAngle;
           if(ignition3StartAngle > CRANK_ANGLE_MAX_IGN) {ignition3StartAngle -= CRANK_ANGLE_MAX_IGN;}
-          ignition4StartAngle = channel4IgnDegrees + CRANK_ANGLE_MAX - currentStatus.advance - dwellAngle;
+
+          ignition4EndAngle = channel4IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
+          ignition4StartAngle = ignition4EndAngle - dwellAngle;
           if(ignition4StartAngle > CRANK_ANGLE_MAX_IGN) {ignition4StartAngle -= CRANK_ANGLE_MAX_IGN;}
           break;
 
@@ -1349,6 +1388,9 @@ void loop()
         default:
           break;
       }
+      //If ignition timing is being tracked per tooth, perform the calcs to get the end teeth
+      //This only needs to be run if the advance figure has changed, otherwise the end teeth will still be the same
+      if( (configPage1.perToothIgn == true) && (lastAdvance != currentStatus.advance) ) { triggerSetEndTeeth(); }
 
       //***********************************************************************************************
       //| BEGIN FUEL SCHEDULES
diff --git a/speeduino/storage.h b/speeduino/storage.h
index 93df9c5b..6d176a7e 100644
--- a/speeduino/storage.h
+++ b/speeduino/storage.h
@@ -48,7 +48,8 @@ Current layout of EEPROM data (Version 3) is as follows (All sizes are in bytes)
 | 1441  |2    | X and Y size4                       |
 | 1443  |36   | PAGE 9 MAP4                         |
 | 1479  |6    | X and Y Bins4                       |
-| 1500  |128  | CANBUS config and data              |
+| 1500  |128  | CANBUS config and data (Table 10_)  |
+| 1628  |192  | Table 11 - General settings         |
 |                                                   |
 | 2559  |512  | Calibration data (O2)               |
 | 3071  |512  | Calibration data (IAT)              |
@@ -115,6 +116,8 @@ Current layout of EEPROM data (Version 3) is as follows (All sizes are in bytes)
 #define EEPROM_CONFIG9_YBINS4 1485
 #define EEPROM_CONFIG10_START 1500
 #define EEPROM_CONFIG10_END   1628
+#define EEPROM_CONFIG11_START 1628
+#define EEPROM_CONFIG11_END   1820
 
 //Calibration data is stored at the end of the EEPROM (This is in case any further calibration tables are needed as they are large blocks)
 #define EEPROM_LAST_BARO      2558
@@ -122,4 +125,4 @@ Current layout of EEPROM data (Version 3) is as follows (All sizes are in bytes)
 #define EEPROM_CALIBRATION_IAT 3071
 #define EEPROM_CALIBRATION_CLT 3583
 
-#endif // STORAGE_H
+#endif // STORAGE_H
diff --git a/speeduino/storage.ino b/speeduino/storage.ino
index 7db0ea66..fcb1ca43 100644
--- a/speeduino/storage.ino
+++ b/speeduino/storage.ino
@@ -271,8 +271,19 @@ void writeConfig()
   {
     if(EEPROM.read(x) != *(pnt_configPage + byte(x - EEPROM_CONFIG10_START))) { EEPROM.write(x, *(pnt_configPage + byte(x - EEPROM_CONFIG10_START))); }
   }
-    //*********************************************************************************************************************************************************************************
 
+  //*********************************************************************************************************************************************************************************
+
+  /*---------------------------------------------------
+  | Config page 11 (See storage.h for data layout)
+  | 192 byte long config table
+  -----------------------------------------------------*/
+  pnt_configPage = (byte *)&configPage11; //Create a pointer to Page 11 in memory
+  //As there are no 3d tables in this page, all 192 bytes can simply be read in
+  for(int x=EEPROM_CONFIG11_START; x<EEPROM_CONFIG11_END; x++)
+  {
+    if(EEPROM.read(x) != *(pnt_configPage + byte(x - EEPROM_CONFIG11_START))) { EEPROM.write(x, *(pnt_configPage + byte(x - EEPROM_CONFIG11_START))); }
+  }
 }
 
 void loadConfig()
@@ -476,6 +487,14 @@ void loadConfig()
 
   //*********************************************************************************************************************************************************************************
 
+  //CONFIG PAGE (11)
+  pnt_configPage = (byte *)&configPage11; //Create a pointer to Page 11 in memory
+  //All 192 bytes can simply be pulled straight from the configTable
+  for(int x=EEPROM_CONFIG11_START; x<EEPROM_CONFIG11_END; x++)
+  {
+    *(pnt_configPage + byte(x - EEPROM_CONFIG11_START)) = EEPROM.read(x);
+  }
+
 }
 
 /*
@@ -519,5 +538,3 @@ void writeCalibration()
   }
 
 }
-
-
diff --git a/speeduino/table.h b/speeduino/table.h
index 27845a34..5652bef1 100644
--- a/speeduino/table.h
+++ b/speeduino/table.h
@@ -23,7 +23,8 @@ struct table2D {
   int *axisX16;
 
   //Store the last X and Y coordinates in the table. This is used to make the next check faster
-  int lastXMax, lastXMin;
+  int lastXMax;
+  int lastXMin;
 };
 
 void table2D_setSize(struct table2D targetTable, byte newSize);
diff --git a/speeduino/table.ino b/speeduino/table.ino
index 0c019ff6..1e8dc663 100644
--- a/speeduino/table.ino
+++ b/speeduino/table.ino
@@ -103,6 +103,7 @@ int table2D_getValue(struct table2D *fromTable, int X_in)
               {
                 returnValue = fromTable->values[x]; //Simply return the coresponding value
                 valueFound = true;
+                break;
               }
               else
               {
@@ -143,6 +144,7 @@ int table2D_getValue(struct table2D *fromTable, int X_in)
               {
                 returnValue = fromTable->values16[x]; //Simply return the coresponding value
                 valueFound = true;
+                break;
               }
               else
               {
@@ -366,7 +368,7 @@ int get3DTableValue(struct table3D *fromTable, int Y_in, int X_in)
     int D = fromTable->values[yMax][xMax];
 
     //Check that all values aren't just the same (This regularly happens with things like the fuel trim maps)
-    if( (A == D) ) { tableResult = A; }
+    if( (A == B) && (A == C) && (A == D) ) { tableResult = A; }
     else
     {
       //Create some normalised position values
@@ -374,13 +376,29 @@ int get3DTableValue(struct table3D *fromTable, int Y_in, int X_in)
 
 
       //Initial check incase the values were hit straight on
+
       long p = (long)X - xMinValue;
       if (xMaxValue == xMinValue) { p = (p << 8); }  //This only occurs if the requested X value was equal to one of the X axis bins
       else { p = ( (p << 8) / (xMaxValue - xMinValue) ); } //This is the standard case
 
-      long q = (long)Y - yMinValue;
-      if (yMaxValue == yMinValue) { q = (q << 8); }
-      else { q = 256 - ( (q << 8) / (yMinValue - yMaxValue) ); }
+      long q;
+      if (yMaxValue == yMinValue)
+      {
+        q = (long)Y - yMinValue;
+        q = (q << 8);
+      }
+      //Standard case
+      else
+      {
+        q = long(Y) - yMaxValue;
+        q = 256 - ( (q << 8) / (yMinValue - yMaxValue) );
+      }
+
+/*
+      long q;
+      if (yMaxValue == yMinValue) { q = ((long)(Y - yMinValue) << 8); }
+      else { q = 256 - (((long)(Y - yMaxValue) << 8) / (yMinValue - yMaxValue)); }
+      */
 
       int m = ((256-p) * (256-q)) >> 8;
       int n = (p * (256-q)) >> 8;
@@ -390,91 +408,87 @@ int get3DTableValue(struct table3D *fromTable, int Y_in, int X_in)
     }
     return tableResult;
 }
-
 /* Executed a benchmark on all options and this is the results
- * @Mega:   Stadard:463028 92 |FP Math:850392 92.57 |Clean code:618308 92  , Number of loops:5000
- * @STM32F1: Stadard:21449 92 |FP Math:125707 92.57  |Clean code:11634 92  , Number of loops:5000
- * @STM32F4: Stadard:7123 92  |FP Math:16503 92.57  |Clean code:4618 92    , Number of loops:5000
- * @STM32F1: Stadard:13000  loops/S |Clean code:15700 loops/S @8000RPM ~20.8% faster
-
+ * Stadard:226224 91 |FP Math:32240 91.89 |Clean code:34056 91, Number of loops:2500
+ *
 //This function pulls a value from a 3D table given a target for X and Y coordinates.
 //It performs a 2D linear interpolation as descibred in: http://www.megamanual.com/v22manual/ve_tuner.pdf
 float get3DTableValueF(struct table3D *fromTable, int Y, int X)
 {
+  float m, n, o ,p, q, r;
   byte xMin, xMax;
   byte yMin, yMax;
   int yMaxValue, yMinValue;
   int xMaxValue, xMinValue;
-  float tableResult = 0;
 
-  xMin = fromTable->lastXMin;
-  yMin = fromTable->lastYMin;
-  if(xMin == 0) { xMin = fromTable->xSize-1; }
-  if(yMin == 0) { yMin = fromTable->ySize-1; }
+  if(fromTable->lastXMin==0) {fromTable->lastXMin = fromTable->xSize-1;}
+  else {xMin = fromTable->lastXMin;}
+  if(fromTable->lastYMin==0) {fromTable->lastYMin = fromTable->ySize-1;}
+  else {yMin = fromTable->lastYMin;}
+  //yMin = fromTable->lastYMin;
+
+  if(xMin>fromTable->xSize-1)
+  {
+    fromTable->lastXMin = fromTable->xSize-1;
+    xMin = fromTable->xSize-1;
+  }
+  if(yMin>fromTable->ySize-1)
+  {
+    fromTable->lastYMin = fromTable->ySize-1;
+    yMin = fromTable->ySize-1;
+  }
 
   do  //RPM axis
   {
-    if(X >= fromTable->axisX[xMin]) {break;}
+    if(X>=fromTable->axisX[xMin]) {break;}
     xMin--;
   }while(1);
-  
+  fromTable->lastXMin = xMin + 1;
   do  //MAP axis
   {
-    if(Y <= fromTable->axisY[yMin]) {break;}
+    if(Y<=fromTable->axisY[yMin]) {break;}
     yMin--;
   }while(1);
-  
+  fromTable->lastYMin = yMin + 1;
+
   xMax = xMin + 1;
   yMax = yMin + 1;
-  if (xMax > fromTable->xSize-1)  //Overflow protection
+  if (xMax>fromTable->xSize-1)  //Overflow protection
   {
     xMax = fromTable->xSize-1;
     xMin = xMax - 1;
   }
-  fromTable->lastXMin = xMin + 1;
-
-  if (yMax > fromTable->ySize-1)  //Overflow protection
+  if (yMax>fromTable->ySize-1)  //Overflow protection
   {
     yMax = fromTable->ySize-1;
     yMin = yMax - 1;
   }
-  fromTable->lastYMin = yMin + 1;
+
+  yMaxValue = fromTable->axisY[yMax];
+  yMinValue = fromTable->axisY[yMin];
+  xMaxValue = fromTable->axisX[xMax];
+  xMinValue = fromTable->axisX[xMin];
 
   int A = fromTable->values[yMin][xMin];
+  int B = fromTable->values[yMin][xMax];
+  int C = fromTable->values[yMax][xMin];
   int D = fromTable->values[yMax][xMax];
-  if(A == D) { tableResult = A; }
-  else
-  {
-    int B = fromTable->values[yMin][xMax];
-    int C = fromTable->values[yMax][xMin];
 
-    yMaxValue = fromTable->axisY[yMax];
-    yMinValue = fromTable->axisY[yMin];
-    xMaxValue = fromTable->axisX[xMax];
-    xMinValue = fromTable->axisX[xMin]; 
-     
-    float m, n, o ,p, q, r;
-    p = float(X - xMinValue) / (xMaxValue - xMinValue); //(RPM - RPM[1])/(RPM[2]- RPM[1])
-    q = float(Y - yMinValue) / (yMaxValue - yMinValue); //(MAP - MAP[1])/(MAP[2]- MAP[1])
+  p = float(X - xMinValue) / (xMaxValue - xMinValue); //(RPM - RPM[1])/(RPM[2]- RPM[1])
+  q = float(Y - yMinValue) / (yMaxValue - yMinValue); //(MAP - MAP[1])/(MAP[2]- MAP[1])
 
-    m = (1.0-p) * (1.0-q);
-    n = p * (1-q);
-    o = (1-p) * q;
-    r = p * q;
-    tableResult = ( (A * m) + (B * n) + (C * o) + (D * r) );
-  }
-  return tableResult;
+  m = (1.0-p) * (1.0-q);
+  n = p * (1-q);
+  o = (1-p) * q;
+  r = p * q;
+
+  return ( (A * m) + (B * n) + (C * o) + (D * r) );
 }
-
-//This function pulls a value from a 3D table given a target for X and Y coordinates.
-//It performs a 2D linear interpolation as descibred in: http://www.megamanual.com/v22manual/ve_tuner.pdf
+*/
 int get3DTableValueS(struct table3D *fromTable, int Y, int X)
 {
   byte xMin, xMax;
   byte yMin, yMax;
-  long p, q;
-  int yMaxValue, yMinValue;
-  int xMaxValue, xMinValue;
   int tableResult = 0;
 
   xMin = fromTable->lastXMin;
@@ -484,54 +498,94 @@ int get3DTableValueS(struct table3D *fromTable, int Y, int X)
 
   do  //RPM axis
   {
-    if(X >= fromTable->axisX[xMin]) {break;}
+    if(X > fromTable->axisX[xMin])
+    {
+      if (xMin == (fromTable->xSize-1) ) { xMax = xMin; }  //Overflow protection
+      else { xMax = xMin + 1; }
+      break;
+    }
+    else if(X == fromTable->axisX[xMin])
+    {
+      xMax = xMin;
+      break;
+    }
     xMin--;
-  }while(1);
-  
+  }while(true);
+
   do  //MAP axis
   {
-    if(Y <= fromTable->axisY[yMin]) {break;}
+    if(Y < fromTable->axisY[yMin])
+    {
+      if (yMin == (fromTable->ySize-1) ) { yMax = yMin; }  //Overflow protection
+      else { yMax = yMin + 1; }
+      break;
+    }
+    else if(Y == fromTable->axisY[yMin])
+    {
+      yMax = yMin;
+      break;
+    }
     yMin--;
-  }while(1);
-  
-  xMax = xMin + 1;
-  yMax = yMin + 1;
-  if (xMax > fromTable->xSize-1)  //Overflow protection
-  {
-    xMax = fromTable->xSize-1;
-    xMin = xMax - 1;
-  }
-  fromTable->lastXMin = xMin + 1;
+  }while(true);
 
-  if (yMax > fromTable->ySize-1)  //Overflow protection
-  {
-    yMax = fromTable->ySize-1;
-    yMin = yMax - 1;
-  }
-  fromTable->lastYMin = yMin + 1;
+  fromTable->lastXMin = xMax;
+  fromTable->lastYMin = yMax;
+
+  //Serial.print("xMin, xMax ");Serial.print(xMin);Serial.print(",");Serial.println(xMax);
+  //Serial.print("yMin, yMax ");Serial.print(yMin);Serial.print(",");Serial.println(yMax);
 
   int A = fromTable->values[yMin][xMin];
-  int D = fromTable->values[yMax][xMax];
-  if(A == D) { tableResult = A; }
+  if((xMin == xMax) && (yMin == yMax)) { tableResult = A; }
+  else if(xMin == xMax) // Simple 2D
+  {
+    int D = fromTable->values[yMax][xMax];
+    int yMaxValue = fromTable->axisY[yMax];
+    int yMinValue = fromTable->axisY[yMin];
+    long q = (long)Y - yMinValue;
+    q = ((q << 6)  * (D - A)) / (yMaxValue - yMinValue);
+    tableResult = A + (q >> 6);
+  }
+  else if(yMin == yMax) // Simple 2D
+  {
+    int D = fromTable->values[yMax][xMax];
+    int xMaxValue = fromTable->axisX[xMax];
+    int xMinValue = fromTable->axisX[xMin];
+    long q = (long)X - xMinValue;
+    q = ((q << 6) * (D - A)) / (xMaxValue - xMinValue);
+    tableResult = A + (q >> 6);
+  }
   else
   {
     int B = fromTable->values[yMin][xMax];
     int C = fromTable->values[yMax][xMin];
+    int D = fromTable->values[yMax][xMax];
 
-    yMaxValue = fromTable->axisY[yMax];
-    yMinValue = fromTable->axisY[yMin];
-    xMaxValue = fromTable->axisX[xMax];
-    xMinValue = fromTable->axisX[xMin]; 
+    int yMaxValue = fromTable->axisY[yMax];
+    int yMinValue = fromTable->axisY[yMin];
+    int xMaxValue = fromTable->axisX[xMax];
+    int xMinValue = fromTable->axisX[xMin];
 
-    p = ((long)(X - xMinValue) << 8) / (xMaxValue - xMinValue); //(RPM - RPM[1])/(RPM[2]- RPM[1])
-    q = 256 - (((long)(Y - yMaxValue) << 8) / (yMinValue - yMaxValue)); //(MAP - MAP[2])/(MAP[2]- MAP[1])
+    long p = (long)X - xMinValue;
+    if (xMaxValue == xMinValue) { p = (p << 8); }  //This only occurs if the requested X value was equal to one of the X axis bins
+    else { p = ( (p << 8) / (xMaxValue - xMinValue) ); } //This is the standard case
+
+    long q;
+    if (yMaxValue == yMinValue)
+    {
+      q = (long)Y - yMinValue;
+      q = (q << 8);
+    }
+    else
+    {
+      q = long(Y) - yMaxValue;
+      q = 256 - ( (q << 8) / (yMinValue - yMaxValue) );
+    }
 
     int m = ((256-p) * (256-q)) >> 8;
     int n = (p * (256-q)) >> 8;
     int o = ((256-p) * q) >> 8;
     int r = (p * q) >> 8;
-    tableResult = ( (A * m) + (B * n) + (C * o) + (D * r) ) >> 8; 
+    tableResult = ( (A * m) + (B * n) + (C * o) + (D * r) ) >> 8;
   }
-  return tableResult;  
+  return tableResult;
 }
-*/
diff --git a/speeduino/timers.h b/speeduino/timers.h
index c0981750..a6bb325a 100644
--- a/speeduino/timers.h
+++ b/speeduino/timers.h
@@ -19,8 +19,10 @@ Hence we will preload the timer with 131 cycles to leave 125 until overflow (1ms
 #ifndef TIMERS_H
 #define TIMERS_H
 
-volatile int loop100ms;
-volatile int loop250ms;
+volatile byte loop33ms;
+volatile byte loop66ms;
+volatile byte loop100ms;
+volatile byte loop250ms;
 volatile int loopSec;
 
 volatile unsigned int dwellLimit_uS;
diff --git a/speeduino/timers.ino b/speeduino/timers.ino
index 35455d1c..fd3f8225 100644
--- a/speeduino/timers.ino
+++ b/speeduino/timers.ino
@@ -44,9 +44,10 @@ void initialiseTimers()
   // Set up an interrupt
   Timer4.setMode(1, TIMER_OUTPUT_COMPARE);
   Timer4.attachInterrupt(1, oneMSInterval);
-  Timer4.resume();
+  Timer4.resume(); //Start Timer
 #endif
 
+  pinMode(LED_BUILTIN, OUTPUT); //pinMode(13, OUTPUT);  
   dwellLimit_uS = (1000 * configPage2.dwellLimit);
   lastRPM_100ms = 0;
 }
@@ -62,6 +63,8 @@ void oneMSInterval() //Most ARM chips can simply call a function
 {
 
   //Increment Loop Counters
+  loop33ms++;
+  loop66ms++;
   loop100ms++;
   loop250ms++;
   loopSec++;
@@ -78,11 +81,27 @@ void oneMSInterval() //Most ARM chips can simply call a function
   if(ignitionSchedule4.Status == RUNNING) { if( (ignitionSchedule4.startTime < targetOverdwellTime) && (configPage2.useDwellLim) ) { endCoil4Charge(); } }
   if(ignitionSchedule5.Status == RUNNING) { if( (ignitionSchedule5.startTime < targetOverdwellTime) && (configPage2.useDwellLim) ) { endCoil5Charge(); } }
 
+  //15Hz loop
+  if (loop66ms == 66)
+  {
+    loop66ms = 0;
+    BIT_SET(TIMER_mask, BIT_TIMER_15HZ);
+  }
+
+  //30Hz loop
+  if (loop33ms == 33)
+  {
+    loop33ms = 0;
+    BIT_SET(TIMER_mask, BIT_TIMER_30HZ);
+  }
+  
   //Loop executed every 100ms loop
   //Anything inside this if statement will run every 100ms.
   if (loop100ms == 100)
   {
     loop100ms = 0; //Reset counter
+    BIT_SET(TIMER_mask, BIT_TIMER_10HZ);
+    digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
 
     currentStatus.rpmDOT = (currentStatus.RPM - lastRPM_100ms) * 10; //This is the RPM per second that the engine has accelerated/decelleratedin the last loop
     lastRPM_100ms = currentStatus.RPM; //Record the current RPM for next calc
@@ -93,6 +112,7 @@ void oneMSInterval() //Most ARM chips can simply call a function
   if (loop250ms == 250)
   {
     loop250ms = 0; //Reset Counter.
+    BIT_SET(TIMER_mask, BIT_TIMER_4HZ);
     #if defined(CORE_AVR)
       //Reset watchdog timer (Not active currently)
       //wdt_reset();
@@ -103,7 +123,7 @@ void oneMSInterval() //Most ARM chips can simply call a function
   if (loopSec == 1000)
   {
     loopSec = 0; //Reset counter.
-
+    BIT_SET(TIMER_mask, BIT_TIMER_1HZ);
     dwellLimit_uS = (1000 * configPage2.dwellLimit); //Update uS value incase setting has changed
     if ( configPage2.ignCranklock && BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK)) { dwellLimit_uS = dwellLimit_uS * 3; } //Make sure the overdwell doesn't clobber the fixed ignition cranking if enabled.
 
diff --git a/speeduino/updates.ino b/speeduino/updates.ino
index b8044392..96ddee79 100644
--- a/speeduino/updates.ino
+++ b/speeduino/updates.ino
@@ -8,7 +8,7 @@
 
 void doUpdates()
 {
-  #define CURRENT_DATA_VERSION    3
+  #define CURRENT_DATA_VERSION    4
 
   //May 2017 firmware introduced a -40 offset on the ignition table. Update that table to +40
   if(EEPROM.read(EEPROM_DATA_VERSION) == 2)
@@ -23,6 +23,19 @@ void doUpdates()
     writeConfig();
     EEPROM.write(EEPROM_DATA_VERSION, 3);
   }
+  //June 2017 required the forced addition of some CAN values to avoid weird errors
+  if(EEPROM.read(EEPROM_DATA_VERSION) == 3)
+  {
+    configPage10.speeduino_tsCanId = 0;
+    configPage10.true_address = 256;
+    configPage10.realtime_base_address = 336;
+
+    //There was a bad value in the May base tune for the spark duration setting, fix it here if it's a problem
+    if(configPage2.sparkDur == 255) { configPage2.sparkDur = 10; }
+
+    writeConfig();
+    EEPROM.write(EEPROM_DATA_VERSION, 4);
+  }
 
   //Final check is always for 255 and 0 (Brand new arduino)
   if( (EEPROM.read(EEPROM_DATA_VERSION) == 0) || (EEPROM.read(EEPROM_DATA_VERSION) == 255) )
diff --git a/speeduino/utils.h b/speeduino/utils.h
index b258be60..d99afc9c 100644
--- a/speeduino/utils.h
+++ b/speeduino/utils.h
@@ -1,15 +1,55 @@
 /*
 These are some utility functions and variables used through the main code
-*/ 
+*/
 #ifndef UTILS_H
 #define UTILS_H
 
 #include <Arduino.h>
 
-unsigned int freeRam ();
+uint16_t freeRam ();
 void setPinMapping(byte boardID);
 unsigned int PW();
 unsigned int PW_SD();
 unsigned int PW_AN();
 
+#if defined(CORE_STM32)
+  //STM32F1/variants/.../board.cpp
+  #if defined (STM32F4)
+    #define A0  PA0
+    #define A1  PA1
+    #define A2  PA2
+    #define A3  PA3
+    #define A4  PA4
+    #define A5  PA5
+    #define A6  PA6
+    #define A7  PA7
+    #define A8  PB0
+    #define A9  PB1
+    #define A10  PC0
+    #define A11  PC1
+    #define A12  PC2
+    #define A13  PC3
+    #define A14  PC4
+    #define A15  PC5
+  #else
+    #define A0  PB0
+    #define A1  PA7
+    #define A2  PA6
+    #define A3  PA5
+    #define A4  PA4
+    #define A5  PA3
+    #define A6  PA2
+    #define A7  PA1
+    #define A8  PA0
+    //STM32F1 have only 9 12bit adc
+    #define A9  PB0
+    #define A10  PA7
+    #define A11  PA6
+    #define A12  PA5
+    #define A13  PA4
+    #define A14  PA3
+    #define A15  PA2
+  #endif
+#endif
+
 #endif // UTILS_H
diff --git a/speeduino/utils.ino b/speeduino/utils.ino
index 5ffb43d8..a982f8f3 100644
--- a/speeduino/utils.ino
+++ b/speeduino/utils.ino
@@ -7,15 +7,18 @@
 
 /*
   Returns how much free dynamic memory exists (between heap and stack)
+  This function is one big MISRA violation. MISRA advisories forbid directly poking at memory addresses, however there is no other way of determining heap size on embedded systems.
 */
 #include "utils.h"
 
-unsigned int freeRam ()
+uint16_t freeRam ()
 {
 #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
   extern int __heap_start, *__brkval;
-  int v;
-  return (int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval);
+  uint16_t v;
+
+  return (uint16_t) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval);
+
 #elif defined(CORE_TEENSY)
   uint32_t stackTop;
   uint32_t heapTop;
@@ -33,52 +36,13 @@ unsigned int freeRam ()
 #elif defined(CORE_STM32)
   char top = 't';
   return &top - reinterpret_cast<char*>(sbrk(0));
+
 #endif
 }
 
 void setPinMapping(byte boardID)
 {
   //This is dumb, but it'll do for now to get things compiling
-  #if defined(CORE_STM32)
-    //STM32F1/variants/.../board.cpp
-    #if defined (STM32F4)
-      #define A0  PA0
-      #define A1  PA1
-      #define A2  PA2
-      #define A3  PA3
-      #define A4  PA4
-      #define A5  PA5
-      #define A6  PA6
-      #define A7  PA7
-      #define A8  PB0
-      #define A9  PB1
-      #define A10  PC0
-      #define A11  PC1
-      #define A12  PC2
-      #define A13  PC3
-      #define A14  PC4
-      #define A15  PC5
-    #else
-      #define A0  PB0
-      #define A1  PA7
-      #define A2  PA6
-      #define A3  PA5
-      #define A4  PA4
-      #define A5  PA3
-      #define A6  PA2
-      #define A7  PA1
-      #define A8  PA0
-      //STM32F1 have only 9 12bit adc
-      #define A9  PB0
-      #define A10  PA7
-      #define A11  PA6
-      #define A12  PA5
-      #define A13  PA4
-      #define A14  PA3
-      #define A15  PA2
-    #endif
-  #endif
-
   switch (boardID)
   {
     case 0:
@@ -183,35 +147,6 @@ void setPinMapping(byte boardID)
         pinCoil4 = 21;
         pinCoil3 = 30;
         pinO2 = A22;
-      #elif defined(CORE_STM32)
-        //http://docs.leaflabs.com/static.leaflabs.com/pub/leaflabs/maple-docs/0.0.12/hardware/maple-mini.html#master-pin-map
-        //pins 23, 24 and 33 couldn't be used
-        pinInjector1 = 15; //Output pin injector 1 is on
-        pinInjector2 = 16; //Output pin injector 2 is on
-        pinInjector3 = 17; //Output pin injector 3 is on
-        pinInjector4 = 18; //Output pin injector 4 is on
-        pinCoil1 = 19; //Pin for coil 1
-        pinCoil2 = 20; //Pin for coil 2
-        pinCoil3 = 21; //Pin for coil 3
-        pinCoil4 = 26; //Pin for coil 4
-        pinCoil5 = 27; //Pin for coil 5
-        pinTPS = A0; //TPS input pin
-        pinMAP = A1; //MAP sensor pin
-        pinIAT = A2; //IAT sensor pin
-        pinCLT = A3; //CLS sensor pin
-        pinO2 = A4; //O2 Sensor pin
-        pinBat = A5; //Battery reference voltage pin
-        pinStepperDir = 12; //Direction pin  for DRV8825 driver
-        pinStepperStep = 13; //Step pin for DRV8825 driver
-        pinStepperEnable = 14; //Enable pin for DRV8825
-        pinDisplayReset = 2; // OLED reset pin
-        pinFan = 1; //Pin for the fan output
-        pinFuelPump = 0; //Fuel pump output
-        pinTachOut = 31; //Tacho output pin
-        //external interrupt enabled pins
-        pinFlex = 32; // Flex sensor (Must be external interrupt enabled)
-        pinTrigger = 25; //The CAS pin
-        pinTrigger2 = 22; //The Cam Sensor pin
       #endif
       break;
 
@@ -271,12 +206,13 @@ void setPinMapping(byte boardID)
         pinCoil3 = PB12; //Pin for coil 3
         pinCoil4 = PB13; //Pin for coil 4
         pinCoil5 = PB14; //Pin for coil 5
-        pinTPS = PA0; //TPS input pin
-        pinMAP = PA1; //MAP sensor pin
-        pinIAT = PA2; //IAT sensor pin
-        pinCLT = PA3; //CLS sensor pin
-        pinO2 = PA4; //O2 Sensor pin
-        pinBat = PA5; //Battery reference voltage pin
+        pinTPS = A0; //TPS input pin
+        pinMAP = A1; //MAP sensor pin
+        pinIAT = A2; //IAT sensor pin
+        pinCLT = A3; //CLS sensor pin
+        pinO2 = A4; //O2 Sensor pin
+        pinBat = A5; //Battery reference voltage pin
+        pinBaro = A6;        
         pinStepperDir = PD8; //Direction pin  for DRV8825 driver
         pinStepperStep = PB15; //Step pin for DRV8825 driver
         pinStepperEnable = PD9; //Enable pin for DRV8825
@@ -317,6 +253,7 @@ void setPinMapping(byte boardID)
         pinFlex = 32; // Flex sensor (Must be external interrupt enabled)
         pinTrigger = 25; //The CAS pin
         pinTrigger2 = 22; //The Cam Sensor pin
+        pinBaro = pinMAP;
       #endif
       break;
 
@@ -324,8 +261,8 @@ void setPinMapping(byte boardID)
       //Pin mappings as per the MX5 PNP shield
       pinInjector1 = 11; //Output pin injector 1 is on
       pinInjector2 = 10; //Output pin injector 2 is on
-      pinInjector3 = 9; //Output pin injector 3 is on
-      pinInjector4 = 8; //Output pin injector 4 is on
+      pinInjector3 = 8; //Output pin injector 3 is on
+      pinInjector4 = 9; //Output pin injector 4 is on
       pinInjector5 = 14; //Output pin injector 5 is on
       pinCoil1 = 39; //Pin for coil 1
       pinCoil2 = 41; //Pin for coil 2
@@ -417,6 +354,7 @@ void setPinMapping(byte boardID)
       pinFan = 47; //Pin for the fan output
       pinFuelPump = 4; //Fuel pump output
       pinTachOut = 49; //Tacho output pin
+      break;
 
     case 30:
       //Pin mappings as per the dazv6 shield
@@ -486,18 +424,15 @@ void setPinMapping(byte boardID)
       break;
   }
 
-  //First time running?
-  if (configPage3.launchPin < BOARD_NR_GPIO_PINS)
-  {
-    //Setup any devices that are using selectable pins
-    if (configPage3.launchPin != 0) { pinLaunch = configPage3.launchPin; }
-    if (configPage2.ignBypassPin != 0) { pinIgnBypass = configPage2.ignBypassPin; }
-    if (configPage1.tachoPin != 0) { pinTachOut = configPage1.tachoPin; }
-    if (configPage2.fuelPumpPin != 0) { pinFuelPump = configPage2.fuelPumpPin; }
-    if (configPage4.fanPin != 0) { pinFan = configPage4.fanPin; }
-    if (configPage3.boostPin != 0) { pinBoost = configPage3.boostPin; }
-    if (configPage3.vvtPin != 0) { pinVVT_1 = configPage3.vvtPin; }
-  }
+  //Setup any devices that are using selectable pins
+  if ( (configPage3.launchPin != 0) && (configPage3.launchPin < BOARD_NR_GPIO_PINS) ) { pinLaunch = configPage3.launchPin; }
+  if ( (configPage2.ignBypassPin != 0) && (configPage2.ignBypassPin < BOARD_NR_GPIO_PINS) ) { pinIgnBypass = configPage2.ignBypassPin; }
+  if ( (configPage1.tachoPin != 0) && (configPage1.tachoPin < BOARD_NR_GPIO_PINS) ) { pinTachOut = configPage1.tachoPin; }
+  if ( (configPage2.fuelPumpPin != 0) && (configPage2.fuelPumpPin < BOARD_NR_GPIO_PINS) ) { pinFuelPump = configPage2.fuelPumpPin; }
+  if ( (configPage4.fanPin != 0) && (configPage4.fanPin < BOARD_NR_GPIO_PINS) ) { pinFan = configPage4.fanPin; }
+  if ( (configPage3.boostPin != 0) && (configPage3.boostPin < BOARD_NR_GPIO_PINS) ) { pinBoost = configPage3.boostPin; }
+  if ( (configPage3.vvtPin != 0) && (configPage3.vvtPin < BOARD_NR_GPIO_PINS) ) { pinVVT_1 = configPage3.vvtPin; }
+  if ( (configPage3.useExtBaro != 0) && (configPage3.baroPin < BOARD_NR_GPIO_PINS) ) { pinBaro = configPage3.baroPin + A0; }
 
   //Finally, set the relevant pin modes for outputs
   pinMode(pinCoil1, OUTPUT);
@@ -546,6 +481,8 @@ void setPinMapping(byte boardID)
 
   tach_pin_port = portOutputRegister(digitalPinToPort(pinTachOut));
   tach_pin_mask = digitalPinToBitMask(pinTachOut);
+  pump_pin_port = portOutputRegister(digitalPinToPort(pinFuelPump));
+  pump_pin_mask = digitalPinToBitMask(pinFuelPump);
 
   //And for inputs
   pinMode(pinMAP, INPUT);
@@ -555,12 +492,12 @@ void setPinMapping(byte boardID)
   pinMode(pinIAT, INPUT);
   pinMode(pinCLT, INPUT);
   pinMode(pinBat, INPUT);
+  pinMode(pinBaro, INPUT);
   pinMode(pinTrigger, INPUT);
   pinMode(pinTrigger2, INPUT);
   pinMode(pinTrigger3, INPUT);
   pinMode(pinFlex, INPUT_PULLUP); //Standard GM / Continental flex sensor requires pullup
-  //  pinMode(pinLaunch, INPUT_PULLUP); //This should work for both NO and NC grounding switches
-  if (configPage3.lnchPullRes) {
+  if (configPage3.lnchPullRes == true) {
     pinMode(pinLaunch, INPUT_PULLUP);
   }
   else {
@@ -598,34 +535,37 @@ unsigned int PW(int REQ_FUEL, byte VE, byte MAP, int corrections, int injOpen)
   //Standard float version of the calculation
   //return (REQ_FUEL * (float)(VE/100.0) * (float)(MAP/100.0) * (float)(TPS/100.0) * (float)(corrections/100.0) + injOpen);
   //Note: The MAP and TPS portions are currently disabled, we use VE and corrections only
-  unsigned int iVE, iMAP, iAFR, iCorrections;
+  uint16_t iVE, iCorrections;
+  uint16_t iMAP = 100;
+  uint16_t iAFR = 147;
 
   //100% float free version, does sacrifice a little bit of accuracy, but not much.
   iVE = ((unsigned int)VE << 7) / 100;
-  if ( configPage1.multiplyMAP ) {
+  if ( configPage1.multiplyMAP == true ) {
     iMAP = ((unsigned int)MAP << 7) / currentStatus.baro;  //Include multiply MAP (vs baro) if enabled
   }
-  if ( configPage1.includeAFR && (configPage3.egoType == 2)) {
+  if ( (configPage1.includeAFR == true) && (configPage3.egoType == 2)) {
     iAFR = ((unsigned int)currentStatus.O2 << 7) / currentStatus.afrTarget;  //Include AFR (vs target) if enabled
   }
   iCorrections = (corrections << 7) / 100;
 
 
   unsigned long intermediate = ((long)REQ_FUEL * (long)iVE) >> 7; //Need to use an intermediate value to avoid overflowing the long
-  if ( configPage1.multiplyMAP ) {
-    intermediate = (intermediate * iMAP) >> 7;
+  if ( configPage1.multiplyMAP == true ) {
+    intermediate = (intermediate * (unsigned long)iMAP) >> 7;
   }
-  if ( configPage1.includeAFR && (configPage3.egoType == 2)) {
-    intermediate = (intermediate * iAFR) >> 7;  //EGO type must be set to wideband for this to be used
+  if ( (configPage1.includeAFR == true) && (configPage3.egoType == 2) ) {
+    intermediate = (intermediate * (unsigned long)iAFR) >> 7;  //EGO type must be set to wideband for this to be used
   }
-  intermediate = (intermediate * iCorrections) >> 7;
-  if (intermediate == 0) {
-    return 0;  //If the pulsewidth is 0, we return here before the opening time gets added
-  }
-
-  intermediate += injOpen; //Add the injector opening time
-  if ( intermediate > 65535) {
-    intermediate = 65535;  //Make sure this won't overflow when we convert to uInt. This means the maximum pulsewidth possible is 65.535mS
+  intermediate = (intermediate * (unsigned long)iCorrections) >> 7;
+  if (intermediate != 0)
+  {
+    //If intermeditate is not 0, we need to add the opening time (0 typically indicates that one of the full fuel cuts is active)
+    intermediate += injOpen; //Add the injector opening time
+    if ( intermediate > 65535)
+    {
+      intermediate = 65535;  //Make sure this won't overflow when we convert to uInt. This means the maximum pulsewidth possible is 65.535mS
+    }
   }
   return (unsigned int)(intermediate);
 
@@ -640,9 +580,5 @@ unsigned int PW_SD(int REQ_FUEL, byte VE, byte MAP, int corrections, int injOpen
 
 unsigned int PW_AN(int REQ_FUEL, byte VE, byte TPS, int corrections, int injOpen)
 {
-  //Sanity check
-  if (TPS > 100) {
-    TPS = 100;
-  }
   return PW(REQ_FUEL, VE, currentStatus.MAP, corrections, injOpen);
 }

From 6d3439060d3f498b9bae1065230312e33cbcaa49 Mon Sep 17 00:00:00 2001
From: VitorBoss <vitor_boss@yahoo.com.br>
Date: Wed, 2 Aug 2017 23:29:05 -0300
Subject: [PATCH 04/10] Added possibility to use another library for STM32

---
 speeduino/scheduler.h                  | 130 +++++++++++++++++--------
 speeduino/scheduler.ino                |  48 ++++++---
 speeduino/src/FastCRC/FastCRC_tables.h |   2 +-
 3 files changed, 122 insertions(+), 58 deletions(-)

diff --git a/speeduino/scheduler.h b/speeduino/scheduler.h
index 41f7d563..397f28f8 100644
--- a/speeduino/scheduler.h
+++ b/speeduino/scheduler.h
@@ -162,59 +162,103 @@ See page 136 of the processors datasheet: http://www.atmel.com/Images/doc2549.pd
   #define uS_TO_TIMER_COMPARE_SLOW(uS) ((uS * 15) >> 5)
 
 #elif defined(CORE_STM32)
-  #include "HardwareTimer.h"
-  //Placeholders ONLY!
-  //https://visualgdb.com/tutorials/arm/stm32/timers/hal/
-  //https://github.com/rogerclarkmelbourne/Arduino_STM32/blob/master/STM32F4/cores/maple/libmaple/timer.h#L51
   #define MAX_TIMER_PERIOD 131070 //The longest period of time (in uS) that the timer can permit (IN this case it is 65535 * 2, as each timer tick is 2uS)
   #define uS_TO_TIMER_COMPARE(uS) (uS >> 1) //Converts a given number of uS into the required number of timer ticks until that time has passed.
   #define uS_TO_TIMER_COMPARE_SLOW(uS) (uS >> 1) //Converts a given number of uS into the required number of timer ticks until that time has passed.
+  #if defined(ARDUINO_ARCH_STM32)
+    #include "HardwareTimer.h"
+    #define FUEL1_COUNTER (TIM2)->CNT
+    #define FUEL2_COUNTER (TIM2)->CNT
+    #define FUEL3_COUNTER (TIM2)->CNT
+    #define FUEL4_COUNTER (TIM2)->CNT
+    #define FUEL5_COUNTER (TIM2)->CNT
 
-  #define FUEL1_COUNTER (TIM2)->CNT
-  #define FUEL2_COUNTER (TIM2)->CNT
-  #define FUEL3_COUNTER (TIM2)->CNT
-  #define FUEL4_COUNTER (TIM2)->CNT
-  #define FUEL5_COUNTER (TIM2)->CNT
+    #define IGN1_COUNTER  (TIM3)->CNT
+    #define IGN2_COUNTER  (TIM3)->CNT
+    #define IGN3_COUNTER  (TIM3)->CNT
+    #define IGN4_COUNTER  (TIM3)->CNT
+    #define IGN5_COUNTER  (TIM3)->CNT
 
-  #define IGN1_COUNTER  (TIM3)->CNT
-  #define IGN2_COUNTER  (TIM3)->CNT
-  #define IGN3_COUNTER  (TIM3)->CNT
-  #define IGN4_COUNTER  (TIM3)->CNT
-  #define IGN5_COUNTER  (TIM3)->CNT
+    #define FUEL1_COMPARE (TIM2)->CCR1
+    #define FUEL2_COMPARE (TIM2)->CCR2
+    #define FUEL3_COMPARE (TIM2)->CCR3
+    #define FUEL4_COMPARE (TIM2)->CCR4
 
-  #define FUEL1_COMPARE (TIM2)->CCR1
-  #define FUEL2_COMPARE (TIM2)->CCR2
-  #define FUEL3_COMPARE (TIM2)->CCR3
-  #define FUEL4_COMPARE (TIM2)->CCR4
+    #define IGN1_COMPARE (TIM3)->CCR1
+    #define IGN2_COMPARE (TIM3)->CCR2
+    #define IGN3_COMPARE (TIM3)->CCR3
+    #define IGN4_COMPARE (TIM3)->CCR4
+    #define IGN5_COMPARE (TIM3)->CCR1
 
-  #define IGN1_COMPARE (TIM3)->CCR1
-  #define IGN2_COMPARE (TIM3)->CCR2
-  #define IGN3_COMPARE (TIM3)->CCR3
-  #define IGN4_COMPARE (TIM3)->CCR4
-  #define IGN5_COMPARE (TIM3)->CCR1
+    //https://github.com/rogerclarkmelbourne/Arduino_STM32/blob/754bc2969921f1ef262bd69e7faca80b19db7524/STM32F1/system/libmaple/include/libmaple/timer.h#L444
+    #define FUEL1_TIMER_ENABLE() (TIM2)->CCER |= TIM_CCER_CC1E
+    #define FUEL2_TIMER_ENABLE() (TIM2)->CCER |= TIM_CCER_CC2E
+    #define FUEL3_TIMER_ENABLE() (TIM2)->CCER |= TIM_CCER_CC3E
+    #define FUEL4_TIMER_ENABLE() (TIM2)->CCER |= TIM_CCER_CC4E
 
-  //https://github.com/rogerclarkmelbourne/Arduino_STM32/blob/754bc2969921f1ef262bd69e7faca80b19db7524/STM32F1/system/libmaple/include/libmaple/timer.h#L444
-  #define FUEL1_TIMER_ENABLE() (TIM2)->CCER |= TIM_CCER_CC1E
-  #define FUEL2_TIMER_ENABLE() (TIM2)->CCER |= TIM_CCER_CC2E
-  #define FUEL3_TIMER_ENABLE() (TIM2)->CCER |= TIM_CCER_CC3E
-  #define FUEL4_TIMER_ENABLE() (TIM2)->CCER |= TIM_CCER_CC4E
+    #define IGN1_TIMER_ENABLE() (TIM3)->CCER |= TIM_CCER_CC1E
+    #define IGN2_TIMER_ENABLE() (TIM3)->CCER |= TIM_CCER_CC2E
+    #define IGN3_TIMER_ENABLE() (TIM3)->CCER |= TIM_CCER_CC3E
+    #define IGN4_TIMER_ENABLE() (TIM3)->CCER |= TIM_CCER_CC4E
+    #define IGN5_TIMER_ENABLE() (TIM1)->CCER |= TIM_CCER_CC1E
 
-  #define IGN1_TIMER_ENABLE() (TIM3)->CCER |= TIM_CCER_CC1E
-  #define IGN2_TIMER_ENABLE() (TIM3)->CCER |= TIM_CCER_CC2E
-  #define IGN3_TIMER_ENABLE() (TIM3)->CCER |= TIM_CCER_CC3E
-  #define IGN4_TIMER_ENABLE() (TIM3)->CCER |= TIM_CCER_CC4E
-  #define IGN5_TIMER_ENABLE() (TIM1)->CCER |= TIM_CCER_CC1E
+    #define FUEL1_TIMER_DISABLE() (TIM2)->CCER &= ~TIM_CCER_CC1E
+    #define FUEL2_TIMER_DISABLE() (TIM2)->CCER &= ~TIM_CCER_CC2E
+    #define FUEL3_TIMER_DISABLE() (TIM2)->CCER &= ~TIM_CCER_CC3E
+    #define FUEL4_TIMER_DISABLE() (TIM2)->CCER &= ~TIM_CCER_CC4E
 
-  #define FUEL1_TIMER_DISABLE() (TIM2)->CCER &= ~TIM_CCER_CC1E
-  #define FUEL2_TIMER_DISABLE() (TIM2)->CCER &= ~TIM_CCER_CC2E
-  #define FUEL3_TIMER_DISABLE() (TIM2)->CCER &= ~TIM_CCER_CC3E
-  #define FUEL4_TIMER_DISABLE() (TIM2)->CCER &= ~TIM_CCER_CC4E
+    #define IGN1_TIMER_DISABLE() (TIM3)->CCER &= ~TIM_CCER_CC1E
+    #define IGN2_TIMER_DISABLE() (TIM3)->CCER &= ~TIM_CCER_CC2E
+    #define IGN3_TIMER_DISABLE() (TIM3)->CCER &= ~TIM_CCER_CC3E
+    #define IGN4_TIMER_DISABLE() (TIM3)->CCER &= ~TIM_CCER_CC4E
+    #define IGN5_TIMER_DISABLE() (TIM1)->CCER &= ~TIM_CCER_CC1E
+  #else
+    #define FUEL1_COUNTER (TIMER2->regs).gen->CNT
+    #define FUEL2_COUNTER (TIMER2->regs).gen->CNT
+    #define FUEL3_COUNTER (TIMER2->regs).gen->CNT
+    #define FUEL4_COUNTER (TIMER2->regs).gen->CNT
+    #define FUEL5_COUNTER (TIMER1->regs).gen->CNT
 
-  #define IGN1_TIMER_DISABLE() (TIM3)->CCER &= ~TIM_CCER_CC1E
-  #define IGN2_TIMER_DISABLE() (TIM3)->CCER &= ~TIM_CCER_CC2E
-  #define IGN3_TIMER_DISABLE() (TIM3)->CCER &= ~TIM_CCER_CC3E
-  #define IGN4_TIMER_DISABLE() (TIM3)->CCER &= ~TIM_CCER_CC4E
-  #define IGN5_TIMER_DISABLE() (TIM1)->CCER &= ~TIM_CCER_CC1E
+    #define IGN1_COUNTER  (TIMER3->regs).gen->CNT
+    #define IGN2_COUNTER  (TIMER3->regs).gen->CNT
+    #define IGN3_COUNTER  (TIMER3->regs).gen->CNT
+    #define IGN4_COUNTER  (TIMER3->regs).gen->CNT
+    #define IGN5_COUNTER  (TIMER1->regs).gen->CNT
+
+    #define FUEL1_COMPARE (TIMER2->regs).gen->CCR1
+    #define FUEL2_COMPARE (TIMER2->regs).gen->CCR2
+    #define FUEL3_COMPARE (TIMER2->regs).gen->CCR3
+    #define FUEL4_COMPARE (TIMER2->regs).gen->CCR4
+
+    #define IGN1_COMPARE (TIMER3->regs).gen->CCR1
+    #define IGN2_COMPARE (TIMER3->regs).gen->CCR2
+    #define IGN3_COMPARE (TIMER3->regs).gen->CCR3
+    #define IGN4_COMPARE (TIMER3->regs).gen->CCR4
+    #define IGN5_COMPARE (TIMER1->regs).gen->CCR1
+
+    //https://github.com/rogerclarkmelbourne/Arduino_STM32/blob/754bc2969921f1ef262bd69e7faca80b19db7524/STM32F1/system/libmaple/include/libmaple/timer.h#L444
+    #define FUEL1_TIMER_ENABLE() (TIMER2->regs).gen->CCER |= TIMER_CCER_CC1E
+    #define FUEL2_TIMER_ENABLE() (TIMER2->regs).gen->CCER |= TIMER_CCER_CC2E
+    #define FUEL3_TIMER_ENABLE() (TIMER2->regs).gen->CCER |= TIMER_CCER_CC3E
+    #define FUEL4_TIMER_ENABLE() (TIMER2->regs).gen->CCER |= TIMER_CCER_CC4E
+
+    #define IGN1_TIMER_ENABLE() (TIMER3->regs).gen->CCER |= TIMER_CCER_CC1E
+    #define IGN2_TIMER_ENABLE() (TIMER3->regs).gen->CCER |= TIMER_CCER_CC2E
+    #define IGN3_TIMER_ENABLE() (TIMER3->regs).gen->CCER |= TIMER_CCER_CC3E
+    #define IGN4_TIMER_ENABLE() (TIMER3->regs).gen->CCER |= TIMER_CCER_CC4E
+    #define IGN5_TIMER_ENABLE() (TIMER1->regs).gen->CCER |= TIMER_CCER_CC1E
+
+    #define FUEL1_TIMER_DISABLE() (TIMER2->regs).gen->CCER &= ~TIMER_CCER_CC1E
+    #define FUEL2_TIMER_DISABLE() (TIMER2->regs).gen->CCER &= ~TIMER_CCER_CC2E
+    #define FUEL3_TIMER_DISABLE() (TIMER2->regs).gen->CCER &= ~TIMER_CCER_CC3E
+    #define FUEL4_TIMER_DISABLE() (TIMER2->regs).gen->CCER &= ~TIMER_CCER_CC4E
+
+    #define IGN1_TIMER_DISABLE() (TIMER3->regs).gen->CCER &= ~TIMER_CCER_CC1E
+    #define IGN2_TIMER_DISABLE() (TIMER3->regs).gen->CCER &= ~TIMER_CCER_CC2E
+    #define IGN3_TIMER_DISABLE() (TIMER3->regs).gen->CCER &= ~TIMER_CCER_CC3E
+    #define IGN4_TIMER_DISABLE() (TIMER3->regs).gen->CCER &= ~TIMER_CCER_CC4E
+    #define IGN5_TIMER_DISABLE() (TIMER1->regs).gen->CCER &= ~TIMER_CCER_CC1E
+  #endif
 #endif
 
 void initialiseSchedulers();
@@ -360,4 +404,4 @@ static inline unsigned int popQueue(volatile Schedule *queue[])
 }
 
 
-#endif // SCHEDULER_H
+#endif // SCHEDULER_H
diff --git a/speeduino/scheduler.ino b/speeduino/scheduler.ino
index 5919e2de..26b0b22b 100644
--- a/speeduino/scheduler.ino
+++ b/speeduino/scheduler.ino
@@ -161,22 +161,42 @@ void initialiseSchedulers()
   NVIC_ENABLE_IRQ(IRQ_FTM1);
 
 #elif defined(CORE_STM32)
-  //see https://github.com/rogerclarkmelbourne/Arduino_STM32/blob/754bc2969921f1ef262bd69e7faca80b19db7524/STM32F1/system/libmaple/include/libmaple/timer.h#L444
-  Timer1.setPrescaleFactor(((Timer1.getBaseFrequency()  / 1000000) << 1) -1);  //2us resolution
-  Timer2.setPrescaleFactor(((Timer2.getBaseFrequency()  / 1000000) << 1) -1);  //2us resolution
-  Timer3.setPrescaleFactor(((Timer3.getBaseFrequency()  / 1000000) << 1) -1);  //2us resolution
+  #if defined(ARDUINO_ARCH_STM32)
+    //see https://github.com/rogerclarkmelbourne/Arduino_STM32/blob/754bc2969921f1ef262bd69e7faca80b19db7524/STM32F1/system/libmaple/include/libmaple/timer.h#L444
+    Timer1.setPrescaleFactor(((Timer1.getBaseFrequency()  / 1000000) << 1) -1);  //2us resolution
+    Timer2.setPrescaleFactor(((Timer2.getBaseFrequency()  / 1000000) << 1) -1);  //2us resolution
+    Timer3.setPrescaleFactor(((Timer3.getBaseFrequency()  / 1000000) << 1) -1);  //2us resolution
 
-  Timer2.setMode(1, TIMER_OUTPUT_COMPARE);
-  Timer2.setMode(2, TIMER_OUTPUT_COMPARE);
-  Timer2.setMode(3, TIMER_OUTPUT_COMPARE);
-  Timer2.setMode(4, TIMER_OUTPUT_COMPARE);
+    Timer2.setMode(1, TIMER_OUTPUT_COMPARE);
+    Timer2.setMode(2, TIMER_OUTPUT_COMPARE);
+    Timer2.setMode(3, TIMER_OUTPUT_COMPARE);
+    Timer2.setMode(4, TIMER_OUTPUT_COMPARE);
 
-  Timer3.setMode(1, TIMER_OUTPUT_COMPARE);
-  Timer3.setMode(2, TIMER_OUTPUT_COMPARE);
-  Timer3.setMode(3, TIMER_OUTPUT_COMPARE);
-  Timer3.setMode(4, TIMER_OUTPUT_COMPARE);
-  Timer1.setMode(1, TIMER_OUTPUT_COMPARE); 
+    Timer3.setMode(1, TIMER_OUTPUT_COMPARE);
+    Timer3.setMode(2, TIMER_OUTPUT_COMPARE);
+    Timer3.setMode(3, TIMER_OUTPUT_COMPARE);
+    Timer3.setMode(4, TIMER_OUTPUT_COMPARE);
+    Timer1.setMode(1, TIMER_OUTPUT_COMPARE); 
 
+  #else
+    //see https://github.com/rogerclarkmelbourne/Arduino_STM32/blob/754bc2969921f1ef262bd69e7faca80b19db7524/STM32F1/system/libmaple/include/libmaple/timer.h#L444
+    (TIMER1->regs).bas->PSC = (CYCLES_PER_MICROSECOND << 1) - 1;  //2us resolution
+    (TIMER2->regs).bas->PSC = (CYCLES_PER_MICROSECOND << 1) - 1;  //2us resolution
+    (TIMER3->regs).bas->PSC = (CYCLES_PER_MICROSECOND << 1) - 1;  //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);
+    Timer2.setMode(TIMER_CH3, TIMER_OUTPUT_COMPARE);
+    Timer2.setMode(TIMER_CH4, TIMER_OUTPUT_COMPARE);
+
+    Timer3.setMode(TIMER_CH1, TIMER_OUTPUT_COMPARE);
+    Timer3.setMode(TIMER_CH2, TIMER_OUTPUT_COMPARE);
+    Timer3.setMode(TIMER_CH3, TIMER_OUTPUT_COMPARE);
+    Timer3.setMode(TIMER_CH4, TIMER_OUTPUT_COMPARE);
+
+  #endif
   Timer2.attachInterrupt(1, fuelSchedule1Interrupt);
   Timer2.attachInterrupt(2, fuelSchedule2Interrupt);
   Timer2.attachInterrupt(3, fuelSchedule3Interrupt);
@@ -790,4 +810,4 @@ void ftm0_isr(void)
   else if(interrupt8) { FTM0_C7SC &= ~FTM_CSC_CHF; ignitionSchedule4Interrupt(); }
 
 }
-#endif
+#endif
diff --git a/speeduino/src/FastCRC/FastCRC_tables.h b/speeduino/src/FastCRC/FastCRC_tables.h
index 47f376dd..033a137c 100755
--- a/speeduino/src/FastCRC/FastCRC_tables.h
+++ b/speeduino/src/FastCRC/FastCRC_tables.h
@@ -31,7 +31,7 @@
 #define FastCRC_tables
 #include "inttypes.h"
 
-#if defined(__AVR__) || defined(STM32_MCU_SERIES) || defined(_VARIANT_ARDUINO_STM32_)
+#if defined(__AVR__) || defined(STM32_MCU_SERIES) || defined(ARDUINO_ARCH_STM32)
 #include <avr/pgmspace.h>
 #else
 #include <pgmspace.h>

From 9f64ba2b6cf5d237971872d303d17d28e13726b2 Mon Sep 17 00:00:00 2001
From: VitorBoss <vitor_boss@yahoo.com.br>
Date: Fri, 4 Aug 2017 00:13:32 -0300
Subject: [PATCH 05/10] idle, boost and vvt PWM added

---
 speeduino/auxiliaries.h                |  22 +++
 speeduino/auxiliaries.ino              |  27 ++--
 speeduino/cancomms.h                   |   7 +-
 speeduino/globals.h                    |  12 +-
 speeduino/idle.h                       |  17 ++-
 speeduino/idle.ino                     |  23 +--
 speeduino/scheduler.h                  |   4 +-
 speeduino/scheduler.ino                |  17 +--
 speeduino/speeduino.ino                |   2 +-
 speeduino/src/FastCRC/FastCRC_tables.h |   2 +-
 speeduino/table.ino                    | 185 ++++++++++++++-----------
 speeduino/utils.ino                    |  27 ++--
 12 files changed, 202 insertions(+), 143 deletions(-)

diff --git a/speeduino/auxiliaries.h b/speeduino/auxiliaries.h
index 5ce8d246..48ef84cb 100644
--- a/speeduino/auxiliaries.h
+++ b/speeduino/auxiliaries.h
@@ -30,7 +30,29 @@ void initialiseFan();
   #define VVT_TIMER_COUNTER     FTM1_CNT
 
 #elif defined(CORE_STM32)
+  #if defined(ARDUINO_ARCH_STM32) // STM32GENERIC core
+    #define ENABLE_BOOST_TIMER()  (TIM1)->CCER |= TIM_CCER_CC2E
+    #define DISABLE_BOOST_TIMER() (TIM1)->CCER &= ~TIM_CCER_CC2E
 
+    #define ENABLE_VVT_TIMER()    (TIM1)->CCER |= TIM_CCER_CC3E
+    #define DISABLE_VVT_TIMER()   (TIM1)->CCER &= ~TIM_CCER_CC3E
+
+    #define BOOST_TIMER_COMPARE   (TIM1)->CCR2
+    #define BOOST_TIMER_COUNTER   (TIM1)->CNT
+    #define VVT_TIMER_COMPARE     (TIM1)->CCR3
+    #define VVT_TIMER_COUNTER     (TIM1)->CNT
+  #else //libmaple core aka STM32DUINO
+    #define ENABLE_BOOST_TIMER()  (TIMER1->regs).gen->CCER |= TIMER_CCER_CC2E
+    #define DISABLE_BOOST_TIMER() (TIMER1->regs).gen->CCER &= ~TIMER_CCER_CC2E
+
+    #define ENABLE_VVT_TIMER()    (TIMER1->regs).gen->CCER |= TIMER_CCER_CC3E
+    #define DISABLE_VVT_TIMER()   (TIMER1->regs).gen->CCER &= ~TIMER_CCER_CC3E
+
+    #define BOOST_TIMER_COMPARE   (TIMER1->regs).gen->CCR2
+    #define BOOST_TIMER_COUNTER   (TIMER1->regs).gen->CNT
+    #define VVT_TIMER_COMPARE     (TIMER1->regs).gen->CCR3
+    #define VVT_TIMER_COUNTER     (TIMER1->regs).gen->CNT
+  #endif
 #endif
 
 #define BOOST_PIN_LOW()  *boost_pin_port &= ~(boost_pin_mask)
diff --git a/speeduino/auxiliaries.ino b/speeduino/auxiliaries.ino
index 88805e40..69deffde 100644
--- a/speeduino/auxiliaries.ino
+++ b/speeduino/auxiliaries.ino
@@ -29,7 +29,6 @@ void fanControl()
   }
 }
 
-#if defined(CORE_AVR) || defined(CORE_TEENSY)
 void initialiseAuxPWM()
 {
   #if defined(CORE_AVR)
@@ -40,15 +39,24 @@ void initialiseAuxPWM()
     TCCR1B = (1 << CS12);   //Timer1 Control Reg B: Timer Prescaler set to 256. 1 tick = 16uS. Refer to http://www.instructables.com/files/orig/F3T/TIKL/H3WSA4V7/F3TTIKLH3WSA4V7.jpg
   #elif defined(CORE_TEENSY)
     //REALLY NEED TO DO THIS!
+  #elif defined(CORE_STM32)
+    Timer1.attachInterrupt(2, boostControl);
+    Timer1.attachInterrupt(3, vvtControl); 
+    Timer1.resume(); 
   #endif
 
   boost_pin_port = portOutputRegister(digitalPinToPort(pinBoost));
   boost_pin_mask = digitalPinToBitMask(pinBoost);
   vvt_pin_port = portOutputRegister(digitalPinToPort(pinVVT_1));
   vvt_pin_mask = digitalPinToBitMask(pinVVT_1);
-
-  boost_pwm_max_count = 1000000L / (16 * configPage3.boostFreq * 2); //Converts the frequency in Hz to the number of ticks (at 16uS) it takes to complete 1 cycle. The x2 is there because the frequency is stored at half value (in a byte) to allow freqneucies up to 511Hz
-  vvt_pwm_max_count = 1000000L / (16 * configPage3.vvtFreq * 2); //Converts the frequency in Hz to the number of ticks (at 16uS) it takes to complete 1 cycle
+  
+  #if defined(CORE_STM32) //2uS resolution Min 8Hz, Max 5KHz
+    boost_pwm_max_count = 1000000L / (configPage3.boostFreq * 2); //Converts the frequency in Hz to the number of ticks (at 16uS) it takes to complete 1 cycle. The x2 is there because the frequency is stored at half value (in a byte) to allow freqneucies up to 511Hz
+    vvt_pwm_max_count = 1000000L / (configPage3.vvtFreq * 2); //Converts the frequency in Hz to the number of ticks (at 16uS) it takes to complete 1 cycle
+  #else
+    boost_pwm_max_count = 1000000L / (16 * configPage3.boostFreq * 2); //Converts the frequency in Hz to the number of ticks (at 16uS) it takes to complete 1 cycle. The x2 is there because the frequency is stored at half value (in a byte) to allow freqneucies up to 511Hz
+    vvt_pwm_max_count = 1000000L / (16 * configPage3.vvtFreq * 2); //Converts the frequency in Hz to the number of ticks (at 16uS) it takes to complete 1 cycle
+  #endif
   //TIMSK1 |= (1 << OCIE1A); <---- Not required as compare A is turned on when needed by boost control
   ENABLE_VVT_TIMER(); //Turn on the B compare unit (ie turn on the interrupt)
 
@@ -138,7 +146,7 @@ void vvtControl()
 //The interrupt to control the Boost PWM
 #if defined(CORE_AVR)
   ISR(TIMER1_COMPA_vect)
-#elif defined (CORE_TEENSY)
+#elif defined (CORE_TEENSY) || defined(CORE_STM32)
   static inline void boostInterrupt() //Most ARM chips can simply call a function
 #endif
 {
@@ -160,7 +168,7 @@ void vvtControl()
 //The interrupt to control the VVT PWM
 #if defined(CORE_AVR)
   ISR(TIMER1_COMPB_vect)
-#elif defined (CORE_TEENSY)
+#elif defined (CORE_TEENSY) || defined(CORE_STM32)
   static inline void vvtInterrupt() //Most ARM chips can simply call a function
 #endif
 {
@@ -179,10 +187,3 @@ void vvtControl()
   }
 }
 
-#elif defined(CORE_STM32)
-//YET TO BE IMPLEMENTED ON STM32
-void initialiseAuxPWM() { }
-void boostControl() { }
-void vvtControl() { }
-
-#endif
diff --git a/speeduino/cancomms.h b/speeduino/cancomms.h
index 38104469..d6f98d73 100644
--- a/speeduino/cancomms.h
+++ b/speeduino/cancomms.h
@@ -15,8 +15,11 @@ uint8_t Glow, Ghigh;
 #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
   HardwareSerial &CANSerial = Serial3;
 #elif defined(CORE_STM32)
-  //HardwareSerial &CANSerial = Serial2;
-  SerialUART &CANSerial = Serial2;
+  #if defined(ARDUINO_ARCH_STM32) // STM32GENERIC core
+    SerialUART &CANSerial = Serial2;
+  #else //libmaple core aka STM32DUINO
+    HardwareSerial &CANSerial = Serial2;
+  #endif
 #elif defined(CORE_TEENSY)
   HardwareSerial &CANSerial = Serial2;
 #endif
diff --git a/speeduino/globals.h b/speeduino/globals.h
index 98a400ec..bedf7169 100644
--- a/speeduino/globals.h
+++ b/speeduino/globals.h
@@ -14,17 +14,17 @@
   #if defined (STM32F1) || defined(__STM32F1__)
     #define BOARD_NR_GPIO_PINS 34
     #define LED_BUILTIN 33
-  #elif defined(ARDUINO_BLACK_F407VE)
-    #define BOARD_NR_GPIO_PINS 78
+  #elif defined(ARDUINO_BLACK_F407VE) || defined(STM32F4)
+    #define BOARD_NR_GPIO_PINS 80
     #define LED_BUILTIN PA7
   #endif
 
   extern "C" char* sbrk(int incr); //Used to freeRam
   inline unsigned char  digitalPinToInterrupt(unsigned char Interrupt_pin) { return Interrupt_pin; } //This isn't included in the stm32duino libs (yet)
-  #if defined(ARDUINO_ARCH_STM32)
+  #if defined(ARDUINO_ARCH_STM32) // STM32GENERIC core
     #define portOutputRegister(port) (volatile byte *)( &(port->ODR) )
     #define portInputRegister(port) (volatile byte *)( &(port->IDR) )
-  #else
+  #else //libmaple core aka STM32DUINO
     #define portOutputRegister(port) (volatile byte *)( &(port->regs->ODR) ) //These are defined in STM32F1/variants/generic_stm32f103c/variant.h but return a non byte* value
     #define portInputRegister(port) (volatile byte *)( &(port->regs->IDR) ) //These are defined in STM32F1/variants/generic_stm32f103c/variant.h but return a non byte* value
   #endif
@@ -36,6 +36,10 @@
 #define BIT_SET(a,b) ((a) |= (1<<(b)))
 #define BIT_CLEAR(a,b) ((a) &= ~(1<<(b)))
 #define BIT_CHECK(var,pos) !!((var) & (1<<(pos)))
+#define Lo(param) ((char *)&param)[0]
+#define Hi(param) ((char *)&param)[1]
+#define Higher(param) ((char *)&param)[2]
+#define Highest(param) ((char *)&param)[3]
 
 #define MS_IN_MINUTE 60000
 #define US_IN_MINUTE 60000000
diff --git a/speeduino/idle.h b/speeduino/idle.h
index 929a7755..d701c3d3 100644
--- a/speeduino/idle.h
+++ b/speeduino/idle.h
@@ -40,14 +40,19 @@ struct StepperIdle
   #define IDLE_TIMER_DISABLE() FTM2_C0SC &= ~FTM_CSC_CHIE
 
 #elif defined(CORE_STM32)
+  #if defined(ARDUINO_ARCH_STM32) // STM32GENERIC core
+    #define IDLE_COUNTER   (TIM1)->CNT
+    #define IDLE_COMPARE   (TIM1)->CCR4
 
-  //Placeholders only
-  #define IDLE_COUNTER 0
-  #define IDLE_COMPARE 0
-
-  #define IDLE_TIMER_ENABLE()
-  #define IDLE_TIMER_DISABLE()
+    #define IDLE_TIMER_ENABLE()  (TIM1)->CCER |= TIM_CCER_CC4E
+    #define IDLE_TIMER_DISABLE() (TIM1)->CCER &= ~TIM_CCER_CC4E
+  #else //libmaple core aka STM32DUINO
+    #define IDLE_COUNTER   (TIMER1->regs).gen->CNT
+    #define IDLE_COMPARE   (TIMER1->regs).gen->CCR4
 
+    #define IDLE_TIMER_ENABLE()  (TIMER1->regs).gen->CCER |= TIMER_CCER_CC4E
+    #define IDLE_TIMER_DISABLE() (TIMER1->regs).gen->CCER &= ~TIMER_CCER_CC4E
+  #endif
 #endif
 
 struct table2D iacClosedLoopTable;
diff --git a/speeduino/idle.ino b/speeduino/idle.ino
index af41d507..03d6d460 100644
--- a/speeduino/idle.ino
+++ b/speeduino/idle.ino
@@ -78,8 +78,9 @@ void initialiseIdle()
     NVIC_ENABLE_IRQ(IRQ_FTM2);
   }
 
-  #elif defined(MCU_STM32F103RB)
-
+  #elif defined(CORE_STM32)
+    Timer1.attachInterrupt(4, idleInterrupt); 
+    Timer1.resume(); 
   #endif
 
   //Initialising comprises of setting the 2D tables with the relevant values from the config pages
@@ -114,7 +115,11 @@ void initialiseIdle()
       idle_pin_mask = digitalPinToBitMask(pinIdle1);
       idle2_pin_port = portOutputRegister(digitalPinToPort(pinIdle2));
       idle2_pin_mask = digitalPinToBitMask(pinIdle2);
-      idle_pwm_max_count = 1000000L / (16 * configPage3.idleFreq * 2); //Converts the frequency in Hz to the number of ticks (at 16uS) it takes to complete 1 cycle. Note that the frequency is divided by 2 coming from TS to allow for up to 512hz
+      #if defined(CORE_STM32)
+        idle_pwm_max_count = 1000000L / (configPage3.idleFreq * 2); //Converts the frequency in Hz to the number of ticks (at 2uS) it takes to complete 1 cycle. Note that the frequency is divided by 2 coming from TS to allow for up to 5KHz
+      #else
+        idle_pwm_max_count = 1000000L / (16 * configPage3.idleFreq * 2); //Converts the frequency in Hz to the number of ticks (at 16uS) it takes to complete 1 cycle. Note that the frequency is divided by 2 coming from TS to allow for up to 512hz
+      #endif
       enableIdle();
       break;
 
@@ -134,7 +139,11 @@ void initialiseIdle()
       idle_pin_mask = digitalPinToBitMask(pinIdle1);
       idle2_pin_port = portOutputRegister(digitalPinToPort(pinIdle2));
       idle2_pin_mask = digitalPinToBitMask(pinIdle2);
-      idle_pwm_max_count = 1000000L / (16 * configPage3.idleFreq * 2); //Converts the frequency in Hz to the number of ticks (at 16uS) it takes to complete 1 cycle. Note that the frequency is divided by 2 coming from TS to allow for up to 512hz
+      #if defined(CORE_STM32)
+        idle_pwm_max_count = 1000000L / (configPage3.idleFreq * 2); //Converts the frequency in Hz to the number of ticks (at 2uS) it takes to complete 1 cycle. Note that the frequency is divided by 2 coming from TS to allow for up to 5KHz
+      #else
+        idle_pwm_max_count = 1000000L / (16 * configPage3.idleFreq * 2); //Converts the frequency in Hz to the number of ticks (at 16uS) it takes to complete 1 cycle. Note that the frequency is divided by 2 coming from TS to allow for up to 512hz
+      #endif
       idlePID.SetOutputLimits(percentage(configPage1.iacCLminDuty, idle_pwm_max_count), percentage(configPage1.iacCLmaxDuty, idle_pwm_max_count));
       idlePID.SetTunings(configPage3.idleKP, configPage3.idleKI, configPage3.idleKD);
       idlePID.SetMode(AUTOMATIC); //Turn PID on
@@ -407,7 +416,6 @@ ISR(TIMER4_COMPC_vect)
 #elif defined (CORE_TEENSY) || defined (CORE_STM32)
 static inline void idleInterrupt() //Most ARM chips can simply call a function
 #endif
-#if defined(CORE_AVR) || defined (CORE_TEENSY)
 {
   if (idle_pwm_state)
   {
@@ -446,8 +454,3 @@ static inline void idleInterrupt() //Most ARM chips can simply call a function
   }
 
 }
-#elif defined (CORE_STM32)
-{
-  //No PWM idle for STM32 yet
-}
-#endif
diff --git a/speeduino/scheduler.h b/speeduino/scheduler.h
index 397f28f8..62d80c91 100644
--- a/speeduino/scheduler.h
+++ b/speeduino/scheduler.h
@@ -165,7 +165,7 @@ See page 136 of the processors datasheet: http://www.atmel.com/Images/doc2549.pd
   #define MAX_TIMER_PERIOD 131070 //The longest period of time (in uS) that the timer can permit (IN this case it is 65535 * 2, as each timer tick is 2uS)
   #define uS_TO_TIMER_COMPARE(uS) (uS >> 1) //Converts a given number of uS into the required number of timer ticks until that time has passed.
   #define uS_TO_TIMER_COMPARE_SLOW(uS) (uS >> 1) //Converts a given number of uS into the required number of timer ticks until that time has passed.
-  #if defined(ARDUINO_ARCH_STM32)
+  #if defined(ARDUINO_ARCH_STM32) // STM32GENERIC core
     #include "HardwareTimer.h"
     #define FUEL1_COUNTER (TIM2)->CNT
     #define FUEL2_COUNTER (TIM2)->CNT
@@ -212,7 +212,7 @@ See page 136 of the processors datasheet: http://www.atmel.com/Images/doc2549.pd
     #define IGN3_TIMER_DISABLE() (TIM3)->CCER &= ~TIM_CCER_CC3E
     #define IGN4_TIMER_DISABLE() (TIM3)->CCER &= ~TIM_CCER_CC4E
     #define IGN5_TIMER_DISABLE() (TIM1)->CCER &= ~TIM_CCER_CC1E
-  #else
+  #else //libmaple core aka STM32DUINO
     #define FUEL1_COUNTER (TIMER2->regs).gen->CNT
     #define FUEL2_COUNTER (TIMER2->regs).gen->CNT
     #define FUEL3_COUNTER (TIMER2->regs).gen->CNT
diff --git a/speeduino/scheduler.ino b/speeduino/scheduler.ino
index 26b0b22b..e24a3e92 100644
--- a/speeduino/scheduler.ino
+++ b/speeduino/scheduler.ino
@@ -161,12 +161,11 @@ void initialiseSchedulers()
   NVIC_ENABLE_IRQ(IRQ_FTM1);
 
 #elif defined(CORE_STM32)
-  #if defined(ARDUINO_ARCH_STM32)
+  Timer1.setPeriod(2); //2us resolution
+  Timer2.setPeriod(2); //2us resolution
+  Timer3.setPeriod(2); //2us resolution
+  #if defined(ARDUINO_ARCH_STM32) // STM32GENERIC core
     //see https://github.com/rogerclarkmelbourne/Arduino_STM32/blob/754bc2969921f1ef262bd69e7faca80b19db7524/STM32F1/system/libmaple/include/libmaple/timer.h#L444
-    Timer1.setPrescaleFactor(((Timer1.getBaseFrequency()  / 1000000) << 1) -1);  //2us resolution
-    Timer2.setPrescaleFactor(((Timer2.getBaseFrequency()  / 1000000) << 1) -1);  //2us resolution
-    Timer3.setPrescaleFactor(((Timer3.getBaseFrequency()  / 1000000) << 1) -1);  //2us resolution
-
     Timer2.setMode(1, TIMER_OUTPUT_COMPARE);
     Timer2.setMode(2, TIMER_OUTPUT_COMPARE);
     Timer2.setMode(3, TIMER_OUTPUT_COMPARE);
@@ -178,14 +177,8 @@ void initialiseSchedulers()
     Timer3.setMode(4, TIMER_OUTPUT_COMPARE);
     Timer1.setMode(1, TIMER_OUTPUT_COMPARE); 
 
-  #else
+  #else //libmaple core aka STM32DUINO
     //see https://github.com/rogerclarkmelbourne/Arduino_STM32/blob/754bc2969921f1ef262bd69e7faca80b19db7524/STM32F1/system/libmaple/include/libmaple/timer.h#L444
-    (TIMER1->regs).bas->PSC = (CYCLES_PER_MICROSECOND << 1) - 1;  //2us resolution
-    (TIMER2->regs).bas->PSC = (CYCLES_PER_MICROSECOND << 1) - 1;  //2us resolution
-    (TIMER3->regs).bas->PSC = (CYCLES_PER_MICROSECOND << 1) - 1;  //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);
     Timer2.setMode(TIMER_CH3, TIMER_OUTPUT_COMPARE);
diff --git a/speeduino/speeduino.ino b/speeduino/speeduino.ino
index 30ffd065..1254cb40 100644
--- a/speeduino/speeduino.ino
+++ b/speeduino/speeduino.ino
@@ -152,7 +152,7 @@ void setup()
 
   #if defined(CORE_STM32)
     EEPROM.init();
-  #endif  
+  #endif
   loadConfig();
   doUpdates(); //Check if any data items need updating (Occurs ith firmware updates)
 
diff --git a/speeduino/src/FastCRC/FastCRC_tables.h b/speeduino/src/FastCRC/FastCRC_tables.h
index 033a137c..1d5142f5 100755
--- a/speeduino/src/FastCRC/FastCRC_tables.h
+++ b/speeduino/src/FastCRC/FastCRC_tables.h
@@ -31,7 +31,7 @@
 #define FastCRC_tables
 #include "inttypes.h"
 
-#if defined(__AVR__) || defined(STM32_MCU_SERIES) || defined(ARDUINO_ARCH_STM32)
+#if defined(__AVR__) || defined(STM32_MCU_SERIES) || defined(ARDUINO_ARCH_STM32) || defined(_VARIANT_ARDUINO_STM32_)
 #include <avr/pgmspace.h>
 #else
 #include <pgmspace.h>
diff --git a/speeduino/table.ino b/speeduino/table.ino
index 1e8dc663..87d5e0c9 100644
--- a/speeduino/table.ino
+++ b/speeduino/table.ino
@@ -409,82 +409,103 @@ int get3DTableValue(struct table3D *fromTable, int Y_in, int X_in)
     return tableResult;
 }
 /* Executed a benchmark on all options and this is the results
- * Stadard:226224 91 |FP Math:32240 91.89 |Clean code:34056 91, Number of loops:2500
- *
+ * @Mega:   Stadard:464276 92 |FP Math:893104 92.57 |Clean code:641280 92  , Number of loops:5000
+ * @STM32F1: Stadard:21449 92 |FP Math:125707 92.57  |Clean code:11634 92  , Number of loops:5000
+ * @STM32F4: Stadard:7275 92  |FP Math:5724 92.57  |Clean code:4742 92    , Number of loops:5000
+
 //This function pulls a value from a 3D table given a target for X and Y coordinates.
 //It performs a 2D linear interpolation as descibred in: http://www.megamanual.com/v22manual/ve_tuner.pdf
 float get3DTableValueF(struct table3D *fromTable, int Y, int X)
 {
-  float m, n, o ,p, q, r;
   byte xMin, xMax;
   byte yMin, yMax;
-  int yMaxValue, yMinValue;
-  int xMaxValue, xMinValue;
+  float tableResult = 0;
 
-  if(fromTable->lastXMin==0) {fromTable->lastXMin = fromTable->xSize-1;}
-  else {xMin = fromTable->lastXMin;}
-  if(fromTable->lastYMin==0) {fromTable->lastYMin = fromTable->ySize-1;}
-  else {yMin = fromTable->lastYMin;}
-  //yMin = fromTable->lastYMin;
-
-  if(xMin>fromTable->xSize-1)
-  {
-    fromTable->lastXMin = fromTable->xSize-1;
-    xMin = fromTable->xSize-1;
-  }
-  if(yMin>fromTable->ySize-1)
-  {
-    fromTable->lastYMin = fromTable->ySize-1;
-    yMin = fromTable->ySize-1;
-  }
+  xMin = fromTable->lastXMin;
+  yMin = fromTable->lastYMin;
+  if(xMin == 0) { xMin = fromTable->xSize-1; }
+  if(yMin == 0) { yMin = fromTable->ySize-1; }
 
   do  //RPM axis
   {
-    if(X>=fromTable->axisX[xMin]) {break;}
+    if(X == fromTable->axisX[xMin])
+    {
+      xMax = xMin;
+      break;
+    }
+    else if(X > fromTable->axisX[xMin])
+    {
+      if (xMin == fromTable->xSize-1) { xMax = xMin; }  //Overflow protection
+      else { xMax = xMin + 1; }
+      break;
+    }
     xMin--;
-  }while(1);
-  fromTable->lastXMin = xMin + 1;
+  }while(true);
+
   do  //MAP axis
   {
-    if(Y<=fromTable->axisY[yMin]) {break;}
+    if(Y == fromTable->axisY[yMin])
+    {
+      yMax = yMin;
+      break;
+    }
+    else if(Y < fromTable->axisY[yMin])
+    {
+      if (yMin == fromTable->ySize-1) { yMax = yMin; }  //Overflow protection
+      else { yMax = yMin + 1; }
+      break;
+    }
     yMin--;
-  }while(1);
-  fromTable->lastYMin = yMin + 1;
+  }while(true);
 
-  xMax = xMin + 1;
-  yMax = yMin + 1;
-  if (xMax>fromTable->xSize-1)  //Overflow protection
-  {
-    xMax = fromTable->xSize-1;
-    xMin = xMax - 1;
-  }
-  if (yMax>fromTable->ySize-1)  //Overflow protection
-  {
-    yMax = fromTable->ySize-1;
-    yMin = yMax - 1;
-  }
-
-  yMaxValue = fromTable->axisY[yMax];
-  yMinValue = fromTable->axisY[yMin];
-  xMaxValue = fromTable->axisX[xMax];
-  xMinValue = fromTable->axisX[xMin];
+  fromTable->lastXMin = xMax;
+  fromTable->lastYMin = yMax;
 
   int A = fromTable->values[yMin][xMin];
   int B = fromTable->values[yMin][xMax];
   int C = fromTable->values[yMax][xMin];
   int D = fromTable->values[yMax][xMax];
+  if( (A == B) && (A == C) && (A == D) ) { tableResult = A; }
+  else if(xMin == xMax) // Same RPM axys do a simple 2D (27% faster on Mega on worst case, up to 500%)
+  {
+    int yMaxValue = fromTable->axisY[yMax];
+    int yMinValue = fromTable->axisY[yMin];
+    float q;
+    q = ((float)(Y - yMinValue) * (C - A)) / (yMaxValue - yMinValue);
+    tableResult = A + q;
+  }
+  else if(yMin == yMax) // Same MAP axys do a simple 2D (27% faster on Mega on worst case, up to 500%)
+  {
+    int xMaxValue = fromTable->axisX[xMax];
+    int xMinValue = fromTable->axisX[xMin];
+    float q;
+    q = ((float)(X - xMinValue) * (B - A)) / (xMaxValue - xMinValue);
+    tableResult = A + q;
+  }
+  else
+  {
+    int yMaxValue = fromTable->axisY[yMax];
+    int yMinValue = fromTable->axisY[yMin];
+    int xMaxValue = fromTable->axisX[xMax];
+    int xMinValue = fromTable->axisX[xMin];
+     
+    float m, n, o , p, q, r;
+    if (xMaxValue == xMinValue) { p = (float)X - xMinValue; }
+    else { p = ((float)X - xMinValue) / (xMaxValue - xMinValue); }
+    if (yMaxValue == yMinValue) { q = (float)Y - yMinValue; }
+    else { q = ((float)Y - yMinValue) / (yMaxValue - yMinValue); }
 
-  p = float(X - xMinValue) / (xMaxValue - xMinValue); //(RPM - RPM[1])/(RPM[2]- RPM[1])
-  q = float(Y - yMinValue) / (yMaxValue - yMinValue); //(MAP - MAP[1])/(MAP[2]- MAP[1])
-
-  m = (1.0-p) * (1.0-q);
-  n = p * (1-q);
-  o = (1-p) * q;
-  r = p * q;
-
-  return ( (A * m) + (B * n) + (C * o) + (D * r) );
+    m = (1.0-p) * (1.0-q);
+    n = p * (1-q);
+    o = (1-p) * q;
+    r = p * q;
+    tableResult = ( (A * m) + (B * n) + (C * o) + (D * r) );
+  }
+  return tableResult;
 }
 */
+//This function pulls a value from a 3D table given a target for X and Y coordinates.
+//It performs a 2D linear interpolation as descibred in: http://www.megamanual.com/v22manual/ve_tuner.pdf
 int get3DTableValueS(struct table3D *fromTable, int Y, int X)
 {
   byte xMin, xMax;
@@ -498,68 +519,62 @@ int get3DTableValueS(struct table3D *fromTable, int Y, int X)
 
   do  //RPM axis
   {
-    if(X > fromTable->axisX[xMin])
-    {
-      if (xMin == (fromTable->xSize-1) ) { xMax = xMin; }  //Overflow protection
-      else { xMax = xMin + 1; }
-      break;
-    }
-    else if(X == fromTable->axisX[xMin])
+    if(X == fromTable->axisX[xMin])
     {
       xMax = xMin;
       break;
     }
+    else if(X > fromTable->axisX[xMin])
+    {
+      if (xMin == fromTable->xSize-1) { xMax = xMin; }  //Overflow protection
+      else { xMax = xMin + 1; }
+      break;
+    }
     xMin--;
   }while(true);
 
   do  //MAP axis
   {
-    if(Y < fromTable->axisY[yMin])
-    {
-      if (yMin == (fromTable->ySize-1) ) { yMax = yMin; }  //Overflow protection
-      else { yMax = yMin + 1; }
-      break;
-    }
-    else if(Y == fromTable->axisY[yMin])
+    if(Y == fromTable->axisY[yMin])
     {
       yMax = yMin;
       break;
     }
+    else if(Y < fromTable->axisY[yMin])
+    {
+      if (yMin == fromTable->ySize-1) { yMax = yMin; }  //Overflow protection
+      else { yMax = yMin + 1; }
+      break;
+    }
     yMin--;
   }while(true);
 
   fromTable->lastXMin = xMax;
   fromTable->lastYMin = yMax;
 
-  //Serial.print("xMin, xMax ");Serial.print(xMin);Serial.print(",");Serial.println(xMax);
-  //Serial.print("yMin, yMax ");Serial.print(yMin);Serial.print(",");Serial.println(yMax);
-
   int A = fromTable->values[yMin][xMin];
-  if((xMin == xMax) && (yMin == yMax)) { tableResult = A; }
-  else if(xMin == xMax) // Simple 2D
+  int B = fromTable->values[yMin][xMax];
+  int C = fromTable->values[yMax][xMin];
+  int D = fromTable->values[yMax][xMax];
+  if( (A == B) && (A == C) && (A == D) ) { tableResult = A; }
+  else if(xMin == xMax) // Same RPM axys do a simple 2D (27% faster on Mega on worst case, up to 500%)
   {
-    int D = fromTable->values[yMax][xMax];
     int yMaxValue = fromTable->axisY[yMax];
     int yMinValue = fromTable->axisY[yMin];
-    long q = (long)Y - yMinValue;
-    q = ((q << 6)  * (D - A)) / (yMaxValue - yMinValue);
+    long q;
+    q = (((long)(Y - yMinValue) << 6)  * (C - A)) / (yMaxValue - yMinValue);
     tableResult = A + (q >> 6);
   }
-  else if(yMin == yMax) // Simple 2D
+  else if(yMin == yMax) // Same MAP axys do a simple 2D (27% faster on Mega on worst case, up to 500%)
   {
-    int D = fromTable->values[yMax][xMax];
     int xMaxValue = fromTable->axisX[xMax];
     int xMinValue = fromTable->axisX[xMin];
-    long q = (long)X - xMinValue;
-    q = ((q << 6) * (D - A)) / (xMaxValue - xMinValue);
+    long q;
+    q = (((long)(X - xMinValue) << 6) * (B - A)) / (xMaxValue - xMinValue);
     tableResult = A + (q >> 6);
   }
   else
   {
-    int B = fromTable->values[yMin][xMax];
-    int C = fromTable->values[yMax][xMin];
-    int D = fromTable->values[yMax][xMax];
-
     int yMaxValue = fromTable->axisY[yMax];
     int yMinValue = fromTable->axisY[yMin];
     int xMaxValue = fromTable->axisX[xMax];
@@ -575,6 +590,7 @@ int get3DTableValueS(struct table3D *fromTable, int Y, int X)
       q = (long)Y - yMinValue;
       q = (q << 8);
     }
+    //Standard case
     else
     {
       q = long(Y) - yMaxValue;
@@ -586,6 +602,7 @@ int get3DTableValueS(struct table3D *fromTable, int Y, int X)
     int o = ((256-p) * q) >> 8;
     int r = (p * q) >> 8;
     tableResult = ( (A * m) + (B * n) + (C * o) + (D * r) ) >> 8;
-  }
+ }
   return tableResult;
 }
+
diff --git a/speeduino/utils.ino b/speeduino/utils.ino
index 2223e16b..7b3b21f8 100644
--- a/speeduino/utils.ino
+++ b/speeduino/utils.ino
@@ -489,14 +489,25 @@ void setPinMapping(byte boardID)
   pump_pin_mask = digitalPinToBitMask(pinFuelPump);
 
   //And for inputs
-  pinMode(pinMAP, INPUT);
-  pinMode(pinO2, INPUT);
-  pinMode(pinO2_2, INPUT);
-  pinMode(pinTPS, INPUT);
-  pinMode(pinIAT, INPUT);
-  pinMode(pinCLT, INPUT);
-  pinMode(pinBat, INPUT);
-  pinMode(pinBaro, INPUT);
+  #ifndef ARDUINO_ARCH_STM32 //libmaple core aka STM32DUINO
+    pinMode(pinMAP, INPUT_ANALOG);
+    pinMode(pinO2, INPUT_ANALOG);
+    pinMode(pinO2_2, INPUT_ANALOG);
+    pinMode(pinTPS, INPUT_ANALOG);
+    pinMode(pinIAT, INPUT_ANALOG);
+    pinMode(pinCLT, INPUT_ANALOG);
+    pinMode(pinBat, INPUT_ANALOG);
+    pinMode(pinBaro, INPUT_ANALOG);
+  #else
+    pinMode(pinMAP, INPUT);
+    pinMode(pinO2, INPUT);
+    pinMode(pinO2_2, INPUT);
+    pinMode(pinTPS, INPUT);
+    pinMode(pinIAT, INPUT);
+    pinMode(pinCLT, INPUT);
+    pinMode(pinBat, INPUT);
+    pinMode(pinBaro, INPUT);
+  #endif
   pinMode(pinTrigger, INPUT);
   pinMode(pinTrigger2, INPUT);
   pinMode(pinTrigger3, INPUT);

From 7c15b2657c6c6bf1cf151aedb4ecb6680f72f13a Mon Sep 17 00:00:00 2001
From: VitorBoss <vitor_boss@yahoo.com.br>
Date: Fri, 4 Aug 2017 16:12:16 -0300
Subject: [PATCH 06/10] Fix last merge

---
 speeduino/comms.ino    |  4 ++--
 speeduino/decoders.ino | 39 ++++++++++++++++++++++++++++-----------
 speeduino/sensors.h    |  4 ++--
 speeduino/sensors.ino  |  7 +++++--
 speeduino/timers.ino   |  2 --
 speeduino/updates.ino  | 20 +++++++++++++++++++-
 6 files changed, 56 insertions(+), 20 deletions(-)

diff --git a/speeduino/comms.ino b/speeduino/comms.ino
index 04d82ed2..bcee111d 100644
--- a/speeduino/comms.ino
+++ b/speeduino/comms.ino
@@ -80,12 +80,12 @@ void command()
       break;
 
     case 'S': // send code version
-      Serial.print("Speeduino 2017.07-dev");
+      Serial.print("Speeduino 2017.07");
       currentStatus.secl = 0; //This is required in TS3 due to its stricter timings
       break;
 
     case 'Q': // send code version
-      Serial.print("speeduino 201707-dev");
+      Serial.print("speeduino 201707");
      break;
 
     case 'V': // send VE table and constants in binary
diff --git a/speeduino/decoders.ino b/speeduino/decoders.ino
index ad683954..07713590 100644
--- a/speeduino/decoders.ino
+++ b/speeduino/decoders.ino
@@ -88,14 +88,18 @@ It takes an argument of the full (COMPLETE) number of teeth per revolution. For
 static inline int crankingGetRPM(byte totalTeeth)
 {
   uint16_t tempRPM = 0;
-  if( (toothLastToothTime > 0) && (toothLastMinusOneToothTime > 0) )
+  if( currentStatus.startRevolutions >= 2 )
   {
-    noInterrupts();
-    revolutionTime = (toothLastToothTime - toothLastMinusOneToothTime) * totalTeeth;
-    interrupts();
-    tempRPM = (US_IN_MINUTE / revolutionTime);
-    if( tempRPM >= MAX_RPM ) { tempRPM = currentStatus.RPM; } //Sanity check. This can prevent spiking caused by noise on individual teeth. The new RPM should never be above 4x the cranking setting value (Remembering that this function is only called is the current RPM is less than the cranking setting)
+    if( (toothLastToothTime > 0) && (toothLastMinusOneToothTime > 0) )
+    {
+      noInterrupts();
+      revolutionTime = (toothLastToothTime - toothLastMinusOneToothTime) * totalTeeth;
+      interrupts();
+      tempRPM = (US_IN_MINUTE / revolutionTime);
+      if( tempRPM >= MAX_RPM ) { tempRPM = currentStatus.RPM; } //Sanity check. This can prevent spiking caused by noise on individual teeth. The new RPM should never be above 4x the cranking setting value (Remembering that this function is only called is the current RPM is less than the cranking setting)
+    }
   }
+
   return tempRPM;
 }
 
@@ -152,6 +156,8 @@ void triggerPri_missingTooth()
        if ( (curGap > targetGap) || (toothCurrentCount > triggerActualTeeth) )
        {
          if(toothCurrentCount < (triggerActualTeeth) && (currentStatus.hasSync == true) ) { currentStatus.hasSync = false; } //This occurs when we're at tooth #1, but haven't seen all the other teeth. This indicates a signal issue so we flag lost sync so this will attempt to resync on the next revolution.
+         //This is to handle a special case on startup where sync can be obtained and the system immediately thinks the revs have jumped:
+         //else if (currentStatus.hasSync == false && toothCurrentCount < checkSyncToothCount ) { triggerFilterTime = 0; }
          else
          {
            toothCurrentCount = 1;
@@ -804,7 +810,7 @@ uint16_t getRPM_4G63()
   //Because these signals aren't even (Alternating 110 and 70 degrees), this needs a special function
   if(currentStatus.hasSync == true)
   {
-    if( currentStatus.RPM < (unsigned int)(configPage2.crankRPM * 100) )
+    if( currentStatus.RPM < ((unsigned int)configPage2.crankRPM * 100) )
     {
       int tempToothAngle;
       unsigned long toothTime;
@@ -817,6 +823,8 @@ uint16_t getRPM_4G63()
         interrupts();
         toothTime = toothTime * 36;
         tempRPM = ((unsigned long)tempToothAngle * 6000000UL) / toothTime;
+        revolutionTime = (10UL * toothTime) / tempToothAngle;
+
       }
     }
     else { tempRPM = stdGetRPM(); }
@@ -1090,7 +1098,7 @@ void triggerPri_Audi135()
 {
    curTime = micros();
    curGap = curTime - toothSystemLastToothTime;
-   if ( curGap > triggerFilterTime )
+   if ( (curGap > triggerFilterTime) || (currentStatus.startRevolutions == 0) )
    {
      toothSystemCount++;
 
@@ -1859,7 +1867,7 @@ void triggerPri_Subaru67()
    toothLastMinusOneToothTime = toothLastToothTime;
    toothLastToothTime = curTime;
 
-   if ( (currentStatus.hasSync == false) || configPage2.useResync)
+   if ( (currentStatus.hasSync == false) || configPage2.useResync || (currentStatus.startRevolutions == 0) )
    {
      //Sync is determined by counting the number of cam teeth that have passed between the crank teeth
      switch(secondaryToothCount)
@@ -1929,7 +1937,15 @@ void triggerSec_Subaru67()
 uint16_t getRPM_Subaru67()
 {
   //if(currentStatus.RPM < configPage2.crankRPM) { return crankingGetRPM(configPage2.triggerTeeth); }
-  return stdGetRPM();
+
+  uint16_t tempRPM = 0;
+  if(currentStatus.startRevolutions > 0)
+  {
+    //As the tooth count is over 720 degrees, we need to double the RPM value and halve the revolution time
+    tempRPM = stdGetRPM() << 1;
+    revolutionTime = revolutionTime >> 1; //Revolution time has to be divided by 2 as otherwise it would be over 720 degrees (triggerActualTeeth = nCylinders)
+  }
+  return tempRPM;
 }
 
 int getCrankAngle_Subaru67(int timePerDegree)
@@ -2005,7 +2021,7 @@ void triggerPri_Daihatsu()
   curTime = micros();
   curGap = curTime - toothLastToothTime;
 
-  //if ( curGap >= triggerFilterTime )
+  //if ( curGap >= triggerFilterTime || (currentStatus.startRevolutions == 0 )
   {
     toothSystemCount++;
 
@@ -2127,3 +2143,4 @@ void triggerSetEndTeeth_Daihatsu()
 {
 
 }
+
diff --git a/speeduino/sensors.h b/speeduino/sensors.h
index c8294a2f..24245d04 100644
--- a/speeduino/sensors.h
+++ b/speeduino/sensors.h
@@ -39,7 +39,6 @@ byte MAPcurRev; //Tracks which revolution we're sampling on
 
 void instanteneousMAPReading();
 void readMAP();
-void readBaro();
 void flexPulse();
 
 #if defined(ANALOG_ISR)
@@ -105,4 +104,5 @@ ISR(ADC_vect)
 }
 #endif
 
-#endif // SENSORS_H
+#endif // SENSORS_H
+
diff --git a/speeduino/sensors.ino b/speeduino/sensors.ino
index ce41fd0c..7aedbb08 100644
--- a/speeduino/sensors.ino
+++ b/speeduino/sensors.ino
@@ -61,7 +61,9 @@ void instanteneousMAPReading()
   if( (tempReading >= VALID_MAP_MAX) || (tempReading <= VALID_MAP_MIN) ) { mapErrorCount += 1; }
   else { mapErrorCount = 0; }
 
-  currentStatus.mapADC = ADC_FILTER(tempReading, ADCFILTER_MAP, currentStatus.mapADC); //Very weak filter
+  //During startup a call is made here to get the baro reading. In this case, we can't apply the ADC filter
+  if(initialisationComplete == true) { currentStatus.mapADC = ADC_FILTER(tempReading, ADCFILTER_MAP, currentStatus.mapADC); } //Very weak filter
+  else { currentStatus.mapADC = tempReading; } //Baro reading (No filter)
 
   currentStatus.MAP = fastMap10Bit(currentStatus.mapADC, configPage1.mapMin, configPage1.mapMax); //Get the current MAP value
   if(currentStatus.MAP < 0) { currentStatus.MAP = 0; } //Sanity check
@@ -219,7 +221,7 @@ void readBaro()
 
     currentStatus.baroADC = ADC_FILTER(tempReading, ADCFILTER_BARO, currentStatus.baroADC); //Very weak filter
 
-    currentStatus.baro = fastMap1023toX(currentStatus.baroADC, configPage1.mapMax); //Get the current MAP value
+    currentStatus.baro = fastMap10Bit(currentStatus.baroADC, configPage1.mapMin, configPage1.mapMax); //Get the current MAP value
   }
 }
 
@@ -262,3 +264,4 @@ void flexPulse()
  {
    ++flexCounter;
  }
+
diff --git a/speeduino/timers.ino b/speeduino/timers.ino
index fd3f8225..76095a7e 100644
--- a/speeduino/timers.ino
+++ b/speeduino/timers.ino
@@ -47,7 +47,6 @@ void initialiseTimers()
   Timer4.resume(); //Start Timer
 #endif
 
-  pinMode(LED_BUILTIN, OUTPUT); //pinMode(13, OUTPUT);  
   dwellLimit_uS = (1000 * configPage2.dwellLimit);
   lastRPM_100ms = 0;
 }
@@ -101,7 +100,6 @@ void oneMSInterval() //Most ARM chips can simply call a function
   {
     loop100ms = 0; //Reset counter
     BIT_SET(TIMER_mask, BIT_TIMER_10HZ);
-    digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
 
     currentStatus.rpmDOT = (currentStatus.RPM - lastRPM_100ms) * 10; //This is the RPM per second that the engine has accelerated/decelleratedin the last loop
     lastRPM_100ms = currentStatus.RPM; //Record the current RPM for next calc
diff --git a/speeduino/updates.ino b/speeduino/updates.ino
index 96ddee79..a184b99e 100644
--- a/speeduino/updates.ino
+++ b/speeduino/updates.ino
@@ -8,7 +8,7 @@
 
 void doUpdates()
 {
-  #define CURRENT_DATA_VERSION    4
+  #define CURRENT_DATA_VERSION    5
 
   //May 2017 firmware introduced a -40 offset on the ignition table. Update that table to +40
   if(EEPROM.read(EEPROM_DATA_VERSION) == 2)
@@ -36,6 +36,23 @@ void doUpdates()
     writeConfig();
     EEPROM.write(EEPROM_DATA_VERSION, 4);
   }
+  //July 2017 adds a cranking enrichment curve in place of the single value. This converts that single value to the curve
+  if(EEPROM.read(EEPROM_DATA_VERSION) == 4)
+  {
+    //Some default values for the bins (Doesn't matter too much here as the values against them will all be identical)
+    configPage11.crankingEnrichBins[0] = 0;
+    configPage11.crankingEnrichBins[1] = 40;
+    configPage11.crankingEnrichBins[2] = 70;
+    configPage11.crankingEnrichBins[3] = 100;
+
+    configPage11.crankingEnrichValues[0] = 100 + configPage1.crankingPct;
+    configPage11.crankingEnrichValues[1] = 100 + configPage1.crankingPct;
+    configPage11.crankingEnrichValues[2] = 100 + configPage1.crankingPct;
+    configPage11.crankingEnrichValues[3] = 100 + configPage1.crankingPct;
+
+    writeConfig();
+    EEPROM.write(EEPROM_DATA_VERSION, 5);
+  }
 
   //Final check is always for 255 and 0 (Brand new arduino)
   if( (EEPROM.read(EEPROM_DATA_VERSION) == 0) || (EEPROM.read(EEPROM_DATA_VERSION) == 255) )
@@ -45,3 +62,4 @@ void doUpdates()
 
   }
 }
+

From 94e0c6c19c4833b7b511f9ac0c33353c8225f8ea Mon Sep 17 00:00:00 2001
From: VitorBoss <vitor_boss@yahoo.com.br>
Date: Fri, 4 Aug 2017 17:05:16 -0300
Subject: [PATCH 07/10] Fix error on no STM32 boards

---
 speeduino/idle.h    | 1 +
 speeduino/utils.ino | 2 ++
 2 files changed, 3 insertions(+)

diff --git a/speeduino/idle.h b/speeduino/idle.h
index d701c3d3..e21919bc 100644
--- a/speeduino/idle.h
+++ b/speeduino/idle.h
@@ -86,5 +86,6 @@ static inline void enableIdle();
 static inline byte isStepperHomed();
 static inline byte checkForStepping();
 static inline void doStep();
+static inline void idleInterrupt();
 
 #endif
diff --git a/speeduino/utils.ino b/speeduino/utils.ino
index 7b3b21f8..90543e8f 100644
--- a/speeduino/utils.ino
+++ b/speeduino/utils.ino
@@ -489,6 +489,7 @@ void setPinMapping(byte boardID)
   pump_pin_mask = digitalPinToBitMask(pinFuelPump);
 
   //And for inputs
+  #if defined(CORE_STM32)
   #ifndef ARDUINO_ARCH_STM32 //libmaple core aka STM32DUINO
     pinMode(pinMAP, INPUT_ANALOG);
     pinMode(pinO2, INPUT_ANALOG);
@@ -508,6 +509,7 @@ void setPinMapping(byte boardID)
     pinMode(pinBat, INPUT);
     pinMode(pinBaro, INPUT);
   #endif
+  #endif
   pinMode(pinTrigger, INPUT);
   pinMode(pinTrigger2, INPUT);
   pinMode(pinTrigger3, INPUT);

From 1defafb38732772eba5b3299ff6420fabfe4d838 Mon Sep 17 00:00:00 2001
From: VitorBoss <vitor_boss@yahoo.com.br>
Date: Mon, 7 Aug 2017 21:40:27 -0300
Subject: [PATCH 08/10] Work on boost/vvt and timers for STM32

---
 speeduino/auxiliaries.h   |  5 ++++-
 speeduino/auxiliaries.ino |  6 +++---
 speeduino/idle.h          |  4 +++-
 speeduino/idle.ino        |  4 ++--
 speeduino/scheduler.ino   | 11 ++++++++---
 speeduino/speeduino.ino   |  7 +++----
 speeduino/table.ino       |  4 ++--
 speeduino/timers.ino      |  6 ++++++
 speeduino/utils.ino       | 14 +++++++++-----
 9 files changed, 40 insertions(+), 21 deletions(-)

diff --git a/speeduino/auxiliaries.h b/speeduino/auxiliaries.h
index 48ef84cb..8597db15 100644
--- a/speeduino/auxiliaries.h
+++ b/speeduino/auxiliaries.h
@@ -81,6 +81,9 @@ volatile bool vvt_pwm_state;
 unsigned int vvt_pwm_max_count; //Used for variable PWM frequency
 volatile unsigned int vvt_pwm_cur_value;
 long vvt_pwm_target_value;
-
+#if defined (CORE_TEENSY) || defined(CORE_STM32)
+  static inline void boostInterrupt();
+  static inline void vvtInterrupt();
+#endif
 
 #endif
diff --git a/speeduino/auxiliaries.ino b/speeduino/auxiliaries.ino
index 69deffde..e05a3a41 100644
--- a/speeduino/auxiliaries.ino
+++ b/speeduino/auxiliaries.ino
@@ -40,9 +40,9 @@ void initialiseAuxPWM()
   #elif defined(CORE_TEENSY)
     //REALLY NEED TO DO THIS!
   #elif defined(CORE_STM32)
-    Timer1.attachInterrupt(2, boostControl);
-    Timer1.attachInterrupt(3, vvtControl); 
-    Timer1.resume(); 
+    Timer1.attachInterrupt(2, boostInterrupt);
+    Timer1.attachInterrupt(3, vvtInterrupt);
+    Timer1.resume();
   #endif
 
   boost_pin_port = portOutputRegister(digitalPinToPort(pinBoost));
diff --git a/speeduino/idle.h b/speeduino/idle.h
index e21919bc..ffa4f22c 100644
--- a/speeduino/idle.h
+++ b/speeduino/idle.h
@@ -86,6 +86,8 @@ static inline void enableIdle();
 static inline byte isStepperHomed();
 static inline byte checkForStepping();
 static inline void doStep();
-static inline void idleInterrupt();
+#if defined (CORE_TEENSY) || defined(CORE_STM32)
+  static inline void idleInterrupt();
+#endif
 
 #endif
diff --git a/speeduino/idle.ino b/speeduino/idle.ino
index 03d6d460..ec0b0d7e 100644
--- a/speeduino/idle.ino
+++ b/speeduino/idle.ino
@@ -79,8 +79,8 @@ void initialiseIdle()
   }
 
   #elif defined(CORE_STM32)
-    Timer1.attachInterrupt(4, idleInterrupt); 
-    Timer1.resume(); 
+    Timer1.attachInterrupt(4, idleInterrupt);
+    Timer1.resume();
   #endif
 
   //Initialising comprises of setting the 2D tables with the relevant values from the config pages
diff --git a/speeduino/scheduler.ino b/speeduino/scheduler.ino
index e24a3e92..70f1a5c8 100644
--- a/speeduino/scheduler.ino
+++ b/speeduino/scheduler.ino
@@ -161,11 +161,11 @@ void initialiseSchedulers()
   NVIC_ENABLE_IRQ(IRQ_FTM1);
 
 #elif defined(CORE_STM32)
-  Timer1.setPeriod(2); //2us resolution
-  Timer2.setPeriod(2); //2us resolution
-  Timer3.setPeriod(2); //2us resolution
   #if defined(ARDUINO_ARCH_STM32) // STM32GENERIC core
     //see https://github.com/rogerclarkmelbourne/Arduino_STM32/blob/754bc2969921f1ef262bd69e7faca80b19db7524/STM32F1/system/libmaple/include/libmaple/timer.h#L444
+    Timer1.setPrescaleFactor((HAL_RCC_GetHCLKFreq() * 2U)-1);  //2us resolution
+    Timer2.setPrescaleFactor((HAL_RCC_GetHCLKFreq() * 2U)-1);  //2us resolution
+    Timer3.setPrescaleFactor((HAL_RCC_GetHCLKFreq() * 2U)-1);  //2us resolution
     Timer2.setMode(1, TIMER_OUTPUT_COMPARE);
     Timer2.setMode(2, TIMER_OUTPUT_COMPARE);
     Timer2.setMode(3, TIMER_OUTPUT_COMPARE);
@@ -179,6 +179,11 @@ void initialiseSchedulers()
 
   #else //libmaple core aka STM32DUINO
     //see https://github.com/rogerclarkmelbourne/Arduino_STM32/blob/754bc2969921f1ef262bd69e7faca80b19db7524/STM32F1/system/libmaple/include/libmaple/timer.h#L444
+    //(CYCLES_PER_MICROSECOND == 72, APB2 at 72MHz, APB1 at 36MHz).
+    //Timer2 to 4 is on APB1, Timer1 on APB2.   http://www.st.com/resource/en/datasheet/stm32f103cb.pdf sheet 12
+    Timer1.setPrescaleFactor((72 * 2U)-1); //2us resolution
+    Timer2.setPrescaleFactor((36 * 2U)-1); //2us resolution
+    Timer3.setPrescaleFactor((36 * 2U)-1); //2us resolution
     Timer2.setMode(TIMER_CH1, TIMER_OUTPUT_COMPARE);
     Timer2.setMode(TIMER_CH2, TIMER_OUTPUT_COMPARE);
     Timer2.setMode(TIMER_CH3, TIMER_OUTPUT_COMPARE);
diff --git a/speeduino/speeduino.ino b/speeduino/speeduino.ino
index 1254cb40..5fc354ce 100644
--- a/speeduino/speeduino.ino
+++ b/speeduino/speeduino.ino
@@ -157,15 +157,15 @@ void setup()
   doUpdates(); //Check if any data items need updating (Occurs ith firmware updates)
 
 #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) //ATmega2561 does not have Serial3
-  if (configPage10.enable_canbus == 1) { Serial3.begin(115200); }
+  if (configPage10.enable_canbus == 1) { CANSerial.begin(115200); }
 #elif defined(CORE_STM32)
-  if (configPage10.enable_canbus == 1) { Serial2.begin(115200); }
+  if (configPage10.enable_canbus == 1) { CANSerial.begin(115200); }
   else if (configPage10.enable_canbus == 2)
   {
     //enable local can interface
   }
 #elif defined(CORE_TEENSY)
-  if (configPage10.enable_canbus == 1) { Serial2.begin(115200); }
+  if (configPage10.enable_canbus == 1) { CANSerial.begin(115200); }
   else if (configPage10.enable_canbus == 2)
   {
     //Teensy onboard CAN not used currently
@@ -1122,7 +1122,6 @@ void loop()
     if (currentStatus.hasSync && (currentStatus.RPM > 0))
     {
         if(currentStatus.startRevolutions >= configPage2.StgCycles)  { ignitionOn = true; fuelOn = true;} //Enable the fuel and ignition, assuming staging revolutions are complete
-        else { ignitionOn = false; fuelOn = false;}
         //If it is, check is we're running or cranking
         if(currentStatus.RPM > ((unsigned int)configPage2.crankRPM * 100)) //Crank RPM stored in byte as RPM / 100
         {
diff --git a/speeduino/table.ino b/speeduino/table.ino
index 87d5e0c9..9d633630 100644
--- a/speeduino/table.ino
+++ b/speeduino/table.ino
@@ -503,7 +503,7 @@ float get3DTableValueF(struct table3D *fromTable, int Y, int X)
   }
   return tableResult;
 }
-*/
+
 //This function pulls a value from a 3D table given a target for X and Y coordinates.
 //It performs a 2D linear interpolation as descibred in: http://www.megamanual.com/v22manual/ve_tuner.pdf
 int get3DTableValueS(struct table3D *fromTable, int Y, int X)
@@ -605,4 +605,4 @@ int get3DTableValueS(struct table3D *fromTable, int Y, int X)
  }
   return tableResult;
 }
-
+*/
diff --git a/speeduino/timers.ino b/speeduino/timers.ino
index 76095a7e..0178df71 100644
--- a/speeduino/timers.ino
+++ b/speeduino/timers.ino
@@ -47,6 +47,9 @@ void initialiseTimers()
   Timer4.resume(); //Start Timer
 #endif
 
+  #if defined(CORE_STM32)
+    pinMode(LED_BUILTIN, OUTPUT);
+  #endif
   dwellLimit_uS = (1000 * configPage2.dwellLimit);
   lastRPM_100ms = 0;
 }
@@ -100,6 +103,9 @@ void oneMSInterval() //Most ARM chips can simply call a function
   {
     loop100ms = 0; //Reset counter
     BIT_SET(TIMER_mask, BIT_TIMER_10HZ);
+    #if defined(CORE_STM32) //debug purpose, only visal for running code
+      digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
+    #endif  
 
     currentStatus.rpmDOT = (currentStatus.RPM - lastRPM_100ms) * 10; //This is the RPM per second that the engine has accelerated/decelleratedin the last loop
     lastRPM_100ms = currentStatus.RPM; //Record the current RPM for next calc
diff --git a/speeduino/utils.ino b/speeduino/utils.ino
index 90543e8f..994c7f39 100644
--- a/speeduino/utils.ino
+++ b/speeduino/utils.ino
@@ -196,11 +196,11 @@ void setPinMapping(byte boardID)
         pinCoil3 = 30;
         pinO2 = A22;
       #elif defined(STM32F4)
-        pinInjector1 = PE11; //Output pin injector 1 is on
-        pinInjector2 = PE12; //Output pin injector 2 is on
-        pinInjector3 = PE13; //Output pin injector 3 is on
-        pinInjector4 = PE14; //Output pin injector 4 is on
-        pinInjector5 = PE15; //Output pin injector 4 is on
+        pinInjector1 = PE7; //Output pin injector 1 is on
+        pinInjector2 = PE8; //Output pin injector 2 is on
+        pinInjector3 = PE9; //Output pin injector 3 is on
+        pinInjector4 = PE10; //Output pin injector 4 is on
+        pinInjector5 = PE11; //Output pin injector 5 is on
         pinCoil1 = PB10; //Pin for coil 1
         pinCoil2 = PB11; //Pin for coil 2
         pinCoil3 = PB12; //Pin for coil 3
@@ -224,6 +224,8 @@ void setPinMapping(byte boardID)
         pinFlex = PC4; // Flex sensor (Must be external interrupt enabled)
         pinTrigger = PC5; //The CAS pin
         pinTrigger2 = PC6; //The Cam Sensor pin
+        pinBoost = PE0; //Boost control
+        pinVVT_1 = PE1; //Default VVT output
       #elif defined(CORE_STM32)
         //http://docs.leaflabs.com/static.leaflabs.com/pub/leaflabs/maple-docs/0.0.12/hardware/maple-mini.html#master-pin-map
         //pins 23, 24 and 33 couldn't be used
@@ -254,6 +256,8 @@ void setPinMapping(byte boardID)
         pinTrigger = 25; //The CAS pin
         pinTrigger2 = 22; //The Cam Sensor pin
         pinBaro = pinMAP;
+        pinBoost = 1; //Boost control
+        pinVVT_1 = 0; //Default VVT output
       #endif
       break;
 

From bf21e87829f4e5ed317faa5ad8f5491f84d50cb1 Mon Sep 17 00:00:00 2001
From: VitorBoss <vitor_boss@yahoo.com.br>
Date: Mon, 7 Aug 2017 22:05:35 -0300
Subject: [PATCH 09/10] MISRA work

---
 speeduino/auxiliaries.ino |    3 +-
 speeduino/speeduino.ino   | 3295 ++++++++++++++++++-------------------
 2 files changed, 1649 insertions(+), 1649 deletions(-)

diff --git a/speeduino/auxiliaries.ino b/speeduino/auxiliaries.ino
index e05a3a41..d7861347 100644
--- a/speeduino/auxiliaries.ino
+++ b/speeduino/auxiliaries.ino
@@ -3,7 +3,8 @@ Speeduino - Simple engine management for the Arduino Mega 2560 platform
 Copyright (C) Josh Stewart
 A full copy of the license may be found in the projects root directory
 */
-//integerPID boostPID(&MAPx100, &boost_pwm_target_value, &boostTargetx100, configPage3.boostKP, configPage3.boostKI, configPage3.boostKD, DIRECT); //This is the PID object if that algorithm is used. Needs to be global as it maintains state outside of each function call
+//Old PID method. Retained incase the new one has issues
+//integerPID boostPID(&MAPx100, &boost_pwm_target_value, &boostTargetx100, configPage3.boostKP, configPage3.boostKI, configPage3.boostKD, DIRECT);
 integerPIDnew boostPID(&currentStatus.MAP, &boost_pwm_target_value, &boost_cl_target_boost, configPage3.boostKP, configPage3.boostKI, configPage3.boostKD, DIRECT); //This is the PID object if that algorithm is used. Needs to be global as it maintains state outside of each function call
 
 /*
diff --git a/speeduino/speeduino.ino b/speeduino/speeduino.ino
index cf69da6c..159ff6c7 100644
--- a/speeduino/speeduino.ino
+++ b/speeduino/speeduino.ino
@@ -1,1649 +1,1648 @@
-/*
-Speeduino - Simple engine management for the Arduino Mega 2560 platform
-Copyright (C) Josh Stewart
-
-This program is free software; you can redistribute it and/or
-modify it under the terms of the GNU General Public License
-as published by the Free Software Foundation; either version 2
-of the License, or (at your option) any later version.
-
-This program is distributed in the hope that it will be useful,la
-but WITHOUT ANY WARRANTY; without even the implied warranty of
-MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-GNU General Public License for more details.
-
-You should have received a copy of the GNU General Public License
-along with this program; if not, write to the Free Software
-Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
-*/
-
-//**************************************************************************************************
-// Config section
-#define engineSquirtsPerCycle 2 //Would be 1 for a 2 stroke
-//**************************************************************************************************
-
-//https://developer.mbed.org/handbook/C-Data-Types
-#include <stdint.h>
-//************************************************
-#include "globals.h"
-#include "utils.h"
-#include "table.h"
-#include "scheduler.h"
-#include "comms.h"
-#include "cancomms.h"
-#include "maths.h"
-#include "corrections.h"
-#include "timers.h"
-//#include "display.h"
-#include "decoders.h"
-#include "idle.h"
-#include "auxiliaries.h"
-#include "sensors.h"
-#include "src/PID_v1/PID_v1.h"
-//#include "src/DigitalWriteFast/digitalWriteFast.h"
-#include "errors.h"
-#include "storage.h"
-#include "scheduledIO.h"
-#include <EEPROM.h>
-#if defined (CORE_TEENSY)
-#include <FlexCAN.h>
-#endif
-
-struct config1 configPage1;
-struct config2 configPage2;
-struct config3 configPage3;
-struct config4 configPage4;
-struct config10 configPage10;
-struct config11 configPage11;
-
-int req_fuel_uS, inj_opentime_uS;
-
-volatile byte ign1LastRev;
-volatile byte ign2LastRev;
-volatile byte ign3LastRev;
-volatile byte ign4LastRev;
-volatile byte ign5LastRev;
-bool ignitionOn = false; //The current state of the ignition system
-bool fuelOn = false; //The current state of the ignition system
-bool fuelPumpOn = false; //The current status of the fuel pump
-
-void (*trigger)(); //Pointer for the trigger function (Gets pointed to the relevant decoder)
-void (*triggerSecondary)(); //Pointer for the secondary trigger function (Gets pointed to the relevant decoder)
-uint16_t (*getRPM)(); //Pointer to the getRPM function (Gets pointed to the relevant decoder)
-int (*getCrankAngle)(int); //Pointer to the getCrank Angle function (Gets pointed to the relevant decoder)
-void (*triggerSetEndTeeth)(); //Pointer to the triggerSetEndTeeth function of each decoder
-
-byte cltCalibrationTable[CALIBRATION_TABLE_SIZE];
-byte iatCalibrationTable[CALIBRATION_TABLE_SIZE];
-byte o2CalibrationTable[CALIBRATION_TABLE_SIZE];
-
-//These variables are used for tracking the number of running sensors values that appear to be errors. Once a threshold is reached, the sensor reading will go to default value and assume the sensor is faulty
-byte mapErrorCount = 0;
-byte iatErrorCount = 0;
-byte cltErrorCount = 0;
-
-unsigned long counter;
-unsigned long currentLoopTime; //The time the current loop started (uS)
-unsigned long previousLoopTime; //The time the previous loop started (uS)
-
-int CRANK_ANGLE_MAX = 720;
-int CRANK_ANGLE_MAX_IGN = 360;
-int CRANK_ANGLE_MAX_INJ = 360; // The number of crank degrees that the system track over. 360 for wasted / timed batch and 720 for sequential
-
-static byte coilHIGH = HIGH;
-static byte coilLOW = LOW;
-static byte fanHIGH = HIGH;             // Used to invert the cooling fan output
-static byte fanLOW = LOW;               // Used to invert the cooling fan output
-
-volatile int mainLoopCount;
-byte deltaToothCount = 0; //The last tooth that was used with the deltaV calc
-int rpmDelta;
-byte ignitionCount;
-uint16_t fixedCrankingOverride = 0;
-int16_t lastAdvance; //Stores the previous advance figure to track changes.
-bool clutchTrigger;
-bool previousClutchTrigger;
-
-unsigned long secCounter; //The next time to incremen 'runSecs' counter.
-int channel1IgnDegrees; //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 channel2IgnDegrees; //The number of crank degrees until cylinder 2 (and 5/6/7/8) is at TDC
-int channel3IgnDegrees; //The number of crank degrees until cylinder 3 (and 5/6/7/8) is at TDC
-int channel4IgnDegrees; //The number of crank degrees until cylinder 4 (and 5/6/7/8) is at TDC
-int channel5IgnDegrees; //The number of crank degrees until cylinder 5 is at TDC
-int channel1InjDegrees; //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 channel2InjDegrees; //The number of crank degrees until cylinder 2 (and 5/6/7/8) is at TDC
-int channel3InjDegrees; //The number of crank degrees until cylinder 3 (and 5/6/7/8) is at TDC
-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
-
-//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)();
-void (*ign2StartFunction)();
-void (*ign2EndFunction)();
-void (*ign3StartFunction)();
-void (*ign3EndFunction)();
-void (*ign4StartFunction)();
-void (*ign4EndFunction)();
-void (*ign5StartFunction)();
-void (*ign5EndFunction)();
-
-int timePerDegree;
-byte degreesPerLoop; //The number of crank degrees that pass for each mainloop of the program
-volatile bool fpPrimed = false; //Tracks whether or not the fuel pump priming has been completed yet
-bool initialisationComplete = false; //Tracks whether the setup() functino has run completely
-
-void setup()
-{
-
-  //Setup the dummy fuel and ignition tables
-  //dummyFuelTable(&fuelTable);
-  //dummyIgnitionTable(&ignitionTable);
-  table3D_setSize(&fuelTable, 16);
-  table3D_setSize(&ignitionTable, 16);
-  table3D_setSize(&afrTable, 16);
-  table3D_setSize(&boostTable, 8);
-  table3D_setSize(&vvtTable, 8);
-  table3D_setSize(&trim1Table, 6);
-  table3D_setSize(&trim2Table, 6);
-  table3D_setSize(&trim3Table, 6);
-  table3D_setSize(&trim4Table, 6);
-  Serial.begin(115200);
-
-  #if defined(CORE_STM32)
-    EEPROM.init();
-  #endif
-  loadConfig();
-  doUpdates(); //Check if any data items need updating (Occurs ith firmware updates)
-
-#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) //ATmega2561 does not have Serial3
-  if (configPage10.enable_canbus == 1) { CANSerial.begin(115200); }
-#elif defined(CORE_STM32)
-  if (configPage10.enable_canbus == 1) { CANSerial.begin(115200); }
-  else if (configPage10.enable_canbus == 2)
-  {
-    //enable local can interface
-  }
-#elif defined(CORE_TEENSY)
-  if (configPage10.enable_canbus == 1) { CANSerial.begin(115200); }
-  else if (configPage10.enable_canbus == 2)
-  {
-    //Teensy onboard CAN not used currently
-    //enable local can interface
-    //setup can interface to 250k
-    //FlexCAN CANbus0(2500000, 0);
-    //static CAN_message_t txmsg,rxmsg;
-    //CANbus0.begin();
-  }
-#endif
-
-  //Repoint the 2D table structs to the config pages that were just loaded
-  taeTable.valueSize = SIZE_BYTE; //Set this table to use byte values
-  taeTable.xSize = 4;
-  taeTable.values = configPage2.taeValues;
-  taeTable.axisX = configPage2.taeBins;
-  WUETable.valueSize = SIZE_BYTE; //Set this table to use byte values
-  WUETable.xSize = 10;
-  WUETable.values = configPage1.wueValues;
-  WUETable.axisX = configPage2.wueBins;
-  crankingEnrichTable.valueSize = SIZE_BYTE;
-  crankingEnrichTable.xSize = 4;
-  crankingEnrichTable.values = configPage11.crankingEnrichValues;
-  crankingEnrichTable.axisX = configPage11.crankingEnrichBins;
-
-  dwellVCorrectionTable.valueSize = SIZE_BYTE;
-  dwellVCorrectionTable.xSize = 6;
-  dwellVCorrectionTable.values = configPage2.dwellCorrectionValues;
-  dwellVCorrectionTable.axisX = configPage3.voltageCorrectionBins;
-  injectorVCorrectionTable.valueSize = SIZE_BYTE;
-  injectorVCorrectionTable.xSize = 6;
-  injectorVCorrectionTable.values = configPage3.injVoltageCorrectionValues;
-  injectorVCorrectionTable.axisX = configPage3.voltageCorrectionBins;
-  IATDensityCorrectionTable.valueSize = SIZE_BYTE;
-  IATDensityCorrectionTable.xSize = 9;
-  IATDensityCorrectionTable.values = configPage3.airDenRates;
-  IATDensityCorrectionTable.axisX = configPage3.airDenBins;
-  IATRetardTable.valueSize = SIZE_BYTE;
-  IATRetardTable.xSize = 6;
-  IATRetardTable.values = configPage2.iatRetValues;
-  IATRetardTable.axisX = configPage2.iatRetBins;
-
-  //Setup the calibration tables
-  loadCalibration();
-
-  //Set the pin mappings
-  if(configPage1.pinMapping > BOARD_NR_GPIO_PINS)
-  {
-    //First time running on this board
-    setPinMapping(3); //Force board to v0.4
-    configPage1.flexEnabled = false; //Have to disable flex. If this isn't done and the wrong flex pin is interrupt attached below, system can hang.
-  }
-  else { setPinMapping(configPage1.pinMapping); }
-
-  //Need to check early on whether the coil charging is inverted. If this is not set straight away it can cause an unwanted spark at bootup
-  if(configPage2.IgInv == 1) { coilHIGH = LOW, coilLOW = HIGH; }
-  else { coilHIGH = HIGH, coilLOW = LOW; }
-  endCoil1Charge();
-  endCoil2Charge();
-  endCoil3Charge();
-  endCoil4Charge();
-  endCoil5Charge();
-
-  //Similar for injectors, make sure they're turned off
-  closeInjector1();
-  closeInjector2();
-  closeInjector3();
-  closeInjector4();
-  closeInjector5();
-
-  //Set the tacho output default state
-  digitalWrite(pinTachOut, HIGH);
-
-  //Perform all initialisations
-  initialiseSchedulers();
-  initialiseTimers();
-  //initialiseDisplay();
-  initialiseIdle();
-  initialiseFan();
-  initialiseAuxPWM();
-  initialiseCorrections();
-  initialiseADC();
-
-  //Lookup the current MAP reading for barometric pressure
-  instanteneousMAPReading();
-  //barometric reading can be taken from either an external sensor if enabled, or simply by using the initial MAP value
-  if ( configPage3.useExtBaro != 0 )
-  {
-    readBaro();
-    EEPROM.update(EEPROM_LAST_BARO, currentStatus.baro);
-  }
-  else
-  {
-    /*
-     * The highest sea-level pressure on Earth occurs in Siberia, where the Siberian High often attains a sea-level pressure above 105 kPa;
-     * with record highs close to 108.5 kPa.
-     * The lowest measurable sea-level pressure is found at the centers of tropical cyclones and tornadoes, with a record low of 87 kPa;
-     */
-    if ((currentStatus.MAP >= BARO_MIN) && (currentStatus.MAP <= BARO_MAX)) //Check if engine isn't running
-    {
-      currentStatus.baro = currentStatus.MAP;
-      EEPROM.update(EEPROM_LAST_BARO, currentStatus.baro);
-    }
-    else
-    {
-      //Attempt to use the last known good baro reading from EEPROM
-      if ((EEPROM.read(EEPROM_LAST_BARO) >= BARO_MIN) && (EEPROM.read(EEPROM_LAST_BARO) <= BARO_MAX)) //Make sure it's not invalid (Possible on first run etc)
-      { currentStatus.baro = EEPROM.read(EEPROM_LAST_BARO); } //last baro correction
-      else { currentStatus.baro = 100; } //Final fall back position.
-    }
-  }
-
-  //Check whether the flex sensor is enabled and if so, attach an interupt for it
-  if(configPage1.flexEnabled)
-  {
-    attachInterrupt(digitalPinToInterrupt(pinFlex), flexPulse, RISING);
-    currentStatus.ethanolPct = 0;
-  }
-
-  //Once the configs have been loaded, a number of one time calculations can be completed
-  req_fuel_uS = configPage1.reqFuel * 100; //Convert to uS and an int. This is the only variable to be used in calculations
-  inj_opentime_uS = configPage1.injOpen * 100; //Injector open time. Comes through as ms*10 (Eg 15.5ms = 155).
-
-  //Begin the main crank trigger interrupt pin setup
-  //The interrupt numbering is a bit odd - See here for reference: http://arduino.cc/en/Reference/AttachInterrupt
-  //These assignments are based on the Arduino Mega AND VARY BETWEEN BOARDS. Please confirm the board you are using and update acordingly.
-  byte triggerInterrupt = 0; // By default, use the first interrupt
-  byte triggerInterrupt2 = 1;
-  currentStatus.RPM = 0;
-  currentStatus.hasSync = false;
-  currentStatus.runSecs = 0;
-  currentStatus.secl = 0;
-  currentStatus.startRevolutions = 0;
-  currentStatus.flatShiftingHard = false;
-  currentStatus.launchingHard = false;
-  triggerFilterTime = 0; //Trigger filter time is the shortest possible time (in uS) that there can be between crank teeth (ie at max RPM). Any pulses that occur faster than this time will be disgarded as noise. This is simply a default value, the actual values are set in the setup() functinos of each decoder
-
-  #if defined(CORE_AVR)
-    switch (pinTrigger) {
-      //Arduino Mega 2560 mapping
-      case 2:
-        triggerInterrupt = 0; break;
-      case 3:
-        triggerInterrupt = 1; break;
-      case 18:
-        triggerInterrupt = 5; break;
-      case 19:
-        triggerInterrupt = 4; break;
-      case 20:
-        triggerInterrupt = 3; break;
-      case 21:
-        triggerInterrupt = 2; break;
-      default:
-        triggerInterrupt = 0; break; //This should NEVER happen
-    }
-  #else
-    triggerInterrupt = pinTrigger;
-  #endif
-
-  #if defined(CORE_AVR)
-    switch (pinTrigger2) {
-      //Arduino Mega 2560 mapping
-      case 2:
-        triggerInterrupt2 = 0; break;
-      case 3:
-        triggerInterrupt2 = 1; break;
-      case 18:
-        triggerInterrupt2 = 5; break;
-      case 19:
-        triggerInterrupt2 = 4; break;
-      case 20:
-        triggerInterrupt2 = 3; break;
-      case 21:
-        triggerInterrupt2 = 2; break;
-      default:
-        triggerInterrupt2 = 0; break; //This should NEVER happen
-    }
-  #else
-    triggerInterrupt2 = pinTrigger2;
-  #endif
-  pinMode(pinTrigger, INPUT);
-  pinMode(pinTrigger2, INPUT);
-  pinMode(pinTrigger3, INPUT);
-  //digitalWrite(pinTrigger, HIGH);
-
-  //Set the trigger function based on the decoder in the config
-  switch (configPage2.TrigPattern)
-  {
-    case 0:
-      //Missing tooth decoder
-      triggerSetup_missingTooth();
-      trigger = triggerPri_missingTooth;
-      triggerSecondary = triggerSec_missingTooth;
-      getRPM = getRPM_missingTooth;
-      getCrankAngle = getCrankAngle_missingTooth;
-      triggerSetEndTeeth = triggerSetEndTeeth_missingTooth;
-
-      if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
-      else { attachInterrupt(triggerInterrupt, trigger, FALLING); }
-      if(configPage2.TrigEdgeSec == 0) { attachInterrupt(triggerInterrupt2, triggerSec_missingTooth, RISING); }
-      else { attachInterrupt(triggerInterrupt2, triggerSec_missingTooth, FALLING); }
-      break;
-
-    case 1:
-      // Basic distributor
-      triggerSetup_BasicDistributor();
-      trigger = triggerPri_BasicDistributor;
-      getRPM = getRPM_BasicDistributor;
-      getCrankAngle = getCrankAngle_BasicDistributor;
-      triggerSetEndTeeth = triggerSetEndTeeth_BasicDistributor;
-
-      if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
-      else { attachInterrupt(triggerInterrupt, trigger, FALLING); }
-      break;
-
-    case 2:
-      triggerSetup_DualWheel();
-      trigger = triggerPri_DualWheel;
-      getRPM = getRPM_DualWheel;
-      getCrankAngle = getCrankAngle_DualWheel;
-      triggerSetEndTeeth = triggerSetEndTeeth_DualWheel;
-
-      if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
-      else { attachInterrupt(triggerInterrupt, trigger, FALLING); }
-      if(configPage2.TrigEdgeSec == 0) { attachInterrupt(triggerInterrupt2, triggerSec_DualWheel, RISING); }
-      else { attachInterrupt(triggerInterrupt2, triggerSec_DualWheel, FALLING); }
-      break;
-
-    case 3:
-      triggerSetup_GM7X();
-      trigger = triggerPri_GM7X;
-      getRPM = getRPM_GM7X;
-      getCrankAngle = getCrankAngle_GM7X;
-      triggerSetEndTeeth = triggerSetEndTeeth_GM7X;
-
-      if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
-      else { attachInterrupt(triggerInterrupt, trigger, FALLING); }
-      break;
-
-    case 4:
-      triggerSetup_4G63();
-      trigger = triggerPri_4G63;
-      getRPM = getRPM_4G63;
-      getCrankAngle = getCrankAngle_4G63;
-      triggerSetEndTeeth = triggerSetEndTeeth_4G63;
-
-      //These may both need to change, not sure
-      if(configPage2.TrigEdge == 0)
-      {
-        attachInterrupt(triggerInterrupt, trigger, CHANGE);  // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
-        attachInterrupt(triggerInterrupt2, triggerSec_4G63, FALLING); //changed
-      }
-      else
-      {
-        attachInterrupt(triggerInterrupt, trigger, CHANGE); // Primary trigger connects to
-        attachInterrupt(triggerInterrupt2, triggerSec_4G63, FALLING);
-      }
-      break;
-
-    case 5:
-      triggerSetup_24X();
-      trigger = triggerPri_24X;
-      getRPM = getRPM_24X;
-      getCrankAngle = getCrankAngle_24X;
-      triggerSetEndTeeth = triggerSetEndTeeth_24X;
-
-      if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
-      else { attachInterrupt(triggerInterrupt, trigger, FALLING); } // Primary trigger connects to
-      attachInterrupt(triggerInterrupt2, triggerSec_24X, CHANGE);
-      break;
-
-    case 6:
-      triggerSetup_Jeep2000();
-      trigger = triggerPri_Jeep2000;
-      getRPM = getRPM_Jeep2000;
-      getCrankAngle = getCrankAngle_Jeep2000;
-      triggerSetEndTeeth = triggerSetEndTeeth_Jeep2000;
-
-      if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
-      else { attachInterrupt(triggerInterrupt, trigger, FALLING); } // Primary trigger connects to
-      attachInterrupt(triggerInterrupt2, triggerSec_Jeep2000, CHANGE);
-      break;
-
-    case 7:
-      triggerSetup_Audi135();
-      trigger = triggerPri_Audi135;
-      getRPM = getRPM_Audi135;
-      getCrankAngle = getCrankAngle_Audi135;
-      triggerSetEndTeeth = triggerSetEndTeeth_Audi135;
-
-      if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
-      else { attachInterrupt(triggerInterrupt, trigger, FALLING); }
-      attachInterrupt(triggerInterrupt2, triggerSec_Audi135, RISING);
-      break;
-
-    case 8:
-      triggerSetup_HondaD17();
-      trigger = triggerPri_HondaD17;
-      getRPM = getRPM_HondaD17;
-      getCrankAngle = getCrankAngle_HondaD17;
-      triggerSetEndTeeth = triggerSetEndTeeth_HondaD17;
-
-      if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
-      else { attachInterrupt(triggerInterrupt, trigger, FALLING); } // Primary trigger connects to
-      attachInterrupt(triggerInterrupt2, triggerSec_HondaD17, CHANGE);
-      break;
-
-    case 9:
-      triggerSetup_Miata9905();
-      trigger = triggerPri_Miata9905;
-      getRPM = getRPM_Miata9905;
-      getCrankAngle = getCrankAngle_Miata9905;
-      triggerSetEndTeeth = triggerSetEndTeeth_Miata9905;
-
-      //These may both need to change, not sure
-      if(configPage2.TrigEdge == 0)
-      {
-        attachInterrupt(triggerInterrupt, trigger, RISING);  // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
-        attachInterrupt(triggerInterrupt2, triggerSec_Miata9905, FALLING); //changed
-      }
-      else
-      {
-        attachInterrupt(triggerInterrupt, trigger, FALLING); // Primary trigger connects to
-        attachInterrupt(triggerInterrupt2, triggerSec_Miata9905, RISING);
-      }
-      break;
-
-    case 10:
-      triggerSetup_MazdaAU();
-      trigger = triggerPri_MazdaAU;
-      getRPM = getRPM_MazdaAU;
-      getCrankAngle = getCrankAngle_MazdaAU;
-      triggerSetEndTeeth = triggerSetEndTeeth_MazdaAU;
-
-      if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
-      else { attachInterrupt(triggerInterrupt, trigger, FALLING); } // Primary trigger connects to
-      attachInterrupt(triggerInterrupt2, triggerSec_MazdaAU, FALLING);
-      break;
-
-    case 11:
-      triggerSetup_non360();
-      trigger = triggerPri_DualWheel; //Is identical to the dual wheel decoder, so that is used. Same goes for the secondary below
-      getRPM = getRPM_non360;
-      getCrankAngle = getCrankAngle_non360;
-      triggerSetEndTeeth = triggerSetEndTeeth_Non360;
-
-      if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
-      else { attachInterrupt(triggerInterrupt, trigger, FALLING); }
-      attachInterrupt(triggerInterrupt2, triggerSec_DualWheel, FALLING); //Note the use of the Dual Wheel trigger function here. No point in having the same code in twice.
-      break;
-
-    case 12:
-        triggerSetup_Nissan360();
-        trigger = triggerPri_Nissan360;
-        getRPM = getRPM_Nissan360;
-        getCrankAngle = getCrankAngle_Nissan360;
-        triggerSetEndTeeth = triggerSetEndTeeth_Nissan360;
-
-        if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
-        else { attachInterrupt(triggerInterrupt, trigger, FALLING); }
-        attachInterrupt(triggerInterrupt2, triggerSec_Nissan360, CHANGE);
-        break;
-
-    case 13:
-            triggerSetup_Subaru67();
-            trigger = triggerPri_Subaru67;
-            getRPM = getRPM_Subaru67;
-            getCrankAngle = getCrankAngle_Subaru67;
-            triggerSetEndTeeth = triggerSetEndTeeth_Subaru67;
-
-            if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
-            else { attachInterrupt(triggerInterrupt, trigger, FALLING); }
-            attachInterrupt(triggerInterrupt2, triggerSec_Subaru67, FALLING);
-            break;
-
-    case 14:
-            triggerSetup_Daihatsu();
-            trigger = triggerPri_Daihatsu;
-            getRPM = getRPM_Daihatsu;
-            getCrankAngle = getCrankAngle_Daihatsu;
-            triggerSetEndTeeth = triggerSetEndTeeth_Daihatsu;
-
-            if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
-            else { attachInterrupt(triggerInterrupt, trigger, FALLING); }
-            //No secondary input required for this pattern
-            break;
-
-    default:
-      trigger = triggerPri_missingTooth;
-      getRPM = getRPM_missingTooth;
-      getCrankAngle = getCrankAngle_missingTooth;
-
-      if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
-      else { attachInterrupt(triggerInterrupt, trigger, FALLING); }
-      break;
-  }
-
-  //End crank triger interrupt attachment
-
-  req_fuel_uS = req_fuel_uS / engineSquirtsPerCycle; //The req_fuel calculation above gives the total required fuel (At VE 100%) in the full cycle. If we're doing more than 1 squirt per cycle then we need to split the amount accordingly. (Note that in a non-sequential 4-stroke setup you cannot have less than 2 squirts as you cannot determine the stroke to make the single squirt on)
-
-  //Initial values for loop times
-  previousLoopTime = 0;
-  currentLoopTime = micros();
-
-  mainLoopCount = 0;
-  ignitionCount = 0;
-
-  //Calculate the number of degrees between cylinders
-  switch (configPage1.nCylinders) {
-    case 1:
-      channel1IgnDegrees = 0;
-      channel1InjDegrees = 0;
-
-      channel1InjEnabled = true;
-      break;
-
-    case 2:
-      channel1IgnDegrees = 0;
-
-      if (configPage1.engineType == EVEN_FIRE )
-      {
-        channel2IgnDegrees = 180;
-      }
-      else { channel2IgnDegrees = configPage1.oddfire2; }
-
-      //For alternating injection, the squirt occurs at different times for each channel
-      if(configPage1.injLayout == INJ_SEMISEQUENTIAL)
-      {
-        channel1InjDegrees = 0;
-        channel2InjDegrees = channel2IgnDegrees; //Set to the same as the ignition degrees (Means there's no need for another if to check for oddfire)
-      }
-      if (!configPage1.injTiming) { channel1InjDegrees = channel2InjDegrees = 0; } //For simultaneous, all squirts happen at the same time
-
-      channel1InjEnabled = true;
-      channel2InjEnabled = true;
-      break;
-
-    case 3:
-      channel1IgnDegrees = 0;
-
-      if (configPage1.engineType == EVEN_FIRE )
-      {
-        if(configPage2.sparkMode == IGN_MODE_SEQUENTIAL)
-        {
-          channel2IgnDegrees = 240;
-          channel3IgnDegrees = 480;
-
-          CRANK_ANGLE_MAX_IGN = 720;
-        }
-        else
-        {
-          channel2IgnDegrees = 120;
-          channel3IgnDegrees = 240;
-        }
-      }
-      else
-      {
-        channel2IgnDegrees = configPage1.oddfire2;
-        channel3IgnDegrees = configPage1.oddfire3;
-      }
-
-      //For alternatiing injection, the squirt occurs at different times for each channel
-      if(configPage1.injLayout == INJ_SEMISEQUENTIAL  || configPage1.injLayout == INJ_PAIRED)
-      {
-        channel1InjDegrees = 0;
-        channel2InjDegrees = 120;
-        channel3InjDegrees = 240;
-      }
-      else if (configPage1.injLayout == INJ_SEQUENTIAL)
-      {
-        channel1InjDegrees = 0;
-        channel2InjDegrees = 240;
-        channel3InjDegrees = 480;
-        CRANK_ANGLE_MAX_INJ = 720;
-        req_fuel_uS = req_fuel_uS * 2;
-      }
-      if (!configPage1.injTiming) { channel1InjDegrees = channel2InjDegrees = channel3InjDegrees = 0; } //For simultaneous, all squirts happen at the same time
-
-      channel1InjEnabled = true;
-      channel2InjEnabled = true;
-      channel3InjEnabled = true;
-      break;
-    case 4:
-      channel1IgnDegrees = 0;
-
-      if (configPage1.engineType == EVEN_FIRE )
-      {
-        channel2IgnDegrees = 180;
-
-        if(configPage2.sparkMode == IGN_MODE_SEQUENTIAL)
-        {
-          channel3IgnDegrees = 360;
-          channel4IgnDegrees = 540;
-
-          CRANK_ANGLE_MAX_IGN = 720;
-        }
-      }
-      else
-      {
-        channel2IgnDegrees = configPage1.oddfire2;
-        channel3IgnDegrees = configPage1.oddfire3;
-        channel4IgnDegrees = configPage1.oddfire4;
-      }
-
-      //For alternatiing injection, the squirt occurs at different times for each channel
-      if(configPage1.injLayout == INJ_SEMISEQUENTIAL || configPage1.injLayout == INJ_PAIRED)
-      {
-        channel1InjDegrees = 0;
-        channel2InjDegrees = 180;
-      }
-      else if (configPage1.injLayout == INJ_SEQUENTIAL)
-      {
-        channel1InjDegrees = 0;
-        channel2InjDegrees = 180;
-        channel3InjDegrees = 360;
-        channel4InjDegrees = 540;
-
-        channel3InjEnabled = true;
-        channel4InjEnabled = true;
-
-        CRANK_ANGLE_MAX_INJ = 720;
-        req_fuel_uS = req_fuel_uS * 2;
-      }
-      if (!configPage1.injTiming) { channel1InjDegrees = channel2InjDegrees = 0; } //For simultaneous, all squirts happen at the same time
-
-      channel1InjEnabled = true;
-      channel2InjEnabled = true;
-      break;
-    case 5:
-      channel1IgnDegrees = 0;
-      channel2IgnDegrees = 72;
-      channel3IgnDegrees = 144;
-      channel4IgnDegrees = 216;
-      channel5IgnDegrees = 288;
-
-      if(configPage2.sparkMode == IGN_MODE_SEQUENTIAL)
-      {
-        channel2IgnDegrees = 144;
-        channel3IgnDegrees = 288;
-        channel4IgnDegrees = 432;
-        channel5IgnDegrees = 576;
-
-        CRANK_ANGLE_MAX_IGN = 720;
-      }
-
-      //For alternatiing injection, the squirt occurs at different times for each channel
-      if(configPage1.injLayout == INJ_SEMISEQUENTIAL || configPage1.injLayout == INJ_PAIRED)
-      {
-        channel1InjDegrees = 0;
-        channel2InjDegrees = 72;
-        channel3InjDegrees = 144;
-        channel4InjDegrees = 216;
-        channel5InjDegrees = 288;
-      }
-      else if (configPage1.injLayout == INJ_SEQUENTIAL)
-      {
-        channel1InjDegrees = 0;
-        channel2InjDegrees = 144;
-        channel3InjDegrees = 288;
-        channel4InjDegrees = 432;
-        channel5InjDegrees = 576;
-
-        CRANK_ANGLE_MAX_INJ = 720;
-      }
-      if (!configPage1.injTiming) { channel1InjDegrees = channel2InjDegrees = channel3InjDegrees = channel4InjDegrees = channel5InjDegrees = 0; } //For simultaneous, all squirts happen at the same time
-
-      channel1InjEnabled = true;
-      channel2InjEnabled = true;
-      channel3InjEnabled = false; //this is disabled as injector 5 function calls 3 & 5 together
-      channel4InjEnabled = true;
-      channel5InjEnabled = true;
-      break;
-    case 6:
-      channel1IgnDegrees = 0;
-      channel1InjDegrees = 0;
-      channel2IgnDegrees = 120;
-      channel2InjDegrees = 120;
-      channel3IgnDegrees = 240;
-      channel3InjDegrees = 240;
-
-      if (!configPage1.injTiming) { channel1InjDegrees = channel2InjDegrees = channel3InjDegrees = 0; } //For simultaneous, all squirts happen at the same time
-
-      configPage1.injLayout = 0; //This is a failsafe. We can never run semi-sequential with more than 4 cylinders
-
-      channel1InjEnabled = true;
-      channel2InjEnabled = true;
-      channel3InjEnabled = true;
-      break;
-    case 8:
-      channel1IgnDegrees = channel1InjDegrees = 0;
-      channel2IgnDegrees = channel2InjDegrees = 90;
-      channel3IgnDegrees = channel3InjDegrees = 180;
-      channel4IgnDegrees = channel4InjDegrees = 270;
-
-      if (!configPage1.injTiming)  { channel1InjDegrees = channel2InjDegrees = channel3InjDegrees = channel4InjDegrees = 0; } //For simultaneous, all squirts happen at the same time
-
-      configPage1.injLayout = 0; //This is a failsafe. We can never run semi-sequential with more than 4 cylinders
-
-      channel1InjEnabled = true;
-      channel2InjEnabled = true;
-      channel3InjEnabled = true;
-      channel4InjEnabled = true;
-      break;
-    default: //Handle this better!!!
-      channel1InjDegrees = 0;
-      channel2InjDegrees = 180;
-      break;
-  }
-
-  switch(configPage2.sparkMode)
-  {
-    case IGN_MODE_WASTED:
-      //Wasted Spark (Normal mode)
-      ign1StartFunction = beginCoil1Charge;
-      ign1EndFunction = endCoil1Charge;
-      ign2StartFunction = beginCoil2Charge;
-      ign2EndFunction = endCoil2Charge;
-      ign3StartFunction = beginCoil3Charge;
-      ign3EndFunction = endCoil3Charge;
-      ign4StartFunction = beginCoil4Charge;
-      ign4EndFunction = endCoil4Charge;
-      ign5StartFunction = beginCoil5Charge;
-      ign5EndFunction = endCoil5Charge;
-      break;
-
-    case IGN_MODE_SINGLE:
-      //Single channel mode. All ignition pulses are on channel 1
-      ign1StartFunction = beginCoil1Charge;
-      ign1EndFunction = endCoil1Charge;
-      ign2StartFunction = beginCoil1Charge;
-      ign2EndFunction = endCoil1Charge;
-      ign3StartFunction = beginCoil1Charge;
-      ign3EndFunction = endCoil1Charge;
-      ign4StartFunction = beginCoil1Charge;
-      ign4EndFunction = endCoil1Charge;
-      ign5StartFunction = beginCoil1Charge;
-      ign5EndFunction = endCoil1Charge;
-      break;
-
-    case IGN_MODE_WASTEDCOP:
-      //Wasted COP mode. Ignition channels 1&3 and 2&4 are paired together
-      //This is not a valid mode for >4 cylinders
-      if( configPage1.nCylinders <= 4 )
-      {
-        ign1StartFunction = beginCoil1and3Charge;
-        ign1EndFunction = endCoil1and3Charge;
-        ign2StartFunction = beginCoil2and4Charge;
-        ign2EndFunction = endCoil2and4Charge;
-
-        ign3StartFunction = nullCallback;
-        ign3EndFunction = nullCallback;
-        ign4StartFunction = nullCallback;
-        ign4EndFunction = nullCallback;
-      }
-      else
-      {
-        //If the person has inadvertantly selected this when running more than 4 cylinders, just use standard Wasted spark mode
-        ign1StartFunction = beginCoil1Charge;
-        ign1EndFunction = endCoil1Charge;
-        ign2StartFunction = beginCoil2Charge;
-        ign2EndFunction = endCoil2Charge;
-        ign3StartFunction = beginCoil3Charge;
-        ign3EndFunction = endCoil3Charge;
-        ign4StartFunction = beginCoil4Charge;
-        ign4EndFunction = endCoil4Charge;
-        ign5StartFunction = beginCoil5Charge;
-        ign5EndFunction = endCoil5Charge;
-      }
-      break;
-
-    case IGN_MODE_SEQUENTIAL:
-      ign1StartFunction = beginCoil1Charge;
-      ign1EndFunction = endCoil1Charge;
-      ign2StartFunction = beginCoil2Charge;
-      ign2EndFunction = endCoil2Charge;
-      ign3StartFunction = beginCoil3Charge;
-      ign3EndFunction = endCoil3Charge;
-      ign4StartFunction = beginCoil4Charge;
-      ign4EndFunction = endCoil4Charge;
-      ign5StartFunction = beginCoil5Charge;
-      ign5EndFunction = endCoil5Charge;
-      break;
-
-    default:
-      //Wasted spark (Shouldn't ever happen anyway)
-      ign1StartFunction = beginCoil1Charge;
-      ign1EndFunction = endCoil1Charge;
-      ign2StartFunction = beginCoil2Charge;
-      ign2EndFunction = endCoil2Charge;
-      ign3StartFunction = beginCoil3Charge;
-      ign3EndFunction = endCoil3Charge;
-      ign4StartFunction = beginCoil4Charge;
-      ign4EndFunction = endCoil4Charge;
-      ign5StartFunction = beginCoil5Charge;
-      ign5EndFunction = endCoil5Charge;
-      break;
-  }
-
-  //Begin priming the fuel pump. This is turned off in the low resolution, 1s interrupt in timers.ino
-  digitalWrite(pinFuelPump, HIGH);
-  fuelPumpOn = true;
-  //Perform the priming pulses. Set these to run at an arbitrary time in the future (100us). The prime pulse value is in ms*10, so need to multiple by 100 to get to uS
-  setFuelSchedule1(100, (unsigned long)(configPage1.primePulse * 100));
-  setFuelSchedule2(100, (unsigned long)(configPage1.primePulse * 100));
-  setFuelSchedule3(100, (unsigned long)(configPage1.primePulse * 100));
-  setFuelSchedule4(100, (unsigned long)(configPage1.primePulse * 100));
-
-  initialisationComplete = true;
-}
-
-void loop()
-{
-      mainLoopCount++;
-      LOOP_TIMER = TIMER_mask;
-      //Check for any requets from serial. Serial operations are checked under 2 scenarios:
-      // 1) Every 64 loops (64 Is more than fast enough for TunerStudio). This function is equivalent to ((loopCount % 64) == 1) but is considerably faster due to not using the mod or division operations
-      // 2) If the amount of data in the serial buffer is greater than a set threhold (See globals.h). This is to avoid serial buffer overflow when large amounts of data is being sent
-      if ( BIT_CHECK(LOOP_TIMER, BIT_TIMER_15HZ) or (Serial.available() > SERIAL_BUFFER_THRESHOLD) )
-      {
-        if (Serial.available() > 0)
-        {
-          command();
-        }
-      }
-
-#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) //ATmega2561 does not have Serial3
-      //if serial3 interface is enabled then check for serial3 requests.
-      if (configPage10.enable_canbus == 1)
-          {
-            if ( BIT_CHECK(LOOP_TIMER, BIT_TIMER_15HZ) or (CANSerial.available() > SERIAL_BUFFER_THRESHOLD) )
-                {
-                  if (CANSerial.available() > 0)
-                    {
-                    canCommand();
-                    }
-                }
-          }
-
-#elif defined(CORE_STM32)
-      //if can or secondary serial interface is enabled then check for requests.
-      if (configPage10.enable_canbus == 1)  //secondary serial interface enabled
-          {
-            if ( BIT_CHECK(LOOP_TIMER, BIT_TIMER_15HZ) or (CANSerial.available() > SERIAL_BUFFER_THRESHOLD) )
-                {
-                  if (CANSerial.available() > 0)
-                    {
-                    canCommand();
-                    }
-                }
-          }
-      else if (configPage10.enable_canbus == 2) // can module enabled
-          {
-            //check local can module
-          }
-#elif defined(CORE_TEENSY)
-      //if can or secondary serial interface is enabled then check for requests.
-      if (configPage10.enable_canbus == 1)  //secondary serial interface enabled
-          {
-            if ( BIT_CHECK(LOOP_TIMER, BIT_TIMER_15HZ) or (CANSerial.available() > SERIAL_BUFFER_THRESHOLD) )
-                {
-                  if (CANSerial.available() > 0)
-                    {
-                    canCommand();
-                    }
-                }
-          }
-      else if (configPage10.enable_canbus == 2) // can module enabled
-          {
-            //check local can module
-            // if ( ((mainLoopCount & 31) == 1) or (CANbus0.available())
-            //    {
-            //      CANbus0.read(rx_msg);
-            //    }
-          }
-#endif
-
-    //Displays currently disabled
-    // if (configPage1.displayType && (mainLoopCount & 255) == 1) { updateDisplay();}
-
-    previousLoopTime = currentLoopTime;
-    currentLoopTime = micros();
-    unsigned long timeToLastTooth = (currentLoopTime - toothLastToothTime);
-    if ( (timeToLastTooth < MAX_STALL_TIME) || (toothLastToothTime > currentLoopTime) ) //Check how long ago the last tooth was seen compared to now. If it was more than half a second ago then the engine is probably stopped. toothLastToothTime can be greater than currentLoopTime if a pulse occurs between getting the lastest time and doing the comparison
-    {
-      currentStatus.RPM = currentStatus.longRPM = getRPM(); //Long RPM is included here
-      FUEL_PUMP_ON();
-      fuelPumpOn = true; //Not sure if this is needed.
-    }
-    else
-    {
-      //We reach here if the time between teeth is too great. This VERY likely means the engine has stopped
-      currentStatus.RPM = 0;
-      currentStatus.PW1 = 0;
-      currentStatus.VE = 0;
-      toothLastToothTime = 0;
-      //toothLastMinusOneToothTime = 0;
-      currentStatus.hasSync = false;
-      currentStatus.runSecs = 0; //Reset the counter for number of seconds running.
-      secCounter = 0; //Reset our seconds counter.
-      currentStatus.startRevolutions = 0;
-      toothSystemCount = 0;
-      MAPcurRev = 0;
-      MAPcount = 0;
-      currentStatus.rpmDOT = 0;
-      ignitionOn = false;
-      fuelOn = false;
-      if (fpPrimed) { digitalWrite(pinFuelPump, LOW); } //Turn off the fuel pump, but only if the priming is complete
-      fuelPumpOn = false;
-      disableIdle(); //Turn off the idle PWM
-      BIT_CLEAR(currentStatus.engine, BIT_ENGINE_CRANK); //Clear cranking bit (Can otherwise get stuck 'on' even with 0 rpm)
-      BIT_CLEAR(currentStatus.engine, BIT_ENGINE_WARMUP); //Same as above except for WUE
-    }
-
-    //Uncomment the following for testing
-    /*
-    currentStatus.hasSync = true;
-    currentStatus.RPM = 500;
-    */
-
-    //***Perform sensor reads***
-    //-----------------------------------------------------------------------------------------------------
-    readMAP();
-
-    //if ((mainLoopCount & 31) == 1) //Every 32 loops
-    if ( BIT_CHECK(LOOP_TIMER, BIT_TIMER_15HZ))
-    {
-      BIT_CLEAR(TIMER_mask, BIT_TIMER_15HZ);
-      readTPS(); //TPS reading to be performed every 32 loops (any faster and it can upset the TPSdot sampling time)
-
-      //Check for launching/flat shift (clutch) can be done around here too
-      previousClutchTrigger = clutchTrigger;
-      if(configPage3.launchHiLo) { clutchTrigger = digitalRead(pinLaunch); }
-      else { clutchTrigger = !digitalRead(pinLaunch); }
-
-      if(previousClutchTrigger != clutchTrigger) { currentStatus.clutchEngagedRPM = currentStatus.RPM; }
-
-      if (configPage3.launchEnabled && clutchTrigger && (currentStatus.clutchEngagedRPM < ((unsigned int)(configPage3.flatSArm) * 100)) && (currentStatus.RPM > ((unsigned int)(configPage3.lnchHardLim) * 100)) ) { currentStatus.launchingHard = true; BIT_SET(currentStatus.spark, BIT_SPARK_HLAUNCH); } //HardCut rev limit for 2-step launch control.
-      else { currentStatus.launchingHard = false; BIT_CLEAR(currentStatus.spark, BIT_SPARK_HLAUNCH); }
-
-      if(configPage3.flatSEnable && clutchTrigger && (currentStatus.RPM > ((unsigned int)(configPage3.flatSArm) * 100)) && (currentStatus.RPM > currentStatus.clutchEngagedRPM) ) { currentStatus.flatShiftingHard = true; }
-      else { currentStatus.flatShiftingHard = false; }
-
-      //Boost cutoff is very similar to launchControl, but with a check against MAP rather than a switch
-      if(configPage3.boostCutType && currentStatus.MAP > (configPage3.boostLimit * 2) ) //The boost limit is divided by 2 to allow a limit up to 511kPa
-      {
-        switch(configPage3.boostCutType)
-        {
-          case 1:
-            BIT_SET(currentStatus.spark, BIT_SPARK_BOOSTCUT);
-            BIT_CLEAR(currentStatus.squirt, BIT_SQUIRT_BOOSTCUT);
-            break;
-          case 2:
-            BIT_SET(currentStatus.squirt, BIT_SQUIRT_BOOSTCUT);
-            BIT_CLEAR(currentStatus.spark, BIT_SPARK_BOOSTCUT);
-            break;
-          case 3:
-            BIT_SET(currentStatus.spark, BIT_SPARK_BOOSTCUT);
-            BIT_SET(currentStatus.squirt, BIT_SQUIRT_BOOSTCUT);
-            break;
-        }
-      }
-      else
-      {
-        BIT_CLEAR(currentStatus.spark, BIT_SPARK_BOOSTCUT);
-        BIT_CLEAR(currentStatus.squirt, BIT_SQUIRT_BOOSTCUT);
-      }
-
-      //And check whether the tooth log buffer is ready
-      if(toothHistoryIndex > TOOTH_LOG_SIZE) { BIT_SET(currentStatus.squirt, BIT_SQUIRT_TOOTHLOG1READY); }
-
-      //Most boost tends to run at about 30Hz, so placing it here ensures a new target time is fetched frequently enough
-      boostControl();
-    }
-    if(BIT_CHECK(LOOP_TIMER, BIT_TIMER_30HZ)) //Every 64 loops
-    {
-      //Nothing here currently
-      BIT_CLEAR(TIMER_mask, BIT_TIMER_30HZ);
-    }
-    //The IAT and CLT readings can be done less frequently. This still runs about 4 times per second
-    if (BIT_CHECK(LOOP_TIMER, BIT_TIMER_4HZ)) //Every 256 loops
-    {
-       BIT_CLEAR(TIMER_mask, BIT_TIMER_4HZ);
-       readCLT();
-       readIAT();
-       readO2();
-       readBat();
-#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) //ATmega2561 does not have Serial3
-      //if Can interface is enabled then check for serial3 requests.
-      if (configPage10.enable_canbus == 1)  // megas only support can via secondary serial
-          {
-            if (configPage10.enable_candata_in)
-              {
-                if (BIT_CHECK(configPage10.caninput_sel,currentStatus.current_caninchannel))  //if current input channel bit is enabled
-                  {
-                    sendCancommand(2,0,currentStatus.current_caninchannel,0,((configPage10.caninput_param_group[currentStatus.current_caninchannel]&2047)+256));    //send an R command for data from paramgroup[currentStatus.current_caninchannel]
-                  }
-                else
-                  {
-                    if (currentStatus.current_caninchannel < 15)
-                        {
-                          currentStatus.current_caninchannel++;   //step to next input channel if under 15
-                        }
-                    else
-                        {
-                          currentStatus.current_caninchannel = 0;   //reset input channel back to 1
-                        }
-                  }
-              }
-          }
-#elif defined(CORE_STM32) || defined(CORE_TEENSY)
-      //if serial3io is enabled then check for serial3 requests.
-            if (configPage10.enable_candata_in)
-              {
-                if (BIT_CHECK(configPage10.caninput_sel,currentStatus.current_caninchannel))  //if current input channel is enabled
-                  {
-                    if (configPage10.enable_canbus == 1)  //can via secondary serial
-                    {
-                      sendCancommand(2,0,currentStatus.current_caninchannel,0,((configPage10.caninput_param_group[currentStatus.current_caninchannel]&2047)+256));    //send an R command for data from paramgroup[currentStatus.current_caninchannel]
-                    }
-                    else if (configPage10.enable_canbus == 2) // can via internal can module
-                    {
-                      sendCancommand(3,configPage10.speeduino_tsCanId,currentStatus.current_caninchannel,0,configPage10.caninput_param_group[currentStatus.current_caninchannel]);    //send via localcanbus the command for data from paramgroup[currentStatus.current_caninchannel]
-                    }
-                  }
-                else
-                  {
-                    if (currentStatus.current_caninchannel < 15)
-                        {
-                          currentStatus.current_caninchannel++;   //step to next input channel if under 15
-                        }
-                    else
-                        {
-                          currentStatus.current_caninchannel = 0;   //reset input channel back to 0
-                        }
-                  }
-              }
-
-#endif
-       vvtControl();
-       idleControl(); //Perform any idle related actions. Even at higher frequencies, running 4x per second is sufficient.
-    }
-    if (BIT_CHECK(LOOP_TIMER, BIT_TIMER_1HZ)) //Every 1024 loops (Approx. 1 per second)
-    {
-      //Approx. once per second
-      BIT_CLEAR(TIMER_mask, BIT_TIMER_1HZ);
-      readBaro();
-    }
-
-    if(configPage4.iacAlgorithm == IAC_ALGORITHM_STEP_OL || configPage4.iacAlgorithm == IAC_ALGORITHM_STEP_CL) { idleControl(); } //Run idlecontrol every loop for stepper idle.
-
-    //Always check for sync
-    //Main loop runs within this clause
-    if (currentStatus.hasSync && (currentStatus.RPM > 0))
-    {
-        if(currentStatus.startRevolutions >= configPage2.StgCycles)  { ignitionOn = true; fuelOn = true; } //Enable the fuel and ignition, assuming staging revolutions are complete
-        //If it is, check is we're running or cranking
-        if(currentStatus.RPM > ((unsigned int)configPage2.crankRPM * 100)) //Crank RPM stored in byte as RPM / 100
-        {
-          BIT_SET(currentStatus.engine, BIT_ENGINE_RUN); //Sets the engine running bit
-          //Only need to do anything if we're transitioning from cranking to running
-          if( BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) )
-          {
-            BIT_CLEAR(currentStatus.engine, BIT_ENGINE_CRANK); //clears the engine cranking bit
-            if(configPage2.ignBypassEnabled) { digitalWrite(pinIgnBypass, HIGH); }
-          }
-        }
-        else
-        {  //Sets the engine cranking bit, clears the engine running bit
-          BIT_SET(currentStatus.engine, BIT_ENGINE_CRANK);
-          BIT_CLEAR(currentStatus.engine, BIT_ENGINE_RUN);
-          currentStatus.runSecs = 0; //We're cranking (hopefully), so reset the engine run time to prompt ASE.
-          if(configPage2.ignBypassEnabled) { digitalWrite(pinIgnBypass, LOW); }
-        }
-      //END SETTING STATUSES
-      //-----------------------------------------------------------------------------------------------------
-
-      //Begin the fuel calculation
-      //Calculate an injector pulsewidth from the VE
-      currentStatus.corrections = correctionsFuel();
-      lastAdvance = currentStatus.advance; //Store the previous advance value
-      if (configPage1.algorithm == 0) //Check which fuelling algorithm is being used
-      {
-        //Speed Density
-        currentStatus.VE = get3DTableValue(&fuelTable, currentStatus.MAP, currentStatus.RPM); //Perform lookup into fuel map for RPM vs MAP value
-        currentStatus.PW1 = PW_SD(req_fuel_uS, currentStatus.VE, currentStatus.MAP, currentStatus.corrections, inj_opentime_uS);
-        currentStatus.advance = get3DTableValue(&ignitionTable, currentStatus.MAP, currentStatus.RPM) - OFFSET_IGNITION; //As above, but for ignition advance
-      }
-      else
-      {
-        //Alpha-N
-        currentStatus.VE = get3DTableValue(&fuelTable, currentStatus.TPS, currentStatus.RPM); //Perform lookup into fuel map for RPM vs TPS value
-        currentStatus.PW1 = PW_AN(req_fuel_uS, currentStatus.VE, currentStatus.TPS, currentStatus.corrections, inj_opentime_uS); //Calculate pulsewidth using the Alpha-N algorithm (in uS)
-        currentStatus.advance = get3DTableValue(&ignitionTable, currentStatus.TPS, currentStatus.RPM) - OFFSET_IGNITION; //As above, but for ignition advance
-      }
-
-      currentStatus.advance = correctionsIgn(currentStatus.advance);
-
-      int injector1StartAngle = 0;
-      int injector2StartAngle = 0;
-      int injector3StartAngle = 0; //Currently used for 3 cylinder only
-      int injector4StartAngle = 0; //Not used until sequential gets written
-      int injector5StartAngle = 0; //For 5 cylinder testing
-      int ignition1StartAngle = 0;
-      int ignition2StartAngle = 0;
-      int ignition3StartAngle = 0; //Currently used for 3 cylinder only
-      int ignition4StartAngle = 0; //Not used until sequential or 4+ cylinders support gets written
-      int ignition5StartAngle = 0; //Not used until sequential or 4+ cylinders support gets written
-      //These are used for comparisons on channels above 1 where the starting angle (for injectors or ignition) can be less than a single loop time
-      //(Don't ask why this is needed, it will break your head)
-      int tempCrankAngle;
-      int tempStartAngle;
-
-      //********************************************************
-      //How fast are we going? Need to know how long (uS) it will take to get from one tooth to the next. We then use that to estimate how far we are between the last tooth and the next one
-      //We use a 1st Deriv accleration prediction, but only when there is an even spacing between primary sensor teeth
-      //Any decoder that has uneven spacing has its triggerToothAngle set to 0
-      if(secondDerivEnabled && toothHistoryIndex >= 3 && currentStatus.RPM < 2000) //toothHistoryIndex must be greater than or equal to 3 as we need the last 3 entries. Currently this mode only runs below 3000 rpm
-      //if(true)
-      {
-        //Only recalculate deltaV if the tooth has changed since last time (DeltaV stays the same until the next tooth)
-        //if (deltaToothCount != toothCurrentCount)
-        {
-          deltaToothCount = toothCurrentCount;
-          int angle1, angle2; //These represent the crank angles that are travelled for the last 2 pulses
-          if(configPage2.TrigPattern == 4)
-          {
-            //Special case for 70/110 pattern on 4g63
-            angle2 = triggerToothAngle; //Angle 2 is the most recent
-            if (angle2 == 70) { angle1 = 110; }
-            else { angle1 = 70; }
-          }
-          else if(configPage2.TrigPattern == 0)
-          {
-            //Special case for missing tooth decoder where the missing tooth was one of the last 2 seen
-            if(toothCurrentCount == 1) { angle2 = 2*triggerToothAngle; angle1 = triggerToothAngle; }
-            else if(toothCurrentCount == 2) { angle1 = 2*triggerToothAngle; angle2 = triggerToothAngle; }
-            else { angle1 = angle2 = triggerToothAngle; }
-          }
-          else { angle1 = angle2 = triggerToothAngle; }
-
-          long toothDeltaV = (1000000L * angle2 / toothHistory[toothHistoryIndex]) - (1000000L * angle1 / toothHistory[toothHistoryIndex-1]);
-          long toothDeltaT = toothHistory[toothHistoryIndex];
-          //long timeToLastTooth = micros() - toothLastToothTime;
-
-          rpmDelta = (toothDeltaV << 10) / (6 * toothDeltaT);
-        }
-
-
-        timePerDegree = ldiv( 166666L, (currentStatus.RPM + rpmDelta)).quot; //There is a small amount of rounding in this calculation, however it is less than 0.001 of a uS (Faster as ldiv than / )
-      }
-      else
-      {
-        long rpm_adjust = ((long)(micros() - toothOneTime) * (long)currentStatus.rpmDOT) / 1000000; //Take into account any likely accleration that has occurred since the last full revolution completed
-
-        //timePerDegree = DIV_ROUND_CLOSEST(166666L, (currentStatus.RPM + rpm_adjust));
-        timePerDegree = ldiv( 166666L, currentStatus.RPM + rpm_adjust).quot; //There is a small amount of rounding in this calculation, however it is less than 0.001 of a uS (Faster as ldiv than / )
-      }
-
-      //Check that the duty cycle of the chosen pulsewidth isn't too high. This is disabled at cranking
-      if( !BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) )
-      {
-        unsigned long pwLimit = percentage(configPage1.dutyLim, revolutionTime); //The pulsewidth limit is determined to be the duty cycle limit (Eg 85%) by the total time it takes to perform 1 revolution
-        if (CRANK_ANGLE_MAX_INJ == 720) { pwLimit = pwLimit * 2; } //For sequential, the maximum pulse time is double (2 revolutions). Wouldn't work for 2 stroke...
-        if (currentStatus.PW1 > pwLimit) { currentStatus.PW1 = pwLimit; }
-      }
-
-
-      //***********************************************************************************************
-      //BEGIN INJECTION TIMING
-      //Determine next firing angles
-      currentStatus.PW2 = currentStatus.PW3 = currentStatus.PW4 = currentStatus.PW1; // Initial state is for all pulsewidths to be the same (This gets changed below)
-      if(!configPage1.indInjAng) {configPage1.inj4Ang = configPage1.inj3Ang = configPage1.inj2Ang = configPage1.inj1Ang;} //Forcing all injector close angles to be the same.
-      int PWdivTimerPerDegree = div(currentStatus.PW1, timePerDegree).quot; //How many crank degrees the calculated PW will take at the current speed
-      injector1StartAngle = configPage1.inj1Ang - ( PWdivTimerPerDegree ); //This is a little primitive, but is based on the idea that all fuel needs to be delivered before the inlet valve opens. See http://www.extraefi.co.uk/sequential_fuel.html for more detail
-      if(injector1StartAngle < 0) {injector1StartAngle += CRANK_ANGLE_MAX_INJ;}
-
-      //Repeat the above for each cylinder
-      switch (configPage1.nCylinders)
-      {
-        //2 cylinders
-        case 2:
-          injector2StartAngle = (configPage1.inj2Ang + channel2InjDegrees - ( PWdivTimerPerDegree ));
-          if(injector2StartAngle > CRANK_ANGLE_MAX_INJ) {injector2StartAngle -= CRANK_ANGLE_MAX_INJ;}
-          break;
-        //3 cylinders
-        case 3:
-          injector2StartAngle = (configPage1.inj2Ang + channel2InjDegrees - ( PWdivTimerPerDegree ));
-          if(injector2StartAngle > CRANK_ANGLE_MAX_INJ) {injector2StartAngle -= CRANK_ANGLE_MAX_INJ;}
-          injector3StartAngle = (configPage1.inj3Ang + channel3InjDegrees - ( PWdivTimerPerDegree ));
-          if(injector3StartAngle > CRANK_ANGLE_MAX_INJ) {injector3StartAngle -= CRANK_ANGLE_MAX_INJ;}
-          break;
-        //4 cylinders
-        case 4:
-          injector2StartAngle = (configPage1.inj2Ang + channel2InjDegrees - ( PWdivTimerPerDegree ));
-          if(injector2StartAngle > CRANK_ANGLE_MAX_INJ) {injector2StartAngle -= CRANK_ANGLE_MAX_INJ;}
-
-          if(configPage1.injLayout == INJ_SEQUENTIAL)
-          {
-            injector3StartAngle = (configPage1.inj3Ang + channel3InjDegrees - ( PWdivTimerPerDegree ));
-            if(injector3StartAngle > CRANK_ANGLE_MAX_INJ) {injector3StartAngle -= CRANK_ANGLE_MAX_INJ;}
-            injector4StartAngle = (configPage1.inj4Ang + channel4InjDegrees - ( PWdivTimerPerDegree ));
-            if(injector4StartAngle > CRANK_ANGLE_MAX_INJ) {injector4StartAngle -= CRANK_ANGLE_MAX_INJ;}
-
-            if(configPage3.fuelTrimEnabled)
-            {
-              unsigned long pw1percent = 100 + (byte)get3DTableValue(&trim1Table, currentStatus.MAP, currentStatus.RPM) - OFFSET_FUELTRIM;
-              unsigned long pw2percent = 100 + (byte)get3DTableValue(&trim2Table, currentStatus.MAP, currentStatus.RPM) - OFFSET_FUELTRIM;
-              unsigned long pw3percent = 100 + (byte)get3DTableValue(&trim3Table, currentStatus.MAP, currentStatus.RPM) - OFFSET_FUELTRIM;
-              unsigned long pw4percent = 100 + (byte)get3DTableValue(&trim4Table, currentStatus.MAP, currentStatus.RPM) - OFFSET_FUELTRIM;
-
-              if (pw1percent != 100) { currentStatus.PW1 = (pw1percent * currentStatus.PW1) / 100; }
-              if (pw2percent != 100) { currentStatus.PW2 = (pw2percent * currentStatus.PW2) / 100; }
-              if (pw3percent != 100) { currentStatus.PW3 = (pw3percent * currentStatus.PW3) / 100; }
-              if (pw4percent != 100) { currentStatus.PW4 = (pw4percent * currentStatus.PW4) / 100; }
-            }
-          }
-          break;
-        //5 cylinders
-        case 5:
-          injector2StartAngle = (configPage1.inj2Ang + channel2InjDegrees - ( PWdivTimerPerDegree ));
-          if(injector2StartAngle > CRANK_ANGLE_MAX_INJ) {injector2StartAngle -= CRANK_ANGLE_MAX_INJ;}
-          injector3StartAngle = (configPage1.inj3Ang + channel3InjDegrees - ( PWdivTimerPerDegree ));
-          if(injector3StartAngle > CRANK_ANGLE_MAX_INJ) {injector3StartAngle -= CRANK_ANGLE_MAX_INJ;}
-          injector4StartAngle = (configPage1.inj4Ang + channel4InjDegrees - ( PWdivTimerPerDegree ));
-          if(injector4StartAngle > CRANK_ANGLE_MAX_INJ) {injector4StartAngle -= CRANK_ANGLE_MAX_INJ;}
-          injector5StartAngle = (configPage1.inj1Ang + channel5InjDegrees - ( PWdivTimerPerDegree ));
-          if(injector5StartAngle > CRANK_ANGLE_MAX_INJ) {injector5StartAngle -= CRANK_ANGLE_MAX_INJ;}
-          break;
-        //6 cylinders
-        case 6:
-          injector2StartAngle = (configPage1.inj2Ang + channel2InjDegrees - ( PWdivTimerPerDegree ));
-          if(injector2StartAngle > CRANK_ANGLE_MAX_INJ) {injector2StartAngle -= CRANK_ANGLE_MAX_INJ;}
-          injector3StartAngle = (configPage1.inj3Ang + channel3InjDegrees - ( PWdivTimerPerDegree ));
-          if(injector3StartAngle > CRANK_ANGLE_MAX_INJ) {injector3StartAngle -= CRANK_ANGLE_MAX_INJ;}
-          break;
-        //8 cylinders
-        case 8:
-          injector2StartAngle = (configPage1.inj2Ang + channel2InjDegrees - ( PWdivTimerPerDegree ));
-          if(injector2StartAngle > CRANK_ANGLE_MAX_INJ) {injector2StartAngle -= CRANK_ANGLE_MAX_INJ;}
-          injector3StartAngle = (configPage1.inj3Ang + channel3InjDegrees - ( PWdivTimerPerDegree ));
-          if(injector3StartAngle > CRANK_ANGLE_MAX_INJ) {injector3StartAngle -= CRANK_ANGLE_MAX_INJ;}
-          injector4StartAngle = (configPage1.inj4Ang + channel4InjDegrees - ( PWdivTimerPerDegree ));
-          if(injector4StartAngle > CRANK_ANGLE_MAX_INJ) {injector4StartAngle -= CRANK_ANGLE_MAX_INJ;}
-          break;
-        //Will hit the default case on 1 cylinder or >8 cylinders. Do nothing in these cases
-        default:
-          break;
-      }
-
-      //***********************************************************************************************
-      //| BEGIN IGNITION CALCULATIONS
-      BIT_CLEAR(currentStatus.spark, BIT_SPARK_HRDLIM);
-      if (currentStatus.RPM > ((unsigned int)(configPage2.HardRevLim) * 100) ) { BIT_SET(currentStatus.spark, BIT_SPARK_HRDLIM); } //Hardcut RPM limit
-
-
-      //Set dwell
-       //Dwell is stored as ms * 10. ie Dwell of 4.3ms would be 43 in configPage2. This number therefore needs to be multiplied by 100 to get dwell in uS
-      if ( BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) ) { currentStatus.dwell =  (configPage2.dwellCrank * 100); }
-      else { currentStatus.dwell =  (configPage2.dwellRun * 100); }
-      currentStatus.dwell = correctionsDwell(currentStatus.dwell);
-
-      int dwellAngle = (div(currentStatus.dwell, timePerDegree).quot ); //Convert the dwell time to dwell angle based on the current engine speed
-
-      //Calculate start angle for each channel
-      //1 cylinder (Everyone gets this)
-      ignition1EndAngle = CRANK_ANGLE_MAX_IGN - currentStatus.advance;
-      ignition1StartAngle = ignition1EndAngle - dwellAngle; // 360 - desired advance angle - number of degrees the dwell will take
-      if(ignition1StartAngle < 0) {ignition1StartAngle += CRANK_ANGLE_MAX_IGN;}
-
-      //This test for more cylinders and do the same thing
-      switch (configPage1.nCylinders)
-      {
-        //2 cylinders
-        case 2:
-          ignition2EndAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
-          ignition2StartAngle = ignition2EndAngle - dwellAngle;
-          if(ignition2StartAngle > CRANK_ANGLE_MAX_IGN) {ignition2StartAngle -= CRANK_ANGLE_MAX_IGN;}
-          break;
-        //3 cylinders
-        case 3:
-          ignition2EndAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
-          ignition2StartAngle = ignition2EndAngle - dwellAngle;
-          if(ignition2StartAngle > CRANK_ANGLE_MAX_IGN) {ignition2StartAngle -= CRANK_ANGLE_MAX_IGN;}
-          ignition3EndAngle = channel3IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
-          ignition3StartAngle = channel3IgnDegrees + 360 - currentStatus.advance - dwellAngle;
-          if(ignition3StartAngle > CRANK_ANGLE_MAX_IGN) {ignition3StartAngle -= CRANK_ANGLE_MAX_IGN;}
-          break;
-        //4 cylinders
-        case 4:
-          ignition2EndAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
-          ignition2StartAngle = ignition2EndAngle - dwellAngle;
-          if(ignition2StartAngle > CRANK_ANGLE_MAX_IGN) {ignition2StartAngle -= CRANK_ANGLE_MAX_IGN;}
-          if(ignition2StartAngle < 0) {ignition2StartAngle += CRANK_ANGLE_MAX_IGN;}
-
-          if(configPage2.sparkMode == IGN_MODE_SEQUENTIAL)
-          {
-            ignition3EndAngle = channel3IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
-            ignition3StartAngle = ignition3EndAngle - dwellAngle;
-            if(ignition3StartAngle > CRANK_ANGLE_MAX_IGN) {ignition3StartAngle -= CRANK_ANGLE_MAX_IGN;}
-
-            ignition4EndAngle = channel4IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
-            ignition4StartAngle = ignition4EndAngle - dwellAngle;
-            if(ignition4StartAngle > CRANK_ANGLE_MAX_IGN) {ignition4StartAngle -= CRANK_ANGLE_MAX_IGN;}
-          }
-          break;
-        //5 cylinders
-        case 5:
-          ignition2EndAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
-          ignition2StartAngle = ignition2EndAngle - dwellAngle;
-          if(ignition2StartAngle > CRANK_ANGLE_MAX_IGN) {ignition2StartAngle -= CRANK_ANGLE_MAX_IGN;}
-          if(ignition2StartAngle < 0) {ignition2StartAngle += CRANK_ANGLE_MAX_IGN;}
-
-          ignition3EndAngle = channel3IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
-          ignition3StartAngle = ignition3EndAngle - dwellAngle;
-          if(ignition3StartAngle > CRANK_ANGLE_MAX_IGN) {ignition3StartAngle -= CRANK_ANGLE_MAX_IGN;}
-
-          ignition4EndAngle = channel4IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
-          ignition4StartAngle = ignition4EndAngle - dwellAngle;
-          if(ignition4StartAngle > CRANK_ANGLE_MAX_IGN) {ignition4StartAngle -= CRANK_ANGLE_MAX_IGN;}
-
-          ignition5StartAngle = channel5IgnDegrees + CRANK_ANGLE_MAX - currentStatus.advance - dwellAngle;
-          if(ignition5StartAngle > CRANK_ANGLE_MAX_IGN) {ignition5StartAngle -= CRANK_ANGLE_MAX_IGN;}
-
-          break;
-        //6 cylinders
-        case 6:
-          ignition2EndAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
-          ignition2StartAngle = ignition2EndAngle - dwellAngle;
-          if(ignition2StartAngle > CRANK_ANGLE_MAX_IGN) {ignition2StartAngle -= CRANK_ANGLE_MAX_IGN;}
-
-          ignition3EndAngle = channel3IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
-          ignition3StartAngle = ignition3EndAngle - dwellAngle;
-          if(ignition3StartAngle > CRANK_ANGLE_MAX_IGN) {ignition3StartAngle -= CRANK_ANGLE_MAX_IGN;}
-          break;
-        //8 cylinders
-        case 8:
-          ignition2EndAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
-          ignition2StartAngle = ignition2EndAngle - dwellAngle;
-          if(ignition2StartAngle > CRANK_ANGLE_MAX_IGN) {ignition2StartAngle -= CRANK_ANGLE_MAX_IGN;}
-
-          ignition3EndAngle = channel3IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
-          ignition3StartAngle = ignition3EndAngle - dwellAngle;
-          if(ignition3StartAngle > CRANK_ANGLE_MAX_IGN) {ignition3StartAngle -= CRANK_ANGLE_MAX_IGN;}
-
-          ignition4EndAngle = channel4IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
-          ignition4StartAngle = ignition4EndAngle - dwellAngle;
-          if(ignition4StartAngle > CRANK_ANGLE_MAX_IGN) {ignition4StartAngle -= CRANK_ANGLE_MAX_IGN;}
-          break;
-
-        //Will hit the default case on 1 cylinder or >8 cylinders. Do nothing in these cases
-        default:
-          break;
-      }
-      //If ignition timing is being tracked per tooth, perform the calcs to get the end teeth
-      //This only needs to be run if the advance figure has changed, otherwise the end teeth will still be the same
-      if( (configPage1.perToothIgn == true) && (lastAdvance != currentStatus.advance) ) { triggerSetEndTeeth(); }
-
-      //***********************************************************************************************
-      //| BEGIN FUEL SCHEDULES
-      //Finally calculate the time (uS) until we reach the firing angles and set the schedules
-      //We only need to set the shcedule if we're BEFORE the open angle
-      //This may potentially be called a number of times as we get closer and closer to the opening time
-
-      //Determine the current crank angle
-      int crankAngle = getCrankAngle(timePerDegree);
-      if (crankAngle > CRANK_ANGLE_MAX_INJ ) { crankAngle -= 360; }
-
-      if (fuelOn && currentStatus.PW1 > 0 && !BIT_CHECK(currentStatus.squirt, BIT_SQUIRT_BOOSTCUT))
-      {
-        if ( (injector1StartAngle <= crankAngle) && (fuelSchedule1.Status == RUNNING) ) { injector1StartAngle += CRANK_ANGLE_MAX_INJ; }
-        if (injector1StartAngle > crankAngle)
-        {
-          setFuelSchedule1(
-                    ((unsigned long)(injector1StartAngle - crankAngle) * (unsigned long)timePerDegree),
-                    (unsigned long)currentStatus.PW1
-                    );
-        }
-
-        /*-----------------------------------------------------------------------------------------
-        | A Note on tempCrankAngle and tempStartAngle:
-        |   The use of tempCrankAngle/tempStartAngle is described below. It is then used in the same way for channels 2, 3 and 4 on both injectors and ignition
-        |   Essentially, these 2 variables are used to realign the current crank angle and the desired start angle around 0 degrees for the given cylinder/output
-        |   Eg: If cylinder 2 TDC is 180 degrees after cylinder 1 (Eg a standard 4 cylidner engine), then tempCrankAngle is 180* less than the current crank angle and
-        |       tempStartAngle is the desired open time less 180*. Thus the cylinder is being treated relative to its own TDC, regardless of its offset
-        |
-        |   This is done to avoid problems with very short of very long times until tempStartAngle.
-        |   This will very likely need to be rewritten when sequential is enabled
-        |------------------------------------------------------------------------------------------
-        */
-        if(channel2InjEnabled)
-        {
-          tempCrankAngle = crankAngle - channel2InjDegrees;
-          if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX_INJ; }
-          tempStartAngle = injector2StartAngle - channel2InjDegrees;
-          if ( tempStartAngle < 0) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
-          if ( (tempStartAngle <= tempCrankAngle) && (fuelSchedule2.Status == RUNNING) ) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
-          if ( tempStartAngle > tempCrankAngle )
-          {
-            setFuelSchedule2(
-                      ((unsigned long)(tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree),
-                      (unsigned long)currentStatus.PW2
-                      );
-          }
-        }
-
-        if(channel3InjEnabled)
-        {
-          tempCrankAngle = crankAngle - channel3InjDegrees;
-          if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX_INJ; }
-          tempStartAngle = injector3StartAngle - channel3InjDegrees;
-          if ( tempStartAngle < 0) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
-          if ( (tempStartAngle <= tempCrankAngle) && (fuelSchedule3.Status == RUNNING) ) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
-          if ( tempStartAngle > tempCrankAngle )
-          {
-            setFuelSchedule3(
-                      ((unsigned long)(tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree),
-                      (unsigned long)currentStatus.PW3
-                      );
-          }
-        }
-
-        if(channel4InjEnabled)
-        {
-          tempCrankAngle = crankAngle - channel4InjDegrees;
-          if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX_INJ; }
-          tempStartAngle = injector4StartAngle - channel4InjDegrees;
-          if ( tempStartAngle < 0) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
-          if ( (tempStartAngle <= tempCrankAngle) && (fuelSchedule4.Status == RUNNING) ) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
-          if ( tempStartAngle > tempCrankAngle )
-          {
-            setFuelSchedule4(
-                      ((unsigned long)(tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree),
-                      (unsigned long)currentStatus.PW4
-                      );
-          }
-        }
-
-        if(channel5InjEnabled)
-        {
-          tempCrankAngle = crankAngle - channel5InjDegrees;
-          if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX_INJ; }
-          tempStartAngle = injector5StartAngle - channel5InjDegrees;
-          if ( tempStartAngle < 0) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
-          if (tempStartAngle <= tempCrankAngle && fuelSchedule5.schedulesSet == 0) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
-          if ( tempStartAngle > tempCrankAngle )
-          {
-            //Note the hacky use of fuel schedule 3 below
-            /*
-            setFuelSchedule3(openInjector3and5,
-                      ((unsigned long)(tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree),
-                      (unsigned long)currentStatus.PW1,
-                      closeInjector3and5
-                    );*/
-            setFuelSchedule3(
-                      ((unsigned long)(tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree),
-                      (unsigned long)currentStatus.PW1
-                      );
-          }
-        }
-      }
-      //***********************************************************************************************
-      //| BEGIN IGNITION SCHEDULES
-      //Likewise for the ignition
-
-      //fixedCrankingOverride is used to extend the dwell during cranking so that the decoder can trigger the spark upon seeing a certain tooth. Currently only available on the basic distributor and 4g63 decoders.
-      if ( configPage2.ignCranklock && BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK)) { fixedCrankingOverride = currentStatus.dwell * 2; }
-      else { fixedCrankingOverride = 0; }
-
-      //Perform an initial check to see if the ignition is turned on (Ignition only turns on after a preset number of cranking revolutions and:
-      //Check for hard cut rev limit (If we're above the hardcut limit, we simply don't set a spark schedule)
-      if(ignitionOn && !currentStatus.launchingHard && !BIT_CHECK(currentStatus.spark, BIT_SPARK_BOOSTCUT) && !BIT_CHECK(currentStatus.spark, BIT_SPARK_HRDLIM) && !currentStatus.flatShiftingHard)
-      {
-
-        //Refresh the current crank angle info
-        //ignition1StartAngle = 335;
-        crankAngle = getCrankAngle(timePerDegree); //Refresh with the latest crank angle
-        if (crankAngle > CRANK_ANGLE_MAX_IGN ) { crankAngle -= 360; }
-
-        //if (ignition1StartAngle <= crankAngle && ignition1.schedulesSet == 0) { ignition1StartAngle += CRANK_ANGLE_MAX_IGN; }
-        if (ignition1StartAngle > crankAngle)
-        {
-            /*
-            long some_time = ((unsigned long)(ignition1StartAngle - crankAngle) * (unsigned long)timePerDegree);
-            long newRPM = (long)(some_time * currentStatus.rpmDOT) / 1000000L;
-            newRPM = currentStatus.RPM + (newRPM/2);
-            unsigned long timePerDegree_1 = ldiv( 166666L, newRPM).quot;
-            unsigned long timeout = (unsigned long)(ignition1StartAngle - crankAngle) * 282UL;
-            */
-            setIgnitionSchedule1(ign1StartFunction,
-                      ((unsigned long)(ignition1StartAngle - crankAngle) * (unsigned long)timePerDegree), //(timeout/10),
-                      currentStatus.dwell + fixedCrankingOverride, //((unsigned long)((unsigned long)currentStatus.dwell* currentStatus.RPM) / newRPM) + fixedCrankingOverride,
-                      ign1EndFunction
-                      );
-        }
-
-        tempCrankAngle = crankAngle - channel2IgnDegrees;
-        if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX_IGN; }
-        tempStartAngle = ignition2StartAngle - channel2IgnDegrees;
-        if ( tempStartAngle < 0) { tempStartAngle += CRANK_ANGLE_MAX_IGN; }
-        //if ( (tempStartAngle > tempCrankAngle)  && ign2LastRev != startRevolutions)
-        //if ( ign2LastRev != startRevolutions )
-        {
-            unsigned long ignition2StartTime = 0;
-            if(tempStartAngle > tempCrankAngle) { ignition2StartTime = ((unsigned long)(tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree); }
-            //else if (tempStartAngle < tempCrankAngle) { ignition2StartTime = ((long)(360 - tempCrankAngle + tempStartAngle) * (long)timePerDegree); }
-            else { ignition2StartTime = 0; }
-
-            if(ignition2StartTime > 0) {
-            setIgnitionSchedule2(ign2StartFunction,
-                      ignition2StartTime,
-                      currentStatus.dwell + fixedCrankingOverride,
-                      ign2EndFunction
-                      );
-            }
-        }
-
-        tempCrankAngle = crankAngle - channel3IgnDegrees;
-        if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX_IGN; }
-        tempStartAngle = ignition3StartAngle - channel3IgnDegrees;
-        if ( tempStartAngle < 0) { tempStartAngle += CRANK_ANGLE_MAX_IGN; }
-        //if (tempStartAngle > tempCrankAngle)
-        {
-            long ignition3StartTime = 0;
-            if(tempStartAngle > tempCrankAngle) { ignition3StartTime = ((unsigned long)(tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree); }
-            //else if (tempStartAngle < tempCrankAngle) { ignition4StartTime = ((long)(360 - tempCrankAngle + tempStartAngle) * (long)timePerDegree); }
-            else { ignition3StartTime = 0; }
-
-            if(ignition3StartTime > 0) {
-            setIgnitionSchedule3(ign3StartFunction,
-                      ignition3StartTime,
-                      currentStatus.dwell + fixedCrankingOverride,
-                      ign3EndFunction
-                      );
-            }
-        }
-
-        tempCrankAngle = crankAngle - channel4IgnDegrees;
-        if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX_IGN; }
-        tempStartAngle = ignition4StartAngle - channel4IgnDegrees;
-        if ( tempStartAngle < 0) { tempStartAngle += CRANK_ANGLE_MAX_IGN; }
-        //if (tempStartAngle > tempCrankAngle)
-        {
-
-            long ignition4StartTime = 0;
-            if(tempStartAngle > tempCrankAngle) { ignition4StartTime = ((unsigned long)(tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree); }
-            //else if (tempStartAngle < tempCrankAngle) { ignition4StartTime = ((long)(360 - tempCrankAngle + tempStartAngle) * (long)timePerDegree); }
-            else { ignition4StartTime = 0; }
-
-            if(ignition4StartTime > 0) {
-            setIgnitionSchedule4(ign4StartFunction,
-                      ignition4StartTime,
-                      currentStatus.dwell + fixedCrankingOverride,
-                      ign4EndFunction
-                      );
-            }
-        }
-
-        tempCrankAngle = crankAngle - channel5IgnDegrees;
-        if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX_IGN; }
-        tempStartAngle = ignition5StartAngle - channel5IgnDegrees;
-        if ( tempStartAngle < 0) { tempStartAngle += CRANK_ANGLE_MAX_IGN; }
-        //if (tempStartAngle > tempCrankAngle)
-        {
-
-            long ignition5StartTime = 0;
-            if(tempStartAngle > tempCrankAngle) { ignition5StartTime = ((unsigned long)(tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree); }
-            //else if (tempStartAngle < tempCrankAngle) { ignition4StartTime = ((long)(360 - tempCrankAngle + tempStartAngle) * (long)timePerDegree); }
-            else { ignition5StartTime = 0; }
-
-            if(ignition5StartTime > 0) {
-            setIgnitionSchedule5(ign5StartFunction,
-                      ignition5StartTime,
-                      currentStatus.dwell + fixedCrankingOverride,
-                      ign5EndFunction
-                      );
-            }
-        }
-      } //Ignition schedules on
-    } //Has sync and RPM
+/*
+Speeduino - Simple engine management for the Arduino Mega 2560 platform
+Copyright (C) Josh Stewart
+
+This program is free software; you can redistribute it and/or
+modify it under the terms of the GNU General Public License
+as published by the Free Software Foundation; either version 2
+of the License, or (at your option) any later version.
+
+This program is distributed in the hope that it will be useful,la
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program; if not, write to the Free Software
+Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+*/
+
+//**************************************************************************************************
+// Config section
+#define engineSquirtsPerCycle 2 //Would be 1 for a 2 stroke
+//**************************************************************************************************
+
+//https://developer.mbed.org/handbook/C-Data-Types
+#include <stdint.h>
+//************************************************
+#include "globals.h"
+#include "utils.h"
+#include "table.h"
+#include "scheduler.h"
+#include "comms.h"
+#include "cancomms.h"
+#include "maths.h"
+#include "corrections.h"
+#include "timers.h"
+//#include "display.h"
+#include "decoders.h"
+#include "idle.h"
+#include "auxiliaries.h"
+#include "sensors.h"
+#include "src/PID_v1/PID_v1.h"
+//#include "src/DigitalWriteFast/digitalWriteFast.h"
+#include "errors.h"
+#include "storage.h"
+#include "scheduledIO.h"
+#include <EEPROM.h>
+#if defined (CORE_TEENSY)
+#include <FlexCAN.h>
+#endif
+
+struct config1 configPage1;
+struct config2 configPage2;
+struct config3 configPage3;
+struct config4 configPage4;
+struct config10 configPage10;
+struct config11 configPage11;
+
+int req_fuel_uS, inj_opentime_uS;
+
+volatile byte ign1LastRev;
+volatile byte ign2LastRev;
+volatile byte ign3LastRev;
+volatile byte ign4LastRev;
+volatile byte ign5LastRev;
+bool ignitionOn = false; //The current state of the ignition system
+bool fuelOn = false; //The current state of the ignition system
+bool fuelPumpOn = false; //The current status of the fuel pump
+
+void (*trigger)(); //Pointer for the trigger function (Gets pointed to the relevant decoder)
+void (*triggerSecondary)(); //Pointer for the secondary trigger function (Gets pointed to the relevant decoder)
+uint16_t (*getRPM)(); //Pointer to the getRPM function (Gets pointed to the relevant decoder)
+int (*getCrankAngle)(int); //Pointer to the getCrank Angle function (Gets pointed to the relevant decoder)
+void (*triggerSetEndTeeth)(); //Pointer to the triggerSetEndTeeth function of each decoder
+
+byte cltCalibrationTable[CALIBRATION_TABLE_SIZE];
+byte iatCalibrationTable[CALIBRATION_TABLE_SIZE];
+byte o2CalibrationTable[CALIBRATION_TABLE_SIZE];
+
+//These variables are used for tracking the number of running sensors values that appear to be errors. Once a threshold is reached, the sensor reading will go to default value and assume the sensor is faulty
+byte mapErrorCount = 0;
+byte iatErrorCount = 0;
+byte cltErrorCount = 0;
+
+unsigned long counter;
+unsigned long currentLoopTime; //The time the current loop started (uS)
+unsigned long previousLoopTime; //The time the previous loop started (uS)
+
+int CRANK_ANGLE_MAX = 720;
+int CRANK_ANGLE_MAX_IGN = 360;
+int CRANK_ANGLE_MAX_INJ = 360; // The number of crank degrees that the system track over. 360 for wasted / timed batch and 720 for sequential
+
+static byte coilHIGH = HIGH;
+static byte coilLOW = LOW;
+static byte fanHIGH = HIGH;             // Used to invert the cooling fan output
+static byte fanLOW = LOW;               // Used to invert the cooling fan output
+
+volatile int mainLoopCount;
+byte deltaToothCount = 0; //The last tooth that was used with the deltaV calc
+int rpmDelta;
+byte ignitionCount;
+uint16_t fixedCrankingOverride = 0;
+int16_t lastAdvance; //Stores the previous advance figure to track changes.
+bool clutchTrigger;
+bool previousClutchTrigger;
+
+unsigned long secCounter; //The next time to incremen 'runSecs' counter.
+int channel1IgnDegrees; //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 channel2IgnDegrees; //The number of crank degrees until cylinder 2 (and 5/6/7/8) is at TDC
+int channel3IgnDegrees; //The number of crank degrees until cylinder 3 (and 5/6/7/8) is at TDC
+int channel4IgnDegrees; //The number of crank degrees until cylinder 4 (and 5/6/7/8) is at TDC
+int channel5IgnDegrees; //The number of crank degrees until cylinder 5 is at TDC
+int channel1InjDegrees; //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 channel2InjDegrees; //The number of crank degrees until cylinder 2 (and 5/6/7/8) is at TDC
+int channel3InjDegrees; //The number of crank degrees until cylinder 3 (and 5/6/7/8) is at TDC
+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
+
+//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)();
+void (*ign2StartFunction)();
+void (*ign2EndFunction)();
+void (*ign3StartFunction)();
+void (*ign3EndFunction)();
+void (*ign4StartFunction)();
+void (*ign4EndFunction)();
+void (*ign5StartFunction)();
+void (*ign5EndFunction)();
+
+int timePerDegree;
+byte degreesPerLoop; //The number of crank degrees that pass for each mainloop of the program
+volatile bool fpPrimed = false; //Tracks whether or not the fuel pump priming has been completed yet
+bool initialisationComplete = false; //Tracks whether the setup() functino has run completely
+
+void setup()
+{
+
+  //Setup the dummy fuel and ignition tables
+  //dummyFuelTable(&fuelTable);
+  //dummyIgnitionTable(&ignitionTable);
+  table3D_setSize(&fuelTable, 16);
+  table3D_setSize(&ignitionTable, 16);
+  table3D_setSize(&afrTable, 16);
+  table3D_setSize(&boostTable, 8);
+  table3D_setSize(&vvtTable, 8);
+  table3D_setSize(&trim1Table, 6);
+  table3D_setSize(&trim2Table, 6);
+  table3D_setSize(&trim3Table, 6);
+  table3D_setSize(&trim4Table, 6);
+  Serial.begin(115200);
+
+  #if defined(CORE_STM32)
+    EEPROM.init();
+  #endif
+  loadConfig();
+  doUpdates(); //Check if any data items need updating (Occurs ith firmware updates)
+
+#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) //ATmega2561 does not have Serial3
+  if (configPage10.enable_canbus == 1) { CANSerial.begin(115200); }
+#elif defined(CORE_STM32)
+  if (configPage10.enable_canbus == 1) { CANSerial.begin(115200); }
+  else if (configPage10.enable_canbus == 2)
+  {
+    //enable local can interface
+  }
+#elif defined(CORE_TEENSY)
+  if (configPage10.enable_canbus == 1) { CANSerial.begin(115200); }
+  else if (configPage10.enable_canbus == 2)
+  {
+    //Teensy onboard CAN not used currently
+    //enable local can interface
+    //setup can interface to 250k
+    //FlexCAN CANbus0(2500000, 0);
+    //static CAN_message_t txmsg,rxmsg;
+    //CANbus0.begin();
+  }
+#endif
+
+  //Repoint the 2D table structs to the config pages that were just loaded
+  taeTable.valueSize = SIZE_BYTE; //Set this table to use byte values
+  taeTable.xSize = 4;
+  taeTable.values = configPage2.taeValues;
+  taeTable.axisX = configPage2.taeBins;
+  WUETable.valueSize = SIZE_BYTE; //Set this table to use byte values
+  WUETable.xSize = 10;
+  WUETable.values = configPage1.wueValues;
+  WUETable.axisX = configPage2.wueBins;
+  crankingEnrichTable.valueSize = SIZE_BYTE;
+  crankingEnrichTable.xSize = 4;
+  crankingEnrichTable.values = configPage11.crankingEnrichValues;
+  crankingEnrichTable.axisX = configPage11.crankingEnrichBins;
+
+  dwellVCorrectionTable.valueSize = SIZE_BYTE;
+  dwellVCorrectionTable.xSize = 6;
+  dwellVCorrectionTable.values = configPage2.dwellCorrectionValues;
+  dwellVCorrectionTable.axisX = configPage3.voltageCorrectionBins;
+  injectorVCorrectionTable.valueSize = SIZE_BYTE;
+  injectorVCorrectionTable.xSize = 6;
+  injectorVCorrectionTable.values = configPage3.injVoltageCorrectionValues;
+  injectorVCorrectionTable.axisX = configPage3.voltageCorrectionBins;
+  IATDensityCorrectionTable.valueSize = SIZE_BYTE;
+  IATDensityCorrectionTable.xSize = 9;
+  IATDensityCorrectionTable.values = configPage3.airDenRates;
+  IATDensityCorrectionTable.axisX = configPage3.airDenBins;
+  IATRetardTable.valueSize = SIZE_BYTE;
+  IATRetardTable.xSize = 6;
+  IATRetardTable.values = configPage2.iatRetValues;
+  IATRetardTable.axisX = configPage2.iatRetBins;
+
+  //Setup the calibration tables
+  loadCalibration();
+
+  //Set the pin mappings
+  if(configPage1.pinMapping > BOARD_NR_GPIO_PINS)
+  {
+    //First time running on this board
+    setPinMapping(3); //Force board to v0.4
+    configPage1.flexEnabled = false; //Have to disable flex. If this isn't done and the wrong flex pin is interrupt attached below, system can hang.
+  }
+  else { setPinMapping(configPage1.pinMapping); }
+
+  //Need to check early on whether the coil charging is inverted. If this is not set straight away it can cause an unwanted spark at bootup
+  if(configPage2.IgInv == 1) { coilHIGH = LOW, coilLOW = HIGH; }
+  else { coilHIGH = HIGH, coilLOW = LOW; }
+  endCoil1Charge();
+  endCoil2Charge();
+  endCoil3Charge();
+  endCoil4Charge();
+  endCoil5Charge();
+
+  //Similar for injectors, make sure they're turned off
+  closeInjector1();
+  closeInjector2();
+  closeInjector3();
+  closeInjector4();
+  closeInjector5();
+
+  //Set the tacho output default state
+  digitalWrite(pinTachOut, HIGH);
+
+  //Perform all initialisations
+  initialiseSchedulers();
+  initialiseTimers();
+  //initialiseDisplay();
+  initialiseIdle();
+  initialiseFan();
+  initialiseAuxPWM();
+  initialiseCorrections();
+  initialiseADC();
+
+  //Lookup the current MAP reading for barometric pressure
+  instanteneousMAPReading();
+  //barometric reading can be taken from either an external sensor if enabled, or simply by using the initial MAP value
+  if ( configPage3.useExtBaro != 0 )
+  {
+    readBaro();
+    EEPROM.update(EEPROM_LAST_BARO, currentStatus.baro);
+  }
+  else
+  {
+    /*
+     * The highest sea-level pressure on Earth occurs in Siberia, where the Siberian High often attains a sea-level pressure above 105 kPa;
+     * with record highs close to 108.5 kPa.
+     * The lowest measurable sea-level pressure is found at the centers of tropical cyclones and tornadoes, with a record low of 87 kPa;
+     */
+    if ((currentStatus.MAP >= BARO_MIN) && (currentStatus.MAP <= BARO_MAX)) //Check if engine isn't running
+    {
+      currentStatus.baro = currentStatus.MAP;
+      EEPROM.update(EEPROM_LAST_BARO, currentStatus.baro);
+    }
+    else
+    {
+      //Attempt to use the last known good baro reading from EEPROM
+      if ((EEPROM.read(EEPROM_LAST_BARO) >= BARO_MIN) && (EEPROM.read(EEPROM_LAST_BARO) <= BARO_MAX)) //Make sure it's not invalid (Possible on first run etc)
+      { currentStatus.baro = EEPROM.read(EEPROM_LAST_BARO); } //last baro correction
+      else { currentStatus.baro = 100; } //Final fall back position.
+    }
+  }
+
+  //Check whether the flex sensor is enabled and if so, attach an interupt for it
+  if(configPage1.flexEnabled)
+  {
+    attachInterrupt(digitalPinToInterrupt(pinFlex), flexPulse, RISING);
+    currentStatus.ethanolPct = 0;
+  }
+
+  //Once the configs have been loaded, a number of one time calculations can be completed
+  req_fuel_uS = configPage1.reqFuel * 100; //Convert to uS and an int. This is the only variable to be used in calculations
+  inj_opentime_uS = configPage1.injOpen * 100; //Injector open time. Comes through as ms*10 (Eg 15.5ms = 155).
+
+  //Begin the main crank trigger interrupt pin setup
+  //The interrupt numbering is a bit odd - See here for reference: http://arduino.cc/en/Reference/AttachInterrupt
+  //These assignments are based on the Arduino Mega AND VARY BETWEEN BOARDS. Please confirm the board you are using and update acordingly.
+  byte triggerInterrupt = 0; // By default, use the first interrupt
+  byte triggerInterrupt2 = 1;
+  currentStatus.RPM = 0;
+  currentStatus.hasSync = false;
+  currentStatus.runSecs = 0;
+  currentStatus.secl = 0;
+  currentStatus.startRevolutions = 0;
+  currentStatus.flatShiftingHard = false;
+  currentStatus.launchingHard = false;
+  triggerFilterTime = 0; //Trigger filter time is the shortest possible time (in uS) that there can be between crank teeth (ie at max RPM). Any pulses that occur faster than this time will be disgarded as noise. This is simply a default value, the actual values are set in the setup() functinos of each decoder
+
+  #if defined(CORE_AVR)
+    switch (pinTrigger) {
+      //Arduino Mega 2560 mapping
+      case 2:
+        triggerInterrupt = 0; break;
+      case 3:
+        triggerInterrupt = 1; break;
+      case 18:
+        triggerInterrupt = 5; break;
+      case 19:
+        triggerInterrupt = 4; break;
+      case 20:
+        triggerInterrupt = 3; break;
+      case 21:
+        triggerInterrupt = 2; break;
+      default:
+        triggerInterrupt = 0; break; //This should NEVER happen
+    }
+  #else
+    triggerInterrupt = pinTrigger;
+  #endif
+
+  #if defined(CORE_AVR)
+    switch (pinTrigger2) {
+      //Arduino Mega 2560 mapping
+      case 2:
+        triggerInterrupt2 = 0; break;
+      case 3:
+        triggerInterrupt2 = 1; break;
+      case 18:
+        triggerInterrupt2 = 5; break;
+      case 19:
+        triggerInterrupt2 = 4; break;
+      case 20:
+        triggerInterrupt2 = 3; break;
+      case 21:
+        triggerInterrupt2 = 2; break;
+      default:
+        triggerInterrupt2 = 0; break; //This should NEVER happen
+    }
+  #else
+    triggerInterrupt2 = pinTrigger2;
+  #endif
+  pinMode(pinTrigger, INPUT);
+  pinMode(pinTrigger2, INPUT);
+  pinMode(pinTrigger3, INPUT);
+  //digitalWrite(pinTrigger, HIGH);
+
+  //Set the trigger function based on the decoder in the config
+  switch (configPage2.TrigPattern)
+  {
+    case 0:
+      //Missing tooth decoder
+      triggerSetup_missingTooth();
+      trigger = triggerPri_missingTooth;
+      triggerSecondary = triggerSec_missingTooth;
+      getRPM = getRPM_missingTooth;
+      getCrankAngle = getCrankAngle_missingTooth;
+      triggerSetEndTeeth = triggerSetEndTeeth_missingTooth;
+
+      if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
+      else { attachInterrupt(triggerInterrupt, trigger, FALLING); }
+      if(configPage2.TrigEdgeSec == 0) { attachInterrupt(triggerInterrupt2, triggerSec_missingTooth, RISING); }
+      else { attachInterrupt(triggerInterrupt2, triggerSec_missingTooth, FALLING); }
+      break;
+
+    case 1:
+      // Basic distributor
+      triggerSetup_BasicDistributor();
+      trigger = triggerPri_BasicDistributor;
+      getRPM = getRPM_BasicDistributor;
+      getCrankAngle = getCrankAngle_BasicDistributor;
+      triggerSetEndTeeth = triggerSetEndTeeth_BasicDistributor;
+
+      if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
+      else { attachInterrupt(triggerInterrupt, trigger, FALLING); }
+      break;
+
+    case 2:
+      triggerSetup_DualWheel();
+      trigger = triggerPri_DualWheel;
+      getRPM = getRPM_DualWheel;
+      getCrankAngle = getCrankAngle_DualWheel;
+      triggerSetEndTeeth = triggerSetEndTeeth_DualWheel;
+
+      if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
+      else { attachInterrupt(triggerInterrupt, trigger, FALLING); }
+      if(configPage2.TrigEdgeSec == 0) { attachInterrupt(triggerInterrupt2, triggerSec_DualWheel, RISING); }
+      else { attachInterrupt(triggerInterrupt2, triggerSec_DualWheel, FALLING); }
+      break;
+
+    case 3:
+      triggerSetup_GM7X();
+      trigger = triggerPri_GM7X;
+      getRPM = getRPM_GM7X;
+      getCrankAngle = getCrankAngle_GM7X;
+      triggerSetEndTeeth = triggerSetEndTeeth_GM7X;
+
+      if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
+      else { attachInterrupt(triggerInterrupt, trigger, FALLING); }
+      break;
+
+    case 4:
+      triggerSetup_4G63();
+      trigger = triggerPri_4G63;
+      getRPM = getRPM_4G63;
+      getCrankAngle = getCrankAngle_4G63;
+      triggerSetEndTeeth = triggerSetEndTeeth_4G63;
+
+      //These may both need to change, not sure
+      if(configPage2.TrigEdge == 0)
+      {
+        attachInterrupt(triggerInterrupt, trigger, CHANGE);  // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
+        attachInterrupt(triggerInterrupt2, triggerSec_4G63, FALLING); //changed
+      }
+      else
+      {
+        attachInterrupt(triggerInterrupt, trigger, CHANGE); // Primary trigger connects to
+        attachInterrupt(triggerInterrupt2, triggerSec_4G63, FALLING);
+      }
+      break;
+
+    case 5:
+      triggerSetup_24X();
+      trigger = triggerPri_24X;
+      getRPM = getRPM_24X;
+      getCrankAngle = getCrankAngle_24X;
+      triggerSetEndTeeth = triggerSetEndTeeth_24X;
+
+      if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
+      else { attachInterrupt(triggerInterrupt, trigger, FALLING); } // Primary trigger connects to
+      attachInterrupt(triggerInterrupt2, triggerSec_24X, CHANGE);
+      break;
+
+    case 6:
+      triggerSetup_Jeep2000();
+      trigger = triggerPri_Jeep2000;
+      getRPM = getRPM_Jeep2000;
+      getCrankAngle = getCrankAngle_Jeep2000;
+      triggerSetEndTeeth = triggerSetEndTeeth_Jeep2000;
+
+      if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
+      else { attachInterrupt(triggerInterrupt, trigger, FALLING); } // Primary trigger connects to
+      attachInterrupt(triggerInterrupt2, triggerSec_Jeep2000, CHANGE);
+      break;
+
+    case 7:
+      triggerSetup_Audi135();
+      trigger = triggerPri_Audi135;
+      getRPM = getRPM_Audi135;
+      getCrankAngle = getCrankAngle_Audi135;
+      triggerSetEndTeeth = triggerSetEndTeeth_Audi135;
+
+      if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
+      else { attachInterrupt(triggerInterrupt, trigger, FALLING); }
+      attachInterrupt(triggerInterrupt2, triggerSec_Audi135, RISING);
+      break;
+
+    case 8:
+      triggerSetup_HondaD17();
+      trigger = triggerPri_HondaD17;
+      getRPM = getRPM_HondaD17;
+      getCrankAngle = getCrankAngle_HondaD17;
+      triggerSetEndTeeth = triggerSetEndTeeth_HondaD17;
+
+      if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
+      else { attachInterrupt(triggerInterrupt, trigger, FALLING); } // Primary trigger connects to
+      attachInterrupt(triggerInterrupt2, triggerSec_HondaD17, CHANGE);
+      break;
+
+    case 9:
+      triggerSetup_Miata9905();
+      trigger = triggerPri_Miata9905;
+      getRPM = getRPM_Miata9905;
+      getCrankAngle = getCrankAngle_Miata9905;
+      triggerSetEndTeeth = triggerSetEndTeeth_Miata9905;
+
+      //These may both need to change, not sure
+      if(configPage2.TrigEdge == 0)
+      {
+        attachInterrupt(triggerInterrupt, trigger, RISING);  // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
+        attachInterrupt(triggerInterrupt2, triggerSec_Miata9905, FALLING); //changed
+      }
+      else
+      {
+        attachInterrupt(triggerInterrupt, trigger, FALLING); // Primary trigger connects to
+        attachInterrupt(triggerInterrupt2, triggerSec_Miata9905, RISING);
+      }
+      break;
+
+    case 10:
+      triggerSetup_MazdaAU();
+      trigger = triggerPri_MazdaAU;
+      getRPM = getRPM_MazdaAU;
+      getCrankAngle = getCrankAngle_MazdaAU;
+      triggerSetEndTeeth = triggerSetEndTeeth_MazdaAU;
+
+      if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
+      else { attachInterrupt(triggerInterrupt, trigger, FALLING); } // Primary trigger connects to
+      attachInterrupt(triggerInterrupt2, triggerSec_MazdaAU, FALLING);
+      break;
+
+    case 11:
+      triggerSetup_non360();
+      trigger = triggerPri_DualWheel; //Is identical to the dual wheel decoder, so that is used. Same goes for the secondary below
+      getRPM = getRPM_non360;
+      getCrankAngle = getCrankAngle_non360;
+      triggerSetEndTeeth = triggerSetEndTeeth_Non360;
+
+      if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
+      else { attachInterrupt(triggerInterrupt, trigger, FALLING); }
+      attachInterrupt(triggerInterrupt2, triggerSec_DualWheel, FALLING); //Note the use of the Dual Wheel trigger function here. No point in having the same code in twice.
+      break;
+
+    case 12:
+        triggerSetup_Nissan360();
+        trigger = triggerPri_Nissan360;
+        getRPM = getRPM_Nissan360;
+        getCrankAngle = getCrankAngle_Nissan360;
+        triggerSetEndTeeth = triggerSetEndTeeth_Nissan360;
+
+        if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
+        else { attachInterrupt(triggerInterrupt, trigger, FALLING); }
+        attachInterrupt(triggerInterrupt2, triggerSec_Nissan360, CHANGE);
+        break;
+
+    case 13:
+            triggerSetup_Subaru67();
+            trigger = triggerPri_Subaru67;
+            getRPM = getRPM_Subaru67;
+            getCrankAngle = getCrankAngle_Subaru67;
+            triggerSetEndTeeth = triggerSetEndTeeth_Subaru67;
+
+            if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
+            else { attachInterrupt(triggerInterrupt, trigger, FALLING); }
+            attachInterrupt(triggerInterrupt2, triggerSec_Subaru67, FALLING);
+            break;
+
+    case 14:
+            triggerSetup_Daihatsu();
+            trigger = triggerPri_Daihatsu;
+            getRPM = getRPM_Daihatsu;
+            getCrankAngle = getCrankAngle_Daihatsu;
+            triggerSetEndTeeth = triggerSetEndTeeth_Daihatsu;
+
+            if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
+            else { attachInterrupt(triggerInterrupt, trigger, FALLING); }
+            //No secondary input required for this pattern
+            break;
+
+    default:
+      trigger = triggerPri_missingTooth;
+      getRPM = getRPM_missingTooth;
+      getCrankAngle = getCrankAngle_missingTooth;
+
+      if(configPage2.TrigEdge == 0) { attachInterrupt(triggerInterrupt, trigger, RISING); } // Attach the crank trigger wheel interrupt (Hall sensor drags to ground when triggering)
+      else { attachInterrupt(triggerInterrupt, trigger, FALLING); }
+      break;
+  }
+
+  //End crank triger interrupt attachment
+
+  req_fuel_uS = req_fuel_uS / engineSquirtsPerCycle; //The req_fuel calculation above gives the total required fuel (At VE 100%) in the full cycle. If we're doing more than 1 squirt per cycle then we need to split the amount accordingly. (Note that in a non-sequential 4-stroke setup you cannot have less than 2 squirts as you cannot determine the stroke to make the single squirt on)
+
+  //Initial values for loop times
+  previousLoopTime = 0;
+  currentLoopTime = micros();
+
+  mainLoopCount = 0;
+  ignitionCount = 0;
+
+  //Calculate the number of degrees between cylinders
+  switch (configPage1.nCylinders) {
+    case 1:
+      channel1IgnDegrees = 0;
+      channel1InjDegrees = 0;
+
+      channel1InjEnabled = true;
+      break;
+
+    case 2:
+      channel1IgnDegrees = 0;
+
+      if (configPage1.engineType == EVEN_FIRE )
+      {
+        channel2IgnDegrees = 180;
+      }
+      else { channel2IgnDegrees = configPage1.oddfire2; }
+
+      //For alternating injection, the squirt occurs at different times for each channel
+      if(configPage1.injLayout == INJ_SEMISEQUENTIAL)
+      {
+        channel1InjDegrees = 0;
+        channel2InjDegrees = channel2IgnDegrees; //Set to the same as the ignition degrees (Means there's no need for another if to check for oddfire)
+      }
+      if (!configPage1.injTiming) { channel1InjDegrees = channel2InjDegrees = 0; } //For simultaneous, all squirts happen at the same time
+
+      channel1InjEnabled = true;
+      channel2InjEnabled = true;
+      break;
+
+    case 3:
+      channel1IgnDegrees = 0;
+
+      if (configPage1.engineType == EVEN_FIRE )
+      {
+        if(configPage2.sparkMode == IGN_MODE_SEQUENTIAL)
+        {
+          channel2IgnDegrees = 240;
+          channel3IgnDegrees = 480;
+
+          CRANK_ANGLE_MAX_IGN = 720;
+        }
+        else
+        {
+          channel2IgnDegrees = 120;
+          channel3IgnDegrees = 240;
+        }
+      }
+      else
+      {
+        channel2IgnDegrees = configPage1.oddfire2;
+        channel3IgnDegrees = configPage1.oddfire3;
+      }
+
+      //For alternatiing injection, the squirt occurs at different times for each channel
+      if(configPage1.injLayout == INJ_SEMISEQUENTIAL  || configPage1.injLayout == INJ_PAIRED)
+      {
+        channel1InjDegrees = 0;
+        channel2InjDegrees = 120;
+        channel3InjDegrees = 240;
+      }
+      else if (configPage1.injLayout == INJ_SEQUENTIAL)
+      {
+        channel1InjDegrees = 0;
+        channel2InjDegrees = 240;
+        channel3InjDegrees = 480;
+        CRANK_ANGLE_MAX_INJ = 720;
+        req_fuel_uS = req_fuel_uS * 2;
+      }
+      if (!configPage1.injTiming) { channel1InjDegrees = channel2InjDegrees = channel3InjDegrees = 0; } //For simultaneous, all squirts happen at the same time
+
+      channel1InjEnabled = true;
+      channel2InjEnabled = true;
+      channel3InjEnabled = true;
+      break;
+    case 4:
+      channel1IgnDegrees = 0;
+
+      if (configPage1.engineType == EVEN_FIRE )
+      {
+        channel2IgnDegrees = 180;
+
+        if(configPage2.sparkMode == IGN_MODE_SEQUENTIAL)
+        {
+          channel3IgnDegrees = 360;
+          channel4IgnDegrees = 540;
+
+          CRANK_ANGLE_MAX_IGN = 720;
+        }
+      }
+      else
+      {
+        channel2IgnDegrees = configPage1.oddfire2;
+        channel3IgnDegrees = configPage1.oddfire3;
+        channel4IgnDegrees = configPage1.oddfire4;
+      }
+
+      //For alternatiing injection, the squirt occurs at different times for each channel
+      if(configPage1.injLayout == INJ_SEMISEQUENTIAL || configPage1.injLayout == INJ_PAIRED)
+      {
+        channel1InjDegrees = 0;
+        channel2InjDegrees = 180;
+      }
+      else if (configPage1.injLayout == INJ_SEQUENTIAL)
+      {
+        channel1InjDegrees = 0;
+        channel2InjDegrees = 180;
+        channel3InjDegrees = 360;
+        channel4InjDegrees = 540;
+
+        channel3InjEnabled = true;
+        channel4InjEnabled = true;
+
+        CRANK_ANGLE_MAX_INJ = 720;
+        req_fuel_uS = req_fuel_uS * 2;
+      }
+      if (!configPage1.injTiming) { channel1InjDegrees = channel2InjDegrees = 0; } //For simultaneous, all squirts happen at the same time
+
+      channel1InjEnabled = true;
+      channel2InjEnabled = true;
+      break;
+    case 5:
+      channel1IgnDegrees = 0;
+      channel2IgnDegrees = 72;
+      channel3IgnDegrees = 144;
+      channel4IgnDegrees = 216;
+      channel5IgnDegrees = 288;
+
+      if(configPage2.sparkMode == IGN_MODE_SEQUENTIAL)
+      {
+        channel2IgnDegrees = 144;
+        channel3IgnDegrees = 288;
+        channel4IgnDegrees = 432;
+        channel5IgnDegrees = 576;
+
+        CRANK_ANGLE_MAX_IGN = 720;
+      }
+
+      //For alternatiing injection, the squirt occurs at different times for each channel
+      if(configPage1.injLayout == INJ_SEMISEQUENTIAL || configPage1.injLayout == INJ_PAIRED)
+      {
+        channel1InjDegrees = 0;
+        channel2InjDegrees = 72;
+        channel3InjDegrees = 144;
+        channel4InjDegrees = 216;
+        channel5InjDegrees = 288;
+      }
+      else if (configPage1.injLayout == INJ_SEQUENTIAL)
+      {
+        channel1InjDegrees = 0;
+        channel2InjDegrees = 144;
+        channel3InjDegrees = 288;
+        channel4InjDegrees = 432;
+        channel5InjDegrees = 576;
+
+        CRANK_ANGLE_MAX_INJ = 720;
+      }
+      if (!configPage1.injTiming) { channel1InjDegrees = channel2InjDegrees = channel3InjDegrees = channel4InjDegrees = channel5InjDegrees = 0; } //For simultaneous, all squirts happen at the same time
+
+      channel1InjEnabled = true;
+      channel2InjEnabled = true;
+      channel3InjEnabled = false; //this is disabled as injector 5 function calls 3 & 5 together
+      channel4InjEnabled = true;
+      channel5InjEnabled = true;
+      break;
+    case 6:
+      channel1IgnDegrees = 0;
+      channel1InjDegrees = 0;
+      channel2IgnDegrees = 120;
+      channel2InjDegrees = 120;
+      channel3IgnDegrees = 240;
+      channel3InjDegrees = 240;
+
+      if (!configPage1.injTiming) { channel1InjDegrees = channel2InjDegrees = channel3InjDegrees = 0; } //For simultaneous, all squirts happen at the same time
+
+      configPage1.injLayout = 0; //This is a failsafe. We can never run semi-sequential with more than 4 cylinders
+
+      channel1InjEnabled = true;
+      channel2InjEnabled = true;
+      channel3InjEnabled = true;
+      break;
+    case 8:
+      channel1IgnDegrees = channel1InjDegrees = 0;
+      channel2IgnDegrees = channel2InjDegrees = 90;
+      channel3IgnDegrees = channel3InjDegrees = 180;
+      channel4IgnDegrees = channel4InjDegrees = 270;
+
+      if (!configPage1.injTiming)  { channel1InjDegrees = channel2InjDegrees = channel3InjDegrees = channel4InjDegrees = 0; } //For simultaneous, all squirts happen at the same time
+
+      configPage1.injLayout = 0; //This is a failsafe. We can never run semi-sequential with more than 4 cylinders
+
+      channel1InjEnabled = true;
+      channel2InjEnabled = true;
+      channel3InjEnabled = true;
+      channel4InjEnabled = true;
+      break;
+    default: //Handle this better!!!
+      channel1InjDegrees = 0;
+      channel2InjDegrees = 180;
+      break;
+  }
+
+  switch(configPage2.sparkMode)
+  {
+    case IGN_MODE_WASTED:
+      //Wasted Spark (Normal mode)
+      ign1StartFunction = beginCoil1Charge;
+      ign1EndFunction = endCoil1Charge;
+      ign2StartFunction = beginCoil2Charge;
+      ign2EndFunction = endCoil2Charge;
+      ign3StartFunction = beginCoil3Charge;
+      ign3EndFunction = endCoil3Charge;
+      ign4StartFunction = beginCoil4Charge;
+      ign4EndFunction = endCoil4Charge;
+      ign5StartFunction = beginCoil5Charge;
+      ign5EndFunction = endCoil5Charge;
+      break;
+
+    case IGN_MODE_SINGLE:
+      //Single channel mode. All ignition pulses are on channel 1
+      ign1StartFunction = beginCoil1Charge;
+      ign1EndFunction = endCoil1Charge;
+      ign2StartFunction = beginCoil1Charge;
+      ign2EndFunction = endCoil1Charge;
+      ign3StartFunction = beginCoil1Charge;
+      ign3EndFunction = endCoil1Charge;
+      ign4StartFunction = beginCoil1Charge;
+      ign4EndFunction = endCoil1Charge;
+      ign5StartFunction = beginCoil1Charge;
+      ign5EndFunction = endCoil1Charge;
+      break;
+
+    case IGN_MODE_WASTEDCOP:
+      //Wasted COP mode. Ignition channels 1&3 and 2&4 are paired together
+      //This is not a valid mode for >4 cylinders
+      if( configPage1.nCylinders <= 4 )
+      {
+        ign1StartFunction = beginCoil1and3Charge;
+        ign1EndFunction = endCoil1and3Charge;
+        ign2StartFunction = beginCoil2and4Charge;
+        ign2EndFunction = endCoil2and4Charge;
+
+        ign3StartFunction = nullCallback;
+        ign3EndFunction = nullCallback;
+        ign4StartFunction = nullCallback;
+        ign4EndFunction = nullCallback;
+      }
+      else
+      {
+        //If the person has inadvertantly selected this when running more than 4 cylinders, just use standard Wasted spark mode
+        ign1StartFunction = beginCoil1Charge;
+        ign1EndFunction = endCoil1Charge;
+        ign2StartFunction = beginCoil2Charge;
+        ign2EndFunction = endCoil2Charge;
+        ign3StartFunction = beginCoil3Charge;
+        ign3EndFunction = endCoil3Charge;
+        ign4StartFunction = beginCoil4Charge;
+        ign4EndFunction = endCoil4Charge;
+        ign5StartFunction = beginCoil5Charge;
+        ign5EndFunction = endCoil5Charge;
+      }
+      break;
+
+    case IGN_MODE_SEQUENTIAL:
+      ign1StartFunction = beginCoil1Charge;
+      ign1EndFunction = endCoil1Charge;
+      ign2StartFunction = beginCoil2Charge;
+      ign2EndFunction = endCoil2Charge;
+      ign3StartFunction = beginCoil3Charge;
+      ign3EndFunction = endCoil3Charge;
+      ign4StartFunction = beginCoil4Charge;
+      ign4EndFunction = endCoil4Charge;
+      ign5StartFunction = beginCoil5Charge;
+      ign5EndFunction = endCoil5Charge;
+      break;
+
+    default:
+      //Wasted spark (Shouldn't ever happen anyway)
+      ign1StartFunction = beginCoil1Charge;
+      ign1EndFunction = endCoil1Charge;
+      ign2StartFunction = beginCoil2Charge;
+      ign2EndFunction = endCoil2Charge;
+      ign3StartFunction = beginCoil3Charge;
+      ign3EndFunction = endCoil3Charge;
+      ign4StartFunction = beginCoil4Charge;
+      ign4EndFunction = endCoil4Charge;
+      ign5StartFunction = beginCoil5Charge;
+      ign5EndFunction = endCoil5Charge;
+      break;
+  }
+
+  //Begin priming the fuel pump. This is turned off in the low resolution, 1s interrupt in timers.ino
+  digitalWrite(pinFuelPump, HIGH);
+  fuelPumpOn = true;
+  //Perform the priming pulses. Set these to run at an arbitrary time in the future (100us). The prime pulse value is in ms*10, so need to multiple by 100 to get to uS
+  setFuelSchedule1(100, (unsigned long)(configPage1.primePulse * 100));
+  setFuelSchedule2(100, (unsigned long)(configPage1.primePulse * 100));
+  setFuelSchedule3(100, (unsigned long)(configPage1.primePulse * 100));
+  setFuelSchedule4(100, (unsigned long)(configPage1.primePulse * 100));
+
+  initialisationComplete = true;
+}
+
+void loop()
+{
+      mainLoopCount++;
+      LOOP_TIMER = TIMER_mask;
+      //Check for any requets from serial. Serial operations are checked under 2 scenarios:
+      // 1) Every 64 loops (64 Is more than fast enough for TunerStudio). This function is equivalent to ((loopCount % 64) == 1) but is considerably faster due to not using the mod or division operations
+      // 2) If the amount of data in the serial buffer is greater than a set threhold (See globals.h). This is to avoid serial buffer overflow when large amounts of data is being sent
+      if ( BIT_CHECK(LOOP_TIMER, BIT_TIMER_15HZ) or (Serial.available() > SERIAL_BUFFER_THRESHOLD) )
+      {
+        if (Serial.available() > 0)
+        {
+          command();
+        }
+      }
+
+#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) //ATmega2561 does not have Serial3
+      //if serial3 interface is enabled then check for serial3 requests.
+      if (configPage10.enable_canbus == 1)
+          {
+            if ( BIT_CHECK(LOOP_TIMER, BIT_TIMER_15HZ) or (CANSerial.available() > SERIAL_BUFFER_THRESHOLD) )
+                {
+                  if (CANSerial.available() > 0)
+                    {
+                    canCommand();
+                    }
+                }
+          }
+
+#elif defined(CORE_STM32)
+      //if can or secondary serial interface is enabled then check for requests.
+      if (configPage10.enable_canbus == 1)  //secondary serial interface enabled
+          {
+            if ( BIT_CHECK(LOOP_TIMER, BIT_TIMER_15HZ) or (CANSerial.available() > SERIAL_BUFFER_THRESHOLD) )
+                {
+                  if (CANSerial.available() > 0)
+                    {
+                    canCommand();
+                    }
+                }
+          }
+      else if (configPage10.enable_canbus == 2) // can module enabled
+          {
+            //check local can module
+          }
+#elif defined(CORE_TEENSY)
+      //if can or secondary serial interface is enabled then check for requests.
+      if (configPage10.enable_canbus == 1)  //secondary serial interface enabled
+          {
+            if ( BIT_CHECK(LOOP_TIMER, BIT_TIMER_15HZ) or (CANSerial.available() > SERIAL_BUFFER_THRESHOLD) )
+                {
+                  if (CANSerial.available() > 0)
+                    {
+                    canCommand();
+                    }
+                }
+          }
+      else if (configPage10.enable_canbus == 2) // can module enabled
+          {
+            //check local can module
+            // if ( ((mainLoopCount & 31) == 1) or (CANbus0.available())
+            //    {
+            //      CANbus0.read(rx_msg);
+            //    }
+          }
+#endif
+
+    //Displays currently disabled
+    // if (configPage1.displayType && (mainLoopCount & 255) == 1) { updateDisplay();}
+
+    previousLoopTime = currentLoopTime;
+    currentLoopTime = micros();
+    unsigned long timeToLastTooth = (currentLoopTime - toothLastToothTime);
+    if ( (timeToLastTooth < MAX_STALL_TIME) || (toothLastToothTime > currentLoopTime) ) //Check how long ago the last tooth was seen compared to now. If it was more than half a second ago then the engine is probably stopped. toothLastToothTime can be greater than currentLoopTime if a pulse occurs between getting the lastest time and doing the comparison
+    {
+      currentStatus.RPM = currentStatus.longRPM = getRPM(); //Long RPM is included here
+      FUEL_PUMP_ON();
+      fuelPumpOn = true; //Not sure if this is needed.
+    }
+    else
+    {
+      //We reach here if the time between teeth is too great. This VERY likely means the engine has stopped
+      currentStatus.RPM = 0;
+      currentStatus.PW1 = 0;
+      currentStatus.VE = 0;
+      toothLastToothTime = 0;
+      //toothLastMinusOneToothTime = 0;
+      currentStatus.hasSync = false;
+      currentStatus.runSecs = 0; //Reset the counter for number of seconds running.
+      secCounter = 0; //Reset our seconds counter.
+      currentStatus.startRevolutions = 0;
+      toothSystemCount = 0;
+      MAPcurRev = 0;
+      MAPcount = 0;
+      currentStatus.rpmDOT = 0;
+      ignitionOn = false;
+      fuelOn = false;
+      if (fpPrimed) { digitalWrite(pinFuelPump, LOW); } //Turn off the fuel pump, but only if the priming is complete
+      fuelPumpOn = false;
+      disableIdle(); //Turn off the idle PWM
+      BIT_CLEAR(currentStatus.engine, BIT_ENGINE_CRANK); //Clear cranking bit (Can otherwise get stuck 'on' even with 0 rpm)
+      BIT_CLEAR(currentStatus.engine, BIT_ENGINE_WARMUP); //Same as above except for WUE
+    }
+
+    //Uncomment the following for testing
+    /*
+    currentStatus.hasSync = true;
+    currentStatus.RPM = 500;
+    */
+
+    //***Perform sensor reads***
+    //-----------------------------------------------------------------------------------------------------
+    readMAP();
+
+    if ( BIT_CHECK(LOOP_TIMER, BIT_TIMER_15HZ))
+    {
+      BIT_CLEAR(TIMER_mask, BIT_TIMER_15HZ);
+      readTPS(); //TPS reading to be performed every 32 loops (any faster and it can upset the TPSdot sampling time)
+
+      //Check for launching/flat shift (clutch) can be done around here too
+      previousClutchTrigger = clutchTrigger;
+      if(configPage3.launchHiLo) { clutchTrigger = digitalRead(pinLaunch); }
+      else { clutchTrigger = !digitalRead(pinLaunch); }
+
+      if(previousClutchTrigger != clutchTrigger) { currentStatus.clutchEngagedRPM = currentStatus.RPM; }
+
+      if (configPage3.launchEnabled && clutchTrigger && (currentStatus.clutchEngagedRPM < ((unsigned int)(configPage3.flatSArm) * 100)) && (currentStatus.RPM > ((unsigned int)(configPage3.lnchHardLim) * 100)) ) { currentStatus.launchingHard = true; BIT_SET(currentStatus.spark, BIT_SPARK_HLAUNCH); } //HardCut rev limit for 2-step launch control.
+      else { currentStatus.launchingHard = false; BIT_CLEAR(currentStatus.spark, BIT_SPARK_HLAUNCH); }
+
+      if(configPage3.flatSEnable && clutchTrigger && (currentStatus.RPM > ((unsigned int)(configPage3.flatSArm) * 100)) && (currentStatus.RPM > currentStatus.clutchEngagedRPM) ) { currentStatus.flatShiftingHard = true; }
+      else { currentStatus.flatShiftingHard = false; }
+
+      //Boost cutoff is very similar to launchControl, but with a check against MAP rather than a switch
+      if(configPage3.boostCutType && currentStatus.MAP > (configPage3.boostLimit * 2) ) //The boost limit is divided by 2 to allow a limit up to 511kPa
+      {
+        switch(configPage3.boostCutType)
+        {
+          case 1:
+            BIT_SET(currentStatus.spark, BIT_SPARK_BOOSTCUT);
+            BIT_CLEAR(currentStatus.squirt, BIT_SQUIRT_BOOSTCUT);
+            break;
+          case 2:
+            BIT_SET(currentStatus.squirt, BIT_SQUIRT_BOOSTCUT);
+            BIT_CLEAR(currentStatus.spark, BIT_SPARK_BOOSTCUT);
+            break;
+          case 3:
+            BIT_SET(currentStatus.spark, BIT_SPARK_BOOSTCUT);
+            BIT_SET(currentStatus.squirt, BIT_SQUIRT_BOOSTCUT);
+            break;
+        }
+      }
+      else
+      {
+        BIT_CLEAR(currentStatus.spark, BIT_SPARK_BOOSTCUT);
+        BIT_CLEAR(currentStatus.squirt, BIT_SQUIRT_BOOSTCUT);
+      }
+
+      //And check whether the tooth log buffer is ready
+      if(toothHistoryIndex > TOOTH_LOG_SIZE) { BIT_SET(currentStatus.squirt, BIT_SQUIRT_TOOTHLOG1READY); }
+
+      //Most boost tends to run at about 30Hz, so placing it here ensures a new target time is fetched frequently enough
+      boostControl();
+    }
+    if(BIT_CHECK(LOOP_TIMER, BIT_TIMER_30HZ)) //Every 64 loops
+    {
+      //Nothing here currently
+      BIT_CLEAR(TIMER_mask, BIT_TIMER_30HZ);
+    }
+    //The IAT and CLT readings can be done less frequently. This still runs about 4 times per second
+    if (BIT_CHECK(LOOP_TIMER, BIT_TIMER_4HZ)) //Every 256 loops
+    {
+       BIT_CLEAR(TIMER_mask, BIT_TIMER_4HZ);
+       readCLT();
+       readIAT();
+       readO2();
+       readBat();
+#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) //ATmega2561 does not have Serial3
+      //if Can interface is enabled then check for serial3 requests.
+      if (configPage10.enable_canbus == 1)  // megas only support can via secondary serial
+          {
+            if (configPage10.enable_candata_in)
+              {
+                if (BIT_CHECK(configPage10.caninput_sel,currentStatus.current_caninchannel))  //if current input channel bit is enabled
+                  {
+                    sendCancommand(2,0,currentStatus.current_caninchannel,0,((configPage10.caninput_param_group[currentStatus.current_caninchannel]&2047)+256));    //send an R command for data from paramgroup[currentStatus.current_caninchannel]
+                  }
+                else
+                  {
+                    if (currentStatus.current_caninchannel < 15)
+                        {
+                          currentStatus.current_caninchannel++;   //step to next input channel if under 15
+                        }
+                    else
+                        {
+                          currentStatus.current_caninchannel = 0;   //reset input channel back to 1
+                        }
+                  }
+              }
+          }
+#elif defined(CORE_STM32) || defined(CORE_TEENSY)
+      //if serial3io is enabled then check for serial3 requests.
+            if (configPage10.enable_candata_in)
+              {
+                if (BIT_CHECK(configPage10.caninput_sel,currentStatus.current_caninchannel))  //if current input channel is enabled
+                  {
+                    if (configPage10.enable_canbus == 1)  //can via secondary serial
+                    {
+                      sendCancommand(2,0,currentStatus.current_caninchannel,0,((configPage10.caninput_param_group[currentStatus.current_caninchannel]&2047)+256));    //send an R command for data from paramgroup[currentStatus.current_caninchannel]
+                    }
+                    else if (configPage10.enable_canbus == 2) // can via internal can module
+                    {
+                      sendCancommand(3,configPage10.speeduino_tsCanId,currentStatus.current_caninchannel,0,configPage10.caninput_param_group[currentStatus.current_caninchannel]);    //send via localcanbus the command for data from paramgroup[currentStatus.current_caninchannel]
+                    }
+                  }
+                else
+                  {
+                    if (currentStatus.current_caninchannel < 15)
+                        {
+                          currentStatus.current_caninchannel++;   //step to next input channel if under 15
+                        }
+                    else
+                        {
+                          currentStatus.current_caninchannel = 0;   //reset input channel back to 0
+                        }
+                  }
+              }
+
+#endif
+       vvtControl();
+       idleControl(); //Perform any idle related actions. Even at higher frequencies, running 4x per second is sufficient.
+    }
+    if (BIT_CHECK(LOOP_TIMER, BIT_TIMER_1HZ)) //Every 1024 loops (Approx. 1 per second)
+    {
+      //Approx. once per second
+      BIT_CLEAR(TIMER_mask, BIT_TIMER_1HZ);
+      readBaro();
+    }
+
+    if(configPage4.iacAlgorithm == IAC_ALGORITHM_STEP_OL || configPage4.iacAlgorithm == IAC_ALGORITHM_STEP_CL) { idleControl(); } //Run idlecontrol every loop for stepper idle.
+
+    //Always check for sync
+    //Main loop runs within this clause
+    if (currentStatus.hasSync && (currentStatus.RPM > 0))
+    {
+        if(currentStatus.startRevolutions >= configPage2.StgCycles)  { ignitionOn = true; fuelOn = true;} //Enable the fuel and ignition, assuming staging revolutions are complete
+        //If it is, check is we're running or cranking
+        if(currentStatus.RPM > ((unsigned int)configPage2.crankRPM * 100)) //Crank RPM stored in byte as RPM / 100
+        {
+          BIT_SET(currentStatus.engine, BIT_ENGINE_RUN); //Sets the engine running bit
+          //Only need to do anything if we're transitioning from cranking to running
+          if( BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) )
+          {
+            BIT_CLEAR(currentStatus.engine, BIT_ENGINE_CRANK); //clears the engine cranking bit
+            if(configPage2.ignBypassEnabled) { digitalWrite(pinIgnBypass, HIGH); }
+          }
+        }
+        else
+        {  //Sets the engine cranking bit, clears the engine running bit
+          BIT_SET(currentStatus.engine, BIT_ENGINE_CRANK);
+          BIT_CLEAR(currentStatus.engine, BIT_ENGINE_RUN);
+          currentStatus.runSecs = 0; //We're cranking (hopefully), so reset the engine run time to prompt ASE.
+          if(configPage2.ignBypassEnabled) { digitalWrite(pinIgnBypass, LOW); }
+        }
+      //END SETTING STATUSES
+      //-----------------------------------------------------------------------------------------------------
+
+      //Begin the fuel calculation
+      //Calculate an injector pulsewidth from the VE
+      currentStatus.corrections = correctionsFuel();
+      lastAdvance = currentStatus.advance; //Store the previous advance value
+      if (configPage1.algorithm == 0) //Check which fuelling algorithm is being used
+      {
+        //Speed Density
+        currentStatus.VE = get3DTableValue(&fuelTable, currentStatus.MAP, currentStatus.RPM); //Perform lookup into fuel map for RPM vs MAP value
+        currentStatus.PW1 = PW_SD(req_fuel_uS, currentStatus.VE, currentStatus.MAP, currentStatus.corrections, inj_opentime_uS);
+        currentStatus.advance = get3DTableValue(&ignitionTable, currentStatus.MAP, currentStatus.RPM) - OFFSET_IGNITION; //As above, but for ignition advance
+      }
+      else
+      {
+        //Alpha-N
+        currentStatus.VE = get3DTableValue(&fuelTable, currentStatus.TPS, currentStatus.RPM); //Perform lookup into fuel map for RPM vs TPS value
+        currentStatus.PW1 = PW_AN(req_fuel_uS, currentStatus.VE, currentStatus.TPS, currentStatus.corrections, inj_opentime_uS); //Calculate pulsewidth using the Alpha-N algorithm (in uS)
+        currentStatus.advance = get3DTableValue(&ignitionTable, currentStatus.TPS, currentStatus.RPM) - OFFSET_IGNITION; //As above, but for ignition advance
+      }
+
+      currentStatus.advance = correctionsIgn(currentStatus.advance);
+
+      int injector1StartAngle = 0;
+      int injector2StartAngle = 0;
+      int injector3StartAngle = 0; //Currently used for 3 cylinder only
+      int injector4StartAngle = 0; //Not used until sequential gets written
+      int injector5StartAngle = 0; //For 5 cylinder testing
+      int ignition1StartAngle = 0;
+      int ignition2StartAngle = 0;
+      int ignition3StartAngle = 0; //Currently used for 3 cylinder only
+      int ignition4StartAngle = 0; //Not used until sequential or 4+ cylinders support gets written
+      int ignition5StartAngle = 0; //Not used until sequential or 4+ cylinders support gets written
+      //These are used for comparisons on channels above 1 where the starting angle (for injectors or ignition) can be less than a single loop time
+      //(Don't ask why this is needed, it will break your head)
+      int tempCrankAngle;
+      int tempStartAngle;
+
+      //********************************************************
+      //How fast are we going? Need to know how long (uS) it will take to get from one tooth to the next. We then use that to estimate how far we are between the last tooth and the next one
+      //We use a 1st Deriv accleration prediction, but only when there is an even spacing between primary sensor teeth
+      //Any decoder that has uneven spacing has its triggerToothAngle set to 0
+      if(secondDerivEnabled && toothHistoryIndex >= 3 && currentStatus.RPM < 2000) //toothHistoryIndex must be greater than or equal to 3 as we need the last 3 entries. Currently this mode only runs below 3000 rpm
+      //if(true)
+      {
+        //Only recalculate deltaV if the tooth has changed since last time (DeltaV stays the same until the next tooth)
+        //if (deltaToothCount != toothCurrentCount)
+        {
+          deltaToothCount = toothCurrentCount;
+          int angle1, angle2; //These represent the crank angles that are travelled for the last 2 pulses
+          if(configPage2.TrigPattern == 4)
+          {
+            //Special case for 70/110 pattern on 4g63
+            angle2 = triggerToothAngle; //Angle 2 is the most recent
+            if (angle2 == 70) { angle1 = 110; }
+            else { angle1 = 70; }
+          }
+          else if(configPage2.TrigPattern == 0)
+          {
+            //Special case for missing tooth decoder where the missing tooth was one of the last 2 seen
+            if(toothCurrentCount == 1) { angle2 = 2*triggerToothAngle; angle1 = triggerToothAngle; }
+            else if(toothCurrentCount == 2) { angle1 = 2*triggerToothAngle; angle2 = triggerToothAngle; }
+            else { angle1 = angle2 = triggerToothAngle; }
+          }
+          else { angle1 = angle2 = triggerToothAngle; }
+
+          long toothDeltaV = (1000000L * angle2 / toothHistory[toothHistoryIndex]) - (1000000L * angle1 / toothHistory[toothHistoryIndex-1]);
+          long toothDeltaT = toothHistory[toothHistoryIndex];
+          //long timeToLastTooth = micros() - toothLastToothTime;
+
+          rpmDelta = (toothDeltaV << 10) / (6 * toothDeltaT);
+        }
+
+
+        timePerDegree = ldiv( 166666L, (currentStatus.RPM + rpmDelta)).quot; //There is a small amount of rounding in this calculation, however it is less than 0.001 of a uS (Faster as ldiv than / )
+      }
+      else
+      {
+        long rpm_adjust = ((long)(micros() - toothOneTime) * (long)currentStatus.rpmDOT) / 1000000; //Take into account any likely accleration that has occurred since the last full revolution completed
+
+        //timePerDegree = DIV_ROUND_CLOSEST(166666L, (currentStatus.RPM + rpm_adjust));
+        timePerDegree = ldiv( 166666L, currentStatus.RPM + rpm_adjust).quot; //There is a small amount of rounding in this calculation, however it is less than 0.001 of a uS (Faster as ldiv than / )
+      }
+
+      //Check that the duty cycle of the chosen pulsewidth isn't too high. This is disabled at cranking
+      if( !BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) )
+      {
+        unsigned long pwLimit = percentage(configPage1.dutyLim, revolutionTime); //The pulsewidth limit is determined to be the duty cycle limit (Eg 85%) by the total time it takes to perform 1 revolution
+        if (CRANK_ANGLE_MAX_INJ == 720) { pwLimit = pwLimit * 2; } //For sequential, the maximum pulse time is double (2 revolutions). Wouldn't work for 2 stroke...
+        if (currentStatus.PW1 > pwLimit) { currentStatus.PW1 = pwLimit; }
+      }
+
+
+      //***********************************************************************************************
+      //BEGIN INJECTION TIMING
+      //Determine next firing angles
+      currentStatus.PW2 = currentStatus.PW3 = currentStatus.PW4 = currentStatus.PW1; // Initial state is for all pulsewidths to be the same (This gets changed below)
+      if(!configPage1.indInjAng) {configPage1.inj4Ang = configPage1.inj3Ang = configPage1.inj2Ang = configPage1.inj1Ang;} //Forcing all injector close angles to be the same.
+      int PWdivTimerPerDegree = div(currentStatus.PW1, timePerDegree).quot; //How many crank degrees the calculated PW will take at the current speed
+      injector1StartAngle = configPage1.inj1Ang - ( PWdivTimerPerDegree ); //This is a little primitive, but is based on the idea that all fuel needs to be delivered before the inlet valve opens. See http://www.extraefi.co.uk/sequential_fuel.html for more detail
+      if(injector1StartAngle < 0) {injector1StartAngle += CRANK_ANGLE_MAX_INJ;}
+
+      //Repeat the above for each cylinder
+      switch (configPage1.nCylinders)
+      {
+        //2 cylinders
+        case 2:
+          injector2StartAngle = (configPage1.inj2Ang + channel2InjDegrees - ( PWdivTimerPerDegree ));
+          if(injector2StartAngle > CRANK_ANGLE_MAX_INJ) {injector2StartAngle -= CRANK_ANGLE_MAX_INJ;}
+          break;
+        //3 cylinders
+        case 3:
+          injector2StartAngle = (configPage1.inj2Ang + channel2InjDegrees - ( PWdivTimerPerDegree ));
+          if(injector2StartAngle > CRANK_ANGLE_MAX_INJ) {injector2StartAngle -= CRANK_ANGLE_MAX_INJ;}
+          injector3StartAngle = (configPage1.inj3Ang + channel3InjDegrees - ( PWdivTimerPerDegree ));
+          if(injector3StartAngle > CRANK_ANGLE_MAX_INJ) {injector3StartAngle -= CRANK_ANGLE_MAX_INJ;}
+          break;
+        //4 cylinders
+        case 4:
+          injector2StartAngle = (configPage1.inj2Ang + channel2InjDegrees - ( PWdivTimerPerDegree ));
+          if(injector2StartAngle > CRANK_ANGLE_MAX_INJ) {injector2StartAngle -= CRANK_ANGLE_MAX_INJ;}
+
+          if(configPage1.injLayout == INJ_SEQUENTIAL)
+          {
+            injector3StartAngle = (configPage1.inj3Ang + channel3InjDegrees - ( PWdivTimerPerDegree ));
+            if(injector3StartAngle > CRANK_ANGLE_MAX_INJ) {injector3StartAngle -= CRANK_ANGLE_MAX_INJ;}
+            injector4StartAngle = (configPage1.inj4Ang + channel4InjDegrees - ( PWdivTimerPerDegree ));
+            if(injector4StartAngle > CRANK_ANGLE_MAX_INJ) {injector4StartAngle -= CRANK_ANGLE_MAX_INJ;}
+
+            if(configPage3.fuelTrimEnabled)
+            {
+              unsigned long pw1percent = 100 + (byte)get3DTableValue(&trim1Table, currentStatus.MAP, currentStatus.RPM) - OFFSET_FUELTRIM;
+              unsigned long pw2percent = 100 + (byte)get3DTableValue(&trim2Table, currentStatus.MAP, currentStatus.RPM) - OFFSET_FUELTRIM;
+              unsigned long pw3percent = 100 + (byte)get3DTableValue(&trim3Table, currentStatus.MAP, currentStatus.RPM) - OFFSET_FUELTRIM;
+              unsigned long pw4percent = 100 + (byte)get3DTableValue(&trim4Table, currentStatus.MAP, currentStatus.RPM) - OFFSET_FUELTRIM;
+
+              if (pw1percent != 100) { currentStatus.PW1 = (pw1percent * currentStatus.PW1) / 100; }
+              if (pw2percent != 100) { currentStatus.PW2 = (pw2percent * currentStatus.PW2) / 100; }
+              if (pw3percent != 100) { currentStatus.PW3 = (pw3percent * currentStatus.PW3) / 100; }
+              if (pw4percent != 100) { currentStatus.PW4 = (pw4percent * currentStatus.PW4) / 100; }
+            }
+          }
+          break;
+        //5 cylinders
+        case 5:
+          injector2StartAngle = (configPage1.inj2Ang + channel2InjDegrees - ( PWdivTimerPerDegree ));
+          if(injector2StartAngle > CRANK_ANGLE_MAX_INJ) {injector2StartAngle -= CRANK_ANGLE_MAX_INJ;}
+          injector3StartAngle = (configPage1.inj3Ang + channel3InjDegrees - ( PWdivTimerPerDegree ));
+          if(injector3StartAngle > CRANK_ANGLE_MAX_INJ) {injector3StartAngle -= CRANK_ANGLE_MAX_INJ;}
+          injector4StartAngle = (configPage1.inj4Ang + channel4InjDegrees - ( PWdivTimerPerDegree ));
+          if(injector4StartAngle > CRANK_ANGLE_MAX_INJ) {injector4StartAngle -= CRANK_ANGLE_MAX_INJ;}
+          injector5StartAngle = (configPage1.inj1Ang + channel5InjDegrees - ( PWdivTimerPerDegree ));
+          if(injector5StartAngle > CRANK_ANGLE_MAX_INJ) {injector5StartAngle -= CRANK_ANGLE_MAX_INJ;}
+          break;
+        //6 cylinders
+        case 6:
+          injector2StartAngle = (configPage1.inj2Ang + channel2InjDegrees - ( PWdivTimerPerDegree ));
+          if(injector2StartAngle > CRANK_ANGLE_MAX_INJ) {injector2StartAngle -= CRANK_ANGLE_MAX_INJ;}
+          injector3StartAngle = (configPage1.inj3Ang + channel3InjDegrees - ( PWdivTimerPerDegree ));
+          if(injector3StartAngle > CRANK_ANGLE_MAX_INJ) {injector3StartAngle -= CRANK_ANGLE_MAX_INJ;}
+          break;
+        //8 cylinders
+        case 8:
+          injector2StartAngle = (configPage1.inj2Ang + channel2InjDegrees - ( PWdivTimerPerDegree ));
+          if(injector2StartAngle > CRANK_ANGLE_MAX_INJ) {injector2StartAngle -= CRANK_ANGLE_MAX_INJ;}
+          injector3StartAngle = (configPage1.inj3Ang + channel3InjDegrees - ( PWdivTimerPerDegree ));
+          if(injector3StartAngle > CRANK_ANGLE_MAX_INJ) {injector3StartAngle -= CRANK_ANGLE_MAX_INJ;}
+          injector4StartAngle = (configPage1.inj4Ang + channel4InjDegrees - ( PWdivTimerPerDegree ));
+          if(injector4StartAngle > CRANK_ANGLE_MAX_INJ) {injector4StartAngle -= CRANK_ANGLE_MAX_INJ;}
+          break;
+        //Will hit the default case on 1 cylinder or >8 cylinders. Do nothing in these cases
+        default:
+          break;
+      }
+
+      //***********************************************************************************************
+      //| BEGIN IGNITION CALCULATIONS
+      BIT_CLEAR(currentStatus.spark, BIT_SPARK_HRDLIM);
+      if (currentStatus.RPM > ((unsigned int)(configPage2.HardRevLim) * 100) ) { BIT_SET(currentStatus.spark, BIT_SPARK_HRDLIM); } //Hardcut RPM limit
+
+
+      //Set dwell
+       //Dwell is stored as ms * 10. ie Dwell of 4.3ms would be 43 in configPage2. This number therefore needs to be multiplied by 100 to get dwell in uS
+      if ( BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) ) { currentStatus.dwell =  (configPage2.dwellCrank * 100); }
+      else { currentStatus.dwell =  (configPage2.dwellRun * 100); }
+      currentStatus.dwell = correctionsDwell(currentStatus.dwell);
+
+      int dwellAngle = (div(currentStatus.dwell, timePerDegree).quot ); //Convert the dwell time to dwell angle based on the current engine speed
+
+      //Calculate start angle for each channel
+      //1 cylinder (Everyone gets this)
+      ignition1EndAngle = CRANK_ANGLE_MAX_IGN - currentStatus.advance;
+      ignition1StartAngle = ignition1EndAngle - dwellAngle; // 360 - desired advance angle - number of degrees the dwell will take
+      if(ignition1StartAngle < 0) {ignition1StartAngle += CRANK_ANGLE_MAX_IGN;}
+
+      //This test for more cylinders and do the same thing
+      switch (configPage1.nCylinders)
+      {
+        //2 cylinders
+        case 2:
+          ignition2EndAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
+          ignition2StartAngle = ignition2EndAngle - dwellAngle;
+          if(ignition2StartAngle > CRANK_ANGLE_MAX_IGN) {ignition2StartAngle -= CRANK_ANGLE_MAX_IGN;}
+          break;
+        //3 cylinders
+        case 3:
+          ignition2EndAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
+          ignition2StartAngle = ignition2EndAngle - dwellAngle;
+          if(ignition2StartAngle > CRANK_ANGLE_MAX_IGN) {ignition2StartAngle -= CRANK_ANGLE_MAX_IGN;}
+          ignition3EndAngle = channel3IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
+          ignition3StartAngle = channel3IgnDegrees + 360 - currentStatus.advance - dwellAngle;
+          if(ignition3StartAngle > CRANK_ANGLE_MAX_IGN) {ignition3StartAngle -= CRANK_ANGLE_MAX_IGN;}
+          break;
+        //4 cylinders
+        case 4:
+          ignition2EndAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
+          ignition2StartAngle = ignition2EndAngle - dwellAngle;
+          if(ignition2StartAngle > CRANK_ANGLE_MAX_IGN) {ignition2StartAngle -= CRANK_ANGLE_MAX_IGN;}
+          if(ignition2StartAngle < 0) {ignition2StartAngle += CRANK_ANGLE_MAX_IGN;}
+
+          if(configPage2.sparkMode == IGN_MODE_SEQUENTIAL)
+          {
+            ignition3EndAngle = channel3IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
+            ignition3StartAngle = ignition3EndAngle - dwellAngle;
+            if(ignition3StartAngle > CRANK_ANGLE_MAX_IGN) {ignition3StartAngle -= CRANK_ANGLE_MAX_IGN;}
+
+            ignition4EndAngle = channel4IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
+            ignition4StartAngle = ignition4EndAngle - dwellAngle;
+            if(ignition4StartAngle > CRANK_ANGLE_MAX_IGN) {ignition4StartAngle -= CRANK_ANGLE_MAX_IGN;}
+          }
+          break;
+        //5 cylinders
+        case 5:
+          ignition2EndAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
+          ignition2StartAngle = ignition2EndAngle - dwellAngle;
+          if(ignition2StartAngle > CRANK_ANGLE_MAX_IGN) {ignition2StartAngle -= CRANK_ANGLE_MAX_IGN;}
+          if(ignition2StartAngle < 0) {ignition2StartAngle += CRANK_ANGLE_MAX_IGN;}
+
+          ignition3EndAngle = channel3IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
+          ignition3StartAngle = ignition3EndAngle - dwellAngle;
+          if(ignition3StartAngle > CRANK_ANGLE_MAX_IGN) {ignition3StartAngle -= CRANK_ANGLE_MAX_IGN;}
+
+          ignition4EndAngle = channel4IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
+          ignition4StartAngle = ignition4EndAngle - dwellAngle;
+          if(ignition4StartAngle > CRANK_ANGLE_MAX_IGN) {ignition4StartAngle -= CRANK_ANGLE_MAX_IGN;}
+
+          ignition5StartAngle = channel5IgnDegrees + CRANK_ANGLE_MAX - currentStatus.advance - dwellAngle;
+          if(ignition5StartAngle > CRANK_ANGLE_MAX_IGN) {ignition5StartAngle -= CRANK_ANGLE_MAX_IGN;}
+
+          break;
+        //6 cylinders
+        case 6:
+          ignition2EndAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
+          ignition2StartAngle = ignition2EndAngle - dwellAngle;
+          if(ignition2StartAngle > CRANK_ANGLE_MAX_IGN) {ignition2StartAngle -= CRANK_ANGLE_MAX_IGN;}
+
+          ignition3EndAngle = channel3IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
+          ignition3StartAngle = ignition3EndAngle - dwellAngle;
+          if(ignition3StartAngle > CRANK_ANGLE_MAX_IGN) {ignition3StartAngle -= CRANK_ANGLE_MAX_IGN;}
+          break;
+        //8 cylinders
+        case 8:
+          ignition2EndAngle = channel2IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
+          ignition2StartAngle = ignition2EndAngle - dwellAngle;
+          if(ignition2StartAngle > CRANK_ANGLE_MAX_IGN) {ignition2StartAngle -= CRANK_ANGLE_MAX_IGN;}
+
+          ignition3EndAngle = channel3IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
+          ignition3StartAngle = ignition3EndAngle - dwellAngle;
+          if(ignition3StartAngle > CRANK_ANGLE_MAX_IGN) {ignition3StartAngle -= CRANK_ANGLE_MAX_IGN;}
+
+          ignition4EndAngle = channel4IgnDegrees + CRANK_ANGLE_MAX_IGN - currentStatus.advance;
+          ignition4StartAngle = ignition4EndAngle - dwellAngle;
+          if(ignition4StartAngle > CRANK_ANGLE_MAX_IGN) {ignition4StartAngle -= CRANK_ANGLE_MAX_IGN;}
+          break;
+
+        //Will hit the default case on 1 cylinder or >8 cylinders. Do nothing in these cases
+        default:
+          break;
+      }
+      //If ignition timing is being tracked per tooth, perform the calcs to get the end teeth
+      //This only needs to be run if the advance figure has changed, otherwise the end teeth will still be the same
+      if( (configPage1.perToothIgn == true) && (lastAdvance != currentStatus.advance) ) { triggerSetEndTeeth(); }
+
+      //***********************************************************************************************
+      //| BEGIN FUEL SCHEDULES
+      //Finally calculate the time (uS) until we reach the firing angles and set the schedules
+      //We only need to set the shcedule if we're BEFORE the open angle
+      //This may potentially be called a number of times as we get closer and closer to the opening time
+
+      //Determine the current crank angle
+      int crankAngle = getCrankAngle(timePerDegree);
+      if (crankAngle > CRANK_ANGLE_MAX_INJ ) { crankAngle -= 360; }
+
+      if (fuelOn && currentStatus.PW1 > 0 && !BIT_CHECK(currentStatus.squirt, BIT_SQUIRT_BOOSTCUT))
+      {
+        if ( (injector1StartAngle <= crankAngle) && (fuelSchedule1.Status == RUNNING) ) { injector1StartAngle += CRANK_ANGLE_MAX_INJ; }
+        if (injector1StartAngle > crankAngle)
+        {
+          setFuelSchedule1(
+                    ((unsigned long)(injector1StartAngle - crankAngle) * (unsigned long)timePerDegree),
+                    (unsigned long)currentStatus.PW1
+                    );
+        }
+
+        /*-----------------------------------------------------------------------------------------
+        | A Note on tempCrankAngle and tempStartAngle:
+        |   The use of tempCrankAngle/tempStartAngle is described below. It is then used in the same way for channels 2, 3 and 4 on both injectors and ignition
+        |   Essentially, these 2 variables are used to realign the current crank angle and the desired start angle around 0 degrees for the given cylinder/output
+        |   Eg: If cylinder 2 TDC is 180 degrees after cylinder 1 (Eg a standard 4 cylidner engine), then tempCrankAngle is 180* less than the current crank angle and
+        |       tempStartAngle is the desired open time less 180*. Thus the cylinder is being treated relative to its own TDC, regardless of its offset
+        |
+        |   This is done to avoid problems with very short of very long times until tempStartAngle.
+        |   This will very likely need to be rewritten when sequential is enabled
+        |------------------------------------------------------------------------------------------
+        */
+        if(channel2InjEnabled)
+        {
+          tempCrankAngle = crankAngle - channel2InjDegrees;
+          if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX_INJ; }
+          tempStartAngle = injector2StartAngle - channel2InjDegrees;
+          if ( tempStartAngle < 0) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
+          if ( (tempStartAngle <= tempCrankAngle) && (fuelSchedule2.Status == RUNNING) ) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
+          if ( tempStartAngle > tempCrankAngle )
+          {
+            setFuelSchedule2(
+                      ((unsigned long)(tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree),
+                      (unsigned long)currentStatus.PW2
+                      );
+          }
+        }
+
+        if(channel3InjEnabled)
+        {
+          tempCrankAngle = crankAngle - channel3InjDegrees;
+          if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX_INJ; }
+          tempStartAngle = injector3StartAngle - channel3InjDegrees;
+          if ( tempStartAngle < 0) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
+          if ( (tempStartAngle <= tempCrankAngle) && (fuelSchedule3.Status == RUNNING) ) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
+          if ( tempStartAngle > tempCrankAngle )
+          {
+            setFuelSchedule3(
+                      ((unsigned long)(tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree),
+                      (unsigned long)currentStatus.PW3
+                      );
+          }
+        }
+
+        if(channel4InjEnabled)
+        {
+          tempCrankAngle = crankAngle - channel4InjDegrees;
+          if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX_INJ; }
+          tempStartAngle = injector4StartAngle - channel4InjDegrees;
+          if ( tempStartAngle < 0) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
+          if ( (tempStartAngle <= tempCrankAngle) && (fuelSchedule4.Status == RUNNING) ) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
+          if ( tempStartAngle > tempCrankAngle )
+          {
+            setFuelSchedule4(
+                      ((unsigned long)(tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree),
+                      (unsigned long)currentStatus.PW4
+                      );
+          }
+        }
+
+        if(channel5InjEnabled)
+        {
+          tempCrankAngle = crankAngle - channel5InjDegrees;
+          if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX_INJ; }
+          tempStartAngle = injector5StartAngle - channel5InjDegrees;
+          if ( tempStartAngle < 0) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
+          if (tempStartAngle <= tempCrankAngle && fuelSchedule5.schedulesSet == 0) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
+          if ( tempStartAngle > tempCrankAngle )
+          {
+            //Note the hacky use of fuel schedule 3 below
+            /*
+            setFuelSchedule3(openInjector3and5,
+                      ((unsigned long)(tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree),
+                      (unsigned long)currentStatus.PW1,
+                      closeInjector3and5
+                    );*/
+            setFuelSchedule3(
+                      ((unsigned long)(tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree),
+                      (unsigned long)currentStatus.PW1
+                      );
+          }
+        }
+      }
+      //***********************************************************************************************
+      //| BEGIN IGNITION SCHEDULES
+      //Likewise for the ignition
+
+      //fixedCrankingOverride is used to extend the dwell during cranking so that the decoder can trigger the spark upon seeing a certain tooth. Currently only available on the basic distributor and 4g63 decoders.
+      if ( configPage2.ignCranklock && BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK)) { fixedCrankingOverride = currentStatus.dwell * 2; }
+      else { fixedCrankingOverride = 0; }
+
+      //Perform an initial check to see if the ignition is turned on (Ignition only turns on after a preset number of cranking revolutions and:
+      //Check for hard cut rev limit (If we're above the hardcut limit, we simply don't set a spark schedule)
+      if(ignitionOn && !currentStatus.launchingHard && !BIT_CHECK(currentStatus.spark, BIT_SPARK_BOOSTCUT) && !BIT_CHECK(currentStatus.spark, BIT_SPARK_HRDLIM) && !currentStatus.flatShiftingHard)
+      {
+
+        //Refresh the current crank angle info
+        //ignition1StartAngle = 335;
+        crankAngle = getCrankAngle(timePerDegree); //Refresh with the latest crank angle
+        if (crankAngle > CRANK_ANGLE_MAX_IGN ) { crankAngle -= 360; }
+
+        //if (ignition1StartAngle <= crankAngle && ignition1.schedulesSet == 0) { ignition1StartAngle += CRANK_ANGLE_MAX_IGN; }
+        if (ignition1StartAngle > crankAngle)
+        {
+            /*
+            long some_time = ((unsigned long)(ignition1StartAngle - crankAngle) * (unsigned long)timePerDegree);
+            long newRPM = (long)(some_time * currentStatus.rpmDOT) / 1000000L;
+            newRPM = currentStatus.RPM + (newRPM/2);
+            unsigned long timePerDegree_1 = ldiv( 166666L, newRPM).quot;
+            unsigned long timeout = (unsigned long)(ignition1StartAngle - crankAngle) * 282UL;
+            */
+            setIgnitionSchedule1(ign1StartFunction,
+                      ((unsigned long)(ignition1StartAngle - crankAngle) * (unsigned long)timePerDegree), //(timeout/10),
+                      currentStatus.dwell + fixedCrankingOverride, //((unsigned long)((unsigned long)currentStatus.dwell* currentStatus.RPM) / newRPM) + fixedCrankingOverride,
+                      ign1EndFunction
+                      );
+        }
+
+        tempCrankAngle = crankAngle - channel2IgnDegrees;
+        if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX_IGN; }
+        tempStartAngle = ignition2StartAngle - channel2IgnDegrees;
+        if ( tempStartAngle < 0) { tempStartAngle += CRANK_ANGLE_MAX_IGN; }
+        //if ( (tempStartAngle > tempCrankAngle)  && ign2LastRev != startRevolutions)
+        //if ( ign2LastRev != startRevolutions )
+        {
+            unsigned long ignition2StartTime = 0;
+            if(tempStartAngle > tempCrankAngle) { ignition2StartTime = ((unsigned long)(tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree); }
+            //else if (tempStartAngle < tempCrankAngle) { ignition2StartTime = ((long)(360 - tempCrankAngle + tempStartAngle) * (long)timePerDegree); }
+            else { ignition2StartTime = 0; }
+
+            if(ignition2StartTime > 0) {
+            setIgnitionSchedule2(ign2StartFunction,
+                      ignition2StartTime,
+                      currentStatus.dwell + fixedCrankingOverride,
+                      ign2EndFunction
+                      );
+            }
+        }
+
+        tempCrankAngle = crankAngle - channel3IgnDegrees;
+        if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX_IGN; }
+        tempStartAngle = ignition3StartAngle - channel3IgnDegrees;
+        if ( tempStartAngle < 0) { tempStartAngle += CRANK_ANGLE_MAX_IGN; }
+        //if (tempStartAngle > tempCrankAngle)
+        {
+            long ignition3StartTime = 0;
+            if(tempStartAngle > tempCrankAngle) { ignition3StartTime = ((unsigned long)(tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree); }
+            //else if (tempStartAngle < tempCrankAngle) { ignition4StartTime = ((long)(360 - tempCrankAngle + tempStartAngle) * (long)timePerDegree); }
+            else { ignition3StartTime = 0; }
+
+            if(ignition3StartTime > 0) {
+            setIgnitionSchedule3(ign3StartFunction,
+                      ignition3StartTime,
+                      currentStatus.dwell + fixedCrankingOverride,
+                      ign3EndFunction
+                      );
+            }
+        }
+
+        tempCrankAngle = crankAngle - channel4IgnDegrees;
+        if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX_IGN; }
+        tempStartAngle = ignition4StartAngle - channel4IgnDegrees;
+        if ( tempStartAngle < 0) { tempStartAngle += CRANK_ANGLE_MAX_IGN; }
+        //if (tempStartAngle > tempCrankAngle)
+        {
+
+            long ignition4StartTime = 0;
+            if(tempStartAngle > tempCrankAngle) { ignition4StartTime = ((unsigned long)(tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree); }
+            //else if (tempStartAngle < tempCrankAngle) { ignition4StartTime = ((long)(360 - tempCrankAngle + tempStartAngle) * (long)timePerDegree); }
+            else { ignition4StartTime = 0; }
+
+            if(ignition4StartTime > 0) {
+            setIgnitionSchedule4(ign4StartFunction,
+                      ignition4StartTime,
+                      currentStatus.dwell + fixedCrankingOverride,
+                      ign4EndFunction
+                      );
+            }
+        }
+
+        tempCrankAngle = crankAngle - channel5IgnDegrees;
+        if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX_IGN; }
+        tempStartAngle = ignition5StartAngle - channel5IgnDegrees;
+        if ( tempStartAngle < 0) { tempStartAngle += CRANK_ANGLE_MAX_IGN; }
+        //if (tempStartAngle > tempCrankAngle)
+        {
+
+            long ignition5StartTime = 0;
+            if(tempStartAngle > tempCrankAngle) { ignition5StartTime = ((unsigned long)(tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree); }
+            //else if (tempStartAngle < tempCrankAngle) { ignition4StartTime = ((long)(360 - tempCrankAngle + tempStartAngle) * (long)timePerDegree); }
+            else { ignition5StartTime = 0; }
+
+            if(ignition5StartTime > 0) {
+            setIgnitionSchedule5(ign5StartFunction,
+                      ignition5StartTime,
+                      currentStatus.dwell + fixedCrankingOverride,
+                      ign5EndFunction
+                      );
+            }
+        }
+      } //Ignition schedules on
+    } //Has sync and RPM
 } //loop()

From 38a89e41d0bf777c4f5e17bb4b06b616f872e6cf Mon Sep 17 00:00:00 2001
From: VitorBoss <vitor_boss@yahoo.com.br>
Date: Sat, 26 Aug 2017 19:48:04 -0300
Subject: [PATCH 10/10] 082617

---
 .gitignore              |  4 ++++
 speeduino/globals.h     |  6 +++++-
 speeduino/speeduino.ino |  2 +-
 speeduino/utils.ino     | 40 ++++++++++++++++++++--------------------
 4 files changed, 30 insertions(+), 22 deletions(-)

diff --git a/.gitignore b/.gitignore
index 9f7304ea..e279d43c 100644
--- a/.gitignore
+++ b/.gitignore
@@ -10,7 +10,11 @@ table.py
 reference/hardware/v0.2/~$schematic v0.2_bom.xlsx
 
 reference/hardware/v0.4/gerbers/Archive.zip
+speeduino/.vscode
+speeduino/.build
+
 .pioenvs
 .piolibdeps
 .clang_complete
 .gcc-flags.json
+.project
diff --git a/speeduino/globals.h b/speeduino/globals.h
index f524c105..13269f5a 100644
--- a/speeduino/globals.h
+++ b/speeduino/globals.h
@@ -119,7 +119,11 @@
 #define OFFSET_FUELTRIM 127 //The fuel trim tables are offset by 128 to allow for -128 to +128 values
 #define OFFSET_IGNITION 40 //Ignition values from the main spark table are offset 40 degrees downards to allow for negative spark timing
 
-#define SERIAL_BUFFER_THRESHOLD 32 // When the serial buffer is filled to greater than this threshold value, the serial processing operations will be performed more urgently in order to avoid it overflowing. Serial buffer is 64 bytes long, so the threshold is set at half this as a reasonable figure
+#if defined(CORE_STM32)
+  #define SERIAL_BUFFER_THRESHOLD 64 // When the serial buffer is filled to greater than this threshold value, the serial processing operations will be performed more urgently in order to avoid it overflowing. Serial buffer is 128 bytes long, so the threshold is set at half this as a reasonable figure
+#else
+  #define SERIAL_BUFFER_THRESHOLD 32 // When the serial buffer is filled to greater than this threshold value, the serial processing operations will be performed more urgently in order to avoid it overflowing. Serial buffer is 64 bytes long, so the threshold is set at half this as a reasonable figure
+#endif
 
 #define FUEL_PUMP_ON() *pump_pin_port |= (pump_pin_mask)
 #define FUEL_PUMP_OFF() *pump_pin_port &= ~(pump_pin_mask)
diff --git a/speeduino/speeduino.ino b/speeduino/speeduino.ino
index 159ff6c7..030bd9f0 100644
--- a/speeduino/speeduino.ino
+++ b/speeduino/speeduino.ino
@@ -148,7 +148,6 @@ void setup()
   table3D_setSize(&trim2Table, 6);
   table3D_setSize(&trim3Table, 6);
   table3D_setSize(&trim4Table, 6);
-  Serial.begin(115200);
 
   #if defined(CORE_STM32)
     EEPROM.init();
@@ -156,6 +155,7 @@ void setup()
   loadConfig();
   doUpdates(); //Check if any data items need updating (Occurs ith firmware updates)
 
+  Serial.begin(115200);
 #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) //ATmega2561 does not have Serial3
   if (configPage10.enable_canbus == 1) { CANSerial.begin(115200); }
 #elif defined(CORE_STM32)
diff --git a/speeduino/utils.ino b/speeduino/utils.ino
index 994c7f39..45092c16 100644
--- a/speeduino/utils.ino
+++ b/speeduino/utils.ino
@@ -494,25 +494,25 @@ void setPinMapping(byte boardID)
 
   //And for inputs
   #if defined(CORE_STM32)
-  #ifndef ARDUINO_ARCH_STM32 //libmaple core aka STM32DUINO
-    pinMode(pinMAP, INPUT_ANALOG);
-    pinMode(pinO2, INPUT_ANALOG);
-    pinMode(pinO2_2, INPUT_ANALOG);
-    pinMode(pinTPS, INPUT_ANALOG);
-    pinMode(pinIAT, INPUT_ANALOG);
-    pinMode(pinCLT, INPUT_ANALOG);
-    pinMode(pinBat, INPUT_ANALOG);
-    pinMode(pinBaro, INPUT_ANALOG);
-  #else
-    pinMode(pinMAP, INPUT);
-    pinMode(pinO2, INPUT);
-    pinMode(pinO2_2, INPUT);
-    pinMode(pinTPS, INPUT);
-    pinMode(pinIAT, INPUT);
-    pinMode(pinCLT, INPUT);
-    pinMode(pinBat, INPUT);
-    pinMode(pinBaro, INPUT);
-  #endif
+    #ifndef ARDUINO_ARCH_STM32 //libmaple core aka STM32DUINO
+      pinMode(pinMAP, INPUT_ANALOG);
+      pinMode(pinO2, INPUT_ANALOG);
+      pinMode(pinO2_2, INPUT_ANALOG);
+      pinMode(pinTPS, INPUT_ANALOG);
+      pinMode(pinIAT, INPUT_ANALOG);
+      pinMode(pinCLT, INPUT_ANALOG);
+      pinMode(pinBat, INPUT_ANALOG);
+      pinMode(pinBaro, INPUT_ANALOG);
+    #else
+      pinMode(pinMAP, INPUT);
+      pinMode(pinO2, INPUT);
+      pinMode(pinO2_2, INPUT);
+      pinMode(pinTPS, INPUT);
+      pinMode(pinIAT, INPUT);
+      pinMode(pinCLT, INPUT);
+      pinMode(pinBat, INPUT);
+      pinMode(pinBaro, INPUT);
+    #endif
   #endif
   pinMode(pinTrigger, INPUT);
   pinMode(pinTrigger2, INPUT);
@@ -602,4 +602,4 @@ unsigned int PW_SD(int REQ_FUEL, byte VE, byte MAP, int corrections, int injOpen
 unsigned int PW_AN(int REQ_FUEL, byte VE, byte TPS, int corrections, int injOpen)
 {
   return PW(REQ_FUEL, VE, currentStatus.MAP, corrections, injOpen);
-}
+}