TCU_Code

/*@!Encoding:1252*/
includes
{

}

variables
{
  int crctable[256] = {0x00, 0x2F, 0x5E, 0x71, 0xBC, 0x93, 0xE2, 0xCD, 0x57, 0x78, 0x09, 0x26, 0xEB, 0xC4, 0xB5, 0x9A,
                       0xAE, 0x81, 0xF0, 0xDF, 0x12, 0x3D, 0x4C, 0x63, 0xF9, 0xD6, 0xA7, 0x88, 0x45, 0x6A, 0x1B, 0x34,
                       0x73, 0x5C, 0x2D, 0x02, 0xCF, 0xE0, 0x91, 0xBE, 0x24, 0x0B, 0x7A, 0x55, 0x98, 0xB7, 0xC6, 0xE9,
                       0xDD, 0xF2, 0x83, 0xAC, 0x61, 0x4E, 0x3F, 0x10, 0x8A, 0xA5, 0xD4, 0xFB, 0x36, 0x19, 0x68, 0x47,
                       0xE6, 0xC9, 0xB8, 0x97, 0x5A, 0x75, 0x04, 0x2B, 0xB1, 0x9E, 0xEF, 0xC0, 0x0D, 0x22, 0x53, 0x7C,
                       0x48, 0x67, 0x16, 0x39, 0xF4, 0xDB, 0xAA, 0x85, 0x1F, 0x30, 0x41, 0x6E, 0xA3, 0x8C, 0xFD, 0xD2,
                       0x95, 0xBA, 0xCB, 0xE4, 0x29, 0x06, 0x77, 0x58, 0xC2, 0xED, 0x9C, 0xB3, 0x7E, 0x51, 0x20, 0x0F,
                       0x3B, 0x14, 0x65, 0x4A, 0x87, 0xA8, 0xD9, 0xF6, 0x6C, 0x43, 0x32, 0x1D, 0xD0, 0xFF, 0x8E, 0xA1,
                       0xE3, 0xCC, 0xBD, 0x92, 0x5F, 0x70, 0x01, 0x2E, 0xB4, 0x9B, 0xEA, 0xC5, 0x08, 0x27, 0x56, 0x79,
                       0x4D, 0x62, 0x13, 0x3C, 0xF1, 0xDE, 0xAF, 0x80, 0x1A, 0x35, 0x44, 0x6B, 0xA6, 0x89, 0xF8, 0xD7,
                       0x90, 0xBF, 0xCE, 0xE1, 0x2C, 0x03, 0x72, 0x5D, 0xC7, 0xE8, 0x99, 0xB6, 0x7B, 0x54, 0x25, 0x0A,
                       0x3E, 0x11, 0x60, 0x4F, 0x82, 0xAD, 0xDC, 0xF3, 0x69, 0x46, 0x37, 0x18, 0xD5, 0xFA, 0x8B, 0xA4,
                       0x05, 0x2A, 0x5B, 0x74, 0xB9, 0x96, 0xE7, 0xC8, 0x52, 0x7D, 0x0C, 0x23, 0xEE, 0xC1, 0xB0, 0x9F,
                       0xAB, 0x84, 0xF5, 0xDA, 0x17, 0x38, 0x49, 0x66, 0xFC, 0xD3, 0xA2, 0x8D, 0x40, 0x6F, 0x1E, 0x31,
                       0x76, 0x59, 0x28, 0x07, 0xCA, 0xE5, 0x94, 0xBB, 0x21, 0x0E, 0x7F, 0x50, 0x9D, 0xB2, 0xC3, 0xEC,
                       0xD8, 0xF7, 0x86, 0xA9, 0x64, 0x4B, 0x3A, 0x15, 0x8F, 0xA0, 0xD1, 0xFE, 0x33, 0x1C, 0x6D, 0x42}; // Look-up table used for E2E
  int dataid[16] = {0x3B, 0x14, 0x65, 0x4A, 0x87, 0xA8, 0xD9, 0xF6, 0x6C, 0x43, 0x32, 0x1D, 0xD0, 0xFF, 0x8E, 0xA1}; // DataID List used for E2E
  int e2e_123, e2e_124; // Used for E2E check of the Rx Messages
  byte data_123[8], data_124[8]; // Used for E2E check of the Rx Messages
  mstimer e2e_ET1, e2e_ET1_old, e2e_ET2, e2e_ET2_old;

  int condm, state;
  float gearr[6] = {3.55, 3.73, 2.05, 1.32, 0.97, 0.65}; // Gearbox drive ratios - [R, 1st, 2nd, 3rd, 4th, 5th].
}

on start // Timers used for E2E function calling
{
  setTimer(e2e_ET1, 150);
  setTimer(e2e_ET2, 150);
}

on timer e2e_ET1{ // Calling the E2E function for ET1 message
  setTimer(e2e_ET1, 100);
  if(isTimerActive(e2e_ET1_old) == 0){
    sysSetVariableString(sysvar::sysNS::E2E_ET1_Status, "Timeout Error");
  }
}

on timer e2e_ET2{ // Calling the E2E function for ET2 message
  setTimer(e2e_ET2, 100);
  if(isTimerActive(e2e_ET2_old) == 0){
    sysSetVariableString(sysvar::sysNS::E2E_ET2_Status, "Timeout Error");
  }
}

on timer e2e_ET1_old{

}

on timer e2e_ET2_old{

}

/// <Rx>
on message Engine_Transmission_1
{
  int i; // where i is used for e2e checking and cond, condd are used for autoshifting in the autoshift function.
  for(i = 0; i < this.dlc; i++)
  {
    data_123[i] = this.byte(i);
  }
  setTimer(e2e_ET1_old, 100);
  e2e_123 = E2ECheck(this.id, this.dlc, data_123);
  if(e2e_123){
    if($sig_engine_PRNDM_Req == 0x0 || $sig_engine_PRNDM_Req == 0x2){ // check if vehicle in park or neutral to allow for manual shifting position
      condm = 1;
      state = 1; // we use to this to know if we need to engage 1st gear
    }
    if($sig_engine_PRNDM_Req == 0x1){
      $Transmission_Engine_1::sig_transmission_RPM = $sig_engine_RPM / gearr[0]; // calculate the rpm for reverse based on throttle input
      if($Transmission_Engine_1::sig_transmission_RPM > 0 ){
        condm = 0; // doesn't allow to go into manual shifting if reverse is engaged
        state = 1;
      }
    }
    if($sig_engine_PRNDM_Req == 0x3){
      AutoShift();
      condm = 1;
      state = 2;
    }
    if($sig_engine_PRNDM_Req == 0x4 && condm == 1){
      ManualShift();
    }
  }
}


/// <Rx>
on message Engine_Transmission_2
{
  int i;
  for(i = 0; i < this.dlc; i++)
  {
    data_124[i] = this.byte(i);
  }
  setTimer(e2e_ET2_old, 100);
  e2e_124 = E2ECheck(this.id, this.dlc, data_124);
  if(e2e_124){
    if($sig_engine_PRNDM_Req == 0x0){ // If in park engage 1st gear to prevent rolling.
      if($sig_transmission_RPM == 0){
        $Transmission_Engine_1::sig_transmission_Gear1 = 1;
        $Transmission_Engine_2::sig_transmission_Gear2 = 1;
        $Transmission_Engine_2::sig_transmission_PRNDM_Conf = $sig_engine_PRNDM_Req;
      }else{
        $sig_transmission_PRNDM_Conf = 5;
      }
    }
    if($sig_engine_PRNDM_Req == 0x1){ // If in reverse engage reverse gear
      if($sig_transmission_RPM == 0){ // Fail safe in case we switch to reverse while in Drive
        $Transmission_Engine_1::sig_transmission_Gear1 = 6;
        $Transmission_Engine_2::sig_transmission_Gear2 = 6;
        $Transmission_Engine_2::sig_transmission_PRNDM_Conf = $sig_engine_PRNDM_Req;
      }else{
        $Transmission_Engine_2::sig_transmission_PRNDM_Conf = 5;
      }
    }
    if($sig_engine_PRNDM_Req == 0x2){ // If in neutral the gearbox is in neutral
      $Transmission_Engine_1::sig_transmission_Gear1 = 0;
      $Transmission_Engine_2::sig_transmission_Gear2 = 0;
      $Transmission_Engine_2::sig_transmission_PRNDM_Conf = $sig_engine_PRNDM_Req;
    }
    if($sig_engine_PRNDM_Req == 0x3){ // If in drive the gearbox is in drive
      $Transmission_Engine_2::sig_transmission_PRNDM_Conf = $sig_engine_PRNDM_Req;
    }
    if($sig_engine_PRNDM_Req == 0x4){ // If in manual shift the gearbox is in manual shift
      $Transmission_Engine_2::sig_transmission_PRNDM_Conf = $sig_engine_PRNDM_Req;
    }
  }
}


void ManualShift()
{
  int condd[6], gear, acc;
  $sig_engine_Man_Shift = 0x4;
  if(state == 1){ // This is used to check if we are in park or neutral when switching to manual shifting
    $Transmission_Engine_1::sig_transmission_Gear1 = 1; // Sets the vehicle in first gear
    $Transmission_Engine_2::sig_transmission_Gear2 = 1;
    state = 2; // Resets the state
  }else{
    // Check if there is an upshift request and that we can still upshift ( current gear is < 5)
    if($sig_engine_Man_Shift == 0x1 && $Transmission_Engine_1::sig_transmission_Gear1 < 5){
      gear = $Transmission_Engine_1::sig_transmission_Gear1 + 1; // Do the upshift
      acc = ($sig_transmission_RPM * gearr[gear]) / 6; // Calculate the accelerator position for the upshift
      if(acc > 50){ // Checks if the upshift is possible else we stick to the same gear.
        @sysvar::sysNS::sys_engine_Acceleratie = ($sig_transmission_RPM * gearr[gear]) / 6; // Set the RPM for upshift
        $sig_engine_Man_Shift == 0x4; // Reset the upshift flag to idle
        $Transmission_Engine_1::sig_transmission_Gear1 = gear; // Confirm the upshift
        $Transmission_Engine_2::sig_transmission_Gear2 = gear;
        $sig_transmission_Conf_Up_Down = 1;
      }else { // Else we stick to the same gear and reset the upshift request flag
        gear = $Transmission_Engine_1::sig_transmission_Gear1 - 1;
        $sig_engine_Man_Shift == 0x4;
        $sig_transmission_Conf_Up_Down = 0;
      }
    }else if($sig_engine_Man_Shift == 0x0 && $Transmission_Engine_1::sig_transmission_Gear1 > 1) {
      gear = $Transmission_Engine_1::sig_transmission_Gear1 - 1; // Do the downshift
      acc = ($sig_transmission_RPM * gearr[gear]) / 6; // Calculate the accelerator position for the downshift
      if(acc < 1000){ // Checks if the downshift is possible else we stick to the same gear.
        @sysvar::sysNS::sys_engine_Acceleratie = ($sig_transmission_RPM * gearr[gear]) / 6; // Set the RPM for downshift
        $sig_engine_Man_Shift == 0x4; // Reset the downshift flag to idle
        $Transmission_Engine_1::sig_transmission_Gear1 = gear; // Confirm the downshift
        $Transmission_Engine_2::sig_transmission_Gear2 = gear;
        $sig_transmission_Conf_Up_Down = 1;
      }else { // Else we stick to the same gear and reset the downshift request flag
        gear = $Transmission_Engine_1::sig_transmission_Gear1 + 1;
        $sig_engine_Man_Shift == 0x4;
        $sig_transmission_Conf_Up_Down = 0;
      }
    }else{ // Calculate the output rpm based on the current engaged gear
      $Transmission_Engine_1::sig_transmission_RPM = $sig_engine_RPM / gearr[gear];
    }
  }
}

void AutoShift()
{
  int cond[6], condd[6];
  $sig_transmission_Conf_Up_Down = 1;

  //1st Gear
  if($sig_transmission_RPM <= 490){
    if(condd[1] == 1){
      @sysvar::sysNS::sys_engine_Acceleratie = (483 * gearr[1]) / 6; // Set the RPM accordingly for downshifts, where 483 is the last possible rpm for 1st gear
      condd[1] = 0; // Reset the downshift condition
    }else{
      $Transmission_Engine_1::sig_transmission_Gear1 = 1; // Engage 1st gear
      $Transmission_Engine_2::sig_transmission_Gear2 = 1;
      $Transmission_Engine_1::sig_transmission_RPM = $sig_engine_RPM / gearr[1]; // calculate the rpm for 1st gear based on throttle input
    }
    if($sig_transmission_RPM >=480){
      cond[2] = 1; // Set the upshift condition
    }
  }

  //2nd Gear
  if($sig_transmission_RPM > 490 && $sig_transmission_RPM <= 970){
    if(condd[2] == 1){
      @sysvar::sysNS::sys_engine_Acceleratie = (951 * gearr[2]) / 6; // Set the RPM accordingly for downshift
      condd[2] = 0; // Reset the downshift condition
    }else if(cond[2] == 1){
      @sysvar::sysNS::sys_engine_Acceleratie = ($sig_transmission_RPM * gearr[2]) / 6; // Set the RPM accordingly for upshift
      cond[2] = 0; // Reset the upshift condition
    }else{
      $Transmission_Engine_1::sig_transmission_Gear1 = 2; // Engage 2nd gear
      $Transmission_Engine_2::sig_transmission_Gear2 = 2;
      $Transmission_Engine_1::sig_transmission_RPM = $sig_engine_RPM / gearr[2]; // calculate the rpm for 2nd gear based on throttle input
    }
    if($sig_transmission_RPM >=950){
      cond[3] = 1; // Set the upshift condition
    }
    if($sig_transmission_RPM <= 500){
      condd[1] = 1; // Set the downshift condition
    }
  }

//3rd Gear
  if($sig_transmission_RPM > 970 && $sig_transmission_RPM <= 1500){
    if(condd[3] == 1){
      @sysvar::sysNS::sys_engine_Acceleratie = (1485 * gearr[3]) / 6; // Set the RPM accordingly for downshift
      condd[3] = 0; // Reset the downshift condition
    }else if(cond[3] == 1){
      @sysvar::sysNS::sys_engine_Acceleratie = ($sig_transmission_RPM * gearr[3]) / 6; // Set the RPM accordingly for upshift
      cond[3] = 0; // Reset the upshift condition
    }else{
      $Transmission_Engine_1::sig_transmission_Gear1 = 3; // Engage 3rd gear
      $Transmission_Engine_2::sig_transmission_Gear2 = 3;
      $Transmission_Engine_1::sig_transmission_RPM = $sig_engine_RPM / gearr[3]; // calculate the rpm for 3rd gear based on throttle input
    }
    if($sig_transmission_RPM >= 1480){
      cond[4] = 1; // Set the upshift condition
    }
    if($sig_transmission_RPM <= 1000){
      condd[2] = 1; // Set the downshift condition
    }
  }

//4th Gear
  if($sig_transmission_RPM > 1500 && $sig_transmission_RPM <= 2000){
    if(condd[4] == 1){
      @sysvar::sysNS::sys_engine_Acceleratie = (1940 * gearr[4]) / 6; // Set the RPM accordingly for downshift
      condd[4] = 0; // Reset the downshift condition
    }else if(cond[4] == 1){
      @sysvar::sysNS::sys_engine_Acceleratie = ($sig_transmission_RPM * gearr[4]) / 6; // Set the RPM accordingly for upshift
      cond[4] = 0; // Reset the upshift condition
    }else{
      $Transmission_Engine_1::sig_transmission_Gear1 = 4; // Engage 4th gear
      $Transmission_Engine_2::sig_transmission_Gear2 = 4;
      $Transmission_Engine_1::sig_transmission_RPM = $sig_engine_RPM / gearr[4]; // calculate the rpm for 4th gear based on throttle input
    }
    if($sig_transmission_RPM >= 1970){
      cond[5] = 1; // Set the upshift condition
    }
    if($sig_transmission_RPM <= 1600){
      condd[3] = 1; // Set the downshift condition
    }
  }

//5th Gear
  if($sig_transmission_RPM > 2000){
    if(cond[5] == 1){
      @sysvar::sysNS::sys_engine_Acceleratie = ($sig_transmission_RPM * gearr[5]) / 6; // Set the RPM accordingly for upshift
      cond[5] = 0; // Reset the upshift condition
    }else{
      $Transmission_Engine_1::sig_transmission_Gear1 = 5; // Engage 5th gear
      $Transmission_Engine_2::sig_transmission_Gear2 = 5;
      $Transmission_Engine_1::sig_transmission_RPM = $sig_engine_RPM / gearr[5]; // calculate the rpm for 5th gear based on throttle input
    }
    if($sig_transmission_RPM <= 2100){ // Downshift into 4th gear
      condd[4] = 1;
    }
  }
}



dword applILTxPending (long aId, dword aDlc, byte data[])
{
  dword i;
  byte xor, crc;
  int e2e_sqcvalue1, e2e_sqcvalue2;
  //The following code increments the SQC value and then calculates the CRC of the 0x321 message.
  if(aId == 0x321)
  {
    if(@sysvar::sysNS::E2E_TE1_MSG_Error == 0){
      e2e_sqcvalue1 = data[1] & 0x0F;
      //This code takes the value of the current sqc
      if(@sysvar::sysNS::E2E_TE1_SQC_Error != 1){
        e2e_sqcvalue1++;
      }else{
        e2e_sqcvalue1 = 0;
      }
      e2e_sqcvalue1 = e2e_sqcvalue1 & 0xF;
      //This code increments the value of the sqc counter
      data[1] = (data[1] & 0xF0) + e2e_sqcvalue1; //Append the new sqc value in the data of the message
      xor = 0xFF;
      for(i = 1; i < aDlc; ++i) {
        xor = xor ^ data[i];
        xor = crctable[xor];
      }
      xor = xor ^ dataid[e2e_sqcvalue1];
      crc = crctable[xor];
      //Calculate the CRC of the message without the final 0xFF xor.
      data[0] = crc ^ 0xFF;
      if(@sysvar::sysNS::E2E_TE1_CRC_Error == 1){
        data[0] = data[0] ^ 0xFF;
      }
    }else{
      return 0;
    }
  }
  //The following code does the same for the 0x322 message, incrementing the sqc value and then calculating the CRC
  if(aId == 0x322)
  {
    if(@sysNS::E2E_TE2_MSG_Error == 0){
      e2e_sqcvalue2 = data[1] & 0x0F;
      if(@sysvar::sysNS::E2E_TE2_SQC_Error != 1){
        e2e_sqcvalue2++;
      }else{
        e2e_sqcvalue1 = 0;
      }
      e2e_sqcvalue2 = e2e_sqcvalue2 & 0xF;
      data[1] = (data[1] & 0xF0) + e2e_sqcvalue2;
      xor = 0xFF;
      for(i = 1; i < aDlc; ++i) {
        xor = xor ^ data[i];
        crc = crctable[xor];
      }
      xor = xor ^ dataid[e2e_sqcvalue2];
      crc = crctable[xor];
      data[0] = crc ^ 0xFF;
      if(@sysvar::sysNS::E2E_TE2_CRC_Error == 1){
        data[0] = data[0] ^ 0xFF;
      }
    }else{
      return 0;
    }
  }
  return 1; //Returning 1 means allowing
}

int E2ECheck(dword aId, byte aDlc, byte data[])
{
  dword i;
  byte xor, crc, crc_123, crc_124;
  int e2e_c_sqcold1, e2e_c_sqcold2;
  int e2e_c_sqcvalue1, e2e_c_sqcvalue2, ver;
  ver = 0;
  if(aId == 0x123)
  {
    e2e_c_sqcvalue1 = data[1] & 0x0F;
    if(((e2e_c_sqcold1 + 1) & 0xF) == e2e_c_sqcvalue1){
      xor = 0xFF;
      for(i = 1; i < aDlc; ++i) {
        xor = xor ^ data[i];
        xor = crctable[xor];
      }
      xor = xor ^ dataid[e2e_c_sqcvalue1];
      crc = crctable[xor];
      crc_123 = crc ^ 0xFF;
      if(data[0] == crc_123){
        ver = 1;
        e2e_c_sqcold1 = e2e_c_sqcvalue1;
        sysSetVariableString(sysvar::sysNS::E2E_ET1_Status, "OK");
      }else {
        ver = 0;
        e2e_c_sqcold1 = e2e_c_sqcvalue1;
        sysSetVariableString(sysvar::sysNS::E2E_ET1_Status, "CRC Error");
      }
    }else {
      ver = 0;
      sysSetVariableString(sysvar::sysNS::E2E_ET1_Status, "SQC Error");
    }
  }
  if(aId == 0x124)
  {
    e2e_c_sqcvalue2 = data[1] & 0x0F;
    if(((e2e_c_sqcold2 + 1) & 0xF) == e2e_c_sqcvalue2){
      xor = 0xFF;
      for(i = 1; i < aDlc; ++i) {
        xor = xor ^ data[i];
        crc = crctable[xor];
      }
      xor = xor ^ dataid[e2e_c_sqcvalue2];
      crc = crctable[xor];
      crc_124 = crc ^ 0xFF;
      if(data[0] == crc_124){
        ver = 1;
        e2e_c_sqcold2 = e2e_c_sqcvalue2;
        sysSetVariableString(sysvar::sysNS::E2E_ET2_Status, "OK");
      }else {
        ver = 0;
        e2e_c_sqcold2 = e2e_c_sqcvalue2;
        sysSetVariableString(sysvar::sysNS::E2E_ET2_Status, "CRC Error");
      }
    }else {
      ver = 0;
      sysSetVariableString(sysvar::sysNS::E2E_ET2_Status, "SQC Error");
    }
  }
  return ver;
}