Merge remote-tracking branch 'refs/remotes/noisymime/master'

This commit is contained in:
VitorBoss 2016-10-27 12:05:47 -02:00
commit 19c19d66a0
8 changed files with 146 additions and 71 deletions

View File

@ -10,15 +10,22 @@ Fan control
*/
void initialiseFan()
{
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
if(configPage4.fanInv) { 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 (currentStatus.coolant >= (configPage4.fanSP - CALIBRATION_TEMPERATURE_OFFSET)) { digitalWrite(pinFan,fanHIGH); }
else if (currentStatus.coolant <= (configPage4.fanSP - configPage4.fanHyster)) { digitalWrite(pinFan, fanLOW); }
if(configPage4.fanEnable)
{
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 defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)

View File

@ -236,7 +236,7 @@ void sendValues(int packetlength, byte portNum)
response[17] = currentStatus.corrections; //Total GammaE (%)
response[18] = currentStatus.VE; //Current VE 1 (%)
response[19] = currentStatus.afrTarget;
response[20] = (byte)(currentStatus.PW / 100); //Pulsewidth 1 multiplied by 10 in ms. Have to convert from uS to mS.
response[20] = (byte)(currentStatus.PW1 / 100); //Pulsewidth 1 multiplied by 10 in ms. Have to convert from uS to mS.
response[21] = currentStatus.tpsDOT; //TPS DOT
response[22] = currentStatus.advance;
response[23] = currentStatus.TPS; // TPS (0% to 100%)
@ -286,13 +286,13 @@ void receiveValue(int valueOffset, byte newValue)
if (valueOffset < 272)
{
//X Axis
fuelTable.axisX[(valueOffset - 256)] = ((int)(newValue) * 100); //The RPM values sent by megasquirt are divided by 100, need to multiple it back by 100 to make it correct
fuelTable.axisX[(valueOffset - 256)] = ((int)(newValue) * TABLE_RPM_MULTIPLIER); //The RPM values sent by megasquirt are divided by 100, need to multiple it back by 100 to make it correct (TABLE_RPM_MULTIPLIER)
}
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);
fuelTable.axisY[valueOffset] = (int)(newValue) * TABLE_LOAD_MULTIPLIER;
}
return;
}
@ -319,13 +319,13 @@ void receiveValue(int valueOffset, byte newValue)
if (valueOffset < 272)
{
//X Axis
ignitionTable.axisX[(valueOffset - 256)] = (int)(newValue) * int(100); //The RPM values sent by megasquirt are divided by 100, need to multiple it back by 100 to make it correct
ignitionTable.axisX[(valueOffset - 256)] = (int)(newValue) * TABLE_RPM_MULTIPLIER; //The RPM values sent by megasquirt are divided by 100, need to multiple it back by 100 to make it correct
}
else
{
//Y Axis
valueOffset = 15 - (valueOffset - 272); //Need to do a translation to flip the order
ignitionTable.axisY[valueOffset] = (int)(newValue);
ignitionTable.axisY[valueOffset] = (int)(newValue) * TABLE_LOAD_MULTIPLIER;
}
return;
}
@ -335,7 +335,7 @@ void receiveValue(int valueOffset, byte newValue)
//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; //Need to subtract 80 because the map and bins (Which make up 80 bytes) aren't part of the config pages
*((byte *)pnt_configPage + (byte)valueOffset) = newValue;
}
break;
@ -351,13 +351,13 @@ void receiveValue(int valueOffset, byte newValue)
if (valueOffset < 272)
{
//X Axis
afrTable.axisX[(valueOffset - 256)] = int(newValue) * int(100); //The RPM values sent by megasquirt are divided by 100, need to multiply it back by 100 to make it correct
afrTable.axisX[(valueOffset - 256)] = int(newValue) * TABLE_RPM_MULTIPLIER; //The RPM values sent by megasquirt are divided by 100, need to multiply it back by 100 to make it correct (TABLE_RPM_MULTIPLIER)
}
else
{
//Y Axis
valueOffset = 15 - (valueOffset - 272); //Need to do a translation to flip the order
afrTable.axisY[valueOffset] = int(newValue);
afrTable.axisY[valueOffset] = int(newValue) * TABLE_LOAD_MULTIPLIER;
}
return;
@ -368,7 +368,7 @@ void receiveValue(int valueOffset, byte newValue)
//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; //Need to subtract 80 because the map and bins (Which make up 80 bytes) aren't part of the config pages
*((byte *)pnt_configPage + (byte)valueOffset) = newValue;
}
break;
@ -388,12 +388,12 @@ void receiveValue(int valueOffset, byte newValue)
}
else if (valueOffset < 72) //New value is on the X (RPM) axis of the boost table
{
boostTable.axisX[(valueOffset - 64)] = int(newValue) * int(100); //The RPM values sent by TunerStudio are divided by 100, need to multiply it back by 100 to make it correct
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);
boostTable.axisY[(7 - (valueOffset - 72))] = int(newValue) * TABLE_LOAD_MULTIPLIER;
return;
}
else if (valueOffset < 144) //New value is part of the vvt map
@ -405,15 +405,33 @@ void receiveValue(int valueOffset, byte 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) * int(100); //The RPM values sent by TunerStudio are divided by 100, need to multiply it back by 100 to make it correct
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;
}
else //New value is on the Y (Load) axis of the vvt table
{
valueOffset = valueOffset - 152;
vvtTable.axisY[(7 - valueOffset)] = int(newValue);
vvtTable.axisY[(7 - valueOffset)] = int(newValue) * TABLE_LOAD_MULTIPLIER;
return;
}
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;
default:
break;
}
@ -661,12 +679,37 @@ void sendPage(bool useChar)
}
case seqFuelPage:
{
byte response[200]; //The size is: (6x6 + 6 + 6) * 4 + 8 (Leftover values)
if(useChar)
{
currentTable = trim1Table;
currentTitleIndex = 121;
//Do.... Something?
}
else
{
//Need to perform a translation of the values[MAP/TPS][RPM] into the MS expected format
byte response[192]; //Bit hacky, but the size is: (6x6 + 6 + 6) * 4 = 192
for (int x = 0; x < 200; x++) { 0; }
//trim1 table
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)] / 100); }
for (int y = 42; y < 48; y++) { response[y] = byte(trim1Table.axisY[5 - (y - 42)]); }
//trim2 table
for (int x = 48; x < 84; x++) { response[x] = trim2Table.values[5 - x / 6][x % 6]; }
for (int x = 84; x < 90; x++) { response[x] = byte(trim2Table.axisX[(x - 84)] / 100); }
for (int y = 90; y < 96; y++) { response[y] = byte(trim2Table.axisY[5 - (y - 90)]); }
//trim3 table
for (int x = 96; x < 132; x++) { response[x] = trim3Table.values[5 - x / 6][x % 6]; }
for (int x = 132; x < 138; x++) { response[x] = byte(trim3Table.axisX[(x - 132)] / 100); }
for (int y = 138; y < 144; y++) { response[y] = byte(trim3Table.axisY[5 - (y - 138)]); }
//trim4 table
for (int x = 144; x < 180; x++) { response[x] = trim4Table.values[5 - x / 6][x % 6]; }
for (int x = 180; x < 186; x++) { response[x] = byte(trim4Table.axisX[(x - 180)] / 100); }
for (int y = 186; y < 192; y++) { response[y] = byte(trim4Table.axisY[5 - (y - 186)]); }
Serial.write((byte *)&response, sizeof(response));
break;
return;
}
break;
}
default:
{

View File

@ -465,6 +465,22 @@ void triggerSetup_4G63()
toothAngles[1] = 105; //Rising edge of tooth #2
toothAngles[2] = 175; //Falling edge of tooth #2
toothAngles[3] = 285; //Rising edge of tooth #1
/*
* https://forums.libreems.org/attachment.php?aid=34
toothAngles[0] = 715; //Falling edge of tooth #1
toothAngles[1] = 49; //Falling edge of wide cam
toothAngles[2] = 105; //Rising edge of tooth #2
toothAngles[3] = 175; //Falling edge of tooth #2
toothAngles[4] = 229; //Rising edge of narrow cam tooth (??)
toothAngles[5] = 285; //Rising edge of tooth #3
toothAngles[6] = 319; //Falling edge of narrow cam tooth
toothAngles[7] = 355; //falling edge of tooth #3
toothAngles[8] = 465; //Rising edge of tooth #4
toothAngles[9] = 535; //Falling edge of tooth #4
toothAngles[10] = 535; //Rising edge of wide cam tooth
toothAngles[11] = 645; //Rising edge of tooth #1
*/
triggerFilterTime = 1500; //10000 rpm, assuming we're triggering on both edges off the crank tooth.
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)
@ -474,7 +490,7 @@ void triggerPri_4G63()
{
curTime = micros();
curGap = curTime - toothLastToothTime;
if ( curGap < triggerFilterTime ) { return; } //Debounce check. Pulses should never be less than triggerFilterTime
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.
@ -504,9 +520,6 @@ void triggerPri_4G63()
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;
}
void triggerSec_4G63()
{
@ -523,8 +536,12 @@ void triggerSec_4G63()
bool crank = digitalRead(pinTrigger);
if(crank == HIGH)
{
//triggerFilterTime = 1; //Effectively turns off the trigger filter for now
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;
*/
}
}
//else { triggerFilterTime = 1500; } //reset filter time (ugly)
@ -543,6 +560,10 @@ int getRPM_4G63()
int tempToothAngle;
noInterrupts();
tempToothAngle = triggerToothAngle;
/* High-res mode
if(toothCurrentCount == 1) { tempToothAngle = 70; }
else { tempToothAngle = toothAngles[toothCurrentCount-1] - toothAngles[toothCurrentCount-2]; }
*/
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;

View File

@ -56,7 +56,7 @@ void updateDisplay()
case 1:
display.print("PW: ");
display.setCursor(28,0);
display.print(currentStatus.PW);
display.print(currentStatus.PW1);
break;
case 2:
display.print("Adv: ");
@ -101,7 +101,7 @@ void updateDisplay()
case 1:
display.print("PW: ");
display.setCursor(28,11);
display.print(currentStatus.PW);
display.print(currentStatus.PW1);
break;
case 2:
display.print("Adv: ");

View File

@ -139,12 +139,16 @@ struct statuses {
byte launchCorrection; //The amount of correction being applied if launch control is active
byte afrTarget;
byte idleDuty;
bool fanOn; //Whether or not the fan is turned on
byte flex; //Ethanol reading (if enabled). 0 = No ethanol, 100 = pure ethanol. Eg E85 = 85.
unsigned long TAEEndTime; //The target end time used whenever TAE is turned on
volatile byte squirt;
volatile byte spark;
byte engine;
unsigned int PW; //In uS
unsigned int PW1; //In uS
unsigned int PW2; //In uS
unsigned int PW3; //In uS
unsigned int PW4; //In uS
volatile byte runSecs; //Counter of seconds since cranking commenced (overflows at 255 obviously)
volatile byte secl; //Continous
volatile unsigned int loopsPerSecond;
@ -355,8 +359,9 @@ struct config3 {
byte boostKI;
byte boostKD;
byte lnchPullRes :2;
byte unused60 : 6;
byte lnchPullRes : 2;
bool fuelTrimEnabled : 1;
byte unused60 : 5;
byte unused61;
byte unused62;
byte unused63;

View File

@ -94,7 +94,7 @@
endianness = little
nPages = 9
burnCommand = "B"
pageSize = 288, 64, 288, 64, 288, 64, 64, 160, 200
pageSize = 288, 64, 288, 64, 288, 64, 64, 160, 192
pageActivationDelay = 10
pageActivate = "P\001", "P\002", "P\003", "P\004", "P\005", "P\006", "P\007", "P\010", "P\011"
pageReadCommand = "V", "V", "V", "V", "V", "V", "V", "V", "V"
@ -381,8 +381,9 @@ page = 6
boostKI = scalar, U08, 58, "%", 1.0, 0.0, 0.0, 200.0, 0 ; * ( 1 byte)
boostKD = scalar, U08, 59, "%", 1.0, 0.0, 0.0, 200.0, 0 ; * ( 1 byte)
lnchPullRes = bits, U08, 60 [0:1], "Float" , "Pullup", "INVALID", "INVALID"
unused6-60 = bits, U08, 60, [2:7], "ONE", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID", "INVALID"
lnchPullRes = bits, U08, 60 [0:1], "Float" , "Pullup", "INVALID", "INVALID"
fuelTrimEnabled= bits, U08, 60, [2:2], "No", "Yes"
unused6-60 = bits, U08, 60, [3:7], "ONE", "INVALID"
unused6-61 = scalar, U08, 61, "RPM", 100.0, 0.0, 100, 25500, 0
unused6-62 = scalar, U08, 62, "RPM", 100.0, 0.0, 100, 25500, 0
unused6-63 = scalar, U08, 63, "RPM", 100.0, 0.0, 100, 25500, 0
@ -423,7 +424,7 @@ page = 7
; Begin fan control vairables
fanInv = bits, U08, 56, [0:0], "No", "Yes"
fanEnable = bits, U08, 56, [1:2], "Off", "On/Off", "PWM", "INVALID"
fanEnable = bits, U08, 56, [1:2], "Off", "On/Off", "INVALID", "INVALID"
unused7-55b = bits, U08, 56, [3:3], "No", "Yes"
unused7-55c = bits, U08, 56, [4:4], "No", "Yes"
unused7-55d = bits, U08, 56, [5:5], "No", "Yes"
@ -495,23 +496,6 @@ page = 9
#elif ALPHA_N
fuelTrim4loadBins = array, U08, 186,[ 6], "TPS", 1.0, 0.0, 0.0, 255.0, 0
#endif
fuelTrimEnabled = bits, U08, 192, [0:0], "No", "Yes"
unused9-192b = bits, U08, 192, [1:1], "No", "Yes"
unused9-192c = bits, U08, 192, [2:2], "No", "Yes"
unused9-192d = bits, U08, 192, [3:3], "No", "Yes"
unused9-192e = bits, U08, 192, [4:4], "No", "Yes"
unused9-192f = bits, U08, 192, [5:5], "No", "Yes"
unused9-192g = bits, U08, 192, [6:6], "No", "Yes"
unused9-192h = bits, U08, 192, [7:7], "No", "Yes"
unused9-193 = scalar, U08, 193, "RPM", 100.0, 0.0, 100, 25500, 0
unused9-194 = scalar, U08, 194, "RPM", 100.0, 0.0, 100, 25500, 0
unused9-195 = scalar, U08, 195, "RPM", 100.0, 0.0, 100, 25500, 0
unused9-196 = scalar, U08, 196, "RPM", 100.0, 0.0, 100, 25500, 0
unused9-197 = scalar, U08, 197, "RPM", 100.0, 0.0, 100, 25500, 0
unused9-198 = scalar, U08, 198, "RPM", 100.0, 0.0, 100, 25500, 0
unused9-199 = scalar, U08, 199, "RPM", 100.0, 0.0, 100, 25500, 0
;-------------------------------------------------------------------------------
@ -695,6 +679,9 @@ menuDialog = main
sparkMode = "Wasted Spark: Ignition outputs are on the channels <= half the number of cylinders. Eg 4 cylinder outputs on IGN1 and IGN2.\nSingle Channel: All ignition pulses are output on IGN1.\nWasted COP: Ignition pulses are output on all ignition channels up to the number of cylinders. Eg 4 cylinder outputs on all ignition channels. No valid for >4 cylinders"
IgInv = "Whether the spark fires when the ignition sign goes high or goes low. Most ignition systems 'Going Low' but please verify this as damage to coils can result from the incorrect selection"
fanInv = ""
fanHyster = "The number of degrees of hysteresis to be used in controlling the fan. Recommended values are between 2 and 5"
taeTime = "The duration of the acceleration enrichment"
iacChannels = "The number of output channels used for PWM valves. Select 1 for 2-wire valves or 2 for 3-wire valves."
@ -846,7 +833,7 @@ menuDialog = main
dialog = fanSettings,"Fan Settings",7
field = "Fan Mode", fanEnable
field = "Fan Output Inverted (see F1)", fanInv
field = "Fan Output Inverted", fanInv
field = "Fan temperature SP", fanSP
field = "Fan hysteresis", fanHyster

View File

@ -837,7 +837,7 @@ void loop()
{
//We reach here if the time between teeth is too great. This VERY likely means the engine has stopped
currentStatus.RPM = 0;
currentStatus.PW = 0;
currentStatus.PW1 = 0;
currentStatus.VE = 0;
toothLastToothTime = 0;
currentStatus.hasSync = false;
@ -955,14 +955,14 @@ void loop()
{
//Speed Density
currentStatus.VE = get3DTableValue(&fuelTable, currentStatus.MAP, currentStatus.RPM); //Perform lookup into fuel map for RPM vs MAP value
currentStatus.PW = PW_SD(req_fuel_uS, currentStatus.VE, currentStatus.MAP, currentStatus.corrections, inj_opentime_uS);
currentStatus.PW1 = PW_SD(req_fuel_uS, currentStatus.VE, currentStatus.MAP, currentStatus.corrections, inj_opentime_uS);
currentStatus.advance = get3DTableValue(&ignitionTable, currentStatus.MAP, currentStatus.RPM); //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.PW = PW_AN(req_fuel_uS, currentStatus.VE, currentStatus.TPS, currentStatus.corrections, inj_opentime_uS); //Calculate pulsewidth using the Alpha-N algorithm (in uS)
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); //As above, but for ignition advance
}
@ -1035,14 +1035,15 @@ void loop()
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 (currentStatus.PW > pwLimit) { currentStatus.PW = pwLimit; }
if (currentStatus.PW1 > pwLimit) { currentStatus.PW1 = pwLimit; }
}
//***********************************************************************************************
//BEGIN INJECTION TIMING
//Determine next firing angles
int PWdivTimerPerDegree = div(currentStatus.PW, timePerDegree).quot; //How many crank degrees the calculated PW will take at the current speed
currentStatus.PW2, currentStatus.PW3, currentStatus.PW4 = currentStatus.PW1; // Initial state is for all pulsewidths to be the same (This gets changed below)
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
@ -1072,10 +1073,18 @@ void loop()
injector4StartAngle = (configPage1.inj4Ang + channel4InjDegrees - ( PWdivTimerPerDegree ));
if(injector4StartAngle > CRANK_ANGLE_MAX_INJ) {injector4StartAngle -= CRANK_ANGLE_MAX_INJ;}
injector1StartAngle += 360;
injector2StartAngle += 360;
injector3StartAngle += 360;
injector4StartAngle += 360;
if(configPage3.fuelTrimEnabled)
{
unsigned long pw1percent = 100 + get3DTableValue(&trim1Table, currentStatus.MAP, currentStatus.RPM);
unsigned long pw2percent = 100 + get3DTableValue(&trim2Table, currentStatus.MAP, currentStatus.RPM);
unsigned long pw3percent = 100 + get3DTableValue(&trim3Table, currentStatus.MAP, currentStatus.RPM);
unsigned long pw4percent = 100 + get3DTableValue(&trim4Table, currentStatus.MAP, currentStatus.RPM);
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
@ -1193,7 +1202,7 @@ void loop()
int crankAngle = getCrankAngle(timePerDegree);
if (crankAngle > CRANK_ANGLE_MAX_INJ ) { crankAngle -= 360; }
if (fuelOn && currentStatus.PW > 0 && !BIT_CHECK(currentStatus.squirt, BIT_SQUIRT_BOOSTCUT))
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)
@ -1202,7 +1211,7 @@ void loop()
{
setFuelSchedule1(openInjector1and4,
((unsigned long)(injector1StartAngle - crankAngle) * (unsigned long)timePerDegree),
(unsigned long)currentStatus.PW,
(unsigned long)currentStatus.PW1,
closeInjector1and4
);
}
@ -1210,7 +1219,7 @@ void loop()
{
setFuelSchedule1(openInjector1,
((unsigned long)(injector1StartAngle - crankAngle) * (unsigned long)timePerDegree),
(unsigned long)currentStatus.PW,
(unsigned long)currentStatus.PW1,
closeInjector1
);
}
@ -1240,7 +1249,7 @@ void loop()
{
setFuelSchedule2(openInjector2and3,
((unsigned long)(tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree),
(unsigned long)currentStatus.PW,
(unsigned long)currentStatus.PW2,
closeInjector2and3
);
}
@ -1248,7 +1257,7 @@ void loop()
{
setFuelSchedule2(openInjector2,
((unsigned long)(tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree),
(unsigned long)currentStatus.PW,
(unsigned long)currentStatus.PW2,
closeInjector2
);
}
@ -1266,7 +1275,7 @@ void loop()
{
setFuelSchedule3(openInjector3,
((unsigned long)(tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree),
(unsigned long)currentStatus.PW,
(unsigned long)currentStatus.PW3,
closeInjector3
);
}
@ -1283,7 +1292,7 @@ void loop()
{
setFuelSchedule4(openInjector4,
((unsigned long)(tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree),
(unsigned long)currentStatus.PW,
(unsigned long)currentStatus.PW4,
closeInjector4
);
}
@ -1300,7 +1309,7 @@ void loop()
{
setFuelSchedule5(openInjector5,
((unsigned long)(tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree),
(unsigned long)currentStatus.PW,
(unsigned long)currentStatus.PW1,
closeInjector5
);
}

View File

@ -5,6 +5,9 @@ This file is used for everything related to maps/tables including their definiti
#define TABLE_H
#include <Arduino.h>
#define TABLE_RPM_MULTIPLIER 100
#define TABLE_LOAD_MULTIPLIER 1
/*
The 2D table can contain either 8-bit (byte) or 16-bit (int) values
The valueSize variable should be set to either 8 or 16 to indicate this BEFORE the table is used