Copter: Heli: rework inverted flight flags and ensure only in a supported mode.

ArduCopter/AP_Arming.cpp

@@ -231,12 +231,6 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
             return false;
         }
 
-        // Inverted flight feature disabled for Heli Single and Dual frames
-        if (copter.g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI_QUAD &&
-            rc().find_channel_for_option(RC_Channel::AUX_FUNC::INVERTED) != nullptr) {
-            check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Inverted flight option not supported");
-            return false;
-        }
         // Ensure an Aux Channel is configured for motor interlock
         if (rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_INTERLOCK) == nullptr) {
             check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Motor Interlock not configured");

ArduCopter/Copter.h

@@ -580,9 +580,8 @@ class Copter : public AP_Vehicle {
     // Tradheli flags
     typedef struct {
         uint8_t dynamic_flight          : 1;    // 0   // true if we are moving at a significant speed (used to turn on/off leaky I terms)
-        uint8_t inverted_flight         : 1;    // 1   // true for inverted flight mode
-        uint8_t in_autorotation         : 1;    // 2   // true when heli is in autorotation
-        bool coll_stk_low                  ;    // 3   // true when collective stick is on lower limit
+        uint8_t in_autorotation         : 1;    // 1   // true when heli is in autorotation
+        bool coll_stk_low                  ;    // 2   // true when collective stick is on lower limit
     } heli_flags_t;
     heli_flags_t heli_flags;
 

ArduCopter/RC_Channel.cpp

@@ -421,17 +421,17 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc
 #if FRAME_CONFIG == HELI_FRAME
             switch (ch_flag) {
             case AuxSwitchPos::HIGH:
-                copter.motors->set_inverted_flight(true);
-                copter.attitude_control->set_inverted_flight(true);
-                copter.heli_flags.inverted_flight = true;
+                if (copter.flightmode->allows_inverted()) {
+                    copter.attitude_control->set_inverted_flight(true);
+                } else {
+                    gcs().send_text(MAV_SEVERITY_WARNING, "Inverted flight not available in %s mode", copter.flightmode->name());
+                }
                 break;
             case AuxSwitchPos::MIDDLE:
                 // nothing
                 break;
             case AuxSwitchPos::LOW:
-                copter.motors->set_inverted_flight(false);
                 copter.attitude_control->set_inverted_flight(false);
-                copter.heli_flags.inverted_flight = false;
                 break;
             }
 #endif

ArduCopter/mode.cpp

@@ -457,6 +457,11 @@ void Copter::exit_mode(Mode *&old_flightmode,
             input_manager.set_stab_col_ramp(0.0);
         }
     }
+
+    // Make sure inverted flight is disabled if not supported in the new mode
+    if (!new_flightmode->allows_inverted()) {
+        attitude_control->set_inverted_flight(false);
+    }
 #endif //HELI_FRAME
 }
 

ArduCopter/mode.h

@@ -128,6 +128,10 @@ class Mode {
     virtual bool allows_autotune() const { return false; }
     virtual bool allows_flip() const { return false; }
 
+#if FRAME_CONFIG == HELI_FRAME
+    virtual bool allows_inverted() const { return false; };
+#endif
+
     // return a string for this flightmode
     virtual const char *name() const = 0;
     virtual const char *name4() const = 0;
@@ -1574,6 +1578,8 @@ class ModeStabilize_Heli : public ModeStabilize {
     bool init(bool ignore_checks) override;
     void run() override;
 
+    bool allows_inverted() const override { return true; };
+
 protected:
 
 private:

libraries/AC_AttitudeControl/AC_AttitudeControl.h

@@ -566,9 +566,6 @@ class AC_AttitudeControl {
     void control_monitor_filter_pid(float value, float &rms_P);
     void control_monitor_update(void);
 
-    // true in inverted flight mode
-    bool _inverted_flight;
-
 public:
     // log a CTRL message
     void control_monitor_log(void) const;

libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp

@@ -538,8 +538,28 @@ void AC_AttitudeControl_Heli::set_throttle_out(float throttle_in, bool apply_ang
 {
     _throttle_in = throttle_in;
     update_althold_lean_angle_max(throttle_in);
+
+    if (_transition_count > 0) {
+        _transition_count -= 1;
+    } else {
+        _transition_count = 0;
+    }
+    float throttle_out = 0.0f;
+    if (_transition_count > 0) {
+        if ((_ahrs.roll_sensor >= -3000 && _ahrs.roll_sensor <= 3000) || _ahrs.roll_sensor >= 15000 || _ahrs.roll_sensor <= -15000) {
+            throttle_out = (throttle_in - ((AP_MotorsHeli&)_motors).get_coll_mid()) / cosf(radians(_ahrs.roll_sensor * 0.01f)) + ((AP_MotorsHeli&)_motors).get_coll_mid();
+        } else if ((_ahrs.roll_sensor > 3000 && _ahrs.roll_sensor < 15000) || (_ahrs.roll_sensor > -15000 && _ahrs.roll_sensor < -3000)) {
+            float scale_factor = cosf(radians(_ahrs.roll_sensor * 0.01f)) / cosf(radians(30.0f));
+            throttle_out = scale_factor * (throttle_in - ((AP_MotorsHeli&)_motors).get_coll_mid())/ cosf(radians(30.0f)) + ((AP_MotorsHeli&)_motors).get_coll_mid();
+        }
+    } else if (_inverted_flight) {
+        throttle_out = 1.0f - throttle_in;
+    } else {
+        throttle_out = throttle_in;
+    }
+
     _motors.set_throttle_filter_cutoff(filter_cutoff);
-    _motors.set_throttle(throttle_in);
+    _motors.set_throttle(throttle_out);
     // Clear angle_boost for logging purposes
     _angle_boost = 0.0f;
 }
@@ -569,4 +589,13 @@ void AC_AttitudeControl_Heli::set_notch_sample_rate(float sample_rate)
     _pid_rate_pitch.set_notch_sample_rate(sample_rate);
     _pid_rate_yaw.set_notch_sample_rate(sample_rate);
 #endif
-}
+}
+
+// enable/disable inverted flight
+void AC_AttitudeControl_Heli::set_inverted_flight(bool inverted)
+{
+    if (_inverted_flight != inverted) {
+        _transition_count = AC_ATTITUDE_HELI_INVERTED_TRANSITION_TIME * AP::scheduler().get_filtered_loop_rate_hz();
+    }
+    _inverted_flight = inverted;
+}

libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h

@@ -28,6 +28,7 @@
 #define AC_ATTITUDE_HELI_RATE_Y_FF_FILTER          20.0f
 #define AC_ATTITUDE_HELI_HOVER_ROLL_TRIM_DEFAULT    300
 #define AC_ATTITUDE_HELI_ACRO_OVERSHOOT_ANGLE_RAD   ToRad(30.0f)
+#define AC_ATTITUDE_HELI_INVERTED_TRANSITION_TIME    3.0f
 
 class AC_AttitudeControl_Heli : public AC_AttitudeControl {
 public:
@@ -82,9 +83,7 @@ class AC_AttitudeControl_Heli : public AC_AttitudeControl {
     void input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw) override;
 
     // enable/disable inverted flight
-    void set_inverted_flight(bool inverted) override {
-        _inverted_flight = inverted;
-    }
+    void set_inverted_flight(bool inverted) override;
 
     // set the PID notch sample rates
     void set_notch_sample_rate(float sample_rate) override;
@@ -101,6 +100,10 @@ class AC_AttitudeControl_Heli : public AC_AttitudeControl {
         uint8_t tail_passthrough    :   1;  // 1 if we should pass through pilots yaw input to tail
     } _flags_heli;
 
+    // true in inverted flight mode
+    bool _inverted_flight;
+    uint16_t _transition_count;
+
     // Integrate vehicle rate into _att_error_rot_vec_rad
     void integrate_bf_rate_error_to_angle_errors();
 

libraries/AP_Motors/AP_MotorsHeli.h

@@ -75,9 +75,6 @@ class AP_MotorsHeli : public AP_Motors {
     // set_collective_for_landing - limits collective from going too low if we know we are landed
     void set_collective_for_landing(bool landing) { _heliflags.landing_collective = landing; }
 
-    // set_inverted_flight - enables/disables inverted flight
-    void set_inverted_flight(bool inverted) { _heliflags.inverted_flight = inverted; }
-
     // get_rsc_mode - gets the current rotor speed control method
     uint8_t get_rsc_mode() const { return _main_rotor.get_control_mode(); }
 
@@ -248,7 +245,6 @@ class AP_MotorsHeli : public AP_Motors {
     struct heliflags_type {
         uint8_t landing_collective      : 1;    // true if collective is setup for landing which has much higher minimum
         uint8_t rotor_runup_complete    : 1;    // true if the rotors have had enough time to wind up
-        uint8_t inverted_flight         : 1;    // true for inverted flight
         uint8_t init_targets_on_arming  : 1;    // 0 if targets were initialized, 1 if targets were not initialized after arming
         uint8_t save_rsc_mode           : 1;    // used to determine the rsc mode needs to be saved while disarmed
         uint8_t in_autorotation         : 1;    // true if aircraft is in autorotation

libraries/AP_Motors/AP_MotorsHeli_Dual.cpp

@@ -415,10 +415,6 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c
         }
     }
 
-    if (_heliflags.inverted_flight) {
-        collective_in = 1 - collective_in;
-    }
-
     // constrain collective input
     float collective_out = collective_in;
     if (collective_out <= 0.0f) {

libraries/AP_Motors/AP_MotorsHeli_Quad.cpp

@@ -207,10 +207,6 @@ void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float c
 
     float collective_range = (_collective_max - _collective_min) * 0.001f;
 
-    if (_heliflags.inverted_flight) {
-        collective_out = 1.0f - collective_out;
-    }
-
     // feed power estimate into main rotor controller
     _main_rotor.set_collective(fabsf(collective_out));
 

libraries/AP_Motors/AP_MotorsHeli_Single.cpp

@@ -382,10 +382,6 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
     limit.throttle_lower = false;
     limit.throttle_upper = false;
 
-    if (_heliflags.inverted_flight) {
-        coll_in = 1 - coll_in;
-    }
-
     // rescale roll_out and pitch_out into the min and max ranges to provide linear motion
     // across the input range instead of stopping when the input hits the constrain value
     // these calculations are based on an assumption of the user specified cyclic_max