From 3934281b43266dcfa69a0dab5d20d667e9a6338e Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Thu, 23 Mar 2017 00:37:45 -0400 Subject: [PATCH] Sub: Remove esc calibration --- ArduSub/Parameters.cpp | 7 -- ArduSub/Parameters.h | 4 +- ArduSub/Sub.h | 3 - ArduSub/esc_calibration.cpp | 143 ------------------------------------ ArduSub/radio.cpp | 3 - 5 files changed, 1 insertion(+), 159 deletions(-) delete mode 100644 ArduSub/esc_calibration.cpp diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 28446e2470..7ed4ba7329 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -279,13 +279,6 @@ const AP_Param::Info Sub::var_info[] = { // @User: Standard GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK), - // @Param: ESC_CALIBRATION - // @DisplayName: ESC Calibration - // @Description: Controls whether ArduSub will enter ESC calibration on the next restart. Do not adjust this parameter manually. - // @User: Advanced - // @Values: 0:Normal Start-up, 1:Start-up in ESC Calibration mode if throttle high, 2:Start-up in ESC Calibration mode regardless of throttle, 9:Disabled - GSCALAR(esc_calibrate, "ESC_CALIBRATION", 0), - // @Param: DISARM_DELAY // @DisplayName: Disarm delay // @Description: Delay before automatic disarm in seconds. A value of zero disables auto disarm. diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index ea85be32be..f87ebed76a 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -197,8 +197,7 @@ public: k_param_compass_enabled, k_param_surface_depth, k_param_rc_speed, // Main output pwm frequency - k_param_esc_calibrate, // Boot-time ESC calibration behavior - k_param_gcs_pid_mask, + k_param_gcs_pid_mask = 178, k_param_throttle_filt, k_param_throttle_deadzone, // Used in auto-throttle modes k_param_disarm_delay, @@ -283,7 +282,6 @@ public: // Misc // AP_Int32 log_bitmask; - AP_Int8 esc_calibrate; AP_Int8 disarm_delay; diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index f8353bbeae..b8309bf676 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -623,9 +623,6 @@ private: bool ekf_over_threshold(); void failsafe_ekf_event(); void failsafe_ekf_off_event(void); - void esc_calibration_startup_check(); - void esc_calibration_passthrough(); - void esc_calibration_auto(); bool should_disarm_on_failsafe(); void failsafe_battery_event(void); void failsafe_gcs_check(); diff --git a/ArduSub/esc_calibration.cpp b/ArduSub/esc_calibration.cpp deleted file mode 100644 index 294e30915e..0000000000 --- a/ArduSub/esc_calibration.cpp +++ /dev/null @@ -1,143 +0,0 @@ -#include "Sub.h" - -/***************************************************************************** -* 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, - ESCCAL_AUTO = 3, - ESCCAL_DISABLED = 9, -}; - -// check if we should enter esc calibration mode -void Sub::esc_calibration_startup_check() -{ - // exit immediately if pre-arm rc checks fail - if (!arming.rc_check()) { - // clear esc flag for next time - if ((g.esc_calibrate != ESCCAL_NONE) && (g.esc_calibrate != ESCCAL_DISABLED)) { - 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 (channel_throttle->get_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(MAV_SEVERITY_CRITICAL,"ESC calibration: Restart board"); - // turn on esc calibration notification - AP_Notify::flags.esc_calibration = true; - // block until we restart - while (1) { - hal.scheduler->delay(5); - } - } - break; - case ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH: - // check if throttle is high - if (channel_throttle->get_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; - case ESCCAL_AUTO: - // perform automatic ESC calibration - esc_calibration_auto(); - break; - case ESCCAL_DISABLED: - default: - // do nothing - break; - } - - // clear esc flag for next time - if (g.esc_calibrate != ESCCAL_DISABLED) { - g.esc_calibrate.set_and_save(ESCCAL_NONE); - } -} - -// esc_calibration_passthrough - pass through pilot throttle to escs -void Sub::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(MAV_SEVERITY_INFO,"ESC calibration: Passing pilot throttle to ESCs"); - - while (1) { - // arm motors - motors.armed(true); - motors.enable(); - - // flash LEDS - AP_Notify::flags.esc_calibration = true; - - // pass through to motors - motors.set_throttle_passthrough_for_esc_calibration(channel_throttle->get_control_in() / 1000.0f); - } -} - -// esc_calibration_auto - calibrate the ESCs automatically using a timer and no pilot input -void Sub::esc_calibration_auto() -{ - bool printed_msg = false; - - // reduce update rate to motors to 50Hz - motors.set_update_rate(50); - - // send message to GCS - gcs_send_text(MAV_SEVERITY_INFO,"ESC calibration: Auto calibration"); - - // arm and enable motors - motors.armed(true); - motors.enable(); - - // flash LEDS - AP_Notify::flags.esc_calibration = true; - - // raise throttle to maximum - hal.scheduler->delay(10); - motors.set_throttle_passthrough_for_esc_calibration(1.0f); - - // wait for safety switch to be pressed - while (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) { - if (!printed_msg) { - gcs_send_text(MAV_SEVERITY_INFO,"ESC calibration: Push safety switch"); - printed_msg = true; - } - hal.scheduler->delay(10); - } - - // delay for 5 seconds - hal.scheduler->delay(5000); - - // reduce throttle to minimum - motors.set_throttle_passthrough_for_esc_calibration(0.0f); - - // clear esc parameter - g.esc_calibrate.set_and_save(ESCCAL_NONE); - - // block until we restart - while (1) { - hal.scheduler->delay(5); - } -} diff --git a/ArduSub/radio.cpp b/ArduSub/radio.cpp index a3ae9d266f..de884925c1 100644 --- a/ArduSub/radio.cpp +++ b/ArduSub/radio.cpp @@ -67,9 +67,6 @@ void Sub::init_rc_out() motors.init((AP_Motors::motor_frame_class)g.frame_configuration.get(), (AP_Motors::motor_frame_type)0); motors.set_throttle_range(channel_throttle->get_radio_min(), channel_throttle->get_radio_max()); - // check if we should enter esc calibration mode - esc_calibration_startup_check(); - // enable output to motors if (arming.rc_check()) { enable_motor_output();