Skip to content

Improved command model 2 #4

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
bnsgeyer wants to merge 5 commits into
base: Copter-4.0
Choose a base branch
from
Open

Improved command model 2 #4

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
1dc608d
AC_AttitudeControl: incorporate time delay with feedforward D gain
bnsgeyer Jan 30, 2020
0cf1f09
AC_AttitudeControl: Command model changes
bnsgeyer Jan 30, 2020
b467420
AC_AttitudeControl: update jerk limiting in command model
bnsgeyer Jan 30, 2020
9d56479
AC_AttitudeControl: remove accel limit function from attitude shaping…
bnsgeyer Feb 1, 2020
ad1c6b2
AC_AttitudeControl: modify rate limiting for new attitude shaping tec…
bnsgeyer Feb 4, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
150 changes: 135 additions & 15 deletions libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -144,6 +144,24 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = {
// @User: Standard
AP_GROUPINFO("INPUT_TC", 20, AC_AttitudeControl, _input_tc, AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT),

// @Param: _TIME_DELAY
// @DisplayName: Time Delay
// @Description: Time Delay of command model states to determine PID error
// @Units: msec
// @Range: 0 100
// @Increment: 1
// @User: Standard
AP_GROUPINFO("TIME_DELAY", 21, AC_AttitudeControl, _time_delay, 0),

// @Param: _RATE_TC
// @DisplayName: Rate Commmand Model Shaping Time Constant
// @Description: Rate Commmand Model Shaping Time Constant. Low numbers lead to sharper response, higher numbers to softer response
// @Units: hz
// @Range: 0.05 1
// @Increment: 0.01
// @User: Standard
AP_GROUPINFO("RATE_TC", 22, AC_AttitudeControl, _input_rate_tc, AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT),

AP_GROUPEND
};

Expand Down Expand Up @@ -235,6 +253,49 @@ void AC_AttitudeControl::input_quaternion(Quaternion attitude_desired_quat)
attitude_controller_run_quat();
}

// calculates the velocity correction from an angle error. The angular velocity has acceleration and
// deceleration limits including basic jerk limiting using _input_tc
float AC_AttitudeControl::input_shaping_angle_2(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
return sqrt_controller(error_angle, 1.0f / MAX(input_tc, 0.01f), accel_max, dt);

}

// attitude shaping
float AC_AttitudeControl::input_attitude_shaping(float input_tc, float euler_desired_angle, float euler_target_angle, float euler_target_rate, float &euler_target_accel, float &euler_target_jerk, float accel_limit, float dt)
{

// Calculate the velocity as error approaches zero with acceleration limited by accel_max_radss
float euler_desired_rate = sqrt_controller(euler_desired_angle - euler_target_angle, 1.0f / MAX(input_tc, 0.01f), accel_limit, dt);

// Acceleration and jerk is limited directly to smooth the beginning of the curve using the rate shaping function.
return input_rate_shaping(MAX(input_tc, 0.01f) / 3.0f, euler_desired_rate, euler_target_rate, euler_target_accel, euler_target_jerk, accel_limit, dt);

}

// rate shaping
float AC_AttitudeControl::input_rate_shaping(float input_tc, float euler_desired_rate, float euler_target_rate, float &euler_target_accel, float &euler_target_jerk, float accel_limit, float dt)
{
float zeta = 0.85;
float freq = 6.283185 / MAX(3.0f*input_tc, 0.01f);
float desired_accel = (euler_desired_rate - euler_target_rate) * freq / (2.0f * zeta);
if (is_positive(accel_limit)) {
desired_accel = constrain_float(desired_accel, -accel_limit, accel_limit);
}
float desired_jerk = (desired_accel - euler_target_accel) * 2 * zeta * freq;
float rc = 3.0f / (freq * 6.283185);
float jerk_error = desired_jerk - euler_target_jerk;
// only limit jerk when acceleration is in the same direction as jerk
if ((is_positive(desired_accel) && is_positive(desired_jerk) && is_positive(jerk_error)) ||
(is_negative(desired_accel) && is_negative(desired_jerk) && is_negative(jerk_error))) {
desired_jerk = euler_target_jerk + jerk_error * dt / (dt + rc);
}
euler_target_accel += desired_jerk * dt;
euler_target_rate += euler_target_accel * dt;
return euler_target_rate;
}

// 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)
{
Expand All @@ -256,19 +317,29 @@ 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), _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);
float desired_euler_roll_rate = input_shaping_angle_2(wrap_PI(euler_roll_angle - _attitude_target_euler_angle.x), _input_tc, euler_accel.x, _attitude_target_euler_rate.x, _dt);
float desired_euler_pitch_rate = input_shaping_angle_2(wrap_PI(euler_pitch_angle - _attitude_target_euler_angle.y), _input_tc, euler_accel.y, _attitude_target_euler_rate.y, _dt);

// Limit desired rate prior to rate shaping function
Vector3f desired_euler_rate = Vector3f(desired_euler_roll_rate, desired_euler_pitch_rate, euler_yaw_rate);
Vector3f desired_euler_ang_vel;
// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
euler_rate_to_ang_vel(_attitude_target_euler_angle, desired_euler_rate, desired_euler_ang_vel);
// Limit the angular velocity
ang_vel_limit(desired_euler_ang_vel, 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(_attitude_target_euler_angle, desired_euler_ang_vel, desired_euler_rate);

// use rate shaping to achieve target rates smoothly providing acheivable jerk and limiting accel. Input TC adjusted for rate shaping to allow balanced acceleration and deceleration to desired rates.
_attitude_target_euler_rate.x = input_rate_shaping(MAX(_input_tc, 0.01f) / 3.3f, desired_euler_rate.x, _attitude_target_euler_rate.x, _attitude_target_euler_accel.x, _attitude_target_euler_jerk.x, euler_accel.x, _dt);
_attitude_target_euler_rate.y = input_rate_shaping(MAX(_input_tc, 0.01f) / 3.3f, desired_euler_rate.y, _attitude_target_euler_rate.y, _attitude_target_euler_accel.y, _attitude_target_euler_jerk.y, euler_accel.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.
_attitude_target_euler_rate.z = input_shaping_ang_vel(_attitude_target_euler_rate.z, euler_yaw_rate, euler_accel.z, _dt);
_attitude_target_euler_rate.z = input_rate_shaping(_input_rate_tc, desired_euler_rate.z, _attitude_target_euler_rate.z, _attitude_target_euler_accel.z, _attitude_target_euler_jerk.z, euler_accel.z, _dt);

// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
euler_rate_to_ang_vel(_attitude_target_euler_angle, _attitude_target_euler_rate, _attitude_target_ang_vel);
// 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));
// 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);
} else {
// When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed.
_attitude_target_euler_angle.x = euler_roll_angle;
Expand Down Expand Up @@ -518,9 +589,12 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, fl
// Compute acceleration-limited body frame rates
// When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing
// the output rate towards the input rate.
_attitude_target_ang_vel.x = input_shaping_ang_vel(_attitude_target_ang_vel.x, roll_rate_rads, get_accel_roll_max_radss(), _dt);
_attitude_target_ang_vel.y = input_shaping_ang_vel(_attitude_target_ang_vel.y, pitch_rate_rads, get_accel_pitch_max_radss(), _dt);
_attitude_target_ang_vel.z = input_shaping_ang_vel(_attitude_target_ang_vel.z, yaw_rate_rads, get_accel_yaw_max_radss(), _dt);
// _attitude_target_ang_vel.x = input_shaping_ang_vel(_attitude_target_ang_vel.x, roll_rate_rads, get_accel_roll_max_radss(), _dt);
_attitude_target_ang_vel.x = input_rate_shaping(_input_rate_tc, roll_rate_rads, _attitude_target_ang_vel.x, _attitude_target_ang_accel.x, _attitude_target_ang_jerk.x, get_accel_roll_max_radss(), _dt);
// _attitude_target_ang_vel.y = input_shaping_ang_vel(_attitude_target_ang_vel.y, pitch_rate_rads, get_accel_pitch_max_radss(), _dt);
_attitude_target_ang_vel.y = input_rate_shaping(_input_rate_tc, pitch_rate_rads, _attitude_target_ang_vel.y, _attitude_target_ang_accel.y, _attitude_target_ang_jerk.y, get_accel_pitch_max_radss(), _dt);
// _attitude_target_ang_vel.z = input_shaping_ang_vel(_attitude_target_ang_vel.z, yaw_rate_rads, get_accel_yaw_max_radss(), _dt);
_attitude_target_ang_vel.z = input_rate_shaping(_input_rate_tc, yaw_rate_rads, _attitude_target_ang_vel.z, _attitude_target_ang_accel.z, _attitude_target_ang_jerk.z, get_accel_yaw_max_radss(), _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);
Expand Down Expand Up @@ -649,9 +723,32 @@ void AC_AttitudeControl::attitude_controller_run_quat()
Quaternion attitude_vehicle_quat;
_ahrs.get_quat_body_to_ned(attitude_vehicle_quat);

// make ang vel quaternion
Quaternion attitude_target_ang_vel_quat = Quaternion(0.0f, _attitude_target_ang_vel.x, _attitude_target_ang_vel.y, _attitude_target_ang_vel.z);

// Get delayed target states
Quaternion delayed_attitude_target_quat;
Quaternion delayed_attitude_target_ang_vel_quat;

if (_time_delay == 0) {
delayed_attitude_target_quat = _attitude_target_quat;
delayed_attitude_target_ang_vel_quat = attitude_target_ang_vel_quat;
} else if (target_states_buffer == nullptr) {
uint16_t buffer_size = constrain_int16(_time_delay, 1, 100) * 0.001f / _dt;
target_states_buffer = new ObjectBuffer<target_states>(buffer_size);
while (target_states_buffer->space() != 0) {
push_to_buffer(_attitude_target_quat, attitude_target_ang_vel_quat);
}
delayed_attitude_target_quat = _attitude_target_quat;
delayed_attitude_target_ang_vel_quat = attitude_target_ang_vel_quat;
} else {
pull_from_buffer(delayed_attitude_target_quat, delayed_attitude_target_ang_vel_quat);
push_to_buffer(_attitude_target_quat, attitude_target_ang_vel_quat);
}

// Compute attitude error
Vector3f attitude_error_vector;
thrust_heading_rotation_angles(_attitude_target_quat, attitude_vehicle_quat, attitude_error_vector, _thrust_error_angle);
thrust_heading_rotation_angles(delayed_attitude_target_quat, attitude_vehicle_quat, attitude_error_vector, _thrust_error_angle);

// Compute the angular velocity target from the attitude error
_rate_target_ang_vel = update_ang_vel_target_from_att_error(attitude_error_vector);
Expand All @@ -663,10 +760,13 @@ void AC_AttitudeControl::attitude_controller_run_quat()

ang_vel_limit(_rate_target_ang_vel, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));

// Add the angular velocity feedforward, rotated into vehicle frame
Quaternion attitude_target_ang_vel_quat = Quaternion(0.0f, _attitude_target_ang_vel.x, _attitude_target_ang_vel.y, _attitude_target_ang_vel.z);
// determine feedforward components of desired velocity
Quaternion to_to_from_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat;
Quaternion desired_ang_vel_quat = to_to_from_quat.inverse() * attitude_target_ang_vel_quat * to_to_from_quat;
Quaternion desired_ang_vel_quat_ff = to_to_from_quat.inverse() * attitude_target_ang_vel_quat * to_to_from_quat;
_desired_ang_vel_ff = Vector3f(desired_ang_vel_quat_ff.q2 + _rate_target_ang_vel.x, desired_ang_vel_quat_ff.q3 + _rate_target_ang_vel.y, desired_ang_vel_quat_ff.q4 + _rate_target_ang_vel.z);

// Add the angular velocity feedforward, rotated into vehicle frame
Quaternion desired_ang_vel_quat = to_to_from_quat.inverse() * delayed_attitude_target_ang_vel_quat * to_to_from_quat;

// Correct the thrust vector and smoothly add feedforward and yaw input
_feedforward_scalar = 1.0f;
Expand Down Expand Up @@ -696,7 +796,7 @@ void AC_AttitudeControl::attitude_controller_run_quat()
_attitude_target_quat.normalize();

// Record error to handle EKF resets
_attitude_ang_error = attitude_vehicle_quat.inverse() * _attitude_target_quat;
_attitude_ang_error = attitude_vehicle_quat.inverse() * delayed_attitude_target_quat;
}

// thrust_heading_rotation_angles - calculates two ordered rotations to move the att_from_quat quaternion to the att_to_quat quaternion.
Expand Down Expand Up @@ -1116,3 +1216,23 @@ bool AC_AttitudeControl::pre_arm_checks(const char *param_prefix,
}
return true;
}

// push target states to buffer
void AC_AttitudeControl::push_to_buffer(Quaternion &attitude_target_quat, Quaternion &rate_target_quat)
{
target_states sample;
sample.attitude = attitude_target_quat;
sample.rate = rate_target_quat;
target_states_buffer->push(sample);

}

// pull target states from buffer
void AC_AttitudeControl::pull_from_buffer(Quaternion &attitude_target_quat, Quaternion &rate_target_quat)
{
target_states sample;
target_states_buffer->pop(sample);
attitude_target_quat = sample.attitude;
rate_target_quat = sample.rate;

}
30 changes: 29 additions & 1 deletion libraries/AC_AttitudeControl/AC_AttitudeControl.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
/// @file AC_AttitudeControl.h
/// @brief ArduCopter attitude control library

#include <AP_HAL/AP_HAL.h>
#include <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h>
#include <AP_Math/AP_Math.h>
Expand Down Expand Up @@ -366,6 +367,9 @@ class AC_AttitudeControl {
// Enable/Disable angle boost
AP_Int8 _angle_boost_enabled;

// time delay of command model states to determine error for PID
AP_Int8 _time_delay;

// angle controller P objects
AC_P _p_angle_roll;
AC_P _p_angle_pitch;
Expand All @@ -374,8 +378,11 @@ class AC_AttitudeControl {
// Angle limit time constant (to maintain altitude)
AP_Float _angle_limit_tc;

// rate controller input smoothing time constant
// Attitude command model input smoothing time constant
AP_Float _input_tc;

// rate command model input smoothing time constant
AP_Float _input_rate_tc;

// Intersampling period in seconds
float _dt;
Expand Down Expand Up @@ -443,6 +450,27 @@ class AC_AttitudeControl {
// Yaw feed forward percent to allow zero yaw actuator output during extreme roll and pitch corrections
float _feedforward_scalar = 1.0f;

// desired angular velocity for feedforward to motors class
Vector3f _desired_ang_vel_ff;

float input_shaping_angle_2(float error_angle, float input_tc, float accel_max, float target_ang_vel, float dt);
float input_attitude_shaping(float input_tc, float euler_desired_angle, float euler_target_angle, float euler_target_rate, float &euler_target_accel, float &euler_target_jerk, float accel_limit, float dt);
float input_rate_shaping(float input_tc, float euler_desired_rate, float euler_target_rate, float &euler_target_accel, float &euler_target_jerk, float accel_limit, float dt);

Vector3f _attitude_target_euler_accel;
Vector3f _attitude_target_ang_accel;
Vector3f _attitude_target_euler_jerk;
Vector3f _attitude_target_ang_jerk;

// buffers to provide time delay
struct target_states {
Quaternion attitude;
Quaternion rate;
};
ObjectBuffer<target_states> *target_states_buffer;
void push_to_buffer(Quaternion &attitude_target_quat, Quaternion &rate_target_quat);
void pull_from_buffer(Quaternion &attitude_target_quat, Quaternion &rate_target_quat);

// References to external libraries
const AP_AHRS_View& _ahrs;
const AP_Vehicle::MultiCopter &_aparm;
Expand Down
Loading