diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 8c9a543466..f330191db0 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -608,6 +608,12 @@ private: RELEASE_GRIPPER = (1<<5), // 32 }; + + enum class FlightOptions { + DISABLE_THRUST_LOSS_CHECK = (1<<0), // 1 + DISABLE_YAW_IMBALANCE_WARNING = (1<<1), // 2 + }; + static constexpr int8_t _failsafe_priorities[] = { Failsafe_Action_Terminate, Failsafe_Action_Land, diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 712ceae844..ae79aedb28 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1045,6 +1045,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { AP_GROUPINFO("RTL_OPTIONS", 43, ParametersG2, rtl_options, 0), #endif + // @Param: FLIGHT_OPTIONS + // @DisplayName: Flight mode options + // @Description: Flight mode specific options + // @Bitmask: 0:Disable thrust loss check, 1:Disable yaw imbalance warning + // @User: Advanced + AP_GROUPINFO("FLIGHT_OPTIONS", 44, ParametersG2, flight_options, 0), + AP_GROUPEND }; diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index cda7d2b5a3..7f3afb1141 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -642,6 +642,8 @@ public: AP_Int32 rtl_options; #endif + AP_Int32 flight_options; + }; extern const AP_Param::Info var_info[]; diff --git a/ArduCopter/crash_check.cpp b/ArduCopter/crash_check.cpp index 217a62b0c8..b68890c3d7 100644 --- a/ArduCopter/crash_check.cpp +++ b/ArduCopter/crash_check.cpp @@ -98,6 +98,11 @@ void Copter::thrust_loss_check() { static uint16_t thrust_loss_counter; // number of iterations vehicle may have been crashed + // no-op if suppresed by flight options param + if ((copter.g2.flight_options & uint32_t(FlightOptions::DISABLE_THRUST_LOSS_CHECK)) != 0) { + return; + } + // exit immediately if thrust boost is already engaged if (motors->get_thrust_boost()) { return; @@ -167,6 +172,11 @@ void Copter::thrust_loss_check() // check for a large yaw imbalance, could be due to badly calibrated ESC or misaligned motors void Copter::yaw_imbalance_check() { + // no-op if suppresed by flight options param + if ((copter.g2.flight_options & uint32_t(FlightOptions::DISABLE_YAW_IMBALANCE_WARNING)) != 0) { + return; + } + // If I is disabled it is unlikely that the issue is not obvious if (!is_positive(attitude_control->get_rate_yaw_pid().kI())) { return;