AP_Motors: Tradheli support for integrator management and hover collective learning

libraries/AP_Motors/AP_MotorsHeli.cpp

@@ -101,6 +101,20 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = {
     // @Path: AP_MotorsHeli_RSC.cpp
     AP_SUBGROUPINFO(_main_rotor, "RSC_", 25, AP_MotorsHeli, AP_MotorsHeli_RSC),
 
+    // @Param: COLL_HOVER
+    // @DisplayName: Collective Hover Value
+    // @Description: Collective needed to hover expressed as a number from 0 to 1 where 0 is H_COL_MIN and 1 is H_COL_MAX
+    // @Range: 0.3 0.8
+    // @User: Advanced
+    AP_GROUPINFO("COLL_HOVER", 26, AP_MotorsHeli, _collective_hover, AP_MOTORS_HELI_COLLECTIVE_HOVER_DEFAULT),
+
+    // @Param: HOVER_LEARN
+    // @DisplayName: Hover Value Learning
+    // @Description: Enable/Disable automatic learning of hover collective
+    // @Values: 0:Disabled, 1:Learn, 2:Learn and Save
+    // @User: Advanced
+    AP_GROUPINFO("HOVER_LEARN", 27, AP_MotorsHeli, _collective_hover_learn, HOVER_LEARN_AND_SAVE),
+
     AP_GROUPEND
 };
 
@@ -307,6 +321,11 @@ void AP_MotorsHeli::output_logic()
             // Motors should be stationary.
             // Servos set to their trim values or in a test condition.
 
+            // set limits flags
+            limit.roll = true;
+            limit.pitch = true;
+            limit.yaw = true;
+
             // make sure the motors are spooling in the correct direction
             if (_spool_desired != DesiredSpoolState::SHUT_DOWN) {
                 _spool_state = SpoolState::GROUND_IDLE;
@@ -317,6 +336,17 @@ void AP_MotorsHeli::output_logic()
 
         case SpoolState::GROUND_IDLE: {
             // Motors should be stationary or at ground idle.
+            // set limits flags
+            if (_heliflags.land_complete) {
+                limit.roll = true;
+                limit.pitch = true;
+                limit.yaw = true;
+            } else {
+                limit.roll = false;
+                limit.pitch = false;
+                limit.yaw = false;
+            }
+
             // Servos should be moving to correct the current attitude.
             if (_spool_desired == DesiredSpoolState::SHUT_DOWN){
                 _spool_state = SpoolState::SHUT_DOWN;
@@ -332,6 +362,17 @@ void AP_MotorsHeli::output_logic()
             // Maximum throttle should move from minimum to maximum.
             // Servos should exhibit normal flight behavior.
 
+            // set limits flags
+            if (_heliflags.land_complete) {
+                limit.roll = true;
+                limit.pitch = true;
+                limit.yaw = true;
+            } else {
+                limit.roll = false;
+                limit.pitch = false;
+                limit.yaw = false;
+            }
+
             // make sure the motors are spooling in the correct direction
             if (_spool_desired != DesiredSpoolState::THROTTLE_UNLIMITED ){
                 _spool_state = SpoolState::SPOOLING_DOWN;
@@ -347,6 +388,17 @@ void AP_MotorsHeli::output_logic()
             // Throttle should exhibit normal flight behavior.
             // Servos should exhibit normal flight behavior.
 
+            // set limits flags
+            if (_heliflags.land_complete) {
+                limit.roll = true;
+                limit.pitch = true;
+                limit.yaw = true;
+            } else {
+                limit.roll = false;
+                limit.pitch = false;
+                limit.yaw = false;
+            }
+
             // make sure the motors are spooling in the correct direction
             if (_spool_desired != DesiredSpoolState::THROTTLE_UNLIMITED) {
                 _spool_state = SpoolState::SPOOLING_DOWN;
@@ -360,6 +412,17 @@ void AP_MotorsHeli::output_logic()
             // Maximum throttle should move from maximum to minimum.
             // Servos should exhibit normal flight behavior.
 
+            // set limits flags
+            if (_heliflags.land_complete) {
+                limit.roll = true;
+                limit.pitch = true;
+                limit.yaw = true;
+            } else {
+                limit.roll = false;
+                limit.pitch = false;
+                limit.yaw = false;
+            }
+
             // make sure the motors are spooling in the correct direction
             if (_spool_desired == DesiredSpoolState::THROTTLE_UNLIMITED) {
                 _spool_state = SpoolState::SPOOLING_UP;
@@ -462,3 +525,27 @@ void AP_MotorsHeli::rc_write_swash(uint8_t chan, float swash_in)
     SRV_Channels::set_output_pwm_trimmed(function, pwm);
 }
 
+// update the collective input filter.  should be called at 100hz
+void AP_MotorsHeli::update_throttle_hover(float dt)
+{
+    if (_collective_hover_learn != HOVER_LEARN_DISABLED) {
+
+        // Don't let _collective_hover go below H_COLL_MID
+        float curr_collective = get_throttle();
+        if (curr_collective < _collective_mid_pct) {
+            curr_collective = _collective_mid_pct;
+        }
+
+        // we have chosen to constrain the hover throttle to be within the range reachable by the third order expo polynomial.
+        _collective_hover = constrain_float(_collective_hover + (dt / (dt + AP_MOTORS_HELI_COLLECTIVE_HOVER_TC)) * (curr_collective - _collective_hover), AP_MOTORS_HELI_COLLECTIVE_HOVER_MIN, AP_MOTORS_HELI_COLLECTIVE_HOVER_MAX);
+    }
+}
+
+// save parameters as part of disarming
+void AP_MotorsHeli::save_params_on_disarm()
+{
+    // save hover throttle
+    if (_collective_hover_learn == HOVER_LEARN_AND_SAVE) {
+        _collective_hover.save();
+    }
+}

libraries/AP_Motors/AP_MotorsHeli.h

@@ -19,6 +19,10 @@
 #define AP_MOTORS_HELI_COLLECTIVE_MIN           1250
 #define AP_MOTORS_HELI_COLLECTIVE_MAX           1750
 #define AP_MOTORS_HELI_COLLECTIVE_MID           1500
+#define AP_MOTORS_HELI_COLLECTIVE_HOVER_DEFAULT 0.5f  // the estimated hover throttle, 0 ~ 1
+#define AP_MOTORS_HELI_COLLECTIVE_HOVER_TC      10.0f // time constant used to update estimated hover throttle, 0 ~ 1
+#define AP_MOTORS_HELI_COLLECTIVE_HOVER_MIN     0.3f  // minimum possible hover throttle
+#define AP_MOTORS_HELI_COLLECTIVE_HOVER_MAX     0.8f // maximum possible hover throttle
 
 // flybar types
 #define AP_MOTORS_HELI_NOFLYBAR                 0
@@ -119,7 +123,11 @@ public:
     // supports_yaw_passthrough
     virtual bool supports_yaw_passthrough() const { return false; }
 
-    float get_throttle_hover() const override { return 0.5f; }
+    // update estimated throttle required to hover
+    void update_throttle_hover(float dt);
+    float get_throttle_hover() const override { return _collective_hover; }
+
+    bool get_takeoff_collective() const { return _heliflags.takeoff_collective; }
 
     // support passing init_targets_on_arming flag to greater code
     bool init_targets_on_arming() const override { return _heliflags.init_targets_on_arming; }
@@ -133,6 +141,9 @@ public:
     // return true if the servo test is still running/pending
     bool servo_test_running() const { return _heliflags.servo_test_running; }
 
+    // set land complete flag
+    void set_land_complete(bool landed) { _heliflags.land_complete = landed; }
+    
     // var_info for holding Parameter information
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -193,6 +204,16 @@ protected:
     // write to a swash servo. output value is pwm
     void rc_write_swash(uint8_t chan, float swash_in);
 
+    // save parameters as part of disarming
+    void save_params_on_disarm() override;
+
+    // enum values for HOVER_LEARN parameter
+    enum HoverLearn {
+        HOVER_LEARN_DISABLED = 0,
+        HOVER_LEARN_ONLY = 1,
+        HOVER_LEARN_AND_SAVE = 2
+    };
+
     // flags bitmask
     struct heliflags_type {
         uint8_t landing_collective      : 1;    // true if collective is setup for landing which has much higher minimum
@@ -203,6 +224,8 @@ protected:
         uint8_t in_autorotation         : 1;    // true if aircraft is in autorotation
         uint8_t enable_bailout          : 1;    // true if allowing RSC to quickly ramp up engine
         uint8_t servo_test_running      : 1;    // true if servo_test is running
+        uint8_t land_complete           : 1;    // true if aircraft is landed
+        uint8_t takeoff_collective      : 1;    // true if collective is above 30% between H_COL_MID and H_COL_MAX
     } _heliflags;
 
     // parameters
@@ -212,6 +235,8 @@ protected:
     AP_Int16        _collective_mid;            // Swash servo position corresponding to zero collective pitch (or zero lift for Asymmetrical blades)
     AP_Int8         _servo_mode;                // Pass radio inputs directly to servos during set-up through mission planner
     AP_Int8         _servo_test;                // sets number of cycles to test servo movement on bootup
+    AP_Float        _collective_hover;          // estimated collective required to hover throttle in the range 0 ~ 1
+    AP_Int8         _collective_hover_learn;    // enable/disabled hover collective learning
 
     // internal variables
     float           _collective_mid_pct = 0.0f;      // collective mid parameter value converted to 0 ~ 1 range

libraries/AP_Motors/AP_MotorsHeli_Dual.cpp

@@ -483,9 +483,6 @@ void AP_MotorsHeli_Dual::update_motor_control(RotorControlState state)
 void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float collective_in, float yaw_out)
 {
     // initialize limits flag
-    limit.roll = false;
-    limit.pitch = false;
-    limit.yaw = false;
     limit.throttle_lower = false;
     limit.throttle_upper = false;
 
@@ -555,6 +552,12 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c
         limit.throttle_lower = true;
     }
 
+    if (collective_out > _collective_mid_pct + 0.5f * (_collective_hover - _collective_mid_pct)) {
+        _heliflags.takeoff_collective = true;
+    } else {
+        _heliflags.takeoff_collective = false;
+    }
+
     // Set rear collective to midpoint if required
     float collective2_out = collective_out;
     if (_servo_mode == SERVO_CONTROL_MODE_MANUAL_CENTER) {

libraries/AP_Motors/AP_MotorsHeli_Quad.cpp

@@ -210,9 +210,6 @@ void AP_MotorsHeli_Quad::update_motor_control(RotorControlState state)
 void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float collective_in, float yaw_out)
 {
     // initialize limits flag
-    limit.roll = false;
-    limit.pitch = false;
-    limit.yaw = false;
     limit.throttle_lower = false;
     limit.throttle_upper = false;
 
@@ -233,6 +230,12 @@ void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float c
         limit.throttle_lower = true;
     }
 
+    if (collective_out > _collective_mid_pct + 0.5f * (_collective_hover - _collective_mid_pct)) {
+        _heliflags.takeoff_collective = true;
+    } else {
+        _heliflags.takeoff_collective = false;
+    }
+
     float collective_range = (_collective_max - _collective_min) * 0.001f;
 
     if (_heliflags.inverted_flight) {

libraries/AP_Motors/AP_MotorsHeli_Single.cpp

@@ -381,9 +381,6 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
     float yaw_offset = 0.0f;
 
     // initialize limits flag
-    limit.roll = false;
-    limit.pitch = false;
-    limit.yaw = false;
     limit.throttle_lower = false;
     limit.throttle_upper = false;
 
@@ -422,6 +419,12 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
         limit.throttle_lower = true;
     }
 
+    if (collective_out > _collective_mid_pct + 0.5f * (_collective_hover - _collective_mid_pct)) {
+        _heliflags.takeoff_collective = true;
+    } else {
+        _heliflags.takeoff_collective = false;
+    }
+
     // if servo output not in manual mode and heli is not in autorotation, process pre-compensation factors
     if (_servo_mode == SERVO_CONTROL_MODE_AUTOMATED && !_heliflags.in_autorotation) {
         // rudder feed forward based on collective