.gitignore

# Byte-compiled / optimized / DLL files
__pycache__/
*.py[cod]
*$py.class

# C extensions
*.so

# Distribution / packaging
.Python
build/
develop-eggs/
dist/
downloads/
eggs/
.eggs/
lib/
lib64/
parts/
sdist/
var/
wheels/
share/python-wheels/
*.egg-info/
.installed.cfg
*.egg
MANIFEST

# PyInstaller
#  Usually these files are written by a python script from a template
#  before PyInstaller builds the exe, so as to inject date/other infos into it.
*.manifest
*.spec

# Installer logs
pip-log.txt
pip-delete-this-directory.txt

# Unit test / coverage reports
htmlcov/
.tox/
.nox/
.coverage
.coverage.*
.cache
nosetests.xml
coverage.xml
*.cover
*.py,cover
.hypothesis/
.pytest_cache/
cover/

# Translations
*.mo
*.pot

# Django stuff:
*.log
local_settings.py
db.sqlite3
db.sqlite3-journal

# Flask stuff:
instance/
.webassets-cache

# Scrapy stuff:
.scrapy

# Sphinx documentation
docs/_build/

# PyBuilder
.pybuilder/
target/

# Jupyter Notebook
.ipynb_checkpoints

# IPython
profile_default/
ipython_config.py

# pyenv
#   For a library or package, you might want to ignore these files since the code is
#   intended to run in multiple environments; otherwise, check them in:
# .python-version

# pipenv
#   According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
#   However, in case of collaboration, if having platform-specific dependencies or dependencies
#   having no cross-platform support, pipenv may install dependencies that don't work, or not
#   install all needed dependencies.
#Pipfile.lock

# poetry
#   Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control.
#   This is especially recommended for binary packages to ensure reproducibility, and is more
#   commonly ignored for libraries.
#   https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control
#poetry.lock

# pdm
#   Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control.
#pdm.lock
#   pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it
#   in version control.

"""
import time
import math

class PID:
    def __init__(self, Kp, Ki, Kd, limit, alpha=0.1):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.limit = limit  # Max angular speed or step
        self.alpha = alpha  
        self.prev_error = 0
        self.integral = 0
        self.filtered_derivative = 0
        

    def update(self, error):
        
        
        # Proportional
        P = self.Kp * error
        
        # Integral
        self.integral += error
        I = self.Ki * self.integral
        
        # 3. Derivative with Low-Pass Filter
        derivative = (error - self.prev_error)
        self.filtered_derivative = (self.alpha * derivative) + (1 - self.alpha) * self.filtered_derivative
        D = self.Kd * self.filtered_derivative
        
        output = P + I + D
        
        output = max(min(output, self.limit), -self.limit)
        
        self.prev_error = error
        
        return output



yaw_pid = PID(Kp=0.6, Ki=0.05, Kd=0.1, limit=15.0) # Limit 15 deg/sec
pitch_pid = PID(Kp=0.6, Ki=0.05, Kd=0.1, limit=15.0)

def calculate_angles(target_x, target_y, current_fov_h, current_fov_v, img_w, img_h):
    # 1. Calculate pixel error from center
    err_x = target_x - (img_w / 2)
    err_y = target_y - (img_h / 2)
    
    # 2. Convert pixel error to angular error (Degrees)
    # Using the linear approximation for speed
    angle_err_x = err_x * (current_fov_h / img_w)
    angle_err_y = err_y * (current_fov_v / img_h)
    
    # 3. Process through PID
    # This returns the DELTA angle or VELOCITY to apply
    delta_yaw = yaw_pid.update(angle_err_x)
    delta_pitch = pitch_pid.update(angle_err_y)
    
    return delta_yaw, delta_pitch

# Example Loop
# current_yaw, current_pitch = get_current_angles()
# dy, dp = calculate_angles(obj.x, obj.y, 60.0, 45.0, 1920, 1080)
# move_to(current_yaw + dy, current_pitch + dp)






import serial
import struct
import threading
import time

# Global status for graceful shutdown
PROGRAM_STATUS = True


class Gimbal:
    def __init__(self, port, baud=115200):
        self.port = port
        self.baud = baud
        self.ser = None

        # --- Internal State (Latest Status) ---
        self.state = {
            "azimuth": 0.0,
            "elevation": 0.0,
            "actuator": "Unknown",
            "brake": "Unknown",
            "last_update": 0,
        }

        self.lock = threading.Lock()
        self.write_lock = threading.Lock()  # Prevents command collision
        self.thread = threading.Thread(target=self._reader_loop, daemon=True)

    # --- Initialization & Lifecycle ---
    def start(self):
        try:
            self.ser = serial.Serial(self.port, self.baud, timeout=0.1)
            self.ser.reset_input_buffer()
            self.thread.start()
            print(f"[Gimbal] Connection established on {self.port}")
            return True
        except Exception as e:
            print(f"[Gimbal] Initialization Error: {e}")
            return False

    def _calculate_xor(self, data):
        xor = 0
        for byte in data:
            xor ^= byte
        return xor

    # --- Background Reader (Status Monitoring) ---
    def _reader_loop(self):
        global PROGRAM_STATUS
        raw_buffer = bytearray()

        while PROGRAM_STATUS:
            if not self.ser or not self.ser.is_open:
                break

            # 1. Read all available bytes into the buffer
            if self.ser.in_waiting > 0:
                raw_buffer.extend(self.ser.read(self.ser.in_waiting))

            # 2. Process the buffer while it has enough data for at least a small packet
            # Minimum packet length is 5 bytes (Header, Len, Type, Data, CS)
            while len(raw_buffer) >= 5:
                # Find the first occurrence of the header 0xAA
                if raw_buffer[0] != 0xAA:
                    raw_buffer.pop(0)  # Discard junk byte
                    continue

                # We found a header, check the length byte
                msg_len = raw_buffer[1]

                # Safety check: if msg_len is impossibly small or large, it's a false header
                if msg_len < 5 or msg_len > 20:
                    raw_buffer.pop(0)  # Discard the fake header
                    continue

                # Do we have the full packet in the buffer yet?
                if len(raw_buffer) < msg_len:
                    break  # Wait for more data to arrive

                # We have enough bytes for a full packet! Extract it.
                packet = raw_buffer[:msg_len]

                # Checksum check
                # Checksum is the XOR of everything except the last byte
                calc_xor = self._calculate_xor(packet[:-1])
                received_xor = packet[-1]

                if calc_xor == received_xor:
                    # VALID PACKET: Parse it!
                    msg_type = packet[2]
                    payload = packet[3:-1]
                    self._update_internal_state(msg_type, payload)

                    # Remove the processed packet from buffer
                    del raw_buffer[:msg_len]
                else:
                    # INVALID CHECKSUM: The 0xAA might have been data, not a header.
                    # Pop the first byte and keep scanning.
                    raw_buffer.pop(0)

            # Tiny sleep to prevent 100% CPU usage if no data is coming
            time.sleep(0.001)

    def _parse_incoming(self, msg_type, payload):
        with self.lock:
            self.state["last_update"] = time.time()
            if msg_type == 0x0A:  # Status: Az/El
                self.state["azimuth"], self.state["elevation"] = struct.unpack(
                    "<ff", payload
                )
            elif msg_type in [0x25, 0x29]:  # Status: Actuator
                mapping = {
                    0x00: "Retracted",
                    0xFF: "Extended",
                    0x55: "In Motion",
                    0x65: "In Motion",
                }
                self.state["actuator"] = mapping.get(payload[0], "Unknown")
            elif msg_type == 0x27:  # Status: Brake Engaged
                self.state["brake"] = "Engaged"
            elif msg_type == 0x28:  # Status: Brake Disengaged
                self.state["brake"] = "Disengaged"

    # --- Command Methods (Sending Data) ---
    def _send_command(self, msg_type, payload_bytes):
        """Internal helper to construct and send packets."""
        header = 0xAA
        # Length = Header(1) + LenByte(1) + Type(1) + Payload(N) + Checksum(1)
        length = 1 + 1 + 1 + len(payload_bytes) + 1

        packet = bytearray([header, length, msg_type])
        packet.extend(payload_bytes)
        packet.append(self._calculate_xor(packet))

        with self.write_lock:
            if self.ser and self.ser.is_open:
                self.ser.write(packet)

    def set_angles(self, azimuth=None, elevation=None):
        """Sets Azimuth and/or Elevation setpoints (4-byte floats)."""
        if elevation is not None:
            self._send_command(0x20, struct.pack("<f", float(elevation)))
        if azimuth is not None:
            self._send_command(0x21, struct.pack("<f", float(azimuth)))

    def set_orientation(self, pitch=None, yaw=None, roll=None):
        """Sets Pitch, Yaw, or Roll setpoints."""
        if pitch is not None:
            self._send_command(0x22, struct.pack("<f", float(pitch)))
        if yaw is not None:
            self._send_command(0x23, struct.pack("<f", float(yaw)))
        if roll is not None:
            self._send_command(0x24, struct.pack("<f", float(roll)))

    def control_actuator(self, command_type="engage", state=0x00):
        """
        command_type: "engage" (0x25) or "extend" (0x29)
        state: 0x00 (Retract), 0xFF (Extend/Engage)
        """
        msg_type = 0x25 if command_type == "engage" else 0x29
        self._send_command(msg_type, bytes([state]))

    def control_brake(self, action="apply"):
        """action: "apply" (0x27) or "release" (0x28)"""
        if action == "apply":
            self._send_command(
                0x27, bytes([0x01])
            )  # Sending 1 byte payload as per struct
        else:
            self._send_command(0x28, bytes([0x01]))

    def get_status(self):
        with self.lock:
            return self.state.copy()


# --- Main Application Logic ---
if __name__ == "__main__":
    gimbal = Gimbal(port="/dev/ttyUSB0")  # Adjust to your port (e.g., 'COM3')

    if gimbal.start():
        try:
            while PROGRAM_STATUS:
                # 1. Read latest status
                status = gimbal.get_status()
                print(
                    f"\r[STATUS] AZ: {status['azimuth']:.2f} EL: {status['elevation']:.2f} "
                    f"Act: {status['actuator']} Brake: {status['brake']}",
                    end="",
                )

                # 2. Example: Send Commands based on logic
                # Let's say we want to move to 45, 45 if we aren't there
                # gimbal.set_angles(azimuth=45.0, elevation=45.0)

                # gimbal.control_brake("release")
                # gimbal.control_actuator("extend", 0xFF)

                time.sleep(0.1)  # Main loop rate

        except KeyboardInterrupt:
            print("\n[Main] User interrupted. Shutting down...")
            PROGRAM_STATUS = False
            time.sleep(0.5)


"""
Azimuth and Elevation Status Command Packet Structure: (12 bytes)
Header  Message_Length  Message_Type    Azimuth     Elevation  Checksum
(0xAA)      (0x0C)        (0x0A)        (4 bytes)   (4 bytes)  (dependant on data)

Actuator Engage Status Command Packet Structure: (5 bytes)
Header  Message_Length  Message_Type    Actuator      Checksum
(0xAA)      (0x05)        (0x25)        (1 bytes)    (dependant on data)
0x00 - Retracted
0xFF - Extended
0x55 - in Motion

Brake Engage Status Command Packet Structure: (5 bytes)
Header  Message_Length  Message_Type    Brake        Checksum
(0xAA)      (0x05)        (0x27)        (0x0B)    (dependant on data)

Brake Disengage Status Command Packet Structure: (5 bytes)
Header  Message_Length  Message_Type    Brake        Checksum
(0xAA)      (0x05)        (0x28)        (0x0C)    (dependant on data)

Actuator Extend Status Command Packet Structure: (5 bytes)
Header  Message_Length  Message_Type    Actuator      Checksum
(0xAA)      (0x05)        (0x29)        (1 bytes)    (dependant on data)
0x00 - Retracted
0xFF - Extended
0x65 - in Motion
"""


"""
Elevation Setpoint Command Packet Structure: (8 bytes)
Header  Message_Length  Message_Type    Elevation  Checksum
(0xAA)      (0x08)        (0x20)        (4 bytes)  (dependant on data)

Azimuth Setpoint Command Packet Structure: (8 bytes)
Header  Message_Length  Message_Type    Azimuth      Checksum
(0xAA)      (0x08)        (0x21)        (4 bytes)    (dependant on data)

Pitch Setpoint Command Packet Structure: (8 bytes)
Header  Message_Length  Message_Type    Pitch        Checksum
(0xAA)      (0x08)        (0x22)        (4 bytes)    (dependant on data)

Yaw Setpoint Command Packet Structure: (8 bytes)
Header  Message_Length  Message_Type    Yaw          Checksum
(0xAA)      (0x08)        (0x23)        (4 bytes)    (dependant on data)

Roll Setpoint Command Packet Structure: (8 bytes)
Header  Message_Length  Message_Type    Roll         Checksum
(0xAA)      (0x08)        (0x24)        (4 bytes)    (dependant on data)

Engage Actuator Command Packet Structure: (5 bytes)
Header  Message_Length  Message_Type    Actuator      Checksum
(0xAA)      (0x05)        (0x25)        (1 bytes)    (dependant on data)

Apply Brake Command Packet Structure: (5 bytes)
Header  Message_Length  Message_Type    Brake        Checksum
(0xAA)      (0x05)        (0x27)        (1 bytes)    (dependant on data)

Release Brake Command Packet Structure: (5 bytes)
Header  Message_Length  Message_Type    Brake        Checksum
(0xAA)      (0x05)        (0x28)        (1 bytes)    (dependant on data)

Extend Actuator Command Packet Structure: (5 bytes)
Header  Message_Length  Message_Type    Actuator      Checksum
(0xAA)      (0x05)        (0x29)        (1 bytes)    (dependant on data)
"""

That is a crucial piece of information. If the exact same code and GCS setup work perfectly on another Jetson, we can rule out your Flask code and the network.

The problem is specific to the **hardware or the low-level configuration** of that particular Jetson Xavier.

Since you aren't running any detection code, those "4 boxes" and "red glitches" are likely being injected by the **Image Signal Processor (ISP)** or the **Camera Firmware** itself.

---

### **Potential Culprits on the Specific Jetson**

#### **1. Privacy Masks or Motion Detection (Most Likely)**

Many high-resolution cameras (especially IP cameras or those using the Sony IMX sensors common with Jetson) have built-in **Privacy Masking** or **Hardware Motion Detection**.

* **The Symptom:** These features often manifest as 4-8 static rectangles. When motion is detected inside them, the box changes color (often to red) or flickers to alert the user.
* **Why it's happening:** This feature might be enabled in the camera's non-volatile memory or via a specific `v4l2` (Video4Linux) control setting that is toggled "ON" on this specific Jetson.

#### **2. Nvidia Argus "Test Pattern" or Debug Overlays**

If you are using the `nvarguscamerasrc` GStreamer element, there are debug flags that can overlay metadata or "regions of interest" (ROI) on the raw stream.

* **The Fix:** Check if there is an environment variable or a configuration file in `/etc/en_nvidia/` that differs between the two Jetsons.

#### **3. Sensor/CSI Cable Interference**

High-resolution streams are very sensitive to EMI (Electromagnetic Interference).

* **The Symptom:** If the MIPI CSI ribbon cable is damaged or poorly seated, you can get "blocky" artifacts. However, these usually look like random digital noise, not perfectly shaped "4 boxes."
* **The "Red" Glitch:** If the "boxes" are perfectly rectangular, it is almost certainly a **software/firmware overlay**, not a physical cable fault.

---

### **How to Isolate the Issue**

To prove it's the Jetson/Camera hardware and not your Flask app, run a standard GStreamer test in the terminal of the "glitchy" Jetson:

```bash
# This bypasses Flask and displays the raw feed directly to a window
gst-launch-1.0 nvarguscamerasrc ! 'video/x-raw(memory:NVMM),width=1920,height=1080' ! nvvidconv ! xvimagesink

```

**If you still see the boxes here:** The issue is in the **Argus Daemon** or the **Camera Firmware**.
**If you don't see the boxes here:** The issue is in how **OpenCV** or **Flask** is accessing the camera on that specific machine (check library versions).

### **Recommended Steps**

1. **Compare V4L2 Controls:** Run this on both Jetsons and look for differences in "Privacy Mask" or "Motion" settings:
`v4l2-ctl -d /dev/video0 --list-ctrls`
2. **Check JetPack Versions:** Ensure both are on the same version.
`cat /etc/nv_tegra_release`
3. **Swap the Cameras:** If possible, swap the camera and cable from the "working" Jetson to the "glitchy" one. This will tell you instantly if the problem is the Xavier or the Camera module itself.

**When you run the GStreamer command above, do the boxes still appear on the monitor connected to the Jetson?**
""

#   https://pdm.fming.dev/#use-with-ide
.pdm.toml

# PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm
__pypackages__/

# Celery stuff
celerybeat-schedule
celerybeat.pid

# SageMath parsed files
*.sage.py

# Environments
.env
.venv
env/
venv/
ENV/
env.bak/
venv.bak/

# Spyder project settings
.spyderproject
.spyproject

# Rope project settings
.ropeproject

# mkdocs documentation
/site

# mypy
.mypy_cache/
.dmypy.json
dmypy.json

# Pyre type checker
.pyre/

# pytype static type analyzer
.pytype/

# Cython debug symbols
cython_debug/

# PyCharm
#  JetBrains specific template is maintained in a separate JetBrains.gitignore that can
#  be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore
#  and can be added to the global gitignore or merged into this file.  For a more nuclear
#  option (not recommended) you can uncomment the following to ignore the entire idea folder.
#.idea/