AC: add hilstate message

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:
 		{