IMU: added delay callback hooks to IMU initialisation

this allows the imu init() call to use a custom replacement for
delay(), which will allow for the processing of MAVLink packets during
IMU initialisation

git-svn-id: https://arducopter.googlecode.com/svn/trunk@2986 f9c3cf11-9bcb-44bc-f272-b75c42450872

libraries/AP_IMU/AP_IMU_Oilpan.cpp

@@ -42,7 +42,7 @@ const float   AP_IMU_Oilpan::_gyro_temp_curve[3][3] = {
 };
 
 void
-AP_IMU_Oilpan::init(Start_style style)
+AP_IMU_Oilpan::init(Start_style style, void (*callback)(unsigned long t))
 {
     // if we are warm-starting, load the calibration data from EEPROM and go
     //
@@ -51,8 +51,8 @@ AP_IMU_Oilpan::init(Start_style style)
     } else {
 
         // do cold-start calibration for both accel and gyro
-        _init_gyro();
+        _init_gyro(callback);
-        _init_accel();
+        _init_accel(callback);
 
         // save calibration
         _sensor_cal.save();
@@ -62,14 +62,14 @@ AP_IMU_Oilpan::init(Start_style style)
 /**************************************************/
 
 void
-AP_IMU_Oilpan::init_gyro()
+AP_IMU_Oilpan::init_gyro(void (*callback)(unsigned long t))
 {
-    _init_gyro();
+    _init_gyro(callback);
     _sensor_cal.save();
 }
 
 void
-AP_IMU_Oilpan::_init_gyro()
+AP_IMU_Oilpan::_init_gyro(void (*callback)(unsigned long t))
 {
 	int flashcount = 0;
 	int tc_temp;
@@ -80,20 +80,20 @@ AP_IMU_Oilpan::_init_gyro()
 
 	// cold start
 	tc_temp = _adc->Ch(_gyro_temp_ch);
- 	delay(500);
+ 	callback(500);
 	Serial.printf_P(PSTR("Init Gyro"));
 
 	for(int c = 0; c < 25; c++){				// Mostly we are just flashing the LED's here to tell the user to keep the IMU still
 		digitalWrite(A_LED_PIN, LOW);
 		digitalWrite(C_LED_PIN, HIGH);
-		delay(20);
+		callback(20);
 
 		for (int i = 0; i < 6; i++)
 			adc_in = _adc->Ch(_sensors[i]);
 
 		digitalWrite(A_LED_PIN, HIGH);
 		digitalWrite(C_LED_PIN, LOW);
-		delay(20);
+		callback(20);
 	}
 
 	for (int j = 0; j <= 2; j++)
@@ -116,7 +116,7 @@ AP_IMU_Oilpan::_init_gyro()
 				_sensor_cal[j] = _sensor_cal[j] * 0.9 + adc_in * 0.1;
 			}
 
-			delay(20);
+			callback(20);
 			if(flashcount == 5) {
 				Serial.printf_P(PSTR("*"));
 				digitalWrite(A_LED_PIN, LOW);
@@ -134,7 +134,7 @@ AP_IMU_Oilpan::_init_gyro()
 		total_change    = fabs(prev[0] - _sensor_cal[0]) + fabs(prev[1] - _sensor_cal[1]) +fabs(prev[2] - _sensor_cal[2]);
 		max_offset      = (_sensor_cal[0] > _sensor_cal[1]) ? _sensor_cal[0] : _sensor_cal[1];
 		max_offset      = (max_offset > _sensor_cal[2]) ? max_offset : _sensor_cal[2];
-		delay(500);
+		callback(500);
 	} while (  total_change > _gyro_total_cal_change || max_offset > _gyro_max_cal_offset);
 }
 
@@ -145,14 +145,14 @@ AP_IMU_Oilpan::save()
 }
 
 void
-AP_IMU_Oilpan::init_accel()
+AP_IMU_Oilpan::init_accel(void (*callback)(unsigned long t))
 {
-    _init_accel();
+    _init_accel(callback);
     _sensor_cal.save();
 }
 
 void
-AP_IMU_Oilpan::_init_accel()
+AP_IMU_Oilpan::_init_accel(void (*callback)(unsigned long t))
 {
 	int flashcount = 0;
 	float adc_in;
@@ -161,7 +161,7 @@ AP_IMU_Oilpan::_init_accel()
 	float max_offset;
 
 	// cold start
- 	delay(500);
+ 	callback(500);
 
 	Serial.printf_P(PSTR("Init Accel"));
 
@@ -177,7 +177,7 @@ AP_IMU_Oilpan::_init_accel()
 
 		for(int i = 0; i < 50; i++){		// We take some readings...
 
-			delay(20);
+			callback(20);
 
 			for (int j = 3; j < 6; j++){
 				adc_in 	    	= _adc->Ch(_sensors[j]);
@@ -206,7 +206,7 @@ AP_IMU_Oilpan::_init_accel()
 		max_offset = (_sensor_cal[3] > _sensor_cal[4]) ? _sensor_cal[3] : _sensor_cal[4];
 		max_offset = (max_offset > _sensor_cal[5]) ? max_offset : _sensor_cal[5];
 
-		delay(500);
+		callback(500);
 	} while (  total_change > _accel_total_cal_change || max_offset > _accel_max_cal_offset);
 
 	Serial.printf_P(PSTR(" "));

libraries/AP_IMU/AP_IMU_Oilpan.h

@@ -41,11 +41,11 @@ public:
 	///                 COLD_START performs calibration of both the accelerometer and gyro.
 	///                 WARM_START loads accelerometer and gyro calibration from a previous cold start.
 	///
-	virtual void		init(Start_style style = COLD_START);
+	virtual void		init(Start_style style = COLD_START, void (*callback)(unsigned long t) = delay);
 
 	virtual void		save();
-	virtual void		init_accel();
+	virtual void		init_accel(void (*callback)(unsigned long t) = delay);
-	virtual void		init_gyro();
+	virtual void		init_gyro(void (*callback)(unsigned long t) = delay);
 	virtual bool		update(void);
 
 	// for jason
@@ -65,8 +65,8 @@ private:
     AP_ADC              *_adc;          ///< ADC that we use for reading sensors
     AP_VarA<float,6>    _sensor_cal;    ///< Calibrated sensor offsets
 
-    virtual void        _init_accel();  ///< no-save implementation
+    virtual void        _init_accel(void (*callback)(unsigned long t));  ///< no-save implementation
-    virtual void        _init_gyro();   ///< no-save implementation
+    virtual void        _init_gyro(void (*callback)(unsigned long t));   ///< no-save implementation
 
     float 		        _sensor_compensation(uint8_t channel, int temp) const;
 	float		        _sensor_in(uint8_t channel, int temperature);

libraries/AP_IMU/AP_IMU_Shim.h

@@ -13,7 +13,7 @@ public:
 
 	/// @name IMU protocol
 	//@{
-	virtual void	init(Start_style style) {}
+	virtual void	init(Start_style style, void (*callback)(unsigned long t)) {}
 	virtual void	init_accel() {};
 	virtual void	init_gyro() {};
 	virtual bool	update(void) {

libraries/AP_IMU/IMU.h

@@ -35,21 +35,21 @@ public:
 	///
 	/// @param style	The initialisation startup style.
 	///
-	virtual void	init(Start_style style) = 0;
+	virtual void	init(Start_style style, void (*callback)(unsigned long t)) = 0;
 
 	/// Perform cold startup initialisation for just the accelerometers.
 	///
 	/// @note This should not be called unless ::init has previously
 	///       been called, as ::init may perform other work.
 	///
-	virtual void	init_accel() = 0;
+	virtual void	init_accel(void (*callback)(unsigned long t)) = 0;
 
 	/// Perform cold-start initialisation for just the gyros.
 	///
 	/// @note This should not be called unless ::init has previously
 	///       been called, as ::init may perform other work
 	///
-	virtual void	init_gyro() = 0;
+	virtual void	init_gyro(void (*callback)(unsigned long t)) = 0;
 
 	/// Give the IMU some cycles to perform/fetch an update from its
 	/// sensors.