V2

.gitignore

@@ -1,2 +1,8 @@
 .idea
-.pyc
+.pyc
+.venv
+__pycache__
+.vscode
+'python=3.6'
+pyvicon-datatstream-module
+DroneController/__pycache__

.vscode/settings.json

@@ -0,0 +1,3 @@
+{
+    "ros.distro": "humble"
+}

DroneController/Drone.py

@@ -0,0 +1,162 @@
+from dronekit import connect, VehicleMode
+from pymavlink import mavutil
+from PyQt5.QtCore import QTimer
+import time 
+from PyQt5.QtCore import QObject, pyqtSignal, pyqtSlot
+
+
+from .utilities import State, DynamicAttributes
+
+import logging
+logging.basicConfig(
+    format='%(asctime)s %(levelname)-8s %(message)s',
+    level=logging.INFO,
+    datefmt='[Drone %H:%M:%S]')
+
+
+class djiDrone(QObject):
+    updateFlightModeGUI = pyqtSignal(str)
+
+    def __init__(self, parent: QObject, config:DynamicAttributes) -> None:
+        super().__init__(parent)
+        self.__config = config
+        self.state = State.LAND
+        self.default_cmd_vel = self.__config.default_cmd_vel
+        self.default_dt = self.__config.default_dt
+        self.mavlink_connect(self.__config.mavlink)
+
+
+    def mavlink_connect(self, connection_string):
+        self.vehicle = connect(connection_string, wait_ready=True)
+        self.state = State.HOVER
+
+        # Display Flight Mode
+        def modeCallback(vehicle, name, mode):
+            # print(vehicle, name, mode)
+            index, modeName = str(mode).split(':')
+            self.updateFlightModeGUI.emit(modeName)
+        self.addObserverAndInit(
+            'mode'
+            , modeCallback
+        )
+
+        self.vehicle.mode = VehicleMode("GUIDED")
+
+
+        self.viz_timer = QTimer()
+        self.viz_timer.timeout.connect(self.updateModeGUI)
+        self.viz_timer.start(1000)
+
+    def updateModeGUI(self):
+        index, modeName = str(self.vehicle.mode).split(':')
+        self.updateFlightModeGUI.emit(modeName)
+        logging.info(modeName)
+        self.viz_timer.stop()
+
+
+    def addObserverAndInit(self, name, cb):
+        """We go ahead and call our observer once at startup to get an initial value"""
+        self.vehicle.add_attribute_listener(name, cb)
+
+
+
+
+############### MAV LINK communication ##########################################################################
+    def publish_cmd_vel(self, velocity_x, velocity_y, velocity_z):
+        msg = self.vehicle.message_factory.set_position_target_local_ned_encode(
+            0,  # time_boot_ms (not used)
+            0, 0,  # target system, target component
+            mavutil.mavlink.MAV_FRAME_LOCAL_NED,  # frame
+            0b0000111111000111,  # type_mask (only speeds enabled)
+            0, 0, 0,  # x, y, z positions (not used)
+            velocity_x, velocity_y, velocity_z,  # x, y, z velocity in m/s
+            0, 0, 0,  # x, y, z acceleration (not supported yet, ignored in GCS_Mavlink)
+            0, 0)  # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink)
+        self.vehicle.send_mavlink(msg)
+
+    def send_ned_velocity(self, velocity_x, velocity_y, velocity_z, duration):
+        # send command to vehicle on x Hz cycle
+        elapsed_time = 0.0
+        while elapsed_time < duration:
+            self.publish_cmd_vel(velocity_x, velocity_y, velocity_z)
+            time.sleep(self.default_dt)
+            elapsed_time += self.default_dt
+############### Joystick communication ##########################################################################
+    def vehicle_validation(self, function):
+        if self.vehicle.mode == "GUIDED":
+            logging.info(f'button clicked {function.__name__}')
+            function()
+    @pyqtSlot()
+    def west_click(self):
+        if self.state != State.HOVER:
+            logging.warning("[Invalid request]: moving west is not possible")
+            return
+        @self.vehicle_validation
+        def west_wrapped():
+            self.send_ned_velocity(0, self.default_cmd_vel, 0, 1)
+            # self.send_ned_velocity(0, 0, 0, 1)
+    @pyqtSlot()
+    def east_click(self):
+        if self.state != State.HOVER:
+            logging.warning("[Invalid request]: moving east is not possible")
+            return
+        @self.vehicle_validation
+        def east_wrapped():
+            self.send_ned_velocity(0, -self.default_cmd_vel, 0, 1)
+            # self.send_ned_velocity(0, 0, 0, 1)
+
+    @pyqtSlot()
+    def north_click(self):
+        if self.state != State.HOVER:
+            logging.warning("[Invalid request]: moving north is not possible")
+            return
+        @self.vehicle_validation
+        def north_wrapped():
+            self.send_ned_velocity(-self.default_cmd_vel, 0, 0, 1)
+            # self.send_ned_velocity(0, 0, 0, 1)
+    @pyqtSlot()
+    def south_click(self):
+        if self.state != State.HOVER:
+            logging.warning("[Invalid request]: moving south is not possible")
+            return
+        @self.vehicle_validation
+        def south_wrapped():
+            self.send_ned_velocity(self.default_cmd_vel, 0, 0, 1)
+            # self.send_ned_velocity(0, 0, 0, 1)
+
+    @pyqtSlot()
+    def rtl_click(self):
+        if self.state == State.LAND or self.state == State.INITIALIZED:
+            logging.warning("[Invalid request]: landing is not possible")
+            return
+        @self.vehicle_validation
+        def rtl_wrapped():
+            self.vehicle.mode = VehicleMode("LAND")
+            self.state = State.LAND
+
+    @pyqtSlot()
+    def up_click(self):
+        if self.state != State.HOVER:
+            logging.warning("[Invalid request]: moving up is not possible")
+            return
+        @self.vehicle_validation
+        def up_wrapped():
+            alt = self.vehicle.location.global_relative_frame.alt
+            if alt < 3:
+                self.send_ned_velocity(0, 0, 0.5 * self.default_cmd_vel, 1)
+                # self.send_ned_velocity(0, 0, 0, 1)
+
+    @pyqtSlot()
+    def down_click(self):
+        if self.state != State.HOVER:
+            logging.warning("[Invalid request]: moving down is not possible")
+            return
+        @self.vehicle_validation
+        def down_wrapped():
+            alt = self.vehicle.location.global_relative_frame.alt
+            if alt > 0.5:
+                self.send_ned_velocity(0, 0, -0.5 * self.default_cmd_vel, 1)
+                # self.send_ned_velocity(0, 0, 0, 1)
+
+
+

DroneController/EKF.py

@@ -0,0 +1,88 @@
+import numpy as np
+
+# Covariance for EKF simulation
+Q = np.diag([
+    0.1,  # variance of location on x-axis
+    0.1,  # variance of location on y-axis
+    0.08,  # variance of velocity on x-axis
+    0.08,  # variance of velocity on y-axis
+    0.06,  # variance of acceleration on x-axis
+    0.06  # variance of acceleration on y-axis
+]) ** 2  # predict state covariance
+R = np.diag([0.01, 0.01]) ** 2  # Observation x,y position covariance
+
+def motion_model(x, u, DT):
+    F = np.array([[1.0, 0, 0, 0, 0.5 * DT * DT, 0],
+                  [0, 1.0, 0, 0, 0, 0.5 * DT * DT],
+                  [0, 0, 0.0, 0, 0, 0],
+                  [0, 0, 0, 0, 0, 0],
+                  [0, 0, 0, 0, 0, 0],
+                  [0, 0, 0, 0, 0, 0]
+                  ], dtype=np.float32)
+
+    B = np.array([[DT, 0],
+                  [DT, 0],
+                  [1.0, 0.0],
+                  [1.0, 0.0],
+                  [1.0 / DT, 0.0],
+                  [1.0 / DT, 0.0]
+                  ], dtype=np.float32)
+
+    x = F @ x + B @ u
+
+    return x
+
+
+def observation_model(x):
+    H = np.array([
+        [1, 0, 0, 0, 0, 0],
+        [0, 1, 0, 0, 0, 0]
+    ], dtype=np.float32)
+    z = H @ x
+
+    return z
+
+
+def jacob_f(x, u, DT):
+    """
+    Jacobian of Motion Model
+
+    motion model
+    """
+    vx = u[0, 0]
+    vy = u[1, 0]
+    ax = x[4, 0]
+    ay = x[5, 0]
+
+    jF = np.array([
+        [1.0, 0.0, DT * vx, 0.0, 0.5 * DT * DT * ax, 0.0],
+        [0.0, 1.0, 0.0, DT * vy, 0.0, 0.5 * DT * DT * ay],
+        [0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 1.0, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 1.0, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
+    ], dtype=np.float32)
+    return jF
+
+
+def jacob_h():
+    jH = np.array([[1, 0, 0, 0, 0, 0],
+                   [0, 1, 0, 0, 0, 0]], dtype=np.float32)
+    return jH
+
+
+def ekf_estimation(xEst, PEst, z, u, DT):
+    #  Predict
+    xPred = motion_model(xEst, u, DT)
+    jF = jacob_f(xEst, u, DT)
+    PPred = jF @ PEst @ jF.T + Q
+
+    #  Update
+    jH = jacob_h()
+    zPred = observation_model(xPred)
+    y = z - zPred
+    S = jH @ PPred @ jH.T + R
+    K = PPred @ jH.T @ np.linalg.inv(S)
+    xEst = xPred + K @ y
+    PEst = (np.eye(len(xEst)) - K @ jH) @ PPred
+    return xEst, PEst

DroneController/GUI.py

@@ -0,0 +1,113 @@
+from dronekit import VehicleMode
+
+from PyQt5.QtWidgets import QMainWindow
+from PyQt5 import uic
+from PyQt5.QtCore import QTimer, QObject, pyqtSignal, pyqtSlot
+
+
+import time
+from .Quadrotor import Quadrotor
+import numpy as np
+from PyQt5.QtCore import QTimer, QObject, pyqtSignal, pyqtSlot
+
+from .ViconClient import ViconClient
+from .Drone import djiDrone
+from .TrajFollower import TrajFollower
+from .utilities import ConfigParser
+
+import logging
+logging.basicConfig(
+    format='%(asctime)s %(levelname)-8s %(message)s',
+    level=logging.INFO,
+    datefmt='[GUI %H:%M:%S]')
+
+
+
+class MainWindow(QMainWindow):
+
+    def __init__(self, config:ConfigParser):
+        super().__init__()
+        self.__config = config.GUI
+        ui_path = self.__config.ui_path
+        uic.loadUi(ui_path, self)
+        self.coord = np.zeros(4) # x, y, z, yaw
+
+         # add visualizer
+        self.quad = Quadrotor(size=0.5)
+        self.viewer.addWidget(self.quad.canvas)
+
+
+        self.vicon = ViconClient(self, config.Vicon)
+        self.vicon.pose_signal.connect(self.pose_callback)
+
+        # viz timer 
+        self.viz_timer = QTimer()
+        self.viz_timer.timeout.connect(self.updatePoseGUI)
+        self.viz_timer.start(self.__config.viz_dt)
+
+
+        # establish mavlink connection 
+        self.dji = djiDrone(self, config.Drone)
+        self.dji.updateFlightModeGUI.connect(self.updateFlightModeGUI)
+
+        self.btnWest.clicked.connect(self.dji.west_click)
+        self.btnEast.clicked.connect(self.dji.east_click)
+        self.btnNorth.clicked.connect(self.dji.north_click)
+        self.btnSouth.clicked.connect(self.dji.south_click)
+        self.btnRTL.clicked.connect(self.dji.rtl_click)
+        self.btnUp.clicked.connect(self.dji.up_click)
+        self.btnDown.clicked.connect(self.dji.down_click)
+
+
+        # trajectory follower 
+        self.traj = TrajFollower(self.coord, config.Trajectory, self.dji, self)
+        self.btnSendTraj.clicked.connect(self.sendTrajectory)
+        self.numPointsText.textChanged.connect(self.updateNumPoints)
+        self.traj.maxVelSignal.connect(self.updateMaxVel)
+
+    def sendTrajectory(self):
+        self.traj.sendTrajectory()
+        self.quad.setPath(self.traj.traj)
+
+
+    def updateNumPoints(self):
+        text = self.numPointsText.text()
+        try:
+            numPoints = int(text)
+            self.traj.updateTrajPoints(numPoints)
+        except:
+            pass
+
+    @pyqtSlot(dict)
+    def pose_callback(self, data):
+        posistion = data['position']
+        yaw = data['orientation']
+
+        x, y, z = posistion
+
+
+        self.coord[0] = x
+        self.coord[1] = y
+        self.coord[2] = z
+        self.coord[3] = yaw
+
+
+        self.lblLongValue.setText(f"{x:.4f}")
+        self.lblLatValue.setText(f"{y:.4f}")
+        self.lblAltValue.setText(f"{z:.4f}")
+        logging.debug(f'location: {x}, {y}, {z}')
+
+    @pyqtSlot(str)
+    def updateFlightModeGUI(self, value):
+        logging.info(f'flight mode change to {value}')
+        # index, mode = str(value).split(':')
+        self.lblFlightModeValue.setText(value)
+        self.progressBar.setValue(100)
+
+    def updatePoseGUI(self):
+        heading = np.deg2rad(45) + self.coord[3]
+        self.quad.update_pose( self.coord[0], self.coord[1], self.coord[2],0,0, heading)
+
+    @pyqtSlot(float)
+    def updateMaxVel(self, value):
+        self.lbMaxVal.setText(f"{value:.3f} m/s")