diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index 9c1be27e07e8a..afdfe21ca409c 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -2,20 +2,10 @@ #include #include #include +#include // Autorotation controller defaults -// Head Speed (HS) controller specific default definitions -#define HS_CONTROLLER_COLLECTIVE_CUTOFF_FREQ 2.0f // low-pass filter on accel error (unit: hz) -#define HS_CONTROLLER_HEADSPEED_P 0.7f // Default P gain for head speed controller (unit: -) -#define HS_CONTROLLER_ENTRY_COL_FILTER 0.7f // Default low pass filter frequency during the entry phase (unit: Hz) -#define HS_CONTROLLER_GLIDE_COL_FILTER 0.1f // Default low pass filter frequency during the glide phase (unit: Hz) - -// Speed Height controller specific default definitions for autorotation use -#define FWD_SPD_CONTROLLER_GND_SPEED_TARGET 1100 // Default target ground speed for speed height controller (unit: cm/s) -#define FWD_SPD_CONTROLLER_MAX_ACCEL 60 // Default acceleration limit for speed height controller (unit: cm/s/s) -#define AP_FW_VEL_P 0.9f -#define AP_FW_VEL_FF 0.15f - +#define HEAD_SPEED_TARGET_RATIO 1.0 // Normalised target main rotor head speed const AP_Param::GroupInfo AC_Autorotation::var_info[] = { @@ -23,7 +13,7 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // @DisplayName: Enable settings for RSC Setpoint // @Description: Allows you to enable (1) or disable (0) the autonomous autorotation capability. // @Values: 0:Disabled,1:Enabled - // @User: Advanced + // @User: Standard AP_GROUPINFO_FLAGS("ENABLE", 1, AC_Autorotation, _param_enable, 0, AP_PARAM_FLAG_ENABLE), // @Param: HS_P @@ -31,349 +21,435 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // @Description: Increase value to increase sensitivity of head speed controller during autonomous autorotation. // @Range: 0.3 1 // @Increment: 0.01 - // @User: Advanced + // @User: Standard AP_SUBGROUPINFO(_p_hs, "HS_", 2, AC_Autorotation, AC_P), // @Param: HS_SET_PT // @DisplayName: Target Head Speed - // @Description: The target head speed in RPM during autorotation. Start by setting to desired hover speed and tune from there as necessary. + // @Description: The target head speed in RPM during autorotation. Start by setting to desired hover speed and tune from there as necessary. // @Units: RPM // @Range: 1000 2800 // @Increment: 1 - // @User: Advanced + // @User: Standard AP_GROUPINFO("HS_SET_PT", 3, AC_Autorotation, _param_head_speed_set_point, 1500), - // @Param: TARG_SP - // @DisplayName: Target Glide Ground Speed + // @Param: FWD_SP_TARG + // @DisplayName: Target Glide Body Frame Forward Speed // @Description: Target ground speed in cm/s for the autorotation controller to try and achieve/ maintain during the glide phase. - // @Units: cm/s - // @Range: 800 2000 - // @Increment: 50 - // @User: Advanced - AP_GROUPINFO("TARG_SP", 4, AC_Autorotation, _param_target_speed, FWD_SPD_CONTROLLER_GND_SPEED_TARGET), + // @Units: m/s + // @Range: 8 20 + // @Increment: 0.5 + // @User: Standard + AP_GROUPINFO("FWD_SP_TARG", 4, AC_Autorotation, _param_target_speed, 11), // @Param: COL_FILT_E // @DisplayName: Entry Phase Collective Filter - // @Description: Cut-off frequency for collective low pass filter. For the entry phase. Acts as a following trim. Must be higher than AROT_COL_FILT_G. + // @Description: Cut-off frequency for collective low pass filter. For the entry phase. Acts as a following trim. Must be higher than AROT_COL_FILT_G. // @Units: Hz // @Range: 0.2 0.5 // @Increment: 0.01 - // @User: Advanced - AP_GROUPINFO("COL_FILT_E", 5, AC_Autorotation, _param_col_entry_cutoff_freq, HS_CONTROLLER_ENTRY_COL_FILTER), + // @User: Standard + AP_GROUPINFO("COL_FILT_E", 5, AC_Autorotation, _param_col_entry_cutoff_freq, 0.7), // @Param: COL_FILT_G // @DisplayName: Glide Phase Collective Filter - // @Description: Cut-off frequency for collective low pass filter. For the glide phase. Acts as a following trim. Must be lower than AROT_COL_FILT_E. + // @Description: Cut-off frequency for collective low pass filter. For the glide phase. Acts as a following trim. Must be lower than AROT_COL_FILT_E. // @Units: Hz // @Range: 0.03 0.15 // @Increment: 0.01 - // @User: Advanced - AP_GROUPINFO("COL_FILT_G", 6, AC_Autorotation, _param_col_glide_cutoff_freq, HS_CONTROLLER_GLIDE_COL_FILTER), - - // @Param: AS_ACC_MAX - // @DisplayName: Forward Acceleration Limit - // @Description: Maximum forward acceleration to apply in speed controller. - // @Units: cm/s/s - // @Range: 30 60 - // @Increment: 10 - // @User: Advanced - AP_GROUPINFO("AS_ACC_MAX", 7, AC_Autorotation, _param_accel_max, FWD_SPD_CONTROLLER_MAX_ACCEL), + // @User: Standard + AP_GROUPINFO("COL_FILT_G", 6, AC_Autorotation, _param_col_glide_cutoff_freq, 0.1), + + // @Param: XY_ACC_MAX + // @DisplayName: Body Frame XY Acceleration Limit + // @Description: Maximum body frame acceleration allowed in the in speed controller. This limit defines a circular constraint in accel. Minimum used is 0.5 m/s/s. + // @Units: m/s/s + // @Range: 0.5 8.0 + // @Increment: 0.1 + // @User: Standard + AP_GROUPINFO("XY_ACC_MAX", 7, AC_Autorotation, _param_accel_max, 2.0), // @Param: HS_SENSOR // @DisplayName: Main Rotor RPM Sensor - // @Description: Allocate the RPM sensor instance to use for measuring head speed. RPM1 = 0. RPM2 = 1. + // @Description: Allocate the RPM sensor instance to use for measuring head speed. RPM1 = 0. RPM2 = 1. // @Units: s // @Range: 0.5 3 // @Increment: 0.1 - // @User: Advanced + // @User: Standard AP_GROUPINFO("HS_SENSOR", 8, AC_Autorotation, _param_rpm_instance, 0), - // @Param: FW_V_P - // @DisplayName: Velocity (horizontal) P gain - // @Description: Velocity (horizontal) P gain. Determines the proportion of the target acceleration based on the velocity error. - // @Range: 0.1 6.0 - // @Increment: 0.1 + // @Param: FWD_P + // @DisplayName: Forward Speed Controller P Gain + // @Description: Converts the difference between desired forward speed and actual speed into an acceleration target that is passed to the pitch angle controller. + // @Range: 1.000 8.000 + // @User: Standard + + // @Param: FWD_I + // @DisplayName: Forward Speed Controller I Gain + // @Description: Corrects long-term difference in desired velocity to a target acceleration. + // @Range: 0.02 1.00 + // @Increment: 0.01 + // @User: Advanced + + // @Param: FWD_IMAX + // @DisplayName: Forward Speed Controller I Gain Maximum + // @Description: Constrains the target acceleration that the I gain will output. + // @Range: 1.000 8.000 + // @User: Standard + + // @Param: FWD_D + // @DisplayName: Forward Speed Controller D Gain + // @Description: Provides damping to velocity controller. + // @Range: 0.00 1.00 + // @Increment: 0.001 // @User: Advanced - AP_SUBGROUPINFO(_p_fw_vel, "FW_V_", 9, AC_Autorotation, AC_P), - // @Param: FW_V_FF - // @DisplayName: Velocity (horizontal) feed forward - // @Description: Velocity (horizontal) input filter. Corrects the target acceleration proportionally to the desired velocity. + // @Param: FWD_FF + // @DisplayName: Forward Speed Controller Feed Forward Gain + // @Description: Produces an output that is proportional to the magnitude of the target. // @Range: 0 1 // @Increment: 0.01 // @User: Advanced - AP_GROUPINFO("FW_V_FF", 10, AC_Autorotation, _param_fwd_k_ff, AP_FW_VEL_FF), + + // @Param: FWD_FLTE + // @DisplayName: Forward Speed Controller Error Filter + // @Description: This filter low pass filter is applied to the input for P and I terms. + // @Range: 0 100 + // @Units: Hz + // @User: Advanced + + // @Param: FWD_FLTD + // @DisplayName: Forward Speed Controller input filter for D term + // @Description: This filter low pass filter is applied to the input for D terms. + // @Range: 0 100 + // @Units: Hz + // @User: Advanced + AP_SUBGROUPINFO(_fwd_speed_pid, "FWD_", 9, AC_Autorotation, AC_PID_Basic), AP_GROUPEND }; // Constructor -AC_Autorotation::AC_Autorotation() : - _p_hs(HS_CONTROLLER_HEADSPEED_P), - _p_fw_vel(AP_FW_VEL_P) +AC_Autorotation::AC_Autorotation(AP_AHRS& ahrs, AP_MotorsHeli*& motors, AC_AttitudeControl*& att_crtl) : + _ahrs(ahrs), + _motors_heli(motors), + _attitude_control(att_crtl), + _p_hs(1.0), + _fwd_speed_pid(2.0, 2.0, 0.2, 0.1, 4.0, 0.0, 10.0) // Default values for kp, ki, kd, kff, imax, filt E Hz, filt D Hz { AP_Param::setup_object_defaults(this, var_info); } - -// Initialisation of head speed controller -void AC_Autorotation::init_hs_controller() +void AC_Autorotation::init(void) { - // Set initial collective position to be the collective position on initialisation - _collective_out = 0.4f; + // Initialisation of head speed controller + // Set initial collective position to be the current collective position for smooth init + const float collective_out = _motors_heli->get_throttle_out(); // Reset feed forward filter - col_trim_lpf.reset(_collective_out); + col_trim_lpf.reset(collective_out); - // Reset flags - _flags.bad_rpm = false; + // Protect against divide by zero TODO: move this to an accessor function + _param_head_speed_set_point.set(MAX(_param_head_speed_set_point, 500.0)); - // Reset RPM health monitoring - _unhealthy_rpm_counter = 0; - _healthy_rpm_counter = 0; - - // Protect against divide by zero - _param_head_speed_set_point.set(MAX(_param_head_speed_set_point,500)); + // Reset the landed reason + _landed_reason.min_speed = false; + _landed_reason.land_col = false; + _landed_reason.is_still = false; } - -bool AC_Autorotation::update_hs_glide_controller(float dt) +// Functions and config that are only to be done once at the beginning of the entry +void AC_Autorotation::init_entry(void) { - // Reset rpm health flag - _flags.bad_rpm = false; - _flags.bad_rpm_warning = false; + gcs().send_text(MAV_SEVERITY_INFO, "Entry Phase"); - // Get current rpm and update healthy signal counters - _current_rpm = get_rpm(true); + // Target head speed is set to rpm at initiation to prevent steps in controller + if (!get_norm_head_speed(_target_head_speed)) { + // Cannot get a valid RPM sensor reading so we default to not slewing the head speed target + _target_head_speed = HEAD_SPEED_TARGET_RATIO; + } - if (_unhealthy_rpm_counter <=30) { - // Normalised head speed - float head_speed_norm = _current_rpm / _param_head_speed_set_point; + // The decay rate to reduce the head speed from the current to the target + _hs_decay = (_target_head_speed - HEAD_SPEED_TARGET_RATIO) / (float(entry_time_ms)*1e-3); - // Set collective trim low pass filter cut off frequency - col_trim_lpf.set_cutoff_frequency(_col_cutoff_freq); + // Set collective following trim low pass filter cut off frequency + col_trim_lpf.set_cutoff_frequency(_param_col_entry_cutoff_freq.get()); - // Calculate the head speed error. Current rpm is normalised by the set point head speed. - // Target head speed is defined as a percentage of the set point. - _head_speed_error = head_speed_norm - _target_head_speed; + // Set collective low-pass cut off filter at 2 Hz + _motors_heli->set_throttle_filter_cutoff(2.0); - _p_term_hs = _p_hs.get_p(_head_speed_error); + // Set speed target to maintain the current speed whilst we enter the autorotation + _desired_vel = _param_target_speed.get(); + _target_vel = get_speed_forward(); - // Adjusting collective trim using feed forward (not yet been updated, so this value is the previous time steps collective position) - _ff_term_hs = col_trim_lpf.apply(_collective_out, dt); + // Reset I term of velocity PID + _fwd_speed_pid.reset_filter(); + _fwd_speed_pid.set_integrator(0.0); +} - // Calculate collective position to be set - _collective_out = _p_term_hs + _ff_term_hs; +// The entry controller just a special case of the glide controller with head speed target slewing +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 head_speed_norm; + if (!get_norm_head_speed(head_speed_norm)) { + // RPM sensor is bad, so we don't attempt to slew the head speed target as we do not know what head speed actually is + // The collective output handling of the rpm sensor failure is handled later in the head speed controller + head_speed_norm = HEAD_SPEED_TARGET_RATIO; + } + if (head_speed_norm > HEAD_SPEED_TARGET_RATIO*1.05f || head_speed_norm < HEAD_SPEED_TARGET_RATIO*0.95f) { + // Outside of 5% of target head speed so we slew target towards the set point + _target_head_speed -= _hs_decay * _dt; } else { - // RPM sensor is bad set collective to minimum - _collective_out = -1.0f; - - _flags.bad_rpm_warning = true; + _target_head_speed = HEAD_SPEED_TARGET_RATIO; } - // Send collective to setting to motors output library - set_collective(HS_CONTROLLER_COLLECTIVE_CUTOFF_FREQ); + run_glide(pilot_norm_accel); +} - return _flags.bad_rpm_warning; +// Functions and config that are only to be done once at the beginning of the glide +void AC_Autorotation::init_glide(void) +{ + gcs().send_text(MAV_SEVERITY_INFO, "Glide Phase"); + + // Set collective following trim low pass filter cut off frequency + col_trim_lpf.set_cutoff_frequency(_param_col_glide_cutoff_freq.get()); + + // Ensure target head speed is set to setpoint, in case it didn't reach the target during entry + _target_head_speed = HEAD_SPEED_TARGET_RATIO; + + // Ensure desired forward speed target is set to param value + _desired_vel = _param_target_speed.get(); } +// Maintain head speed and forward speed as we glide to the ground +void AC_Autorotation::run_glide(float pilot_norm_accel) +{ + update_headspeed_controller(); + + update_forward_speed_controller(pilot_norm_accel); +} -// Function to set collective and collective filter in motor library -void AC_Autorotation::set_collective(float collective_filter_cutoff) const +void AC_Autorotation::update_headspeed_controller(void) { - AP_Motors *motors = AP::motors(); - if (motors) { - motors->set_throttle_filter_cutoff(collective_filter_cutoff); - motors->set_throttle(_collective_out); + // Get current rpm and update healthy signal counters + float head_speed_norm; + if (!get_norm_head_speed(head_speed_norm)) { + // RPM sensor is bad, set collective to angle of -2 deg and hope for the best + _motors_heli->set_coll_from_ang(-2.0); + return; } -} + // Calculate the head speed error. + _head_speed_error = head_speed_norm - _target_head_speed; + + _p_term_hs = _p_hs.get_p(_head_speed_error); + + // Adjusting collective trim using feed forward (not yet been updated, so this value is the previous time steps collective position) + _ff_term_hs = col_trim_lpf.apply(_motors_heli->get_throttle(), _dt); + + // Calculate collective position to be set + const float collective_out = constrain_value((_p_term_hs + _ff_term_hs), 0.0f, 1.0f); + + // Send collective to setting to motors output library + _motors_heli->set_throttle(collective_out); + +#if HAL_LOGGING_ENABLED + // @LoggerMessage: ARHS + // @Vehicles: Copter + // @Description: Helicopter autorotation head speed controller information + // @Field: TimeUS: Time since system startup + // @Field: Tar: Normalised target head speed + // @Field: Act: Normalised measured head speed + // @Field: Err: Head speed controller error + // @Field: P: P-term for head speed controller response + // @Field: FF: FF-term for head speed controller response + + // Write to data flash log + AP::logger().WriteStreaming("ARHS", + "TimeUS,Tar,Act,Err,P,FF", + "Qfffff", + AP_HAL::micros64(), + _target_head_speed, + head_speed_norm, + _head_speed_error, + _p_term_hs, + _ff_term_hs); +#endif +} -//function that gets rpm and does rpm signal checking to ensure signal is reliable -//before using it in the controller -float AC_Autorotation::get_rpm(bool update_counter) +// Get measured head speed and normalise by head speed set point. Returns false if a valid rpm measurement cannot be obtained +bool AC_Autorotation::get_norm_head_speed(float& norm_rpm) const { - float current_rpm = 0.0f; + // Assuming zero rpm is safer as it will drive collective in the direction of increasing head speed + float current_rpm = 0.0; #if AP_RPM_ENABLED // Get singleton for RPM library const AP_RPM *rpm = AP_RPM::get_singleton(); - //Get current rpm, checking to ensure no nullptr - if (rpm != nullptr) { - //Check requested rpm instance to ensure either 0 or 1. Always defaults to 0. - if ((_param_rpm_instance > 1) || (_param_rpm_instance < 0)) { - _param_rpm_instance.set(0); - } - - //Get RPM value - uint8_t instance = _param_rpm_instance; - - //Check RPM sensor is returning a healthy status - if (!rpm->get_rpm(instance, current_rpm) || current_rpm <= -1) { - //unhealthy, rpm unreliable - _flags.bad_rpm = true; - } + // Checking to ensure no nullptr, we do have a pre-arm check for this so it will be very bad if RPM has gone away + if (rpm == nullptr) { + return false; + } - } else { - _flags.bad_rpm = true; + // Check RPM sensor is returning a healthy status + if (!rpm->get_rpm(_param_rpm_instance.get(), current_rpm)) { + return false; } -#else - _flags.bad_rpm = true; #endif - if (_flags.bad_rpm) { - //count unhealthy rpm updates and reset healthy rpm counter - _unhealthy_rpm_counter++; - _healthy_rpm_counter = 0; + // Protect against div by zeros later in the code + float head_speed_set_point = MAX(1.0, _param_head_speed_set_point.get()); - } else if (!_flags.bad_rpm && _unhealthy_rpm_counter > 0) { - //poor rpm reading may have cleared. Wait 10 update cycles to clear. - _healthy_rpm_counter++; + // Normalize the RPM by the setpoint + norm_rpm = current_rpm/head_speed_set_point; + return true; +} + +// Update speed controller +void AC_Autorotation::update_forward_speed_controller(float pilot_norm_accel) +{ + // Limiting the desired velocity based on the max acceleration limit to get an update target + const float min_vel = _target_vel - get_accel_max() * _dt; + const float max_vel = _target_vel + get_accel_max() * _dt; + _target_vel = constrain_float(_desired_vel, min_vel, max_vel); // (m/s) + + // Calculate acceleration target + const float fwd_accel_target = _fwd_speed_pid.update_all(_target_vel, get_speed_forward(), _dt, _limit_accel); // (m/s/s) + + // Build the body frame XY accel vector. + // Pilot can request as much as 1/2 of the max accel laterally to perform a turn. + // We only allow up to half as we need to prioritize building/maintaining airspeed. + Vector2f bf_accel_target = {fwd_accel_target, pilot_norm_accel * get_accel_max() * 0.5}; + + // Ensure we do not exceed the accel limit + _limit_accel = bf_accel_target.limit_length(get_accel_max()); + + // Calculate roll and pitch targets from angles, negative accel for negative pitch (pitch forward) + const float roll_angle_cdeg = accel_to_angle(bf_accel_target.y) * 100.0; + Vector2f angle_target_cdeg = { accel_to_angle(-bf_accel_target.x) * 100.0, // Pitch + roll_angle_cdeg}; // Roll + + // Ensure that the requested angles do not exceed angle max + _limit_accel = _limit_accel || angle_target_cdeg.limit_length(_attitude_control->lean_angle_max_cd()); + + // we may have scaled the lateral accel in the angle limit scaling, so we need to account + // for the ratio in the next yaw rate calculation + float accel_scale_ratio = 1.0; + if (is_positive(fabsf(roll_angle_cdeg))) { + accel_scale_ratio = fabsf(angle_target_cdeg.y / roll_angle_cdeg); + } - if (_healthy_rpm_counter >= 10) { - //poor rpm health has cleared, reset counters - _unhealthy_rpm_counter = 0; - _healthy_rpm_counter = 0; - } + // Calc yaw rate from desired body-frame accels + // this seems suspiciously simple, but it is correct + // accel = r * w^2, r = radius and w = angular rate + // radius can be calculated as the distance traveled in the time it takes 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 + float yaw_rate_cds = 0.0; + if (is_positive(fabsf(_target_vel))) { + yaw_rate_cds = degrees(bf_accel_target.y * accel_scale_ratio / _target_vel) * 100.0; } - return current_rpm; -} + // Output to attitude controller + _attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(angle_target_cdeg.y, angle_target_cdeg.x, yaw_rate_cds); #if HAL_LOGGING_ENABLED -void AC_Autorotation::Log_Write_Autorotation(void) const -{ -// @LoggerMessage: AROT -// @Vehicles: Copter -// @Description: Helicopter AutoRotation information -// @Field: TimeUS: Time since system startup -// @Field: P: P-term for headspeed controller response -// @Field: hserr: head speed error; difference between current and desired head speed -// @Field: ColOut: Collective Out -// @Field: FFCol: FF-term for headspeed controller response -// @Field: CRPM: current headspeed RPM -// @Field: SpdF: current forward speed -// @Field: CmdV: desired forward speed -// @Field: p: p-term of velocity response -// @Field: ff: ff-term of velocity response -// @Field: AccO: forward acceleration output -// @Field: AccT: forward acceleration target -// @Field: PitT: pitch target - - //Write to data flash log - AP::logger().WriteStreaming("AROT", - "TimeUS,P,hserr,ColOut,FFCol,CRPM,SpdF,CmdV,p,ff,AccO,AccT,PitT", - "Qffffffffffff", + // @LoggerMessage: ARSC + // @Vehicles: Copter + // @Description: Helicopter autorotation speed controller information + // @Field: TimeUS: Time since system startup + // @Field: Des: Desired forward velocity + // @Field: Tar: Target forward velocity + // @Field: Act: Measured forward velocity + // @Field: P: Velocity to acceleration P-term component + // @Field: I: Velocity to acceleration I-term component + // @Field: D: Velocity to acceleration D-term component + // @Field: FF: Velocity to acceleration feed forward component + // @Field: Lim: Accel limit flag + // @Field: FA: Forward acceleration target + // @Field: LA: Lateral acceleration target + + const AP_PIDInfo& pid_info = _fwd_speed_pid.get_pid_info(); + AP::logger().WriteStreaming("ARSC", + "TimeUS,Des,Tar,Act,P,I,D,FF,Lim,FA,LA", + "QfffffffBff", AP_HAL::micros64(), - (double)_p_term_hs, - (double)_head_speed_error, - (double)_collective_out, - (double)_ff_term_hs, - (double)_current_rpm, - (double)_speed_forward, - (double)_cmd_vel, - (double)_vel_p, - (double)_vel_ff, - (double)_accel_out, - (double)_accel_target, - (double)_pitch_target); + _desired_vel, + pid_info.target, + pid_info.actual, + pid_info.P, + pid_info.I, + pid_info.D, + pid_info.FF, + uint8_t(_limit_accel), + bf_accel_target.x, + bf_accel_target.y); +#endif } -#endif // HAL_LOGGING_ENABLED -// Initialise forward speed controller -void AC_Autorotation::init_fwd_spd_controller(void) +// smoothly zero velocity and accel +void AC_Autorotation::run_landed(void) { - // Reset I term and acceleration target - _accel_target = 0.0f; - - // Ensure parameter acceleration doesn't exceed hard-coded limit - _accel_max = MIN(_param_accel_max, 60.0f); - - // Reset cmd vel and last accel to sensible values - _cmd_vel = calc_speed_forward(); //(cm/s) - _accel_out_last = _cmd_vel*_param_fwd_k_ff; + _desired_vel *= 0.95; + update_forward_speed_controller(0.0); } - -// set_dt - sets time delta in seconds for all controllers -void AC_Autorotation::set_dt(float delta_sec) +// Determine the body frame forward speed +float AC_Autorotation::get_speed_forward(void) const { - _dt = delta_sec; + Vector3f vel_NED = {0,0,0}; + if (_ahrs.get_velocity_NED(vel_NED)) { + vel_NED = _ahrs.earth_to_body(vel_NED); + } + // TODO: need to improve the handling of the velocity NED not ok case + return vel_NED.x; } - -// update speed controller -void AC_Autorotation::update_forward_speed_controller(void) +#if HAL_LOGGING_ENABLED +// Logging of lower rate autorotation specific variables. This is meant for stuff that +// doesn't need a high rate, e.g. controller variables that are need for tuning. +void AC_Autorotation::log_write_autorotation(void) const { - // Specify forward velocity component and determine delta velocity with respect to time - _speed_forward = calc_speed_forward(); //(cm/s) - - _delta_speed_fwd = _speed_forward - _speed_forward_last; //(cm/s) - _speed_forward_last = _speed_forward; //(cm/s) - - // 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; + // enum class for bitmask documentation in logging + enum class AC_Autorotation_Landed_Reason : uint8_t { + LOW_SPEED = 1<<0, // true if below 1 m/s + LAND_COL = 1<<1, // true if collective below land col min + IS_STILL = 1<<2, // passes inertial nav is_still() check + }; + + uint8_t reason = 0; + if (_landed_reason.min_speed) { + reason |= uint8_t(AC_Autorotation_Landed_Reason::LOW_SPEED); } - - //Limiting acceleration based on velocity gained during the previous time step - if (fabsf(_delta_speed_fwd) > _accel_max * _dt) { - _flag_limit_accel = true; - } else { - _flag_limit_accel = false; + if (_landed_reason.land_col) { + reason |= uint8_t(AC_Autorotation_Landed_Reason::LAND_COL); } - - if ((_flag_limit_accel && fabsf(_accel_target) < fabsf(_accel_out_last)) || !_flag_limit_accel) { - _accel_out = _accel_target; - } else { - _accel_out = _accel_out_last; + if (_landed_reason.is_still) { + reason |= uint8_t(AC_Autorotation_Landed_Reason::IS_STILL); } - _accel_out_last = _accel_out; - // update angle targets that will be passed to stabilize controller - _pitch_target = accel_to_angle(-_accel_out*0.01) * 100; + // @LoggerMessage: AROT + // @Vehicles: Copter + // @Description: Helicopter autorotation information + // @Field: LR: Landed Reason state flags + // @FieldBitmaskEnum: LR: AC_Autorotation::AC_Autorotation_Landed_Reason + // Write to data flash log + AP::logger().WriteStreaming("AROT", + "TimeUS,LR", + "QB", + AP_HAL::micros64(), + _landed_reason); } - - -// Determine the forward ground speed component from measured components -float AC_Autorotation::calc_speed_forward(void) -{ - auto &ahrs = AP::ahrs(); - Vector2f groundspeed_vector = ahrs.groundspeed_vector(); - float speed_forward = (groundspeed_vector.x*ahrs.cos_yaw() + groundspeed_vector.y*ahrs.sin_yaw())* 100; //(c/s) - return speed_forward; -} +#endif // HAL_LOGGING_ENABLED // Arming checks for autorotation, mostly checking for miss-configurations bool AC_Autorotation::arming_checks(size_t buflen, char *buffer) const @@ -408,6 +484,36 @@ bool AC_Autorotation::arming_checks(size_t buflen, char *buffer) const } #endif + // Check that heli motors is configured for autorotation + if (!_motors_heli->rsc_autorotation_enabled()) { + hal.util->snprintf(buffer, buflen, "H_RSC_AROT_* not configured"); + return false; + } + return true; } +// Check if we believe we have landed. We need the landed state to zero all +// controls and make sure that the copter landing detector will trip +bool AC_Autorotation::check_landed(void) +{ + // minimum speed (m/s) used for "is moving" check + const float min_moving_speed = 1.0; + + Vector3f velocity; + _landed_reason.min_speed = _ahrs.get_velocity_NED(velocity) && (velocity.length() < min_moving_speed); + _landed_reason.land_col = _motors_heli->get_below_land_min_coll(); + _landed_reason.is_still = AP::ins().is_still(); + + return _landed_reason.min_speed && _landed_reason.land_col && _landed_reason.is_still; +} + +// Dynamically update time step used in autorotation controllers +void AC_Autorotation::set_dt(float delta_sec) +{ + if (is_positive(delta_sec)) { + _dt = delta_sec; + return; + } + _dt = 2.5e-3; // Assume 400 Hz +} diff --git a/libraries/AC_Autorotation/AC_Autorotation.h b/libraries/AC_Autorotation/AC_Autorotation.h index ec6ace93e74ce..7e27121f05727 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.h +++ b/libraries/AC_Autorotation/AC_Autorotation.h @@ -5,102 +5,100 @@ #include #include #include -#include #include #include - +#include +#include class AC_Autorotation { public: //Constructor - AC_Autorotation(); + AC_Autorotation(AP_AHRS& ahrs, AP_MotorsHeli*& motors, AC_AttitudeControl*& att_crtl); + + void init(void); - //--------Functions-------- bool enabled(void) const { return _param_enable.get() > 0; } + // Init and run entry phase controller + void init_entry(void); + void run_entry(float pilot_norm_accel); + + // Init and run the glide phase controller + void init_glide(void); + void run_glide(float pilot_norm_accel); + // Run the landed phase controller to zero the desired vels and accels + void run_landed(void); // Arming checks for autorotation, mostly checking for miss-configurations bool arming_checks(size_t buflen, char *buffer) const; + // Logging of lower rate autorotation specific variables + void log_write_autorotation(void) const; + // Returns true if we have met the autorotation-specific reasons to think we have landed + bool check_landed(void); - // not yet checked - void init_hs_controller(void); // Initialise head speed controller - void init_fwd_spd_controller(void); // Initialise forward speed controller - bool update_hs_glide_controller(float dt); // Update head speed controller - float get_rpm(void) const { return _current_rpm; } // Function just returns the rpm as last read in this library - float get_rpm(bool update_counter); // Function fetches fresh rpm update and continues sensor health monitoring - void set_target_head_speed(float ths) { _target_head_speed = ths; } // Sets the normalised target head speed - void set_col_cutoff_freq(float freq) { _col_cutoff_freq = freq; } // Sets the collective low pass filter cut off frequency - int16_t get_hs_set_point(void) { return _param_head_speed_set_point; } - float get_col_entry_freq(void) { return _param_col_entry_cutoff_freq; } - float get_col_glide_freq(void) { return _param_col_glide_cutoff_freq; } - float get_last_collective() const { return _collective_out; } - void Log_Write_Autorotation(void) const; - void update_forward_speed_controller(void); // Update forward speed controller - void set_desired_fwd_speed(void) { _vel_target = _param_target_speed; } // Overloaded: Set desired speed for forward controller to parameter value - void set_desired_fwd_speed(float speed) { _vel_target = speed; } // Overloaded: Set desired speed to argument value - int32_t get_pitch(void) const { return _pitch_target; } // Get pitch target - float calc_speed_forward(void); // Calculates the forward speed in the horizontal plane + // Dynamically update time step used in autorotation controllers void set_dt(float delta_sec); + // Helper to get measured head speed that has been normalised by head speed set point + bool get_norm_head_speed(float& norm_rpm) const; + // User Settable Parameters static const struct AP_Param::GroupInfo var_info[]; + static const uint32_t entry_time_ms = 2000; // (ms) Number of milliseconds that the entry phase operates for + private: - //--------Internal Variables-------- - float _current_rpm; - float _collective_out; - float _head_speed_error; // Error between target head speed and current head speed. Normalised by head speed set point RPM. - float _col_cutoff_freq; // Lowpass filter cutoff frequency (Hz) for collective. - uint8_t _unhealthy_rpm_counter; // Counter used to track RPM sensor unhealthy signal. - uint8_t _healthy_rpm_counter; // Counter used to track RPM sensor healthy signal. - float _target_head_speed; // Normalised target head speed. Normalised by head speed set point RPM. - 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 _pitch_target; // Pitch angle target. - float _accel_max; // Maximum acceleration limit. - int16_t _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 _delta_speed_fwd; // Change in forward speed between computation cycles. - float _dt; // Time step. - int16_t _speed_forward; // Measured forward speed. - float _vel_p; // Forward velocity P term. - float _vel_ff; // Forward velocity Feed Forward term. - float _accel_out; // Acceleration value used to calculate pitch target. - - LowPassFilterFloat _accel_target_filter; // acceleration target filter - - //--------Parameter Values-------- + // Calculates the forward ground speed in the horizontal plane + float get_speed_forward(void) const; + + // (s) Time step, updated dynamically from vehicle + float _dt; + + // Forward speed controller + void update_forward_speed_controller(float pilot_norm_accel); + AC_PID_Basic _fwd_speed_pid; // PID object for vel to accel controller + bool _limit_accel; // flag used for limiting integrator wind up if vehicle is against an accel or angle limit + float _desired_vel; // (m/s) This is the velocity that we want. This is the variable that is set by the invoking function to request a certain speed + float _target_vel; // (m/s) This is the acceleration constrained velocity that we are allowed + + // Head speed controller variables + void update_headspeed_controller(void); // Update controller used to drive head speed with collective + float _hs_decay; // The head speed target acceleration during the entry phase + float _head_speed_error; // Error between target head speed and current head speed. Normalised by head speed set point RPM. + float _target_head_speed; // Normalised target head speed. Normalised by head speed set point RPM. + float _p_term_hs; // Proportional contribution to collective setting. + float _ff_term_hs; // Following trim feed forward contribution to collective setting. + LowPassFilterFloat col_trim_lpf; // Low pass filter for collective trim + + // Flags used to check if we believe the aircraft has landed + struct { + bool min_speed; + bool land_col; + bool is_still; + } _landed_reason; + + // Parameter values AP_Int8 _param_enable; AC_P _p_hs; - AC_P _p_fw_vel; - AP_Int16 _param_head_speed_set_point; - AP_Int16 _param_target_speed; + AP_Float _param_head_speed_set_point; + AP_Float _param_target_speed; AP_Float _param_col_entry_cutoff_freq; AP_Float _param_col_glide_cutoff_freq; - AP_Int16 _param_accel_max; + AP_Float _param_accel_max; AP_Int8 _param_rpm_instance; AP_Float _param_fwd_k_ff; - //--------Internal Flags-------- - struct controller_flags { - bool bad_rpm : 1; - bool bad_rpm_warning : 1; - } _flags; - - //--------Internal Functions-------- - void set_collective(float _collective_filter_cutoff) const; + // Parameter accessors that provide value constraints + float get_accel_max(void) const { return MAX(_param_accel_max.get(), 0.5); } - // low pass filter for collective trim - LowPassFilterFloat col_trim_lpf; + // References to other libraries + AP_AHRS& _ahrs; + AP_MotorsHeli*& _motors_heli; + AC_AttitudeControl*& _attitude_control; }; diff --git a/libraries/AC_Autorotation/RSC_Autorotation.cpp b/libraries/AC_Autorotation/RSC_Autorotation.cpp index 53992de1a0467..44b71d7531a29 100644 --- a/libraries/AC_Autorotation/RSC_Autorotation.cpp +++ b/libraries/AC_Autorotation/RSC_Autorotation.cpp @@ -56,7 +56,7 @@ RSC_Autorotation::RSC_Autorotation(void) // to force the state to be immediately deactivated, then the force_state bool is used void RSC_Autorotation::set_active(bool active, bool force_state) { - if (enable.get() != 1) { + if (!enabled()) { return; } diff --git a/libraries/AC_Autorotation/RSC_Autorotation.h b/libraries/AC_Autorotation/RSC_Autorotation.h index 8c0bd230a7849..c6ecf154d4347 100644 --- a/libraries/AC_Autorotation/RSC_Autorotation.h +++ b/libraries/AC_Autorotation/RSC_Autorotation.h @@ -20,6 +20,7 @@ class RSC_Autorotation // state accessors bool active(void) const { return state == State::ACTIVE; } bool bailing_out(void) const { return state == State::BAILING_OUT; } + bool enabled(void) const { return enable.get() == 1; } // update idle throttle when in autorotation bool get_idle_throttle(float& idle_throttle);