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.
are you sure? If I remember correctly we ran into issues with 2? Maybe some number betw 2 and 10?