#include "AC_Autorotation.h" #include #include #include #include extern const AP_HAL::HAL& hal; // Autorotation controller defaults #define HEAD_SPEED_TARGET_RATIO 1.0 // Normalised target main rotor head speed const AP_Param::GroupInfo AC_Autorotation::var_info[] = { // @Param: ENABLE // @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: Standard AP_GROUPINFO_FLAGS("ENABLE", 1, AC_Autorotation, _param_enable, 0, AP_PARAM_FLAG_ENABLE), // @Param: HS_P // @DisplayName: P gain for head speed controller // @Description: Increase value to increase sensitivity of head speed controller during autonomous autorotation. // @Range: 0.3 1 // @Increment: 0.01 // @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. // @Units: RPM // @Range: 1000 2800 // @Increment: 1 // @User: Standard AP_GROUPINFO("HS_SET_PT", 3, AC_Autorotation, _param_head_speed_set_point, 1500), // @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: 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. // @Units: Hz // @Range: 0.2 0.5 // @Increment: 0.01 // @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. // @Units: Hz // @Range: 0.03 0.15 // @Increment: 0.01 // @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. // @Units: s // @Range: 0.5 3 // @Increment: 0.1 // @User: Standard AP_GROUPINFO("HS_SENSOR", 8, AC_Autorotation, _param_rpm_instance, 0), // @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 // @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 // @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(AP_MotorsHeli*& motors, AC_AttitudeControl*& att_crtl) : _motors_heli(motors), _attitude_control(att_crtl) { AP_Param::setup_object_defaults(this, var_info); } void AC_Autorotation::init(void) { // 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); // 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 the landed reason _landed_reason.min_speed = false; _landed_reason.land_col = false; _landed_reason.is_still = false; } // Functions and config that are only to be done once at the beginning of the entry void AC_Autorotation::init_entry(void) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AROT: Entry Phase"); // 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; } // The rate to move the head speed from the current measurement to the target _hs_accel = (HEAD_SPEED_TARGET_RATIO - _target_head_speed) / (float(entry_time_ms)*1e-3); // Set collective following trim low pass filter cut off frequency col_trim_lpf.set_cutoff_frequency(_param_col_entry_cutoff_freq.get()); // Set collective low-pass cut off filter at 2 Hz _motors_heli->set_throttle_filter_cutoff(2.0); // Set speed target to maintain the current speed whilst we enter the autorotation _desired_vel = _param_target_speed.get(); _target_vel = get_speed_forward(); // Reset I term of velocity PID _fwd_speed_pid.reset_filter(); _fwd_speed_pid.set_integrator(0.0); } // 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; } // Slew the head speed target from the initial condition to the target head speed ratio for the glide const float max_change = _hs_accel * _dt; _target_head_speed = constrain_float(HEAD_SPEED_TARGET_RATIO, _target_head_speed - max_change, _target_head_speed + max_change); run_glide(pilot_norm_accel); } // 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, "AROT: 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); } void AC_Autorotation::update_headspeed_controller(void) { // 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 (ARHS) 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", "s-----", "F00000", "Qfffff", AP_HAL::micros64(), _target_head_speed, head_speed_norm, _head_speed_error, _p_term_hs, _ff_term_hs); #endif } // 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 { // 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(); // 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; } // Check RPM sensor is returning a healthy status if (!rpm->get_rpm(_param_rpm_instance.get(), current_rpm)) { return false; } #endif // Protect against div by zeros later in the code float head_speed_set_point = MAX(1.0, _param_head_speed_set_point.get()); // 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) Vector2f angle_target = { accel_to_angle(-bf_accel_target.x), // Pitch accel_to_angle(bf_accel_target.y)}; // Roll // Ensure that the requested angles do not exceed angle max _limit_accel |= angle_target.limit_length(_attitude_control->lean_angle_max_cd()); // we may have scaled the lateral accel in the angle limit scaling, so we need to // back calculate the resulting accel from this constrained angle for the yaw rate calc const float bf_lat_accel_target = angle_to_accel(angle_target.y); // 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_zero(_target_vel)) { yaw_rate_cds = degrees(bf_lat_accel_target / _target_vel) * 100.0; } // Output to attitude controller _attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(angle_target.y * 100.0, angle_target.x * 100.0, yaw_rate_cds); #if HAL_LOGGING_ENABLED // @LoggerMessage: ARSC // @Vehicles: Copter // @Description: Helicopter AutoRotation Speed Controller (ARSC) 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", "snnn-----oo", "F0000000-00", "QfffffffBff", AP_HAL::micros64(), _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 } // smoothly zero velocity and accel void AC_Autorotation::run_landed(void) { _desired_vel *= 0.95; update_forward_speed_controller(0.0); } // Determine the body frame forward speed float AC_Autorotation::get_speed_forward(void) const { Vector3f vel_NED = {0,0,0}; const AP_AHRS &ahrs = AP::ahrs(); 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; } #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 { // 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); } if (_landed_reason.land_col) { reason |= uint8_t(AC_Autorotation_Landed_Reason::LAND_COL); } if (_landed_reason.is_still) { reason |= uint8_t(AC_Autorotation_Landed_Reason::IS_STILL); } // @LoggerMessage: AROT // @Vehicles: Copter // @Description: Helicopter AutoROTation (AROT) information // @Field: TimeUS: Time since system startup // @Field: LR: Landed Reason state flags // @FieldBitmaskEnum: LR: AC_Autorotation_Landed_Reason // Write to data flash log AP::logger().WriteStreaming("AROT", "TimeUS,LR", "s-", "F-", "QB", AP_HAL::micros64(), _landed_reason); } #endif // HAL_LOGGING_ENABLED // Arming checks for autorotation, mostly checking for miss-configurations bool AC_Autorotation::arming_checks(size_t buflen, char *buffer) const { if (!enabled()) { // Don't run arming checks if not enabled return true; } // Check for correct RPM sensor config #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) { hal.util->snprintf(buffer, buflen, "Can't access RPM"); return false; } // Sanity check that the designated rpm sensor instance is there if (_param_rpm_instance.get() < 0) { hal.util->snprintf(buffer, buflen, "RPM instance <0"); return false; } if (!rpm->enabled(_param_rpm_instance.get())) { hal.util->snprintf(buffer, buflen, "RPM%i not enabled", _param_rpm_instance.get()+1); return false; } #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; const AP_AHRS &ahrs = AP::ahrs(); _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 }