From 576ee75669bf9edab4df419f839dbaee12dfab74 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Thu, 29 Feb 2024 23:30:54 +0000 Subject: [PATCH] AC_AttitudeControl: Heli: invert throttle in inverted flight, move state down to heli --- libraries/AC_AttitudeControl/AC_AttitudeControl.h | 3 --- libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp | 5 +++++ libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h | 3 +++ 3 files changed, 8 insertions(+), 3 deletions(-) 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();