Exemple #1
0
class ControlsFlyer(UnityDrone):
    def __init__(self, connection):
        super().__init__(connection)
        self.controller = NonlinearController(
            z_k_p=18,  # kpPosZ
            z_k_d=6.8,  # kpVelZ
            x_k_p=2.6,  # kpPosXY
            x_k_d=1.7,  # kpVelXY
            y_k_p=2.6,  # kpPosXY
            y_k_d=1.7,  # kpVelXY
            k_p_roll=5.4,  # kpBank
            k_p_pitch=5.4,  # kpBank
            k_p_yaw=1,  # kpYaw
            k_p_p=12,  # kpPQR[0]
            k_p_q=12,  # kpPQR[1]
            k_p_r=4.5  # kpPQR[2]
        )
        self.target_position = np.array([0.0, 0.0, 0.0])
        self.all_waypoints = []
        self.in_mission = True
        self.check_state = {}

        # initial state
        self.flight_state = States.MANUAL

        # register all your callbacks here
        self.register_callback(MsgID.LOCAL_POSITION,
                               self.local_position_callback)
        self.register_callback(MsgID.LOCAL_VELOCITY, self.velocity_callback)
        self.register_callback(MsgID.STATE, self.state_callback)

        self.register_callback(MsgID.ATTITUDE, self.attitude_callback)
        self.register_callback(MsgID.RAW_GYROSCOPE, self.gyro_callback)

    def position_controller(self):
        (self.local_position_target, self.local_velocity_target,
         yaw_cmd) = self.controller.trajectory_control(
             self.position_trajectory, self.yaw_trajectory,
             self.time_trajectory, time.time())
        self.attitude_target = np.array((0.0, 0.0, yaw_cmd))
        acceleration_cmd = self.controller.lateral_position_control(
            self.local_position_target[0:2], self.local_velocity_target[0:2],
            self.local_position[0:2], self.local_velocity[0:2])
        self.local_acceleration_target = np.array(
            [acceleration_cmd[0], acceleration_cmd[1], 0.0])

    def attitude_controller(self):
        self.thrust_cmd = self.controller.altitude_control(
            -self.local_position_target[2], -self.local_velocity_target[2],
            -self.local_position[2], -self.local_velocity[2], self.attitude,
            9.81)
        roll_pitch_rate_cmd = self.controller.roll_pitch_controller(
            self.local_acceleration_target[0:2], self.attitude,
            self.thrust_cmd)
        yawrate_cmd = self.controller.yaw_control(self.attitude_target[2],
                                                  self.attitude[2])
        self.body_rate_target = np.array(
            [roll_pitch_rate_cmd[0], roll_pitch_rate_cmd[1], yawrate_cmd])

    def bodyrate_controller(self):
        moment_cmd = self.controller.body_rate_control(self.body_rate_target,
                                                       self.gyro_raw)
        self.cmd_moment(moment_cmd[0], moment_cmd[1], moment_cmd[2],
                        self.thrust_cmd)

    def attitude_callback(self):
        if self.flight_state == States.WAYPOINT:
            self.attitude_controller()

    def gyro_callback(self):
        if self.flight_state == States.WAYPOINT:
            self.bodyrate_controller()

    def local_position_callback(self):
        if self.flight_state == States.MANUAL:
            self.takeoff_transition()
        if self.flight_state == States.TAKEOFF:
            if -1.0 * self.local_position[2] > 0.95 * self.target_position[2]:
                # self.all_waypoints = self.calculate_box()
                (self.position_trajectory, self.time_trajectory,
                 self.yaw_trajectory) = self.load_test_trajectory(time_mult=1)
                self.all_waypoints = self.position_trajectory.copy()
                self.waypoint_number = -1
                self.waypoint_transition()
        elif self.flight_state == States.WAYPOINT:
            if time.time() > self.time_trajectory[self.waypoint_number]:
                if len(self.all_waypoints) > 0:
                    self.waypoint_transition()
                else:
                    if np.linalg.norm(self.local_velocity[0:2]) < 1.0:
                        self.landing_transition()

    def velocity_callback(self):
        if self.flight_state == States.LANDING:
            if abs(self.local_position[2] < 0.01):
                self.manual_transition()
        if self.flight_state == States.WAYPOINT:
            self.position_controller()

    def state_callback(self):
        if self.in_mission:
            if self.flight_state == States.MANUAL:
                self.arming_transition()
            elif self.flight_state == States.ARMING:
                if self.armed:
                    self.takeoff_transition()
            elif self.flight_state == States.DISARMING:
                if ~self.armed & ~self.guided:
                    self.manual_transition()

    def calculate_box(self):
        cp = self.local_position
        cp[2] = 0
        local_waypoints = [
            cp + [0.2, 0.0, 0.3], cp + [0.2, 0.2, 0.3], cp + [0.0, 0.2, 0.3],
            cp + [0.0, 0.0, 0.3]
        ]
        return local_waypoints

    def arming_transition(self):
        print("arming transition")
        self.take_control()
        self.arm()
        # set the current location to be the home position
        self.set_home_position(self.global_position[0],
                               self.global_position[1],
                               self.global_position[2])

        self.flight_state = States.ARMING

    def takeoff_transition(self):
        print("takeoff transition")
        target_altitude = 3.0
        self.target_position[2] = target_altitude
        self.takeoff(target_altitude)
        self.flight_state = States.TAKEOFF

    def waypoint_transition(self):
        print("waypoint transition")
        self.waypoint_number = self.waypoint_number + 1
        self.target_position = self.all_waypoints.pop(0)
        print('target position', self.target_position)
        # self.local_position_target = np.array([0.0, 0.0, -10.0])
        self.local_position_target = np.array(
            (self.target_position[0], self.target_position[1],
             self.target_position[2]))
        # self.local_position_target = np.array([0.0, 0.0, -3.0])

        self.flight_state = States.WAYPOINT

    def landing_transition(self):
        print("landing transition")
        self.land()
        self.flight_state = States.LANDING

    def disarming_transition(self):
        print("disarm transition")
        self.disarm()
        self.release_control()
        self.flight_state = States.DISARMING

    def manual_transition(self):
        print("manual transition")
        self.stop()
        self.in_mission = False
        self.flight_state = States.MANUAL

    def start(self):
        self.start_log("Logs", "NavLog.txt")
        # self.connect()

        print("starting connection")
        # self.connection.start()

        super().start()

        # Only required if they do threaded
        # while self.in_mission:
        #    pass

        self.stop_log()
class BackyardFlyer(Drone):
    def __init__(self, connection):
        super().__init__(connection)
        self.target_position = np.array([0.0, 0.0, 0.0])
        self.all_waypoints = []
        self.in_mission = True
        self.check_state = {}

        self.controller = NonlinearController()

        # initial state
        self.flight_state = States.MANUAL

        # register all your callbacks here
        self.register_callback(MsgID.LOCAL_POSITION,
                               self.local_position_callback)
        self.register_callback(MsgID.LOCAL_VELOCITY, self.velocity_callback)
        self.register_callback(MsgID.STATE, self.state_callback)

        self.register_callback(MsgID.ATTITUDE, self.attitude_callback)
        self.register_callback(MsgID.RAW_GYROSCOPE, self.gyro_callback)

    def attitude_callback(self):
        if self.flight_state == States.WAYPOINT:
            self.attitude_controller()

    def gyro_callback(self):
        if self.flight_state == States.WAYPOINT:
            self.bodyrate_controller()

    def local_position_callback(self):
        if self.flight_state == States.TAKEOFF:
            if -1.0 * self.local_position[2] > 0.95 * self.target_position[2]:
                #self.all_waypoints = self.calculate_box()
                (self.position_trajectory, self.time_trajectory,
                 self.yaw_trajectory) = self.load_test_trajectory(
                     time_mult=0.5)
                self.all_waypoints = self.position_trajectory.copy()
                self.waypoint_number = -1
                self.waypoint_transition()
        elif self.flight_state == States.WAYPOINT:
            #if np.linalg.norm(self.target_position[0:2] - self.local_position[0:2]) < 1.0:
            if time.time() > self.time_trajectory[self.waypoint_number]:
                if len(self.all_waypoints) > 0:
                    self.waypoint_transition()
                else:
                    if np.linalg.norm(self.local_velocity[0:2]) < 1.0:
                        self.landing_transition()

    def velocity_callback(self):
        if self.flight_state == States.LANDING:
            if self.global_position[2] - self.global_home[2] < 0.1:
                if abs(self.local_position[2]) < 0.01:
                    self.disarming_transition()
        if self.flight_state == States.WAYPOINT:
            self.position_controller()

    def state_callback(self):
        if self.in_mission:
            if self.flight_state == States.MANUAL:
                self.arming_transition()
            elif self.flight_state == States.ARMING:
                if self.armed:
                    self.takeoff_transition()
            elif self.flight_state == States.DISARMING:
                if ~self.armed & ~self.guided:
                    self.manual_transition()

    def calculate_box(self):
        print("Setting Home")
        local_waypoints = [[10.0, 0.0, 3.0], [10.0, 10.0, 3.0],
                           [0.0, 10.0, 3.0], [0.0, 0.0, 3.0]]
        return local_waypoints

    def arming_transition(self):
        print("arming transition")
        self.take_control()
        self.arm()
        self.set_home_position(
            self.global_position[0], self.global_position[1],
            self.global_position[2]
        )  # set the current location to be the home position

        self.flight_state = States.ARMING

    def takeoff_transition(self):
        print("takeoff transition")
        # self.global_home = np.copy(self.global_position)  # can't write to this variable!
        target_altitude = 3.0
        self.target_position[2] = target_altitude
        self.takeoff(target_altitude)
        self.flight_state = States.TAKEOFF

    def waypoint_transition(self):
        print("waypoint transition")
        self.target_position = self.all_waypoints.pop(0)
        print('target position', self.target_position)
        #self.cmd_position(self.target_position[0], self.target_position[1], self.target_position[2], 0.0)
        self.local_position_target = np.array(
            (self.target_position[0], self.target_position[1],
             self.target_position[2]))
        self.flight_state = States.WAYPOINT
        self.waypoint_number = self.waypoint_number + 1

    def landing_transition(self):
        print("landing transition")
        self.land()
        self.flight_state = States.LANDING

    def disarming_transition(self):
        print("disarm transition")
        self.disarm()
        self.release_control()
        self.flight_state = States.DISARMING

    def manual_transition(self):
        print("manual transition")
        self.stop()
        self.in_mission = False
        self.flight_state = States.MANUAL

    def start(self):
        self.start_log("Logs", "NavLog.txt")
        # self.connect()

        print("starting connection")
        # self.connection.start()

        super().start()

        # Only required if they do threaded
        # while self.in_mission:
        #    pass

        self.stop_log()

    def position_controller(self):
        """Sets the local acceleration target using the local position and local velocity"""

        (self.local_position_target, self.local_velocity_target,
         yaw_cmd) = self.controller.trajectory_control(
             self.position_trajectory, self.yaw_trajectory,
             self.time_trajectory, time.time())
        self.attitude_target = np.array((0.0, 0.0, yaw_cmd))

        acceleration_cmd = self.controller.lateral_position_control(
            self.local_position_target[0:2], self.local_velocity_target[0:2],
            self.local_position[0:2], self.local_velocity[0:2])
        self.local_acceleration_target = np.array(
            [acceleration_cmd[0], acceleration_cmd[1], 0.0])

    def attitude_controller(self):
        """Sets the body rate target using the acceleration target and attitude"""
        self.thrust_cmd = self.controller.altitude_control(
            -self.local_position_target[2], -self.local_velocity_target[2],
            -self.local_position[2], -self.local_velocity[2], self.attitude,
            9.81)
        roll_pitch_rate_cmd = self.controller.roll_pitch_controller(
            self.local_acceleration_target[0:2], self.attitude,
            self.thrust_cmd)
        yawrate_cmd = self.controller.yaw_control(self.attitude_target[2],
                                                  self.attitude[2])
        self.body_rate_target = np.array(
            [roll_pitch_rate_cmd[0], roll_pitch_rate_cmd[1], yawrate_cmd])

    def bodyrate_controller(self):
        """Commands a moment to the vehicle using the body rate target and body rates"""
        moment_cmd = self.controller.body_rate_control(self.body_rate_target,
                                                       self.gyro_raw)
        self.cmd_moment(moment_cmd[0], moment_cmd[1], moment_cmd[2],
                        self.thrust_cmd)
class ControlsFlyer(UnityDrone):
    def __init__(self, connection):
        super().__init__(connection)
        self.controller = NonlinearController()
        self.target_position = np.array([0.0, 0.0, 0.0])
        self.all_waypoints = []
        self.in_mission = True
        self.check_state = {}

        # initial state
        self.flight_state = States.MANUAL

        # register all your callbacks here
        self.register_callback(MsgID.LOCAL_POSITION,
                               self.local_position_callback)
        self.register_callback(MsgID.LOCAL_VELOCITY, self.velocity_callback)
        self.register_callback(MsgID.STATE, self.state_callback)

        self.register_callback(MsgID.ATTITUDE, self.attitude_callback)
        self.register_callback(MsgID.RAW_GYROSCOPE, self.gyro_callback)

        # default opens up to http://localhost:8097
        self.v = visdom.Visdom()

    #    # Plot NE
    #     ne = np.array(self.local_position[:2]).reshape(-1, 2)

    #     self.ne_plot = self.v.scatter(ne, opts=dict(
    #         title="Local position (north, east)",
    #         xlabel='North',
    #         ylabel='East'
    #     ))

    #     # Plot D
    #     d = np.array([self.local_position[2]])
    #     self.t = 1
    #     print(d.ndim)
    #     self.d_plot = self.v.line(d, X=np.array([self.t]), opts=dict(
    #         title="Altitude (meters)",
    #         xlabel='Timestep',
    #         ylabel='Down'
    #     ))

    # for plotting realtime NEO data with visdom
    # self.register_callback(MsgID.LOCAL_POSITION, self.update_ne_plot)
    # self.register_callback(MsgID.LOCAL_POSITION, self.update_d_plot)

    # # for plotting realtime NEO data with visdom
    # def update_ne_plot(self):
    #     ne = np.array([self.local_position[0], self.local_position[1]]).reshape(-1, 2)
    #     self.v.scatter(ne, win=self.ne_plot, update='append')

    # # for plotting realtime NEO data with visdom
    # def update_d_plot(self):
    #     d = -np.array([self.local_position[2]])
    #     # update timestep
    #     self.t += 1
    #     self.v.line(d, X=np.array([self.t]), win=self.d_plot, update='append')

    def position_controller(self):
        (self.local_position_target, self.local_velocity_target,
         yaw_cmd) = self.controller.trajectory_control(
             self.position_trajectory, self.yaw_trajectory,
             self.time_trajectory, time.time())
        self.attitude_target = np.array((0.0, 0.0, yaw_cmd))
        acceleration_cmd = self.controller.lateral_position_control(
            self.local_position_target[0:2], self.local_velocity_target[0:2],
            self.local_position[0:2], self.local_velocity[0:2])
        self.local_acceleration_target = np.array(
            [acceleration_cmd[0], acceleration_cmd[1], 0.0])

    def attitude_controller(self):
        self.thrust_cmd = self.controller.altitude_control(
            -self.local_position_target[2], -self.local_velocity_target[2],
            -self.local_position[2], -self.local_velocity[2], self.attitude,
            9.81)
        roll_pitch_rate_cmd = self.controller.roll_pitch_controller(
            self.local_acceleration_target[0:2], self.attitude,
            self.thrust_cmd)
        yawrate_cmd = self.controller.yaw_control(self.attitude_target[2],
                                                  self.attitude[2])
        self.body_rate_target = np.array(
            [roll_pitch_rate_cmd[0], roll_pitch_rate_cmd[1], yawrate_cmd])

    def bodyrate_controller(self):
        moment_cmd = self.controller.body_rate_control(self.body_rate_target,
                                                       self.gyro_raw)
        self.cmd_moment(moment_cmd[0], moment_cmd[1], moment_cmd[2],
                        self.thrust_cmd)

    def attitude_callback(self):
        if self.flight_state == States.WAYPOINT:
            self.attitude_controller()

    def gyro_callback(self):
        if self.flight_state == States.WAYPOINT:
            self.bodyrate_controller()

    def local_position_callback(self):
        if self.flight_state == States.TAKEOFF:
            if -1.0 * self.local_position[2] > 0.95 * self.target_position[2]:
                #self.all_waypoints = self.calculate_box()
                (self.position_trajectory, self.time_trajectory,
                 self.yaw_trajectory) = self.load_test_trajectory(
                     time_mult=0.5)
                self.all_waypoints = self.position_trajectory.copy()
                self.waypoint_number = -1
                self.waypoint_transition()
        elif self.flight_state == States.WAYPOINT:

            if time.time() > self.time_trajectory[self.waypoint_number]:
                if len(self.all_waypoints) > 0:
                    self.waypoint_transition()
                else:
                    if np.linalg.norm(self.local_velocity[0:2]) < 1.0:
                        self.landing_transition()

    def velocity_callback(self):
        if self.flight_state == States.LANDING:
            if self.global_position[2] - self.global_home[2] < 0.1:
                if abs(self.local_position[2]) < 0.01:
                    self.disarming_transition()
        if self.flight_state == States.WAYPOINT:
            self.position_controller()

    def state_callback(self):
        if self.in_mission:
            if self.flight_state == States.MANUAL:
                self.arming_transition()
            elif self.flight_state == States.ARMING:
                if self.armed:
                    self.takeoff_transition()
            elif self.flight_state == States.DISARMING:
                if ~self.armed & ~self.guided:
                    self.manual_transition()

    def calculate_box(self):
        print("Setting Home")
        local_waypoints = [[10.0, 0.0, -3.0], [10.0, 10.0, -3.0],
                           [0.0, 10.0, -3.0], [0.0, 0.0, -3.0]]
        return local_waypoints

    def arming_transition(self):
        print("arming transition")
        self.take_control()
        self.arm()
        # set the current location to be the home position
        self.set_home_position(self.global_position[0],
                               self.global_position[1],
                               self.global_position[2])

        self.flight_state = States.ARMING

    def takeoff_transition(self):
        print("takeoff transition")
        target_altitude = 3.0
        self.target_position[2] = target_altitude
        self.takeoff(target_altitude)
        self.flight_state = States.TAKEOFF

    def waypoint_transition(self):
        #print("waypoint transition")
        self.waypoint_number = self.waypoint_number + 1
        self.target_position = self.all_waypoints.pop(0)
        self.flight_state = States.WAYPOINT

    def landing_transition(self):
        print("landing transition")
        self.land()
        self.flight_state = States.LANDING

    def disarming_transition(self):
        print("disarm transition")
        self.disarm()
        self.release_control()
        self.flight_state = States.DISARMING

    def manual_transition(self):
        print("manual transition")
        self.stop()
        self.in_mission = False
        self.flight_state = States.MANUAL

    def start(self):
        self.start_log("Logs", "NavLog.txt")
        # self.connect()

        print("starting connection")
        # self.connection.start()

        super().start()

        # Only required if they do threaded
        # while self.in_mission:
        #    pass

        self.stop_log()
Exemple #4
0
class ControlsFlyer(UnityDrone):
    def __init__(self, connection):
        super().__init__(connection)
        self.controller = NonlinearController()
        self.target_position = np.array([0.0, 0.0, 0.0])
        self.all_waypoints = []
        self.in_mission = True
        self.check_state = {}

        # initial state
        self.flight_state = States.MANUAL
        self.prev_time = time.time()
        self.att_time = 0.0
        self.pos_time = 0.0
        self.pulse = False

        # register callbacks
        self.register_callback(MsgID.LOCAL_POSITION,
                               self.local_position_callback)
        self.register_callback(MsgID.LOCAL_VELOCITY, self.velocity_callback)
        self.register_callback(MsgID.STATE, self.state_callback)

        self.register_callback(MsgID.ATTITUDE, self.attitude_callback)
        self.register_callback(MsgID.RAW_GYROSCOPE, self.gyro_callback)

    def position_controller(self):
        (self.local_position_target, self.local_velocity_target, yaw_cmd,
         accff) = self.controller.trajectory_control(self.position_trajectory,
                                                     self.yaw_trajectory,
                                                     self.time_trajectory,
                                                     time.time(), True)
        self.attitude_target = np.array((0.0, 0.0, yaw_cmd))
        cur_time = time.time()
        acceleration_cmd = self.controller.lateral_position_control(
            self.local_position_target[0:2], self.local_velocity_target[0:2],
            self.local_position[0:2], self.local_velocity[0:2], accff[0:2],
            cur_time - self.prev_time)
        self.prev_time = cur_time
        self.local_acceleration_target = np.array(
            [acceleration_cmd[0], acceleration_cmd[1], 0.0])

    def attitude_controller(self):
        self.thrust_cmd = self.controller.altitude_control(
            -self.local_position_target[2], -self.local_velocity_target[2],
            -self.local_position[2], -self.local_velocity[2], self.attitude,
            9.81)
        roll_pitch_rate_cmd = self.controller.roll_pitch_controller(
            self.local_acceleration_target[0:2], self.attitude,
            self.thrust_cmd)
        if self.pulse:
            roll_pitch_rate_cmd[0] = 30.0
            roll_pitch_rate_cmd[1] = 0.0
            self.pulse = False
        yawrate_cmd = self.controller.yaw_control(self.attitude_target[2],
                                                  self.attitude[2])
        self.body_rate_target = np.array(
            [roll_pitch_rate_cmd[0], roll_pitch_rate_cmd[1], yawrate_cmd])

    def bodyrate_controller(self):
        moment_cmd = self.controller.body_rate_control(self.body_rate_target,
                                                       self.gyro_raw)
        self.cmd_moment(moment_cmd[0], moment_cmd[1], moment_cmd[2],
                        self.thrust_cmd)

    def attitude_callback(self):  # 40 frames per second
        if time.time() - self.att_time > 0.05:  # pace the calls to 20fps
            if self.flight_state == States.WAYPOINT:
                self.attitude_controller()
                self.att_time = time.time()

    def gyro_callback(self):  # 40 frames per second
        if self.flight_state == States.WAYPOINT:
            self.bodyrate_controller()

    def local_position_callback(self):  # 40 frames per second
        #
        #print(time.time())
        if self.flight_state == States.TAKEOFF:
            if abs(self.local_position[2] +
                   self.target_position[2]) < 0.025 and np.linalg.norm(
                       self.local_velocity[0:2]) < 0.1:
                if TRAJECTORY is 1:
                    (self.position_trajectory, self.time_trajectory,
                     self.yaw_trajectory) = self.calculate_box_trajectory()
                elif TRAJECTORY is 2:
                    (self.position_trajectory, self.time_trajectory,
                     self.yaw_trajectory) = self.calculate_fig8_trajectory()
                elif TRAJECTORY is 3:
                    (self.position_trajectory, self.time_trajectory,
                     self.yaw_trajectory) = self.calculate_hover()
                elif TRAJECTORY is 4:
                    (self.position_trajectory, self.time_trajectory,
                     self.yaw_trajectory
                     ) = self.calculate_diagonal_trajectory()
                elif TRAJECTORY is 5:
                    (self.position_trajectory, self.time_trajectory,
                     self.yaw_trajectory
                     ) = self.calculate_leftright_trajectory()
                else:  # otherwise, perform the default test trajectory
                    print("Loading Test Trajectory...")
                    (self.position_trajectory, self.time_trajectory,
                     self.yaw_trajectory) = self.load_test_trajectory(
                         time_mult=0.5)
                self.all_waypoints = self.position_trajectory.copy()
                self.waypoint_number = -1
                self.waypoint_transition()
                print("Begin flight...")
        elif self.flight_state == States.WAYPOINT:
            if time.time() > self.time_trajectory[self.waypoint_number]:
                if len(self.all_waypoints) > 0:
                    if time.time(
                    ) - self.pos_time > 0.04:  # pace the calls to 25fps
                        self.waypoint_transition()
                        self.pos_time = time.time()
                else:
                    if np.linalg.norm(self.local_velocity[0:2]) < 1.0:
                        self.landing_transition()

    def velocity_callback(self):  # 80 frames per second
        if time.time() - self.prev_time > 0.05:  # pace the calls to 20fps
            if self.flight_state == States.LANDING:
                if self.global_position[2] - self.global_home[2] < 0.1:
                    if abs(self.local_position[2]) < 0.01:
                        self.disarming_transition()
            if self.flight_state == States.WAYPOINT:
                self.position_controller()

    def state_callback(self):  # 1 frame per second
        if self.in_mission:
            if self.flight_state == States.MANUAL:
                self.arming_transition()
            elif self.flight_state == States.ARMING:
                if self.armed:
                    self.takeoff_transition()
            elif self.flight_state == States.DISARMING:
                if ~self.armed & ~self.guided:
                    self.manual_transition()

    def calculate_fig8_trajectory(self):
        print("Creating Figure 8 Trajectory..."
              )  # same distance and timing as test trajectory
        total_time = 19.4
        t = np.linspace(0.0, total_time,
                        int(total_time / 0.05))  # inspired by class exercise
        xpath = 7.7186622 * np.sin(0.323875531 * (t + 2.35) * 2) - 7.70955541
        ypath = 7.7186622 * np.cos(0.323875531 *
                                   (t + 2.35)) - 7.7186622 + 2.12979
        zpath = np.cos(0.323875531 * (t + 2.35)) - 4.0 + 0.2759274
        position_trajectory = []
        for n in range(0, len(xpath)):
            position_trajectory.append(np.array([xpath[n], ypath[n],
                                                 zpath[n]]))
        yaw_trajectory = []
        for i in range(0, len(position_trajectory) - 1):
            yaw_trajectory.append(
                np.arctan2(
                    position_trajectory[i + 1][1] - position_trajectory[i][1],
                    position_trajectory[i + 1][0] - position_trajectory[i][0]))
        yaw_trajectory.append(yaw_trajectory[-1])
        current_time = time.time()  #+ 0.18
        time_trajectory = np.linspace(current_time, current_time + total_time,
                                      int(total_time / 0.05))
        return (position_trajectory, time_trajectory, yaw_trajectory)

    def calculate_leftright_trajectory(self):
        print("Creating Left-Right Trajectory..."
              )  # same distance and timing as test trajectory
        position_trajectory = [self.local_position]
        for i in range(12):
            position_trajectory.append(np.array([0.0, 1.5, -3.5]))
            position_trajectory.append(np.array([0.0, -1.5, -3.5]))
        position_trajectory.append(np.array([0.0, 1.5, -3.5]))
        position_trajectory.append(np.array([0.0, 0.0, -3.0]))
        current_time = time.time()
        time_trajectory = []
        for i in range(27):
            time_trajectory.append(current_time + 0.18 + i * 0.746154)
        yaw_trajectory = []
        for i in range(0, len(position_trajectory)):
            yaw_trajectory.append(0.0)
        return (position_trajectory, time_trajectory, yaw_trajectory)

    def calculate_box_trajectory(self):
        print("Creating Box Trajectory..."
              )  # same distance and timing as test trajectory
        position_trajectory = [
            self.local_position,
            np.array([18.195, 0.0, -4.0]),
            np.array([18.195, 18.195, -5.0]),
            np.array([0.0, 18.195, -4.0]),
            np.array([0.0, 0.0, -3.0])
        ]
        current_time = time.time()
        time_trajectory = [
            current_time + 0.18, current_time + 4.85 + 0.18,
            current_time + 9.7 + 0.18, current_time + 14.55 + 0.18,
            current_time + 19.4 + 0.18
        ]
        yaw_trajectory = []
        for i in range(0, len(position_trajectory) - 1):
            yaw_trajectory.append(
                np.arctan2(
                    position_trajectory[i + 1][1] - position_trajectory[i][1],
                    position_trajectory[i + 1][0] - position_trajectory[i][0]))
        yaw_trajectory.append(yaw_trajectory[-1])
        return (position_trajectory, time_trajectory, yaw_trajectory)

    def calculate_diagonal_trajectory(self):
        print("Creating Box Trajectory..."
              )  # same distance and timing as test trajectory
        position_trajectory = [
            self.local_position,
            np.array([57.2, 45.0, -3.0])
        ]
        current_time = time.time()
        time_trajectory = [current_time + 0.18, current_time + 19.4 + 0.18]
        yaw_trajectory = []
        for i in range(0, len(position_trajectory) - 1):
            yaw_trajectory.append(
                np.arctan2(
                    position_trajectory[i + 1][1] - position_trajectory[i][1],
                    position_trajectory[i + 1][0] - position_trajectory[i][0]))
        yaw_trajectory.append(yaw_trajectory[-1])
        return (position_trajectory, time_trajectory, yaw_trajectory)

    def calculate_hover(self):
        print("Creating hover trajectory...")  # same timing as test trajectory
        position_trajectory = [
            np.array([0.0, 0.0, -3.0]),
            np.array([0.0, 0.0, -3.0])
        ]  #[self.local_position, self.local_position]
        current_time = time.time()
        time_trajectory = [current_time + 0.18, current_time + 19.4 + 0.18]
        yaw_trajectory = []
        for i in range(0, len(position_trajectory) - 1):
            yaw_trajectory.append(
                np.arctan2(
                    position_trajectory[i + 1][1] - position_trajectory[i][1],
                    position_trajectory[i + 1][0] - position_trajectory[i][0]))
        yaw_trajectory.append(yaw_trajectory[-1])
        return (position_trajectory, time_trajectory, yaw_trajectory)

    def arming_transition(self):
        print("arming transition")
        self.take_control()
        self.arm()
        # set the current location to be the home position
        self.set_home_position(self.global_position[0],
                               self.global_position[1],
                               self.global_position[2])

        self.flight_state = States.ARMING

    def takeoff_transition(self):
        print("takeoff transition")
        target_altitude = 3.0
        self.target_position[2] = target_altitude
        self.takeoff(target_altitude)
        self.flight_state = States.TAKEOFF

    def waypoint_transition(self):
        self.waypoint_number = self.waypoint_number + 1
        self.target_position = self.all_waypoints.pop(0)
        #self.target_position = np.array([0.0, 0.0, -3.0]) # #######
        #print("Waypoint transition:", self.waypoint_number, self.target_position)
        self.flight_state = States.WAYPOINT

    def landing_transition(self):
        print("landing transition")
        self.land()
        self.flight_state = States.LANDING

    def disarming_transition(self):
        print("disarm transition")
        self.disarm()
        self.release_control()
        self.flight_state = States.DISARMING

    def manual_transition(self):
        print("manual transition")
        self.stop()
        self.in_mission = False
        self.flight_state = States.MANUAL

    def start(self):
        self.start_log("Logs", "NavLog.txt")
        # self.connect()

        print("starting connection")
        # self.connection.start()

        super().start()

        # Only required if they do threaded
        # while self.in_mission:
        #    pass

        self.stop_log()
class ControlsFlyer(UnityDrone):
    def __init__(self, connection):
        super().__init__(connection)
        self.target_position = np.array([0.0, 0.0, 0.0])
        self.all_waypoints = []
        self.in_mission = True
        self.check_state = {}

        # initial state
        self.flight_state = States.MANUAL

        self.controller = NonlinearController()
        # debug counters
        self.attitude_cnt = 0
        self.gyro_cnt = 0
        self.velocity_cnt = 0
        self.start_time = time.time()

        # register all your callbacks here
        self.register_callback(MsgID.STATE, self.state_callback)
        self.register_callback(MsgID.LOCAL_POSITION,
                               self.local_position_callback)
        self.register_callback(MsgID.LOCAL_VELOCITY, self.velocity_callback)
        self.register_callback(MsgID.ATTITUDE, self.attitude_callback)
        self.register_callback(MsgID.RAW_GYROSCOPE, self.gyro_callback)

    def state_callback(self):
        if self.in_mission:
            if self.flight_state == States.MANUAL:
                self.arming_transition()
            elif self.flight_state == States.ARMING:
                if self.armed:
                    self.takeoff_transition()
            elif self.flight_state == States.DISARMING:
                if ~self.armed & ~self.guided:
                    self.manual_transition()

    def local_position_callback(self):
        if self.flight_state == States.TAKEOFF:
            if -1.0 * self.local_position[2] > 0.95 * self.target_position[2]:
                # PLAN PATH
                print("plan trajectory")
                #self.all_waypoints = self.calculate_box()
                time_mult = 0.5
                (self.position_trajectory, self.time_trajectory,
                 self.yaw_trajectory) = self.load_test_trajectory(time_mult)
                self.all_waypoints = self.position_trajectory.copy()
                self.waypoint_number = -1
                # EXECUTE PATH: start
                print(
                    "execute trajectory"
                )  # not quite. that just tracks where we should be in time, but does not drive the controls
                self.waypoint_transition()
        elif self.flight_state == States.WAYPOINT:
            if time.time() > self.time_trajectory[self.waypoint_number]:
                if len(self.all_waypoints) > 0:
                    # EXECUTE PATH: continue
                    self.waypoint_transition()
                    #pass
                else:
                    if np.linalg.norm(self.local_velocity[0:2]) < 1.0:
                        # EXECUTE PATH: finish
                        self.landing_transition()

    def attitude_callback(self):
        if self.flight_state == States.WAYPOINT:
            self.attitude_cnt += 1
            #self.attitude_controller()
            self.thrust_cmd = self.controller.altitude_control(
                -self.local_position_target[2], -self.local_velocity_target[2],
                -self.local_position[2], -self.local_velocity[2],
                self.attitude, 9.81)
            roll_pitch_rate_cmd = self.controller.roll_pitch_controller(
                self.local_acceleration_target[0:2], self.attitude,
                self.thrust_cmd)
            yawrate_cmd = self.controller.yaw_control(self.attitude_target[2],
                                                      self.attitude[2])
            self.body_rate_target = np.array(
                [roll_pitch_rate_cmd[0], roll_pitch_rate_cmd[1], yawrate_cmd])

    def gyro_callback(self):
        if self.flight_state == States.WAYPOINT:
            #self.bodyrate_controller()
            # use body_rate_target set in attitude callback
            # this controller runs a faster loop

            self.gyro_cnt += 1
            if self.gyro_cnt % 20 == 0:
                print('time: {:.4f} secs'.format(time.time() -
                                                 self.start_time))
                print('gyro {}, att {}, vel {}'.format(self.gyro_cnt,
                                                       self.attitude_cnt,
                                                       self.velocity_cnt))

            moment_cmd = self.controller.body_rate_control(
                self.body_rate_target, self.gyro_raw)

            #            print("alt {:.4f}/{:.4f}; alt_vel {:.4f}/{:.4f}; thrust {:.4f}".format(
            #                self.local_position[2], self.local_position_target[2],
            #                self.local_velocity[2], self.local_velocity_target[2],
            #                self.thrust_cmd))

            #            print("roll {:.4f}/{:.4f}; pitch {:.4f}/{:.4f}; yaw {:.4f}/{:.4f}; moments {:.4f}, {:.4f}, {:.4f}; thrust {:.4f}".format(
            #                self.attitude[0], self.attitude_target[0],
            #                self.attitude[1], self.attitude_target[1],
            #                self.attitude[2], self.attitude_target[2],
            #                moment_cmd[0], moment_cmd[1], moment_cmd[2],
            #                self.thrust_cmd
            #            )
            #            )

            self.cmd_moment(moment_cmd[0], moment_cmd[1], moment_cmd[2],
                            self.thrust_cmd)

    def velocity_callback(self):
        if self.flight_state == States.LANDING:
            # landed? disarm
            if self.global_position[2] - self.global_home[2] < 0.1:
                if abs(self.local_position[2]) < 0.01:
                    self.disarming_transition()
        if self.flight_state == States.WAYPOINT:
            # in mission? control position and speed
            #self.position_controller()
            self.velocity_cnt += 1

            (self.local_position_target, self.local_velocity_target,
             yaw_cmd) = self.controller.trajectory_control(
                 self.position_trajectory, self.yaw_trajectory,
                 self.time_trajectory, time.time())
            #            print('time: {:.4f}; target position: {:.4f}/{:.4f}; target vel: {:.4f}/{:.4f}'.format(time.time() - self.start_time,
            #                                                                           self.local_position_target[0], self.local_position_target[1],
            #                                                                           self.local_velocity_target[0], self.local_velocity_target[1]))

            self.attitude_target = np.array((0.0, 0.0, yaw_cmd))

            acceleration_cmd = self.controller.lateral_position_control(
                self.local_position_target[0:2],
                self.local_velocity_target[0:2], self.local_position[0:2],
                self.local_velocity[0:2])
            #acceleration_cmd = np.array([0.0, 0.0])
            #            print("              acc:{:.4f}/{:.4f} --- pos diff:{:.4f}/{:.4f} --- vel diff:{:.4f}/{:.4f}".format(
            #                acceleration_cmd[0], acceleration_cmd[1],
            #                self.local_position_target[0] - self.local_position[0], self.local_position_target[1] - self.local_position[1],
            #                self.local_velocity_target[0] - self.local_velocity[0], self.local_velocity_target[1] - self.local_velocity[1]
            #            ))
            self.local_acceleration_target = np.array(
                [acceleration_cmd[0], acceleration_cmd[1], 0.0])

    def arming_transition(self):
        print("arming transition")
        self.take_control()
        self.arm()
        # set the current location to be the home position
        self.set_home_position(self.global_position[0],
                               self.global_position[1],
                               self.global_position[2])

        self.flight_state = States.ARMING

    def takeoff_transition(self):
        print("takeoff transition")
        target_altitude = 3.0
        self.target_position[2] = target_altitude
        self.takeoff(target_altitude)
        self.flight_state = States.TAKEOFF

    def waypoint_transition(self):
        self.waypoint_number = self.waypoint_number + 1
        self.target_position = self.all_waypoints.pop(0)
        print('time: {:.4f}; planned waypoint position: {}, {}'.format(
            time.time() - self.start_time, self.target_position[0],
            self.target_position[1]))
        self.flight_state = States.WAYPOINT

    def landing_transition(self):
        print("landing transition")
        self.land()
        self.flight_state = States.LANDING

    def disarming_transition(self):
        print("disarm transition")
        self.disarm()
        self.release_control()
        self.flight_state = States.DISARMING

    def manual_transition(self):
        print("manual transition")
        self.stop()
        self.in_mission = False
        self.flight_state = States.MANUAL

    def start(self):
        self.start_log("Logs", "NavLog.txt")
        # self.connect()

        print("starting connection")
        # self.connection.start()

        super().start()

        # Only required if they do threaded
        # while self.in_mission:
        #    pass

        self.stop_log()
Exemple #6
0
class BackyardFlyer(UnityDrone):
    def __init__(self, connection):
        super().__init__(connection)
        self.target_position = np.array([0.0, 0.0, 0.0])
        self.all_waypoints = []
        self.in_mission = True
        self.check_state = {}

        # initial state
        self.flight_state = States.MANUAL

        # TODO: Register all your callbacks here
        self.register_callback(MsgID.LOCAL_POSITION,
                               self.local_position_callback)
        self.register_callback(MsgID.LOCAL_VELOCITY, self.velocity_callback)
        self.register_callback(MsgID.STATE, self.state_callback)

        # register callbacks for RAW_GYROSCOPE, ATTITUDE, and LOCAL_VELOCITY messages.
        self.register_callback(MsgID.ATTITUDE, self.attitude_callback)
        self.register_callback(MsgID.RAW_GYROSCOPE, self.gyro_callback)
        self.register_callback(MsgID.LOCAL_VELOCITY, self.velocity_callback)

        # Add a controller object
        self.controller = NonlinearController()

    # add three controller methods
    def position_controller(self):
        """Sets the local acceleration target using the local position and local velocity"""

        (self.local_position_target, self.local_velocity_target,
         yaw_cmd) = self.controller.trajectory_control(
             self.position_trajectory, self.yaw_trajectory,
             self.time_trajectory, time.time())
        self.attitude_target = np.array((0.0, 0.0, yaw_cmd))
        acceleration_cmd = self.controller.lateral_position_control(
            self.local_position_target[0:2], self.local_velocity_target[0:2],
            self.local_position[0:2], self.local_velocity[0:2])
        self.local_acceleration_target = np.array(
            [acceleration_cmd[0], acceleration_cmd[1], 0.0])

    def attitude_controller(self):
        """Sets the body rate target using the acceleration target and attitude"""
        self.thrust_cmd = self.controller.altitude_control(
            -self.local_position_target[2], -self.local_velocity_target[2],
            -self.local_position[2], -self.local_velocity[2], self.attitude,
            9.81)
        roll_pitch_rate_cmd = self.controller.roll_pitch_controller(
            self.local_acceleration_target[0:2], self.attitude,
            self.thrust_cmd)
        yawrate_cmd = self.controller.yaw_control(self.attitude_target[2],
                                                  self.attitude[2])

        self.body_rate_target = np.array(
            [roll_pitch_rate_cmd[0], roll_pitch_rate_cmd[1], yawrate_cmd])

    def bodyrate_controller(self):
        """Commands a moment to the vehicle using the body rate target and body rates"""
        moment_cmd = self.controller.body_rate_control(self.body_rate_target,
                                                       self.gyro_raw)
        self.cmd_moment(moment_cmd[0], moment_cmd[1], moment_cmd[2],
                        self.thrust_cmd)

    def local_position_callback(self):
        """
        TODO: Implement this method

        This triggers when `MsgID.LOCAL_POSITION` is received and self.local_position contains new data
        """
        if self.flight_state == States.TAKEOFF:

            # coordinate conversion to positive value
            altitude = -1.0 * self.local_position[
                2]  # self.local_position[2] is vertical coordinate, positive pointing down

            # check if altitude is within 95% of target
            if altitude > 0.95 * self.target_position[2]:
                # self.all_waypoints = self.calculate_box()
                (self.position_trajectory, self.time_trajectory,
                 self.yaw_trajectory) = self.load_test_trajectory(
                     time_mult=0.5)
                self.all_waypoints = self.position_trajectory.copy()
                self.waypoint_number = -1
                self.waypoint_transition()  # call state transition function

        elif self.flight_state == States.WAYPOINT:
            #when approaching target position within 1m
            # if np.linalg.norm(self.target_position[0:2] - self.local_position[0:2]) < 1.0:
            if time.time() > self.time_trajectory[self.waypoint_number]:
                if len(self.all_waypoints
                       ) > 0:  # not yet finished way points following
                    self.waypoint_transition()
                else:
                    # when velocity less than 1.0, land down
                    if np.linalg.norm(self.local_velocity[0:2]) < 1.0:
                        self.landing_transition()

    def attitude_callback(self):

        if self.flight_state == States.WAYPOINT:
            self.attitude_controller()

    def gyro_callback(self):

        if self.flight_state == States.WAYPOINT:
            self.bodyrate_controller()

    def velocity_callback(self):
        """
        TODO: Implement this method

        This triggers when `MsgID.LOCAL_VELOCITY` is received and self.local_velocity contains new data
        """
        if self.flight_state == States.LANDING:
            if ((self.global_position[2] - self.global_home[2] < 0.1)
                    and abs(self.local_position[2]) < 0.01):
                self.disarming_transition()

        # for controller
        if self.flight_state == States.WAYPOINT:
            self.position_controller()

    def state_callback(self):
        """
        TODO: Implement this method

        This triggers when `MsgID.STATE` is received and self.armed and self.guided contain new data
        """
        if not self.in_mission:
            return
        if self.flight_state == States.MANUAL:
            self.arming_transition()
        elif self.flight_state == States.ARMING:
            if self.armed:
                self.takeoff_transition()
        elif self.flight_state == States.DISARMING:
            if not self.armed:
                self.manual_transition()

    def calculate_box(self):
        """TODO: Fill out this method
        
        1. Return waypoints to fly a box
        """
        print("Setting Home")
        local_waypoints = [[5.0, 0.0, 5.0], [5.0, 5.0, 5.0], [0.0, 5.0, 5.0],
                           [0.0, 0.0, 5.0]]

        return local_waypoints

    def arming_transition(self):
        """TODO: Fill out this method
        
        1. Take control of the drone
        2. Pass an arming command
        3. Set the home location to current position
        4. Transition to the ARMING state
        """
        print("arming transition")
        self.take_control()
        self.arm()

        # set the current location to be the home position
        self.set_home_position(self.global_position[0],
                               self.global_position[1],
                               self.global_position[2])

        self.flight_state = States.ARMING

    def takeoff_transition(self):
        """TODO: Fill out this method
        
        1. Set target_position altitude to 5.0m
        2. Command a takeoff to 5.0m
        3. Transition to the TAKEOFF state
        """
        print("takeoff transition")
        target_altitude = 5.0
        self.target_position[2] = target_altitude
        self.takeoff(target_altitude)
        self.flight_state = States.TAKEOFF

    def waypoint_transition(self):
        """TODO: Fill out this method
    
        1. Command the next waypoint position
        2. Transition to WAYPOINT state
        """
        print("waypoint transition")
        # get the waypoints for target_position
        self.target_position = self.all_waypoints.pop(0)
        print('target position', self.target_position)
        # input the global coordinates (vertical coordinate is positive when pointing upwards)
        # self.cmd_position(self.target_position[0], self.target_position[1], self.target_position[2], 0.0)
        # setting the target local position
        self.local_position_target = np.array(
            (self.target_position[0], self.target_position[1],
             self.target_position[2]))
        self.flight_state = States.WAYPOINT
        self.waypoint_number = self.waypoint_number + 1

    def landing_transition(self):
        """TODO: Fill out this method
        
        1. Command the drone to land
        2. Transition to the LANDING state
        """
        print("landing transition")
        self.land()
        self.flight_state = States.LANDING

    def disarming_transition(self):
        """TODO: Fill out this method
        
        1. Command the drone to disarm
        2. Transition to the DISARMING state
        """
        print("disarm transition")
        self.disarm()
        self.flight_state = States.DISARMING

    def manual_transition(self):
        """This method is provided
        
        1. Release control of the drone
        2. Stop the connection (and telemetry log)
        3. End the mission
        4. Transition to the MANUAL state
        """
        print("manual transition")

        self.release_control()
        self.stop()
        self.in_mission = False
        self.flight_state = States.MANUAL
        originDistance = 100 * np.linalg.norm(
            self.local_position)  # vector norm from numpy
        print(f'Distance to origin : {originDistance:5.3f} cm')

    def start(self):
        """This method is provided
        
        1. Open a log file
        2. Start the drone connection
        3. Close the log file
        """
        print("Creating log file")
        self.start_log("Logs", "NavLog.txt")
        print("starting connection")
        self.connection.start()
        print("Closing log file")
        self.stop_log()
Exemple #7
0
class ControlsFlyer(UnityDrone):
    def __init__(self, connection):
        super().__init__(connection)
        self.controller = NonlinearController()
        self.target_position = np.array([0.0, 0.0, 0.0])
        self.all_waypoints = []
        self.in_mission = True
        self.check_state = {}

        # initial state
        self.flight_state = States.MANUAL

        # register all your callbacks here
        self.register_callback(MsgID.LOCAL_POSITION,
                               self.local_position_callback)
        self.register_callback(MsgID.LOCAL_VELOCITY, self.velocity_callback)
        self.register_callback(MsgID.STATE, self.state_callback)

        self.register_callback(MsgID.ATTITUDE, self.attitude_callback)
        self.register_callback(MsgID.RAW_GYROSCOPE, self.gyro_callback)

        # flight history
        self.target_traj = []
        self.actual_traj = []
        self.traj_t = []
        self.target_v = []
        self.actual_v = []
        self.v_t = []

    def position_controller(self):
        (self.local_position_target, self.local_velocity_target,
         yaw_cmd) = self.controller.trajectory_control(
             self.position_trajectory, self.yaw_trajectory,
             self.time_trajectory, time.time())
        self.attitude_target = np.array((0.0, 0.0, yaw_cmd))
        acceleration_cmd = self.controller.lateral_position_control(
            self.local_position_target[0:2], self.local_velocity_target[0:2],
            self.local_position[0:2], self.local_velocity[0:2])
        self.local_acceleration_target = np.array(
            [acceleration_cmd[0], acceleration_cmd[1], 0.0])

    def attitude_controller(self):
        self.thrust_cmd = self.controller.altitude_control(
            -self.local_position_target[2], -self.local_velocity_target[2],
            -self.local_position[2], -self.local_velocity[2], self.attitude,
            9.81)
        roll_pitch_rate_cmd = self.controller.roll_pitch_controller(
            self.local_acceleration_target[0:2], self.attitude,
            self.thrust_cmd)
        yawrate_cmd = self.controller.yaw_control(self.attitude_target[2],
                                                  self.attitude[2])
        self.body_rate_target = np.array(
            [roll_pitch_rate_cmd[0], roll_pitch_rate_cmd[1], yawrate_cmd])

    def bodyrate_controller(self):
        moment_cmd = self.controller.body_rate_control(self.body_rate_target,
                                                       self.gyro_raw)
        self.cmd_moment(moment_cmd[0], moment_cmd[1], moment_cmd[2],
                        self.thrust_cmd)

    def attitude_callback(self):
        if self.flight_state == States.WAYPOINT:
            self.attitude_controller()

    def gyro_callback(self):
        if self.flight_state == States.WAYPOINT:
            self.bodyrate_controller()

    def local_position_callback(self):
        if self.flight_state == States.TAKEOFF:
            if -1.0 * self.local_position[2] > 0.95 * self.target_position[2]:
                #self.all_waypoints = self.calculate_box()
                (self.position_trajectory, self.time_trajectory,
                 self.yaw_trajectory) = self.load_test_trajectory(
                     time_mult=0.5)
                self.all_waypoints = self.position_trajectory.copy()
                self.waypoint_number = -1
                self.waypoint_transition()
        elif self.flight_state == States.WAYPOINT:
            t = time.time()
            self.traj_t.append(t)
            self.target_traj.append(self.local_position_target)
            self.actual_traj.append(self.local_position)
            if time.time() > self.time_trajectory[self.waypoint_number]:
                if len(self.all_waypoints) > 0:
                    self.waypoint_transition()
                else:
                    if np.linalg.norm(self.local_velocity[0:2]) < 1.0:
                        self.landing_transition()

    def velocity_callback(self):
        if self.flight_state == States.LANDING:
            if self.global_position[2] - self.global_home[2] < 0.1:
                if abs(self.local_position[2]) < 0.01:
                    self.disarming_transition()
        if self.flight_state == States.WAYPOINT:
            # print("target v: {}, actual v: {}".format(np.linalg.norm(self.local_velocity_target), np.linalg.norm(self.local_velocity)))
            t = time.time()
            self.v_t.append(t)
            self.target_v.append(self.local_velocity_target)
            self.actual_v.append(self.local_velocity)
            self.position_controller()

    def state_callback(self):
        if self.in_mission:
            if self.flight_state == States.MANUAL:
                self.arming_transition()
            elif self.flight_state == States.ARMING:
                if self.armed:
                    self.takeoff_transition()
            elif self.flight_state == States.DISARMING:
                if ~self.armed & ~self.guided:
                    self.manual_transition()

    def calculate_box(self):
        print("Setting Home")
        local_waypoints = [[10.0, 0.0, -3.0], [10.0, 10.0, -3.0],
                           [0.0, 10.0, -3.0], [0.0, 0.0, -3.0]]
        return local_waypoints

    def arming_transition(self):
        print("arming transition")
        self.take_control()
        self.arm()
        # set the current location to be the home position
        self.set_home_position(self.global_position[0],
                               self.global_position[1],
                               self.global_position[2])

        self.flight_state = States.ARMING

    def takeoff_transition(self):
        print("takeoff transition")
        target_altitude = 3.0
        self.target_position[2] = target_altitude
        self.takeoff(target_altitude)
        self.flight_state = States.TAKEOFF

    def waypoint_transition(self):
        #print("waypoint transition")
        self.waypoint_number = self.waypoint_number + 1
        self.target_position = self.all_waypoints.pop(0)
        self.flight_state = States.WAYPOINT

    def landing_transition(self):
        print("landing transition")
        self.land()
        self.flight_state = States.LANDING

    def disarming_transition(self):
        print("disarm transition")
        self.disarm()
        self.release_control()
        self.flight_state = States.DISARMING

    def manual_transition(self):
        print("manual transition")
        self.stop()
        self.in_mission = False
        self.flight_state = States.MANUAL

    def write_flight_log(self):
        import pickle
        logs = [
            self.traj_t, self.target_traj, self.actual_traj, self.v_t,
            self.target_v, self.actual_v
        ]
        with open('flight_log', 'wb') as f:
            pickle.dump(logs, f)

    def start(self):
        self.start_log("Logs", "NavLog.txt")
        # self.connect()

        print("starting connection")
        # self.connection.start()

        super().start()

        # Only required if they do threaded
        # while self.in_mission:
        #    pass

        self.stop_log()
        self.write_flight_log()