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

AC_AttitudeControl: replace smoothing gain with INPUT_TC

Browse Source
This commit is contained in:
Randy Mackay 2018-01-26 14:56:54 +09:00
parent e1e224b68b
commit 59a2445ad3
2 changed files with 37 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -2,6 +2,14 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
// default gains for Plane
# define AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT 0.2f // Soft
#else
// default gains for Copter and Sub
# define AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT 0.15f // Medium
#endif
// table of user settable parameters
const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = {
@ -122,6 +130,15 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = {
// @User: Advanced
AP_GROUPINFO("RATE_Y_MAX", 19, AC_AttitudeControl, _ang_vel_yaw_max, 0.0f),
// @Param: INPUT_TC
// @DisplayName: Attitude control input time constant (aka smoothing)
// @Description: Attitude control input time constant. Low numbers lead to sharper response, higher numbers to softer response
// @Range: 0 1
// @Increment: 0.01
// @Values: 0.5:Very Soft, 0.2:Soft, 0.15:Medium, 0.1:Crisp, 0.05:Very Crisp
// @User: Standard
AP_GROUPINFO("INPUT_TC", 20, AC_AttitudeControl, _input_tc, AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT),
AP_GROUPEND
};
@ -198,10 +215,10 @@ void AC_AttitudeControl::input_quaternion(Quaternion attitude_desired_quat)
if (_rate_bf_ff_enabled & _use_ff_and_input_shaping) {
// 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 _smoothing_gain at the end.
_attitude_target_ang_vel.x = input_shaping_angle(wrap_PI(attitude_error_angle.x), _smoothing_gain, get_accel_roll_max_radss(), _attitude_target_ang_vel.x, _dt);
_attitude_target_ang_vel.y = input_shaping_angle(wrap_PI(attitude_error_angle.y), _smoothing_gain, get_accel_pitch_max_radss(), _attitude_target_ang_vel.y, _dt);
_attitude_target_ang_vel.z = input_shaping_angle(wrap_PI(attitude_error_angle.z), _smoothing_gain, get_accel_yaw_max_radss(), _attitude_target_ang_vel.z, _dt);
// and an exponential decay specified by _input_tc at the end.
_attitude_target_ang_vel.x = input_shaping_angle(wrap_PI(attitude_error_angle.x), _input_tc, get_accel_roll_max_radss(), _attitude_target_ang_vel.x, _dt);
_attitude_target_ang_vel.y = input_shaping_angle(wrap_PI(attitude_error_angle.y), _input_tc, get_accel_pitch_max_radss(), _attitude_target_ang_vel.y, _dt);
_attitude_target_ang_vel.z = input_shaping_angle(wrap_PI(attitude_error_angle.z), _input_tc, get_accel_yaw_max_radss(), _attitude_target_ang_vel.z, _dt);
// Limit the angular velocity
ang_vel_limit(_attitude_target_ang_vel, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));
@ -240,8 +257,8 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler
// 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 smoothing_gain at the end.
_attitude_target_euler_rate.x = input_shaping_angle(wrap_PI(euler_roll_angle-_attitude_target_euler_angle.x), _smoothing_gain, euler_accel.x, _attitude_target_euler_rate.x, _dt);
_attitude_target_euler_rate.y = input_shaping_angle(wrap_PI(euler_pitch_angle-_attitude_target_euler_angle.y), _smoothing_gain, euler_accel.y, _attitude_target_euler_rate.y, _dt);
_attitude_target_euler_rate.x = input_shaping_angle(wrap_PI(euler_roll_angle-_attitude_target_euler_angle.x), _input_tc, euler_accel.x, _attitude_target_euler_rate.x, _dt);
_attitude_target_euler_rate.y = input_shaping_angle(wrap_PI(euler_pitch_angle-_attitude_target_euler_angle.y), _input_tc, euler_accel.y, _attitude_target_euler_rate.y, _dt);
// 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.
@ -290,10 +307,10 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle
// 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 _smoothing_gain at the end.
_attitude_target_euler_rate.x = input_shaping_angle(wrap_PI(euler_roll_angle-_attitude_target_euler_angle.x), _smoothing_gain, euler_accel.x, _attitude_target_euler_rate.x, _dt);
_attitude_target_euler_rate.y = input_shaping_angle(wrap_PI(euler_pitch_angle-_attitude_target_euler_angle.y), _smoothing_gain, euler_accel.y, _attitude_target_euler_rate.y, _dt);
_attitude_target_euler_rate.z = input_shaping_angle(wrap_PI(euler_yaw_angle-_attitude_target_euler_angle.z), _smoothing_gain, euler_accel.z, _attitude_target_euler_rate.z, _dt);
// and an exponential decay specified by _input_tc at the end.
_attitude_target_euler_rate.x = input_shaping_angle(wrap_PI(euler_roll_angle-_attitude_target_euler_angle.x), _input_tc, euler_accel.x, _attitude_target_euler_rate.x, _dt);
_attitude_target_euler_rate.y = input_shaping_angle(wrap_PI(euler_pitch_angle-_attitude_target_euler_angle.y), _input_tc, euler_accel.y, _attitude_target_euler_rate.y, _dt);
_attitude_target_euler_rate.z = input_shaping_angle(wrap_PI(euler_yaw_angle-_attitude_target_euler_angle.z), _input_tc, euler_accel.z, _attitude_target_euler_rate.z, _dt);
if (slew_yaw) {
_attitude_target_euler_rate.z = constrain_float(_attitude_target_euler_rate.z, -get_slew_yaw_rads(), get_slew_yaw_rads());
}
@ -537,11 +554,11 @@ void AC_AttitudeControl::thrust_heading_rotation_angles(Quaternion& att_to_quat,
}
// calculates the velocity correction from an angle error. The angular velocity has acceleration and
// deceleration limits including basic jerk limiting using _smoothing_gain
float AC_AttitudeControl::input_shaping_angle(float error_angle, float smoothing_gain, float accel_max, float target_ang_vel, float dt)
// deceleration limits including basic jerk limiting using _input_tc
float AC_AttitudeControl::input_shaping_angle(float error_angle, float input_tc, float accel_max, float target_ang_vel, float dt)
{
// Calculate the velocity as error approaches zero with acceleration limited by accel_max_radss
float desired_ang_vel = sqrt_controller(error_angle, smoothing_gain, accel_max, dt);
float desired_ang_vel = sqrt_controller(error_angle, 1.0f / MAX(input_tc, 0.01f), accel_max, dt);
// Acceleration is limited directly to smooth the beginning of the curve.
return input_shaping_ang_vel(target_ang_vel, desired_ang_vel, accel_max, dt);
@ -565,8 +582,8 @@ void AC_AttitudeControl::input_shaping_rate_predictor(Vector2f error_angle, Vect
{
if (_rate_bf_ff_enabled) {
// translate the roll pitch and yaw acceleration limits to the euler axis
target_ang_vel.x = input_shaping_angle(wrap_PI(error_angle.x), _smoothing_gain, get_accel_roll_max_radss(), target_ang_vel.x, dt);
target_ang_vel.y = input_shaping_angle(wrap_PI(error_angle.y), _smoothing_gain, get_accel_pitch_max_radss(), target_ang_vel.y, dt);
target_ang_vel.x = input_shaping_angle(wrap_PI(error_angle.x), _input_tc, get_accel_roll_max_radss(), target_ang_vel.x, dt);
target_ang_vel.y = input_shaping_angle(wrap_PI(error_angle.y), _input_tc, get_accel_pitch_max_radss(), target_ang_vel.y, dt);
} else {
target_ang_vel.x = _p_angle_roll.get_p(wrap_PI(error_angle.x));
target_ang_vel.y = _p_angle_pitch.get_p(wrap_PI(error_angle.y));

10
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -104,8 +104,8 @@ public:
// Sets and saves the yaw acceleration limit in centidegrees/s/s
void save_accel_yaw_max(float accel_yaw_max) { _accel_yaw_max.set_and_save(accel_yaw_max); }
// Sets the yaw acceleration limit in centidegrees/s/s
void set_smoothing_gain(float smoothing_gain) { _smoothing_gain = constrain_float(smoothing_gain,1.0f,1/_dt); }
// set the rate control input smoothing time constant
void set_input_tc(float input_tc) { _input_tc = constrain_float(input_tc, 0.0f, 1.0f); }
// Ensure attitude controller have zero errors to relax rate controller output
void relax_attitude_controllers();
@ -338,6 +338,9 @@ protected:
// Angle limit time constant (to maintain altitude)
AP_Float _angle_limit_tc;
// rate controller input smoothing time constant
AP_Float _input_tc;
// Intersampling period in seconds
float _dt;
@ -385,9 +388,6 @@ protected:
// mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1
float _throttle_rpy_mix;
// smoothing gain (should be replaced with s-curve generation)
float _smoothing_gain;
// References to external libraries
const AP_AHRS_View& _ahrs;
const AP_Vehicle::MultiCopter &_aparm;