diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 00b9b71b2f..586d06063b 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1188,7 +1188,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) if (packet.param1 == 1 || packet.param2 == 1 || packet.param3 == 1) { - ins.init_accel(flash_leds); + ins.init_accel(); ahrs.set_trim(Vector3f(0,0,0)); // clear out saved trim } if (packet.param4 == 1) { @@ -1198,7 +1198,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) float trim_roll, trim_pitch; // this blocks AP_InertialSensor_UserInteract_MAVLink interact(chan); - if(ins.calibrate_accel(flash_leds, &interact, trim_roll, trim_pitch)) { + if(ins.calibrate_accel(&interact, trim_roll, trim_pitch)) { // reset ahrs's trim to suggested values from calibration routine ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); } diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index 729ea62f54..0484911063 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -83,10 +83,9 @@ setup_accel_scale(uint8_t argc, const Menu::arg *argv) cliSerial->println_P(PSTR("Initialising gyros")); ahrs.init(); ins.init(AP_InertialSensor::COLD_START, - ins_sample_rate, - flash_leds); + ins_sample_rate); AP_InertialSensor_UserInteractStream interact(hal.console); - if(ins.calibrate_accel(flash_leds, &interact, trim_roll, trim_pitch)) { + if(ins.calibrate_accel(&interact, trim_roll, trim_pitch)) { // reset ahrs's trim to suggested values from calibration routine ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); } @@ -660,9 +659,8 @@ setup_accel(uint8_t argc, const Menu::arg *argv) { ahrs.init(); ins.init(AP_InertialSensor::COLD_START, - ins_sample_rate, - flash_leds); - ins.init_accel(flash_leds); + ins_sample_rate); + ins.init_accel(); ahrs.set_trim(Vector3f(0,0,0)); // clear out saved trim report_ins(); return(0); diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 789322ef87..185bf7af76 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -273,8 +273,7 @@ static void startup_ground(void) // Warm up and read Gyro offsets // ----------------------------- ins.init(AP_InertialSensor::COLD_START, - ins_sample_rate, - flash_leds); + ins_sample_rate); #if CLI_ENABLED == ENABLED report_ins(); #endif @@ -560,16 +559,6 @@ static void check_usb_mux(void) } } -/* - * called by gyro/accel init to flash LEDs so user - * has some mesmerising lights to watch while waiting - */ -void flash_leds(bool on) -{ - //digitalWrite(A_LED_PIN, on ? LED_OFF : LED_ON); - //digitalWrite(C_LED_PIN, on ? LED_ON : LED_OFF); -} - /* * Read Vcc vs 1.1v internal reference */ diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index 12f52117fd..c4c990700b 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -177,8 +177,7 @@ test_compass(uint8_t argc, const Menu::arg *argv) // we need the AHRS initialised for this test ins.init(AP_InertialSensor::COLD_START, - ins_sample_rate, - flash_leds); + ins_sample_rate); ahrs.reset(); int16_t counter = 0; float heading = 0; @@ -277,8 +276,7 @@ test_ins(uint8_t argc, const Menu::arg *argv) ahrs.init(); ins.init(AP_InertialSensor::COLD_START, - ins_sample_rate, - flash_leds); + ins_sample_rate); cliSerial->printf_P(PSTR("...done\n")); delay(50);