From 022314ed4ce3e208dd03871e3bacec819bd3a3cd Mon Sep 17 00:00:00 2001
From: "tridge60@gmail.com"
 <tridge60@gmail.com@f9c3cf11-9bcb-44bc-f272-b75c42450872>
Date: Mon, 1 Aug 2011 08:08:52 +0000
Subject: [PATCH] process MAVLInk packets during gps detection and IMU
 calibration

this allows the GCS to connect immediately on startup

git-svn-id: https://arducopter.googlecode.com/svn/trunk@2994 f9c3cf11-9bcb-44bc-f272-b75c42450872
---
 ArduCopterMega/GCS_Mavlink.pde | 54 ++++++++++++++++++++++++++++++++--
 ArduCopterMega/motors.pde      |  2 +-
 ArduCopterMega/system.pde      |  3 +-
 3 files changed, 55 insertions(+), 4 deletions(-)

diff --git a/ArduCopterMega/GCS_Mavlink.pde b/ArduCopterMega/GCS_Mavlink.pde
index c6af9d082f..9b9621a3c1 100644
--- a/ArduCopterMega/GCS_Mavlink.pde
+++ b/ArduCopterMega/GCS_Mavlink.pde
@@ -323,7 +323,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 					break;
 				
 				case MAV_ACTION_CALIBRATE_ACC:
-					imu.init_accel();
+					imu.init_accel(mavlink_delay);
 					result=1;
 					break;
 				
@@ -1127,5 +1127,55 @@ GCS_MAVLINK::_queued_send()
 	}
 }
 
-#endif
+#endif // GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK || HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
 
+static void send_rate(uint8_t low, uint8_t high) {
+#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
+    gcs.data_stream_send(low, high);
+#endif
+#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
+    hil.data_stream_send(low,high);
+#endif
+}
+
+/*
+ a delay() callback that processes MAVLink packets. We set this as the
+ callback in long running library initialisation routines to allow
+ MAVLink to process packets while waiting for the initialisation to
+ complete
+*/
+static void mavlink_delay(unsigned long t)
+{
+    unsigned long tstart;
+    static unsigned long last_1hz, last_3hz, last_10hz, last_50hz;
+
+    tstart = millis();
+    do {
+        unsigned long tnow = millis();
+        if (tnow - last_1hz > 1000) {
+            last_1hz = tnow;
+            gcs.send_message(MSG_HEARTBEAT);
+#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
+            hil.send_message(MSG_HEARTBEAT);
+#endif
+            hil.data_stream_send(1,3);
+        }
+        if (tnow - last_3hz > 333) {
+            last_3hz = tnow;
+            hil.data_stream_send(3,5);
+        }
+        if (tnow - last_10hz > 100) {
+            last_10hz = tnow;
+            hil.data_stream_send(5,45);
+        }
+        if (tnow - last_50hz > 20) {
+            last_50hz = tnow;
+            gcs.update();
+#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
+            hil.update();
+#endif
+            hil.data_stream_send(45,1000);
+        }
+        delay(1);
+    } while (millis() - tstart < t);
+}
diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde
index 4eb620d47d..47be698c0d 100644
--- a/ArduCopterMega/motors.pde
+++ b/ArduCopterMega/motors.pde
@@ -62,7 +62,7 @@ static void arm_motors()
 			//Serial.print(arming_counter, DEC);
 			if (arming_counter > LEVEL_DELAY){
 				//Serial.print("init");
-				imu.init_accel();
+				imu.init_accel(mavlink_delay);
 				arming_counter = 0;
 
 			}else if (arming_counter == DISARM_DELAY){
diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde
index 49fdff3eb2..74f0b7e657 100644
--- a/ArduCopterMega/system.pde
+++ b/ArduCopterMega/system.pde
@@ -177,6 +177,7 @@ static void init_ardupilot()
 	// Do GPS init
 	g_gps = &g_gps_driver;
 	g_gps->init();			// GPS Initialization
+    g_gps->callback = mavlink_delay;
 
 	// init the GCS
 	#if GCS_PORT == 3
@@ -317,7 +318,7 @@ static void startup_ground(void)
 	#if HIL_MODE != HIL_MODE_ATTITUDE
 		// Warm up and read Gyro offsets
 		// -----------------------------
-		imu.init_gyro();
+		imu.init_gyro(mavlink_delay);
 		report_imu();
 	#endif