diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index f6bdbe811c..1257304384 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -1181,7 +1181,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) if (packet.param5 == 1) { float trim_roll, trim_pitch; 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/ArduPlane/config.h b/ArduPlane/config.h index 41fefd285c..32c3d916b7 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -89,22 +89,12 @@ // main board differences // #if CONFIG_HAL_BOARD == HAL_BOARD_APM1 - # define A_LED_PIN 37 - # define B_LED_PIN 36 - # define C_LED_PIN 35 - # define LED_ON HIGH - # define LED_OFF LOW # define BATTERY_VOLT_PIN 0 // Battery voltage on A0 # define BATTERY_CURR_PIN 1 // Battery current on A1 # define CONFIG_INS_TYPE CONFIG_INS_OILPAN # define CONFIG_BARO AP_BARO_BMP085 # define CONFIG_COMPASS AP_COMPASS_HMC5843 #elif CONFIG_HAL_BOARD == HAL_BOARD_APM2 - # define A_LED_PIN 27 - # define B_LED_PIN 26 - # define C_LED_PIN 25 - # define LED_ON LOW - # define LED_OFF HIGH # define BATTERY_VOLT_PIN 1 // Battery voltage on A1 # define BATTERY_CURR_PIN 2 // Battery current on A2 # define CONFIG_INS_TYPE CONFIG_INS_MPU6000 @@ -116,22 +106,12 @@ # endif # define CONFIG_COMPASS AP_COMPASS_HMC5843 #elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL - # define A_LED_PIN 27 - # define B_LED_PIN 26 - # define C_LED_PIN 25 - # define LED_ON LOW - # define LED_OFF HIGH # define BATTERY_VOLT_PIN 1 // Battery voltage on A1 # define BATTERY_CURR_PIN 2 // Battery current on A2 # define CONFIG_INS_TYPE CONFIG_INS_STUB # define CONFIG_BARO AP_BARO_HIL # define CONFIG_COMPASS AP_COMPASS_HIL #elif CONFIG_HAL_BOARD == HAL_BOARD_PX4 - # define A_LED_PIN 27 - # define B_LED_PIN 26 - # define C_LED_PIN 25 - # define LED_ON LOW - # define LED_OFF HIGH # define BATTERY_VOLT_PIN -1 # define BATTERY_CURR_PIN -1 # define CONFIG_INS_TYPE CONFIG_INS_PX4 diff --git a/ArduPlane/setup.pde b/ArduPlane/setup.pde index 61c9a984c0..8f4ed29ed3 100644 --- a/ArduPlane/setup.pde +++ b/ArduPlane/setup.pde @@ -397,11 +397,9 @@ setup_accel_scale(uint8_t argc, const Menu::arg *argv) ahrs.set_fly_forward(true); ahrs.set_wind_estimation(true); - ins.init(AP_InertialSensor::COLD_START, - ins_sample_rate, - flash_leds); + ins.init(AP_InertialSensor::COLD_START, ins_sample_rate); AP_InertialSensor_UserInteractStream interact(hal.console); - bool success = ins.calibrate_accel(flash_leds, &interact, trim_roll, trim_pitch); + bool success = ins.calibrate_accel(&interact, trim_roll, trim_pitch); if (success) { // reset ahrs's trim to suggested values from calibration routine ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 484a86c9e8..cb8cfcadb9 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -170,9 +170,6 @@ static void init_ardupilot() init_rc_in(); // sets up rc channels from radio init_rc_out(); // sets up the timer libs - pinMode(C_LED_PIN, OUTPUT); // GPS status LED - pinMode(A_LED_PIN, OUTPUT); // GPS status LED - pinMode(B_LED_PIN, OUTPUT); // GPS status LED relay.init(); #if FENCE_TRIGGERED_PIN > 0 @@ -439,11 +436,9 @@ static void startup_INS_ground(bool do_accel_init) ahrs.set_fly_forward(true); ahrs.set_wind_estimation(true); - ins.init(style, - ins_sample_rate, - flash_leds); + ins.init(style, ins_sample_rate); if (do_accel_init) { - ins.init_accel(flash_leds); + ins.init_accel(); ahrs.set_trim(Vector3f(0, 0, 0)); } ahrs.reset(); @@ -459,10 +454,6 @@ static void startup_INS_ground(bool do_accel_init) } else { gcs_send_text_P(SEVERITY_LOW,PSTR("NO airspeed")); } - - digitalWrite(B_LED_PIN, LED_ON); // Set LED B high to indicate INS ready - digitalWrite(A_LED_PIN, LED_OFF); - digitalWrite(C_LED_PIN, LED_OFF); } // updates the status of the notify objects @@ -520,16 +511,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/ArduPlane/test.pde b/ArduPlane/test.pde index 3de2f4baf0..96126917f9 100644 --- a/ArduPlane/test.pde +++ b/ArduPlane/test.pde @@ -469,8 +469,7 @@ test_ins(uint8_t argc, const Menu::arg *argv) ahrs.set_wind_estimation(true); ins.init(AP_InertialSensor::COLD_START, - ins_sample_rate, - flash_leds); + ins_sample_rate); ahrs.reset(); print_hit_enter(); @@ -536,8 +535,7 @@ test_mag(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(); uint16_t counter = 0; @@ -663,15 +661,11 @@ test_rawgps(uint8_t argc, const Menu::arg *argv) while(1) { // Blink Yellow LED if we are sending data to GPS if (hal.uartC->available()) { - digitalWrite(B_LED_PIN, LED_ON); hal.uartB->write(hal.uartC->read()); - digitalWrite(B_LED_PIN, LED_OFF); } // Blink Red LED if we are receiving data from GPS if (hal.uartB->available()) { - digitalWrite(C_LED_PIN, LED_ON); hal.uartC->write(hal.uartB->read()); - digitalWrite(C_LED_PIN, LED_OFF); } if(cliSerial->available() > 0) { return (0);