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

AC_AttitudeControl: incorporate SysID into tradheli

Browse Source
This commit is contained in:
bnsgeyer 2019-09-29 22:30:13 -04:00 committed by Andrew Tridgell
parent e9ff670df5
commit 47b1a20238
1 changed files with 9 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
View File

@ -248,6 +248,8 @@ void AC_AttitudeControl_Heli::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cd
// should be called at 100hz or more // should be called at 100hz or more
void AC_AttitudeControl_Heli::rate_controller_run() void AC_AttitudeControl_Heli::rate_controller_run()
{ {
_rate_target_ang_vel += _rate_sysid_ang_vel;
Vector3f gyro_latest = _ahrs.get_gyro_latest(); Vector3f gyro_latest = _ahrs.get_gyro_latest();
// call rate controllers and send output to motors object // call rate controllers and send output to motors object
@ -263,6 +265,10 @@ void AC_AttitudeControl_Heli::rate_controller_run()
} else { } else {
_motors.set_yaw(rate_target_to_motor_yaw(gyro_latest.z, _rate_target_ang_vel.z)); _motors.set_yaw(rate_target_to_motor_yaw(gyro_latest.z, _rate_target_ang_vel.z));
} }
_rate_sysid_ang_vel.zero();
_actuator_sysid.zero();
} }
// Update Alt_Hold angle maximum // Update Alt_Hold angle maximum
@ -287,13 +293,13 @@ void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(const Vector3f &rate_r
if (_flags_heli.leaky_i) { if (_flags_heli.leaky_i) {
_pid_rate_roll.update_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE); _pid_rate_roll.update_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);
} }
float roll_pid = _pid_rate_roll.update_all(rate_roll_target_rads, rate_rads.x, _flags_heli.limit_roll); float roll_pid = _pid_rate_roll.update_all(rate_roll_target_rads, rate_rads.x, _flags_heli.limit_roll) + _actuator_sysid.x;
if (_flags_heli.leaky_i) { if (_flags_heli.leaky_i) {
_pid_rate_pitch.update_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE); _pid_rate_pitch.update_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);
} }
float pitch_pid = _pid_rate_pitch.update_all(rate_pitch_target_rads, rate_rads.y, _flags_heli.limit_pitch); float pitch_pid = _pid_rate_pitch.update_all(rate_pitch_target_rads, rate_rads.y, _flags_heli.limit_pitch) + _actuator_sysid.y;
// use pid library to calculate ff // use pid library to calculate ff
float roll_ff = _pid_rate_roll.get_ff(); float roll_ff = _pid_rate_roll.get_ff();
@ -349,7 +355,7 @@ float AC_AttitudeControl_Heli::rate_target_to_motor_yaw(float rate_yaw_actual_ra
_pid_rate_yaw.update_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE); _pid_rate_yaw.update_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);
} }
float pid = _pid_rate_yaw.update_all(rate_target_rads, rate_yaw_actual_rads, _flags_heli.limit_yaw); float pid = _pid_rate_yaw.update_all(rate_target_rads, rate_yaw_actual_rads, _flags_heli.limit_yaw) + _actuator_sysid.z;
// use pid library to calculate ff // use pid library to calculate ff
float vff = _pid_rate_yaw.get_ff(); float vff = _pid_rate_yaw.get_ff();