Some cleanup and velocity stuff seem ok, started flight testing