diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index f752309804..29ec8eae60 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -566,9 +566,6 @@ protected: 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; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index 982ff2048a..21cf2ffae7 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -538,6 +538,11 @@ 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 (_inverted_flight) { + throttle_in = 1.0 - throttle_in; + } + _motors.set_throttle_filter_cutoff(filter_cutoff); _motors.set_throttle(throttle_in); // Clear angle_boost for logging purposes diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h index 0d519bf87d..d978d62f34 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h @@ -101,6 +101,9 @@ private: 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; + // Integrate vehicle rate into _att_error_rot_vec_rad void integrate_bf_rate_error_to_angle_errors();