From eeda86ccb449520f0a7cababbdf859c029c90abb Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 24 Sep 2024 10:27:56 +0100 Subject: [PATCH] AC_AttitudeControl: rate_controller_run_dt() takes dt as last argument --- libraries/AC_AttitudeControl/AC_AttitudeControl.h | 2 +- libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp | 4 ++-- libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 2c53f9957b..319c2269d2 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -245,7 +245,7 @@ public: 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) { AP_BoardConfig::config_error("rate_controller_run_dt() must be defined"); }; + virtual void rate_controller_run_dt(const Vector3f& gyro, float dt) { AP_BoardConfig::config_error("rate_controller_run_dt() must be defined"); }; // 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 7957c5bb7c..5beb517fde 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp @@ -439,7 +439,7 @@ void AC_AttitudeControl_Multi::update_throttle_rpy_mix() _throttle_rpy_mix = constrain_float(_throttle_rpy_mix, 0.1f, AC_ATTITUDE_CONTROL_MAX); } -void AC_AttitudeControl_Multi::rate_controller_run_dt(float dt, const Vector3f& gyro) +void AC_AttitudeControl_Multi::rate_controller_run_dt(const Vector3f& gyro, float dt) { // take a copy of the target so that it can't be changed from under us. Vector3f ang_vel_body = _ang_vel_body; @@ -481,7 +481,7 @@ void AC_AttitudeControl_Multi::rate_controller_target_reset() void AC_AttitudeControl_Multi::rate_controller_run() { Vector3f gyro_latest = _ahrs.get_gyro_latest(); - rate_controller_run_dt(_dt, gyro_latest); + rate_controller_run_dt(gyro_latest, _dt); } // sanity check parameters. should be called once before takeoff diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h index f0c9bd1230..c1e9b15a25 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h @@ -76,7 +76,7 @@ public: bool is_throttle_mix_min() const override { return (_throttle_rpy_mix < 1.25f * _thr_mix_min); } // 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_run_dt(const Vector3f& gyro, float dt) override; void rate_controller_target_reset() override; void rate_controller_run() override;