Add selectable Pi 5 I2C/MIPI mode

core/frontend/src/components/video-manager/VideoManager.vue

@@ -68,6 +68,26 @@
             label="Raspberry legacy camera support"
             @change="toggleLegacyMode"
           />
+          <v-switch
+            v-if="is_raspberry_pi_5"
+            v-model="pi5_mipi_camera_mode"
+            :disabled="updating_pi5_i2c_mode"
+            :loading="updating_pi5_i2c_mode"
+            label="Raspberry Pi 5 MIPI camera support"
+            hint="Requires a reboot and disables external Navigator I²C sensors in ArduPilot."
+            persistent-hint
+            @change="togglePi5MipiCameraMode"
+          />
+          <v-alert
+            v-if="is_raspberry_pi_5 && pi5_mipi_camera_mode"
+            class="mt-3"
+            dense
+            text
+            type="warning"
+          >
+            The Navigator external I²C port is moved to bus 8 in this mode. Current Navigator ArduPilot firmware
+            probes external sensors on bus 6, so those sensors will not be detected.
+          </v-alert>
           <v-btn
             @click="resetSettings"
           >
@@ -88,7 +108,8 @@ import Vue from 'vue'
 import SpinningLogo from '@/components/common/SpinningLogo.vue'
 import Notifier from '@/libs/notifier'
 import settings from '@/libs/settings'
-import commander from '@/store/commander'
+import commander, { Pi5I2CMode } from '@/store/commander'
+import system_information from '@/store/system-information'
 import video from '@/store/video'
 import { commander_service } from '@/types/frontend_services'
 import {
@@ -114,9 +135,14 @@ export default Vue.extend({
     return {
       show_settings_dialog: false,
       legacy_mode: false,
+      pi5_mipi_camera_mode: false,
+      updating_pi5_i2c_mode: false,
     }
   },
   computed: {
+    is_raspberry_pi_5(): boolean {
+      return system_information.platform?.raspberry?.model?.includes('Raspberry Pi 5') ?? false
+    },
     are_video_devices_available(): boolean {
       return !this.video_devices.isEmpty()
     },
@@ -163,6 +189,9 @@ export default Vue.extend({
   },
   async mounted() {
     await this.updateCameraLegacy()
+    if (this.is_raspberry_pi_5) {
+      await this.updatePi5I2CMode()
+    }
   },
   methods: {
     async updateCameraLegacy(): Promise<void> {
@@ -178,11 +207,29 @@ export default Vue.extend({
     async toggleLegacyMode(): Promise<void> {
       await this.setCameraLegacy(this.legacy_mode)
     },
+    async updatePi5I2CMode(): Promise<void> {
+      this.pi5_mipi_camera_mode = await commander.getPi5I2CMode() === Pi5I2CMode.MipiCamera
+    },
+    async togglePi5MipiCameraMode(): Promise<void> {
+      this.updating_pi5_i2c_mode = true
+      const mode = this.pi5_mipi_camera_mode ? Pi5I2CMode.MipiCamera : Pi5I2CMode.ExternalSensor
+      const updated = await commander.setPi5I2CMode(mode)
+      if (updated) {
+        const message = 'Reboot is required for the Raspberry Pi I²C/MIPI change to take effect.'
+        notifier.pushInfo('DO_PI5_I2C_MODE_REBOOT_REQUIRED', message, true)
+      } else {
+        await this.updatePi5I2CMode()
+      }
+      this.updating_pi5_i2c_mode = false
+    },
     is_redirect_source(device: Device): boolean {
       return device.source === 'Redirect'
     },
-    openSettingsDialog(): void {
+    async openSettingsDialog(): Promise<void> {
       this.show_settings_dialog = true
+      if (this.is_raspberry_pi_5) {
+        await this.updatePi5I2CMode()
+      }
     },
     resetSettings(): void {
       video.resetSettings()

core/frontend/src/store/commander.ts

@@ -10,6 +10,11 @@ import back_axios, { isBackendOffline } from '@/utils/api'
 
 const notifier = new Notifier(commander_service)
 
+export enum Pi5I2CMode {
+  ExternalSensor = 'external_sensor',
+  MipiCamera = 'mipi_camera',
+}
+
 @Module({
   dynamic: true,
   store,
@@ -150,6 +155,45 @@ class CommanderStore extends VuexModule {
       })
   }
 
+  @Action
+  async getPi5I2CMode(): Promise<Pi5I2CMode | undefined> {
+    return back_axios({
+      method: 'get',
+      url: `${this.API_URL}/raspi_config/pi5_i2c_mode`,
+      timeout: 5000,
+    })
+      .then((response) => response.data?.mode)
+      .catch((error) => {
+        if (isBackendOffline(error)) {
+          return undefined
+        }
+        const message = 'Could not get Raspberry Pi 5 I²C/MIPI mode:'
+          + ` ${error.response?.data?.detail ?? error.message}.`
+        notifier.pushError('COMMANDER_GET_PI5_I2C_MODE_FAIL', message, true)
+        return undefined
+      })
+  }
+
+  @Action
+  async setPi5I2CMode(mode: Pi5I2CMode): Promise<boolean> {
+    return back_axios({
+      method: 'post',
+      url: `${this.API_URL}/raspi_config/pi5_i2c_mode`,
+      timeout: 5000,
+      params: { mode },
+    })
+      .then(() => true)
+      .catch((error) => {
+        if (isBackendOffline(error)) {
+          return false
+        }
+        const message = 'Could not set Raspberry Pi 5 I²C/MIPI mode:'
+          + ` ${error.response?.data?.detail ?? error.message}.`
+        notifier.pushError('COMMANDER_SET_PI5_I2C_MODE_FAIL', message, true)
+        return false
+      })
+  }
+
   @Action
   async getVcgencmd(): Promise<undefined | Record<string, ReturnStruct>> {
     return back_axios({

core/services/commander/main.py

@@ -11,8 +11,13 @@
 
 import appdirs
 from commonwealth.utils.apis import GenericErrorHandlingRoute
-from commonwealth.utils.commands import run_command
-from commonwealth.utils.general import delete_everything, delete_everything_stream
+from commonwealth.utils.commands import load_file, locate_file, run_command, save_file
+from commonwealth.utils.general import (
+    CpuType,
+    delete_everything,
+    delete_everything_stream,
+    get_cpu_type,
+)
 from commonwealth.utils.logs import InterceptHandler, init_logger
 from commonwealth.utils.sentry_config import init_sentry_async
 from commonwealth.utils.streaming import streamer
@@ -22,6 +27,12 @@
 from fastapi_versioning import VersionedFastAPI, version
 from filebrowser.filebrowser import filebrowser
 from loguru import logger
+from pi5_i2c import (
+    Pi5I2CConfigurationError,
+    Pi5I2CMode,
+    get_pi5_i2c_mode,
+    set_pi5_i2c_mode,
+)
 from uvicorn import Config, Server
 
 SERVICE_NAME = "commander"
@@ -131,6 +142,63 @@ async def raspi_config_camera_legacy_set(enable: bool = True) -> Any:
     return output
 
 
+def pi5_boot_config_file() -> str:
+    if get_cpu_type() != CpuType.PI5:
+        raise HTTPException(
+            status_code=status.HTTP_400_BAD_REQUEST,
+            detail="The I2C/MIPI mode is only available on Raspberry Pi 5.",
+        )
+    config_file = locate_file(["/boot/firmware/config.txt", "/boot/config.txt"])
+    if not config_file:
+        raise HTTPException(
+            status_code=status.HTTP_400_BAD_REQUEST,
+            detail="Raspberry Pi boot configuration was not found.",
+        )
+    return config_file
+
+
+@app.get("/raspi_config/pi5_i2c_mode", status_code=status.HTTP_200_OK)
+@version(1, 0)
+async def raspi_config_pi5_i2c_mode() -> Any:
+    try:
+        mode = get_pi5_i2c_mode(load_file(pi5_boot_config_file()))
+    except Pi5I2CConfigurationError as error:
+        raise HTTPException(
+            status_code=status.HTTP_400_BAD_REQUEST, detail=str(error)
+        ) from error
+    return {"mode": mode}
+
+
+@app.post("/raspi_config/pi5_i2c_mode", status_code=status.HTTP_200_OK)
+@version(1, 0)
+async def raspi_config_pi5_i2c_mode_set(mode: Pi5I2CMode) -> Any:
+    config_file = pi5_boot_config_file()
+    config_content = load_file(config_file)
+    try:
+        updated_content = set_pi5_i2c_mode(config_content, mode)
+    except Pi5I2CConfigurationError as error:
+        raise HTTPException(
+            status_code=status.HTTP_400_BAD_REQUEST, detail=str(error)
+        ) from error
+
+    reboot_required = updated_content != config_content
+    if reboot_required:
+        save_file(config_file, updated_content, "before_pi5_i2c_mode")
+        try:
+            saved_mode = get_pi5_i2c_mode(load_file(config_file))
+        except Pi5I2CConfigurationError as error:
+            raise HTTPException(
+                status_code=status.HTTP_500_INTERNAL_SERVER_ERROR, detail=str(error)
+            ) from error
+        if saved_mode != mode:
+            raise HTTPException(
+                status_code=status.HTTP_500_INTERNAL_SERVER_ERROR,
+                detail="Raspberry Pi I2C/MIPI mode could not be saved.",
+            )
+
+    return {"mode": mode, "reboot_required": reboot_required}
+
+
 @app.get("/raspi/vcgencmd", status_code=status.HTTP_200_OK)
 @version(1, 0)
 async def vcgencmd(i_know_what_i_am_doing: bool = False) -> Any:

core/services/commander/pi5_i2c.py

@@ -0,0 +1,91 @@
+import re
+from enum import Enum
+
+
+class Pi5I2CMode(str, Enum):
+    EXTERNAL_SENSOR = "external_sensor"
+    MIPI_CAMERA = "mipi_camera"
+
+
+class Pi5I2CConfigurationError(ValueError):
+    pass
+
+
+def _navigator_i2c_overlay(config_content: str) -> tuple[int, int]:
+    section = "all"
+    matches: list[tuple[int, int]] = []
+
+    for index, line in enumerate(config_content.splitlines()):
+        section_match = re.match(r"^\s*\[([^]]+)]", line)
+        if section_match:
+            section = section_match.group(1).strip().lower()
+            continue
+        if section != "pi5":
+            continue
+
+        configuration = line.partition("#")[0].strip()
+        if not configuration.startswith("dtoverlay=i2c-gpio,"):
+            continue
+
+        parameters = {
+            key.strip(): value.strip()
+            for parameter in configuration.split(",")[1:]
+            if "=" in parameter
+            for key, value in [parameter.split("=", maxsplit=1)]
+        }
+        if (
+            parameters.get("i2c_gpio_sda") != "22"
+            or parameters.get("i2c_gpio_scl") != "23"
+        ):
+            continue
+
+        try:
+            bus = int(parameters["bus"])
+        except (KeyError, ValueError) as error:
+            raise Pi5I2CConfigurationError(
+                "Navigator I2C overlay has no valid bus number."
+            ) from error
+        matches.append((index, bus))
+
+    if not matches:
+        raise Pi5I2CConfigurationError(
+            "Navigator I2C overlay was not found in the [pi5] section."
+        )
+    if len(matches) > 1:
+        raise Pi5I2CConfigurationError(
+            "Multiple Navigator I2C overlays were found in the [pi5] section."
+        )
+    return matches[0]
+
+
+def get_pi5_i2c_mode(config_content: str) -> Pi5I2CMode:
+    _, bus = _navigator_i2c_overlay(config_content)
+    if bus == 6:
+        return Pi5I2CMode.EXTERNAL_SENSOR
+    if bus == 8:
+        return Pi5I2CMode.MIPI_CAMERA
+    raise Pi5I2CConfigurationError(f"Navigator I2C overlay uses unsupported bus {bus}.")
+
+
+def set_pi5_i2c_mode(config_content: str, mode: Pi5I2CMode) -> str:
+    line_index, _ = _navigator_i2c_overlay(config_content)
+    if mode == Pi5I2CMode.EXTERNAL_SENSOR:
+        bus = 6
+    elif mode == Pi5I2CMode.MIPI_CAMERA:
+        bus = 8
+    else:
+        raise Pi5I2CConfigurationError(f"Unsupported Raspberry Pi 5 I2C mode: {mode}.")
+    lines = config_content.splitlines(keepends=True)
+    configuration, comment_separator, comment = lines[line_index].partition("#")
+    updated_configuration, replacements = re.subn(
+        r"(?P<prefix>(?:^|,)\s*bus\s*=\s*)\d+",
+        rf"\g<prefix>{bus}",
+        configuration,
+        count=1,
+    )
+    if replacements != 1:
+        raise Pi5I2CConfigurationError(
+            "Navigator I2C bus parameter could not be updated."
+        )
+    lines[line_index] = updated_configuration + comment_separator + comment
+    return "".join(lines)