From 6b5da48641ac80499925a3fe346659b3923993b3 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 1 Aug 2024 22:10:34 +0100 Subject: [PATCH] AC_AttitudeControl: provide function to reset target modifiers --- libraries/AC_AttitudeControl/AC_AttitudeControl.h | 5 ++++- .../AC_AttitudeControl/AC_AttitudeControl_Multi.cpp | 13 +++++++++---- .../AC_AttitudeControl/AC_AttitudeControl_Multi.h | 1 + 3 files changed, 14 insertions(+), 5 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index c326b76643..29856b84d6 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -220,8 +220,11 @@ public: // Run angular velocity controller and send outputs to the motors virtual void rate_controller_run() = 0; + // reset target loop rate modifications + virtual void rate_controller_target_reset() {} + // optional variant to allow running with different dt - virtual void rate_controller_run_dt(float dt, const Vector3f& gyro) { rate_controller_run(); } + virtual void rate_controller_run_dt(float dt, const Vector3f& gyro) {} // Convert a 321-intrinsic euler angle derivative to an angular velocity vector void euler_rate_to_ang_vel(const Quaternion& att, const Vector3f& euler_rate_rads, Vector3f& ang_vel_rads); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp index cc7398aa31..6d1b35ecae 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp @@ -465,15 +465,20 @@ void AC_AttitudeControl_Multi::rate_controller_run_dt(float dt, const Vector3f& _motors.set_yaw(get_rate_yaw_pid().update_all(ang_vel_body.z, gyro.z, dt, _motors.limit.yaw, _pd_scale.z) + _actuator_sysid.z); _motors.set_yaw_ff(get_rate_yaw_pid().get_ff()*_feedforward_scalar); - _sysid_ang_vel_body.zero(); - _actuator_sysid.zero(); - _pd_scale_used = _pd_scale; - _pd_scale = VECTORF_111; control_monitor_update(); } +// reset the rate controller target loop updates +void AC_AttitudeControl_Multi::rate_controller_target_reset() +{ + _sysid_ang_vel_body.zero(); + _actuator_sysid.zero(); + _pd_scale = VECTORF_111; +} + +// run the rate controller using the configured _dt and latest gyro void AC_AttitudeControl_Multi::rate_controller_run() { Vector3f gyro_latest = _ahrs.get_gyro_latest(); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h index 756188b6e7..f0c9bd1230 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h @@ -77,6 +77,7 @@ public: // run lowest level body-frame rate controller and send outputs to the motors void rate_controller_run_dt(float dt, const Vector3f& gyro) override; + void rate_controller_target_reset() override; void rate_controller_run() override; // sanity check parameters. should be called once before take-off