Merge pull request #975 from iNavFlight/failsafe-stage1-fix