Fix frame confusion and some minor issues in altitude/vspeed calculations.