Copter: add FLIGHT_OPTIONS param and options bits to disable thrust loss and yaw imbalance warnings

This commit is contained in:
Iampete1 2021-02-17 11:19:22 +00:00 committed by Randy Mackay
parent 447af29ef1
commit 2e9c11fbdf
4 changed files with 25 additions and 0 deletions

View File

@ -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,

View File

@ -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
};

View File

@ -642,6 +642,8 @@ public:
AP_Int32 rtl_options;
#endif
AP_Int32 flight_options;
};
extern const AP_Param::Info var_info[];

View File

@ -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;