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

AC_AttitudeControl: Remove Unused Function

This commit is contained in:
Leonard Hall 2021-04-15 22:06:27 +09:30 committed by Randy Mackay
parent 726074be91
commit e253c94f63
2 changed files with 0 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -299,67 +299,6 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler
attitude_controller_run_quat(); attitude_controller_run_quat();
} }
// Command an euler roll pitch and yaw angle and an euler yaw rate for use with input shaped commands
void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, float euler_yaw_rate_cds)
{
// Convert from centidegrees on public interface to radians
float euler_roll_angle = radians(euler_roll_angle_cd * 0.01f);
float euler_pitch_angle = radians(euler_pitch_angle_cd * 0.01f);
float euler_yaw_angle = radians(euler_yaw_angle_cd * 0.01f);
float euler_yaw_rate = radians(euler_yaw_rate_cds * 0.01f);
// calculate the attitude target euler angles
_attitude_target.to_euler(_euler_angle_target.x, _euler_angle_target.y, _euler_angle_target.z);
// Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
euler_roll_angle += get_roll_trim_rad();
if (_rate_bf_ff_enabled) {
// translate the roll pitch and yaw acceleration limits to the euler axis
const Vector3f euler_accel = euler_accel_limit(_euler_angle_target, Vector3f{get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()});
// When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler
// angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration
// and an exponential decay specified by _input_tc at the end.
_attitude_correction_euler_rate.x = input_shaping_angle(wrap_PI(euler_roll_angle - _euler_angle_target.x), 0.0f, euler_accel.x, _attitude_correction_euler_rate.x, _dt);
_attitude_correction_euler_rate.y = input_shaping_angle(wrap_PI(euler_pitch_angle - _euler_angle_target.y), 0.0f, euler_accel.y, _attitude_correction_euler_rate.y, _dt);
_attitude_correction_euler_rate.z = input_shaping_angle(wrap_PI(euler_yaw_angle - _euler_angle_target.z), _input_tc, euler_accel.z, _attitude_correction_euler_rate.z, _dt);
_attitude_correction_euler_rate.z = constrain_float(_attitude_correction_euler_rate.z, -get_slew_yaw_rads(), get_slew_yaw_rads());
_euler_rate_target = _attitude_correction_euler_rate;
// When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
// the output rate towards the input rate.
_attitude_desired_euler_rate.z = input_shaping_ang_vel(_attitude_desired_euler_rate.z, euler_yaw_rate, euler_accel.z, _dt);
_euler_rate_target.z += _attitude_desired_euler_rate.z;
_euler_rate_target.z = constrain_float(_euler_rate_target.z, -get_slew_yaw_rads(), get_slew_yaw_rads());
// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
euler_rate_to_ang_vel(_euler_angle_target, _euler_rate_target, _ang_vel_target);
// Limit the angular velocity
ang_vel_limit(_ang_vel_target, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));
// Convert body-frame angular velocity into euler angle derivative of desired attitude
ang_vel_to_euler_rate(_euler_angle_target, _ang_vel_target, _euler_rate_target);
} else {
// When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed.
_euler_angle_target.x = euler_roll_angle;
_euler_angle_target.y = euler_pitch_angle;
// Compute constrained angle error
float angle_error = constrain_float(wrap_PI(euler_yaw_angle - _euler_angle_target.z), -get_slew_yaw_rads() * _dt, get_slew_yaw_rads() * _dt);
// Update attitude target from constrained angle error
_euler_angle_target.z = wrap_PI(angle_error + _euler_angle_target.z);
// Compute quaternion target attitude
_attitude_target.from_euler(_euler_angle_target.x, _euler_angle_target.y, _euler_angle_target.z);
// Set rate feedforward requests to zero
_euler_rate_target.zero();
_ang_vel_target.zero();
}
// Call quaternion attitude controller
attitude_controller_run_quat();
}
// Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing // Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing
void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw) void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw)
{ {

7
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -146,10 +146,6 @@ public:
// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing // Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
virtual void input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds); virtual 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 and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
virtual void input_euler_angle_roll_pitch_yaw_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, float euler_yaw_rate_cds);
// Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing // Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing
virtual 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); virtual 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);
@ -416,9 +412,6 @@ protected:
// second. // second.
Vector3f _euler_rate_target; Vector3f _euler_rate_target;
Vector3f _attitude_desired_euler_rate;
Vector3f _attitude_correction_euler_rate;
// This represents a quaternion rotation in NED frame to the target (setpoint) // This represents a quaternion rotation in NED frame to the target (setpoint)
// attitude used in the attitude controller. // attitude used in the attitude controller.
Quaternion _attitude_target; Quaternion _attitude_target;