Initial merge of PatrikE's Fixed Wing RTH

Creating a new PR that is clean.