AP_Motors: updates to new integrator and make selectable

libraries/AP_Motors/AP_MotorsHeli.cpp

@@ -115,6 +115,14 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("HOVER_LEARN", 27, AP_MotorsHeli, _collective_hover_learn, HOVER_LEARN_AND_SAVE),
 
+    // @Param: OPTIONS
+    // @DisplayName: Heli_Options
+    // @Description: Bitmask of heli options.  Bit 0 changes how the pitch, roll, and yaw axis integrator term is managed for low speed and takeoff/landing. In AC 4.0 and earlier, scheme uses a leaky integrator for ground speeds less than 5 m/s and won't let the steady state integrator build above ILMI. The integrator is allowed to build to the ILMI value when it is landed.  The other integrator management scheme bases integrator limiting on takeoff and landing.  Whenever the aircraft is landed the integrator is set to zero.  When the aicraft is airborne, the integrator is only limited by IMAX. 
+    // @Values: 0:I term management based landed state, 1:Leaky I(4.0 and earlier)
+    // @Bitmask: 0:Use Leaky I
+    // @User: Standard
+    AP_GROUPINFO("OPTIONS", 28, AP_MotorsHeli, _heli_options, (uint8_t)HeliOption::USE_LEAKY_I),
+
     AP_GROUPEND
 };
 
@@ -173,9 +181,7 @@ void AP_MotorsHeli::output_min()
     update_motor_control(ROTOR_CONTROL_STOP);
 
     // override limits flags
-    limit.roll = true;
+    set_limit_flag_pitch_roll_yaw(true);
-    limit.pitch = true;
-    limit.yaw = true;
     limit.throttle_lower = true;
     limit.throttle_upper = false;
 }
@@ -322,9 +328,11 @@ void AP_MotorsHeli::output_logic()
             // Servos set to their trim values or in a test condition.
 
             // set limits flags
-            limit.roll = true;
+            if (!using_leaky_integrator()) {
-            limit.pitch = true;
+                set_limit_flag_pitch_roll_yaw(true);
-            limit.yaw = true;
+            } else {
+                set_limit_flag_pitch_roll_yaw(false);
+            }
 
             // make sure the motors are spooling in the correct direction
             if (_spool_desired != DesiredSpoolState::SHUT_DOWN) {
@@ -337,14 +345,10 @@ void AP_MotorsHeli::output_logic()
         case SpoolState::GROUND_IDLE: {
             // Motors should be stationary or at ground idle.
             // set limits flags
-            if (_heliflags.land_complete) {
+            if (_heliflags.land_complete && !using_leaky_integrator()) {
-                limit.roll = true;
+                set_limit_flag_pitch_roll_yaw(true);
-                limit.pitch = true;
-                limit.yaw = true;
             } else {
-                limit.roll = false;
+                set_limit_flag_pitch_roll_yaw(false);
-                limit.pitch = false;
-                limit.yaw = false;
             }
 
             // Servos should be moving to correct the current attitude.
@@ -363,14 +367,10 @@ void AP_MotorsHeli::output_logic()
             // Servos should exhibit normal flight behavior.
 
             // set limits flags
-            if (_heliflags.land_complete) {
+            if (_heliflags.land_complete && !using_leaky_integrator()) {
-                limit.roll = true;
+                set_limit_flag_pitch_roll_yaw(true);
-                limit.pitch = true;
-                limit.yaw = true;
             } else {
-                limit.roll = false;
+                set_limit_flag_pitch_roll_yaw(false);
-                limit.pitch = false;
-                limit.yaw = false;
             }
 
             // make sure the motors are spooling in the correct direction
@@ -389,14 +389,10 @@ void AP_MotorsHeli::output_logic()
             // Servos should exhibit normal flight behavior.
 
             // set limits flags
-            if (_heliflags.land_complete) {
+            if (_heliflags.land_complete && !using_leaky_integrator()) {
-                limit.roll = true;
+                set_limit_flag_pitch_roll_yaw(true);
-                limit.pitch = true;
-                limit.yaw = true;
             } else {
-                limit.roll = false;
+                set_limit_flag_pitch_roll_yaw(false);
-                limit.pitch = false;
-                limit.yaw = false;
             }
 
             // make sure the motors are spooling in the correct direction
@@ -413,14 +409,10 @@ void AP_MotorsHeli::output_logic()
             // Servos should exhibit normal flight behavior.
 
             // set limits flags
-            if (_heliflags.land_complete) {
+            if (_heliflags.land_complete && !using_leaky_integrator()) {
-                limit.roll = true;
+                set_limit_flag_pitch_roll_yaw(true);
-                limit.pitch = true;
-                limit.yaw = true;
             } else {
-                limit.roll = false;
+                set_limit_flag_pitch_roll_yaw(false);
-                limit.pitch = false;
-                limit.yaw = false;
             }
 
             // make sure the motors are spooling in the correct direction
@@ -536,7 +528,7 @@ void AP_MotorsHeli::update_throttle_hover(float dt)
             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.
+        // we have chosen to constrain the hover collective 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);
     }
 }
@@ -549,3 +541,20 @@ void AP_MotorsHeli::save_params_on_disarm()
         _collective_hover.save();
     }
 }
+
+// updates the takeoff collective flag
+void AP_MotorsHeli::update_takeoff_collective_flag(float coll_out)
+{
+    if (coll_out > _collective_mid_pct + 0.5f * (_collective_hover - _collective_mid_pct)) {
+        _heliflags.takeoff_collective = true;
+    } else {
+        _heliflags.takeoff_collective = false;
+    }
+}
+
+// Determines if _heli_options bit is set
+bool AP_MotorsHeli::heli_option(HeliOption opt) const
+{
+    return (_heli_options & (uint8_t)opt);
+}
+

libraries/AP_Motors/AP_MotorsHeli.h

@@ -127,6 +127,7 @@ public:
     void update_throttle_hover(float dt);
     float get_throttle_hover() const override { return _collective_hover; }
 
+    // accessor to get the takeoff collective flag signifying that current collective is greater than collective required to indicate takeoff
     bool get_takeoff_collective() const { return _heliflags.takeoff_collective; }
 
     // support passing init_targets_on_arming flag to greater code
@@ -144,6 +145,14 @@ public:
     // set land complete flag
     void set_land_complete(bool landed) { _heliflags.land_complete = landed; }
     
+    // enum for heli optional features
+    enum class HeliOption {
+        USE_LEAKY_I                     = (1<<0),   // 1
+    };
+
+    // use leaking integrator management scheme
+    bool using_leaky_integrator() const { return heli_option(HeliOption::USE_LEAKY_I); }
+    
     // var_info for holding Parameter information
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -207,6 +216,12 @@ protected:
     // save parameters as part of disarming
     void save_params_on_disarm() override;
 
+    // Determines if _heli_options bit is set
+    bool heli_option(HeliOption opt) const;
+
+    // updates the takeoff collective flag indicating that current collective is greater than collective required to indicate takeoff.
+    void update_takeoff_collective_flag(float coll_out);
+
     // enum values for HOVER_LEARN parameter
     enum HoverLearn {
         HOVER_LEARN_DISABLED = 0,
@@ -237,6 +252,7 @@ protected:
     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
+    AP_Int8         _heli_options;              // bitmask for optional features
 
     // internal variables
     float           _collective_mid_pct = 0.0f;      // collective mid parameter value converted to 0 ~ 1 range

libraries/AP_Motors/AP_MotorsHeli_Dual.cpp

@@ -552,11 +552,8 @@ 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)) {
+    // updates takeoff collective flag based on 50% hover collective
-        _heliflags.takeoff_collective = true;
+    update_takeoff_collective_flag(collective_out);
-    } else {
-        _heliflags.takeoff_collective = false;
-    }
 
     // Set rear collective to midpoint if required
     float collective2_out = collective_out;

libraries/AP_Motors/AP_MotorsHeli_Quad.cpp

@@ -230,11 +230,8 @@ 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)) {
+    // updates takeoff collective flag based on 50% hover collective
-        _heliflags.takeoff_collective = true;
+    update_takeoff_collective_flag(collective_out);
-    } else {
-        _heliflags.takeoff_collective = false;
-    }
 
     float collective_range = (_collective_max - _collective_min) * 0.001f;
 

libraries/AP_Motors/AP_MotorsHeli_Single.cpp

@@ -419,11 +419,8 @@ 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)) {
+    // updates takeoff collective flag based on 50% hover collective
-        _heliflags.takeoff_collective = true;
+    update_takeoff_collective_flag(collective_out);
-    } 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) {

libraries/AP_Motors/AP_Motors_Class.cpp

@@ -177,9 +177,17 @@ void AP_Motors::add_motor_num(int8_t motor_num)
     }
 }
 
+    // set limit flag for pitch, roll and yaw
+void AP_Motors::set_limit_flag_pitch_roll_yaw(bool flag)
+{
+    limit.roll = flag;
+    limit.pitch = flag;
+    limit.yaw = flag;
+}
+
 namespace AP {
     AP_Motors *motors()
     {
         return AP_Motors::get_singleton();
     }
-}
+}

libraries/AP_Motors/AP_Motors_Class.h

@@ -145,6 +145,9 @@ public:
         uint8_t throttle_upper  : 1; // we have reached throttle's upper limit
     } limit;
 
+    // set limit flag for pitch, roll and yaw
+    void set_limit_flag_pitch_roll_yaw(bool flag);
+
     //
     // virtual functions that should be implemented by child classes
     //