Skip to content

Commit

Permalink
AC_Autorotation: get basic pos ctrl implimentation working
Browse files Browse the repository at this point in the history
  • Loading branch information
@MattKear
MattKear committed Sep 11, 2024
1 parent 8bd282a commit 375b4f2
Show file tree
Hide file tree
Showing 2 changed files with 111 additions and 90 deletions.
180 changes: 96 additions & 84 deletions libraries/AC_Autorotation/AC_Autorotation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,14 +67,14 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = {
// @User: Standard
AP_GROUPINFO("COL_FILT_G", 6, AC_Autorotation, _param_col_glide_cutoff_freq, 0.1),

// @Param: AS_ACC_MAX
// @Param: FW_ACC_MAX
// @DisplayName: Forward Acceleration Limit
// @Description: Maximum forward acceleration to apply in speed controller.
// @Units: m/s/s
// @Range: 0.3 2.0
// @Increment: 0.1
// @User: Standard
AP_GROUPINFO("AS_ACC_MAX", 7, AC_Autorotation, _param_accel_max, 0.6),
AP_GROUPINFO("FW_ACC_MAX", 7, AC_Autorotation, _param_accel_max, 1.4),

// @Param: HS_SENSOR
// @DisplayName: Main Rotor RPM Sensor
Expand All @@ -101,18 +101,29 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = {
// @User: Standard
AP_GROUPINFO("FW_V_FF", 10, AC_Autorotation, _param_fwd_k_ff, 1.5),

// @Param: FW_JERK_MAX
// @DisplayName: Forward jerk limit
// @Description: Maximum forward jerk to apply in speed controller.
// @Units: m/s/s/s
// @Range: 0.1 4.0
// @Increment: 0.01
// @User: Standard
AP_GROUPINFO("FW_JERK_MAX", 11, AC_Autorotation, _param_jerk_max, 0.46),

AP_GROUPEND
};

// Constructor
AC_Autorotation::AC_Autorotation(AP_AHRS& ahrs, AP_MotorsHeli*& motors, AC_PosControl*& pos_ctrl) :
AC_Autorotation::AC_Autorotation(AP_AHRS& ahrs, AP_MotorsHeli*& motors, AC_PosControl*& pos_ctrl, AC_AttitudeControl*& att_crtl) :
_ahrs(ahrs),
_motors_heli(motors),
_pos_control(pos_ctrl),
_attitude_control(att_crtl),
_p_hs(HS_CONTROLLER_HEADSPEED_P),
_p_fw_vel(AP_FW_VEL_P)
{
AP_Param::setup_object_defaults(this, var_info);
_desired_heading.heading_mode = AC_AttitudeControl::HeadingMode::Rate_Only;
}

void AC_Autorotation::init(void) {
Expand All @@ -126,13 +137,13 @@ void AC_Autorotation::init(void) {
// Protect against divide by zero
_param_head_speed_set_point.set(MAX(_param_head_speed_set_point, 500.0));

// Initialise forward speed controller
_accel_target = 0.0;

// Reset cmd vel and last accel to sensible values
_cmd_vel = get_speed_forward();
_accel_out_last = _cmd_vel * _param_fwd_k_ff;
// Initialise xy pos controller
_pos_control->init_xy_controller();
_pos_control->set_max_speed_accel_xy(_param_target_speed.get()*100.0, _param_accel_max*100.0);

// Init to current heading
_desired_heading.yaw_angle_cd = _ahrs.get_yaw()*100.0;
_desired_heading.yaw_rate_cds = 0.0;
}

// Functions and config that are only to be done once at the beginning of the entry
Expand All @@ -141,7 +152,7 @@ void AC_Autorotation::init_entry(void)
gcs().send_text(MAV_SEVERITY_INFO, "Entry Phase");

// Set desired forward speed target
_vel_target = _param_target_speed.get();
_fwd_spd_desired = _param_target_speed.get();

// Target head speed is set to rpm at initiation to prevent steps in controller
_target_head_speed = get_norm_head_speed();
Expand All @@ -157,7 +168,7 @@ void AC_Autorotation::init_entry(void)
}

// The entry controller just a special case of the glide controller with head speed target slewing
void AC_Autorotation::run_entry(float& pitch_target)
void AC_Autorotation::run_entry(float pilot_yaw_rate)
{
// Slowly change the target head speed until the target head speed matches the parameter defined value
float norm_rpm = get_norm_head_speed();
Expand All @@ -168,7 +179,7 @@ void AC_Autorotation::run_entry(float& pitch_target)
_target_head_speed = HEAD_SPEED_TARGET_RATIO;
}

run_glide(pitch_target);
run_glide(pilot_yaw_rate);
}

// Functions and config that are only to be done once at the beginning of the glide
Expand All @@ -183,15 +194,15 @@ void AC_Autorotation::init_glide(void)
_target_head_speed = HEAD_SPEED_TARGET_RATIO;

// Ensure desired forward speed target is set to param value
_vel_target = _param_target_speed.get();
_fwd_spd_desired = _param_target_speed.get();
}

// Maintain head speed and forward speed as we glide to the ground
void AC_Autorotation::run_glide(float& pitch_target)
void AC_Autorotation::run_glide(float pilot_yaw_rate)
{
update_headspeed_controller();

update_forward_speed_controller(pitch_target);
update_xy_speed_controller(pilot_yaw_rate);
}

void AC_Autorotation::update_headspeed_controller(void)
Expand Down Expand Up @@ -250,6 +261,74 @@ float AC_Autorotation::get_norm_head_speed(void) const
return current_rpm/head_speed_set_point;
}

// Update speed controller
// Vehicle is trying to achieve and maintain the desired speed in the body-frame forward direction.
// During the entry and glide phases the pilot can navigate via a yaw rate input and coordinated roll is calculated.
void AC_Autorotation::update_xy_speed_controller(float pilot_yaw_rate_cdegs)
{
// Update the heading and project to next time step
_desired_heading.yaw_rate_cds = pilot_yaw_rate_cdegs;
_desired_heading.yaw_angle_cd = radians(_ahrs.get_yaw())*100 + pilot_yaw_rate_cdegs*_dt;

const float fwd_speed = get_speed_forward();
float fwd_speed_error = _fwd_spd_desired - fwd_speed;

// constrain the desired speed by the maximum accel limit
const float sign = is_positive(fwd_speed_error) ? 1.0 : -1.0;
fwd_speed_error = MIN(fabsf(fwd_speed_error), accel_max()*_dt) * sign;

// Update the target body-frame velocity based on the now constrained forward speed
desired_velocity_bf.x = fwd_speed + fwd_speed_error;
desired_velocity_bf.y = 0.0; // Always want zero side slip

// We may not be constrained against the max accel to achieve the desired change in forward speed so we still check
// that we are only requesting what accel we actually need to maintain a kinematically consistant request
if (!is_positive(_dt)) {
// If we have not had a valid dt update protect against div by zero by assuming 400 Hz update
_dt = 2.5e-3;
}
desired_accel_bf.x = fwd_speed_error / _dt;

// Compute the lateral accel that we need to maintain a coordinated turn, based on the pilots yaw rate request
// Calc body-frame accels
// this seems suspiciously simple, but it is correct
// accel = r * w^2
// radius can be calculated as the distance traveled in the time it take to do 360 deg
// One rotation takes: (2*pi)/w seconds
// Distance traveled in that time: (s*2*pi)/w
// radius for that distance: ((s*2*pi)/w) / (2*pi)
// r = s / w
// accel = (s / w) * w^2
// accel = s * w
desired_accel_bf.y = pilot_yaw_rate_cdegs*0.01 * fwd_speed;

// TODO???? maybe we want a circular constrain on the accel here. This is probably taken care of in the pos controller but we should check

// Convert from body-frame to earth-frame
Vector2f desired_velocity_ef = _ahrs.body_to_earth2D(desired_velocity_bf);
Vector2f desired_accel_ef = _ahrs.body_to_earth2D(desired_accel_bf);

// convert m to cm for position controller
desired_velocity_ef *= 100.0;
desired_accel_ef *= 100.0;

// update the position controller
_pos_control->input_vel_accel_xy(desired_velocity_ef, desired_accel_ef, false);

_pos_control->update_xy_controller();

// output to the attitude controller
_attitude_control->input_thrust_vector_heading(_pos_control->get_thrust_vector(), _desired_heading);
}


// Determine the forward ground speed component from measured components
float AC_Autorotation::get_speed_forward(void) const
{
Vector2f groundspeed_vector = _ahrs.groundspeed_vector();
return groundspeed_vector.x*_ahrs.cos_yaw() + groundspeed_vector.y*_ahrs.sin_yaw(); // (m/s)
}


#if HAL_LOGGING_ENABLED
void AC_Autorotation::log_write_autorotation(void) const
Expand All @@ -263,7 +342,7 @@ void AC_Autorotation::log_write_autorotation(void) const
// @Field: ColOut: Collective Out
// @Field: FFCol: FF-term for headspeed controller response
// @Field: SpdF: current forward speed
// @Field: CmdV: desired forward speed
// @Field: DFS: desired forward speed
// @Field: p: p-term of velocity response
// @Field: ff: ff-term of velocity response
// @Field: AccT: forward acceleration target
Expand All @@ -280,81 +359,14 @@ void AC_Autorotation::log_write_autorotation(void) const
_collective_out,
_ff_term_hs,
get_speed_forward(),
_cmd_vel,
_fwd_spd_desired,
_vel_p,
_vel_ff,
_accel_target,
_landed_reason);
}
#endif // HAL_LOGGING_ENABLED

// Update speed controller
void AC_Autorotation::update_forward_speed_controller(float& pitch_target)
{
const float speed_forward = get_speed_forward();

// Limiting the target velocity based on the max acceleration limit
if (_cmd_vel < _vel_target) {
_cmd_vel += accel_max() * _dt;
if (_cmd_vel > _vel_target) {
_cmd_vel = _vel_target;
}
} else {
_cmd_vel -= accel_max() * _dt;
if (_cmd_vel < _vel_target) {
_cmd_vel = _vel_target;
}
}

// Get p
_vel_p = _p_fw_vel.get_p(_cmd_vel - speed_forward);

// Get ff
_vel_ff = _cmd_vel*_param_fwd_k_ff;

// Calculate acceleration target based on PI controller
_accel_target = _vel_ff + _vel_p;

// Filter correction acceleration
_accel_target_filter.set_cutoff_frequency(10.0f);
_accel_target_filter.apply(_accel_target, _dt);

// Limits the maximum change in pitch attitude based on acceleration
if (_accel_target > _accel_out_last + accel_max()) {
_accel_target = _accel_out_last + accel_max();
} else if (_accel_target < _accel_out_last - accel_max()) {
_accel_target = _accel_out_last - accel_max();
}

// Limiting acceleration based on velocity gained during the previous time step
const float delta_speed = speed_forward - _speed_forward_last;
_speed_forward_last = speed_forward;
if (fabsf(delta_speed) > accel_max() * _dt) {
_flag_limit_accel = true;
} else {
_flag_limit_accel = false;
}

if ((_flag_limit_accel && fabsf(_accel_target) < fabsf(_accel_out_last)) || !_flag_limit_accel) {
_accel_out = _accel_target;
} else {
_accel_out = _accel_out_last;
}
_accel_out_last = _accel_out;

// Update angle targets that will be passed to stabilize controller
pitch_target = accel_to_angle(-_accel_out);
}


// Determine the forward ground speed component from measured components
float AC_Autorotation::get_speed_forward(void) const
{
auto &ahrs = AP::ahrs();
Vector2f groundspeed_vector = ahrs.groundspeed_vector();
return groundspeed_vector.x*ahrs.cos_yaw() + groundspeed_vector.y*ahrs.sin_yaw(); // (m/s)
}

// Arming checks for autorotation, mostly checking for miss-configurations
bool AC_Autorotation::arming_checks(size_t buflen, char *buffer) const
{
Expand Down
21 changes: 15 additions & 6 deletions libraries/AC_Autorotation/AC_Autorotation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,31 +9,32 @@
#include <Filter/LowPassFilter.h>
#include <AC_PID/AC_P.h>
#include <AC_AttitudeControl/AC_PosControl.h>
#include <AC_AttitudeControl/AC_AttitudeControl.h>

class AC_Autorotation
{
public:

//Constructor
AC_Autorotation(AP_AHRS& ahrs, AP_MotorsHeli*& motors, AC_PosControl*& pos_ctrl);
AC_Autorotation(AP_AHRS& ahrs, AP_MotorsHeli*& motors, AC_PosControl*& pos_ctrl, AC_AttitudeControl*& att_crtl);

void init(void);

bool enabled(void) const { return _param_enable.get() > 0; }

// Init and run entry phase controller
void init_entry(void);
void run_entry(float& pitch_target);
void run_entry(float pilot_yaw_rate);

// Init and run the glide phase controller
void init_glide(void);
void run_glide(float& pitch_target);
void run_glide(float pilot_yaw_rate);

// Update controller used to drive head speed with collective
void update_headspeed_controller(void);

// Update controller used to control speed via vehicle pitch
void update_forward_speed_controller(float& pitch_target); // Update forward speed controller
void update_xy_speed_controller(float pilot_yaw_rate);

// Arming checks for autorotation, mostly checking for miss-configurations
bool arming_checks(size_t buflen, char *buffer) const;
Expand All @@ -44,6 +45,7 @@ class AC_Autorotation

// Helper functions
float accel_max(void) { return MAX(_param_accel_max.get(), 0.1); }
float jerk_max(void) { return MAX(_param_jerk_max.get(), 0.1); }
void set_dt(float delta_sec) { _dt = delta_sec; }

// Helper to get measured head speed that has been normalised by head speed set point
Expand All @@ -64,6 +66,11 @@ class AC_Autorotation

uint8_t _landed_reason; // Bitmask of the reasons we think we have landed. Stored in lib for logging.

AC_AttitudeControl::HeadingCommand _desired_heading;

Vector2f desired_accel_bf;
Vector2f desired_velocity_bf;

//--------- Not Checked vars
float _collective_out;
float _head_speed_error; // Error between target head speed and current head speed. Normalised by head speed set point RPM.
Expand All @@ -72,12 +79,12 @@ class AC_Autorotation
float _p_term_hs; // Proportional contribution to collective setting.
float _ff_term_hs; // Following trim feed forward contribution to collective setting.

float _vel_target; // Forward velocity target.
float _fwd_spd_desired; // Forward speed target.
float _speed_forward_last; // The forward speed calculated in the previous cycle.

bool _flag_limit_accel; // Maximum acceleration limit reached flag.
float _accel_out_last; // Acceleration value used to calculate pitch target in previous cycle.
float _cmd_vel; // Command velocity, used to get PID values for acceleration calculation.

float _accel_target; // Acceleration target, calculated from PID.

float _vel_p; // Forward velocity P term.
Expand All @@ -97,6 +104,7 @@ class AC_Autorotation
AP_Float _param_accel_max;
AP_Int8 _param_rpm_instance;
AP_Float _param_fwd_k_ff;
AP_Float _param_jerk_max;

// low pass filter for collective trim
LowPassFilterFloat col_trim_lpf;
Expand All @@ -105,4 +113,5 @@ class AC_Autorotation
AP_AHRS& _ahrs;
AP_MotorsHeli*& _motors_heli;
AC_PosControl*& _pos_control;
AC_AttitudeControl*& _attitude_control;
};

0 comments on commit 375b4f2

Please sign in to comment.