From c8572502aacd83fea16d47ccf8a42095df6069ec Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Fri, 13 Dec 2019 11:15:16 -0500 Subject: [PATCH] AC_InputManager: Change STAB_COL params to percent --- .../AC_InputManager/AC_InputManager_Heli.cpp | 121 +++++++++++------- .../AC_InputManager/AC_InputManager_Heli.h | 9 +- 2 files changed, 84 insertions(+), 46 deletions(-) diff --git a/libraries/AC_InputManager/AC_InputManager_Heli.cpp b/libraries/AC_InputManager/AC_InputManager_Heli.cpp index 7c9822d7b6..d88a7e18e2 100644 --- a/libraries/AC_InputManager/AC_InputManager_Heli.cpp +++ b/libraries/AC_InputManager/AC_InputManager_Heli.cpp @@ -1,6 +1,7 @@ #include "AC_InputManager_Heli.h" #include #include +#include extern const AP_HAL::HAL& hal; @@ -9,48 +10,50 @@ const AP_Param::GroupInfo AC_InputManager_Heli::var_info[] = { // parameters from parent vehicle AP_NESTEDGROUPINFO(AC_InputManager, 0), - // @Param: STAB_COL_1 - // @DisplayName: Stabilize Mode Collective Point 1 - // @Description: Helicopter's minimum collective pitch setting at zero throttle input in Stabilize mode - // @Range: 0 500 - // @Units: d% + // Indicies 1-4 (STAB_COL_1 thru STAB_COL_4) have been replaced. + + // @Param: ACRO_COL_EXP + // @DisplayName: Acro Mode Collective Expo + // @Description: Used to soften collective pitch inputs near center point in Acro mode. + // @Values: 0:Disabled,0.1:Very Low,0.2:Low,0.3:Medium,0.4:High,0.5:Very High + // @User: Advanced + AP_GROUPINFO("ACRO_COL_EXP", 5, AC_InputManager_Heli, _acro_col_expo, 0), + + // @Param: STAB_COL_MIN + // @DisplayName: Stabilize Collective Low + // @Description: Helicopter's minimum collective pitch setting at zero collective stick input in Stabilize mode. Set this as a percent of collective range given by H_COL_MAX minus H_COL_MIN. + // @Range: 0 50 + // @Units: % // @Increment: 1 // @User: Standard - AP_GROUPINFO("STAB_COL_1", 1, AC_InputManager_Heli, _heli_stab_col_min, AC_ATTITUDE_HELI_STAB_COLLECTIVE_MIN_DEFAULT), + AP_GROUPINFO("STB_COL_1", 6, AC_InputManager_Heli, _heli_stab_col_min, AC_ATTITUDE_HELI_STAB_COLLECTIVE_MIN_DEFAULT), - // @Param: STAB_COL_2 - // @DisplayName: Stabilize Mode Collective Point 2 - // @Description: Helicopter's collective pitch setting at mid-low throttle input in Stabilize mode - // @Range: 0 500 - // @Units: d% + // @Param: STAB_COL_LOW + // @DisplayName: Stabilize Collective Mid-Low + // @Description: Helicopter's collective pitch setting at mid-low (40%) collective stick input in Stabilize mode. Set this as a percent of collective range given by H_COL_MAX minus H_COL_MIN. + // @Range: 0 50 + // @Units: % // @Increment: 1 // @User: Standard - AP_GROUPINFO("STAB_COL_2", 2, AC_InputManager_Heli, _heli_stab_col_low, AC_ATTITUDE_HELI_STAB_COLLECTIVE_LOW_DEFAULT), + AP_GROUPINFO("STB_COL_2", 7, AC_InputManager_Heli, _heli_stab_col_low, AC_ATTITUDE_HELI_STAB_COLLECTIVE_LOW_DEFAULT), - // @Param: STAB_COL_3 - // @DisplayName: Stabilize Mode Collective Point 3 - // @Description: Helicopter's collective pitch setting at mid-high throttle input in Stabilize mode - // @Range: 500 1000 - // @Units: d% + // @Param: STAB_COL_HGH + // @DisplayName: Stabilize Collective Mid-High + // @Description: Helicopter's collective pitch setting at mid-high (60%) collective stick input in Stabilize mode. Set this as a percent of collective range given by H_COL_MAX minus H_COL_MIN. + // @Range: 50 100 + // @Units: % // @Increment: 1 // @User: Standard - AP_GROUPINFO("STAB_COL_3", 3, AC_InputManager_Heli, _heli_stab_col_high, AC_ATTITUDE_HELI_STAB_COLLECTIVE_HIGH_DEFAULT), + AP_GROUPINFO("STB_COL_3", 8, AC_InputManager_Heli, _heli_stab_col_high, AC_ATTITUDE_HELI_STAB_COLLECTIVE_HIGH_DEFAULT), - // @Param: STAB_COL_4 - // @DisplayName: Stabilize Mode Collective Point 4 - // @Description: Helicopter's maximum collective pitch setting at full throttle input in Stabilize mode - // @Range: 500 1000 - // @Units: d% + // @Param: STAB_COL_MAX + // @DisplayName: Stabilize Collective High + // @Description: Helicopter's maximum collective pitch setting at full collective stick input in Stabilize mode. Set this as a percent of collective range given by H_COL_MAX minus H_COL_MIN. + // @Range: 50 100 + // @Units: % // @Increment: 1 // @User: Standard - AP_GROUPINFO("STAB_COL_4", 4, AC_InputManager_Heli, _heli_stab_col_max, AC_ATTITUDE_HELI_STAB_COLLECTIVE_MAX_DEFAULT), - - // @Param: ACRO_COL_EXP - // @DisplayName: Acro Mode Collective Expo - // @Description: Used to soften collective pitch inputs near center point in Acro mode. - // @Values: 0:Disabled,0.1:Very Low,0.2:Low,0.3:Medium,0.4:High,0.5:Very High - // @User: Advanced - AP_GROUPINFO("ACRO_COL_EXP", 5, AC_InputManager_Heli, _acro_col_expo, 0), + AP_GROUPINFO("STB_COL_4", 9, AC_InputManager_Heli, _heli_stab_col_max, AC_ATTITUDE_HELI_STAB_COLLECTIVE_MAX_DEFAULT), AP_GROUPEND }; @@ -63,21 +66,21 @@ float AC_InputManager_Heli::get_pilot_desired_collective(int16_t control_in) // calculate stabilize collective value which scales pilot input to reduced collective range // code implements a 3-segment curve with knee points at 40% and 60% throttle input - if (control_in < 400){ - slope_low = _heli_stab_col_min / 1000.0f; - slope_high = _heli_stab_col_low / 1000.0f; + if (control_in < 400){ // control_in ranges from 0 to 1000 + slope_low = _heli_stab_col_min / 100.0f; + slope_high = _heli_stab_col_low / 100.0f; slope_range = 0.4f; slope_run = control_in / 1000.0f; - } else if(control_in <600){ - slope_low = _heli_stab_col_low / 1000.0f; - slope_high = _heli_stab_col_high / 1000.0f; + } else if(control_in <600){ // control_in ranges from 0 to 1000 + slope_low = _heli_stab_col_low / 100.0f; + slope_high = _heli_stab_col_high / 100.0f; slope_range = 0.2f; - slope_run = (control_in - 400) / 1000.0f; + slope_run = (control_in - 400) / 1000.0f; // control_in ranges from 0 to 1000 } else { - slope_low = _heli_stab_col_high / 1000.0f; - slope_high = _heli_stab_col_max / 1000.0f; + slope_low = _heli_stab_col_high / 100.0f; + slope_high = _heli_stab_col_max / 100.0f; slope_range = 0.4f; - slope_run = (control_in - 600) / 1000.0f; + slope_run = (control_in - 600) / 1000.0f; // control_in ranges from 0 to 1000 } scalar = (slope_high - slope_low)/slope_range; @@ -92,11 +95,11 @@ float AC_InputManager_Heli::get_pilot_desired_collective(int16_t control_in) } if (_acro_col_expo <= 0.0f) { - acro_col_out = control_in / 1000.0f; + acro_col_out = control_in / 1000.0f; // control_in ranges from 0 to 1000 } else { // expo variables float col_in, col_in3, col_out; - col_in = (float)(control_in-500)/500.0f; + col_in = (float)(control_in-500)/500.0f; // control_in ranges from 0 to 1000 col_in3 = col_in*col_in*col_in; col_out = (_acro_col_expo * col_in3) + ((1.0f-_acro_col_expo)*col_in); acro_col_out = 0.5f + col_out*0.5f; @@ -119,4 +122,36 @@ float AC_InputManager_Heli::get_pilot_desired_collective(int16_t control_in) return collective_out; } +// parameter_check - check if input manager specific parameters are sensible +bool AC_InputManager_Heli::parameter_check(char* fail_msg, uint8_t fail_msg_len) const +{ + + const struct StabCheck { + const char *name; + int16_t value; + } stab_checks[] = { + {"IM_STB_COL_1", _heli_stab_col_min }, + {"IM_STB_COL_2", _heli_stab_col_low }, + {"IM_STB_COL_3", _heli_stab_col_high }, + {"IM_STB_COL_4", _heli_stab_col_max }, + }; + + // check values are within valid range + for (uint8_t i=0; i 100)){ + hal.util->snprintf(fail_msg, fail_msg_len, "%s out of range", check.name); + return false; + } + } + // check values are in correct order + for (uint8_t i=1; i= stab_checks[i].value)){ + hal.util->snprintf(fail_msg, fail_msg_len, "%s must be < %s", stab_checks[i-1].name, stab_checks[i].name); + return false; + } + } + // all other cases parameters are OK + return true; +} diff --git a/libraries/AC_InputManager/AC_InputManager_Heli.h b/libraries/AC_InputManager/AC_InputManager_Heli.h index a4a7f4a03a..225c093c3d 100644 --- a/libraries/AC_InputManager/AC_InputManager_Heli.h +++ b/libraries/AC_InputManager/AC_InputManager_Heli.h @@ -8,9 +8,9 @@ #include "AC_InputManager.h" # define AC_ATTITUDE_HELI_STAB_COLLECTIVE_MIN_DEFAULT 0 -# define AC_ATTITUDE_HELI_STAB_COLLECTIVE_LOW_DEFAULT 400 -# define AC_ATTITUDE_HELI_STAB_COLLECTIVE_HIGH_DEFAULT 600 -# define AC_ATTITUDE_HELI_STAB_COLLECTIVE_MAX_DEFAULT 1000 +# define AC_ATTITUDE_HELI_STAB_COLLECTIVE_LOW_DEFAULT 40 +# define AC_ATTITUDE_HELI_STAB_COLLECTIVE_HIGH_DEFAULT 60 +# define AC_ATTITUDE_HELI_STAB_COLLECTIVE_MAX_DEFAULT 100 /// @class AP_InputManager_Heli /// @brief Class managing the pilot's control inputs for Conventional Helicopter @@ -37,6 +37,9 @@ public: // set_heli_stab_col_ramp - setter function void set_stab_col_ramp(float ramp) { _stab_col_ramp = constrain_float(ramp, 0.0, 1.0); } + // parameter_check - returns true if input manager specific parameters are sensible, used for pre-arm check + bool parameter_check(char* fail_msg, uint8_t fail_msg_len) const; + static const struct AP_Param::GroupInfo var_info[]; private: