Advertisement
Guest User

Untitled

a guest
Aug 19th, 2013
242
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 1.62 KB | None | 0 0
  1.         # experimental thrust curve
  2.         # DELTA vs raw_thrust is linear with m (<1) slope up to raw_thust = k (<1)
  3.         # and  linear with increased slope from m to 1
  4.         # Gives DELTA between -1 and 1, to be converted to thrust units
  5.         m = 0.3 # m between 0 and 0.5
  6.         k = 0.9 # k between 0 and 1, the closer to 1 the later the higher slope kicks in
  7.         lam = 250 # convert DELTA (-1 to 1) to thrust units
  8.        
  9.         try:
  10.             data = self.inputdevice.readInput()
  11.             roll = data["roll"] * self.maxRPAngle
  12.             pitch = data["pitch"] * self.maxRPAngle
  13.             yaw = data["yaw"]
  14.             raw_thrust = data["thrust"]
  15.             emergency_stop = data["estop"]
  16.             trim_roll = data["rollcal"]
  17.             trim_pitch = data["pitchcal"]        
  18.  
  19.             # thrust computation
  20.             # Thust limiting (slew, minimum)
  21.             if self.emergencyStop != emergency_stop:
  22.                 self.emergencyStop = emergency_stop
  23.                 self.emergency_stop_updated.call(self.emergencyStop)
  24.             if emergency_stop:
  25.                 thrust = 0
  26.             else:
  27.                 t = abs(raw_thrust)
  28.                 if t < k:
  29.                     DELTA = m * t
  30.                 else:
  31.                     DELTA = m * t + (1-m)*(t-k)/(1-k)
  32.                 if raw_thrust < 0:
  33.                     DELTA = -DELTA
  34.                 thrust = self.oldThrust + lam*DELTA  
  35.                 if thrust > self.maxThrust:
  36.                     thrust = self.maxThrust
  37.                 elif thrust < 0:
  38.                     thrust = 0
  39.             self.oldThrust = thrust
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement