From 142381024779fb17dbb74a3c9c72fb5f00b81d33 Mon Sep 17 00:00:00 2001
From: Michael Oborne <mich146@hotmail.com>
Date: Mon, 4 Jun 2012 16:24:08 +0800
Subject: [PATCH] AC: add hilstate message

---
 ArduCopter/GCS_Mavlink.pde | 57 +++++++++++++++++++++++++++++++++++++-
 1 file changed, 56 insertions(+), 1 deletion(-)

diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde
index dd6c97b722..b5df926334 100644
--- a/ArduCopter/GCS_Mavlink.pde
+++ b/ArduCopter/GCS_Mavlink.pde
@@ -1846,7 +1846,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             APM_RC.setHIL(v);
             break;
         }
-
+        
 #if HIL_MODE != HIL_MODE_DISABLED && MAVLINK10 != ENABLED
         // This is used both as a sensor and to pass the location
         // in HIL_ATTITUDE mode. Note that MAVLINK10 uses HIL_STATE
@@ -1871,6 +1871,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             }
             break;
         }
+        
 #if HIL_MODE == HIL_MODE_ATTITUDE
     case MAVLINK_MSG_ID_ATTITUDE: //30
         {
@@ -1913,6 +1914,60 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         }
 #endif // HIL_MODE_ATTITUDE
 #endif // HIL_MODE != HIL_MODE_DISABLED && MAVLINK10 != ENABLED
+        
+#if MAVLINK10 == ENABLED && HIL_MODE != HIL_MODE_DISABLED
+	case MAVLINK_MSG_ID_HIL_STATE:
+		{
+			mavlink_hil_state_t packet;
+			mavlink_msg_hil_state_decode(msg, &packet);
+			
+			float vel = sqrt((packet.vx * (float)packet.vx) + (packet.vy * (float)packet.vy));
+			float cog = wrap_360(ToDeg(atan2(packet.vx, packet.vy)) * 100);
+			
+            // set gps hil sensor
+            g_gps->setHIL(packet.time_usec/1000.0,
+                          packet.lat*1.0e-7, packet.lon*1.0e-7, packet.alt*1.0e-3,
+                          vel*1.0e-2, cog*1.0e-2, 0, 10);
+                          
+            if (gps_base_alt == 0) {
+                gps_base_alt = g_gps->altitude;
+            }
+            current_loc.lng = g_gps->longitude;
+            current_loc.lat = g_gps->latitude;
+            current_loc.alt = g_gps->altitude - gps_base_alt;
+            if (!home_is_set) {
+                init_home();
+            }
+			
+			#if HIL_MODE == HIL_MODE_SENSORS
+			
+			// rad/sec
+            Vector3f gyros;
+            gyros.x = (float)packet.xgyro / 1000.0;
+            gyros.y = (float)packet.ygyro / 1000.0;
+            gyros.z = (float)packet.zgyro / 1000.0;
+            // m/s/s
+            Vector3f accels;
+            accels.x = (float)packet.xacc / 1000.0;
+            accels.y = (float)packet.yacc / 1000.0;
+            accels.z = (float)packet.zacc / 1000.0;
+
+            imu.set_gyro(gyros);
+
+            imu.set_accel(accels);
+			
+			#else
+
+			// set AHRS hil sensor
+            ahrs.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
+            packet.pitchspeed,packet.yawspeed);
+
+			#endif
+
+			break;
+		}
+#endif //  MAVLINK10 == ENABLED && HIL_MODE != HIL_MODE_DISABLED
+        
 /*
 	case MAVLINK_MSG_ID_HEARTBEAT:
 		{