AP_Motors: add support for offsetting collective when landed

libraries/AP_Motors/AP_MotorsHeli.cpp

@@ -152,6 +152,15 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = {
     // @Path: ../AC_Autorotation/RSC_Autorotation.cpp
     AP_SUBGROUPINFO(_main_rotor.autorotation, "RSC_AROT_", 33, AP_MotorsHeli, RSC_Autorotation),
 
+    // @Param: COL_LAND_OFSET
+    // @DisplayName: Offset of Collective Blade Pitch when Landed
+    // @Description: Offset of the collective blade pitch angle when landed in degrees for non-manual collective modes (i.e. modes that use altitude hold).
+    // @Range: -2 2
+    // @Units: deg
+    // @Increment: 0.1
+    // @User: Standard
+    AP_GROUPINFO("COL_LAND_OFSET", 34, AP_MotorsHeli, _collective_land_offset_deg, 0.0),
+
     AP_GROUPEND
 };
 
@@ -349,7 +358,7 @@ void AP_MotorsHeli::output_logic()
         case SpoolState::GROUND_IDLE: {
             // Motors should be stationary or at ground idle.
             // set limits flags
-            if (_heliflags.land_complete && !using_leaky_integrator()) {
+            if ((_heliflags.land_complete || _heliflags.land_complete_maybe) && !using_leaky_integrator()) {
                 set_limit_flag_pitch_roll_yaw(true);
             } else {
                 set_limit_flag_pitch_roll_yaw(false);
@@ -371,7 +380,7 @@ void AP_MotorsHeli::output_logic()
             // Servos should exhibit normal flight behavior.
 
             // set limits flags
-            if (_heliflags.land_complete && !using_leaky_integrator()) {
+            if ((_heliflags.land_complete || _heliflags.land_complete_maybe) && !using_leaky_integrator()) {
                 set_limit_flag_pitch_roll_yaw(true);
             } else {
                 set_limit_flag_pitch_roll_yaw(false);
@@ -393,7 +402,7 @@ void AP_MotorsHeli::output_logic()
             // Servos should exhibit normal flight behavior.
 
             // set limits flags
-            if (_heliflags.land_complete && !using_leaky_integrator()) {
+            if ((_heliflags.land_complete || _heliflags.land_complete_maybe) && !using_leaky_integrator()) {
                 set_limit_flag_pitch_roll_yaw(true);
             } else {
                 set_limit_flag_pitch_roll_yaw(false);
@@ -413,7 +422,7 @@ void AP_MotorsHeli::output_logic()
             // Servos should exhibit normal flight behavior.
 
             // set limits flags
-            if (_heliflags.land_complete && !using_leaky_integrator()) {
+            if ((_heliflags.land_complete || _heliflags.land_complete_maybe) && !using_leaky_integrator()) {
                 set_limit_flag_pitch_roll_yaw(true);
             } else {
                 set_limit_flag_pitch_roll_yaw(false);

libraries/AP_Motors/AP_MotorsHeli.h

@@ -25,6 +25,7 @@
 #define AP_MOTORS_HELI_COLLECTIVE_MIN_DEG      -90.0f // minimum collective blade pitch angle in deg
 #define AP_MOTORS_HELI_COLLECTIVE_MAX_DEG       90.0f // maximum collective blade pitch angle in deg
 #define AP_MOTORS_HELI_COLLECTIVE_LAND_MIN      -2.0f // minimum landed collective blade pitch angle in deg for modes using althold
+#define AP_MOTORS_HELI_COLLECTIVE_FILTER_FREQ   2.0f // time constant used to update estimated hover throttle, 0 ~ 1
 
 
 // flybar types
@@ -132,7 +133,10 @@ public:
 
     // set land complete flag
     void set_land_complete(bool landed) { _heliflags.land_complete = landed; }
-	
+
+    // set land complete maybe flag
+    void set_land_complete_maybe(bool landed) { _heliflags.land_complete_maybe = landed; }
+
 	//return zero lift collective position
     float get_coll_mid() const { return _collective_zero_thrust_pct; }
 
@@ -254,6 +258,7 @@ protected:
         uint8_t below_land_min_coll     : 1;    // true if collective is below H_COL_LAND_MIN
         uint8_t rotor_spooldown_complete : 1;    // true if the rotors have spooled down completely
         uint8_t start_engine            : 1;    // true if turbine start RC option is initiated
+        uint8_t land_complete_maybe     : 1;    // true if aircraft is landed_maybe
     } _heliflags;
 
     // parameters
@@ -269,11 +274,14 @@ protected:
     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)
     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)
     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)
+    AP_Float        _collective_land_offset_deg;// Offset of Landed collective blade pitch in degrees for non-manual collective modes (i.e. modes that use altitude hold)
 
     // internal variables
     float           _collective_zero_thrust_pct;      // collective zero thrutst parameter value converted to 0 ~ 1 range
     float           _collective_land_min_pct;      // collective land min parameter value converted to 0 ~ 1 range
+    float           _collective_land_offset_pct;   // collective land offset parameter value converted to fraction of collective range
     uint8_t         _servo_test_cycle_counter = 0;   // number of test cycles left to run after bootup
+    float           _collective_offset_landed;  // current value of collective offset
 
     motor_frame_type _frame_type;
     motor_frame_class _frame_class;

libraries/AP_Motors/AP_MotorsHeli_Dual.cpp

@@ -269,6 +269,7 @@ void AP_MotorsHeli_Dual::calculate_scalars()
     _collective_zero_thrust_deg.set(constrain_float(_collective_zero_thrust_deg, _collective_min_deg, _collective_max_deg));
 
     _collective_land_min_deg.set(constrain_float(_collective_land_min_deg, _collective_min_deg, _collective_max_deg));
+    _collective_land_offset_deg.set(constrain_float(_collective_land_offset_deg, 0.0, _collective_zero_thrust_deg-_collective_land_min_deg));
 
     if (!is_equal((float)_collective_max_deg, (float)_collective_min_deg)) {
         // calculate collective zero thrust point as a number from 0 to 1
@@ -276,6 +277,7 @@ void AP_MotorsHeli_Dual::calculate_scalars()
 
         // calculate collective land min point as a number from 0 to 1
         _collective_land_min_pct = (_collective_land_min_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
+        _collective_land_offset_pct = _collective_land_offset_deg/(_collective_max_deg-_collective_min_deg);
     } else {
         _collective_zero_thrust_pct = 0.0f;
         _collective_land_min_pct = 0.0f;
@@ -420,14 +422,27 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c
         limit.throttle_upper = true;
     }
 
-    // ensure not below landed/landing collective
+    // In non-manual collective modes, ensure not below landed/landing collective.  If collective offset is used, don't allow negative
-    if (_heliflags.landing_collective && collective_out < _collective_land_min_pct) {
+    // collective offsets.  Only allow positive collective offsets to keep collective above landed collective.  
+    if (_heliflags.land_complete && _heliflags.landing_collective && collective_out <= _collective_land_min_pct + _collective_offset_landed) {
+        float alpha = _dt / (_dt + 1.0/(2.0 * M_PI * AP_MOTORS_HELI_COLLECTIVE_FILTER_FREQ)); //calculate LP filter alpha for 2 hz cutoff freq
+        _collective_offset_landed += alpha * (_collective_land_offset_pct - _collective_offset_landed);
+        // constrain collective_out so it never goes above collective zero thrust
+        collective_out = constrain_float((_collective_land_min_pct + _collective_offset_landed), _collective_land_min_pct, _collective_zero_thrust_pct);
+        limit.throttle_lower = true;
+    } else if (_heliflags.landing_collective && collective_out < _collective_land_min_pct && !_main_rotor.in_autorotation()) {
         collective_out = _collective_land_min_pct;
         limit.throttle_lower = true;
+    } else {
+        _collective_offset_landed = 0.0;
     }
 
     // updates below land min collective flag
-    if (collective_out <= _collective_land_min_pct) {
+    float collective_land_min = _collective_land_min_pct;
+    if (_heliflags.land_complete && _heliflags.landing_collective) {
+        collective_land_min += _collective_offset_landed;
+    }
+    if (collective_out <= collective_land_min) {
         _heliflags.below_land_min_coll = true;
     } else {
         _heliflags.below_land_min_coll = false;

libraries/AP_Motors/AP_MotorsHeli_Single.cpp

@@ -284,6 +284,7 @@ void AP_MotorsHeli_Single::calculate_scalars()
     _collective_zero_thrust_deg.set(constrain_float(_collective_zero_thrust_deg, _collective_min_deg, _collective_max_deg));
 
     _collective_land_min_deg.set(constrain_float(_collective_land_min_deg, _collective_min_deg, _collective_max_deg));
+    _collective_land_offset_deg.set(constrain_float(_collective_land_offset_deg, 0.0, _collective_zero_thrust_deg-_collective_land_min_deg));
 
     if (!is_equal((float)_collective_max_deg, (float)_collective_min_deg)) {
         // calculate collective zero thrust point as a number from 0 to 1
@@ -291,6 +292,7 @@ void AP_MotorsHeli_Single::calculate_scalars()
 
         // calculate collective land min point as a number from 0 to 1
         _collective_land_min_pct = (_collective_land_min_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
+        _collective_land_offset_pct = _collective_land_offset_deg/(_collective_max_deg-_collective_min_deg);
     } else {
         _collective_zero_thrust_pct = 0.0f;
         _collective_land_min_pct = 0.0f;
@@ -387,14 +389,27 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
         limit.throttle_upper = true;
     }
 
-    // ensure not below landed/landing collective
+    // In non-manual collective modes, ensure not below landed/landing collective.  If collective offset is used, don't allow negative
-    if (_heliflags.landing_collective && collective_out < _collective_land_min_pct && !_main_rotor.in_autorotation()) {
+    // collective offsets.  Only allow positive collective offsets to keep collective above landed collective.  
+    if (_heliflags.land_complete && _heliflags.landing_collective && collective_out <= _collective_land_min_pct + _collective_offset_landed) {
+        float alpha = _dt / (_dt + 1.0/(2.0 * M_PI * AP_MOTORS_HELI_COLLECTIVE_FILTER_FREQ)); //calculate LP filter alpha for 2 hz cutoff freq
+        _collective_offset_landed += alpha * (_collective_land_offset_pct - _collective_offset_landed);
+        // constrain collective_out so it never goes above collective zero thrust
+        collective_out = constrain_float((_collective_land_min_pct + _collective_offset_landed), _collective_land_min_pct, _collective_zero_thrust_pct);
+        limit.throttle_lower = true;
+    } else if (_heliflags.landing_collective && collective_out < _collective_land_min_pct && !_main_rotor.in_autorotation()) {
         collective_out = _collective_land_min_pct;
         limit.throttle_lower = true;
+    } else {
+        _collective_offset_landed = 0.0;
     }
 
     // updates below land min collective flag
-    if (collective_out <= _collective_land_min_pct) {
+    float collective_land_min = _collective_land_min_pct;
+    if (_heliflags.land_complete && _heliflags.landing_collective) {
+        collective_land_min += _collective_offset_landed;
+    }
+    if (collective_out <= collective_land_min) {
         _heliflags.below_land_min_coll = true;
     } else {
         _heliflags.below_land_min_coll = false;