Skip to content

Commit

Permalink
AC_Autorotation: Completed pos control implementation for entry and g…
Browse files Browse the repository at this point in the history 
…lide
  • Loading branch information
@MattKear
MattKear committed Sep 12, 2024
1 parent 8fd5c72 commit 609ce96
Show file tree
Hide file tree
Showing 2 changed files with 50 additions and 86 deletions.
123 changes: 45 additions & 78 deletions libraries/AC_Autorotation/AC_Autorotation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -137,19 +137,14 @@ 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 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);
_pos_control->set_correction_speed_accel_xy(_param_target_speed.get()*100.0, _param_accel_max*100.0);
// Set limits and initialise xy pos controller
_pos_control->set_max_speed_accel_xy(_param_target_speed.get()*100.0, _param_accel_max.get()*100.0);
_pos_control->set_correction_speed_accel_xy(_param_target_speed.get()*100.0, _param_accel_max.get()*100.0);
_pos_control->set_pos_error_max_xy_cm(1000);
_pos_control->init_xy_controller();

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

// init last desired vels and accels to current values. This ensures that we smoothly apply our kinematic shaping to the desired
last_desired_velocity_bf = get_bf_vel().xy();
last_desired_accel_bf = get_bf_accel().xy();
_yaw_rate = _ahrs.get_yaw_rate_earth();
}

// Functions and config that are only to be done once at the beginning of the entry
Expand All @@ -174,7 +169,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 pilot_yaw_rate)
void AC_Autorotation::run_entry(float pilot_norm_accel)
{
// 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 @@ -185,7 +180,7 @@ void AC_Autorotation::run_entry(float pilot_yaw_rate)
_target_head_speed = HEAD_SPEED_TARGET_RATIO;
}

run_glide(pilot_yaw_rate);
run_glide(pilot_norm_accel);
}

// Functions and config that are only to be done once at the beginning of the glide
Expand All @@ -204,11 +199,42 @@ void AC_Autorotation::init_glide(void)
}

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

update_xy_speed_controller(pilot_yaw_rate);
// Set body frame velocity targets
desired_velocity_bf.x = _fwd_spd_desired;
desired_velocity_bf.y = 0.0; // Always want zero side slip

// Set body frame accel targets. Pilot requests lateral accel.
desired_accel_bf.y = desired_accel_bf.y * 0.975 + pilot_norm_accel * _param_accel_max.get() * 0.025; // approx 10 hz low-pass filter
desired_accel_bf.x = 0.0; // Do not use forward accel feed-forward

// Based on the requested lateral accel, calc the necessary yaw rate to achieve a coordinated turn
const float curr_fwd_speed = get_speed_forward();
const float delta_v = MIN((_fwd_spd_desired - curr_fwd_speed), (_param_accel_max.get() * _dt));
const float projected_vel = curr_fwd_speed + delta_v; // predicted velocity in the next time step

// Calc yaw rate from desired 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
// w = accel / s
const float yaw_rate = desired_accel_bf.y / projected_vel;

// Smoothly update desired yaw rate
_yaw_rate = _yaw_rate * 0.975 + yaw_rate * 0.025; // approx 10 hz low-pass filter
_desired_heading.yaw_rate_cds = degrees(_yaw_rate)*100.0;

// Update the position controller
update_xy_speed_controller();
}

void AC_Autorotation::update_headspeed_controller(void)
Expand Down Expand Up @@ -270,76 +296,17 @@ float AC_Autorotation::get_norm_head_speed(void) const
// 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)
void AC_Autorotation::update_xy_speed_controller(void)
{
// 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;

desired_velocity_bf.x = _fwd_spd_desired;
desired_velocity_bf.y = 0.0; // Always want zero side slip
desired_accel_bf.x = accel_max();
// iterate over the body-frame forward speed to ensure it is converged on a kinematically consistant jerk and accel limited solution
for (uint8_t i=0; i<3; i++) {
float fwd_speed_error = desired_velocity_bf.x - last_desired_velocity_bf.x;

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

// Update the target body-frame velocity based on the now constrained forward speed
desired_velocity_bf.x = last_desired_velocity_bf.x + fwd_speed_error;

// 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;

// jerk limit the accel
float fwd_accel_error = desired_accel_bf.x - last_desired_accel_bf.x;
sign = is_positive(fwd_accel_error) ? 1.0 : -1.0;
fwd_accel_error = MIN(fabsf(fwd_accel_error), jerk_max() *_dt) * sign;
desired_accel_bf.x = last_desired_accel_bf.x + fwd_accel_error;
}

// 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 * M_PI / 180.0 * (desired_velocity_bf.x); // desired velocity in the next time step

// store last
last_desired_velocity_bf = desired_velocity_bf;
last_desired_accel_bf = desired_accel_bf;

// 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
// Convert from body-frame to earth-frame
desired_velocity_ef = _ahrs.body_to_earth2D(desired_velocity_bf) * 100.0;
desired_accel_ef = _ahrs.body_to_earth2D(desired_accel_bf) * 100.0;

// Vector2p target_pos = _pos_control->get_pos_target_cm().xy();

// update the position controller
// _pos_control->input_vel_accel_xy(desired_velocity_ef, desired_accel_ef, false);
// _pos_control->input_vel_accel_xy(desired_velocity_ef, desired_accel_ef, true);
// _pos_control->set_pos_vel_accel_xy(target_pos, desired_velocity_ef, desired_accel_ef);
_pos_control->set_vel_desired_xy_cms(desired_velocity_ef);
_pos_control->set_accel_desired_xy_cmss(desired_accel_ef);

// Update the position controller
_pos_control->input_vel_accel_xy(desired_velocity_ef, desired_accel_ef, true);
_pos_control->update_xy_controller();

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

Expand Down
13 changes: 5 additions & 8 deletions libraries/AC_Autorotation/AC_Autorotation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,17 +24,17 @@ class AC_Autorotation

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

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

// 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_xy_speed_controller(float pilot_yaw_rate);
void update_xy_speed_controller(void);

// Arming checks for autorotation, mostly checking for miss-configurations
bool arming_checks(size_t buflen, char *buffer) const;
Expand Down Expand Up @@ -73,14 +73,11 @@ class AC_Autorotation

Vector2f desired_accel_bf;
Vector2f desired_velocity_bf;

Vector2f last_desired_accel_bf;
Vector2f last_desired_velocity_bf;

// --- TEMP ---
Vector2f desired_velocity_ef;
Vector2f desired_accel_ef;

float _yaw_rate;

//--------- 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 Down

0 comments on commit 609ce96

Please sign in to comment.