class InternalCode: def __init__(self, simulator): print("initializing simulation at %s" % time.time()) self.simulator = simulator if use_planned_mission: mission = MissionPlanner() self.mission_setup(mission) self.controller = PositionController(self.simulator.drone, mission.commands, True) def mission_setup(self, planner): import quadrotor.command as cmd # guess what this flightplan will do :) commands = [ cmd.down(0.5), cmd.right(1), cmd.turn_left(45), cmd.forward(math.sqrt(2)), cmd.turn_right(45), cmd.right(1), cmd.turn_left(45), cmd.forward(math.sqrt(0.5)), cmd.turn_left(90), cmd.forward(math.sqrt(0.5)), cmd.turn_left(45), cmd.forward(1), cmd.turn_right(45), cmd.backward(math.sqrt(2)), cmd.turn_left(45), cmd.forward(1), ] planner.add_commands(commands) def measurement_callback(self, t, dt, navdata): """ called for each measurement done by the drone """ if use_planned_mission: # apply the computed control to as simulation input lin_vel, yaw_vel = self.controller.compute_input() self.simulator.set_input_world(lin_vel, yaw_vel) # plot navdata in 2d graph: # callback time step: plot.plot("d_time", dt) # onboard speed: plot.plot("v_x", navdata.vx) plot.plot("v_y", navdata.vy) plot.plot("v_z", navdata.vz)
class InternalCode: def __init__(self, simulator): from simulator.controller import MissionPlanner, PositionController self.simulator = simulator mission = MissionPlanner() plan_mission(mission) self.controller = PositionController(self.simulator.drone, mission.commands, True); def measurement_callback(self, t, dt, navdata): lin_vel, yaw_vel = self.controller.compute_input() self.simulator.set_input_world(lin_vel, yaw_vel)
class InternalCode: def __init__(self, simulator): from simulator.controller import MissionPlanner, PositionController self.simulator = simulator mission = MissionPlanner() mission.turn_left(45).forward(2).turn_left(90).forward(2).turn_left(90).forward(2).turn_left(90).forward(4).turn_right(90).forward(2).turn_right(90).forward(2).turn_right(90).forward(2) self.controller = PositionController(self.simulator.drone, mission.commands, False); self.user = UserCode() def measurement_callback(self, t, dt, navdata): self.user.measurement_callback(t, dt, navdata) lin_vel, yaw_vel = self.controller.compute_input() self.simulator.set_input_world(lin_vel, yaw_vel)
class InternalCode: def __init__(self, simulator): from simulator.controller import MissionPlanner, PositionController self.simulator = simulator mission = MissionPlanner() mission.turn_left(45).forward(2).turn_left(90).forward(2).turn_left( 90).forward(2).turn_left(90).forward(4).turn_right(90).forward( 2).turn_right(90).forward(2).turn_right(90).forward(2) self.controller = PositionController(self.simulator.drone, mission.commands, False) self.user = UserCode() def measurement_callback(self, t, dt, navdata): self.user.measurement_callback(t, dt, navdata) lin_vel, yaw_vel = self.controller.compute_input() self.simulator.set_input_world(lin_vel, yaw_vel)