From ab8db97b4dedd1521fa01ad3de5f1f70251421e6 Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Sat, 10 Oct 2015 19:51:37 -0400 Subject: [PATCH] AC_InputManager: Initial class creation --- ArduCopter/make.inc | 1 + libraries/AC_InputManager/AC_InputManager.cpp | 14 ++ libraries/AC_InputManager/AC_InputManager.h | 33 +++++ .../AC_InputManager/AC_InputManager_Heli.cpp | 124 ++++++++++++++++++ .../AC_InputManager/AC_InputManager_Heli.h | 58 ++++++++ 5 files changed, 230 insertions(+) create mode 100644 libraries/AC_InputManager/AC_InputManager.cpp create mode 100644 libraries/AC_InputManager/AC_InputManager.h create mode 100644 libraries/AC_InputManager/AC_InputManager_Heli.cpp create mode 100644 libraries/AC_InputManager/AC_InputManager_Heli.h diff --git a/ArduCopter/make.inc b/ArduCopter/make.inc index b0689a2187..f0c26c1750 100644 --- a/ArduCopter/make.inc +++ b/ArduCopter/make.inc @@ -64,3 +64,4 @@ LIBRARIES += AP_Parachute LIBRARIES += AP_LandingGear LIBRARIES += AP_Terrain LIBRARIES += AP_RPM +LIBRARIES += AC_InputManager diff --git a/libraries/AC_InputManager/AC_InputManager.cpp b/libraries/AC_InputManager/AC_InputManager.cpp new file mode 100644 index 0000000000..c461e9c4fb --- /dev/null +++ b/libraries/AC_InputManager/AC_InputManager.cpp @@ -0,0 +1,14 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include "AC_InputManager.h" +#include +#include + +extern const AP_HAL::HAL& hal; + +const AP_Param::GroupInfo AC_InputManager::var_info[] PROGMEM = { + + // Placeholder for future parameters in this class. + + AP_GROUPEND +}; diff --git a/libraries/AC_InputManager/AC_InputManager.h b/libraries/AC_InputManager/AC_InputManager.h new file mode 100644 index 0000000000..d6779745a4 --- /dev/null +++ b/libraries/AC_InputManager/AC_InputManager.h @@ -0,0 +1,33 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/// @file AC_InputManager.h +/// @brief Pilot manual control input library + +#ifndef AC_INPUTMANAGER_H +#define AC_INPUTMANAGER_H + +#include +#include +#include + +/// @class AC_InputManager +/// @brief Class managing the pilot's control inputs +class AC_InputManager{ +public: + AC_InputManager(uint16_t loop_rate): + _loop_rate(loop_rate) + { + // setup parameter defaults + AP_Param::setup_object_defaults(this, var_info); + } + + static const struct AP_Param::GroupInfo var_info[]; + +protected: + + // internal variables + uint16_t _loop_rate; // rate at which output() function is called (normally 400hz) + +}; + +#endif /* AC_INPUTMANAGER_H */ diff --git a/libraries/AC_InputManager/AC_InputManager_Heli.cpp b/libraries/AC_InputManager/AC_InputManager_Heli.cpp new file mode 100644 index 0000000000..c57587cd36 --- /dev/null +++ b/libraries/AC_InputManager/AC_InputManager_Heli.cpp @@ -0,0 +1,124 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include "AC_InputManager_Heli.h" +#include +#include + +extern const AP_HAL::HAL& hal; + +const AP_Param::GroupInfo AC_InputManager_Heli::var_info[] PROGMEM = { + + // 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: Percent*10 + // @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: Percent*10 + // @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: Percent*10 + // @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: Percent*10 + // @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_GROUPEND +}; + +// get_pilot_desired_collective - rescale's pilot collective pitch input in Stabilize and Acro modes +int16_t AC_InputManager_Heli::get_pilot_desired_collective(int16_t control_in) +{ + float slope_low, slope_high, slope_range, slope_run, scalar; + int16_t stab_col_out, acro_col_out; + + // 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; + slope_high = _heli_stab_col_low; + slope_range = 400; + slope_run = control_in; + } else if(control_in <600){ + slope_low = _heli_stab_col_low; + slope_high = _heli_stab_col_high; + slope_range = 200; + slope_run = control_in - 400; + } else { + slope_low = _heli_stab_col_high; + slope_high = _heli_stab_col_max; + slope_range = 400; + slope_run = control_in - 600; + } + + scalar = (slope_high - slope_low)/slope_range; + stab_col_out = slope_low + slope_run * scalar; + stab_col_out = constrain_int16(stab_col_out, 0, 1000); + + // + // calculate expo-scaled acro collective + // range check expo + if (_acro_col_expo > 1.0f) { + _acro_col_expo = 1.0f; + } + + if (_acro_col_expo <= 0) { + acro_col_out = control_in; + } else { + // expo variables + float col_in, col_in3, col_out; + col_in = (float)(control_in-500)/500.0f; + col_in3 = col_in*col_in*col_in; + col_out = (_acro_col_expo * col_in3) + ((1-_acro_col_expo)*col_in); + acro_col_out = 500 + col_out*500; + } + acro_col_out = constrain_int16(acro_col_out, 0, 1000); + + // ramp to and from stab col over 1/2 second + if (_im_flags_heli.use_stab_col && (_stab_col_ramp < 1.0)){ + _stab_col_ramp += 2.0f/(float)_loop_rate; + } else if(!_im_flags_heli.use_stab_col && (_stab_col_ramp > 0.0)){ + _stab_col_ramp -= 2.0f/(float)_loop_rate; + } + _stab_col_ramp = constrain_float(_stab_col_ramp, 0.0, 1.0); + + // scale collective output smoothly between acro and stab col + int16_t collective_out; + collective_out = (float)((1.0-_stab_col_ramp)*acro_col_out + _stab_col_ramp*stab_col_out); + collective_out = constrain_int16(collective_out, 0, 1000); + + return collective_out; +} + + diff --git a/libraries/AC_InputManager/AC_InputManager_Heli.h b/libraries/AC_InputManager/AC_InputManager_Heli.h new file mode 100644 index 0000000000..b8d8d86bc2 --- /dev/null +++ b/libraries/AC_InputManager/AC_InputManager_Heli.h @@ -0,0 +1,58 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/// @file AC_InputManager_Heli.h +/// @brief Pilot manual control input library for Conventional Helicopter + +#ifndef AC_INPUTMANAGER_HELI_H +#define AC_INPUTMANAGER_HELI_H + +#include +#include +#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 + +/// @class AP_InputManager_Heli +/// @brief Class managing the pilot's control inputs for Conventional Helicopter +class AC_InputManager_Heli : public AC_InputManager { +public: + // Constructor + AC_InputManager_Heli(uint16_t loop_rate): + AC_InputManager(loop_rate) + { + // setup parameter defaults + AP_Param::setup_object_defaults(this, var_info); + } + + // get_pilot_desired_collective - rescale's pilot collective pitch input in Stabilize and Acro modes + int16_t get_pilot_desired_collective(int16_t control_in); + + // set_use_stab_col - setter function + void set_use_stab_col(bool use) { _im_flags_heli.use_stab_col = use; } + + // set_heli_stab_col_ramp - setter function + void set_stab_col_ramp(float ramp) { _stab_col_ramp = constrain_float(ramp, 0.0, 1.0); } + + static const struct AP_Param::GroupInfo var_info[]; + +private: + + struct InputManagerHeliFlags { + uint8_t use_stab_col : 1; // 1 if we should use Stabilise mode collective range, 0 for Acro range + } _im_flags_heli; + + // factor used to smoothly ramp collective from Acro value to Stab-Col value + float _stab_col_ramp = 0; + + AP_Int16 _heli_stab_col_min; // minimum collective pitch setting at zero throttle input in Stabilize mode + AP_Int16 _heli_stab_col_low; // collective pitch setting at mid-low throttle input in Stabilize mode + AP_Int16 _heli_stab_col_high; // collective pitch setting at mid-high throttle input in Stabilize mode + AP_Int16 _heli_stab_col_max; // maximum collective pitch setting at full throttle input in Stabilize mode + AP_Float _acro_col_expo; // used to soften collective pitch inputs near center point in Acro mode + +}; + +#endif /* AC_INPUTMANAGER_HELI_H */ \ No newline at end of file