class Calculator:
'''
In charge of the calculation on ATS software
'''
def __init__(self, rocket):
self.data_buffer = CircularQueue(10)
self.alt_buffer = CircularQueue(10)
self.v_buffer = CircularQueue(10)
self.rocket = rocket
self.base_alt = 0
def setBaseAlt(self, pressure):
'''
Sets the base altitude based on ground air pressure
We need to offset the altitude calculated from the US Standard Atmosphere model since it assumes
ground pressure to be 1013.25 hPa at mean sea level, not at launch site sea level
'''
self.base_alt = pressure2alt(pressure)
def compute(self, data):
'''
computes and stores the altitude and velocity at the new data point
'''
self.data_buffer.add(data)
self.alt_buffer.add(pressure2alt(data.pressure))
if (self.v_buffer.size() == 0):
self.v_buffer.add(0)
else:
curr_alt = self.alt_buffer.get_last()
prev_alt = self.alt_buffer.get_second_last()
curr_time = data.time
prev_time = self.data_buffer.get_second_last().time
self.v_buffer.add(fda(prev_alt, curr_alt, prev_time, curr_time))
def ode(self, g, A, Cd, rho, m, v):
'''
returns the acceleration of the launch vehicle during the coast state
the launch vehicle's equation of motion is simplified to be only the sum of weight and drag
'''
return -g - ((A * Cd * rho * (v**2) * 0.5) / m)
def predict(self):
'''
returns the predicted apogee altitude
average value of velocity in the buffer is used as the initial value of velocity
average value of altitude in the buffer is used as the initial value of altitude
air density is initialized using the initial altitude value
the predict function utilizes Runge-Kutta method to predict the apogee altitude
'''
v_n = self.v_buffer.average()
alt_n = self.alt_buffer.average()
rho_n = alt2rho(alt_n)
while (v_n > 0):
k1 = self.ode(STANDARD_GRAVITY, self.rocket.surface_area,
self.rocket.drag_coeff, rho_n, self.rocket.mass, v_n)
k2 = self.ode(STANDARD_GRAVITY, self.rocket.surface_area,
self.rocket.drag_coeff, rho_n, self.rocket.mass,
v_n + k1 * 0.5)
k3 = self.ode(STANDARD_GRAVITY, self.rocket.surface_area,
self.rocket.drag_coeff, rho_n, self.rocket.mass,
v_n + k2 * 0.5)
k4 = self.ode(STANDARD_GRAVITY, self.rocket.surface_area,
self.rocket.drag_coeff, rho_n, self.rocket.mass,
v_n + k3)
v_n = v_n + STEP_SIZE * (k1 + 2.0 * k2 + 2.0 * k3 + k4) / 6.0
alt_n = alt_n + STEP_SIZE * v_n
rho_n = alt2rho(alt_n + self.base_alt)
return alt_n
def v_current(self):
'''
returns the current velocity of the launch vehicle as an average of the most recent velocity values
'''
return self.v_buffer.average()
def run(self):
prelaunch_buffer = CircularQueue(PRELAUNCH_BUFFER_SIZE)
print("moving to prelaunch state")
while (self.rocket.state != RocketState.GROUND):
data = self.sensor.getData()
if (self.rocket.state == RocketState.PRELAUNCH):
# PRELAUNCH state
prelaunch_buffer.add(data)
# Checking if the launch vehicle took off
if data.total_accel() > TAKEOFF_DETECTION_THRESHOLD:
arr = prelaunch_buffer.toArray()
self.start_time = arr[
0].time # setting start time of the launch vehicle
arr[-1].event = Event.TAKE_OFF # the latest data will be marked as TAKE_OFF
total = 0.0
for point in arr:
point.normalize(self.start_time)
self.logger.write(point)
total += point.pressure
self.rocket.state = RocketState.MOTOR_ASCENT
self.calculator.setBaseAlt(
total / PRELAUNCH_BUFFER_SIZE
) # base altitude is based on average pressure
print("moving to motor ascent state")
elif (self.rocket.state == RocketState.MOTOR_ASCENT):
# MOTOR_ASCENT state
data.normalize(self.start_time)
self.calculator.compute(data)
# Checking if the motor burned up by checking the duration since take off
if data.time >= self.rocket.motor_burnout_t + 1:
self.rocket.state = RocketState.COAST
data.event = Event.MOTOR_BURNOUT
print("moving to coast state")
self.base_time = data.time
self.logger.write(data)
elif (self.rocket.state == RocketState.COAST):
# COAST state
data.normalize(self.start_time)
self.calculator.compute(data)
if self.ats_state == 0 and (data.time - self.base_time >= 1):
# if been in closed state for more than 1 second, start opening the flaps
self.ats_state = 2
self.base_time = data.time
self.actuator.actuate()
data.command = "ACTUATE"
elif self.ats_state == 1 and (data.time - self.base_time >= 1):
self.ats_state = -1
self.base_time = data.time
self.actuator.retract()
data.command = "RETRACT"
elif self.ats_state == 2:
if (data.time - self.base_time < 2):
self.actuator.actuate()
else:
self.ats_state = 1
self.base_time = data.time
data.command = "COMPLETED ACTUATION"
else:
if (data.time - self.base_time < 2):
self.actuator.retract()
else:
self.ats_state = 0
self.base_time = data.time
data.command = "COMPLETED RETRACTION"
# if self.calculator.predict() > self.rocket.target_alt:
# self.actuator.actuate()
# else:
# self.actuator.retract()
print("Current velocity: {}".format(
self.calculator.v_current()))
# Checking if velocity of the launch vehicle is negative
if self.calculator.v_current() < 0:
data.event = Event.APOGEE
self.actuator.retract()
self.rocket.state = RocketState.DESCENT
self.apogee_time = data.time
print("moving to descent state")
self.logger.write(data)
elif (self.rocket.state == RocketState.DESCENT):
# DESCENT state
data.normalize(self.start_time)
self.calculator.compute(data)
print("Current velocity: {}".format(
self.calculator.v_current()))
# Detect landing when velocity is be greated than -2 m/s and it has been at least 5 seconds after apogee
if self.calculator.v_current(
) > -2 and data.time - self.apogee_time > 5:
data.event = Event.TOUCH_DOWN
self.rocket.state = RocketState.GROUND
print("moving to ground state")
self.logger.write(data)
# GROUND state
for _ in range(5):
data = self.sensor.getData()
data.normalize(self.start_time)
self.logger.write(data)
self.logger.purge()
self.shutdown()
