From 8d757758645e6bf42387d4cc0f728e57191a12fc Mon Sep 17 00:00:00 2001 From: Daniil Mordanov <153565951+Daniiiil1@users.noreply.github.com> Date: Wed, 5 Aug 2026 00:33:42 +0700 Subject: [PATCH] Add Raspberry Pi 5 I2C camera mode --- .../components/video-manager/VideoManager.vue | 51 ++++++++++- core/frontend/src/store/commander.ts | 44 +++++++++ core/services/commander/main.py | 72 ++++++++++++++- core/services/commander/pi5_i2c.py | 91 +++++++++++++++++++ core/services/commander/test_pi5_i2c.py | 78 ++++++++++++++++ install/boards/bcm_2712.sh | 13 ++- 6 files changed, 344 insertions(+), 5 deletions(-) create mode 100644 core/services/commander/pi5_i2c.py create mode 100644 core/services/commander/test_pi5_i2c.py diff --git a/core/frontend/src/components/video-manager/VideoManager.vue b/core/frontend/src/components/video-manager/VideoManager.vue index c872ea3c7b..eb121b95ce 100644 --- a/core/frontend/src/components/video-manager/VideoManager.vue +++ b/core/frontend/src/components/video-manager/VideoManager.vue @@ -68,6 +68,26 @@ label="Raspberry legacy camera support" @change="toggleLegacyMode" /> + + + 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. + @@ -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 { @@ -178,11 +207,29 @@ export default Vue.extend({ async toggleLegacyMode(): Promise { await this.setCameraLegacy(this.legacy_mode) }, + async updatePi5I2CMode(): Promise { + this.pi5_mipi_camera_mode = await commander.getPi5I2CMode() === Pi5I2CMode.MipiCamera + }, + async togglePi5MipiCameraMode(): Promise { + 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 { this.show_settings_dialog = true + if (this.is_raspberry_pi_5) { + await this.updatePi5I2CMode() + } }, resetSettings(): void { video.resetSettings() diff --git a/core/frontend/src/store/commander.ts b/core/frontend/src/store/commander.ts index ab5c505408..71a67b40c1 100644 --- a/core/frontend/src/store/commander.ts +++ b/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 { + 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 { + 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> { return back_axios({ diff --git a/core/services/commander/main.py b/core/services/commander/main.py index 044e37235c..98393e7984 100755 --- a/core/services/commander/main.py +++ b/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: diff --git a/core/services/commander/pi5_i2c.py b/core/services/commander/pi5_i2c.py new file mode 100644 index 0000000000..bc80fe3059 --- /dev/null +++ b/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(?:^|,)\s*bus\s*=\s*)\d+", + rf"\g{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) diff --git a/core/services/commander/test_pi5_i2c.py b/core/services/commander/test_pi5_i2c.py new file mode 100644 index 0000000000..0f3ceaf6fd --- /dev/null +++ b/core/services/commander/test_pi5_i2c.py @@ -0,0 +1,78 @@ +import pytest +from pi5_i2c import ( + Pi5I2CConfigurationError, + Pi5I2CMode, + get_pi5_i2c_mode, + set_pi5_i2c_mode, +) + +SENSOR_CONFIGURATION = """[all] +dtparam=i2c_arm=on + +[pi5] +dtoverlay=i2c-gpio,i2c_gpio_sda=22,i2c_gpio_scl=23,bus=6,i2c_gpio_delay_us=0 +""" + + +def test_get_pi5_i2c_mode(): + assert get_pi5_i2c_mode(SENSOR_CONFIGURATION) == Pi5I2CMode.EXTERNAL_SENSOR + camera_configuration = SENSOR_CONFIGURATION.replace("bus=6", "bus=8") + assert get_pi5_i2c_mode(camera_configuration) == Pi5I2CMode.MIPI_CAMERA + + +def test_set_pi5_i2c_mode_preserves_other_configuration(): + protected_configuration = SENSOR_CONFIGURATION.replace( + "i2c_gpio_delay_us=0", "i2c_gpio_delay_us=0 # custom" + ) + + camera_configuration = set_pi5_i2c_mode( + protected_configuration, Pi5I2CMode.MIPI_CAMERA + ) + + assert "bus=8" in camera_configuration + assert "i2c_gpio_delay_us=0 # custom" in camera_configuration + assert camera_configuration.replace("bus=8", "bus=6") == protected_configuration + + +def test_set_pi5_i2c_mode_only_changes_pi5_section(): + configuration = SENSOR_CONFIGURATION.replace( + "[all]", + "[all]\ndtoverlay=i2c-gpio,i2c_gpio_sda=22,i2c_gpio_scl=23,bus=4,i2c_gpio_delay_us=0", + ) + + camera_configuration = set_pi5_i2c_mode(configuration, Pi5I2CMode.MIPI_CAMERA) + + assert "bus=4" in camera_configuration + assert camera_configuration.count("bus=8") == 1 + + +def test_set_pi5_i2c_mode_preserves_crlf_and_accepts_spaced_section(): + configuration = SENSOR_CONFIGURATION.replace("[pi5]", "[ pi5 ]").replace( + "\n", "\r\n" + ) + + camera_configuration = set_pi5_i2c_mode(configuration, Pi5I2CMode.MIPI_CAMERA) + + assert camera_configuration.count("\r\n") == configuration.count("\r\n") + assert "bus=8" in camera_configuration + + +def test_set_pi5_i2c_mode_rejects_unknown_mode(): + with pytest.raises(Pi5I2CConfigurationError): + set_pi5_i2c_mode(SENSOR_CONFIGURATION, "automatic") # type: ignore[arg-type] + + +@pytest.mark.parametrize( + "configuration", + [ + "[pi5]\ndtoverlay=i2c1\n", + SENSOR_CONFIGURATION.replace(",bus=6", ""), + SENSOR_CONFIGURATION.replace("bus=6", "bus=7"), + SENSOR_CONFIGURATION + SENSOR_CONFIGURATION.split("[pi5]\n", maxsplit=1)[1], + ], +) +def test_get_pi5_i2c_mode_rejects_ambiguous_or_unsupported_configuration( + configuration: str, +): + with pytest.raises(Pi5I2CConfigurationError): + get_pi5_i2c_mode(configuration) diff --git a/install/boards/bcm_2712.sh b/install/boards/bcm_2712.sh index 0c505718fe..83f18c7e50 100644 --- a/install/boards/bcm_2712.sh +++ b/install/boards/bcm_2712.sh @@ -20,8 +20,19 @@ DTS_NAME="spi0-led" curl -fsSL -o /tmp/$DTS_NAME $DTS_PATH/$DTS_NAME.dts dtc -@ -Hepapr -I dts -O dtb -o /boot/overlays/$DTS_NAME.dtbo /tmp/$DTS_NAME +# Preserve the Pi 5 I2C/MIPI mode selected in Commander across BlueOS updates. +NAVIGATOR_I2C_BUS=6 +if grep -E '^[[:space:]]*dtoverlay=i2c-gpio,' "$CONFIG_FILE" \ + | grep -E '(^|,)[[:space:]]*i2c_gpio_sda[[:space:]]*=[[:space:]]*22([,[:space:]#]|$)' \ + | grep -E '(^|,)[[:space:]]*i2c_gpio_scl[[:space:]]*=[[:space:]]*23([,[:space:]#]|$)' \ + | grep -Eq '(^|,)[[:space:]]*bus[[:space:]]*=[[:space:]]*8([,[:space:]#]|$)'; then + NAVIGATOR_I2C_BUS=8 +fi + # Remove any configuration related to i2c and spi/spi1 and do the necessary changes for navigator echo "- Enable I2C, SPI and UART." +# ArduPilot expects Navigator external sensors on bus 6. Commander can move this software bus to 8 +# when the Pi 5 MIPI camera needs the hardware i2c6 controller. for STRING in \ "enable_uart=" \ "dtoverlay=uart" \ @@ -55,7 +66,7 @@ for STRING in \ "dtoverlay=i2c1" \ "dtoverlay=i2c3-pi5,baudrate=400000" \ "dtoverlay=i2c3-pi5.baudrate=400000" \ - "dtoverlay=i2c-gpio,i2c_gpio_sda=22,i2c_gpio_scl=23,bus=6,i2c_gpio_delay_us=0" \ + "dtoverlay=i2c-gpio,i2c_gpio_sda=22,i2c_gpio_scl=23,bus=$NAVIGATOR_I2C_BUS,i2c_gpio_delay_us=0" \ "dtparam=spi=on" \ "dtoverlay=spi0-led" \ "dtoverlay=spi1-3cs" \