diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 385f8bb219..f3d02ce3b6 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -5,6 +5,7 @@ #include #include +#include extern const AP_HAL::HAL& hal; @@ -159,9 +160,6 @@ AP_InertialSensor::_init_gyro(void (*flash_leds_cb)(bool on)) update(); ins_gyro = get_gyro(); - - // give leds a chance to update - AP_Notify::update(); } // the strategy is to average 200 points over 1 second, then do it @@ -185,8 +183,6 @@ AP_InertialSensor::_init_gyro(void (*flash_leds_cb)(bool on)) ins_gyro = get_gyro(); gyro_sum += ins_gyro; hal.scheduler->delay(5); - // give leds a chance to update - AP_Notify::update(); } gyro_avg = gyro_sum / i; @@ -285,9 +281,6 @@ AP_InertialSensor::_init_accel(void (*flash_leds_cb)(bool on)) flashcount = 0; } flashcount++; - - // give leds a chance to update - AP_Notify::update(); } // null gravity from the Z accel diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index d8a324b80a..fb87056b12 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -10,7 +10,6 @@ #include #include #include -#include #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.