Some mavlink fixes to use vehicle_control_mode, WIP