diff --git a/rocketpy/Flight.py b/rocketpy/Flight.py index 49a473e33..8040e7304 100644 --- a/rocketpy/Flight.py +++ b/rocketpy/Flight.py @@ -539,15 +539,18 @@ def __init__( heading : int, float, optional Heading angle relative to north given in degrees. Default is 90, which points in the x direction. - initialSolution : array, optional + initialSolution : array, Flight, optional Initial solution array to be used. Format is - initialSolution = [self.tInitial, - xInit, yInit, zInit, - vxInit, vyInit, vzInit, - e0Init, e1Init, e2Init, e3Init, - w1Init, w2Init, w3Init]. - If None, the initial solution will start with all null values, - except for the euler parameters which will be calculated based + initialSolution = [ + self.tInitial, + xInit, yInit, zInit, + vxInit, vyInit, vzInit, + e0Init, e1Init, e2Init, e3Init, + w1Init, w2Init, w3Init + ]. + If a Flight object is used, the last state vector will be used as + initial solution. If None, the initial solution will start with + all null values, except for the euler parameters which will be calculated based on given values of inclination and heading. Default is None. terminateOnApogee : boolean, optional Whether to terminate simulation when rocket reaches apogee. @@ -613,67 +616,8 @@ def __init__( # Flight initialization self.__init_post_process_variables() - # Initialize solution monitors - self.outOfRailTime = 0 - self.outOfRailState = np.array([0]) - self.outOfRailVelocity = 0 - self.apogeeState = np.array([0]) - self.apogeeTime = 0 - self.apogeeX = 0 - self.apogeeY = 0 - self.apogee = 0 - self.xImpact = 0 - self.yImpact = 0 - self.impactVelocity = 0 - self.impactState = np.array([0]) - self.parachuteEvents = [] - self.postProcessed = False - self.latitude = 0 # Function(0) - self.longitude = 0 # Function(0) - # Initialize solver monitors - self.functionEvaluations = [] - self.functionEvaluationsPerTimeStep = [] - self.timeSteps = [] - # Initialize solution state - self.solution = [] - if self.initialSolution is None: - # Initialize time and state variables - self.tInitial = 0 - xInit, yInit, zInit = 0, 0, self.env.elevation - vxInit, vyInit, vzInit = 0, 0, 0 - w1Init, w2Init, w3Init = 0, 0, 0 - # Initialize attitude - psiInit = -heading * (np.pi / 180) # Precession / Heading Angle - thetaInit = (inclination - 90) * (np.pi / 180) # Nutation Angle - e0Init = np.cos(psiInit / 2) * np.cos(thetaInit / 2) - e1Init = np.cos(psiInit / 2) * np.sin(thetaInit / 2) - e2Init = np.sin(psiInit / 2) * np.sin(thetaInit / 2) - e3Init = np.sin(psiInit / 2) * np.cos(thetaInit / 2) - # Store initial conditions - self.initialSolution = [ - self.tInitial, - xInit, - yInit, - zInit, - vxInit, - vyInit, - vzInit, - e0Init, - e1Init, - e2Init, - e3Init, - w1Init, - w2Init, - w3Init, - ] - # Set initial derivative for rail phase - self.initialDerivative = self.uDotRail1 - else: - # Initial solution given, ignore rail phase - # TODO: Check if rocket is actually out of rail. Otherwise, start at rail - self.outOfRailState = self.initialSolution[1:] - self.outOfRailTime = self.initialSolution[0] - self.initialDerivative = self.uDot + self.__init_solution_monitors() + self.__init_flight_state() self.tInitial = self.initialSolution[0] self.solution.append(self.initialSolution) @@ -1191,6 +1135,85 @@ def __init_post_process_variables(self): self.difference = Function(0) self.safetyFactor = Function(0) + def __init_solution_monitors(self): + """Initialize all variables related to solution monitoring.""" + # Initialize trajectory monitors + self.outOfRailTime = 0 + self.outOfRailState = np.array([0]) + self.outOfRailVelocity = 0 + self.apogeeState = np.array([0]) + self.apogeeTime = 0 + self.apogeeX = 0 + self.apogeeY = 0 + self.apogee = 0 + self.xImpact = 0 + self.yImpact = 0 + self.impactVelocity = 0 + self.impactState = np.array([0]) + self.parachuteEvents = [] + self.postProcessed = False + self.latitude = 0 # Function(0) + self.longitude = 0 # Function(0) + # Initialize solver monitors + self.functionEvaluations = [] + self.functionEvaluationsPerTimeStep = [] + self.timeSteps = [] + # Initialize solution state + self.solution = [] + + # TODO: Need unit tests + def __init_flight_state(self): + """Initialize flight state.""" + if self.initialSolution is None: + # Initialize time and state variables based on heading and inclination + self.tInitial = 0 + xInit, yInit, zInit = 0, 0, self.env.elevation + vxInit, vyInit, vzInit = 0, 0, 0 + w1Init, w2Init, w3Init = 0, 0, 0 + # Initialize attitude + psiInit = -self.heading * (np.pi / 180) # Precession / self. Angle + thetaInit = (self.inclination - 90) * (np.pi / 180) # Nutation Angle + e0Init = np.cos(psiInit / 2) * np.cos(thetaInit / 2) + e1Init = np.cos(psiInit / 2) * np.sin(thetaInit / 2) + e2Init = np.sin(psiInit / 2) * np.sin(thetaInit / 2) + e3Init = np.sin(psiInit / 2) * np.cos(thetaInit / 2) + # Store initial conditions + self.initialSolution = [ + self.tInitial, + xInit, + yInit, + zInit, + vxInit, + vyInit, + vzInit, + e0Init, + e1Init, + e2Init, + e3Init, + w1Init, + w2Init, + w3Init, + ] + # Set initial derivative for rail phase + self.initialDerivative = self.uDotRail1 + # TODO: Need unit tests + elif isinstance(self.initialSolution, Flight): + # TODO: Add optional argument to specify wether to merge/concatenate flight or not + # Initialize time and state variables based on last solution of + # previous flight + self.initialSolution = self.initialSolution.solution[-1] + # Set unused monitors + self.outOfRailState = self.initialSolution[1:] + self.outOfRailTime = self.initialSolution[0] + # Set initial derivative for 6-DOF flight phase + self.initialDerivative = self.uDot + else: + # Initial solution given, ignore rail phase + # TODO: Check if rocket is actually out of rail. Otherwise, start at rail + self.outOfRailState = self.initialSolution[1:] + self.outOfRailTime = self.initialSolution[0] + self.initialDerivative = self.uDot + def uDotRail1(self, t, u, postProcessing=False): """Calculates derivative of u state vector with respect to time when rocket is flying in 1 DOF motion in the rail.