AC_Autorotation: Created autorotation library

libraries/AC_Autorotation/AC_Autorotation.cpp

@@ -0,0 +1,367 @@
+#include "AC_Autorotation.h"
+#include <AP_Logger/AP_Logger.h>
+#include <AP_RPM/AP_RPM.h>
+#include <AP_AHRS/AP_AHRS.h>
+
+//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;
+}
+

libraries/AC_Autorotation/AC_Autorotation.h

@@ -0,0 +1,108 @@
+#pragma once
+
+#include <AP_Common/AP_Common.h>
+#include <AP_Param/AP_Param.h>
+#include <AP_Math/AP_Math.h>
+#include <AP_Motors/AP_Motors.h>
+#include <AP_Motors/AP_MotorsHeli_RSC.h>
+#include <Filter/Filter.h>
+#include <Filter/LowPassFilter.h>
+#include <AC_PID/AC_P.h>
+#include <AP_InertialNav/AP_InertialNav.h>
+#include <AP_AHRS/AP_AHRS.h>
+#include <AP_HAL/AP_HAL.h>
+
+
+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;
+
+};