Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-04 23:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: add set smoothing gain

Browse Source 
Smoothing gain value should be set once when entering a mode
This commit is contained in:
Leonard Hall 2017-06-26 00:03:16 +09:30 committed by Randy Mackay
parent 213f7a4061
commit 6175a896ee
4 changed files with 29 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -156,14 +156,11 @@ void AC_AttitudeControl::reset_rate_controller_I_terms()
// Command a Quaternion attitude with feedforward and smoothing
void AC_AttitudeControl::input_quaternion(Quaternion attitude_desired_quat, float smoothing_gain)
void AC_AttitudeControl::input_quaternion(Quaternion attitude_desired_quat)
{
// 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);
// ensure smoothing gain can not cause overshoot
smoothing_gain = constrain_float(smoothing_gain,1.0f,1/_dt);
Quaternion attitude_error_quat = _attitude_target_quat.inverse() * attitude_desired_quat;
Vector3f attitude_error_angle;
attitude_error_quat.to_axis_angle(attitude_error_angle);
@ -171,10 +168,10 @@ void AC_AttitudeControl::input_quaternion(Quaternion attitude_desired_quat, floa
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 _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);
// Convert body-frame angular velocity into euler angle derivative of desired attitude
ang_vel_to_euler_rate(_attitude_target_euler_angle, _attitude_target_ang_vel, _attitude_target_euler_rate);
@ -191,7 +188,7 @@ void AC_AttitudeControl::input_quaternion(Quaternion attitude_desired_quat, floa
}
// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds, float smoothing_gain)
void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_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);
@ -201,9 +198,6 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler
// 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);
// ensure smoothing gain can not cause overshoot
smoothing_gain = constrain_float(smoothing_gain,1.0f,1/_dt);
// Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
euler_roll_angle += get_roll_trim_rad();
@ -214,8 +208,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), _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);
// 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.
@ -241,7 +235,7 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler
}
// 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, float smoothing_gain)
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)
{
// Convert from centidegrees on public interface to radians
float euler_roll_angle = radians(euler_roll_angle_cd*0.01f);
@ -251,9 +245,6 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle
// 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);
// ensure smoothing gain can not cause overshoot
smoothing_gain = constrain_float(smoothing_gain,1.0f,1/_dt);
// Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
euler_roll_angle += get_roll_trim_rad();
@ -263,10 +254,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 _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);
if (slew_yaw) {
_attitude_target_euler_rate.z = constrain_float(_attitude_target_euler_rate.z, -get_slew_yaw_rads(), get_slew_yaw_rads());
}
@ -498,7 +489,7 @@ 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
// 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)
{
// Calculate the velocity as error approaches zero with acceleration limited by accel_max_radss

12
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -104,6 +104,9 @@ 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); }
// Ensure attitude controller have zero errors to relax rate controller output
void relax_attitude_controllers();
@ -120,13 +123,13 @@ public:
void shift_ef_yaw_target(float yaw_shift_cd);
// Command a Quaternion attitude with feedforward and smoothing
void input_quaternion(Quaternion attitude_desired_quat, float smoothing_gain);
void input_quaternion(Quaternion attitude_desired_quat);
// 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, float smoothing_gain);
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, 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, float smoothing_gain);
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);
// Command an euler roll, pitch, and yaw rate with angular velocity feedforward and smoothing
void input_euler_rate_roll_pitch_yaw(float euler_roll_rate_cds, float euler_pitch_rate_cds, float euler_yaw_rate_cds);
@ -370,6 +373,9 @@ 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;

8
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
View File

@ -439,19 +439,19 @@ void AC_AttitudeControl_Heli::set_throttle_out(float throttle_in, bool apply_ang
}
// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
void AC_AttitudeControl_Heli::input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds, float smoothing_gain)
void AC_AttitudeControl_Heli::input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds)
{
if (_inverted_flight) {
euler_roll_angle_cd = wrap_180_cd(euler_roll_angle_cd + 18000);
}
AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(euler_roll_angle_cd, euler_pitch_angle_cd, euler_yaw_rate_cds, smoothing_gain);
AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(euler_roll_angle_cd, euler_pitch_angle_cd, euler_yaw_rate_cds);
}
// Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing
void AC_AttitudeControl_Heli::input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw, float smoothing_gain)
void AC_AttitudeControl_Heli::input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw)
{
if (_inverted_flight) {
euler_roll_angle_cd = wrap_180_cd(euler_roll_angle_cd + 18000);
}
AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(euler_roll_angle_cd, euler_pitch_angle_cd, euler_yaw_angle_cd, slew_yaw, smoothing_gain);
AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(euler_roll_angle_cd, euler_pitch_angle_cd, euler_yaw_angle_cd, slew_yaw);
}

4
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
View File

@ -97,10 +97,10 @@ public:
void set_throttle_out(float throttle_in, bool apply_angle_boost, float filt_cutoff) override;
// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
void input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds, float smoothing_gain) override;
void input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds) override;
// Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing
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, float smoothing_gain) override;
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) override;
// enable/disable inverted flight
void set_inverted_flight(bool inverted) override {