From 952c56e990c7d017ad5f32133a21e4fdcff00cc3 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 18 Mar 2013 16:19:31 +1100
Subject: [PATCH] Copter: fixed barometer init in HIL sensors mode

---
 ArduCopter/GCS_Mavlink.pde | 12 +++++++++++-
 ArduCopter/system.pde      |  9 +++++++++
 2 files changed, 20 insertions(+), 1 deletion(-)

diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde
index 2a8da3588a..02bd0f64de 100644
--- a/ArduCopter/GCS_Mavlink.pde
+++ b/ArduCopter/GCS_Mavlink.pde
@@ -527,7 +527,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
     // if we don't have at least 1ms remaining before the main loop
     // wants to fire then don't send a mavlink message. We want to
     // prioritise the main flight control loop over communications
-    if (scheduler.time_available_usec() < 800) {
+    if (scheduler.time_available_usec() < 800 && motors.armed()) {
         gcs_out_of_time = true;
         return false;
     }
@@ -1917,6 +1917,16 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
         ins.set_accel_offsets(accels);
 
+        // approximate a barometer
+        float y;
+        const float Temp = 312;
+
+        y = (packet.alt - 584000.0) / 29271.267;
+        y /= (Temp / 10.0) + 273.15;
+        y = 1.0/exp(y);
+        y *= 95446.0;
+
+        barometer.setHIL(Temp, y);
 
  #if HIL_MODE == HIL_MODE_ATTITUDE
         // set AHRS hil sensor
diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde
index d960f20141..8c15056ae8 100644
--- a/ArduCopter/system.pde
+++ b/ArduCopter/system.pde
@@ -238,6 +238,15 @@ static void init_ardupilot()
 #endif
 #endif // CLI_ENABLED
 
+#if HIL_MODE != HIL_MODE_DISABLED
+    while (!barometer.healthy) {
+        // the barometer becomes healthy when we get the first
+        // HIL_STATE message
+        gcs_send_text_P(SEVERITY_LOW, PSTR("Waiting for first HIL_STATE message"));
+        delay(1000);
+    }
+#endif
+
 #if HIL_MODE != HIL_MODE_ATTITUDE
     // read Baro pressure at ground
     //-----------------------------