Skip to content

Commit

Permalink
AC_Autorotation
Browse files Browse the repository at this point in the history 
-Fix constants definition
-Description of variables in header
  • Loading branch information
@Ferruccio1984 @bnsgeyer
Ferruccio1984 authored and bnsgeyer committed Jan 20, 2024
1 parent af175b9 commit 4ce4e2c
Show file tree
Hide file tree
Showing 2 changed files with 43 additions and 43 deletions.
26 changes: 13 additions & 13 deletions libraries/AC_Autorotation/AC_Autorotation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -355,21 +355,21 @@ void AC_Autorotation::initial_flare_estimate(void)
{
// Estimate hover thrust
float _col_hover_rad = radians(_motors_heli->get_hover_coll_ang());
float b = _param_solidity * 6.28f;
_disc_area = 3.14 * sq(_param_diameter * 0.5f);
float b = _param_solidity * M_2PI;
_disc_area = M_PI * sq(_param_diameter * 0.5f);
float lambda = (-(b / 8.0f) + safe_sqrt((sq(b)) / 64.0f +((b / 3.0f) * _col_hover_rad))) * 0.5f;
float freq = _motors_heli->get_rpm_setpoint() / 60.0f;
float tip_speed= freq * 6.28f * _param_diameter * 0.5f;
_lift_hover = ((1.225f * sq(tip_speed) * (_param_solidity * _disc_area)) * ((_col_hover_rad / 3.0f) - (lambda / 2.0f)) * 5.8f) * 0.5f;
float tip_speed= freq * M_2PI * _param_diameter * 0.5f;
_lift_hover = ((SSL_AIR_DENSITY * sq(tip_speed) * (_param_solidity * _disc_area)) * ((_col_hover_rad / 3.0f) - (lambda / 2.0f)) * 5.8f) * 0.5f;

// Estimate rate of descent
float omega_auto = (_param_head_speed_set_point / 60.0f) * 6.28f;
float omega_auto = (_param_head_speed_set_point / 60.0f) * M_2PI;
float tip_speed_auto = omega_auto * _param_diameter * 0.5f;
float c_t = _lift_hover / (0.6125f * _disc_area * sq(tip_speed));
_est_rod = ((0.25f * (_param_solidity * 0.011f / c_t) * tip_speed_auto) + ((sq(c_t) / (_param_solidity * 0.011f)) * tip_speed_auto));

// Estimate rotor C_d
_c = (_lift_hover / (sq(_est_rod) * 0.5f * 1.225f * _disc_area)) * 1.15f;
_c = (_lift_hover / (sq(_est_rod) * 0.5f * SSL_AIR_DENSITY * _disc_area)) * 1.15f;
_c = constrain_float(_c, 0.7f, 1.4f);

// Calc flare altitude
Expand All @@ -384,30 +384,30 @@ void AC_Autorotation::initial_flare_estimate(void)
_index_sink_rate = 0;
memset(_curr_sink_deriv, 0, sizeof(_curr_sink_deriv));

gcs().send_text(MAV_SEVERITY_INFO, "Ct/sigma=%f W=%f kg flare_alt=%f C=%f", c_t/_param_solidity, _lift_hover/9.8065f, _flare_alt_calc*0.01f, _c);
gcs().send_text(MAV_SEVERITY_INFO, "Ct/sigma=%f W=%f kg flare_alt=%f C=%f", c_t/_param_solidity, _lift_hover/GRAVITY_MSS, _flare_alt_calc*0.01f, _c);
}


void AC_Autorotation::calc_flare_alt(float sink_rate, float fwd_speed)
{
// Compute speed module and glide path angle during descent
float speed_module = norm(sink_rate, fwd_speed);
float glide_angle = safe_asin(3.14 / 2 - (fwd_speed / speed_module));
float glide_angle = safe_asin(M_PI / 2 - (fwd_speed / speed_module));

// Estimate inflow velocity at beginning of flare
float inflow = - speed_module * sinf(glide_angle + radians(AP_ALPHA_TPP));

// Estimate flare duration
float k_1 = fabsf((-sink_rate + 0.001f - safe_sqrt(_lift_hover / _c))/(-sink_rate + 0.001f + safe_sqrt(_lift_hover / _c)));
float k_2 = fabsf((inflow - safe_sqrt(_lift_hover / _c)) / (inflow + safe_sqrt(_lift_hover / _c)));
float delta_t_flare = (0.5f * (_lift_hover / (9.8065 * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_1)) - (0.5f * (_lift_hover / (9.8065 * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_2));
float delta_t_flare = (0.5f * (_lift_hover / (GRAVITY_MSS * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_1)) - (0.5f * (_lift_hover / (GRAVITY_MSS * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_2));

// Estimate flare delta altitude
float a = (2 * safe_sqrt(_c * 9.8065 / (_lift_hover / 9.8065))) * delta_t_flare + (2 * safe_sqrt(_c * 9.8065 / (_lift_hover / 9.8065))) * (0.5f * (_lift_hover / (9.8065 * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_1));
float a = (2 * safe_sqrt(_c * GRAVITY_MSS / (_lift_hover / GRAVITY_MSS))) * delta_t_flare + (2 * safe_sqrt(_c * GRAVITY_MSS / (_lift_hover / GRAVITY_MSS))) * (0.5f * (_lift_hover / (GRAVITY_MSS * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_1));
float x = 1 - expf(a);
float s = 1 - expf((2 * safe_sqrt(_c * 9.8065 / (_lift_hover / 9.8065))) * (0.5f * (_lift_hover/(9.8065 * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_1)));
float s = 1 - expf((2 * safe_sqrt(_c * GRAVITY_MSS / (_lift_hover / GRAVITY_MSS))) * (0.5f * (_lift_hover/(GRAVITY_MSS * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_1)));
float d = safe_sqrt(_lift_hover / _c);
float flare_distance = ((2 * d / (2 * safe_sqrt(_c * 9.8065 / (_lift_hover / 9.8065)))) * (a - logf(fabsf(x)) - (2 * safe_sqrt(_c * 9.8065 / (_lift_hover / 9.8065))) * (0.5f * (_lift_hover / (9.8065 * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_1)) + logf(fabsf(s)))) - d * delta_t_flare;
float flare_distance = ((2 * d / (2 * safe_sqrt(_c * GRAVITY_MSS / (_lift_hover / GRAVITY_MSS)))) * (a - logf(fabsf(x)) - (2 * safe_sqrt(_c * GRAVITY_MSS / (_lift_hover / GRAVITY_MSS))) * (0.5f * (_lift_hover / (GRAVITY_MSS * _c)) * safe_sqrt(_c / _lift_hover) * logf(k_1)) + logf(fabsf(s)))) - d * delta_t_flare;
float delta_h = -flare_distance * cosf(radians(AP_ALPHA_TPP));

// Estimate altitude to begin collective pull
Expand Down Expand Up @@ -558,7 +558,7 @@ void AC_Autorotation::update_flare_alt(void)
if ((_speed_forward >= 0.8f * _param_target_speed) && (delta_v_z <= 1) && (fabsf(_avg_sink_deriv)<=0.005)) {
float vel_z = _inav.get_velocity_z_up_cms() * 0.01f;
float spd_fwd = _speed_forward * 0.01f;
_c = (_lift_hover / (sq(vel_z) * 0.5f * 1.225f * _disc_area)) * 1.15f;
_c = (_lift_hover / (sq(vel_z) * 0.5f * SSL_AIR_DENSITY * _disc_area)) * 1.15f;
_c = constrain_float(_c, 0.7f, 1.4f);
calc_flare_alt(vel_z,spd_fwd);
_flare_update_check = true;
Expand Down
60 changes: 30 additions & 30 deletions libraries/AC_Autorotation/AC_Autorotation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -149,36 +149,36 @@ class AC_Autorotation
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.
float _entry_sink_rate;
float _entry_alt;
float _radar_alt;
float _flare_entry_speed;
float _desired_speed;
float _time_to_ground;
float _desired_sink_rate;
float _ground_clearance;
float _est_alt;
float _descent_rate_filtered;
float _radar_alt_prev;
float _radar_alt_calc;
float _avg_acc_z;
float _acc_z_sum;
int16_t _index;
float _curr_acc_z[10]{};
float _flare_alt_calc;
float _lift_hover;
float _c;
float _cushion_alt;
float _disc_area;
float _last_vertical_speed;
float _sink_deriv;
float _est_rod;
float _avg_sink_deriv;
float _avg_sink_deriv_sum;
int16_t _index_sink_rate;
float _curr_sink_deriv[20]{};
bool _flare_complete;
bool _flare_update_check;
float _entry_sink_rate; // Descent rate at beginning of touvhdown collective pull
float _entry_alt; // Altitude at beginning of touchdown coll pull
float _radar_alt; // Altitude above ground (RF)
float _flare_entry_speed; // Traslational velocity at beginning of flare maneuver
float _desired_speed; // Desired traslational velocity during flare
float _time_to_ground; // Time to ground
float _desired_sink_rate; // Desired vertical velocity during touchdown
float _ground_clearance; // Sensor offset distance
float _est_alt; // Estimated altitude above ground
float _descent_rate_filtered; // Filtered vertical speed
float _radar_alt_prev; // Last cycle calculated altitude
float _radar_alt_calc; // Inertial calculated altitude
float _avg_acc_z; // Averaged vertical acceleration
float _acc_z_sum; // Sum of vertical acceleration samples
int16_t _index; // Index for vertical acceleration rolling average
float _curr_acc_z[10]{}; // Array for storing vertical acceleration samples
float _flare_alt_calc; // Calculated flare altitude
float _lift_hover; // Main rotor thrust in hover condition
float _c; // Main rotor drag coefficient
float _cushion_alt; // Altitude for touchdown collective pull
float _disc_area; // Main rotor disc area
float _last_vertical_speed; // Last cycle measured vertical speed
float _sink_deriv; // Derivative of sink rate
float _est_rod; // Estimated rate of descent (vertical autorotation)
float _avg_sink_deriv; // Averaged derivative of rate of descent
float _avg_sink_deriv_sum; // Sum of averaged sink rate derivative
int16_t _index_sink_rate; // Index for sink rate derivative rolling average
float _curr_sink_deriv[20]{}; // Array for storing sink rate derivatives
bool _flare_complete; // Flare completed
bool _flare_update_check; // Check for flare altitude updating

LowPassFilterFloat _accel_target_filter; // acceleration target filter

Expand Down

0 comments on commit 4ce4e2c

Please sign in to comment.