changes with altitude hold of 1m, heading hold code modified