velautotune.py

#!/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))