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

AC_AttitudeControl: incorporate SysID into tradheli

This commit is contained in:
bnsgeyer 2019-09-29 22:30:13 -04:00 committed by Randy Mackay
parent 0834726b48
commit d1a0635ec2
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
void AC_AttitudeControl_Heli::rate_controller_run()
{
_rate_target_ang_vel += _rate_sysid_ang_vel;
Vector3f gyro_latest = _ahrs.get_gyro_latest();
// call rate controllers and send output to motors object
@ -263,6 +265,10 @@ void AC_AttitudeControl_Heli::rate_controller_run()
} else {
_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
@ -287,13 +293,13 @@ void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(const Vector3f &rate_r
if (_flags_heli.leaky_i) {
_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) {
_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
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);
}
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
float vff = _pid_rate_yaw.get_ff();