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

AC_AttControl: add step input for autotune

This commit is contained in:
Leonard Hall 2017-06-15 22:40:01 +09:30 committed by Randy Mackay
parent 51c20637e5
commit 0544cf1d82
2 changed files with 29 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -376,6 +376,32 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, fl
attitude_controller_run_quat(); attitude_controller_run_quat();
} }
// Command an angular step (i.e change) in body frame angle
// Used to command a step in angle without exciting the orthogonal axis during autotune
void AC_AttitudeControl::input_angle_step_bf_roll_pitch_yaw(float roll_angle_step_bf_cd, float pitch_angle_step_bf_cd, float yaw_angle_step_bf_cd)
{
// Convert from centidegrees on public interface to radians
float roll_step_rads = radians(roll_angle_step_bf_cd*0.01f);
float pitch_step_rads = radians(pitch_angle_step_bf_cd*0.01f);
float yaw_step_rads = radians(yaw_angle_step_bf_cd*0.01f);
// rotate attitude target by desired step
Quaternion attitude_target_update_quat;
attitude_target_update_quat.from_axis_angle(Vector3f(roll_step_rads, pitch_step_rads, yaw_step_rads));
_attitude_target_quat = _attitude_target_quat * attitude_target_update_quat;
_attitude_target_quat.normalize();
// calculate the attitude target euler angles
_attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
// Set rate feedforward requests to zero
_attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);
_attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
// Call quaternion attitude controller
attitude_controller_run_quat();
}
// Calculates the body frame angular velocities to follow the target attitude // Calculates the body frame angular velocities to follow the target attitude
void AC_AttitudeControl::attitude_controller_run_quat() void AC_AttitudeControl::attitude_controller_run_quat()
{ {

3
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -128,6 +128,9 @@ public:
// Command an angular velocity with angular velocity feedforward and smoothing // Command an angular velocity with angular velocity feedforward and smoothing
virtual void input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds); virtual void input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds);
// Command an angular step (i.e change) in body frame angle
virtual void input_angle_step_bf_roll_pitch_yaw(float roll_angle_step_bf_cd, float pitch_angle_step_bf_cd, float yaw_angle_step_bf_cd);
// 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;