Modified DIYDrones autotest (https://github.com/diydrones/ardupilot/tree/master/Tools/autotest)