INS: integrate AP_Notify

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -148,20 +148,20 @@ AP_InertialSensor::_init_gyro(void (*flash_leds_cb)(bool on))
     hal.scheduler->delay(100);
     hal.console->printf_P(PSTR("Init Gyro"));
 
+    // flash leds to tell user to keep the IMU still
+    AP_Notify::flags.initialising = true;
+
     // remove existing gyro offsets
     _gyro_offset = Vector3f(0,0,0);
 
     for(int8_t c = 0; c < 25; c++) {
-        // Mostly we are just flashing the LED's here
-        // to tell the user to keep the IMU still
-        FLASH_LEDS(true);
         hal.scheduler->delay(20);
 
         update();
         ins_gyro = get_gyro();
 
-        FLASH_LEDS(false);
+        // give leds a chance to update
-        hal.scheduler->delay(20);
+        AP_Notify::update();
     }
 
     // the strategy is to average 200 points over 1 second, then do it
@@ -184,12 +184,9 @@ AP_InertialSensor::_init_gyro(void (*flash_leds_cb)(bool on))
             update();
             ins_gyro = get_gyro();
             gyro_sum += ins_gyro;
-            if (i % 40 == 20) {
-                FLASH_LEDS(true);
-            } else if (i % 40 == 0) {
-                FLASH_LEDS(false);
-            }
             hal.scheduler->delay(5);
+            // give leds a chance to update
+            AP_Notify::update();
         }
         gyro_avg = gyro_sum / i;
 
@@ -203,7 +200,8 @@ AP_InertialSensor::_init_gyro(void (*flash_leds_cb)(bool on))
             // we want the average to be within 0.1 bit, which is 0.04 degrees/s
             last_average = (gyro_avg * 0.5) + (last_average * 0.5);
             _gyro_offset = last_average;
-
+            // stop flashing leds
+            AP_Notify::flags.initialising = false;
             // all done
             return;
         } else if (diff_norm < best_diff) {
@@ -213,6 +211,9 @@ AP_InertialSensor::_init_gyro(void (*flash_leds_cb)(bool on))
         last_average = gyro_avg;
     }
 
+    // stop flashing leds
+    AP_Notify::flags.initialising = false;
+
     // we've kept the user waiting long enough - use the best pair we
     // found so far
     hal.console->printf_P(PSTR("\ngyro did not converge: diff=%f dps\n"), ToDeg(best_diff));
@@ -245,6 +246,9 @@ AP_InertialSensor::_init_accel(void (*flash_leds_cb)(bool on))
 
     hal.console->printf_P(PSTR("Init Accel"));
 
+    // flash leds to tell user to keep the IMU still
+    AP_Notify::flags.initialising = true;
+
     // clear accelerometer offsets and scaling
     _accel_offset = Vector3f(0,0,0);
     _accel_scale = Vector3f(1,1,1);
@@ -276,16 +280,14 @@ AP_InertialSensor::_init_accel(void (*flash_leds_cb)(bool on))
             accel_offset = accel_offset * 0.9 + ins_accel * 0.1;
 
             // display some output to the user
-            if(flashcount == 5) {
-                hal.console->printf_P(PSTR("*"));
-                FLASH_LEDS(true);
-            }
-
             if(flashcount >= 10) {
+                hal.console->printf_P(PSTR("*"));
                 flashcount = 0;
-                FLASH_LEDS(false);
             }
             flashcount++;
+
+            // give leds a chance to update
+            AP_Notify::update();
         }
 
         // null gravity from the Z accel
@@ -301,6 +303,9 @@ AP_InertialSensor::_init_accel(void (*flash_leds_cb)(bool on))
     // set the global accel offsets
     _accel_offset = accel_offset;
 
+    // stop flashing the leds
+    AP_Notify::flags.initialising = false;
+
     hal.console->printf_P(PSTR(" "));
 
 }

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -10,6 +10,7 @@
 #include <stdint.h>
 #include <AP_HAL.h>
 #include <AP_Math.h>
+#include <AP_Notify.h>
 #include "AP_InertialSensor_UserInteract.h"
 /* AP_InertialSensor is an abstraction for gyro and accel measurements
  * which are correctly aligned to the body axes and scaled to SI units.