Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_Autorotation: Created autorotation library

This commit is contained in:
Gone4Dirt 2019-11-28 20:19:17 +00:00 committed by Randy Mackay
parent 401dfc3e0e
commit ef72d89d26
2 changed files with 475 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

367
libraries/AC_Autorotation/AC_Autorotation.cpp Normal file
View File

@ -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;
}

108
libraries/AC_Autorotation/AC_Autorotation.h Normal file
View File

@ -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;
};