From 58602bd1aefa959eaed4574e9360ca73ad2a658d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 19 Sep 2014 22:16:31 +0900 Subject: [PATCH] Copter: move esc calibration to separate file Restructured into case statement Replaced use of g.throttle_max with definition Added more comments Send message to ground station instead of printing on console (although probably both are unlikely to be read) --- ArduCopter/esc_calibration.pde | 90 ++++++++++++++++++++++++++++++++++ ArduCopter/radio.pde | 25 +--------- ArduCopter/setup.pde | 28 ----------- 3 files changed, 92 insertions(+), 51 deletions(-) create mode 100644 ArduCopter/esc_calibration.pde diff --git a/ArduCopter/esc_calibration.pde b/ArduCopter/esc_calibration.pde new file mode 100644 index 0000000000..11623303ad --- /dev/null +++ b/ArduCopter/esc_calibration.pde @@ -0,0 +1,90 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/***************************************************************************** +* esc_calibration.pde : functions to check and perform ESC calibration +*****************************************************************************/ + +#define ESC_CALIBRATION_HIGH_THROTTLE 950 + +// enum for ESC CALIBRATION +enum ESCCalibrationModes { + ESCCAL_NONE = 0, + ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH = 1, + ESCCAL_PASSTHROUGH_ALWAYS = 2 +}; + +// check if we should enter esc calibration mode +static void esc_calibration_startup_check() +{ + // exit immediately if pre-arm rc checks fail + pre_arm_rc_checks(); + if (!ap.pre_arm_rc_check) { + // clear esc flag for next time + if (g.esc_calibrate != ESCCAL_NONE) { + g.esc_calibrate.set_and_save(ESCCAL_NONE); + } + return; + } + + // check ESC parameter + switch (g.esc_calibrate) { + case ESCCAL_NONE: + // check if throttle is high + if (g.rc_3.control_in >= ESC_CALIBRATION_HIGH_THROTTLE) { + // we will enter esc_calibrate mode on next reboot + g.esc_calibrate.set_and_save(ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH); + // send message to gcs + gcs_send_text_P(SEVERITY_HIGH,PSTR("ESC Cal: restart board")); + // turn on esc calibration notification + AP_Notify::flags.esc_calibration = true; + // block until we restart + while(1) { delay(5); } + } + break; + case ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH: + // check if throttle is high + if (g.rc_3.control_in >= ESC_CALIBRATION_HIGH_THROTTLE) { + // pass through pilot throttle to escs + esc_calibration_passthrough(); + } + break; + case ESCCAL_PASSTHROUGH_ALWAYS: + // pass through pilot throttle to escs + esc_calibration_passthrough(); + break; + default: + // do nothing + break; + } + // clear esc flag for next time + g.esc_calibrate.set_and_save(ESCCAL_NONE); +} + +// esc_calibration_passthrough - pass through pilot throttle to escs +static void esc_calibration_passthrough() +{ + // clear esc flag for next time + g.esc_calibrate.set_and_save(ESCCAL_NONE); + + // reduce update rate to motors to 50Hz + motors.set_update_rate(50); + + // send message to GCS + gcs_send_text_P(SEVERITY_HIGH,PSTR("ESC Cal: passing pilot thr to ESCs")); + + while(1) { + // arm motors + motors.armed(true); + motors.enable(); + + // flash LEDS + AP_Notify::flags.esc_calibration = true; + + // read pilot input + read_radio(); + delay(10); + + // pass through to motors + motors.throttle_pass_through(g.rc_3.radio_in); + } +} diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index d3914d34ff..6a61d9dd96 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -56,29 +56,8 @@ static void init_rc_out() // we want the input to be scaled correctly g.rc_3.set_range_out(0,1000); - // full throttle means to enter ESC calibration - if(g.rc_3.control_in >= (g.throttle_max - 50) || (g.esc_calibrate == 2)) { - if(g.esc_calibrate == 0) { - // we will enter esc_calibrate mode on next reboot - g.esc_calibrate.set_and_save(1); - // display message on console - cliSerial->printf_P(PSTR("Entering ESC Calibration: please restart APM.\n")); - // turn on esc calibration notification - AP_Notify::flags.esc_calibration = true; - // block until we restart - while(1) { delay(5); } - }else{ - cliSerial->printf_P(PSTR("ESC Calibration active: passing throttle through to ESCs.\n")); - // clear esc flag - g.esc_calibrate.set_and_save(0); - // pass through user throttle to escs - init_esc(); - } - }else{ - // did we abort the calibration? - if(g.esc_calibrate == 1) - g.esc_calibrate.set_and_save(0); - } + // check if we should enter esc calibration mode + esc_calibration_startup_check(); // enable output to motors pre_arm_rc_checks(); diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index c1bc2c67a5..f1e2fb4804 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -522,34 +522,6 @@ static void print_enabled(bool b) cliSerial->print_P(PSTR("abled\n")); } - -static void -init_esc() -{ - // reduce update rate to motors to 50Hz - motors.set_update_rate(50); - - uint32_t last_print_ms = 0; - while(1) { - motors.armed(true); - motors.enable(); - read_radio(); - delay(10); - AP_Notify::flags.esc_calibration = true; - motors.throttle_pass_through(); - - uint32_t now = hal.scheduler->millis(); - if (now - last_print_ms > 1000) { - hal.console->printf_P(PSTR("ESC cal input: %u %u %u %u output: %u %u %u %u\n"), - (unsigned)hal.rcin->read(0), (unsigned)hal.rcin->read(1), - (unsigned)hal.rcin->read(2), (unsigned)hal.rcin->read(3), - (unsigned)hal.rcout->read(0), (unsigned)hal.rcout->read(1), - (unsigned)hal.rcout->read(2), (unsigned)hal.rcout->read(3)); - last_print_ms = now; - } - } -} - static void report_version() { cliSerial->printf_P(PSTR("FW Ver: %d\n"),(int)g.k_format_version);