Fix mission dive

This includes various small fixes that came up when investigating issue #649.

It has already been flown several times on fixedwing and multicopter because it had been merged into [beta_mavlink](https://github.com/PX4/Firmware/tree/beta_mavlink) and can be merged.