diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index a4c75b3eb3b63..188f91e5a3687 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -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 @@ -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) { @@ -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 @@ -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(); @@ -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(); @@ -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 @@ -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) @@ -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 @@ -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 @@ -280,7 +359,7 @@ 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, @@ -288,73 +367,6 @@ void AC_Autorotation::log_write_autorotation(void) const } #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 { diff --git a/libraries/AC_Autorotation/AC_Autorotation.h b/libraries/AC_Autorotation/AC_Autorotation.h index b0fddc86e56f0..207fd7d87b4bb 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.h +++ b/libraries/AC_Autorotation/AC_Autorotation.h @@ -9,13 +9,14 @@ #include #include #include +#include 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); @@ -23,17 +24,17 @@ class AC_Autorotation // 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; @@ -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 @@ -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. @@ -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. @@ -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; @@ -105,4 +113,5 @@ class AC_Autorotation AP_AHRS& _ahrs; AP_MotorsHeli*& _motors_heli; AC_PosControl*& _pos_control; + AC_AttitudeControl*& _attitude_control; };