Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_InputManager: Change STAB_COL params to percent

Browse Source
This commit is contained in:
bnsgeyer 2019-12-13 11:15:16 -05:00 committed by Randy Mackay
parent 98ea8e0627
commit d5fed8ee4c
2 changed files with 88 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

129
libraries/AC_InputManager/AC_InputManager_Heli.cpp
View File

@ -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,41 +10,7 @@ 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%
// @Increment: 1
// @User: Standard
AP_GROUPINFO("STAB_COL_1", 1, 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%
// @Increment: 1
// @User: Standard
AP_GROUPINFO("STAB_COL_2", 2, 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%
// @Increment: 1
// @User: Standard
AP_GROUPINFO("STAB_COL_3", 3, 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%
// @Increment: 1
// @User: Standard
AP_GROUPINFO("STAB_COL_4", 4, AC_InputManager_Heli, _heli_stab_col_max, AC_ATTITUDE_HELI_STAB_COLLECTIVE_MAX_DEFAULT),
// Indicies 1-4 (STAB_COL_1 thru STAB_COL_4) have been replaced.
// @Param: ACRO_COL_EXP
// @DisplayName: Acro Mode Collective Expo
@ -52,6 +19,42 @@ const AP_Param::GroupInfo AC_InputManager_Heli::var_info[] = {
// @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("STB_COL_1", 6, AC_InputManager_Heli, _heli_stab_col_min, AC_ATTITUDE_HELI_STAB_COLLECTIVE_MIN_DEFAULT),
// @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("STB_COL_2", 7, AC_InputManager_Heli, _heli_stab_col_low, AC_ATTITUDE_HELI_STAB_COLLECTIVE_LOW_DEFAULT),
// @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("STB_COL_3", 8, AC_InputManager_Heli, _heli_stab_col_high, AC_ATTITUDE_HELI_STAB_COLLECTIVE_HIGH_DEFAULT),
// @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("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<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
View File

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