diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index f3d02ce3b6..54761dbc04 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -10,8 +10,6 @@ extern const AP_HAL::HAL& hal; -#define FLASH_LEDS(on) do { if (flash_leds_cb != NULL) flash_leds_cb(on); } while (0) - #define SAMPLE_UNIT 1 // Class level parameters @@ -102,8 +100,7 @@ AP_InertialSensor::AP_InertialSensor() : void AP_InertialSensor::init( Start_style style, - Sample_rate sample_rate, - void (*flash_leds_cb)(bool on)) + Sample_rate sample_rate) { _product_id = _init_sensor(sample_rate); @@ -116,7 +113,7 @@ AP_InertialSensor::init( Start_style style, if (WARM_START != style) { // do cold-start calibration for gyro only - _init_gyro(flash_leds_cb); + _init_gyro(); } } @@ -130,16 +127,16 @@ void AP_InertialSensor::_save_parameters() } void -AP_InertialSensor::init_gyro(void (*flash_leds_cb)(bool on)) +AP_InertialSensor::init_gyro() { - _init_gyro(flash_leds_cb); + _init_gyro(); // save calibration _save_parameters(); } void -AP_InertialSensor::_init_gyro(void (*flash_leds_cb)(bool on)) +AP_InertialSensor::_init_gyro() { Vector3f last_average, best_avg; Vector3f ins_gyro; @@ -219,16 +216,16 @@ AP_InertialSensor::_init_gyro(void (*flash_leds_cb)(bool on)) void -AP_InertialSensor::init_accel(void (*flash_leds_cb)(bool on)) +AP_InertialSensor::init_accel() { - _init_accel(flash_leds_cb); + _init_accel(); // save calibration _save_parameters(); } void -AP_InertialSensor::_init_accel(void (*flash_leds_cb)(bool on)) +AP_InertialSensor::_init_accel() { int8_t flashcount = 0; Vector3f ins_accel; @@ -310,10 +307,9 @@ AP_InertialSensor::_init_accel(void (*flash_leds_cb)(bool on)) // http://chionophilous.wordpress.com/2011/10/24/accelerometer-calibration-iv-1-implementing-gauss-newton-on-an-atmega/ // original sketch available at // http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde -bool AP_InertialSensor::calibrate_accel(void (*flash_leds_cb)(bool on), - AP_InertialSensor_UserInteract* interact, - float &trim_roll, - float &trim_pitch) +bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact, + float &trim_roll, + float &trim_pitch) { Vector3f samples[6]; Vector3f new_offsets; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index ff0f973c3e..58a9fe6b88 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -49,21 +49,19 @@ public: /// @param style The initialisation startup style. /// virtual void init( Start_style style, - Sample_rate sample_rate, - void (*flash_leds_cb)(bool on)); + Sample_rate sample_rate); /// 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(void (*flash_leds_cb)(bool on)); + virtual void init_accel(); #if !defined( __AVR_ATmega1280__ ) // perform accelerometer calibration including providing user instructions // and feedback - virtual bool calibrate_accel(void (*flash_leds_cb)(bool on), - AP_InertialSensor_UserInteract *interact, + virtual bool calibrate_accel(AP_InertialSensor_UserInteract *interact, float& trim_roll, float& trim_pitch); #endif @@ -79,7 +77,7 @@ public: /// @note This should not be called unless ::init has previously /// been called, as ::init may perform other work /// - virtual void init_gyro(void (*flash_leds_cb)(bool on)); + virtual void init_gyro(void); /// Fetch the current gyro values /// @@ -144,9 +142,9 @@ protected: virtual uint16_t _init_sensor( Sample_rate sample_rate ) = 0; // no-save implementations of accel and gyro initialisation routines - virtual void _init_accel(void (*flash_leds_cb)(bool on) = NULL); + virtual void _init_accel(); - virtual void _init_gyro(void (*flash_leds_cb)(bool on) = NULL); + virtual void _init_gyro(); #if !defined( __AVR_ATmega1280__ ) // Calibration routines borrowed from Rolfe Schmidt diff --git a/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde b/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde index 926fa52954..0029a99da0 100644 --- a/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde +++ b/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde @@ -18,29 +18,13 @@ #include #include -#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 -#define A_LED_PIN 27 -#define C_LED_PIN 25 -#else -#define A_LED_PIN 37 -#define C_LED_PIN 35 -#endif - const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; AP_InertialSensor_MPU6000 ins; -static void flash_leds(bool on) { - hal.gpio->write(A_LED_PIN, on); - hal.gpio->write(C_LED_PIN, ~on); -} - void setup(void) { hal.console->println("AP_InertialSensor startup..."); - hal.gpio->pinMode(A_LED_PIN, GPIO_OUTPUT); - hal.gpio->pinMode(C_LED_PIN, GPIO_OUTPUT); - #if CONFIG_HAL_BOARD == HAL_BOARD_APM2 // we need to stop the barometer from holding the SPI bus hal.gpio->pinMode(40, GPIO_OUTPUT); @@ -162,7 +146,7 @@ void run_level() } // run accel level - ins.init_accel(flash_leds); + ins.init_accel(); // display results display_offsets_and_scaling(); diff --git a/libraries/AP_InertialSensor/examples/OilPan/OilPan.pde b/libraries/AP_InertialSensor/examples/OilPan/OilPan.pde index e9f70d4605..3735a79853 100644 --- a/libraries/AP_InertialSensor/examples/OilPan/OilPan.pde +++ b/libraries/AP_InertialSensor/examples/OilPan/OilPan.pde @@ -18,9 +18,6 @@ #include #include -#define A_LED_PIN 37 -#define C_LED_PIN 35 - const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; #if CONFIG_HAL_BOARD == HAL_BOARD_APM1 @@ -28,18 +25,10 @@ const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; static AP_ADC_ADS7844 adc; AP_InertialSensor_Oilpan ins( &adc ); -static void flash_leds(bool on) { - hal.gpio->write(A_LED_PIN, on); - hal.gpio->write(C_LED_PIN, ~on); -} - void setup(void) { hal.console->println("AP_InertialSensor startup..."); - hal.gpio->pinMode(A_LED_PIN, GPIO_OUTPUT); - hal.gpio->pinMode(C_LED_PIN, GPIO_OUTPUT); - #if CONFIG_HAL_BOARD == HAL_BOARD_APM2 // we need to stop the barometer from holding the SPI bus hal.gpio->pinMode(40, GPIO_OUTPUT); @@ -161,7 +150,7 @@ void run_level() } // run accel level - ins.init_accel(flash_leds); + ins.init_accel(); // display results display_offsets_and_scaling(); diff --git a/libraries/AP_InertialSensor/examples/PX4/PX4.pde b/libraries/AP_InertialSensor/examples/PX4/PX4.pde index 708e60c9f3..d77bdff233 100644 --- a/libraries/AP_InertialSensor/examples/PX4/PX4.pde +++ b/libraries/AP_InertialSensor/examples/PX4/PX4.pde @@ -120,10 +120,6 @@ void display_offsets_and_scaling() gyro_offsets.z); } -static void flash_leds(bool on) { - // no LEDs yet on PX4 -} - void run_level() { // clear off any input in the buffer @@ -143,7 +139,7 @@ void run_level() } // run accel level - ins.init_accel(flash_leds); + ins.init_accel(); // display results display_offsets_and_scaling();