diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index d466e545ff9..4454bc3abdc 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -117,10 +117,10 @@ public: void input_quaternion(Quaternion attitude_desired_quat, float smoothing_gain); // Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing - void input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds, float smoothing_gain); + virtual void input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds, float smoothing_gain); // Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing - 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, float smoothing_gain); + virtual 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, float smoothing_gain); // Command an euler roll, pitch, and yaw rate with angular velocity feedforward and smoothing void input_euler_rate_roll_pitch_yaw(float euler_roll_rate_cds, float euler_pitch_rate_cds, float euler_yaw_rate_cds); @@ -263,6 +263,9 @@ public: // passthrough_bf_roll_pitch_rate_yaw - roll and pitch are passed through directly, body-frame rate target for yaw virtual void passthrough_bf_roll_pitch_rate_yaw(float roll_passthrough, float pitch_passthrough, float yaw_rate_bf_cds) {}; + + // enable inverted flight on backends that support it + virtual void set_inverted_flight(bool inverted) {} // User settable parameters static const struct AP_Param::GroupInfo var_info[]; @@ -394,6 +397,9 @@ 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); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index 0ff12e6055b..36c53a70704 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -437,3 +437,21 @@ void AC_AttitudeControl_Heli::set_throttle_out(float throttle_in, bool apply_ang // Clear angle_boost for logging purposes _angle_boost = 0.0f; } + +// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing +void AC_AttitudeControl_Heli::input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds, float smoothing_gain) +{ + if (_inverted_flight) { + euler_roll_angle_cd = wrap_180_cd(euler_roll_angle_cd + 18000); + } + AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(euler_roll_angle_cd, euler_pitch_angle_cd, euler_yaw_rate_cds, smoothing_gain); +} + +// Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing +void AC_AttitudeControl_Heli::input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw, float smoothing_gain) +{ + if (_inverted_flight) { + euler_roll_angle_cd = wrap_180_cd(euler_roll_angle_cd + 18000); + } + AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(euler_roll_angle_cd, euler_pitch_angle_cd, euler_yaw_angle_cd, slew_yaw, smoothing_gain); +} diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h index aaf1dc185dd..0d3214078eb 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h @@ -95,6 +95,17 @@ public: // Set output throttle void set_throttle_out(float throttle_in, bool apply_angle_boost, float filt_cutoff) override; + // Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing + void input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds, float smoothing_gain) override; + + // Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing + 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, float smoothing_gain) override; + + // enable/disable inverted flight + void set_inverted_flight(bool inverted) override { + _inverted_flight = inverted; + } + // user settable parameters static const struct AP_Param::GroupInfo var_info[];