Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Tradheli- make input manager stabilize collective parameters percent #12860

Open
wants to merge 4 commits into
base: master
from
Open
Changes from all commits
Commits
File filter...
Filter file types
Jump to…
Jump to file or symbol
Failed to load files and symbols.

Always

Just for now

@@ -195,6 +195,13 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Heli motors checks failed");
return false;
}

// check input mangager parameters
if (!copter.input_manager.parameter_check(display_failure)) {
check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Heli Stab Col checks failed");
return false;
}

// Inverted flight feature disabled for Heli Single and Dual frames
if (copter.g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI_QUAD &&
rc().find_channel_for_option(RC_Channel::aux_func_t::INVERTED) != nullptr) {
@@ -1537,6 +1537,20 @@ void Copter::convert_tradheli_parameters(void)
AP_Param::set_and_save_by_name("H_TAIL_SPEED", tailspeed_pct );
}

// table of stabilize collective parameters to be converted with scaling
const AP_Param::ConversionInfo collhelipct_conversion_info[] = {
{ Parameters::k_param_input_manager, 1, AP_PARAM_INT16, "IM_STB_COL_PCT_1" },
{ Parameters::k_param_input_manager, 2, AP_PARAM_INT16, "IM_STB_COL_PCT_2" },
{ Parameters::k_param_input_manager, 3, AP_PARAM_INT16, "IM_STB_COL_PCT_3" },
{ Parameters::k_param_input_manager, 4, AP_PARAM_INT16, "IM_STB_COL_PCT_4" },
};

// convert stabilize collective parameters with scaling
table_size = ARRAY_SIZE(collhelipct_conversion_info);
for (uint8_t i=0; i<table_size; i++) {
AP_Param::convert_old_parameter(&collhelipct_conversion_info[i], 0.1f);
}

}
#endif

@@ -75,9 +75,8 @@ RC8_MIN 1000.000000
RC8_TRIM 1500.000000
WP_YAW_BEHAVIOR 2
IM_ACRO_COL_EXP 0.2
IM_STAB_COL_1 400
IM_STAB_COL_2 600
IM_STAB_COL_3 700
IM_STAB_COL_4 900
ATC_PIRO_COMP 1
IM_STB_COL_PCT_1 40
IM_STB_COL_PCT_2 60
IM_STB_COL_PCT_3 70
IM_STB_COL_PCT_4 90

@@ -1,6 +1,7 @@
#include "AC_InputManager_Heli.h"
#include <AP_Math/AP_Math.h>
#include <AP_HAL/AP_HAL.h>
#include <GCS_MAVLink/GCS.h>

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
// 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: STB_COL_PCT_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%
// @Description: Helicopter's minimum collective pitch setting at zero throttle 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_PCT_1", 6, AC_InputManager_Heli, _heli_stab_col_min, AC_ATTITUDE_HELI_STAB_COLLECTIVE_MIN_DEFAULT),

// @Param: STAB_COL_2
// @Param: STB_COL_PCT_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%
// @Description: Helicopter's collective pitch setting at mid-low throttle 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_PCT_2", 7, AC_InputManager_Heli, _heli_stab_col_low, AC_ATTITUDE_HELI_STAB_COLLECTIVE_LOW_DEFAULT),

// @Param: STAB_COL_3
// @Param: STB_COL_PCT_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%
// @Description: Helicopter's collective pitch setting at mid-high throttle 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_PCT_3", 8, AC_InputManager_Heli, _heli_stab_col_high, AC_ATTITUDE_HELI_STAB_COLLECTIVE_HIGH_DEFAULT),

// @Param: STAB_COL_4
// @Param: STB_COL_PCT_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%
// @Description: Helicopter's maximum collective pitch setting at full throttle 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_PCT_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,42 @@ 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(bool display_msg) const

This comment has been minimized.

{
// returns false if STAB_COL_1 is outside of range
if ((_heli_stab_col_min < 0) || (_heli_stab_col_min > 100)){
if (display_msg) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: IM_STAB_PCT_COL_1 out of range");
}
return false;
}

// returns false if STAB_COL_1 is outside of range
if ((_heli_stab_col_low < 0) || (_heli_stab_col_low > 100)){
if (display_msg) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: IM_STAB_PCT_COL_2 out of range");
}
return false;
}

// returns false if STAB_COL_1 is outside of range
if ((_heli_stab_col_high < 0) || (_heli_stab_col_high > 100)){
if (display_msg) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: IM_STB_PCT_COL_3 out of range");
}
return false;
}

// returns false if STAB_COL_1 is outside of range
if ((_heli_stab_col_max < 0) || (_heli_stab_col_max > 100)){
if (display_msg) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: IM_STB_PCT_COL_4 out of range");
}
return false;
}

// all other cases parameters are OK
return true;
}

@@ -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 @@ class AC_InputManager_Heli : public AC_InputManager {
// 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(bool display_msg) const;

static const struct AP_Param::GroupInfo var_info[];

private:
ProTip! Use n and p to navigate between commits in a pull request.
You can’t perform that action at this time.