mirror of https://github.com/ArduPilot/ardupilot
AC_Autorotation: Mode restructure and speed controller improvement
This commit is contained in:
MattKear
parent
b18d05c2d9
commit
8ef67b4e60
|
|
@ -2,20 +2,10 @@
|
|||
#include <AP_Logger/AP_Logger.h>
|
||||
#include <AP_RPM/AP_RPM.h>
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
|
||||
// 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,7 +21,7 @@ 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
|
||||
|
|
@ -40,17 +30,17 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = {
|
|||
// @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
|
||||
|
|
@ -58,8 +48,8 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = {
|
|||
// @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
|
||||
|
|
@ -67,17 +57,17 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = {
|
|||
// @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),
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("COL_FILT_G", 6, AC_Autorotation, _param_col_glide_cutoff_freq, 0.1),
|
||||
|
||||
// @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: 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
|
||||
|
|
@ -85,296 +75,382 @@ const AP_Param::GroupInfo AC_Autorotation::var_info[] = {
|
|||
// @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
|
||||
// @User: Advanced
|
||||
AP_SUBGROUPINFO(_p_fw_vel, "FW_V_", 9, AC_Autorotation, AC_P),
|
||||
// @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: FW_V_FF
|
||||
// @DisplayName: Velocity (horizontal) feed forward
|
||||
// @Description: Velocity (horizontal) input filter. Corrects the target acceleration proportionally to the desired velocity.
|
||||
// @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
|
||||
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");
|
||||
|
||||
// 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 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 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;
|
||||
}
|
||||
|
||||
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 {
|
||||
_target_head_speed = HEAD_SPEED_TARGET_RATIO;
|
||||
}
|
||||
|
||||
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, "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
|
||||
_current_rpm = get_rpm(true);
|
||||
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;
|
||||
}
|
||||
|
||||
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.
|
||||
// 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(_collective_out, dt);
|
||||
_ff_term_hs = col_trim_lpf.apply(_motors_heli->get_throttle(), _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;
|
||||
}
|
||||
const float collective_out = constrain_value((_p_term_hs + _ff_term_hs), 0.0f, 1.0f);
|
||||
|
||||
// Send collective to setting to motors output library
|
||||
set_collective(HS_CONTROLLER_COLLECTIVE_CUTOFF_FREQ);
|
||||
_motors_heli->set_throttle(collective_out);
|
||||
|
||||
return _flags.bad_rpm_warning;
|
||||
#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 to set collective and collective filter in motor library
|
||||
void AC_Autorotation::set_collective(float collective_filter_cutoff) const
|
||||
// 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
|
||||
{
|
||||
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)
|
||||
{
|
||||
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);
|
||||
// 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;
|
||||
}
|
||||
|
||||
//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;
|
||||
// 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;
|
||||
}
|
||||
#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++;
|
||||
|
||||
if (_healthy_rpm_counter >= 10) {
|
||||
//poor rpm health has cleared, reset counters
|
||||
_unhealthy_rpm_counter = 0;
|
||||
_healthy_rpm_counter = 0;
|
||||
}
|
||||
}
|
||||
return current_rpm;
|
||||
// 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);
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
// 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
|
||||
// @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
|
||||
|
||||
//Write to data flash log
|
||||
AP::logger().WriteStreaming("AROT",
|
||||
"TimeUS,P,hserr,ColOut,FFCol,CRPM,SpdF,CmdV,p,ff,AccO,AccT,PitT",
|
||||
"Qffffffffffff",
|
||||
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
|
||||
}
|
||||
|
||||
// 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};
|
||||
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 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);
|
||||
}
|
||||
#endif // HAL_LOGGING_ENABLED
|
||||
|
||||
// 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)
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
//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 = accel_to_angle(-_accel_out*0.01) * 100;
|
||||
|
||||
}
|
||||
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
// 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
|
||||
}
|
||||
|
|
|
|||
|
|
@ -5,102 +5,100 @@
|
|||
#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 <AC_PID/AC_PID_Basic.h>
|
||||
#include <AC_AttitudeControl/AC_AttitudeControl.h>
|
||||
|
||||
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;
|
||||
// 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 _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.
|
||||
LowPassFilterFloat col_trim_lpf; // Low pass filter for collective trim
|
||||
|
||||
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.
|
||||
// Flags used to check if we believe the aircraft has landed
|
||||
struct {
|
||||
bool min_speed;
|
||||
bool land_col;
|
||||
bool is_still;
|
||||
} _landed_reason;
|
||||
|
||||
LowPassFilterFloat _accel_target_filter; // acceleration target filter
|
||||
|
||||
//--------Parameter Values--------
|
||||
// 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;
|
||||
// Parameter accessors that provide value constraints
|
||||
float get_accel_max(void) const { return MAX(_param_accel_max.get(), 0.5); }
|
||||
|
||||
//--------Internal Functions--------
|
||||
void set_collective(float _collective_filter_cutoff) const;
|
||||
|
||||
// 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;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -20,6 +20,7 @@ public:
|
|||
// 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);
|
||||
|
|
|
|||
Loading…
Reference in New Issue