cx-copter-app/Aplikacja/Aplikacja.pyw

from concurrent.futures import thread

import gi
gi.require_version('Gtk', '4.0')
from gi.repository import Gtk, GLib, Gdk

import threading
from inputs import get_gamepad
import math

# Importowanie stylów CSS
css_provider = Gtk.CssProvider()
css_provider.load_from_path('style.css')
Gtk.StyleContext.add_provider_for_display(Gdk.Display.get_default(), css_provider, Gtk.STYLE_PROVIDER_PRIORITY_APPLICATION)

# Funkcja pomocnicza
def setMarginAll(widget, margin):
    widget.set_margin_top(margin)
    widget.set_margin_bottom(margin)
    widget.set_margin_start(margin)
    widget.set_margin_end(margin)

class DroneApplication(Gtk.ApplicationWindow):
    
    def __init__(self, application):
        super().__init__(application=application, title="cx.copter")

        self.set_default_size(1280, 960)
        settings = Gtk.Settings.get_default()
        settings.set_property("gtk-application-prefer-dark-theme", True)

        # Domyślne wartości dla zmiennych przechowujących stan gamepada
        self.gamepad_connected = False
        self.right_x    = 0
        self.right_y    = 0
        self.left_y     = 0
        self.flaps_zl   = 0
        self.flaps_zr   = 0
        self.flaps_z    = 0
        self.toggle     = False
        self.prev       = 1

        # Inne zmienne
        self.ZAKRES = 10            # Jaki zakres [-n; n] mają mieć sygnały wejścia gamepada.
        self.PITCH_STEPS = 8        # Ile ma być łącznie szczebli głównych na połowie sztucznego horyzontu.
        self.PITCH_DEG_STEP = 15    # Co ile stopni rysować szczebel główny w sztucznym horyzoncie.

        # Uruchomienie wątku do monitorowania gamepada
        thread = threading.Thread(target=self.monitor_controller, daemon=True)
        thread.start()

        # INTERFEJS UŻYTKOWNIKA
        main = Gtk.Box(orientation=Gtk.Orientation.VERTICAL)
        
        # Nagłówek
        logo = Gtk.Image.new_from_file("assets/logo.png")
        logo.props.pixel_size = 64

        title = Gtk.Label(label = "Aplikacja do sterowania dronem")
        title.add_css_class("title-1")

        batteryBar = Gtk.ProgressBar()
        batteryBar.set_fraction(0.5)
        batteryBar.set_text("Bateria drona: NULL%")
        batteryBar.set_show_text(True)
        batteryBar.set_valign(Gtk.Align.CENTER)
        batteryBar.set_css_classes(['battery-bar'])

        header = Gtk.CenterBox(
            start_widget=logo,
            center_widget=title,
            end_widget=batteryBar
        )

        setMarginAll(logo, 10)
        setMarginAll(batteryBar, 10)

        main.append(header)

        main.append(Gtk.Separator(orientation=Gtk.Orientation.HORIZONTAL))

        # Ciało
        body = Gtk.Box(orientation=Gtk.Orientation.HORIZONTAL, homogeneous=True, spacing=15)

        self.sliderAltitudeBox = Gtk.Box(orientation=Gtk.Orientation.VERTICAL) # sterowanie wysokością (lewy joystick)

        self.sliderAltitudeLabel = Gtk.Label(label=f"ALT: {self.left_y}")

        self.sliderAltitude = Gtk.Scale(orientation=Gtk.Orientation.VERTICAL) 
        self.sliderAltitude.set_range(-self.ZAKRES, self.ZAKRES)
        self.sliderAltitude.set_sensitive(False)
        self.sliderAltitude.set_inverted(True)
        self.sliderAltitude.set_size_request(54, 250)
        self.sliderAltitude.set_vexpand(True)

        self.sliderAltitudeBox.append(self.sliderAltitudeLabel)
        self.sliderAltitudeBox.append(self.sliderAltitude)

        self.movementAreaBox = Gtk.Box(orientation=Gtk.Orientation.VERTICAL) # sterowanie poruszaniem się (prawy joystick)

        self.movementAreaLabel = Gtk.Label(label=f"PORUSZANIE")

        self.movementArea = Gtk.DrawingArea() 
        self.movementArea.set_hexpand(True)
        self.movementArea.set_vexpand(True)
        self.movementArea.set_draw_func(self.draw_movement)

        self.movementAreaBox.append(self.movementAreaLabel)
        self.movementAreaBox.append(self.movementArea)

        self.sliderZAxisBox = Gtk.Box(orientation=Gtk.Orientation.VERTICAL) # sterowanie poruszaniem po osi Z (klapy)

        self.sliderZAxisLabel = Gtk.Label(label=f"OBRACANIE: {self.flaps_z}")
        
        self.sliderZAxis = Gtk.Scale() 
        self.sliderZAxis.set_range(-self.ZAKRES, self.ZAKRES)
        self.sliderZAxis.set_sensitive(False)

        self.sliderZAxisBox.append(self.sliderZAxisLabel)
        self.sliderZAxisBox.append(self.sliderZAxis)

        self.horizon = Gtk.DrawingArea() # odczyt z żyroskopu - sztuczny horyzont
        self.horizon.set_hexpand(True)
        self.horizon.set_vexpand(True)
        self.horizon.set_draw_func(self.draw_horizon)
        
        horizonFrame = Gtk.AspectFrame( xalign=0.5, yalign=0.5, ratio=1.0, obey_child=False )
        horizonFrame.set_child(self.horizon)

        self.sliderAltitude.set_css_classes(['grid-elements'])
        self.movementArea.set_css_classes(['grid-elements'])
        self.sliderZAxis.set_css_classes(['grid-elements'])
        self.horizon.set_css_classes(['grid-elements'])

        leftPanelGrid = Gtk.Grid(hexpand=True)
        leftPanelGrid.attach(self.sliderAltitudeBox, 0, 0, 1, 1)
        leftPanelGrid.attach(self.movementAreaBox, 1, 0, 1, 1)
        leftPanelGrid.attach(self.sliderZAxisBox, 0, 1, 2, 1)

        body.append(leftPanelGrid)
        body.append(horizonFrame)
        setMarginAll(body, 20)

        main.append(body)
        main.append(Gtk.Separator(orientation=Gtk.Orientation.HORIZONTAL))

        # Stopka
        footer = Gtk.Box(orientation=Gtk.Orientation.HORIZONTAL, spacing=5)
        footer.set_size_request(-1, 150)
        setMarginAll(footer, 20)

        diagnosticBox = Gtk.Box(orientation=Gtk.Orientation.VERTICAL)
        diagnosticLabel = Gtk.Label(label="DIAGNOSTYKA", halign=Gtk.Align.CENTER)
        diagnosticLabel.add_css_class("title-4")
        diagnosticBox.append(diagnosticLabel)
        
        self.droneStatusLabel = Gtk.Label(label="Dron: Not Found")
        self.droneStatusLabel.set_halign(Gtk.Align.START)
        self.droneStatusLabel.set_size_request(180, -1)
        self.droneStatusLabel.set_halign(Gtk.Align.CENTER)
        self.droneStatusLabel.set_css_classes(['status-not-ok'])

        self.gamepadStatusLabel = Gtk.Label(label="Gamepad: Not Found")
        self.gamepadStatusLabel.set_halign(Gtk.Align.START)
        self.gamepadStatusLabel.set_size_request(180, -1)
        self.gamepadStatusLabel.set_css_classes(['status-inaccessible'])

        self.motorsStatusButton = Gtk.Button(label="Silniki: Off", sensitive=False)
        self.motorsStatusButton.set_halign(Gtk.Align.START)
        self.motorsStatusButton.set_size_request(180, -1)
        self.motorsStatusButton.set_css_classes(['status-inaccessible'])

        diagnosticBox.append(self.droneStatusLabel)
        diagnosticBox.append(self.gamepadStatusLabel)
        diagnosticBox.append(self.motorsStatusButton)

        logsBox = Gtk.Box(orientation=Gtk.Orientation.HORIZONTAL, spacing=15, homogeneous=True) # Telemetria
        logsBox.set_hexpand(True)

        sentBox = Gtk.Box(orientation=Gtk.Orientation.VERTICAL)
        sentLabel = Gtk.Label(label="WYSŁANE:")
        sentBox.append(sentLabel)

        sentScroll = Gtk.ScrolledWindow()
        sentScroll.set_policy(Gtk.PolicyType.NEVER, Gtk.PolicyType.AUTOMATIC)
        sentScroll.set_vexpand(True)
        sentScroll.set_size_request(150, 40)
        self.sent_logs = Gtk.TextView()
        self.sent_logs.set_editable(False)
        self.sent_logs.set_css_classes(['grid-elements'])
        sentScroll.set_child(self.sent_logs)

        sentBox.append(sentScroll)

        recvBox = Gtk.Box(orientation=Gtk.Orientation.VERTICAL)
        recvLabel = Gtk.Label(label="ODEBRANE:")
        recvBox.append(recvLabel)

        recvScroll = Gtk.ScrolledWindow()
        recvScroll.set_policy(Gtk.PolicyType.NEVER, Gtk.PolicyType.AUTOMATIC)
        recvScroll.set_vexpand(True)
        recvScroll.set_size_request(150, 40)
        self.recv_logs = Gtk.TextView()
        self.recv_logs.set_editable(False)
        self.recv_logs.set_css_classes(['grid-elements'])
        recvScroll.set_child(self.recv_logs)

        recvBox.append(recvScroll)

        logsBox.append(sentBox)
        logsBox.append(recvBox)

        actionsBox = Gtk.Box(orientation=Gtk.Orientation.VERTICAL, spacing=10) # Przyciski
        actionsBox.set_valign(Gtk.Align.CENTER)

        self.btn_test = Gtk.Button(label="Test LED / Silników")
        self.btn_test.set_size_request(150, -1)

        self.btn_cali = Gtk.Button(label="Kalibracja Żyroskopu")
        self.btn_cali.set_size_request(150, -1)

        actionsBox.append(self.btn_test)
        actionsBox.append(self.btn_cali)

        footer.append(diagnosticBox)
        footer.append(logsBox)
        footer.append(actionsBox)

        main.append(footer)
        
        
        self.set_child(main)

    # Funkcja do rysowania kierunku poruszania
    def draw_movement(self, area, c, width, height):
        cx = width / 2
        cy = height / 2

        target_x = cx + ((self.right_x / self.ZAKRES) * cx)
        target_y = cy - ((self.right_y / self.ZAKRES) * cy)

        # Linia
        c.set_source_rgb(1, 0, 0)
        c.move_to(cx, cy)
        c.line_to(target_x, target_y)
        c.stroke()

        # Kropka
        c.set_source_rgb(1, 0, 0)
        c.arc(target_x, target_y, 4, 0, 2 * math.pi)
        c.fill()
        
        # Tekst
        c.set_source_rgb(1, 1, 1)
        c.move_to(cx, cy)
        c.set_font_size(12)
        c.show_text(f"  {self.right_x} | {self.right_y}")
        c.move_to(target_x, target_y)

    # Funkcja do rysowania sztucznego horyzontu
    def draw_horizon(self, area, c, width, height):
        cx = width / 2.0
        cy = height / 2.0

        # DLA CELÓW SYMULACJI
        max_pitch = 45
        max_roll = 45

        pixels_per_step = cy - (cy + (height / self.PITCH_STEPS))

        roll_angle = (self.right_x / self.ZAKRES) * max_roll
        pitch_offset = (self.right_y / self.ZAKRES) * max_pitch

        pitch_final = pixels_per_step * (pitch_offset / self.PITCH_DEG_STEP)

        c.save()
        c.translate(cx, cy)
        c.rotate(-math.radians(roll_angle))
        c.translate(0, pitch_final)
        c.translate(-cx, -cy)

        # Rysowanie tarczy
        c.set_source_rgb(0.14, 0.63, 1) # niebo
        c.rectangle(-width, -height, width*4, height*2)
        c.fill()

        c.set_source_rgb(0.55, 0.38, 0.22) # ziemia
        c.rectangle(-width, cy, width*4, height*2)
        c.fill()

        # Rysowanie linii
        c.set_source_rgb(1, 1, 1)
        c.set_line_width(2)
        c.move_to(-cx, cy)
        c.line_to(width*2, cy)
        c.stroke()

        for x in range(-self.PITCH_STEPS, self.PITCH_STEPS + 1):
            if x == 0: continue # pomijamy środkową kreskę

            # główna linia
            target_pitch_y = cy + ((height / self.PITCH_STEPS) * x)

            c.set_source_rgb(1, 1, 1)
            c.set_line_width(2)
            c.move_to(cx / 2, target_pitch_y)
            c.line_to((cx / 2) + cx, target_pitch_y)
            c.stroke()

            pitch_text = str(-x * self.PITCH_DEG_STEP)
            c.set_font_size(max(10, cx * 0.06))
            text_width = c.text_extents(pitch_text).width

            c.move_to(cx / 2, target_pitch_y - 5)
            c.show_text(pitch_text)
            c.move_to((cx / 2) + cx - text_width, target_pitch_y - 5)
            c.show_text(pitch_text)
            
            # pomocnicza linia
            kierunek = 1 if x > 0 else -1
            target_pitch_y_sub = cy + ((height / self.PITCH_STEPS) * (x - 0.5 * kierunek))

            c.set_source_rgb(1, 1, 1)
            c.set_line_width(2)
            c.move_to(cx - (cx / 4), target_pitch_y_sub)
            c.line_to(cx + (cx / 4), target_pitch_y_sub)
            c.stroke()

        c.restore()

        # Rysowanie dziobu
        c.set_source_rgb(1, 1, 0)
        scalar = cx * 0.08
        c.set_line_width(max(2.0, scalar * 0.2))
        
        c.move_to(cx - 6 * scalar, cy)
        c.line_to(cx - 2 * scalar, cy)
        c.line_to(cx - 1 * scalar, cy + 1 * scalar)
        c.line_to(cx, cy)
        c.line_to(cx + 1 * scalar, cy + 1 * scalar)
        c.line_to(cx + 2 * scalar, cy)
        c.line_to(cx + 6 * scalar, cy)
        c.stroke()
        
        c.arc(cx, cy, max(2.0, scalar * 0.15), 0, 2 * math.pi)
        c.fill()
    
    # Funkcja do aktualizacji interfejsu użytkownika z aktualnymi wartościami stanu gamepada
    def update_ui(self):
        # Input gamepada
        self.sliderAltitude.set_value(self.left_y)
        self.sliderAltitudeLabel.set_text(f"ALT: {self.left_y}")
        self.movementArea.queue_draw()
        self.movementAreaLabel.set_text(f"PORUSZANIE")
        self.sliderZAxis.set_value(self.flaps_z)
        self.sliderZAxisLabel.set_text(f"OBRACANIE: {self.flaps_z}")

        self.horizon.queue_draw()

        # Statusy
        if self.gamepad_connected:
            self.gamepadStatusLabel.set_text("Gamepad: Ok")
            self.gamepadStatusLabel.set_css_classes(['status-ok'])
        else:
            self.gamepadStatusLabel.set_text("Gamepad: Not Found")
            self.gamepadStatusLabel.set_css_classes(['status-not-ok'])
        return False

    # Słuchanie zdarzeń z kontrolera
    def monitor_controller(self):
        raw_rx = 0
        raw_ry = 0
        while True:
            try:
                events = get_gamepad()

                if not self.gamepad_connected:
                    print("Pad podłączony")
                    self.gamepad_connected = True

                for event in events:
                    if event.code == 'BTN_START': # Przycisk START
                        if self.prev == 1 and event.state == 1:
                            self.toggle = not self.toggle
                        self.prev = not self.prev
                            
                    elif event.code == 'ABS_Y': # Lewy Joystick, oś Y
                        if abs(event.state) > 3276:
                            self.left_y = round((event.state / 32768.0) * self.ZAKRES, 2)
                        else:
                            self.left_y = 0

                    elif event.code == 'ABS_RX': # Prawy Joystick, oś X
                        raw_rx = event.state

                    elif event.code == 'ABS_RY': # Prawy Joystick, oś Y
                        raw_ry = event.state

                    elif event.code == 'ABS_Z': # Klapa tylna lewa
                        self.flaps_zl = event.state

                    elif event.code == 'ABS_RZ': # Klapa tylna prawa
                        self.flaps_zr = event.state

                # Wyprowadzenie wejścia z prawego joysticka z wykluczeniem kołowej martwej strefy
                magnitude = math.sqrt(raw_rx**2 + raw_ry**2)
                if magnitude > 3276:
                    self.right_x = round((raw_rx / 32768.0) * self.ZAKRES, 2)
                    self.right_y = round((raw_ry / 32768.0) * self.ZAKRES, 2)
                else:
                    self.right_x = 0
                    self.right_y = 0

                # Przeliczenie różnicy klap i skalowanie
                self.flaps_z = round(((self.flaps_zr - self.flaps_zl) / 255) * self.ZAKRES, 2)

                GLib.idle_add(self.update_ui)
            except Exception as e:
                if self.gamepad_connected:
                    print(f"Błąd: {e}")
                    self.gamepad_connected = False
                    self.right_x = 0
                    self.right_y = 0
                    self.left_y = 0
                    self.flaps_z = 0
                    GLib.idle_add(self.update_ui)

                time.sleep(1)

# Inicjalizacja aplikacji
def on_activate(app):
    window = DroneApplication(application=app)
    window.present()

app = Gtk.Application(application_id="com.cx.copter.app")
app.connect("activate", on_activate)
app.run(None)