From 1e36ebc31ba29346ccfbeaefb7a1e0923998ac0c Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sat, 23 Nov 2013 12:37:05 +1100
Subject: [PATCH] Plane: reset HIL attitude if off by more than 5 degrees

---
 ArduPlane/GCS_Mavlink.pde | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde
index 4db1ec716e..3cb81a8165 100644
--- a/ArduPlane/GCS_Mavlink.pde
+++ b/ArduPlane/GCS_Mavlink.pde
@@ -2065,6 +2065,13 @@ mission_failed:
         barometer.setHIL(packet.alt*0.001f);
         compass.setHIL(packet.roll, packet.pitch, packet.yaw);
         airspeed.disable();
+
+        // cope with DCM getting badly off due to HIL lag
+        if (fabsf(packet.roll - ahrs.roll) > ToRad(5) ||
+            fabsf(packet.pitch - ahrs.pitch) > ToRad(5) ||
+            fabsf(packet.yaw - ahrs.yaw) > ToRad(5)) {
+            ahrs.reset_attitude(packet.roll, packet.pitch, packet.yaw);
+        }
         break;
     }
 #endif // HIL_MODE