Flightplan improvment for easier testing and added sonar also