AC_InputManager_Heli.h 2.4 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556
  1. #pragma once
  2. /// @file AC_InputManager_Heli.h
  3. /// @brief Pilot manual control input library for Conventional Helicopter
  4. #include <AP_Param/AP_Param.h>
  5. #include <AP_Common/AP_Common.h>
  6. #include "AC_InputManager.h"
  7. # define AC_ATTITUDE_HELI_STAB_COLLECTIVE_MIN_DEFAULT 0
  8. # define AC_ATTITUDE_HELI_STAB_COLLECTIVE_LOW_DEFAULT 400
  9. # define AC_ATTITUDE_HELI_STAB_COLLECTIVE_HIGH_DEFAULT 600
  10. # define AC_ATTITUDE_HELI_STAB_COLLECTIVE_MAX_DEFAULT 1000
  11. /// @class AP_InputManager_Heli
  12. /// @brief Class managing the pilot's control inputs for Conventional Helicopter
  13. class AC_InputManager_Heli : public AC_InputManager {
  14. public:
  15. // Constructor
  16. AC_InputManager_Heli()
  17. : AC_InputManager()
  18. {
  19. // setup parameter defaults
  20. AP_Param::setup_object_defaults(this, var_info);
  21. }
  22. /* Do not allow copies */
  23. AC_InputManager_Heli(const AC_InputManager_Heli &other) = delete;
  24. AC_InputManager_Heli &operator=(const AC_InputManager_Heli&) = delete;
  25. // get_pilot_desired_collective - rescale's pilot collective pitch input in Stabilize and Acro modes
  26. float get_pilot_desired_collective(int16_t control_in);
  27. // set_use_stab_col - setter function
  28. void set_use_stab_col(bool use) { _im_flags_heli.use_stab_col = use; }
  29. // set_heli_stab_col_ramp - setter function
  30. void set_stab_col_ramp(float ramp) { _stab_col_ramp = constrain_float(ramp, 0.0, 1.0); }
  31. static const struct AP_Param::GroupInfo var_info[];
  32. private:
  33. struct InputManagerHeliFlags {
  34. uint8_t use_stab_col : 1; // 1 if we should use Stabilise mode collective range, 0 for Acro range
  35. } _im_flags_heli;
  36. // factor used to smoothly ramp collective from Acro value to Stab-Col value
  37. float _stab_col_ramp = 0;
  38. AP_Int16 _heli_stab_col_min; // minimum collective pitch setting at zero throttle input in Stabilize mode
  39. AP_Int16 _heli_stab_col_low; // collective pitch setting at mid-low throttle input in Stabilize mode
  40. AP_Int16 _heli_stab_col_high; // collective pitch setting at mid-high throttle input in Stabilize mode
  41. AP_Int16 _heli_stab_col_max; // maximum collective pitch setting at full throttle input in Stabilize mode
  42. AP_Float _acro_col_expo; // used to soften collective pitch inputs near center point in Acro mode
  43. };