[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
paparazzi · Oct 12, 2014 · 9563cd2 · 9563cd2
1 parent 1532734
commit 9563cd2
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 1 deletion.
diff --git 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
@@ -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);