Line data    Source code

       1             : /// @file       AP_MotorsHeli.h
       2             : /// @brief      Motor control class for Traditional Heli
       3             : #pragma once
       4             : 
       5             : #include <inttypes.h>
       6             : 
       7             : #include <AP_Common/AP_Common.h>
       8             : #include <AP_Math/AP_Math.h>            // ArduPilot Mega Vector/Matrix math Library
       9             : #include <RC_Channel/RC_Channel.h>
      10             : #include <SRV_Channel/SRV_Channel.h>
      11             : #include "AP_Motors_Class.h"
      12             : #include "AP_MotorsHeli_RSC.h"
      13             : 
      14             : // servo output rates
      15             : #define AP_MOTORS_HELI_SPEED_DEFAULT            125     // default servo update rate for helicopters
      16             : 
      17             : // default swash min and max angles and positions
      18             : #define AP_MOTORS_HELI_SWASH_CYCLIC_MAX         2500
      19             : #define AP_MOTORS_HELI_COLLECTIVE_MIN           1250
      20             : #define AP_MOTORS_HELI_COLLECTIVE_MAX           1750
      21             : #define AP_MOTORS_HELI_COLLECTIVE_HOVER_DEFAULT 0.5f  // the estimated hover throttle, 0 ~ 1
      22             : #define AP_MOTORS_HELI_COLLECTIVE_HOVER_TC      10.0f // time constant used to update estimated hover throttle, 0 ~ 1
      23             : #define AP_MOTORS_HELI_COLLECTIVE_HOVER_MIN     0.3f  // minimum possible hover throttle
      24             : #define AP_MOTORS_HELI_COLLECTIVE_HOVER_MAX     0.8f // maximum possible hover throttle
      25             : #define AP_MOTORS_HELI_COLLECTIVE_MIN_DEG      -90.0f // minimum collective blade pitch angle in deg
      26             : #define AP_MOTORS_HELI_COLLECTIVE_MAX_DEG       90.0f // maximum collective blade pitch angle in deg
      27             : #define AP_MOTORS_HELI_COLLECTIVE_LAND_MIN      -2.0f // minimum landed collective blade pitch angle in deg for modes using althold
      28             : 
      29             : 
      30             : // rsc function output channels.
      31             : #define AP_MOTORS_HELI_RSC                      CH_8
      32             : 
      33             : class AP_HeliControls;
      34             : 
      35             : /// @class      AP_MotorsHeli
      36             : class AP_MotorsHeli : public AP_Motors {
      37             : public:
      38             : 
      39             :     /// Constructor
      40          83 :     AP_MotorsHeli( uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) :
      41             :         AP_Motors(speed_hz),
      42          83 :         _main_rotor(SRV_Channel::k_heli_rsc, AP_MOTORS_HELI_RSC, 0U)
      43             :     {
      44          83 :         AP_Param::setup_object_defaults(this, var_info);
      45          83 :     };
      46             : 
      47             :     // init
      48             :     void init(motor_frame_class frame_class, motor_frame_type frame_type) override;
      49             : 
      50             :     // set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
      51        2260 :     void set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type) override {
      52        2260 :         _frame_class = frame_class;
      53        2260 :         _frame_type = frame_type;
      54        2260 :     }
      55             : 
      56             :     // set update rate to motors - a value in hertz
      57             :     virtual void set_update_rate( uint16_t speed_hz ) override = 0;
      58             : 
      59             :     // output_min - sets servos to neutral point with motors stopped
      60             :     void output_min() override;
      61             : 
      62             :     // set_desired_spool_state - set desired spool state with safety constraints
      63             :     void set_desired_spool_state(DesiredSpoolState spool) override;
      64             : 
      65             :     //
      66             :     // heli specific methods
      67             :     //
      68             : 
      69             :     //set turbine start flag on to initiaize starting sequence
      70          10 :     void set_turb_start(bool turb_start) { _heliflags.start_engine = turb_start; }
      71             : 
      72             :     // set_collective_for_landing - limits collective from going too low if we know we are landed
      73      404223 :     void set_collective_for_landing(bool landing) { _heliflags.landing_collective = landing; }
      74             : 
      75             :     // get_rsc_mode - gets the current rotor speed control method
      76      404475 :     uint8_t get_rsc_mode() const { return _main_rotor.get_rsc_control_mode(); }
      77             : 
      78             :     // get_rsc_setpoint - gets contents of _rsc_setpoint parameter (0~1)
      79             :     float get_rsc_setpoint() const { return _main_rotor._rsc_setpoint.get() * 0.01f; }
      80             : 
      81             :     // set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1
      82             :     virtual void set_desired_rotor_speed(float desired_speed);
      83             : 
      84             :     // get_desired_rotor_speed - gets target rotor speed as a number from 0 ~ 1
      85     1813914 :     float get_desired_rotor_speed() const { return _main_rotor.get_desired_rotor_speed(); }
      86             : 
      87             :     // get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
      88             :     //  this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
      89             :     virtual uint32_t get_motor_mask() override;
      90             : 
      91             :     // output - sends commands to the motors
      92             :     void output() override;
      93             : 
      94             :     // update estimated throttle required to hover
      95             :     void update_throttle_hover(float dt);
      96     5274951 :     float get_throttle_hover() const override { return constrain_float(_collective_hover, AP_MOTORS_HELI_COLLECTIVE_HOVER_MIN, AP_MOTORS_HELI_COLLECTIVE_HOVER_MAX); }
      97             : 
      98             :     // accessor to get the takeoff collective flag signifying that current collective is greater than collective required to indicate takeoff
      99      485907 :     bool get_takeoff_collective() const { return _heliflags.takeoff_collective; }
     100             : 
     101             :     // accessor to get the land min collective flag signifying that current collective is lower than collective required for landing
     102      168158 :     bool get_below_land_min_coll() const { return _heliflags.below_land_min_coll; }
     103             : 
     104             :     // support passing init_targets_on_arming flag to greater code
     105       67407 :     bool init_targets_on_arming() const override { return _heliflags.init_targets_on_arming; }
     106             : 
     107             :     // helper for vehicle code to request autorotation states in the RSC.
     108     1803521 :     void set_autorotation_active(bool tf) { _main_rotor.autorotation.set_active(tf, false); }
     109             : 
     110             :     // true if we are using a manual collective flight mode
     111     3233979 :     void set_using_manual_collective_mode(bool using_manual_collective_mode) { _main_rotor.set_using_manual_collective_mode(using_manual_collective_mode); }
     112             : 
     113             :     // helper to force the RSC autorotation state to deactivated
     114     1430458 :     void force_deactivate_autorotation(void) { _main_rotor.autorotation.set_active(false, true); }
     115             : 
     116             :     // true if RSC is actively autorotating or bailing out
     117     3366425 :     bool in_autorotation(void) const { return _main_rotor.in_autorotation(); }
     118             : 
     119             :     // true if bailing out autorotation
     120       14401 :     bool autorotation_bailout(void) const { return _main_rotor.autorotation.bailing_out(); }
     121             : 
     122             :     // true if the autorotation functionality within the rsc has been enabled
     123          80 :     bool rsc_autorotation_enabled(void) const { return _main_rotor.autorotation.enabled(); }
     124             : 
     125             :     // set land complete flag
     126     3233979 :     void set_land_complete(bool landed) { _heliflags.land_complete = landed; }
     127             : 
     128             :     // function to calculate and set the normalised collective position given a desired blade pitch angle (deg)
     129             :     void set_coll_from_ang(float col_ang_deg);
     130             : 
     131             :     //return zero lift collective position
     132     3181156 :     float get_coll_mid() const { return _collective_zero_thrust_pct; }
     133             : 
     134             :     // enum for heli optional features
     135             :     enum class HeliOption {
     136             :         USE_LEAKY_I                     = (1<<0),   // 1
     137             :     };
     138             : 
     139             :     // use leaking integrator management scheme
     140     4258859 :     bool using_leaky_integrator() const { return heli_option(HeliOption::USE_LEAKY_I); }
     141             : 
     142             :     // Run arming checks
     143             :     bool arming_checks(size_t buflen, char *buffer) const override;
     144             : 
     145             :     // Tell user motor test is disabled on heli
     146             :     bool motor_test_checks(size_t buflen, char *buffer) const override;
     147             : 
     148             :     // output_test_seq - disabled on heli, do nothing
     149           0 :     void _output_test_seq(uint8_t motor_seq, int16_t pwm) override {};
     150             : 
     151             :     // Helper function for param conversions to be done in motors class
     152             :     virtual void heli_motors_param_conversions(void);
     153             : 
     154             :     // var_info for holding Parameter information
     155             :     static const struct AP_Param::GroupInfo var_info[];
     156             : 
     157             : protected:
     158             : 
     159             :     // manual servo modes (used for setup)
     160             :     enum ServoControlModes {
     161             :         SERVO_CONTROL_MODE_AUTOMATED = 0,
     162             :         SERVO_CONTROL_MODE_MANUAL_PASSTHROUGH,
     163             :         SERVO_CONTROL_MODE_MANUAL_MAX,
     164             :         SERVO_CONTROL_MODE_MANUAL_CENTER,
     165             :         SERVO_CONTROL_MODE_MANUAL_MIN,
     166             :         SERVO_CONTROL_MODE_MANUAL_OSCILLATE,
     167             :     };
     168             : 
     169             :     // output - sends commands to the motors
     170             :     void output_armed_stabilizing() override;
     171             :     void output_disarmed();
     172             : 
     173             :     // external objects we depend upon
     174             :     AP_MotorsHeli_RSC   _main_rotor;            // main rotor
     175             : 
     176             :     // update_motor_controls - sends commands to motor controllers
     177             :     virtual void update_motor_control(AP_MotorsHeli_RSC::DesiredRSCSpoolState state) = 0;
     178             : 
     179             :     // update_spool_state - updates the spool state based on the desired state
     180             :     virtual AP_Motors::SpoolState update_spool_state(AP_MotorsHeli_RSC::DesiredRSCSpoolState state) = 0;
     181             : 
     182             :     // run spool logic
     183             :     void                output_logic();
     184             : 
     185             :     // output_to_motors - sends commands to the motors
     186             :     virtual void        output_to_motors() = 0;
     187             : 
     188             :     // reset_flight_controls - resets all controls and scalars to flight status
     189             :     void reset_flight_controls();
     190             : 
     191             :     // update the throttle input filter
     192             :     void update_throttle_filter() override;
     193             : 
     194             :     // move_actuators - moves swash plate and tail rotor
     195             :     virtual void move_actuators(float roll_out, float pitch_out, float coll_in, float yaw_out) = 0;
     196             : 
     197             :     // init_outputs - initialise Servo/PWM ranges and endpoints.  This
     198             :     // method also updates the initialised flag.
     199             :     virtual void init_outputs() = 0;
     200             : 
     201             :     // calculate_armed_scalars - recalculates scalars that can change while armed
     202             :     virtual void calculate_armed_scalars();
     203             : 
     204             :     // calculate_scalars - must be implemented by child classes
     205             :     virtual void calculate_scalars() = 0;
     206             : 
     207             :     // servo_test - move servos through full range of movement
     208             :     // to be overloaded by child classes, different vehicle types would have different movement patterns
     209             :     virtual void servo_test() = 0;
     210             : 
     211             :     // save parameters as part of disarming
     212             :     void save_params_on_disarm() override;
     213             : 
     214             :     // Determines if _heli_options bit is set
     215             :     bool heli_option(HeliOption opt) const;
     216             : 
     217             :     // updates the takeoff collective flag indicating that current collective is greater than collective required to indicate takeoff.
     218             :     void update_takeoff_collective_flag(float coll_out);
     219             : 
     220         240 :     const char* _get_frame_string() const override { return "HELI"; }
     221             : 
     222             :     // update turbine start flag
     223             :     void update_turbine_start();
     224             : 
     225             :     // Update _heliflags.rotor_runup_complete value writing log event on state change
     226             :     void set_rotor_runup_complete(bool new_value);
     227             : 
     228             : #if HAL_LOGGING_ENABLED
     229             :     // Returns the scaling value required to convert the collective angle parameters into the cyclic-output-to-angle conversion for blade angle logging
     230             :     float get_cyclic_angle_scaler(void) const;
     231             : #endif
     232             : 
     233             :     // enum values for HOVER_LEARN parameter
     234             :     enum HoverLearn {
     235             :         HOVER_LEARN_DISABLED = 0,
     236             :         HOVER_LEARN_ONLY = 1,
     237             :         HOVER_LEARN_AND_SAVE = 2
     238             :     };
     239             : 
     240             :     // flags bitmask
     241             :     struct heliflags_type {
     242             :         uint8_t landing_collective      : 1;    // true if collective is setup for landing which has much higher minimum
     243             :         uint8_t rotor_runup_complete    : 1;    // true if the rotors have had enough time to wind up
     244             :         uint8_t init_targets_on_arming  : 1;    // 0 if targets were initialized, 1 if targets were not initialized after arming
     245             :         uint8_t save_rsc_mode           : 1;    // used to determine the rsc mode needs to be saved while disarmed
     246             :         uint8_t servo_test_running      : 1;    // true if servo_test is running
     247             :         uint8_t land_complete           : 1;    // true if aircraft is landed
     248             :         uint8_t takeoff_collective      : 1;    // true if collective is above 30% between H_COL_MID and H_COL_MAX
     249             :         uint8_t below_land_min_coll     : 1;    // true if collective is below H_COL_LAND_MIN
     250             :         uint8_t start_engine            : 1;    // true if turbine start RC option is initiated
     251             :     } _heliflags;
     252             : 
     253             :     // parameters
     254             :     AP_Int16        _cyclic_max;                // Maximum cyclic angle of the swash plate in centi-degrees
     255             :     AP_Int16        _collective_min;            // Lowest possible servo position for the swashplate
     256             :     AP_Int16        _collective_max;            // Highest possible servo position for the swashplate
     257             :     AP_Int8         _servo_mode;                // Pass radio inputs directly to servos during set-up through mission planner
     258             :     AP_Int8         _servo_test;                // sets number of cycles to test servo movement on bootup
     259             :     AP_Float        _collective_hover;          // estimated collective required to hover throttle in the range 0 ~ 1
     260             :     AP_Int8         _collective_hover_learn;    // enable/disabled hover collective learning
     261             :     AP_Int8         _heli_options;              // bitmask for optional features
     262             :     AP_Float        _collective_zero_thrust_deg;// Zero thrust blade collective pitch in degrees
     263             :     AP_Float        _collective_land_min_deg;   // Minimum Landed collective blade pitch in degrees for non-manual collective modes (i.e. modes that use altitude hold)
     264             :     AP_Float        _collective_max_deg;        // Maximum collective blade pitch angle in deg that corresponds to the PWM set for maximum collective pitch (H_COL_MAX)
     265             :     AP_Float        _collective_min_deg;        // Minimum collective blade pitch angle in deg that corresponds to the PWM set for minimum collective pitch (H_COL_MIN)
     266             : 
     267             :     // internal variables
     268             :     float           _collective_zero_thrust_pct;      // collective zero thrutst parameter value converted to 0 ~ 1 range
     269             :     float           _collective_land_min_pct;      // collective land min parameter value converted to 0 ~ 1 range
     270             :     uint8_t         _servo_test_cycle_counter = 0;   // number of test cycles left to run after bootup
     271             : 
     272             :     motor_frame_type _frame_type;
     273             :     motor_frame_class _frame_class;
     274             : };