From 36327d56de92f6354764d4b8d10ad32b7b460d7f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 4 Oct 2019 18:08:35 +0900 Subject: [PATCH] Copter: reject reboot request from GCS if auto esc cal on next reboot this resolves an edge case in which the motors could spin up on the next reboot because the user didn't unplug the battery to reboot the flight controller --- ArduCopter/Copter.h | 9 +++++++++ ArduCopter/GCS_Mavlink.cpp | 12 ++++++++++++ ArduCopter/GCS_Mavlink.h | 2 +- ArduCopter/esc_calibration.cpp | 31 +++++++++++-------------------- 4 files changed, 33 insertions(+), 21 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 86a1cf09fa..1d6f207764 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -588,6 +588,15 @@ private: static const AP_Param::Info var_info[]; static const struct LogStructure log_structure[]; + // enum for ESC CALIBRATION + enum ESCCalibrationModes : uint8_t { + ESCCAL_NONE = 0, + ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH = 1, + ESCCAL_PASSTHROUGH_ALWAYS = 2, + ESCCAL_AUTO = 3, + ESCCAL_DISABLED = 9, + }; + enum Failsafe_Action { Failsafe_Action_None = 0, Failsafe_Action_Land = 1, diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 8866fd63a4..3cb091e112 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -558,6 +558,18 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_do_set_roi(const Location &roi_loc return MAV_RESULT_ACCEPTED; } +MAV_RESULT GCS_MAVLINK_Copter::handle_preflight_reboot(const mavlink_command_long_t &packet) +{ + // reject reboot if user has also specified they want the "Auto" ESC calibration on next reboot + if (copter.g.esc_calibrate == (uint8_t)Copter::ESCCalibrationModes::ESCCAL_AUTO) { + send_text(MAV_SEVERITY_CRITICAL, "Reboot rejected, ESC cal on reboot"); + return MAV_RESULT_FAILED; + } + + // call parent + return GCS_MAVLINK::handle_preflight_reboot(packet); +} + bool GCS_MAVLINK_Copter::set_home_to_current_location(bool lock) { return copter.set_home_to_current_location(lock); } diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h index 7b60a03947..abb4202987 100644 --- a/ArduCopter/GCS_Mavlink.h +++ b/ArduCopter/GCS_Mavlink.h @@ -29,7 +29,7 @@ protected: void send_position_target_local_ned() override; MAV_RESULT handle_command_do_set_roi(const Location &roi_loc) override; - + MAV_RESULT handle_preflight_reboot(const mavlink_command_long_t &packet) override; MAV_RESULT handle_command_mount(const mavlink_command_long_t &packet) override; MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet) override; MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet) override; diff --git a/ArduCopter/esc_calibration.cpp b/ArduCopter/esc_calibration.cpp index f39dd9adbf..152388d1ed 100644 --- a/ArduCopter/esc_calibration.cpp +++ b/ArduCopter/esc_calibration.cpp @@ -6,15 +6,6 @@ #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 Copter::esc_calibration_startup_check() { @@ -34,19 +25,19 @@ void Copter::esc_calibration_startup_check() // exit immediately if pre-arm rc checks fail if (!arming.rc_calibration_checks(true)) { // 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); + if ((g.esc_calibrate != ESCCalibrationModes::ESCCAL_NONE) && (g.esc_calibrate != ESCCalibrationModes::ESCCAL_DISABLED)) { + g.esc_calibrate.set_and_save(ESCCalibrationModes::ESCCAL_NONE); } return; } // check ESC parameter switch (g.esc_calibrate) { - case ESCCAL_NONE: + case ESCCalibrationModes::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); + g.esc_calibrate.set_and_save(ESCCalibrationModes::ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH); // send message to gcs gcs().send_text(MAV_SEVERITY_CRITICAL,"ESC calibration: Restart board"); // turn on esc calibration notification @@ -55,30 +46,30 @@ void Copter::esc_calibration_startup_check() while(1) { hal.scheduler->delay(5); } } break; - case ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH: + case ESCCalibrationModes::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: + case ESCCalibrationModes::ESCCAL_PASSTHROUGH_ALWAYS: // pass through pilot throttle to escs esc_calibration_passthrough(); break; - case ESCCAL_AUTO: + case ESCCalibrationModes::ESCCAL_AUTO: // perform automatic ESC calibration esc_calibration_auto(); break; - case ESCCAL_DISABLED: + case ESCCalibrationModes::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); + if (g.esc_calibrate != ESCCalibrationModes::ESCCAL_DISABLED) { + g.esc_calibrate.set_and_save(ESCCalibrationModes::ESCCAL_NONE); } #endif // FRAME_CONFIG != HELI_FRAME } @@ -189,4 +180,4 @@ void Copter::esc_calibration_setup() motors->armed(true); SRV_Channels::enable_by_mask(motors->get_motor_mask()); hal.util->set_soft_armed(true); -} \ No newline at end of file +}