From c28e38e9b50ad7eb686f64bb20c2ec1ff10aa76e Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 13 May 2024 12:25:50 +0100 Subject: [PATCH] AC_AttitudeControl: pass in gyro sample to rate controller --- .../AC_AttitudeControl/AC_AttitudeControl.h | 3 +++ .../AC_AttitudeControl_Multi.cpp | 16 +++++++++++----- .../AC_AttitudeControl_Multi.h | 1 + 3 files changed, 15 insertions(+), 5 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 70271ac9cc..5c8627b64f 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -217,6 +217,9 @@ public: // Run angular velocity controller and send outputs to the motors virtual void rate_controller_run() = 0; + // optional variant to allow running with different dt + virtual void rate_controller_run_dt(float dt, const Vector3f& gyro) { rate_controller_run(); } + // 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 91378c8bfa..b31f4d4415 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() +void AC_AttitudeControl_Multi::rate_controller_run_dt(float dt, const Vector3f& gyro) { // boost angle_p/pd each cycle on high throttle slew update_throttle_gain_boost(); @@ -449,16 +449,16 @@ void AC_AttitudeControl_Multi::rate_controller_run() _ang_vel_body += _sysid_ang_vel_body; - _rate_gyro = _ahrs.get_gyro_latest(); + _rate_gyro = gyro; _rate_gyro_time_us = AP_HAL::micros64(); - _motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, _rate_gyro.x, _dt, _motors.limit.roll, _pd_scale.x) + _actuator_sysid.x); + _motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, gyro.x, dt, _motors.limit.roll, _pd_scale.x) + _actuator_sysid.x); _motors.set_roll_ff(get_rate_roll_pid().get_ff()); - _motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, _rate_gyro.y, _dt, _motors.limit.pitch, _pd_scale.y) + _actuator_sysid.y); + _motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, gyro.y, dt, _motors.limit.pitch, _pd_scale.y) + _actuator_sysid.y); _motors.set_pitch_ff(get_rate_pitch_pid().get_ff()); - _motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, _rate_gyro.z, _dt, _motors.limit.yaw, _pd_scale.z) + _actuator_sysid.z); + _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(); @@ -470,6 +470,12 @@ void AC_AttitudeControl_Multi::rate_controller_run() control_monitor_update(); } +void AC_AttitudeControl_Multi::rate_controller_run() +{ + Vector3f gyro_latest = _ahrs.get_gyro_latest(); + rate_controller_run_dt(_dt, gyro_latest); +} + // sanity check parameters. should be called once before takeoff void AC_AttitudeControl_Multi::parameter_sanity_check() { diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h index 55359a1edb..756188b6e7 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h @@ -76,6 +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() override; // sanity check parameters. should be called once before take-off