Flight modes ready to merge

Saved testing code, prepared functions to merge with examples how to
call function