Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- # experimental thrust curve
- # DELTA vs raw_thrust is linear with m (<1) slope up to raw_thust = k (<1)
- # and linear with increased slope from m to 1
- # Gives DELTA between -1 and 1, to be converted to thrust units
- m = 0.3 # m between 0 and 0.5
- k = 0.9 # k between 0 and 1, the closer to 1 the later the higher slope kicks in
- lam = 250 # convert DELTA (-1 to 1) to thrust units
- try:
- data = self.inputdevice.readInput()
- roll = data["roll"] * self.maxRPAngle
- pitch = data["pitch"] * self.maxRPAngle
- yaw = data["yaw"]
- raw_thrust = data["thrust"]
- emergency_stop = data["estop"]
- trim_roll = data["rollcal"]
- trim_pitch = data["pitchcal"]
- # thrust computation
- # Thust limiting (slew, minimum)
- if self.emergencyStop != emergency_stop:
- self.emergencyStop = emergency_stop
- self.emergency_stop_updated.call(self.emergencyStop)
- if emergency_stop:
- thrust = 0
- else:
- t = abs(raw_thrust)
- if t < k:
- DELTA = m * t
- else:
- DELTA = m * t + (1-m)*(t-k)/(1-k)
- if raw_thrust < 0:
- DELTA = -DELTA
- thrust = self.oldThrust + lam*DELTA
- if thrust > self.maxThrust:
- thrust = self.maxThrust
- elif thrust < 0:
- thrust = 0
- self.oldThrust = thrust
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement