From 9563cd2859830c6b7aff21f95a537946878e88ca Mon Sep 17 00:00:00 2001
From: Felix Ruess <felix.ruess@gmail.com>
Date: Mon, 13 Oct 2014 00:32:15 +0200
Subject: [PATCH] [rotorcraft] only go to HOME mode if in NAV

then you don't have this "failsafe" in manual modes anymore,
but it will prevent issues like #850
---
 conf/xsens_MTi-G.xml                         | 26 ++++++++++++++++++++
 sw/airborne/firmwares/rotorcraft/autopilot.c |  2 +-
 2 files changed, 27 insertions(+), 1 deletion(-)

diff --git a/conf/xsens_MTi-G.xml b/conf/xsens_MTi-G.xml
index e6f83279a16..f6765bc334d 100644
--- a/conf/xsens_MTi-G.xml
+++ b/conf/xsens_MTi-G.xml
@@ -80,6 +80,32 @@
   </message>
   <message name="SetPeriodAck" ID="0x05" to="host"/>
 
+  <message name="ReqObjectAlignment" ID="0xE0" to="MT"/>
+  <message name="ReqObjectAlignmentAck" ID="0xE1" to="host">
+    <field name="a" format="R4"/>
+    <field name="b" format="R4"/>
+    <field name="c" format="R4"/>
+    <field name="d" format="R4"/>
+    <field name="e" format="R4"/>
+    <field name="f" format="R4"/>
+    <field name="g" format="R4"/>
+    <field name="h" format="R4"/>
+    <field name="i" format="R4"/>
+  </message>
+
+  <message name="SetObjectAlignment" ID="0xE0" to="MT" length="36">
+    <field name="a" format="R4"/>
+    <field name="b" format="R4"/>
+    <field name="c" format="R4"/>
+    <field name="d" format="R4"/>
+    <field name="e" format="R4"/>
+    <field name="f" format="R4"/>
+    <field name="g" format="R4"/>
+    <field name="h" format="R4"/>
+    <field name="i" format="R4"/>
+  </message>
+  <message name="SetObjectAlignmentAck" ID="0xD1" to="host"/>
+
   <message name="ReqOutputMode" ID="0xD0" to="MT"/>
   <message name="ReqOutputModeAck" ID="0xD1" to="host">
     <field name="mode" format="U2"/>
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c
index d0727cbd0a1..1173fd0da6e 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot.c
+++ b/sw/airborne/firmwares/rotorcraft/autopilot.c
@@ -290,7 +290,7 @@ void autopilot_periodic(void) {
 
   RunOnceEvery(NAV_PRESCALER, compute_dist2_to_home());
 
-  if (autopilot_in_flight) {
+  if (autopilot_in_flight && autopilot_mode == AP_MODE_NAV) {
     if (too_far_from_home) {
       if (dist2_to_home > failsafe_mode_dist2)
         autopilot_set_mode(FAILSAFE_MODE_TOO_FAR_FROM_HOME);