diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index f494f71f89..668d896639 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -388,17 +388,6 @@ AP_InertialSensor::_detect_backends(void) _product_id.set(_backends[0]->product_id()); } -void -AP_InertialSensor::init_accel() -{ - _init_accel(); - - // save calibration - _save_parameters(); - - check_3D_calibration(); -} - #if !defined( __AVR_ATmega1280__ ) // calibrate_accel - perform accelerometer calibration including providing user // instructions and feedback Gauss-Newton accel calibration routines borrowed @@ -631,117 +620,6 @@ bool AP_InertialSensor::get_accel_health_all(void) const return (get_accel_count() > 0); } -void -AP_InertialSensor::_init_accel() -{ - uint8_t num_accels = min(get_accel_count(), INS_MAX_INSTANCES); - uint8_t flashcount = 0; - Vector3f prev[INS_MAX_INSTANCES]; - Vector3f accel_offset[INS_MAX_INSTANCES]; - float total_change[INS_MAX_INSTANCES]; - float max_offset[INS_MAX_INSTANCES]; - - memset(max_offset, 0, sizeof(max_offset)); - memset(total_change, 0, sizeof(total_change)); - - // exit immediately if calibration is already in progress - if (_calibrating) { - return; - } - - // record we are calibrating - _calibrating = true; - - // flash leds to tell user to keep the IMU still - AP_Notify::flags.initialising = true; - - // cold start - hal.scheduler->delay(100); - - hal.console->print_P(PSTR("Init Accel")); - - // clear accelerometer offsets and scaling - for (uint8_t k=0; kdelay(20); - update(); - - // low pass filter the offsets - for (uint8_t k=0; k= 10) { - hal.console->print_P(PSTR("*")); - flashcount = 0; - } - flashcount++; - } - - for (uint8_t k=0; k accel_offset[k].y) ? accel_offset[k].x : accel_offset[k].y; - max_offset[k] = (max_offset[k] > accel_offset[k].z) ? max_offset[k] : accel_offset[k].z; - } - - uint8_t num_converged = 0; - for (uint8_t k=0; kdelay(500); - } - - // set the global accel offsets - for (uint8_t k=0; kprint_P(PSTR(" ")); - -} - void AP_InertialSensor::_init_gyro() { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 033efc17ac..d1f3a49ba0 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -70,14 +70,6 @@ public: void init( Start_style style, 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. - /// - void init_accel(); - - /// Register a new gyro/accel driver, allocating an instance /// number uint8_t register_gyro(void); @@ -218,8 +210,7 @@ private: void _add_backend(AP_InertialSensor_Backend *(detect)(AP_InertialSensor &)); void _detect_backends(void); - // accel and gyro initialisation - void _init_accel(); + // gyro initialisation void _init_gyro(); #if !defined( __AVR_ATmega1280__ ) diff --git a/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.pde b/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.pde index 2c97524bd8..5253ec436b 100644 --- a/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.pde +++ b/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.pde @@ -97,11 +97,6 @@ void loop(void) display_offsets_and_scaling(); } - if( user_input == 'l' || user_input == 'L' ) { - run_level(); - display_offsets_and_scaling(); - } - if( user_input == 't' || user_input == 'T' ) { run_test(); } @@ -153,31 +148,6 @@ void display_offsets_and_scaling() gyro_offsets.z); } -void run_level() -{ - // clear off any input in the buffer - while( hal.console->available() ) { - hal.console->read(); - } - - // display message to user - hal.console->print("Place APM on a level surface and press any key..\n"); - - // wait for user input - while( !hal.console->available() ) { - hal.scheduler->delay(20); - } - while( hal.console->available() ) { - hal.console->read(); - } - - // run accel level - ins.init_accel(); - - // display results - display_offsets_and_scaling(); -} - void run_test() { Vector3f accel;