Skip to content

Instantly share code, notes, and snippets.

@mlyle
Created March 8, 2018 21:00
Show Gist options
  • Save mlyle/5dbe97aa964d3709db3a11c189471d70 to your computer and use it in GitHub Desktop.
Save mlyle/5dbe97aa964d3709db3a11c189471d70 to your computer and use it in GitHub Desktop.
#!/usr/bin/python
from math import pi
# per-vehicle constants
thrusttau = 0.070
hoverthrust = 0.52
max_climb = 4.0
# universal constants
onegee = 9.8
# tuning
base_kp = 0.5
tau_derate = 0.025
ki_adjust = 1.5
outer_slowdown = 0.125
max_sane_decel = 0.9 * onegee
kp = (base_kp * hoverthrust / onegee) / (thrusttau + tau_derate)
ki = kp / (ki_adjust * 2 * pi * (thrusttau + tau_derate))
# Decay the velocity target 8x slower than adj thrust tau
okp = outer_slowdown / (thrusttau + tau_derate)
# Check whether we're limited by freefall
altokp = max_sane_decel / max_climb
if altokp < okp:
print("Reducing poskp- never ask for more than %0.1f m/s/s single ended\n"%(max_sane_decel))
okp = altokp
print("Craft details, tau=%0.3f, hoverthrust=%0.2f"%(thrusttau, hoverthrust))
print("velkp=%0.3f velki=%0.3f poskp=%0.2f"%(kp, ki, okp))
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment