zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AC_InputManager: Change STAB_COL params to percent

zr-v5.1
bnsgeyer 5 years ago committed by Randy Mackay
parent
98ea8e0627
commit
d5fed8ee4c
2 changed files with 84 additions and 46 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 121
      libraries/AC_InputManager/AC_InputManager_Heli.cpp
  2. 9
      libraries/AC_InputManager/AC_InputManager_Heli.h

121
libraries/AC_InputManager/AC_InputManager_Heli.cpp
Unescape View File

@ -1,6 +1,7 @@ @@ -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[] = { @@ -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) @@ -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) @@ -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) @@ -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<ARRAY_SIZE(stab_checks); i++) {
const StabCheck check = stab_checks[i];
if ((check.value < 0) || (check.value > 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<ARRAY_SIZE(stab_checks); i++) {
if ((stab_checks[i-1].value >= 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;
}

9
libraries/AC_InputManager/AC_InputManager_Heli.h
Unescape View File

@ -8,9 +8,9 @@ @@ -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: @@ -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:

Write Preview
Loading…
Cancel
Save