aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_AttitudeControl: fixup more names

Browse Source
This commit is contained in:
Jonathan Challinger 2015-12-02 10:45:34 -08:00 committed by Randy Mackay
parent 41e580e53a
commit 0baf86c485
3 changed files with 15 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -240,7 +240,7 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle
// Convert from centidegrees on public interface to radians
float euler_roll_angle_rad = radians(euler_roll_angle_cd*0.01f);
float euler_pitch_angle_rad = radians(euler_pitch_angle_cd*0.01f);
float yaw_angle_ef_rad = radians(euler_yaw_angle_cd*0.01f);
float euler_yaw_angle_rad = radians(euler_yaw_angle_cd*0.01f);
Vector3f att_error_euler_rad;
@ -250,7 +250,7 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle
// Set attitude targets from input.
_att_target_euler_rad.x = constrain_float(euler_roll_angle_rad, -get_tilt_limit_rad(), get_tilt_limit_rad());
_att_target_euler_rad.y = constrain_float(euler_pitch_angle_rad, -get_tilt_limit_rad(), get_tilt_limit_rad());
_att_target_euler_rad.z = yaw_angle_ef_rad;
_att_target_euler_rad.z = euler_yaw_angle_rad;
// Update attitude error.
att_error_euler_rad.x = wrap_PI(_att_target_euler_rad.x - _ahrs.roll);
@ -374,12 +374,12 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, fl
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)
void AC_AttitudeControl::input_att_quat_bf_ang_vel(const Quaternion& att_target_quat, const Vector3f& att_target_ang_vel_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);
_att_target_ang_vel_rads = att_target_ang_vel_rads;
ang_vel_to_euler_derivative(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), att_target_ang_vel_rads, _att_target_euler_deriv_rads);
// Retrieve quaternion vehicle attitude
// TODO add _ahrs.get_quaternion()

10
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -120,13 +120,13 @@ public:
void input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds);
// Command an euler roll, pitch and yaw angle
void input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_ef_cd, float euler_yaw_angle_cd, bool slew_yaw);
void input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw);
// 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);
void input_att_quat_bf_ang_vel(const Quaternion& att_target_quat, const Vector3f& att_target_ang_vel_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);
@ -217,13 +217,13 @@ public:
protected:
// Update _att_target_euler_rad.x by integrating a 321-intrinsic euler roll angle derivative
void update_att_target_and_error_roll(float roll_rate_ef_rads, Vector3f &angle_ef_error_rad, float overshoot_max_rad);
void update_att_target_and_error_roll(float euler_roll_rate_rads, Vector3f &att_error_euler_rad, float overshoot_max_rad);
// Update _att_target_euler_rad.y by integrating a 321-intrinsic euler pitch angle derivative
void update_att_target_and_error_pitch(float pitch_rate_ef_rads, Vector3f &angle_ef_error_rad, float overshoot_max_rad);
void update_att_target_and_error_pitch(float euler_pitch_rate_rads, Vector3f &att_error_euler_rad, float overshoot_max_rad);
// Update _att_target_euler_rad.z by integrating a 321-intrinsic euler yaw angle derivative
void update_att_target_and_error_yaw(float yaw_rate_ef_rads, Vector3f &angle_ef_error_rad, float overshoot_max_rad);
void update_att_target_and_error_yaw(float euler_yaw_rate_rads, Vector3f &att_error_euler_rad, float overshoot_max_rad);
// Integrate vehicle rate into _att_error_rot_vec_rad
void integrate_bf_rate_error_to_angle_errors();

10
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
View File

@ -60,14 +60,14 @@ void AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(float roll_pass
_att_error_rot_vec_rad.y = 0;
// update our earth-frame angle targets
Vector3f angle_ef_error_rad;
Vector3f att_error_euler_rad;
// convert angle error rotation vector into 321-intrinsic euler angle difference
// NOTE: this results an an approximation linearized about the vehicle's attitude
if (ang_vel_to_euler_derivative(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), _att_error_rot_vec_rad, angle_ef_error_rad)) {
_att_target_euler_rad.x = wrap_PI(angle_ef_error_rad.x + _ahrs.roll);
_att_target_euler_rad.y = wrap_PI(angle_ef_error_rad.y + _ahrs.pitch);
_att_target_euler_rad.z = wrap_2PI(angle_ef_error_rad.z + _ahrs.yaw);
if (ang_vel_to_euler_derivative(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), _att_error_rot_vec_rad, att_error_euler_rad)) {
_att_target_euler_rad.x = wrap_PI(att_error_euler_rad.x + _ahrs.roll);
_att_target_euler_rad.y = wrap_PI(att_error_euler_rad.y + _ahrs.pitch);
_att_target_euler_rad.z = wrap_2PI(att_error_euler_rad.z + _ahrs.yaw);
}
// handle flipping over pitch axis