UAV Outback challenge improvements from Team Swiss Fang

This is the collection of improvements done by the swiss team at the outback challenge. Needs validation / polishing.

@thomasgubler A merge with master would be great, I got confused by the (only two!) conflicts.
Is this tested on FMUv1 with MAVLink over USB?