// *****************************************************************************
// * Config_SIOC ver 3.7B1   -     By Manolo Vélez    -    www.opencockpits.com
// *****************************************************************************
// * FileName : autothrottle.txt
// * Date : 11/05/2010
// * Author: Unknown (if you knows he, please write to info@opencockpits.com)
// *****************************************************************************



Var 1000, Value 0     // Initialisation 

Var 0000, name lever2_motor     // Motor control 

Var 0001, name lever1_motor, Value 1     // starts program 

Var 0002, name elevator_motor     // Motor control

Var 0003, name ENGINENUMBERS, Link FSUIPC_INOUT, Offset $0AEC, Length 2

Var 0049, name motor_stopped1, Value 1     // Lever1 

Var 0050, name motor_stopped2, Value 1     // Lever2 

Var 0051, name motor_stopped3, Value 1     // Elevator 

Var 0022, name AutoThrottle, Link FSUIPC_IN, Offset $07DC, Length 4     // is A/T active? 

Var 0023, name Lever2_to_FS, Link FSUIPC_OUT, Offset $0924, Length 2     // Send value of Lever2 to FS 

Var 0024, name Lever1_to_FS, Link FSUIPC_OUT, Offset $088C, Length 2     // Send value of Lever1 to FS 

Var 0020, name Trim_to_FS, Link FSUIPC_OUT, Offset $0BC0, Length 2     // Send value of PitchTrim to FS 

Var 0021, name AP_ALT_hold, Link FSUIPC_IN, Offset $07D0, Length 4     // AutoPilot ALT hold 

Var 0029, name AP_APP, Link FSUIPC_IN, Offset $07FC, Length 4     // AutoPilot Glideslope hold 

Var 0030, name TOGA, Link FSUIPC_IN, Offset $080C, Length 4     // is TO/GA switch active? 

Var 0025, name lever2_diff     // difference where the Lever2 is and where

Var 0026, name lever1_diff     // difference where the Lever1 is and where

Var 0027, name corr_Trim_pos     // Where ElevTrim should be corrected for P

Var 0028, name trim_diff     // difference where the Trim is and where i

Var 0100, name Lever1_upper, Value 135     // Upper potentiometer value of Lever1 

Var 0101, name Lever2_upper, Value 130     // Upper potentiometer value of Lever2 

Var 0102, name Lever1_idle, Value 30     // Idle potentiometer value of Lever1 

Var 0103, name Lever2_idle, Value 31     // Idle potentiometer value of Lever2 

Var 0104, name Lever1_lower, Value 8     // Lower potentiometer value of Lever1 

Var 0105, name Lever2_lower, Value 11     // Lower potentiometer value of Lever2 

Var 0106, name Lever1_factor     // Distance FS / PointPot 

Var 0107, name Lever2_factor     // Apply Distance_Pot to Distance FS 

Var 0108, name Pot1_factor     // Distance Pot / PointPot 

Var 0109, name Pot2_factor     // Apply Distance_Pot to Distance Lever 

Var 0013, name Lever1_target, Link FSUIPC_IN, Offset $088C, Length 2     // Where Lever1 should be 
{
  L0 = &Lever1_upper - &Lever1_idle     // Upper Potentiomer Value - Idle Potentiom
  L1 = 12800 / L0     // Distance Pot = 128 * 100 becouse no deci
  &Pot1_factor = L1    
  L0 = &Lever1_Pot - &Lever1_idle     // Corrected value from Pot to FS 
  L1 = L0 * &Pot1_factor     // apply length lever to pot 
  L1 = L1 / 100     // same as above, becouse of no decimal val
  L2 = &Lever1_target / 128     // value from FSUIPC must be adjusted to po
  &lever1_diff = L1 - L2     // where the lever is and where it should b
  IF &AutoThrottle = 1     // A/T is on 
  {
    CALL &Motor_Lever1     // subroutine for motor1 motion 
  }
  IF &TOGA = 1     // TO/GA switch is on 
  {
    CALL &Motor_Lever1     // subroutine for motor1 motion 
  }
}

Var 0012, name Lever2_target, Link FSUIPC_IN, Offset $0924, Length 2     // Where Lever2 should be 
{
  L0 = &Lever2_upper - &Lever2_idle     // Upper Potentiomer Value - Idle Potentiom
  L1 = 12800 / L0     // Distance Pot = 128 
  &Pot2_factor = L1    
  L0 = &Lever2_Pot - &Lever2_idle     // Corrected value from Pot to FS 
  L1 = L0 * &Pot2_factor
  L1 = L1 / 100
  L2 = &Lever2_target / 128
  &lever2_diff = L1 - L2
  IF &AutoThrottle = 1     // A/T is on 
  {
    CALL &Motor_Lever2     // subroutine for motor2 motion 
  }
  IF &TOGA = 1     // TO/GA switch is on 
  {
    CALL &Motor_Lever2     // subroutine for motor2 motion 
  }
}

Var 0018, name Trim_target, Link FSUIPC_IN, Offset $0BC0, Length 2     // Where Elevator Trim should be 
{
  IF &Trim_target > 33000
  {
    L0 = 81918 - &Trim_target     // Trim down (49153 .. 65535) 
    L1 = L0 / 128     // 65535 + 16383 = 81918 
    L1 = TRUNC L1
  }
  ELSE
  {
    L0 = 16383 - &Trim_target     // Trim up (0 .. 16383) 
    L1 = L0 / 128
    L1 = TRUNC L1
  }
  &corr_Trim_pos = L1    
  &trim_diff = &corr_Trim_pos - &Trim_Pot
  IF &AP_ALT_hold = 1     // ALT hold is on 
  {
    CALL &Motor_Elevator     // Elevator trim motor motion 
  }
  IF &AP_APP = 1     // APP hold is on 
  {
    CALL &Motor_Elevator     // Elevator trim motor motion 
  }
}

Var 0006, name Lever1_Pot, Link USB_ANALOGIC, Input 1, PosL 0, PosC 127, PosR 254     // Lever1 Pot position 
{
  L0 = &Lever1_upper - &Lever1_idle     // Upper Potentiomer Value - Idle Potentiom
  L1 = 16383 / L0     // Distance FS = 16383 
  L1 = TRUNC L1
  &Lever1_factor = L1    
  L0 = &Lever1_Pot - &Lever1_idle     // Corrected value from Pot to FS 
  L1 = L0 * &Lever1_factor
  L1 = TRUNC L1
  IF &AutoThrottle = 0     // A/T is OFF 
  {
    IF &TOGA = 0     // TOGA not aktiv 
    {
      &lever1_motor = 0         // motor stops 
    }
    IF &ENGINENUMBERS = 2
    {
      &Lever1_to_FS = L1         // Send Lever1 Position to FS 
    }
    IF &ENGINENUMBERS = 4
    {
      &Lever1_to_FS = L1         // Send Lever1 Position to FS 
      &Lever2_to_FS = L1         // Send Lever2 Position to FS 
    }
  }
}

Var 0007, name Lever2_Pot, Link USB_ANALOGIC, Input 2, PosL 0, PosC 127, PosR 254     // Lever2 Pot position 
{
  L0 = &Lever2_upper - &Lever2_idle     // Upper Potentiomer Value - Idle Potentiom
  L1 = 16383 / L0     // Distance FS = 16383 
  L1 = TRUNC L1
  &Lever2_factor = L1    
  L0 = &Lever2_Pot - &Lever2_idle     // Corrected value from Pot to FS 
  L1 = L0 * &Lever2_factor
  L1 = TRUNC L1
  IF &AutoThrottle = 0     // A/T is OFF 
  {
    IF &TOGA = 0     // TOGA not aktiv 
    {
      &lever2_motor = 0         // motor stops 
    }
    IF &ENGINENUMBERS = 2
    {
      &Lever2_to_FS = L1         // Send Lever2 Position to FS 
    }
  }
}

Var 0008, name Trim_Pot, Link USB_ANALOGIC, Input 3, PosL 0, PosC 127, PosR 254     // Trim Potentiometer position 
{
  L0 = &Trim_Pot * 128
  L1 = 16383 - L0
  IF &AP_ALT_hold = 0     // ALT hold is off 
  {
    IF &AP_APP = 0     // APPR mode is off 
    {
      &Trim_to_FS = L1    
      &elevator_motor = 0         // motor stops 
    }
  }
}

Var 0014, name dead_area1, Value 7     // % error 

Var 0015, name break_dist1, Value 13     // points for aproximation break 

Var 0016, name current_break1, Value 0     // Speed for motor when whithin break_dista

Var 0017, name current_run1, Value 40     // Speed for motor when running 

Var 0048, name current_start1, Value 40     // Speed for motor when starting 

Var 0034, name dead_area2, Value 7     // % error 

Var 0035, name break_dist2, Value 15     // points for aproximation break 

Var 0036, name current_break2, Value 0     // Speed for follow objective 

Var 0037, name current_run2, Value 42

Var 0052, name current_start2, Value 42

Var 0044, name margen3, Value 6     // % error 

Var 0045, name aprox3, Value 30     // points for aproximation break 

Var 0046, name velr3, Value 80     // Speed for motor when small difference 

Var 0047, name vell3, Value 90     // Speed for motor when big difference 

Var 0120, name Motor_Lever1, Link SUBRUTINE     // USB_DC controls motor for Lever1 
{
  L0 = &lever1_diff    
  L1 = 0    
  IF L0 < 0
  {
    L1 = 129    
  }
  L0 = ABS L0
  L2 = &current_run1 + L1
  IF L0 <= &break_dist1
  {
    IF L1 = 0
    {
      L2 = &current_break1 + 129
    }
    ELSE
    {
      L2 = &current_break1    
    }
    IF &motor_stopped1 = 1
    {
      L2 = &current_start1 + L1
      &lever1_motor = L2    
      &motor_stopped1 = 0    
      L2 = &current_run1 + L1
    }
  }
  L0 = ABS L0
  IF L0 <= &dead_area1
  {
    L2 = 0    
    &motor_stopped1 = 1    
  }
  IF &AutoThrottle = 0     // A/T is off 
  {
    IF &TOGA = 0     // TOGA not aktiv 
    {
      L2 = 0    
      &motor_stopped1 = 1    
    }
  }
  &lever1_motor = L2    
  L0 = &lever2_diff    
  L1 = 129    
  IF L0 < 0
  {
    L1 = 0    
  }
  L0 = ABS L0
  L2 = &current_run2 + L1
  IF L0 <= &break_dist2
  {
    IF L1 = 0
    {
      L2 = &current_break2 + 129
    }
    ELSE
    {
      L2 = &current_break2    
    }
    L2 = &current_break2 + L1
    IF &motor_stopped2 = 1
    {
      L2 = &current_start2 + L1
      &lever2_motor = L2    
      &motor_stopped2 = 0    
      L2 = &current_run2 + L1
    }
  }
  L0 = ABS L0
  IF L0 <= &dead_area2
  {
    L2 = 0    
    &motor_stopped2 = 1    
  }
  IF &AutoThrottle = 0     // A/T is off 
  {
    IF &TOGA = 0     // TOGA not aktiv 
    {
      L2 = 0    
      &motor_stopped2 = 1    
    }
  }
  &lever2_motor = L2    
}

Var 0125, name Motor_Lever2, Link SUBRUTINE     // USB_DC controls motor for Lever2 

Var 0130, name Motor_Elevator, Link SUBRUTINE     // USB_DC controls motor for Elevator 
{
  L0 = &trim_diff    
  L1 = 0    
  IF L0 < 0
  {
    L1 = 128    
  }
  L0 = ABS L0
  L2 = &velr3 + L1
  IF L0 <= &aprox3
  {
    L2 = &vell3 + L1
  }
  L0 = ABS L0
  IF L0 <= &margen3
  {
    L2 = 0    
  }
  IF &AP_ALT_hold = 0     // ALT hold is off 
  {
    IF &AP_APP = 0     // APPR mode is off 
    {
      L2 = 0    
    }
  }
  &elevator_motor = L2    
}