AC_Autorotation: Derivation: Add reworked math

libraries/AC_Autorotation/Derivation/sensitivity_study.py

@@ -22,6 +22,20 @@ def safe_asin(f):
         return -2 * math.pi
     return math.asin(f)
 
+def safe_acos(f):
+    if f >= 1.0:
+        return 0
+    if f <= -1.0:
+        return -math.pi
+    return math.acos(f)
+
+def coth(x):
+    # print(x)
+    try:
+        return (math.exp(2*x) + 1) / (math.exp(2*x) - 1)
+    except:
+        return 0.0
+
 # =====================================================================================
 # Math from Ferruccio's original PR
 class Flare_Calc_Pre_Changes():
@@ -63,7 +77,8 @@ def initial_flare_estimate(self):
         # calc flare altitude
         des_spd_fwd=self.var['_param_target_speed']*0.01
 
-        return (self.calc_flare_alt(-_est_rod, des_spd_fwd), self._lift_hover/GRAVITY_MSS, lambda_eq)
+        flare_alt, delta_t_flare = self.calc_flare_alt(-_est_rod, des_spd_fwd)
+        return (flare_alt, self._lift_hover/GRAVITY_MSS, delta_t_flare)
 
     def calc_flare_alt(self, sink_rate, fwd_speed):
 
@@ -93,11 +108,93 @@ def calc_flare_alt(self, sink_rate, fwd_speed):
         # total delta altitude to ground
         _flare_alt_calc = _cushion_alt+delta_h*100.0
 
-        return _flare_alt_calc*1e-2
+        return _flare_alt_calc*1e-2, delta_t_flare
+# =====================================================================================
+
+# =====================================================================================
+# Math from Ferruccio's reworked math
+class Flare_Calc_Reworked_Math():
+    def __init__(self):
+        self.reset_to_defaults()
+
+    def reset_to_defaults(self):
+        self.var = {}
+        self.var['_param_solidity'] = 0.05
+        self.var['_c_l_alpha'] = np.pi*2
+        self.var['_param_diameter'] = 1.25
+        self.var['blade_pitch_hover_deg'] = 5.0
+        self.var['_param_head_speed_set_point'] = 1500
+        self.var['_param_target_speed'] = 1100
+        self.var['_t_tch'] = 0.55
+
+    def initial_flare_estimate(self):
+
+        blade_pitch_hover_rad = deg2rad(self.var['blade_pitch_hover_deg'])
+        blade_pitch_hover_rad = max(blade_pitch_hover_rad, deg2rad(0.1))
+
+        b = self.var['_param_solidity'] * self.var['_c_l_alpha']
+        _disc_area = np.pi * 0.25 * self.var['_param_diameter']**2
+
+        # Calculating the equivalent inflow ratio (average across the whole blade)
+        # lambda_eq = -b / 16.0 + math.sqrt((b**2) / 256.0 + b * blade_pitch_hover_rad / 8.0)
+        # lambda_eq = -b / 16.0 + math.sqrt((b**2) / 256.0 + b * blade_pitch_hover_rad / 12.0) # <--------
+        lambda_eq = -b / 8.0 + math.sqrt((b**2) / 64.0 + b * blade_pitch_hover_rad / 6.0) # <--------
+
+        # Tip speed = head speed (rpm) / 60 * 2pi * rotor diameter/2. Eq below is simplified.
+        tip_speed_auto = self.var['_param_head_speed_set_point'] * np.pi * self.var['_param_diameter'] / 60.0
+
+        # Calc the coefficient of thrust in the hover
+        c_t_hover = 0.5 * b * (blade_pitch_hover_rad / 3.0 - lambda_eq / 2.0)
+        self._lift_hover = c_t_hover * SSL_AIR_DENSITY * _disc_area * tip_speed_auto**2
+
+        # Estimate rate of descent
+        _est_rod = ((0.25 * self.var['_param_solidity'] * ASSUMED_CD0 / c_t_hover) + (c_t_hover**2 / (self.var['_param_solidity'] * ASSUMED_CD0))) * tip_speed_auto
+
+        # Estimate rotor C_d
+        self._c = self._lift_hover / (_est_rod**2)
+
+        # Calc flare altitude
+        des_spd_fwd = self.var['_param_target_speed'] * 0.01
+
+        flare_alt, delta_t_flare = self.calc_flare_alt(-_est_rod, des_spd_fwd)
+        return (flare_alt, self._lift_hover/GRAVITY_MSS, -_est_rod)
+
+    def calc_flare_alt(self, sink_rate, fwd_speed):
+
+        # Compute speed module and glide path angle during descent
+        speed_module = max(norm(sink_rate, fwd_speed), 0.1)
+        # glide_angle = safe_asin(np.pi / 2 - (fwd_speed / speed_module))
+        # glide_angle = safe_acos(- (fwd_speed / speed_module))
+        glide_angle = np.pi / 2 - safe_asin(fwd_speed / speed_module)
+
+        # Estimate inflow velocity at beginning of flare
+        entry_inflow = - speed_module * math.sin(glide_angle + deg2rad(AP_ALPHA_TPP))
+
+        # Estimate flare duration
+        m = self._lift_hover / GRAVITY_MSS
+        k_1 = math.sqrt(self._lift_hover / self._c)
+        k_3 = math.sqrt((self._c * GRAVITY_MSS) / m)
+        k_2 = 1 / (2 * k_3) * math.log(abs((entry_inflow - k_1)/(entry_inflow + k_1)))
+        a = math.log(abs((sink_rate - 0.05 - k_1)/(sink_rate - 0.05 + k_1)))
+        b = math.log(abs((entry_inflow - k_1)/(entry_inflow + k_1)))
+        delta_t_flare = (1 / (2 * k_3)) * (a - b)
+
+        # Estimate flare delta altitude
+        k_4 = (2 * k_2 * k_3) + (2 * k_3 * delta_t_flare)
+        flare_distance = ((k_1 / k_3) * (k_4 - math.log(abs(1-math.exp(k_4))) - (2 * k_2 * k_3 - math.log(abs(1 - math.exp(2 * k_2 * k_3)))))) - k_1 * delta_t_flare
+        delta_h = -flare_distance * math.cos(deg2rad(AP_ALPHA_TPP));
+
+        # Estimate altitude to begin collective pull
+        _cushion_alt = (-(sink_rate * math.cos(deg2rad(AP_ALPHA_TPP))) * self.var['_t_tch']) * 100.0
+
+        # Total delta altitude to ground
+        _flare_alt_calc = _cushion_alt + delta_h * 100.0
+
+        return _flare_alt_calc*1e-2, glide_angle
 # =====================================================================================
 
 # =====================================================================================
-# Math from my branch, after my dicking around
+# Math from my branch, after my reworks
 class Flare_Calc():
     def __init__(self):
         self.reset_to_defaults()
@@ -144,6 +241,37 @@ def initial_flare_estimate(self):
         flare_alt, delta_t_flare = self.calc_flare_alt(-_est_rod, des_spd_fwd)
         return (flare_alt, self._lift_hover/GRAVITY_MSS, delta_t_flare)
 
+    # def calc_flare_alt(self, sink_rate, fwd_speed):
+
+    #     # Compute speed module and glide path angle during descent
+    #     speed_module = max(norm(sink_rate, fwd_speed), 0.1)
+    #     glide_angle = safe_asin(np.pi / 2 - (fwd_speed / speed_module))
+
+    #     # Estimate inflow velocity at beginning of flare
+    #     inflow = - speed_module * math.sin(glide_angle + deg2rad(AP_ALPHA_TPP))
+
+    #     # Estimate flare duration
+    #     k_1 = np.abs((-sink_rate + 0.001 - math.sqrt(self._lift_hover / self._c))/(-sink_rate + 0.001 + math.sqrt(self._lift_hover / self._c)))
+    #     k_2 = np.abs((inflow - math.sqrt(self._lift_hover / self._c)) / (inflow + math.sqrt(self._lift_hover / self._c)))
+    #     delta_t_flare = (0.5 * (self._lift_hover / (GRAVITY_MSS * self._c)) * math.sqrt(self._c / self._lift_hover) * math.log(k_1)) - (0.5 * (self._lift_hover / (GRAVITY_MSS * self._c)) * math.sqrt(self._c / self._lift_hover) * math.log(k_2))
+
+    #     # Estimate flare delta altitude
+    #     sq_gravity = GRAVITY_MSS**2
+    #     a = (2 * math.sqrt(self._c * sq_gravity / self._lift_hover )) * delta_t_flare + (2 * math.sqrt(self._c * sq_gravity / self._lift_hover )) * (0.5 * (self._lift_hover / (GRAVITY_MSS * self._c)) * math.sqrt(self._c / self._lift_hover) * math.log(k_1))
+    #     x = 1 - math.exp(a)
+    #     s = 1 - math.exp((2 * math.sqrt(self._c * sq_gravity / self._lift_hover )) * (0.5 * (self._lift_hover/(GRAVITY_MSS * self._c)) * math.sqrt(self._c / self._lift_hover) * math.log(k_1)))
+    #     d = math.sqrt(self._lift_hover / self._c)
+    #     flare_distance = ((2 * d / (2 * math.sqrt(self._c * sq_gravity / self._lift_hover ))) * (a - math.log(abs(x)) - (2 * math.sqrt(self._c * sq_gravity / self._lift_hover )) * (0.5 * (self._lift_hover / (GRAVITY_MSS * self._c)) * math.sqrt(self._c / self._lift_hover) * math.log(k_1)) + math.log(abs(s)))) - d * delta_t_flare
+    #     delta_h = -flare_distance * math.cos(deg2rad(AP_ALPHA_TPP))
+
+    #     # Estimate altitude to begin collective pull
+    #     _cushion_alt = (-(sink_rate * math.cos(deg2rad(AP_ALPHA_TPP))) * self.var['_t_tch']) * 100.0
+
+    #     # Total delta altitude to ground
+    #     _flare_alt_calc = _cushion_alt + delta_h * 100.0
+
+    #     return _flare_alt_calc*1e-2, delta_t_flare
+
     def calc_flare_alt(self, sink_rate, fwd_speed):
 
         # Compute speed module and glide path angle during descent
@@ -154,6 +282,12 @@ def calc_flare_alt(self, sink_rate, fwd_speed):
         inflow = - speed_module * math.sin(glide_angle + deg2rad(AP_ALPHA_TPP))
 
         # Estimate flare duration
+        b = math.sqrt(self._lift_hover / self._c)
+        v_initial = inflow
+        v_final = -sink_rate + 0.001
+        delta_t_flare = (self._lift_hover / (self._c*GRAVITY_MSS)) * ((-coth(v_final/b)/b) - (-coth(v_initial/b)/b))
+
+        # Old Estimate for flare duration
         k_1 = np.abs((-sink_rate + 0.001 - math.sqrt(self._lift_hover / self._c))/(-sink_rate + 0.001 + math.sqrt(self._lift_hover / self._c)))
         k_2 = np.abs((inflow - math.sqrt(self._lift_hover / self._c)) / (inflow + math.sqrt(self._lift_hover / self._c)))
         delta_t_flare = (0.5 * (self._lift_hover / (GRAVITY_MSS * self._c)) * math.sqrt(self._c / self._lift_hover) * math.log(k_1)) - (0.5 * (self._lift_hover / (GRAVITY_MSS * self._c)) * math.sqrt(self._c / self._lift_hover) * math.log(k_2))
@@ -176,8 +310,14 @@ def calc_flare_alt(self, sink_rate, fwd_speed):
         return _flare_alt_calc*1e-2, delta_t_flare
 # =====================================================================================
 
-
+n_col = 3
+n_row = 2
+fig_flare_alt, ax_fa = plt.subplots(n_row, n_col, figsize=(15,8))
+fig_mass_est, ax_me = plt.subplots(n_row, n_col, figsize=(15,8))
+fig_extra_var, ax_ev = plt.subplots(n_row, n_col, figsize=(15,8))
+plot_index = 0
 def plot_single_var_sensitivity(obj, var_name, var_array):
+    global plot_index, n_col, ax_fa, ax_me, ax_ev
     flare_alt = []
     mass_est = []
     other_var = []
@@ -188,31 +328,38 @@ def plot_single_var_sensitivity(obj, var_name, var_array):
         mass_est.append(mass)
         other_var.append(delta_t_f)
 
+    r = math.floor(plot_index / n_col)
+    c = plot_index % n_col
+
     # Plot against flare alt
-    fig, ax = plt.subplots()
-    ax.plot(var_array, flare_alt)
-    ax.set_xlabel(var_name)
-    ax.set_ylabel('Flare Alt (m)')
-
-    # Plot against mass estimate
-    fig, ax = plt.subplots()
-    ax.plot(var_array, mass_est)
-    ax.set_xlabel(var_name)
-    ax.set_ylabel('Mass Est (kg)')
-
-     # Plot against inflow ratio
-    fig, ax = plt.subplots()
-    ax.plot(var_array, other_var)
-    ax.set_xlabel(var_name)
-    ax.set_ylabel('Flare Time ()')
+    ax_fa[r,c].plot(var_array, flare_alt)
+    ax_fa[r,c].set_xlabel(var_name)
+    ax_fa[r,c].set_ylabel('Flare Alt (m)')
+    plt.tight_layout()
+
+    # # Plot against mass estimate
+    ax_me[r,c].plot(var_array, mass_est)
+    ax_me[r,c].set_xlabel(var_name)
+    ax_me[r,c].set_ylabel('Mass Est (kg)')
+    plt.tight_layout()
+
+     # Plot against ...
+    ax_ev[r,c].plot(var_array, other_var)
+    ax_ev[r,c].set_xlabel(var_name)
+    # ax_ev[r,c].set_ylabel('Flare Time ()')
+    ax_ev[r,c].set_ylabel('Other Var ()')
+    plt.tight_layout()
+
+    plot_index+=1
 
     obj.reset_to_defaults()
 
 
 
 if __name__=='__main__':
     # Setup flare object to do calculations
-    flare_obj = Flare_Calc_Pre_Changes()
+    # flare_obj = Flare_Calc_Pre_Changes()
+    flare_obj = Flare_Calc_Reworked_Math()
     # flare_obj = Flare_Calc()
 
     N_PTS = 100000