def __init__(self, args):
"""Constructor"""
self.args = args
self.fdm = FGFDMExec(root_dir=args["jsbsim_root"])
self.fdm.load_model("Rascal110-JSBSim")
# settings
self.sim_end_time_s = 120
self.dt = 0.005
self.dt_total_energy = 0.02
self.ic = {
"hgt": 400 * ureg.meter
}
# self.mode = "attitude"
self.mode = "position"
self.noise_enabled = True
self.sigmas = {
"airspeed": 5.0,
"altitude": 2.0,
"speed_body_u": 1.0, # TODO think again about introducing the noise in the body frame
"speed_body_v": 1.0,
"speed_body_w": 1.0,
}
self.parameters = {
"airspeed_trim": 20.0,
"airspeed_min": 7.0,
"airspeed_max": 60.0,
"coordinated_min_speed": 1000.0,
"coordinated_method": 0.0,
"att_tc": 0.5,
"k_p": 0.08,
"k_ff": 0.4,
"k_i": 0.05,
"i_max": 0.4,
"pitch_max_rate_pos": 0.0, # 0: disable
"pitch_max_rate_neg": 0.0, # 0: disable
"pitch_roll_ff": 0.0,
"throttle_default": 0.2,
"mtecs_acc_p": 0.01,
"mtecs_fpa_p": 0.01,
"mtecs_throttle_ff": 0.0,
"mtecs_throttle_p": 0.1,
"mtecs_throttle_i": 0.25,
"mtecs_pitch_ff": 0.0,
"mtecs_pitch_p": 0.1,
"mtecs_pitch_i": 0.03,
"mtecs_airspeed_lowpass_cutoff": 0.1,
"mtecs_airspeed_derivative_lowpass_cutoff": 0.1,
"mtecs_altitude_lowpass_cutoff": 0.1,
"mtecs_flightpathangle_lowpass_cutoff": 0.1,
}
self.control_surface_scaler = 1.0
self.controller = FixedWingController(self.parameters, self.dt_total_energy/self.dt, self.mode)
class Simulator:
"""Simulate mtecs"""
def __init__(self, args):
"""Constructor"""
self.args = args
self.fdm = FGFDMExec(root_dir=args["jsbsim_root"])
self.fdm.load_model("Rascal110-JSBSim")
# settings
self.sim_end_time_s = 120
self.dt = 0.005
self.dt_total_energy = 0.02
self.ic = {
"hgt": 400 * ureg.meter
}
# self.mode = "attitude"
self.mode = "position"
self.noise_enabled = True
self.sigmas = {
"airspeed": 5.0,
"altitude": 2.0,
"speed_body_u": 1.0, # TODO think again about introducing the noise in the body frame
"speed_body_v": 1.0,
"speed_body_w": 1.0,
}
self.parameters = {
"airspeed_trim": 20.0,
"airspeed_min": 7.0,
"airspeed_max": 60.0,
"coordinated_min_speed": 1000.0,
"coordinated_method": 0.0,
"att_tc": 0.5,
"k_p": 0.08,
"k_ff": 0.4,
"k_i": 0.05,
"i_max": 0.4,
"pitch_max_rate_pos": 0.0, # 0: disable
"pitch_max_rate_neg": 0.0, # 0: disable
"pitch_roll_ff": 0.0,
"throttle_default": 0.2,
"mtecs_acc_p": 0.01,
"mtecs_fpa_p": 0.01,
"mtecs_throttle_ff": 0.0,
"mtecs_throttle_p": 0.1,
"mtecs_throttle_i": 0.25,
"mtecs_pitch_ff": 0.0,
"mtecs_pitch_p": 0.1,
"mtecs_pitch_i": 0.03,
"mtecs_airspeed_lowpass_cutoff": 0.1,
"mtecs_airspeed_derivative_lowpass_cutoff": 0.1,
"mtecs_altitude_lowpass_cutoff": 0.1,
"mtecs_flightpathangle_lowpass_cutoff": 0.1,
}
self.control_surface_scaler = 1.0
self.controller = FixedWingController(self.parameters, self.dt_total_energy/self.dt, self.mode)
def init_sim(self):
"""init/reset simulation"""
# init states (dictionary of lists (each list contains a time series of
# a state/value))
self.jsbs_states = {
"ic/gamma-rad": [0],
"position/h-sl-meters": [self.ic["hgt"].magnitude],
"attitude/phi-rad": [0],
"velocities/p-rad_sec": [0],
"attitude/theta-rad": [0],
"velocities/q-rad_sec": [0],
"attitude/psi-rad": [0],
"velocities/r-rad_sec": [0],
"velocities/u-fps": [0],
"velocities/v-fps": [0],
"velocities/w-fps": [0],
"accelerations/udot-ft_sec2": [0],
"accelerations/vdot-ft_sec2": [0],
"accelerations/wdot-ft_sec2": [0],
"velocities/vt-fps": [ureg.Quantity(self.parameters["airspeed_trim"], "m/s").to(ureg["ft/s"]).magnitude], # XXX is this true airspeed, check...
"flight-path/gamma-rad": [0],
"propulsion/engine/thrust-lbs": [0]
}
self.jsbs_ic = {
"ic/h-sl-ft": [self.ic["hgt"].to(ureg.foot).magnitude],
"ic/vt-kts": [ureg.Quantity(self.parameters["airspeed_trim"], "m/s").to(ureg["kt"]).magnitude], # XXX is this true airspeed, check...
"ic/gamma-rad": [0],
}
self.jsbs_inputs = {
"fcs/aileron-cmd-norm": [0],
"fcs/elevator-cmd-norm": [0],
"fcs/rudder-cmd-norm": [0],
"fcs/throttle-cmd-norm": [0.0],
"fcs/mixture-cmd-norm": [0.87],
"propulsion/magneto_cmd": [3],
"propulsion/starter_cmd": [1]
}
self.sim_states = {
"t": [0.0],
}
self.setpoints = {}
self.update_setpoints(0)
self.noisy_states = {}
self.update_noisy_states(self.get_state())
self.control_data_log = {}
# set initial conditions and trim
for k, v in self.jsbs_ic.items():
self.fdm.set_property_value(k, v[0])
self.fdm.set_dt(self.dt)
self.fdm.reset_to_initial_conditions(0)
self.fdm.do_trim(0)
def get_state(self):
"""
creates a dictionary of the current state, to be used as control input
"""
x = {}
x["t"] = self.sim_states["t"][-1]
x["roll"] = self.jsbs_states["attitude/phi-rad"][-1]
x["roll_rate"] = self.jsbs_states["velocities/p-rad_sec"][-1]
x["pitch"] = self.jsbs_states["attitude/theta-rad"][-1]
x["pitch_rate"] = self.jsbs_states["velocities/q-rad_sec"][-1]
x["yaw"] = self.jsbs_states["attitude/psi-rad"][-1]
x["yaw_rate"] = self.jsbs_states["velocities/r-rad_sec"][-1]
x["speed_body_u"] = ureg.Quantity(
self.jsbs_states["velocities/u-fps"][-1],
"ft/s").to(ureg["m/s"]).magnitude
x["speed_body_v"] = ureg.Quantity(
self.jsbs_states["velocities/v-fps"][-1],
"ft/s").to(ureg["m/s"]).magnitude
x["speed_body_w"] = ureg.Quantity(
self.jsbs_states["velocities/w-fps"][-1],
"ft/s").to(ureg["m/s"]).magnitude
x["acc_body_x"] = self.jsbs_states["accelerations/udot-ft_sec2"][-1]
x["acc_body_y"] = self.jsbs_states["accelerations/vdot-ft_sec2"][-1]
x["acc_body_z"] = self.jsbs_states["accelerations/wdot-ft_sec2"][-1]
x["airspeed"] = ureg.Quantity(
self.jsbs_states["velocities/vt-fps"][-1],
"ft/s").to(ureg["m/s"]).magnitude
x["altitude"] = self.jsbs_states["position/h-sl-meters"][-1]
x["flightpathangle"] = self.jsbs_states["flight-path/gamma-rad"][-1]
# additonal/secondary data that is not a state in the physical sense but is needed
# by the controller and describes the aircraft state as well:
if x["airspeed"] > self.parameters["airspeed_min"]:
x["scaler"] = self.parameters["airspeed_trim"] / x["airspeed"]
else:
x["scaler"] = self.parameters["airspeed_trim"] \
/ self.parameters["airspeed_min"]
x["lock_integrator"] = False
return x
def calc_setpoints(self, time):
"""Generate setpoint to be used in the controller"""
r = {}
r["roll"] = 0.0
r["pitch"] = 0.0
r["yaw"] = 0.0
r["roll_rate"] = 0.0
r["pitch_rate"] = 0.0
r["yaw_rate"] = 0.0
r["altitude"] = self.ic["hgt"].magnitude if time < 20 else self.ic["hgt"].magnitude + 10
# r["altitude"] = self.ic["hgt"].magnitude
r["velocity"] = self.parameters["airspeed_trim"]
return r
def update_setpoints(self, time):
"""updates the setpoint"""
sp = self.calc_setpoints(time)
for k, v in sp.items():
self.setpoints.setdefault(k,[]).append(v)
def step(self):
"""Perform one simulation step
implementation is accoding to FGFDMExec's own simulate but we don't
want to move the parameters in and out manually
"""
# control
# self.jsbs_inputs["fcs/elevator-cmd-norm"].append(0.01 * (400 -
# self.jsbs_states["position/h-sl-meters"][-1]))
self.update_setpoints(self.fdm.get_sim_time())
state = self.get_state()
self.update_noisy_states(state)
state_estimate = self.apply_noise(state) # estimate is simulated as true state plus gaussian noise
u, control_data = self.controller.control(state=state_estimate,
setpoint={k: v[-1] for k, v in self.setpoints.items()},
parameters = self.parameters)
self.jsbs_inputs["fcs/aileron-cmd-norm"].append(u[0] * self.control_surface_scaler)
self.jsbs_inputs["fcs/elevator-cmd-norm"].append(-u[1] * self.control_surface_scaler)
self.jsbs_inputs["fcs/rudder-cmd-norm"].append(u[2] * self.control_surface_scaler)
self.jsbs_inputs["fcs/throttle-cmd-norm"].append(u[3])
# copy control data to for later plotting
for k,v in control_data.items():
self.control_data_log.setdefault(k, [0.0]).append(v)
# pass control resultto jsbsim
for k, v in self.jsbs_inputs.items():
self.fdm.set_property_value(k, v[-1])
# do one step in jsbsim
self.fdm.run()
# read out result from jsbsim
for k, v in self.jsbs_states.items():
self.jsbs_states[k].append(self.fdm.get_property_value(k))
return self.fdm.get_sim_time()
def output_results(self):
"""Generate a report of the simulation"""
rg = HtmlReportGenerator(self.args)
add_plots(self, rg) # change add_plots to show different plots!
rg.variables.update(analyse(self))
rg.generate()
rg.save()
print("Report saved to {0}".format(self.args["filename_out"]))
def apply_noise(self, state):
"""replaces entries in state with the noisy data (for states for which noise data exists)"""
state_estimate = copy.copy(state)
for k,v in self.noisy_states.items():
state_estimate[k] = self.noisy_states[k][-1]
return state_estimate
def update_noisy_states(self, state):
"""caclculate noisy version of state for which noise data exists"""
for k, v in self.sigmas.items():
self.noisy_states.setdefault(k,[]).append(state[k] + random.gauss(0,v))
def main(self):
"""main method of the simulator"""
self.init_sim()
# run simulation
bar = pyprind.ProgBar(self.sim_end_time_s)
time_last_bar_update = 0
while self.sim_states["t"][-1] < self.sim_end_time_s:
self.sim_states["t"].append(self.step())
if self.sim_states["t"][-1] >= time_last_bar_update + 1: # throttle update of progress bar
bar.update()
time_last_bar_update = self.sim_states["t"][-1]
bar.update()
self.output_results()
