From a193bd645a5c5d7f7db672d52df9b43f5ed15e06 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 13 May 2024 12:25:50 +0100 Subject: [PATCH 01/15] 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 70271ac9cc48f..5c8627b64f6ee 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -217,6 +217,9 @@ class AC_AttitudeControl { // 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 91378c8bfa067..b31f4d44158f6 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 55359a1edb6fb..756188b6e779d 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h @@ -76,6 +76,7 @@ class AC_AttitudeControl_Multi : public AC_AttitudeControl { 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 From 9890f13dae808e627635bf64bac3250332aa1845 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 15 May 2024 20:51:14 +0100 Subject: [PATCH 02/15] AC_AttitudeControl: ensure the rate and attitude controllers can't interfere with the target at the same time --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 37 ++++++++++++------- .../AC_AttitudeControl/AC_AttitudeControl.h | 3 -- .../AC_AttitudeControl_Multi.cpp | 12 ++++-- 3 files changed, 31 insertions(+), 21 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index ef3317fe6bd2d..05c224d83b6c5 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -188,15 +188,16 @@ float AC_AttitudeControl::get_slew_yaw_max_degs() const // Ensure attitude controller have zero errors to relax rate controller output void AC_AttitudeControl::relax_attitude_controllers() { + // take a copy of the last gyro used by the rate controller before using it + Vector3f gyro = _rate_gyro; // Initialize the attitude variables to the current attitude _ahrs.get_quat_body_to_ned(_attitude_target); _attitude_target.to_euler(_euler_angle_target); _attitude_ang_error.initialise(); // Initialize the angular rate variables to the current rate - _ang_vel_target = _ahrs.get_gyro(); + _ang_vel_target = gyro; ang_vel_to_euler_rate(_attitude_target, _ang_vel_target, _euler_rate_target); - _ang_vel_body = _ahrs.get_gyro(); // Initialize remaining variables _thrust_error_angle = 0.0f; @@ -208,6 +209,8 @@ void AC_AttitudeControl::relax_attitude_controllers() // Reset the I terms reset_rate_controller_I_terms(); + // finally update the attitude target + _ang_vel_body = gyro; } void AC_AttitudeControl::reset_rate_controller_I_terms() @@ -531,6 +534,7 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_2(float roll_rate_bf_cds, _attitude_target.to_euler(_euler_angle_target); // Convert body-frame angular velocity into euler angle derivative of desired attitude ang_vel_to_euler_rate(_attitude_target, _ang_vel_target, _euler_rate_target); + // finally update the attitude target _ang_vel_body = _ang_vel_target; } @@ -554,7 +558,7 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds, _attitude_ang_error.from_axis_angle(attitude_error); } - Vector3f gyro_latest = _ahrs.get_gyro_latest(); + Vector3f gyro_latest = _rate_gyro; attitude_ang_error_update_quat.from_axis_angle((_ang_vel_target - gyro_latest) * _dt); _attitude_ang_error = attitude_ang_error_update_quat * _attitude_ang_error; _attitude_ang_error.normalize(); @@ -582,8 +586,11 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds, // Compute the angular velocity target from the integrated rate error _attitude_ang_error.to_axis_angle(attitude_error); - _ang_vel_body = update_ang_vel_target_from_att_error(attitude_error); - _ang_vel_body += _ang_vel_target; + Vector3f ang_vel_body = update_ang_vel_target_from_att_error(attitude_error); + ang_vel_body += _ang_vel_target; + + // finally update the attitude target + _ang_vel_body = ang_vel_body; } // Command an angular step (i.e change) in body frame angle @@ -788,33 +795,35 @@ void AC_AttitudeControl::attitude_controller_run_quat() thrust_heading_rotation_angles(_attitude_target, attitude_body, attitude_error, _thrust_angle, _thrust_error_angle); // Compute the angular velocity corrections in the body frame from the attitude error - _ang_vel_body = update_ang_vel_target_from_att_error(attitude_error); + Vector3f ang_vel_body = update_ang_vel_target_from_att_error(attitude_error); // ensure angular velocity does not go over configured limits - ang_vel_limit(_ang_vel_body, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max)); + ang_vel_limit(ang_vel_body, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max)); // rotation from the target frame to the body frame Quaternion rotation_target_to_body = attitude_body.inverse() * _attitude_target; // target angle velocity vector in the body frame Vector3f ang_vel_body_feedforward = rotation_target_to_body * _ang_vel_target; - + Vector3f gyro = _rate_gyro; // Correct the thrust vector and smoothly add feedforward and yaw input _feedforward_scalar = 1.0f; if (_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE * 2.0f) { - _ang_vel_body.z = _ahrs.get_gyro().z; + ang_vel_body.z = gyro.z; } else if (_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE) { _feedforward_scalar = (1.0f - (_thrust_error_angle - AC_ATTITUDE_THRUST_ERROR_ANGLE) / AC_ATTITUDE_THRUST_ERROR_ANGLE); - _ang_vel_body.x += ang_vel_body_feedforward.x * _feedforward_scalar; - _ang_vel_body.y += ang_vel_body_feedforward.y * _feedforward_scalar; - _ang_vel_body.z += ang_vel_body_feedforward.z; - _ang_vel_body.z = _ahrs.get_gyro().z * (1.0 - _feedforward_scalar) + _ang_vel_body.z * _feedforward_scalar; + ang_vel_body.x += ang_vel_body_feedforward.x * _feedforward_scalar; + ang_vel_body.y += ang_vel_body_feedforward.y * _feedforward_scalar; + ang_vel_body.z += ang_vel_body_feedforward.z; + ang_vel_body.z = gyro.z * (1.0 - _feedforward_scalar) + ang_vel_body.z * _feedforward_scalar; } else { - _ang_vel_body += ang_vel_body_feedforward; + ang_vel_body += ang_vel_body_feedforward; } // Record error to handle EKF resets _attitude_ang_error = attitude_body.inverse() * _attitude_target; + // finally update the attitude target + _ang_vel_body = ang_vel_body; } // thrust_heading_rotation_angles - calculates two ordered rotations to move the attitude_body quaternion to the attitude_target quaternion. diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 5c8627b64f6ee..6829ceb8c529e 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -352,9 +352,6 @@ class AC_AttitudeControl { // Calculates the body frame angular velocities to follow the target attitude void update_attitude_target(); - // Calculates the body frame angular velocities to follow the target attitude - void attitude_controller_run_quat(); - // thrust_heading_rotation_angles - calculates two ordered rotations to move the attitude_body quaternion to the attitude_target quaternion. // The maximum error in the yaw axis is limited based on the angle yaw P value and acceleration. void thrust_heading_rotation_angles(Quaternion& attitude_target, const Quaternion& attitude_body, Vector3f& attitude_error, float& thrust_angle, float& thrust_error_angle) const; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp index b31f4d44158f6..cc7398aa3189d 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp @@ -441,24 +441,28 @@ void AC_AttitudeControl_Multi::update_throttle_rpy_mix() void AC_AttitudeControl_Multi::rate_controller_run_dt(float dt, const Vector3f& gyro) { + _rate_gyro = gyro; + // take a copy of the target so that it can't be changed from under us. + Vector3f ang_vel_body = _ang_vel_body; + // boost angle_p/pd each cycle on high throttle slew update_throttle_gain_boost(); // move throttle vs attitude mixing towards desired (called from here because this is conveniently called on every iteration) update_throttle_rpy_mix(); - _ang_vel_body += _sysid_ang_vel_body; + ang_vel_body += _sysid_ang_vel_body; _rate_gyro = gyro; _rate_gyro_time_us = AP_HAL::micros64(); - _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(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, 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, 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(); From 9d184098523ee5aafe4ed225f634bc979168b999 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 16 May 2024 23:40:47 +0930 Subject: [PATCH 03/15] AC_AttitudeControl: Use rate step command --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 20 +++++++++++++++++++ .../AC_AttitudeControl/AC_AttitudeControl.h | 3 +++ 2 files changed, 23 insertions(+) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 05c224d83b6c5..50dbc52963501 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -534,6 +534,7 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_2(float roll_rate_bf_cds, _attitude_target.to_euler(_euler_angle_target); // Convert body-frame angular velocity into euler angle derivative of desired attitude ang_vel_to_euler_rate(_attitude_target, _ang_vel_target, _euler_rate_target); + // finally update the attitude target _ang_vel_body = _ang_vel_target; } @@ -619,6 +620,25 @@ void AC_AttitudeControl::input_angle_step_bf_roll_pitch_yaw(float roll_angle_ste attitude_controller_run_quat(); } +// Command an rate step (i.e change) in body frame rate +// Used to command a step in rate without exciting the orthogonal axis during autotune +// Done as a single thread-safe function to avoid intermediate zero values being seen by the attitude controller +void AC_AttitudeControl::input_rate_step_bf_roll_pitch_yaw(float roll_rate_step_bf_cd, float pitch_rate_step_bf_cd, float yaw_rate_step_bf_cd) +{ + // Update the unused targets attitude based on current attitude to condition mode change + _ahrs.get_quat_body_to_ned(_attitude_target); + _attitude_target.to_euler(_euler_angle_target); + // Set the target angular velocity to be zero to minimize target overshoot after the rate step finishes + _ang_vel_target.zero(); + // Convert body-frame angular velocity into euler angle derivative of desired attitude + _euler_rate_target.zero(); + + Vector3f ang_vel_body {roll_rate_step_bf_cd, pitch_rate_step_bf_cd, yaw_rate_step_bf_cd}; + ang_vel_body = ang_vel_body * 0.01f * DEG_TO_RAD; + // finally update the attitude target + _ang_vel_body = ang_vel_body; +} + // Command a thrust vector and heading rate void AC_AttitudeControl::input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds, bool slew_yaw) { diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 6829ceb8c529e..c326b76643eaa 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -206,6 +206,9 @@ class AC_AttitudeControl { // Command an angular step (i.e change) in body frame angle virtual void input_angle_step_bf_roll_pitch_yaw(float roll_angle_step_bf_cd, float pitch_angle_step_bf_cd, float yaw_angle_step_bf_cd); + // Command an angular rate step (i.e change) in body frame rate + virtual void input_rate_step_bf_roll_pitch_yaw(float roll_rate_step_bf_cd, float pitch_rate_step_bf_cd, float yaw_rate_step_bf_cd); + // Command a thrust vector in the earth frame and a heading angle and/or rate virtual void input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds, bool slew_yaw = true); virtual void input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_angle_cd, float heading_rate_cds); From 91a4c9ee004a1c0a7f46b623d5459beea03b46b5 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 16 May 2024 23:41:33 +0930 Subject: [PATCH 04/15] AC_Autotune_Multi: Use rate step command --- libraries/AC_AutoTune/AC_AutoTune_Multi.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp index 213fc41a2b416..5e7aa6c5d9836 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp @@ -1254,23 +1254,22 @@ void AC_AutoTune_Multi::twitch_test_run(AxisType test_axis, const float dir_sign attitude_control->input_rate_bf_roll_pitch_yaw(0.0, 0.0, 0.0); } } else { - attitude_control->input_rate_bf_roll_pitch_yaw_2(0.0, 0.0, 0.0); // Testing rate P and D gains so will set body-frame rate targets. // Rate controller will use existing body-frame rates and convert to motor outputs // for all axes except the one we override here. switch (test_axis) { case AxisType::ROLL: // override body-frame roll rate - attitude_control->rate_bf_roll_target(dir_sign * target_rate + start_rate); + attitude_control->input_rate_step_bf_roll_pitch_yaw(dir_sign * target_rate + start_rate, 0.0f, 0.0f); break; case AxisType::PITCH: // override body-frame pitch rate - attitude_control->rate_bf_pitch_target(dir_sign * target_rate + start_rate); + attitude_control->input_rate_step_bf_roll_pitch_yaw(0.0f, dir_sign * target_rate + start_rate, 0.0f); break; case AxisType::YAW: case AxisType::YAW_D: // override body-frame yaw rate - attitude_control->rate_bf_yaw_target(dir_sign * target_rate + start_rate); + attitude_control->input_rate_step_bf_roll_pitch_yaw(0.0f, 0.0f, dir_sign * target_rate + start_rate); break; } } From 3da60d1de16c1ffecfe2dbc71ec9a0b1f77141e5 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 1 Aug 2024 22:10:34 +0100 Subject: [PATCH 05/15] 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 c326b76643eaa..29856b84d6635 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -220,8 +220,11 @@ class AC_AttitudeControl { // 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 cc7398aa3189d..6d1b35ecaef50 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 756188b6e779d..f0c9bd1230374 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h @@ -77,6 +77,7 @@ class AC_AttitudeControl_Multi : public AC_AttitudeControl { // 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 From 667ba60b4695e8bc26479e4e10f62297e9f050c2 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 15 Aug 2024 10:21:56 +0100 Subject: [PATCH 06/15] Copter: reset sysid and other temporary inputs after rate cycle --- ArduCopter/Attitude.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index f79a555c62d88..d4a7e472ccf5a 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -15,7 +15,9 @@ void Copter::run_rate_controller() pos_control->set_dt(last_loop_time_s); // run low level rate controllers that only require IMU data - attitude_control->rate_controller_run(); + attitude_control->rate_controller_run(); + // reset sysid and other temporary inputs + attitude_control->rate_controller_target_reset(); } /************************************************************* From 1b9b96f34160bf3dbe79de14b0cb77d3ec9c66e1 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 15 Aug 2024 10:22:29 +0100 Subject: [PATCH 07/15] Plane: reset sysid and other temporary inputs after rate cycle --- ArduPlane/quadplane.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index c4b66a7eddd79..a3c27cc4fd35a 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1968,6 +1968,8 @@ void QuadPlane::motors_output(bool run_rate_controller) attitude_control->set_dt(last_loop_time_s); pos_control->set_dt(last_loop_time_s); attitude_control->rate_controller_run(); + // reset sysid and other temporary inputs + attitude_control->rate_controller_target_reset(); last_att_control_ms = now; } From 9d1163aa5d50c0b5ec72a512714178965fb1a414 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 19 Aug 2024 20:53:01 +0100 Subject: [PATCH 08/15] AC_AttitudeControl: add commentary about functions which modify the rate loop target --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 3 ++ .../AC_AttitudeControl/AC_AttitudeControl.h | 34 +++++++++++++++---- 2 files changed, 30 insertions(+), 7 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 50dbc52963501..6b3150171550e 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -474,6 +474,7 @@ void AC_AttitudeControl::input_euler_rate_roll_pitch_yaw(float euler_roll_rate_c attitude_controller_run_quat(); } +// Fully stabilized acro // Command an angular velocity with angular velocity feedforward and smoothing void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds) { @@ -514,6 +515,7 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, fl attitude_controller_run_quat(); } +// Rate-only acro with no attitude feedback - used only by Copter rate-only acro // Command an angular velocity with angular velocity smoothing using rate loops only with no attitude loop stabilization void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_2(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds) { @@ -539,6 +541,7 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_2(float roll_rate_bf_cds, _ang_vel_body = _ang_vel_target; } +// Acro with attitude feedback that does not rely on attitude - used only by Plane acro // Command an angular velocity with angular velocity smoothing using rate loops only with integrated rate error stabilization void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds) { diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 29856b84d6635..ed85b1034c18f 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -151,6 +151,7 @@ class AC_AttitudeControl { // set the rate control input smoothing time constant void set_input_tc(float input_tc) { _input_tc.set(constrain_float(input_tc, 0.0f, 1.0f)); } + // rate loop visible functions // Ensure attitude controller have zero errors to relax rate controller output void relax_attitude_controllers(); @@ -177,6 +178,24 @@ class AC_AttitudeControl { // handle reset of attitude from EKF since the last iteration void inertial_frame_reset(); + // Command euler yaw rate and pitch angle with roll angle specified in body frame + // (implemented only in AC_AttitudeControl_TS for tailsitter quadplanes) + virtual void input_euler_rate_yaw_euler_angle_pitch_bf_roll(bool plane_controls, float euler_roll_angle_cd, + float euler_pitch_angle_cd, float euler_yaw_rate_cds) {} + + ////// begin rate update functions ////// + // These functions all update _ang_vel_body which is used as the rate target by the rate controller. + // Since _ang_vel_body can be seen by the rate controller thread all these functions only set it + // at the end once all of the calculations have been performed. This avoids intermediate results being + // used by the rate controller when running concurrently. _ang_vel_body is accessed so commonly that + // locking proves to be moderately expensive, however since this is changing incrementally values combining + // previous and current elements are safe and do not have an impact on control. + // Any additional functions that are added to manipulate _ang_vel_body should follow this pattern. + + // Calculates the body frame angular velocities to follow the target attitude + // This is used by most of the subsequent functions + void attitude_controller_run_quat(); + // Command a Quaternion attitude with feedforward and smoothing // attitude_desired_quat: is updated on each time_step (_dt) by the integral of the body frame angular velocity virtual void input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_body); @@ -186,20 +205,18 @@ class AC_AttitudeControl { // Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing 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); - // Command euler yaw rate and pitch angle with roll angle specified in body frame - // (implemented only in AC_AttitudeControl_TS for tailsitter quadplanes) - virtual void input_euler_rate_yaw_euler_angle_pitch_bf_roll(bool plane_controls, float euler_roll_angle_cd, - float euler_pitch_angle_cd, float euler_yaw_rate_cds) {} - // Command an euler roll, pitch, and yaw rate with angular velocity feedforward and smoothing virtual void input_euler_rate_roll_pitch_yaw(float euler_roll_rate_cds, float euler_pitch_rate_cds, float euler_yaw_rate_cds); + // Fully stabilized acro // Command an angular velocity with angular velocity feedforward and smoothing virtual void input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds); - // Command an angular velocity with angular velocity feedforward and smoothing + // Rate-only acro with no attitude feedback - used only by Copter rate-only acro + // Command an angular velocity with angular velocity smoothing using rate loops only with no attitude loop stabilization virtual void input_rate_bf_roll_pitch_yaw_2(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds); + // Acro with attitude feedback that does not rely on attitude - used only by Plane acro // Command an angular velocity with angular velocity smoothing using rate loops only with integrated rate error stabilization virtual void input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds); @@ -211,9 +228,12 @@ class AC_AttitudeControl { // Command a thrust vector in the earth frame and a heading angle and/or rate virtual void input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds, bool slew_yaw = true); + virtual void input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_angle_cd, float heading_rate_cds); void input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_cd) {input_thrust_vector_heading(thrust_vector, heading_cd, 0.0f);} + ////// end rate update functions ////// + // Converts thrust vector and heading angle to quaternion rotation in the earth frame Quaternion attitude_from_thrust_vector(Vector3f thrust_vector, float heading_angle) const; @@ -526,7 +546,7 @@ class AC_AttitudeControl { Vector3f _ang_vel_target; // This represents the angular velocity in radians per second in the body frame, used in the angular - // velocity controller. + // velocity controller and most importantly the rate controller. Vector3f _ang_vel_body; // This is the angular velocity in radians per second in the body frame, added to the output angular From c257bd9ad4f735e4af7075f0183c1e6687619c73 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 10 Sep 2024 18:00:08 +0100 Subject: [PATCH 09/15] AC_AttitudeControl: Write_Rate() should be thread-safe --- libraries/AC_AttitudeControl/AC_AttitudeControl_Logging.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Logging.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Logging.cpp index 80d7512742efa..9106ac49a3e09 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Logging.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Logging.cpp @@ -30,7 +30,7 @@ void AC_AttitudeControl::Write_ANG() const // Write a rate packet void AC_AttitudeControl::Write_Rate(const AC_PosControl &pos_control) const { - const Vector3f &rate_targets = rate_bf_targets(); + const Vector3f rate_targets = rate_bf_targets(); const Vector3f &accel_target = pos_control.get_accel_target_cmss(); const Vector3f &gyro_rate = _rate_gyro; const struct log_Rate pkt_rate{ From 34220f530d83216fb565dd4096c95dd0ee46c1a7 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 10 Sep 2024 18:06:38 +0100 Subject: [PATCH 10/15] AC_AttitudeControl: remove unused functions --- libraries/AC_AttitudeControl/AC_AttitudeControl.h | 6 ------ 1 file changed, 6 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index ed85b1034c18f..5724d76f528f8 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -273,12 +273,6 @@ class AC_AttitudeControl { // Return the angle between the target thrust vector and the current thrust vector. float get_att_error_angle_deg() const { return degrees(_thrust_error_angle); } - // Set x-axis angular velocity in centidegrees/s - void rate_bf_roll_target(float rate_cds) { _ang_vel_body.x = radians(rate_cds * 0.01f); } - - // Set y-axis angular velocity in centidegrees/s - void rate_bf_pitch_target(float rate_cds) { _ang_vel_body.y = radians(rate_cds * 0.01f); } - // Set z-axis angular velocity in centidegrees/s void rate_bf_yaw_target(float rate_cds) { _ang_vel_body.z = radians(rate_cds * 0.01f); } From 904c10f3cb31cc277b110753f12b4a1b27788332 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 21 Sep 2024 12:22:13 +0100 Subject: [PATCH 11/15] AC_AttitudeControl: ensure plane always gets the latest gyro --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 34 +++++++++++++++++-- .../AC_AttitudeControl/AC_AttitudeControl.h | 3 ++ .../AC_AttitudeControl_Multi.cpp | 1 - 3 files changed, 34 insertions(+), 4 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 6b3150171550e..fbfa3305be080 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -9,10 +9,12 @@ extern const AP_HAL::HAL& hal; // default gains for Plane # define AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT 0.2f // Soft #define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_MIN 5.0 // Min lean angle so that vehicle can maintain limited control + #define AC_ATTITUDE_CONTROL_AFTER_RATE_CONTROL 0 #else // default gains for Copter and Sub # define AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT 0.15f // Medium #define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_MIN 10.0 // Min lean angle so that vehicle can maintain limited control + #define AC_ATTITUDE_CONTROL_AFTER_RATE_CONTROL 1 #endif AC_AttitudeControl *AC_AttitudeControl::_singleton; @@ -185,11 +187,37 @@ float AC_AttitudeControl::get_slew_yaw_max_degs() const return MIN(_ang_vel_yaw_max, _slew_yaw * 0.01); } +// get the latest gyro for the purposes of attitude control +// Counter-inuitively the lowest latency for rate control is achieved by running rate control +// *before* attitude control. This is because you want rate control to run as close as possible +// to the time that a gyro sample was read to minimise jitter and control errors. Running rate +// control after attitude control might makes sense logically, but the overhead of attitude +// control calculations (and other updates) compromises the actual rate control. +// +// In the case of running rate control in a separate thread, the ordering between rate and attitude +// updates is less important, except that gyro sample used should be the latest +// +// Currently quadplane runs rate control after attitude control, necessitating the following code +// to minimise latency. +// However this code can be removed once quadplane updates it's structure to run the rate loops before +// the Attitude controller. +const Vector3f AC_AttitudeControl::get_latest_gyro() const +{ +#if AC_ATTITUDE_CONTROL_AFTER_RATE_CONTROL + // rate updates happen before attitude updates so the last gyro value is the last rate gyro value + // this also allows a separate rate thread to be the source of gyro data + return _rate_gyro; +#else + // rate updates happen after attitude updates so the AHRS must be consulted for the last gyro value + return _ahrs.get_gyro_latest(); +#endif +} + // Ensure attitude controller have zero errors to relax rate controller output void AC_AttitudeControl::relax_attitude_controllers() { // take a copy of the last gyro used by the rate controller before using it - Vector3f gyro = _rate_gyro; + Vector3f gyro = get_latest_gyro(); // Initialize the attitude variables to the current attitude _ahrs.get_quat_body_to_ned(_attitude_target); _attitude_target.to_euler(_euler_angle_target); @@ -562,7 +590,7 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds, _attitude_ang_error.from_axis_angle(attitude_error); } - Vector3f gyro_latest = _rate_gyro; + Vector3f gyro_latest = get_latest_gyro(); attitude_ang_error_update_quat.from_axis_angle((_ang_vel_target - gyro_latest) * _dt); _attitude_ang_error = attitude_ang_error_update_quat * _attitude_ang_error; _attitude_ang_error.normalize(); @@ -828,7 +856,7 @@ void AC_AttitudeControl::attitude_controller_run_quat() // target angle velocity vector in the body frame Vector3f ang_vel_body_feedforward = rotation_target_to_body * _ang_vel_target; - Vector3f gyro = _rate_gyro; + Vector3f gyro = get_latest_gyro(); // Correct the thrust vector and smoothly add feedforward and yaw input _feedforward_scalar = 1.0f; if (_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE * 2.0f) { diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 5724d76f528f8..ecac8aa720f8d 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -474,6 +474,9 @@ class AC_AttitudeControl { // Return the yaw slew rate limit in radians/s float get_slew_yaw_max_rads() const { return radians(get_slew_yaw_max_degs()); } + // get the latest gyro for the purposes of attitude control + const Vector3f get_latest_gyro() const; + // Maximum rate the yaw target can be updated in Loiter, RTL, Auto flight modes AP_Float _slew_yaw; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp index 6d1b35ecaef50..7957c5bb7c4e1 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp @@ -441,7 +441,6 @@ void AC_AttitudeControl_Multi::update_throttle_rpy_mix() void AC_AttitudeControl_Multi::rate_controller_run_dt(float dt, const Vector3f& gyro) { - _rate_gyro = gyro; // take a copy of the target so that it can't be changed from under us. Vector3f ang_vel_body = _ang_vel_body; From c476d2029c89bf0ca3330b06d7187e23499061a8 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 22 Sep 2024 13:57:29 +0100 Subject: [PATCH 12/15] AC_AttitudeControl: ensure Write_Rate() can be called from autotune --- .../AC_AttitudeControl_Logging.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Logging.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Logging.cpp index 9106ac49a3e09..d9327f6abf382 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Logging.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Logging.cpp @@ -30,20 +30,20 @@ void AC_AttitudeControl::Write_ANG() const // Write a rate packet void AC_AttitudeControl::Write_Rate(const AC_PosControl &pos_control) const { - const Vector3f rate_targets = rate_bf_targets(); + const Vector3f rate_targets = rate_bf_targets() * RAD_TO_DEG; const Vector3f &accel_target = pos_control.get_accel_target_cmss(); - const Vector3f &gyro_rate = _rate_gyro; + const Vector3f gyro_rate = _rate_gyro * RAD_TO_DEG; const struct log_Rate pkt_rate{ LOG_PACKET_HEADER_INIT(LOG_RATE_MSG), time_us : _rate_gyro_time_us, - control_roll : degrees(rate_targets.x), - roll : degrees(gyro_rate.x), + control_roll : rate_targets.x, + roll : gyro_rate.x, roll_out : _motors.get_roll()+_motors.get_roll_ff(), - control_pitch : degrees(rate_targets.y), - pitch : degrees(gyro_rate.y), + control_pitch : rate_targets.y, + pitch : gyro_rate.y, pitch_out : _motors.get_pitch()+_motors.get_pitch_ff(), - control_yaw : degrees(rate_targets.z), - yaw : degrees(gyro_rate.z), + control_yaw : rate_targets.z, + yaw : gyro_rate.z, yaw_out : _motors.get_yaw()+_motors.get_yaw_ff(), control_accel : (float)accel_target.z, accel : (float)(-(_ahrs.get_accel_ef().z + GRAVITY_MSS) * 100.0f), From 54deea9dc2cfbc2727163898e423bce5520ea289 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 23 Sep 2024 13:49:05 +0100 Subject: [PATCH 13/15] AC_AttitudeControl: raise a config error if rate_controller_run_dt() is called by accident --- libraries/AC_AttitudeControl/AC_AttitudeControl.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index ecac8aa720f8d..2c53f9957b726 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -11,6 +11,7 @@ #include #include #include +#include #define AC_ATTITUDE_CONTROL_ANGLE_P 4.5f // default angle P gain for roll, pitch and yaw @@ -244,7 +245,7 @@ class AC_AttitudeControl { 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) {} + virtual void rate_controller_run_dt(float dt, const Vector3f& gyro) { 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); From 0ed6e794a9b410c32ca92a82e4db09df30487023 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 24 Sep 2024 10:27:56 +0100 Subject: [PATCH 14/15] 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 2c53f9957b726..319c2269d2471 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -245,7 +245,7 @@ class AC_AttitudeControl { 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 7957c5bb7c4e1..5beb517fde739 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 f0c9bd1230374..c1e9b15a250a3 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h @@ -76,7 +76,7 @@ class AC_AttitudeControl_Multi : public AC_AttitudeControl { 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; From ea48819ecbc4898ca3562d6f6e87a6f51ec225da Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 24 Sep 2024 12:41:54 +0100 Subject: [PATCH 15/15] Copter: remove whitespace --- ArduCopter/Attitude.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index d4a7e472ccf5a..3e71782c18c00 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -16,7 +16,7 @@ void Copter::run_rate_controller() // run low level rate controllers that only require IMU data attitude_control->rate_controller_run(); - // reset sysid and other temporary inputs + // reset sysid and other temporary inputs attitude_control->rate_controller_target_reset(); }