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.
Looks like debugging noise.