ENH: add coriolis to parachute

Author: MateusStano

rocketpy/simulation/flight.py

@@ -1197,6 +1197,10 @@ def __init_equations_of_motion(self):
         if self.equations_of_motion == "solid_propulsion":
             # NOTE: The u_dot is faster, but only works for solid propulsion
             self.u_dot_generalized = self.u_dot
+        if self.equations_of_motion == "2":
+            self.u_dot_generalized = self.u_dot_generalized_std
+        if self.equations_of_motion == "3":
+            self.u_dot_generalized = self.u_dot_generalized_moving_omega
 
     def __init_controllers(self):
         """Initialize controllers and sensors"""
@@ -1935,6 +1939,216 @@ def u_dot_generalized(self, t, u, post_processing=False):  # pylint: disable=too
 
         return u_dot
 
+    def u_dot_generalized_std(self, t, u, post_processing=False):  # pylint: disable=too-many-locals,too-many-statements
+        """Calculates derivative of u state vector with respect to time when the
+        rocket is flying in 6 DOF motion in space and significant mass variation
+        effects exist. Typical flight phases include powered ascent after launch
+        rail.
+
+        Parameters
+        ----------
+        t : float
+            Time in seconds
+        u : list
+            State vector defined by u = [x, y, z, vx, vy, vz, q0, q1,
+            q2, q3, omega1, omega2, omega3].
+        post_processing : bool, optional
+            If True, adds flight data information directly to self variables
+            such as self.angle_of_attack, by default False.
+
+        Returns
+        -------
+        u_dot : list
+            State vector defined by u_dot = [vx, vy, vz, ax, ay, az,
+            e0_dot, e1_dot, e2_dot, e3_dot, alpha1, alpha2, alpha3].
+        """
+        # Retrieve integration data
+        _, _, z, vx, vy, vz, e0, e1, e2, e3, omega1, omega2, omega3 = u
+
+        # Create necessary vectors
+        # r = Vector([x, y, z])  # CDM position vector
+        v = Vector([vx, vy, vz])  # CDM velocity vector
+        e = [e0, e1, e2, e3]  # Euler parameters/quaternions
+        w = Vector([omega1, omega2, omega3])  # Angular velocity vector
+
+        # Retrieve necessary quantities
+        ## Rocket mass
+        total_mass = self.rocket.total_mass.get_value_opt(t)
+        total_mass_dot = self.rocket.total_mass_flow_rate.get_value_opt(t)
+        total_mass_ddot = self.rocket.total_mass_flow_rate.differentiate_complex_step(t)
+        ## CM position vector and time derivatives relative to CDM in body frame
+        r_CM_z = self.rocket.com_to_cdm_function
+        r_CM_t = r_CM_z.get_value_opt(t)
+        r_CM = Vector([0, 0, r_CM_t])
+        r_CM_dot = Vector([0, 0, r_CM_z.differentiate_complex_step(t)])
+        r_CM_ddot = Vector([0, 0, r_CM_z.differentiate(t, order=2)])
+        ## Nozzle position vector
+        r_NOZ = Vector([0, 0, self.rocket.nozzle_to_cdm])
+        ## Nozzle gyration tensor
+        S_nozzle = self.rocket.nozzle_gyration_tensor
+        ## Inertia tensor
+        inertia_tensor = self.rocket.get_inertia_tensor_at_time(t)
+        ## Inertia tensor time derivative in the body frame
+        I_dot = self.rocket.get_inertia_tensor_derivative_at_time(t)
+
+        # Calculate the Inertia tensor relative to CM
+        H = (r_CM.cross_matrix @ -r_CM.cross_matrix) * total_mass
+        I_CM = inertia_tensor - H
+
+        # Prepare transformation matrices
+        K = Matrix.transformation(e)
+        Kt = K.transpose
+
+        # Compute aerodynamic forces and moments
+        R1, R2, R3, M1, M2, M3 = 0, 0, 0, 0, 0, 0
+
+        ## Drag force
+        rho = self.env.density.get_value_opt(z)
+        wind_velocity_x = self.env.wind_velocity_x.get_value_opt(z)
+        wind_velocity_y = self.env.wind_velocity_y.get_value_opt(z)
+        wind_velocity = Vector([wind_velocity_x, wind_velocity_y, 0])
+        free_stream_speed = abs((wind_velocity - Vector(v)))
+        speed_of_sound = self.env.speed_of_sound.get_value_opt(z)
+        free_stream_mach = free_stream_speed / speed_of_sound
+
+        if self.rocket.motor.burn_start_time < t < self.rocket.motor.burn_out_time:
+            pressure = self.env.pressure.get_value_opt(z)
+            net_thrust = max(
+                self.rocket.motor.thrust.get_value_opt(t)
+                + self.rocket.motor.pressure_thrust(pressure),
+                0,
+            )
+            drag_coeff = self.rocket.power_on_drag.get_value_opt(free_stream_mach)
+        else:
+            net_thrust = 0
+            drag_coeff = self.rocket.power_off_drag.get_value_opt(free_stream_mach)
+        R3 += -0.5 * rho * (free_stream_speed**2) * self.rocket.area * drag_coeff
+        for air_brakes in self.rocket.air_brakes:
+            if air_brakes.deployment_level > 0:
+                air_brakes_cd = air_brakes.drag_coefficient.get_value_opt(
+                    air_brakes.deployment_level, free_stream_mach
+                )
+                air_brakes_force = (
+                    -0.5
+                    * rho
+                    * (free_stream_speed**2)
+                    * air_brakes.reference_area
+                    * air_brakes_cd
+                )
+                if air_brakes.override_rocket_drag:
+                    R3 = air_brakes_force  # Substitutes rocket drag coefficient
+                else:
+                    R3 += air_brakes_force
+        # Get rocket velocity in body frame
+        velocity_in_body_frame = Kt @ v
+        # Calculate lift and moment for each component of the rocket
+        for aero_surface, _ in self.rocket.aerodynamic_surfaces:
+            # Component cp relative to CDM in body frame
+            comp_cp = self.rocket.surfaces_cp_to_cdm[aero_surface]
+            # Component absolute velocity in body frame
+            comp_vb = velocity_in_body_frame + (w ^ comp_cp)
+            # Wind velocity at component altitude
+            comp_z = z + (K @ comp_cp).z
+            comp_wind_vx = self.env.wind_velocity_x.get_value_opt(comp_z)
+            comp_wind_vy = self.env.wind_velocity_y.get_value_opt(comp_z)
+            # Component freestream velocity in body frame
+            comp_wind_vb = Kt @ Vector([comp_wind_vx, comp_wind_vy, 0])
+            comp_stream_velocity = comp_wind_vb - comp_vb
+            comp_stream_speed = abs(comp_stream_velocity)
+            comp_stream_mach = comp_stream_speed / speed_of_sound
+            # Reynolds at component altitude
+            # TODO: Reynolds is only used in generic surfaces. This calculation
+            # should be moved to the surface class for efficiency
+            comp_reynolds = (
+                self.env.density.get_value_opt(comp_z)
+                * comp_stream_speed
+                * aero_surface.reference_length
+                / self.env.dynamic_viscosity.get_value_opt(comp_z)
+            )
+            # Forces and moments
+            X, Y, Z, M, N, L = aero_surface.compute_forces_and_moments(
+                comp_stream_velocity,
+                comp_stream_speed,
+                comp_stream_mach,
+                rho,
+                comp_cp,
+                w,
+                comp_reynolds,
+            )
+            R1 += X
+            R2 += Y
+            R3 += Z
+            M1 += M
+            M2 += N
+            M3 += L
+
+        # Off center moment
+        M1 += (
+            self.rocket.cp_eccentricity_y * R3
+            + self.rocket.thrust_eccentricity_y * net_thrust
+        )
+        M2 -= (
+            self.rocket.cp_eccentricity_x * R3
+            + self.rocket.thrust_eccentricity_x * net_thrust
+        )
+        M3 += self.rocket.cp_eccentricity_x * R2 - self.rocket.cp_eccentricity_y * R1
+
+        weight_in_body_frame = Kt @ Vector(
+            [0, 0, -total_mass * self.env.gravity.get_value_opt(z)]
+        )
+
+        T00 = total_mass * r_CM
+        T03 = 2 * total_mass_dot * (r_NOZ - r_CM) - 2 * total_mass * r_CM_dot
+        T04 = (
+            Vector([0, 0, net_thrust])
+            - total_mass * r_CM_ddot
+            - 2 * total_mass_dot * r_CM_dot
+            + total_mass_ddot * (r_NOZ - r_CM)
+        )
+        T05 = total_mass_dot * S_nozzle - I_dot
+
+        T20 = (
+            ((w ^ T00) ^ w)
+            + (w ^ T03)
+            + T04
+            + weight_in_body_frame
+            + Vector([R1, R2, R3])
+        )
+
+        T21 = (
+            ((inertia_tensor @ w) ^ w)
+            + T05 @ w
+            - (weight_in_body_frame ^ r_CM)
+            + Vector([M1, M2, M3])
+        )
+
+        # Angular velocity derivative
+        w_dot = I_CM.inverse @ (T21 + (T20 ^ r_CM))
+
+        # Velocity vector derivative + Coriolis acceleration
+        v_dot = K @ (T20 / total_mass - (r_CM ^ w_dot))
+
+        # Euler parameters derivative
+        e_dot = [
+            0.5 * (-omega1 * e1 - omega2 * e2 - omega3 * e3),
+            0.5 * (omega1 * e0 + omega3 * e2 - omega2 * e3),
+            0.5 * (omega2 * e0 - omega3 * e1 + omega1 * e3),
+            0.5 * (omega3 * e0 + omega2 * e1 - omega1 * e2),
+        ]
+
+        # Position vector derivative
+        r_dot = [vx, vy, vz]
+
+        # Create u_dot
+        u_dot = [*r_dot, *v_dot, *e_dot, *w_dot]
+
+        if post_processing:
+            self.__post_processed_variables.append(
+                [t, *v_dot, *w_dot, R1, R2, R3, M1, M2, M3, net_thrust]
+            )
+
+        return u_dot
+
     def u_dot_parachute(self, t, u, post_processing=False):
         """Calculates derivative of u state vector with respect to time
         when rocket is flying under parachute. A 3 DOF approximation is
@@ -2001,6 +2215,12 @@ def u_dot_parachute(self, t, u, post_processing=False):
         ay = Dy / (mp + ma)
         az = (Dz - 9.8 * mp) / (mp + ma)
 
+        # Add coriolis acceleration
+        _, w_earth_y, w_earth_z = self.env.earth_rotation_vector
+        ax -= 2 * (vz * w_earth_y - vy * w_earth_z)
+        ay -= 2 * (vx * w_earth_z)
+        az -= 2 * (-vx * w_earth_y)
+
         if post_processing:
             self.__post_processed_variables.append(
                 [t, ax, ay, az, 0, 0, 0, Dx, Dy, Dz, 0, 0, 0, 0]