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

AC_AttitudeControl: use FF from AC_PID

Browse Source
This commit is contained in:
Andrew Tridgell 2017-02-10 16:25:38 +11:00
parent b97bf5d15e
commit 5acbf5d16e
2 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -607,7 +607,7 @@ float AC_AttitudeControl::rate_target_to_motor_roll(float rate_target_rads)
}
// Compute output in range -1 ~ +1
float output = get_rate_roll_pid().get_p() + integrator + get_rate_roll_pid().get_d();
float output = get_rate_roll_pid().get_p() + integrator + get_rate_roll_pid().get_d() + get_rate_roll_pid().get_ff(rate_target_rads);
// Constrain output
return constrain_float(output, -1.0f, 1.0f);
@ -631,7 +631,7 @@ float AC_AttitudeControl::rate_target_to_motor_pitch(float rate_target_rads)
}
// Compute output in range -1 ~ +1
float output = get_rate_pitch_pid().get_p() + integrator + get_rate_pitch_pid().get_d();
float output = get_rate_pitch_pid().get_p() + integrator + get_rate_pitch_pid().get_d() + get_rate_pitch_pid().get_ff(rate_target_rads);
// Constrain output
return constrain_float(output, -1.0f, 1.0f);
@ -655,7 +655,7 @@ float AC_AttitudeControl::rate_target_to_motor_yaw(float rate_target_rads)
}
// Compute output in range -1 ~ +1
float output = get_rate_yaw_pid().get_p() + integrator + get_rate_yaw_pid().get_d();
float output = get_rate_yaw_pid().get_p() + integrator + get_rate_yaw_pid().get_d() + get_rate_yaw_pid().get_ff(rate_target_rads);
// Constrain output
return constrain_float(output, -1.0f, 1.0f);

6
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
View File

@ -305,8 +305,8 @@ void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(float rate_roll_target
}
// For legacy reasons, we convert to centi-degrees before inputting to the feedforward
roll_ff = roll_feedforward_filter.apply(_pid_rate_roll.get_vff(rate_roll_target_rads), _dt);
pitch_ff = pitch_feedforward_filter.apply(_pid_rate_pitch.get_vff(rate_pitch_target_rads), _dt);
roll_ff = roll_feedforward_filter.apply(_pid_rate_roll.get_ff(rate_roll_target_rads), _dt);
pitch_ff = pitch_feedforward_filter.apply(_pid_rate_pitch.get_ff(rate_pitch_target_rads), _dt);
// add feed forward and final output
roll_out = roll_pd + roll_i + roll_ff;
@ -390,7 +390,7 @@ float AC_AttitudeControl_Heli::rate_target_to_motor_yaw(float rate_target_rads)
}
// For legacy reasons, we convert to centi-degrees before inputting to the feedforward
vff = yaw_velocity_feedforward_filter.apply(_pid_rate_yaw.get_vff(rate_target_rads), _dt);
vff = yaw_velocity_feedforward_filter.apply(_pid_rate_yaw.get_ff(rate_target_rads), _dt);
// add feed forward
yaw_out = pd + i + vff;