wip hw setup

wip

core/services/ardupilot_manager/ArduPilotManager.py

@@ -178,6 +178,7 @@ def get_default_params_cmdline(self, platform: Platform) -> str:
 
     async def start_linux_board(self, board: LinuxFlightController) -> None:
         self._current_board = board
+        board.setup_board()
         if not self.firmware_manager.is_firmware_installed(self._current_board):
             if board.platform == Platform.Navigator:
                 self.firmware_manager.install_firmware_from_file(

core/services/ardupilot_manager/flight_controller_detector/linux/hardware_setup/overlay_source/spi0-led.dts

@@ -0,0 +1,44 @@
+// This is a custom device tree overlay for the spi0 peripheral on the
+// Raspberry Pi 4. It will configure only the spi0 mosi pin
+// (The other spi0 pins will not be driven by the spi0 peripheral,
+// and can be used for other functions). This is to be used with
+// the Blue Robotics Navigator autopilot hat, where the RGB
+// 'neopixel' led data pin is connected to the spi0 mosi pin on the
+// Raspberry Pi 4.
+
+/dts-v1/;
+/plugin/;
+
+
+/ {
+    compatible = "brcm,bcm2835";
+
+    fragment@0 {
+        target = <&spi0_cs_pins>;
+        frag0: __overlay__ {
+            brcm,pins = <>;
+        };
+    };
+
+    fragment@1 {
+        target = <&spi0>;
+        frag1: __overlay__ {
+            cs-gpios = <>;
+            status = "okay";
+        };
+    };
+
+    fragment@2 {
+        target = <&spidev1>;
+        __overlay__ {
+            status = "disabled";
+        };
+    };
+
+    fragment@3 {
+        target = <&spi0_pins>;
+        __overlay__ {
+            brcm,pins = <10>;
+        };
+    };
+};

core/services/ardupilot_manager/flight_controller_detector/linux/hardware_setup/peripheral_configuration.py

@@ -0,0 +1,158 @@
+import re
+import shlex
+import subprocess
+import time
+from dataclasses import dataclass
+from typing import Optional
+
+all_dtparams = [
+    "i2c_vc=on",
+    "i2c_arm_baudrate=1000000",
+    "spi=on",
+    "enable_uart=1",
+]
+
+
+other_overlays = [
+    "dwc2 dr_mode=otg",
+]
+devices = {
+    "ADS1115": (0x48, 1),
+    "AK09915": (0x0C, 1),
+    "BME280": (0x76, 1),
+    "PCA9685": (0x40, 4),
+}
+
+
+@dataclass
+class I2cDevice:
+    device: str
+    overlay: str
+    pins: dict
+
+
+@dataclass
+class SpiDevice:
+    device: str
+    overlay: str
+    pins: dict
+
+
+@dataclass
+class SerialDevice:
+    device: str
+    overlay: str
+    pins: dict
+
+
+@dataclass
+class GpioSetup:
+    number: int
+    function: str
+    pull: str
+    value: str
+
+
+i2c_module = "i2c-dev"
+i2c_dtparams = [
+    "i2c_vc=on",
+    "i2c_arm_baudrate=1000000",
+]
+
+i2c_devices = [
+    I2cDevice(device="i2c-6", overlay="i2c6 pins_22_23=true baudrate=400000", pins={22: "SDA6", 23: "SCL6"}),
+    I2cDevice(device="i2c-1", overlay="i2c1", pins={2: "SDA1", 3: "SCL1"}),
+    I2cDevice(device="i2c-4", overlay="i2c4 pins_6_7=true baudrate=400000", pins={6: "SDA4", 7: "SCL4"}),
+]
+
+spi_devices = [
+    SpiDevice(device="spidev1.0", overlay="spi1-3cs", pins={19: "SPI1_MISO", 20: "SPI1_MOSI", 21: "SPI1_SCLK"}),
+    SpiDevice(
+        device="spidev0.0",
+        overlay="spi0-led",
+        pins={
+            10: "SPI0_MOSI",
+        },
+    ),
+]
+
+gpios = [
+    GpioSetup(number=11, function="OUT", pull="UP", value="HIGH"),
+    GpioSetup(number=24, function="OUT", pull="UP", value="HIGH"),
+    GpioSetup(number=25, function="OUT", pull="UP", value="HIGH"),
+    GpioSetup(number=37, function="OUT", pull="DOWN", value="LOW"),
+]
+
+
+def enable_i2c_module():
+    modules = subprocess.check_output("lsmod")
+    if "i2c_dev" in str(modules):
+        return
+    print(f"loading module {i2c_module}...")
+    output = subprocess.check_output(shlex.split(f"modprobe {i2c_module}"))
+    print(output)
+
+
+def enable_spi_module():
+    modules = subprocess.check_output("lsmod")
+    if "spi_dev" in str(modules):
+        return
+    print(f"loading module {i2c_module}...")
+    output = subprocess.check_output(shlex.split(f"modprobe {i2c_module}"))
+    print(output)
+
+
+@dataclass
+class GpioState:
+    number: int
+    level: int
+    fsel: int
+    alt: Optional[int]
+    func: str
+    pull: str
+
+
+def get_gpios_state():
+    output = subprocess.check_output(["raspi-gpio", "get"]).decode("utf-8")
+    pattern = r"GPIO (?P<gpio>\d+): level=(?P<level>\d) fsel=(?P<fsel>\d)(?: alt=(?P<alt>\d))? func=(?P<func>[\w\d_]+) pull=(?P<pull>\w+)"
+
+    # Using findall to extract all matches
+    matches = re.finditer(pattern, output)
+    gpios = {}
+    # Print each match
+    for match in matches:
+        gpios[int(match.group("gpio"))] = GpioState(
+            number=int(match.group("gpio")),
+            level=int(match.group("level")),
+            fsel=int(match.group("fsel")),
+            alt=int(match.group("alt")) if match.group("alt") else None,
+            func=match.group("func"),
+            pull=match.group("pull"),
+        )
+    return gpios
+
+
+def load_overlay(overlay: str):
+    output = subprocess.check_output(shlex.split(f"dtoverlay {overlay}")).decode("utf-8")
+
+
+enable_i2c_module()
+enable_spi_module()
+
+states = get_gpios_state()
+
+for device in [*i2c_devices, *spi_devices]:
+    needs_reload = False
+    for gpio, function in device.pins.items():
+        if states[gpio].func != function:
+            print(f"GPIO {gpio} is not configured as {function}, instad it is {states[gpio].func}")
+            print(f"{device.overlay} needs to be loaded")
+            needs_reload = True
+    if needs_reload:
+        load_overlay(device.overlay)
+        time.sleep(2)
+        new_state = get_gpios_state()
+        for gpio, function in device.pins.items():
+            if states[gpio].func != function:
+                print(f"GPIO {gpio} is STILL not configured as {function}, instad it is {states[gpio].func}")
+                raise Exception("Failed to configure device")

core/services/ardupilot_manager/flight_controller_detector/linux/navigator.py

@@ -1,15 +1,26 @@
+from enum import Enum
 from typing import List
 
-from commonwealth.utils.commands import load_file
+import RPi.GPIO as GPIO
+from commonwealth.utils.commands import load_file, locate_file, run_command, save_file
+from loguru import logger
+from smbus2 import SMBus
 
 from flight_controller_detector.linux.linux_boards import LinuxFlightController
+from flight_controller_detector.linux.overlay_loader import (
+    DtParam,
+    load_overlays_in_runtime,
+)
 from typedefs import Platform, Serial
 
+GPIO.setmode(GPIO.BCM)
+
 
 class Navigator(LinuxFlightController):
     name = "Navigator"
     manufacturer = "Blue Robotics"
     platform = Platform.Navigator
+    gpio_config = {}
 
     def is_pi5(self) -> bool:
         with open("/proc/cpuinfo", "r", encoding="utf-8") as f:
@@ -18,17 +29,17 @@ def is_pi5(self) -> bool:
     def detect(self) -> bool:
         return False
 
-    def get_serials(self) -> List[Serial]:
-        raise NotImplementedError
+    def setup_board(self) -> None:
+        load_overlays_in_runtime(self.all_dtparams, self.all_overlays, ["i2c_dev"])
+        for gpio, config in self.gpio_config.items():
+            self.setup_gpio(gpio, config["direction"], config["value"])
 
+    def setup_gpio(self, pin, direction, value) -> None:
+        GPIO.setup(pin, direction)
+        GPIO.output(pin, value)
 
-class NavigatorPi5(Navigator):
-    devices = {
-        "ADS1115": (0x48, 1),
-        "AK09915": (0x0C, 1),
-        "BME280": (0x76, 1),
-        "PCA9685": (0x40, 3),
-    }
+    def setup_board_for_detection(self) -> None:
+        load_overlays_in_runtime(self.all_dtparams, self.i2c_overlays, ["i2c_dev"])
 
     def get_serials(self) -> List[Serial]:
         return [
@@ -38,42 +49,108 @@ def get_serials(self) -> List[Serial]:
             Serial(port="F", endpoint="/dev/ttyAMA4"),
         ]
 
-    def detect(self) -> bool:
+    def build_led_overlay(self) -> None:
+        temp_overlay_file_at_host = "/tmp/spi0-led.dts"
+        target_overlay_location_pi4 = "/boot/overlays/spi0-led.dtbo"
+        target_overlay_location_pi5 = "/boot/firmware/overlays/spi0-led.dtbo"
+        overlay_exists = locate_file([target_overlay_location_pi4, target_overlay_location_pi5])
+        if overlay_exists:
+            logger.info(f"spi0-led overlay found at {overlay_exists}")
+            return False
+        dts = load_file(
+            "/home/pi/services/ardupilot_manager/flight_controller_detector/linux/overlay_source/spi0-led.dts"
+        )
+        save_file(temp_overlay_file_at_host, dts, "")
+        command = f"sudo dtc -@ -Hepapr -I dts -O dtb -o {target_overlay_location_pi4} {temp_overlay_file_at_host}"
+        run_command(command, False)
+        copy_command = (
+            f"if [ -d /boot/firmware ]; then sudo cp {target_overlay_location_pi4} {target_overlay_location_pi5}; fi"
+        )
+        run_command(copy_command, False)
+        # we should be able to load the just-built overlay, no need to restart
+        return False
+
+
+class NavigatorPi5(Navigator):
+    all_dtparams = [
+        "i2c_arm=on",
+        "i2c_arm_baudrate=1000000",
+    ]
+    # i2c overlays are required to detect the board
+    i2c_overlays = [
+        # i2c1: ADS1115, AK09915, BME280
+        "i2c1-pi5 baudrate=400000",
+        # i2c3: PCA
+        "i2c3-pi5 baudrate=400000",
+    ]
+    all_overlays = [
+        # serial ports, checked individually
+        "uart0-pi5",  # Navigator serial1
+        "uart3-pi5",  # Navigator serial4
+        "uart4-pi5",  # Navigator serial5
+        "uart2-pi5",  # Navigator serial3
+        # i2c-6: bar30 and friends
+        "i2c-gpio i2c_gpio_sda=22 i2c_gpio_scl=23 bus=6 i2c_gpio_delay_us=0",
+        # i2c1: ADS1115, AK09915, BME280
+        "i2c3-pi5 baudrate=400000",
+        # i2c3: PCA
+        "i2c3-pi5 baudrate=400000",
+        # SPI1: MMC5983
+        "spi1-3cs",
+        # SPI0: LED
+        "spi0-led",
+    ]
+    devices = {
+        "ADS1115": (0x48, 1),
+        "AK09915": (0x0C, 1),
+        "BME280": (0x76, 1),
+        "PCA9685": (0x40, 3),
+    }
+
+    def detect(self):
         if not self.is_pi5():
             return False
+        self.setup_board_for_detection()
         return all(self.check_for_i2c_device(bus, address) for address, bus in self.devices.values())
 
 
 class NavigatorPi4(Navigator):
+    all_dtparams = [
+        "i2c_vc=on",
+        "i2c_arm_baudrate=1000000",
+        "spi=on",
+        "enable_uart=1",
+    ]
+
+    # i2c overlays are required to detect the board
+    i2c_overlays = [
+        # i2c1: ADS1115, AK09915, BME280
+        "i2c1",
+        # i2c3: PCA
+        "i2c4 pins_6_7=true baudrate=1000000",
+    ]
+    all_overlays = [
+        # serial ports, checked individually
+        # serial ports, checked individually
+        "uart4",
+        "uart5",
+        "i2c6 pins_22_23=true baudrate=400000",
+        "spi0-led",
+        "spi1-3cs",
+        "dwc2 dr_mode=otg",
+    ]
     devices = {
         "ADS1115": (0x48, 1),
         "AK09915": (0x0C, 1),
         "BME280": (0x76, 1),
         "PCA9685": (0x40, 4),
     }
-
-    def get_serials(self) -> List[Serial]:
-        release = "Bullseye"
-        os_release = load_file("/etc/os-release")
-        if "bookworm" in os_release:
-            release = "Bookworm"
-
-        match release:
-            case "Bullseye":
-                return [
-                    Serial(port="C", endpoint="/dev/ttyS0"),
-                    Serial(port="B", endpoint="/dev/ttyAMA1"),
-                    Serial(port="E", endpoint="/dev/ttyAMA2"),
-                    Serial(port="F", endpoint="/dev/ttyAMA3"),
-                ]
-            case "Bookworm":
-                return [
-                    Serial(port="C", endpoint="/dev/ttyS0"),
-                    Serial(port="B", endpoint="/dev/ttyAMA3"),
-                    Serial(port="E", endpoint="/dev/ttyAMA4"),
-                    Serial(port="F", endpoint="/dev/ttyAMA5"),
-                ]
-        raise RuntimeError("Unknown release, unable to map ports")
+    gpio_config = {
+        11: {"direction": GPIO.OUT, "value": GPIO.HIGH},
+        24: {"direction": GPIO.OUT, "value": GPIO.HIGH},
+        25: {"direction": GPIO.OUT, "value": GPIO.HIGH},
+        37: {"direction": GPIO.OUT, "value": GPIO.LOW},
+    }
 
     def detect(self) -> bool:
         if self.is_pi5():