Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: pass in gyro sample to rate controller

This commit is contained in:
Andy Piper 2024-05-13 12:25:50 +01:00 committed by Peter Barker
parent 88957235d2
commit c28e38e9b5
3 changed files with 15 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -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);

16
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
View File

@ -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()
{

1
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h
View File

@ -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