/*@@var:*/
/*
 * FMS Gateway ECU                                 Version 1.1
 *
 * Copyright 2005, Vector Informatik GmbH - All right reserved
 *
 * History:
 * 1.0 (Jr) Created 
 * 1.1 (Jr) Minor changes, fixed initialization issue
 */
variables
{
  // Constants
  const int  kNullAddr    = 0xfe;   // Null address
  const int  kGlobalAddr  = 0xff;   // Global address
  const byte kCAN_Channel = 1;
  const byte kFMS_Channel = 2;

  // TX Parameter Groups
  pg FMS::ACL   TX_ACL;
  pg FMS::EEC1  TX_EEC1;
  pg FMS::EEC2  TX_EEC2;
  pg FMS::TCO1  TX_TCO1;
  pg FMS::CCVS  TX_CCVS;
  pg FMS::DD    TX_DD;
  pg FMS::ET1   TX_ET1;
  pg FMS::HOURS TX_HOURS;
  pg FMS::LFC   TX_LFC;
  pg FMS::SERV  TX_SERV;
  pg FMS::VDHR  TX_VDHR;
  pg FMS::VW    TX_VW;
  pg FMS::FMSSI TX_FMSSI;
  pg FMS::VI    TX_VI;

  // Timers
  msTimer Timer20ms;
  msTimer Timer50ms;
  msTimer Timer100ms;
  msTimer Timer1000ms;
  msTimer Timer10s;
  msTimer ACLTimer;
}
/*@@end*/

/*@@timer:Timer20ms:*/
/*
 * Transmit Parameter Groups with 20ms cycle time.
 */
on timer Timer20ms
{
  output( TX_EEC1 );

  setTimer( Timer20ms, 20 );
}
/*@@end*/

/*@@timer:Timer50ms:*/
/*
 * Transmit Parameter Groups with 50ms cycle time.
 */
on timer Timer50ms
{
  output( TX_EEC2 );
  output( TX_TCO1 );

  setTimer( Timer50ms, 50 );
}
/*@@end*/

/*@@timer:Timer100ms:*/
/*
 * Transmit Parameter Groups with 100ms cycle time.
 */
on timer Timer100ms
{
  output( TX_CCVS );

  setTimer( Timer100ms, 100 );
}
/*@@end*/

/*@@timer:Timer1000ms:*/
/*
 * Transmit Parameter Groups with 1000ms cycle time.
 */
on timer Timer1000ms
{
  output( TX_DD );
  output( TX_ET1 );
  output( TX_HOURS );
  output( TX_LFC );
  output( TX_SERV );
  output( TX_LFC );
  output( TX_VDHR );
  output( TX_VW );

  setTimer( Timer1000ms, 1000 );
}
/*@@end*/

/*@@timer:Timer10s:*/
/*
 * Transmit Parameter Groups with 10 seconds cycle time.
 */
on timer Timer10s
{
  output( TX_FMSSI );
  output( TX_VI );

  setTimer( Timer10s, 10000 );
}
/*@@end*/

/*@@caplFunc:VGWStartUp():*/
/*
 * Start the ECU. 
 * The functions starts the Address Claiming procedure.
 */
void VGWStartUp()
{
  TX_ACL.SA                      = FMS::VGW.NmStationAddress;
  TX_ACL.DA                      = kGlobalAddr;
  TX_ACL.ArbitraryAddressCapable = FMS::VGW.NmJ1939AAC;
  TX_ACL.IndustryGroup           = FMS::VGW.NmJ1939IndustryGroup;
  TX_ACL.VehicleSystem           = FMS::VGW.NmJ1939System;
  TX_ACL.VehicleSystemInstance   = FMS::VGW.NmJ1939SystemInstance;
  TX_ACL.Function                = FMS::VGW.NmJ1939Function;
  TX_ACL.FunctionInstance        = FMS::VGW.NmJ1939FunctionInstance;
  TX_ACL.ECUInstance             = FMS::VGW.NmJ1939ECUInstance;
  TX_ACL.ManufacturerCode        = FMS::VGW.NmJ1939ManufacturerCode;
  TX_ACL.IdentityNumber          = FMS::VGW.NmJ1939IdentityNumber;

  VGWInit();

  TX_ACL.CAN = kFMS_Channel;
  output( TX_ACL );
  TX_ACL.CAN = kCAN_Channel;
  output( TX_ACL );

  setTimer( ACLTimer, 250 );
}
/*@@end*/

/*@@caplFunc:VGWShutDown():*/
/*
 * Shut down the ECU. Stop timers and transmission
 * of the Parameter Groups.
 */
void VGWShutDown()
{
  cancelTimer( Timer20ms );
  cancelTimer( Timer50ms );
  cancelTimer( Timer100ms );
  cancelTimer( Timer1000ms );
  cancelTimer( Timer10s );
}
/*@@end*/

/*@@caplFunc:VGWInit():*/
/*
 * Initialize the Parameter Group buffers
 */
void VGWInit()
{
  TX_EEC1.CAN  = kFMS_Channel;
  TX_EEC1.SA   = TX_ACL.SA;
  TX_EEC1.dword(0) = TX_EEC1.dword(4) = 0xffffffff;

  TX_EEC2.CAN  = kFMS_Channel;
  TX_EEC2.SA   = TX_ACL.SA;
  TX_EEC2.dword(0) = TX_EEC2.dword(4) = 0xffffffff;

  TX_TCO1.CAN  = kFMS_Channel;
  TX_TCO1.SA   = TX_ACL.SA;
  TX_TCO1.dword(0) = TX_TCO1.dword(4) = 0xffffffff;

  TX_CCVS.CAN  = kFMS_Channel;
  TX_CCVS.SA   = TX_ACL.SA;
  TX_CCVS.dword(0) = TX_CCVS.dword(4) = 0xffffffff;

  TX_DD.CAN    = kFMS_Channel;
  TX_DD.SA     = TX_ACL.SA;
  TX_DD.dword(0) = TX_DD.dword(4) = 0xffffffff;

  TX_ET1.CAN   = kFMS_Channel;
  TX_ET1.SA    = TX_ACL.SA;
  TX_ET1.dword(0) = TX_ET1.dword(4) = 0xffffffff;

  TX_HOURS.CAN = kFMS_Channel;
  TX_HOURS.SA  = TX_ACL.SA;
  TX_HOURS.dword(0) = TX_HOURS.dword(4) = 0xffffffff;

  TX_LFC.CAN   = kFMS_Channel;
  TX_LFC.SA    = TX_ACL.SA;
  TX_LFC.dword(0) = TX_LFC.dword(4) = 0xffffffff;

  TX_SERV.CAN  = kFMS_Channel;
  TX_SERV.SA   = TX_ACL.SA;
  TX_SERV.dword(0) = TX_SERV.dword(4) = 0xffffffff;

  TX_VDHR.CAN  = kFMS_Channel;
  TX_VDHR.SA   = TX_ACL.SA;
  TX_VDHR.dword(0) = TX_VDHR.dword(4) = 0xffffffff;

  TX_VW.CAN    = kFMS_Channel;
  TX_VW.SA     = TX_ACL.SA;
  TX_VW.dword(0) = TX_VW.dword(4) = 0xffffffff;

  TX_FMSSI.CAN = kFMS_Channel;
  TX_FMSSI.SA  = TX_ACL.SA;
  TX_FMSSI.dword(0) = TX_FMSSI.dword(4) = 0xffffffff;
  TX_FMSSI.SoftwareVersion      = ('0' << 24) | ('1' << 16) |  ('0' << 8) | ('0');
  TX_FMSSI.RequestsSupported    = 0;
  TX_FMSSI.DiagnosticsSupported = 0;

  TX_VI.CAN    = kFMS_Channel;
  TX_VI.SA     = TX_ACL.SA;
  TX_VI.DLC     = 8;
  TX_VI.dword(0) = TX_VI.dword(4) = 0xffffffff;
}
/*@@end*/

/*@@startStart:Start:*/
on start
{
  VGWStartUp();
}
/*@@end*/

/*@@stop:StopMeasurement:*/
on stopMeasurement
{
  VGWShutDown();
}
/*@@end*/

/*@@caplFunc:VGWClaimingComplete():*/
/*
 * The ECU has successfully claimed an address. Start
 * the timers an transmit the Parameter Groups on the
 * FMS-Bus
 */
void VGWClaimingComplete()
{
  setTimer( Timer20ms, 20 );
  setTimer( Timer50ms, 50 );
  setTimer( Timer100ms, 100 );
  setTimer( Timer1000ms, 1000 );
  setTimer( Timer10s, 10000 );
}
/*@@end*/

/*@@pg:CAN1.Powertrain::ACL (0xEEFEFEX):*/
/*
 * Received Address Claim Parameter Group
 */
on pg Powertrain::ACL
{
  pg Powertrain::ACL acl;
  int                compareResult;

  if (this.DIR != RX) return;
  if ((this.CAN != kFMS_Channel) && (this.CAN != kCAN_Channel)) return;

  if (TX_ACL.SA == this.SA) {
    acl.DWORD(0) = this.DWORD(0);
    acl.DWORD(4) = this.DWORD(4);

    compareResult = utilCompareDeviceName( TX_ACL, acl );
    
    // compare names for priority
    if (compareResult > 0) {
      // send address claiming PG again
      TX_ACL.CAN = kFMS_Channel;
      output( TX_ACL );
      TX_ACL.CAN = kCAN_Channel;
      output( TX_ACL );
    }
    else if (compareResult < 0) {
      // go offline, we have a lower priority
      cancelTimer( ACLTimer );

      TX_ACL.SA  = kNullAddr;
      TX_ACL.CAN = kFMS_Channel;
      output( TX_ACL );
      TX_ACL.CAN = kCAN_Channel;
      output( TX_ACL );

      VGWShutDown();
    }
  }
}
/*@@end*/

/*@@caplFunc:utilCompareDeviceName(pg418316030x,pg418316030x):*/
/*
 * Compare device names
 *
 * if pg1 has lower priority than pg2, return -1
 * if pg1 has higher priority than pg2, return 1
 * if both names are equal, return 0
 */
int utilCompareDeviceName( pg FMS::ACL pg1, pg FMS::ACL pg2 )
{
  int i = 0;
  for( i = 7; i >= 0; i-- ) {
    if (pg1.byte(i) > pg2.byte(i)) {
      return -1;
    }
    else if (pg1.byte(i) < pg2.byte(i)) {
      return 1;
    }
  }

  return 0;
}
/*@@end*/

/*@@timer:ACLTimer:*/
/*
 * If this timer expires, we have successfully claimed an address.
 */
on timer ACLTimer
{
  VGWClaimingComplete();
}
/*@@end*/

/*@@pg:CAN1.Powertrain::EEC1_EMS (0xF00400X):*/
on pg Powertrain::EEC1_EMS
{
  if ((this.dir == RX) && (this.can == kCAN_Channel)) {
    TX_EEC1.EngSpeed = this.EngSpeed;
  }
}
/*@@end*/

/*@@pg:CAN1.Powertrain::EEC2_EMS (0xF00300X):*/
on pg Powertrain::EEC2_EMS
{
  if ((this.dir == RX) && (this.can == kCAN_Channel)) {
    TX_EEC2.AccelPedalPosition1 = this.AccelPedalPos1;
  }
}
/*@@end*/

/*@@pg:CAN1.Powertrain::TCO1_TCO (0xFE6CEEX):*/
on pg Powertrain::TCO1_TCO
{
  if ((this.dir == RX) && (this.can == kCAN_Channel)) {
    TX_TCO1.DirectionIndicator       = this.DirectionIndicator;
    TX_TCO1.Driver1TimeRelatedStates = this.Driver1TimeRelatedStates;
    TX_TCO1.Driver1WorkingState      = this.Driver1WorkingState;
    TX_TCO1.Driver2TimeRelatedStates = this.Driver2TimeRelatedStates;
    TX_TCO1.Driver2WorkingState      = this.Driver2WorkingState;
    TX_TCO1.DriverCardDriver1        = this.DriverCardDriver1;
    TX_TCO1.DriverCardDriver2        = this.DriverCardDriver2;
    TX_TCO1.DriveRecognize           = this.DriveRecognize;
    TX_TCO1.HandlingInformation      = this.HandlingInformation;
    TX_TCO1.SystemEvent              = this.SystemEvent;
    TX_TCO1.TachographPerformance    = this.TachographPerformance;
    TX_TCO1.TachographVehicleSpeed   = this.TachographVehicleSpeed;
  }
}
/*@@end*/

/*@@pg:CAN1.Powertrain::CCVS_EMS (0xFEF100X):*/
on pg Powertrain::CCVS_EMS
{
  if ((this.dir == RX) && (this.can == kCAN_Channel)) {
    TX_CCVS.WheelBasedVehicleSpeed = this.WheelBasedVehicleSpeed;
    TX_CCVS.BrakeSwitch            = this.BrakeSwitch;
    TX_CCVS.ClutchSwitch           = this.ClutchSwitch;
    TX_CCVS.CruiseCtrlActive       = this.CruiseCtrlActive;
    TX_CCVS.PTOState               = this.PTOState;
  }
}
/*@@end*/

/*@@pg:CAN1.Powertrain::ET1_EMS (0xFEEE00X):*/
on pg Powertrain::ET1_EMS
{
  if ((this.dir == RX) && (this.can == kCAN_Channel)) {
    TX_ET1.EngCoolantTemp = this.EngCoolantTemp;
  }
}
/*@@end*/

/*@@pg:CAN1.Powertrain::HOURS_EMS (0xFEE500X):*/
on pg Powertrain::HOURS_EMS
{
  if ((this.dir == RX) && (this.can == kCAN_Channel)) {
    TX_HOURS.EngTotalHoursOfOperation = this.EngTotalHoursOfOperation;
  }
}
/*@@end*/

/*@@pg:CAN1.Powertrain::LFC_EMS (0xFEE900X):*/
on pg Powertrain::LFC_EMS
{
  if ((this.dir == RX) && (this.can == kCAN_Channel)) {
    TX_LFC.EngTotalFuelUsed = this.EngTotalFuelUsed;
  }
}
/*@@end*/

/*@@pg:CAN1.Powertrain::SERV (0xFEC0FEX):*/
on pg Powertrain::SERV
{
  if ((this.dir == RX) && (this.can == kCAN_Channel)) {
    TX_SERV.ServiceDistance = this.ServiceDistance;
  }
}
/*@@end*/

/*@@pg:CAN1.Powertrain::VDHR_EMS (0xFEC100X):*/
on pg Powertrain::VDHR_EMS
{
  if ((this.dir == RX) && (this.can == kCAN_Channel)) {
    TX_VDHR.HghRslutionTotalVehicleDistance = this.HghRslutionTotalVehicleDistance;
  }
}
/*@@end*/

/*@@pg:CAN1.Powertrain::VW_SSC (0xFEEA2FX):*/
on pg Powertrain::VW_SSC
{
  if ((this.dir == RX) && (this.can == kCAN_Channel)) {
    TX_VW.AxleLocation = this.AxleLocation;
    TX_VW.AxleWeight   = this.AxleWeight;
  }
}
/*@@end*/

/*@@pg:CAN1.Powertrain::VI_EMS (0xFEEC00X):*/
on pg Powertrain::VI_EMS
{
  TX_VI = (pg FMS::VI)this;
  TX_VI.CAN = kFMS_Channel;
  TX_VI.SA     = TX_ACL.SA;
}
/*@@end*/