Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: provide function to reset target modifiers

Browse Source
This commit is contained in:
Andy Piper 2024-08-01 22:10:34 +01:00 committed by Peter Barker
parent 2cddea8d9f
commit 6b5da48641
3 changed files with 14 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -220,8 +220,11 @@ public:
// Run angular velocity controller and send outputs to the motors // Run angular velocity controller and send outputs to the motors
virtual void rate_controller_run() = 0; 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 // 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 // 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); void euler_rate_to_ang_vel(const Quaternion& att, const Vector3f& euler_rate_rads, Vector3f& ang_vel_rads);

13
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
View File

@ -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(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); _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_used = _pd_scale;
_pd_scale = VECTORF_111;
control_monitor_update(); 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() void AC_AttitudeControl_Multi::rate_controller_run()
{ {
Vector3f gyro_latest = _ahrs.get_gyro_latest(); Vector3f gyro_latest = _ahrs.get_gyro_latest();

1
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h
View File

@ -77,6 +77,7 @@ public:
// run lowest level body-frame rate controller and send outputs to the motors // 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(float dt, const Vector3f& gyro) override;
void rate_controller_target_reset() override;
void rate_controller_run() override; void rate_controller_run() override;
// sanity check parameters. should be called once before take-off // sanity check parameters. should be called once before take-off