diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index 085cad6e7fe0de..2979ce9238673d 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -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 @@ -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(); @@ -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 @@ -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) @@ -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); } diff --git a/libraries/AC_Autorotation/AC_Autorotation.h b/libraries/AC_Autorotation/AC_Autorotation.h index 67a852a36093c9..fde5207ccb8132 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.h +++ b/libraries/AC_Autorotation/AC_Autorotation.h @@ -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; @@ -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.