From c2f3cf0cf53e1e2b6099f0bf69e5f52003d8ac4c Mon Sep 17 00:00:00 2001
From: Michael Oborne <mich146@hotmail.com>
Date: Sat, 21 Jan 2012 07:52:52 +0800
Subject: [PATCH] AC fix attitude level hil

---
 ArduCopter/GCS_Mavlink.pde | 36 ++++++++++--------------------------
 1 file changed, 10 insertions(+), 26 deletions(-)

diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde
index 6e2e6715dc..c9369f7099 100644
--- a/ArduCopter/GCS_Mavlink.pde
+++ b/ArduCopter/GCS_Mavlink.pde
@@ -1520,6 +1520,16 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             // set dcm hil sensor
             dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
             packet.pitchspeed,packet.yawspeed);
+        
+                    // rad/sec
+            Vector3f gyros;
+            gyros.x = (float)packet.rollspeed;
+            gyros.y = (float)packet.pitchspeed;
+            gyros.z = (float)packet.yawspeed;
+        
+            imu.set_gyro(gyros);
+
+            //imu.set_accel(accels);
             break;
         }
 #endif
@@ -1547,32 +1557,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 			packet.v,packet.hdg,0,0);
 			break;
 		}
-
-		//	Is this resolved? - MAVLink protocol change.....
-	case MAVLINK_MSG_ID_VFR_HUD:
-		{
-			// decode
-			mavlink_vfr_hud_t packet;
-			mavlink_msg_vfr_hud_decode(msg, &packet);
-
-			// set airspeed
-			airspeed = 100*packet.airspeed;
-			break;
-		}
-
-#endif
-#if HIL_MODE == HIL_MODE_ATTITUDE
-	case MAVLINK_MSG_ID_ATTITUDE:
-		{
-			// decode
-			mavlink_attitude_t packet;
-			mavlink_msg_attitude_decode(msg, &packet);
-
-			// set dcm hil sensor
-			dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
-			packet.pitchspeed,packet.yawspeed);
-			break;
-		}
 #endif
 */
 #if HIL_MODE == HIL_MODE_SENSORS