diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp new file mode 100644 index 0000000000..7cacb4d633 --- /dev/null +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -0,0 +1,367 @@ +#include "AC_Autorotation.h" +#include +#include +#include + +//Autorotation controller defaults +#define AROT_BAIL_OUT_TIME 2.0f // Default time for bail out controller to run (unit: s) + +// 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 + + +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: Advanced + AP_GROUPINFO_FLAGS("ENABLE", 1, AC_Autorotation, _param_enable, 0, AP_PARAM_FLAG_ENABLE), + + // @Param: HS_P + // @DisplayName: P gain for head spead controller + // @Description: Increase value to increase sensitivity of head speed controller during autonomous autorotation. + // @Range: 0.3 1 + // @Increment: 0.01 + // @User: Advanced + 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: Advanced + AP_GROUPINFO("HS_SET_PT", 3, AC_Autorotation, _param_head_speed_set_point, 1500), + + // @Param: TARG_SP + // @DisplayName: Target Glide Ground 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), + + // @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: Advanced + AP_GROUPINFO("COL_FILT_E", 5, AC_Autorotation, _param_col_entry_cutoff_freq, HS_CONTROLLER_ENTRY_COL_FILTER), + + // @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: 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), + + // @Param: BAIL_TIME + // @DisplayName: Bail Out Timer + // @Description: Time in seconds from bail out initiated to the exit of autorotation flight mode. + // @Units: s + // @Range: 0.5 4 + // @Increment: 0.1 + // @User: Advanced + AP_GROUPINFO("BAIL_TIME", 8, AC_Autorotation, _param_bail_time, AROT_BAIL_OUT_TIME), + + // @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: Advanced + AP_GROUPINFO("HS_SENSOR", 9, AC_Autorotation, _param_rpm_instance, 0), + + // @Param: FW_V_P + // @DisplayName: Velocity (horizontal) P gain + // @Description: Velocity (horizontal) P gain. Determines the propotion of the target acceleration based on the velocity error. + // @Range: 0.1 6.0 + // @Increment: 0.1 + // @User: Advanced + AP_SUBGROUPINFO(_p_fw_vel, "FW_V_", 10, 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. + // @Range: 0 1 + // @Increment: 0.01 + // @User: Advanced + AP_GROUPINFO("FW_V_FF", 11, AC_Autorotation, _param_fwd_k_ff, AP_FW_VEL_FF), + + AP_GROUPEND +}; + +// Constructor +AC_Autorotation::AC_Autorotation(AP_InertialNav& inav) : + _inav(inav), + _p_hs(HS_CONTROLLER_HEADSPEED_P), + _p_fw_vel(AP_FW_VEL_P) + { + AP_Param::setup_object_defaults(this, var_info); + } + +// Initialisation of head speed controller +void AC_Autorotation::init_hs_controller() +{ + // Set initial collective position to be the collective position on initialisation + _collective_out = 0.4f; + + // Reset feed forward filter + col_trim_lpf.reset(_collective_out); + + // Reset flags + _flags.bad_rpm = false; + + // Reset RPM health monitoring + _unhealthy_rpm_counter = 0; + _healthy_rpm_counter = 0; + + // Protect against divide by zero + _param_head_speed_set_point = MAX(_param_head_speed_set_point,500); +} + + +bool AC_Autorotation::update_hs_glide_controller(float dt) +{ + // Reset rpm health flag + _flags.bad_rpm = false; + _flags.bad_rpm_warning = false; + + // Get current rpm and update healthly signal counters + _current_rpm = get_rpm(true); + + if (_unhealthy_rpm_counter <=30) { + // Normalised head speed + float head_speed_norm = _current_rpm / _param_head_speed_set_point; + + // Set collective trim low pass filter cut off frequency + col_trim_lpf.set_cutoff_frequency(_col_cutoff_freq); + + // 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; + + _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(_collective_out, dt); + + // Calculate collective position to be set + _collective_out = _p_term_hs + _ff_term_hs; + + } else { + // RPM sensor is bad set collective to minimum + _collective_out = -1.0f; + + _flags.bad_rpm_warning = true; + } + + // Send collective to setting to motors output library + set_collective(HS_CONTROLLER_COLLECTIVE_CUTOFF_FREQ); + + return _flags.bad_rpm_warning; +} + + +// Function to set collective and collective filter in motor library +void AC_Autorotation::set_collective(float collective_filter_cutoff) +{ + AP_Motors *motors = AP::motors(); + if (motors) { + motors->set_throttle_filter_cutoff(collective_filter_cutoff); + motors->set_throttle(_collective_out); + } +} + + +//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 singleton for RPM library + const AP_RPM *rpm = AP_RPM::get_singleton(); + + float current_rpm = 0.0f; + + //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 = 0; + } + + //Get RPM value + uint8_t instance = _param_rpm_instance; + current_rpm = rpm->get_rpm(instance); + + //Check RPM sesnor is returning a healthy status + if (current_rpm <= -1) { + //unhealthy, rpm unreliable + _flags.bad_rpm = true; + } + + } else { + _flags.bad_rpm = true; + } + + if (_flags.bad_rpm) { + //count unhealthy rpm updates and reset healthy rpm counter + _unhealthy_rpm_counter++; + _healthy_rpm_counter = 0; + + } else if (!_flags.bad_rpm && _unhealthy_rpm_counter > 0) { + //poor rpm reading may have cleared. Wait 10 update cycles to clear. + _healthy_rpm_counter++; + + if (_healthy_rpm_counter >= 10) { + //poor rpm health has cleared, reset counters + _unhealthy_rpm_counter = 0; + _healthy_rpm_counter = 0; + } + } + return current_rpm; +} + + +void AC_Autorotation::Log_Write_Autorotation(void) +{ + //Write to data flash log + AP::logger().Write("AROT", + "TimeUS,P,hserr,ColOut,FFCol,CRPM,SpdF,CmdV,p,ff,AccO,AccT,PitT", + "Qffffffffffff", + 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); +} + + +// Initialise forward speed controller +void AC_Autorotation::init_fwd_spd_controller(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; +} + + +// set_dt - sets time delta in seconds for all controllers +void AC_Autorotation::set_dt(float delta_sec) +{ + _dt = delta_sec; +} + + +// update speed controller +void AC_Autorotation::update_forward_speed_controller(void) +{ + // 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) + + // Limitng 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 + if (fabsf(_delta_speed_fwd) > _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 = atanf(-_accel_out/(GRAVITY_MSS * 100.0f))*(18000.0f/M_PI); + +} + + +// 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; +} + diff --git a/libraries/AC_Autorotation/AC_Autorotation.h b/libraries/AC_Autorotation/AC_Autorotation.h new file mode 100644 index 0000000000..54b500ed57 --- /dev/null +++ b/libraries/AC_Autorotation/AC_Autorotation.h @@ -0,0 +1,108 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +class AC_Autorotation +{ +public: + + //Constructor + AC_Autorotation(AP_InertialNav& inav); + + //--------Functions-------- + 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_bail_time(void) { return _param_bail_time; } + float get_last_collective() const { return _collective_out; } + bool is_enable(void) { return _param_enable; } + void Log_Write_Autorotation(void); + void update_forward_speed_controller(void); // Update foward 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 + void set_dt(float delta_sec); + + // User Settable Parameters + static const struct AP_Param::GroupInfo var_info[]; + +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. + uint16_t _log_counter; + 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 _vel_error; // Velocity error. + 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-------- + 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_col_entry_cutoff_freq; + AP_Float _param_col_glide_cutoff_freq; + AP_Int16 _param_accel_max; + AP_Float _param_bail_time; + 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); + + // low pass filter for collective trim + LowPassFilterFloat col_trim_lpf; + + //--------References to Other Libraries-------- + AP_InertialNav& _inav; + +};