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

AC_AttitudeControl: add input_att_quat_bf_ang_vel and use for ACRO

This commit is contained in:
Jonathan Challinger 2015-11-30 13:33:07 -08:00 committed by Randy Mackay
parent dfab21171b
commit 41e580e53a
2 changed files with 15 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -338,8 +338,6 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, fl
float pitch_rate_bf_rads = radians(pitch_rate_bf_cds*0.01f);
float yaw_rate_bf_rads = radians(yaw_rate_bf_cds*0.01f);
Vector3f att_error_euler_rad;
// Compute acceleration-limited body-frame roll rate
if (get_accel_roll_max_radss() > 0.0f) {
float rate_change_limit_rads = get_accel_roll_max_radss() * _dt;
@ -372,10 +370,19 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, fl
att_target_quat.rotate(_att_target_ang_vel_rads*_dt);
att_target_quat.normalize();
// Update euler attitude target
// Call quaternion attitude controller
input_att_quat_bf_ang_vel(att_target_quat, _att_target_ang_vel_rads);
}
void AC_AttitudeControl::input_att_quat_bf_ang_vel(const Quaternion& att_target_quat, const Vector3f& ang_vel_target_rads)
{
// Update euler attitude target and angular velocity targets
att_target_quat.to_euler(_att_target_euler_rad.x,_att_target_euler_rad.y,_att_target_euler_rad.z);
_att_target_ang_vel_rads = ang_vel_target_rads;
ang_vel_to_euler_derivative(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), ang_vel_target_rads, _att_target_euler_deriv_rads);
// Retrieve quaternion vehicle attitude
// TODO add _ahrs.get_quaternion()
Quaternion att_vehicle_quat;
att_vehicle_quat.from_rotation_matrix(_ahrs.get_dcm_matrix());
@ -386,12 +393,11 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, fl
update_ang_vel_target_from_att_error();
// Add the angular velocity feedforward
// NOTE: Rotation of _att_target_ang_vel_rads from desired body frame to estimated body frame is possibly omitted here
_ang_vel_target_rads += _att_target_ang_vel_rads;
// Keep euler derivative updated
ang_vel_to_euler_derivative(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), _att_target_ang_vel_rads, _att_target_euler_deriv_rads);
}
void AC_AttitudeControl::rate_controller_run()
{
_motors.set_roll(rate_bf_to_motor_roll(_ang_vel_target_rads.x));

3
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -125,6 +125,9 @@ public:
// Command an euler roll, pitch, and yaw rate
void input_euler_rate_roll_pitch_yaw(float euler_roll_rate_cds, float euler_pitch_rate_cds, float euler_yaw_rate_cds);
// Command a quaternion attitude and a body-frame angular velocity
void input_att_quat_bf_ang_vel(const Quaternion& att_target_quat, const Vector3f& ang_vel_target_rads);
// Command an angular velocity
virtual void input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds);