Merge branch 'master' into custom-esp32

* Make sure to call randomSeed() on esp32

* Randomize the top 22 bits of the Message ID

* Make it clear that we are not calling randomSeed() on purpose

---------

Co-authored-by: Ben Meadors <benmmeadors@gmail.com>

.github/ISSUE_TEMPLATE/Bug Report.yml

@@ -52,6 +52,7 @@ body:
         - Raspberry Pi Pico (W)
         - Relay v1
         - Relay v2
+        - Seeed Wio Tracker 1110
         - DIY
         - Other
     validations:

.github/actions/setup-base/action.yml

@@ -11,23 +11,11 @@ runs:
         ref: ${{github.event.pull_request.head.ref}}
         repository: ${{github.event.pull_request.head.repo.full_name}}
 
-    - name: Install cppcheck
+    - name: Install dependencies 
       shell: bash
       run: |
-        sudo apt-get install -y cppcheck
-
-    - name: Install libbluetooth
-      shell: bash
-      run: |
-        sudo apt-get install -y libbluetooth-dev
-    - name: Install libgpiod
-      shell: bash
-      run: |
-        sudo apt-get install -y libgpiod-dev
-    - name: Install libyaml-cpp
-      shell: bash
-      run: |
-        sudo apt-get install -y libyaml-cpp-dev
+        sudo apt-get -y update --fix-missing
+        sudo apt-get install -y cppcheck libbluetooth-dev libgpiod-dev libyaml-cpp-dev
 
     - name: Setup Python
       uses: actions/setup-python@v5

.github/workflows/build_native.yml

@@ -0,0 +1,85 @@
+name: Build Native
+
+on: workflow_call
+
+permissions:
+  contents: write
+  packages: write
+
+jobs:
+  build-native:
+    runs-on: ubuntu-latest
+    steps:
+      - name: Install libbluetooth
+        shell: bash
+        run: |
+          sudo apt-get update --fix-missing
+          sudo apt-get install -y libbluetooth-dev libgpiod-dev libyaml-cpp-dev openssl libssl-dev libulfius-dev liborcania-dev
+
+      - name: Checkout code
+        uses: actions/checkout@v4
+        with:
+          submodules: recursive
+          ref: ${{github.event.pull_request.head.ref}}
+          repository: ${{github.event.pull_request.head.repo.full_name}}
+
+      - name: Upgrade python tools
+        shell: bash
+        run: |
+          python -m pip install --upgrade pip
+          pip install -U platformio adafruit-nrfutil
+          pip install -U meshtastic --pre
+
+      - name: Upgrade platformio
+        shell: bash
+        run: |
+          pio upgrade
+
+      - name: Build Native
+        run: bin/build-native.sh
+
+      - name: Get release version string
+        run: echo "version=$(./bin/buildinfo.py long)" >> $GITHUB_OUTPUT
+        id: version
+
+      - name: Store binaries as an artifact
+        uses: actions/upload-artifact@v4
+        with:
+          name: firmware-native-${{ steps.version.outputs.version }}.zip
+          overwrite: true
+          path: |
+            release/meshtasticd_linux_x86_64
+            bin/config-dist.yaml
+
+      - name: Docker login
+        if: ${{ github.event_name != 'pull_request_target' && github.event_name != 'pull_request' }}
+        uses: docker/login-action@v3
+        continue-on-error: true # FIXME: Failing docker login auth
+        with:
+          username: meshtastic
+          password: ${{ secrets.DOCKER_FIRMWARE_TOKEN }}
+
+      - name: Docker setup
+        if: ${{ github.event_name != 'pull_request_target' && github.event_name != 'pull_request' }}
+        continue-on-error: true # FIXME: Failing docker login auth
+        uses: docker/setup-buildx-action@v3
+
+      - name: Docker build and push tagged versions
+        if: ${{ github.event_name == 'workflow_dispatch' }}
+        continue-on-error: true # FIXME: Failing docker login auth
+        uses: docker/build-push-action@v5
+        with:
+          context: .
+          file: ./Dockerfile
+          push: true
+          tags: meshtastic/device-simulator:${{ steps.version.outputs.version }}
+
+      - name: Docker build and push
+        if: ${{ github.ref == 'refs/heads/master' && github.event_name != 'pull_request_target' && github.event_name != 'pull_request' }}
+        continue-on-error: true # FIXME: Failing docker login auth
+        uses: docker/build-push-action@v5
+        with:
+          context: .
+          file: ./Dockerfile
+          push: true
+          tags: meshtastic/device-simulator:latest

.github/workflows/build_nrf52.yml

@@ -29,6 +29,7 @@ jobs:
           name: firmware-${{ inputs.board }}-${{ steps.version.outputs.version }}.zip
           overwrite: true
           path: |
+            release/*.hex
             release/*.uf2
             release/*.elf
             release/*.zip

.github/workflows/build_raspbian.yml

@@ -13,6 +13,7 @@ jobs:
       - name: Install libbluetooth
         shell: bash
         run: |
+          apt-get update -y --fix-missing
           apt-get install -y libbluetooth-dev libgpiod-dev libyaml-cpp-dev openssl libssl-dev libulfius-dev liborcania-dev
 
       - name: Checkout code

.github/workflows/build_raspbian_armv7l.yml

@@ -13,6 +13,7 @@ jobs:
       - name: Install libbluetooth
         shell: bash
         run: |
+          apt-get update -y --fix-missing
           apt-get install -y libbluetooth-dev libgpiod-dev libyaml-cpp-dev openssl libssl-dev libulfius-dev liborcania-dev
 
       - name: Checkout code

.github/workflows/main_matrix.yml

@@ -109,67 +109,8 @@ jobs:
   package-raspbian-armv7l:
     uses: ./.github/workflows/package_raspbian_armv7l.yml
 
-  build-native:
-    runs-on: ubuntu-latest
-    steps:
-      - uses: actions/checkout@v4
-      - name: Build base
-        id: base
-        uses: ./.github/actions/setup-base
-
-      # We now run integration test before other build steps (to quickly see runtime failures)
-      #- name: Build for native
-      #  run: platformio run -e native
-      #- name: Integration test
-      # run: |
-      #.pio/build/native/program
-      #& sleep 20 # 5 seconds was not enough
-      #echo "Simulator started, launching python test..."
-      #python3 -c 'from meshtastic.test import testSimulator; testSimulator()'
-
-      - name: Build Native
-        run: bin/build-native.sh
-
-      - name: Get release version string
-        run: echo "version=$(./bin/buildinfo.py long)" >> $GITHUB_OUTPUT
-        id: version
-
-      - name: Store binaries as an artifact
-        uses: actions/upload-artifact@v4
-        with:
-          name: firmware-native-${{ steps.version.outputs.version }}.zip
-          overwrite: true
-          path: |
-            release/device-*.sh
-            release/device-*.bat
-
-      - name: Docker login
-        if: ${{ github.event_name != 'pull_request_target' && github.event_name != 'pull_request' }}
-        uses: docker/login-action@v3
-        with:
-          username: meshtastic
-          password: ${{ secrets.DOCKER_TOKEN }}
-      - name: Docker setup
-        if: ${{ github.event_name != 'pull_request_target' && github.event_name != 'pull_request' }}
-        uses: docker/setup-buildx-action@v3
-
-      - name: Docker build and push tagged versions
-        if: ${{ github.event_name == 'workflow_dispatch' }}
-        uses: docker/build-push-action@v5
-        with:
-          context: .
-          file: ./Dockerfile
-          push: true
-          tags: meshtastic/device-simulator:${{ steps.version.outputs.version }}
-
-      - name: Docker build and push
-        if: ${{ github.ref == 'refs/heads/master' && github.event_name != 'pull_request_target' && github.event_name != 'pull_request' }}
-        uses: docker/build-push-action@v5
-        with:
-          context: .
-          file: ./Dockerfile
-          push: true
-          tags: meshtastic/device-simulator:latest
+  package-native:
+    uses: ./.github/workflows/package_amd64.yml
 
   after-checks:
     runs-on: ubuntu-latest
@@ -192,10 +133,10 @@ jobs:
         build-esp32-s3,
         build-esp32-c3,
         build-nrf52,
-        build-native,
         build-rpi2040,
         package-raspbian,
         package-raspbian-armv7l,
+        package-native,
       ]
     steps:
       - name: Checkout code
@@ -217,7 +158,7 @@ jobs:
         id: version
 
       - name: Move files up
-        run: mv -b -t ./ ./release/meshtasticd_linux_aarch64 ./release/meshtasticd_linux_armv7l ./bin/config-dist.yaml
+        run: mv -b -t ./ ./release/meshtasticd_linux_* ./bin/config-dist.yaml ./bin/device-*.sh ./bin/device-*.bat
 
       - name: Repackage in single firmware zip
         uses: actions/upload-artifact@v4
@@ -227,6 +168,7 @@ jobs:
           path: |
             ./firmware-*.bin
             ./firmware-*.uf2
+            ./firmware-*.hex
             ./firmware-*-ota.zip
             ./device-*.sh
             ./device-*.bat
@@ -313,7 +255,7 @@ jobs:
           chmod +x ./output/device-update.sh
 
       - name: Zip firmware
-        run: zip -j -9 -r ./firmware-${{ steps.version.outputs.version }}.zip ./output
+        run: zip -j -9 -r ./firmware-${{ steps.version.outputs.version }}.zip ./output -x *.deb
 
       - uses: actions/download-artifact@v4
         with:
@@ -381,6 +323,16 @@ jobs:
           asset_name: meshtasticd_${{ steps.version.outputs.version }}_armhf.deb
           asset_content_type: application/vnd.debian.binary-package
 
+      - name: Add raspbian amd64 .deb
+        uses: actions/upload-release-asset@v1
+        env:
+          GITHUB_TOKEN: ${{ github.token }}
+        with:
+          upload_url: ${{ steps.create_release.outputs.upload_url }}
+          asset_path: ./output/meshtasticd_${{ steps.version.outputs.version }}_amd64.deb
+          asset_name: meshtasticd_${{ steps.version.outputs.version }}_amd64.deb
+          asset_content_type: application/vnd.debian.binary-package
+
       - name: Bump version.properties
         run: >-
           bin/bump_version.py

.github/workflows/package_amd64.yml

@@ -0,0 +1,78 @@
+name: Package Native
+
+on:
+  workflow_call:
+  workflow_dispatch:
+
+permissions:
+  contents: write
+  packages: write
+
+jobs:
+  build-native:
+    uses: ./.github/workflows/build_native.yml
+
+  package-native:
+    runs-on: ubuntu-latest
+    needs: build-native
+    steps:
+      - name: Checkout code
+        uses: actions/checkout@v4
+        with:
+          submodules: recursive
+          ref: ${{github.event.pull_request.head.ref}}
+          repository: ${{github.event.pull_request.head.repo.full_name}}
+
+      - name: Pull web ui
+        uses: dsaltares/fetch-gh-release-asset@master
+        with:
+          repo: meshtastic/web
+          file: build.tar
+          target: build.tar
+          token: ${{ secrets.GITHUB_TOKEN }}
+
+      - name: Get release version string
+        run: echo "version=$(./bin/buildinfo.py long)" >> $GITHUB_OUTPUT
+        id: version
+
+      - name: Download artifacts
+        uses: actions/download-artifact@v4
+        with:
+          name: firmware-native-${{ steps.version.outputs.version }}.zip
+          merge-multiple: true
+
+      - name: Display structure of downloaded files
+        run: ls -R
+
+      - name: build .debpkg
+        run: |
+          mkdir -p .debpkg/DEBIAN
+          mkdir -p .debpkg/usr/share/doc/meshtasticd/web
+          mkdir -p .debpkg/usr/sbin
+          mkdir -p .debpkg/etc/meshtasticd
+          mkdir -p .debpkg/usr/lib/systemd/system/
+          tar -xf build.tar -C .debpkg/usr/share/doc/meshtasticd/web
+          gunzip .debpkg/usr/share/doc/meshtasticd/web/*.gz
+          cp release/meshtasticd_linux_x86_64 .debpkg/usr/sbin/meshtasticd
+          cp bin/config-dist.yaml .debpkg/etc/meshtasticd/config.yaml
+          chmod +x .debpkg/usr/sbin/meshtasticd
+          cp bin/meshtasticd.service .debpkg/usr/lib/systemd/system/meshtasticd.service
+          echo "/etc/meshtasticd/config.yaml" > .debpkg/DEBIAN/conffiles
+          chmod +x .debpkg/DEBIAN/conffiles
+
+      - uses: jiro4989/build-deb-action@v3
+        with:
+          package: meshtasticd
+          package_root: .debpkg
+          maintainer: Jonathan Bennett
+          version: ${{ steps.version.outputs.version }} # refs/tags/v*.*.*
+          arch: amd64
+          depends: libyaml-cpp0.7, openssl, libulfius2.7
+          desc: Native Linux Meshtastic binary.
+
+      - uses: actions/upload-artifact@v4
+        with:
+          name: meshtasticd_${{ steps.version.outputs.version }}_amd64.deb
+          overwrite: true
+          path: |
+            ./*.deb

.semgrepignore

@@ -1,2 +1,2 @@
 .github/workflows/main_matrix.yml
-src/mesh/compression/unishox2.c
+src/mesh/compression/unishox2.cpp

.trunk/configs/.bandit

@@ -0,0 +1,2 @@
+[bandit]
+skips = B101

.trunk/trunk.yaml

@@ -1,6 +1,6 @@
 version: 0.1
 cli:
-  version: 1.22.1
+  version: 1.22.2
 plugins:
   sources:
     - id: trunk
@@ -31,6 +31,10 @@ lint:
     - gitleaks@8.18.2
     - clang-format@16.0.3
     - prettier@3.2.5
+  ignore:
+    - linters: [ALL]
+      paths:
+        - bin/**
 runtimes:
   enabled:
     - python@3.10.8

.vscode/extensions.json

@@ -2,7 +2,7 @@
     // See http://go.microsoft.com/fwlink/?LinkId=827846
     // for the documentation about the extensions.json format
     "recommendations": [
-        "ms-vscode.cpptools",
+		"ms-vscode.cpptools",
         "platformio.platformio-ide",
         "trunk.io"
     ],

.vscode/settings.json

@@ -3,5 +3,9 @@
   "editor.defaultFormatter": "trunk.io",
   "trunk.enableWindows": true,
   "files.insertFinalNewline": false,
-  "files.trimFinalNewlines": false
+  "files.trimFinalNewlines": false,
+  "cmake.configureOnOpen": false,
+  "[cpp]": {
+    "editor.defaultFormatter": "trunk.io"
+  }
 }

Dockerfile

@@ -7,8 +7,6 @@ ENV TZ=Etc/UTC
 # > At the moment, setting "LANG=C" on a Linux system *fundamentally breaks Python 3*, and that's not OK.
 ENV LANG C.UTF-8
 
-SHELL ["/bin/bash", "-o", "pipefail", "-c"]
-
 # Install build deps
 USER root
 
@@ -24,10 +22,10 @@ USER mesh
 
 WORKDIR /tmp/firmware
 RUN python3 -m venv /tmp/firmware 
-RUN source ./bin/activate && pip3 install --no-cache-dir -U platformio==6.1.14
+RUN bash -o pipefail -c "source bin/activate; pip3 install --no-cache-dir -U platformio==6.1.15"
 # trunk-ignore(terrascan/AC_DOCKER_00024): We would actually like these files to be owned by mesh tyvm
 COPY --chown=mesh:mesh . /tmp/firmware
-RUN source ./bin/activate && chmod +x /tmp/firmware/bin/build-native.sh && ./bin/build-native.sh
+RUN bash -o pipefail -c "source ./bin/activate && bash ./bin/build-native.sh"
 RUN cp "/tmp/firmware/release/meshtasticd_linux_$(uname -m)" "/tmp/firmware/release/meshtasticd"
 
 
@@ -48,6 +46,7 @@ USER mesh
 WORKDIR /home/mesh
 COPY --from=builder /tmp/firmware/release/meshtasticd /home/mesh/
 
+RUN mkdir data
 VOLUME /home/mesh/data
 
 CMD [ "sh",  "-cx", "./meshtasticd -d /home/mesh/data --hwid=${HWID:-$RANDOM}" ]

arch/esp32/esp32.ini

@@ -44,7 +44,7 @@ lib_deps =
   ${networking_base.lib_deps}
   ${environmental_base.lib_deps}
   https://github.com/meshtastic/esp32_https_server.git#23665b3adc080a311dcbb586ed5941b5f94d6ea2
-  h2zero/NimBLE-Arduino@^1.4.1
+  h2zero/NimBLE-Arduino@^1.4.2
   https://github.com/dbSuS/libpax.git#7bcd3fcab75037505be9b122ab2b24cc5176b587
   https://github.com/lewisxhe/XPowersLib.git#84b7373faea3118b6c37954d52f98b8a337148d6
   https://github.com/meshtastic/ESP32_Codec2.git#633326c78ac251c059ab3a8c430fcdf25b41672f

arch/nrf52/nrf52.ini

@@ -1,6 +1,6 @@
 [nrf52_base]
 ; Instead of the standard nordicnrf52 platform, we use our fork which has our added variant files
-platform = platformio/nordicnrf52@^10.4.0
+platform = platformio/nordicnrf52@^10.5.0
 extends = arduino_base
 
 build_type = debug

bin/build-nrf52.sh

@@ -2,8 +2,8 @@
 
 set -e
 
-VERSION=`bin/buildinfo.py long`
-SHORT_VERSION=`bin/buildinfo.py short`
+VERSION=$(bin/buildinfo.py long)
+SHORT_VERSION=$(bin/buildinfo.py short)
 
 OUTDIR=release/
 
@@ -11,7 +11,7 @@ rm -f $OUTDIR/firmware*
 rm -r $OUTDIR/* || true
 
 # Important to pull latest version of libs into all device flavors, otherwise some devices might be stale
-platformio pkg update 
+platformio pkg update
 
 echo "Building for $1 with $PLATFORMIO_BUILD_FLAGS"
 rm -f .pio/build/$1/firmware.*
@@ -23,14 +23,26 @@ basename=firmware-$1-$VERSION
 
 pio run --environment $1 # -v
 SRCELF=.pio/build/$1/firmware.elf
-DFUPKG=.pio/build/$1/firmware.zip
 cp $SRCELF $OUTDIR/$basename.elf
+
+echo "Generating NRF52 dfu file"
+DFUPKG=.pio/build/$1/firmware.zip
 cp $DFUPKG $OUTDIR/$basename-ota.zip
 
 echo "Generating NRF52 uf2 file"
 SRCHEX=.pio/build/$1/firmware.hex
-bin/uf2conv.py $SRCHEX -c -o $OUTDIR/$basename.uf2 -f 0xADA52840
 
-cp bin/device-install.* $OUTDIR
-cp bin/device-update.* $OUTDIR
-cp bin/*.uf2 $OUTDIR
+# if WM1110 target, merge hex with softdevice 7.3.0
+if (echo $1 | grep -q "wio-sdk-wm1110"); then
+	echo "Merging with softdevice"
+	bin/mergehex -m bin/s140_nrf52_7.3.0_softdevice.hex $SRCHEX -o .pio/build/$1/$basename.hex
+	SRCHEX=.pio/build/$1/$basename.hex
+	bin/uf2conv.py $SRCHEX -c -o $OUTDIR/$basename.uf2 -f 0xADA52840
+	cp $SRCHEX $OUTDIR
+	cp bin/*.uf2 $OUTDIR
+else
+	bin/uf2conv.py $SRCHEX -c -o $OUTDIR/$basename.uf2 -f 0xADA52840
+	cp bin/device-install.* $OUTDIR
+	cp bin/device-update.* $OUTDIR
+	cp bin/*.uf2 $OUTDIR
+fi

bin/platformio-custom.py

@@ -7,6 +7,7 @@ from readprops import readProps
 
 Import("env")
 platform = env.PioPlatform()
+board = env.GetProjectOption("board")
 
 
 def esp32_create_combined_bin(source, target, env):
@@ -77,6 +78,9 @@ if platform.name == "espressif32":
     else:
         # For newer ESP32 targets, using newlib nano works better.
         env.Append(LINKFLAGS=["--specs=nano.specs", "-u", "_printf_float"])
+    if board == "ttgo-t-beam":
+        print("patching esp32 libs")
+        env.Execute("tar -xvf bin/arduino-esp32-libs-release_*tar.gz -C ~/.platformio/packages/framework-arduinoespressif32/")
 
 Import("projenv")
 
@@ -90,4 +94,4 @@ projenv.Append(
         "-DAPP_VERSION=" + verObj["long"],
         "-DAPP_VERSION_SHORT=" + verObj["short"],
     ]
-)
+)

bin/setup-python-for-esp-debug.sh

@@ -0,0 +1,12 @@
+# shellcheck shell=bash
+# (this minor script is actually shell agnostic, and is intended to be sourced rather than run in a subshell)
+
+# This is a little script you can source if you want to make ESP debugging work on a modern (24.04) ubuntu machine
+# It assumes you have built and installed python 2.7 from source with:
+# ./configure --enable-optimizations --enable-shared --enable-unicode=ucs4
+# sudo make clean
+# make
+# sudo make altinstall
+
+export LD_LIBRARY_PATH=$HOME/packages/python-2.7.18/
+export PYTHON_HOME=/usr/local/lib/python2.7/

bin/uf2conv.py

@@ -1,39 +1,38 @@
 #!/usr/bin/env python3
-import sys
-import struct
-import subprocess
-import re
+import argparse
 import os
 import os.path
-import argparse
+import re
+import struct
+import subprocess
+import sys
 
-
-UF2_MAGIC_START0 = 0x0A324655 # "UF2\n"
-UF2_MAGIC_START1 = 0x9E5D5157 # Randomly selected
-UF2_MAGIC_END    = 0x0AB16F30 # Ditto
+UF2_MAGIC_START0 = 0x0A324655  # "UF2\n"
+UF2_MAGIC_START1 = 0x9E5D5157  # Randomly selected
+UF2_MAGIC_END = 0x0AB16F30  # Ditto
 
 families = {
-    'SAMD21': 0x68ed2b88,
-    'SAML21': 0x1851780a,
-    'SAMD51': 0x55114460,
-    'NRF52': 0x1b57745f,
-    'STM32F0': 0x647824b6,
-    'STM32F1': 0x5ee21072,
-    'STM32F2': 0x5d1a0a2e,
-    'STM32F3': 0x6b846188,
-    'STM32F4': 0x57755a57,
-    'STM32F7': 0x53b80f00,
-    'STM32G0': 0x300f5633,
-    'STM32G4': 0x4c71240a,
-    'STM32H7': 0x6db66082,
-    'STM32L0': 0x202e3a91,
-    'STM32L1': 0x1e1f432d,
-    'STM32L4': 0x00ff6919,
-    'STM32L5': 0x04240bdf,
-    'STM32WB': 0x70d16653,
-    'STM32WL': 0x21460ff0,
-    'ATMEGA32': 0x16573617,
-    'MIMXRT10XX': 0x4FB2D5BD
+    "SAMD21": 0x68ED2B88,
+    "SAML21": 0x1851780A,
+    "SAMD51": 0x55114460,
+    "NRF52": 0x1B57745F,
+    "STM32F0": 0x647824B6,
+    "STM32F1": 0x5EE21072,
+    "STM32F2": 0x5D1A0A2E,
+    "STM32F3": 0x6B846188,
+    "STM32F4": 0x57755A57,
+    "STM32F7": 0x53B80F00,
+    "STM32G0": 0x300F5633,
+    "STM32G4": 0x4C71240A,
+    "STM32H7": 0x6DB66082,
+    "STM32L0": 0x202E3A91,
+    "STM32L1": 0x1E1F432D,
+    "STM32L4": 0x00FF6919,
+    "STM32L5": 0x04240BDF,
+    "STM32WB": 0x70D16653,
+    "STM32WL": 0x21460FF0,
+    "ATMEGA32": 0x16573617,
+    "MIMXRT10XX": 0x4FB2D5BD,
 }
 
 INFO_FILE = "/INFO_UF2.TXT"
@@ -46,15 +45,17 @@ def is_uf2(buf):
     w = struct.unpack("<II", buf[0:8])
     return w[0] == UF2_MAGIC_START0 and w[1] == UF2_MAGIC_START1
 
+
 def is_hex(buf):
     try:
         w = buf[0:30].decode("utf-8")
     except UnicodeDecodeError:
         return False
-    if w[0] == ':' and re.match(b"^[:0-9a-fA-F\r\n]+$", buf):
+    if w[0] == ":" and re.match(b"^[:0-9a-fA-F\r\n]+$", buf):
         return True
     return False
 
+
 def convert_from_uf2(buf):
     global appstartaddr
     numblocks = len(buf) // 512
@@ -62,7 +63,7 @@ def convert_from_uf2(buf):
     outp = b""
     for blockno in range(numblocks):
         ptr = blockno * 512
-        block = buf[ptr:ptr + 512]
+        block = buf[ptr : ptr + 512]
         hd = struct.unpack(b"<IIIIIIII", block[0:32])
         if hd[0] != UF2_MAGIC_START0 or hd[1] != UF2_MAGIC_START1:
             print("Skipping block at " + ptr + "; bad magic")
@@ -80,7 +81,7 @@ def convert_from_uf2(buf):
         padding = newaddr - curraddr
         if padding < 0:
             assert False, "Block out of order at " + ptr
-        if padding > 10*1024*1024:
+        if padding > 10 * 1024 * 1024:
             assert False, "More than 10M of padding needed at " + ptr
         if padding % 4 != 0:
             assert False, "Non-word padding size at " + ptr
@@ -91,6 +92,7 @@ def convert_from_uf2(buf):
         curraddr = newaddr + datalen
     return outp
 
+
 def convert_to_carray(file_content):
     outp = "const unsigned char bindata[] __attribute__((aligned(16))) = {"
     for i in range(len(file_content)):
@@ -100,6 +102,7 @@ def convert_to_carray(file_content):
     outp += "\n};\n"
     return outp
 
+
 def convert_to_uf2(file_content):
     global familyid
     datapadding = b""
@@ -109,13 +112,21 @@ def convert_to_uf2(file_content):
     outp = b""
     for blockno in range(numblocks):
         ptr = 256 * blockno
-        chunk = file_content[ptr:ptr + 256]
+        chunk = file_content[ptr : ptr + 256]
         flags = 0x0
         if familyid:
             flags |= 0x2000
-        hd = struct.pack(b"<IIIIIIII",
-            UF2_MAGIC_START0, UF2_MAGIC_START1,
-            flags, ptr + appstartaddr, 256, blockno, numblocks, familyid)
+        hd = struct.pack(
+            b"<IIIIIIII",
+            UF2_MAGIC_START0,
+            UF2_MAGIC_START1,
+            flags,
+            ptr + appstartaddr,
+            256,
+            blockno,
+            numblocks,
+            familyid,
+        )
         while len(chunk) < 256:
             chunk += b"\x00"
         block = hd + chunk + datapadding + struct.pack(b"<I", UF2_MAGIC_END)
@@ -123,6 +134,7 @@ def convert_to_uf2(file_content):
         outp += block
     return outp
 
+
 class Block:
     def __init__(self, addr):
         self.addr = addr
@@ -133,35 +145,44 @@ class Block:
         flags = 0x0
         if familyid:
             flags |= 0x2000
-        hd = struct.pack("<IIIIIIII",
-            UF2_MAGIC_START0, UF2_MAGIC_START1,
-            flags, self.addr, 256, blockno, numblocks, familyid)
+        hd = struct.pack(
+            "<IIIIIIII",
+            UF2_MAGIC_START0,
+            UF2_MAGIC_START1,
+            flags,
+            self.addr,
+            256,
+            blockno,
+            numblocks,
+            familyid,
+        )
         hd += self.bytes[0:256]
         while len(hd) < 512 - 4:
             hd += b"\x00"
         hd += struct.pack("<I", UF2_MAGIC_END)
         return hd
 
+
 def convert_from_hex_to_uf2(buf):
     global appstartaddr
     appstartaddr = None
     upper = 0
     currblock = None
     blocks = []
-    for line in buf.split('\n'):
+    for line in buf.split("\n"):
         if line[0] != ":":
             continue
         i = 1
         rec = []
         while i < len(line) - 1:
-            rec.append(int(line[i:i+2], 16))
+            rec.append(int(line[i : i + 2], 16))
             i += 2
         tp = rec[3]
         if tp == 4:
             upper = ((rec[4] << 8) | rec[5]) << 16
         elif tp == 2:
             upper = ((rec[4] << 8) | rec[5]) << 4
-            assert (upper & 0xffff) == 0
+            assert (upper & 0xFFFF) == 0
         elif tp == 1:
             break
         elif tp == 0:
@@ -170,10 +191,10 @@ def convert_from_hex_to_uf2(buf):
                 appstartaddr = addr
             i = 4
             while i < len(rec) - 1:
-                if not currblock or currblock.addr & ~0xff != addr & ~0xff:
-                    currblock = Block(addr & ~0xff)
+                if not currblock or currblock.addr & ~0xFF != addr & ~0xFF:
+                    currblock = Block(addr & ~0xFF)
                     blocks.append(currblock)
-                currblock.bytes[addr & 0xff] = rec[i]
+                currblock.bytes[addr & 0xFF] = rec[i]
                 addr += 1
                 i += 1
     numblocks = len(blocks)
@@ -182,17 +203,28 @@ def convert_from_hex_to_uf2(buf):
         resfile += blocks[i].encode(i, numblocks)
     return resfile
 
+
 def to_str(b):
     return b.decode("utf-8")
 
+
 def get_drives():
     drives = []
     if sys.platform == "win32":
-        r = subprocess.check_output(["wmic", "PATH", "Win32_LogicalDisk",
-                                     "get", "DeviceID,", "VolumeName,",
-                                     "FileSystem,", "DriveType"])
-        for line in to_str(r).split('\n'):
-            words = re.split('\s+', line)
+        r = subprocess.check_output(
+            [
+                "wmic",
+                "PATH",
+                "Win32_LogicalDisk",
+                "get",
+                "DeviceID,",
+                "VolumeName,",
+                "FileSystem,",
+                "DriveType",
+            ]
+        )
+        for line in to_str(r).split("\n"):
+            words = re.split("\\s+", line)
             if len(words) >= 3 and words[1] == "2" and words[2] == "FAT":
                 drives.append(words[0])
     else:
@@ -206,7 +238,6 @@ def get_drives():
         for d in os.listdir(rootpath):
             drives.append(os.path.join(rootpath, d))
 
-
     def has_info(d):
         try:
             return os.path.isfile(d + INFO_FILE)
@@ -217,7 +248,7 @@ def get_drives():
 
 
 def board_id(path):
-    with open(path + INFO_FILE, mode='r') as file:
+    with open(path + INFO_FILE, mode="r") as file:
         file_content = file.read()
     return re.search("Board-ID: ([^\r\n]*)", file_content).group(1)
 
@@ -235,30 +266,61 @@ def write_file(name, buf):
 
 def main():
     global appstartaddr, familyid
+
     def error(msg):
         print(msg)
         sys.exit(1)
-    parser = argparse.ArgumentParser(description='Convert to UF2 or flash directly.')
-    parser.add_argument('input', metavar='INPUT', type=str, nargs='?',
-                        help='input file (HEX, BIN or UF2)')
-    parser.add_argument('-b' , '--base', dest='base', type=str,
-                        default="0x2000",
-                        help='set base address of application for BIN format (default: 0x2000)')
-    parser.add_argument('-o' , '--output', metavar="FILE", dest='output', type=str,
-                        help='write output to named file; defaults to "flash.uf2" or "flash.bin" where sensible')
-    parser.add_argument('-d' , '--device', dest="device_path",
-                        help='select a device path to flash')
-    parser.add_argument('-l' , '--list', action='store_true',
-                        help='list connected devices')
-    parser.add_argument('-c' , '--convert', action='store_true',
-                        help='do not flash, just convert')
-    parser.add_argument('-D' , '--deploy', action='store_true',
-                        help='just flash, do not convert')
-    parser.add_argument('-f' , '--family', dest='family', type=str,
-                        default="0x0",
-                        help='specify familyID - number or name (default: 0x0)')
-    parser.add_argument('-C' , '--carray', action='store_true',
-                        help='convert binary file to a C array, not UF2')
+
+    parser = argparse.ArgumentParser(description="Convert to UF2 or flash directly.")
+    parser.add_argument(
+        "input",
+        metavar="INPUT",
+        type=str,
+        nargs="?",
+        help="input file (HEX, BIN or UF2)",
+    )
+    parser.add_argument(
+        "-b",
+        "--base",
+        dest="base",
+        type=str,
+        default="0x2000",
+        help="set base address of application for BIN format (default: 0x2000)",
+    )
+    parser.add_argument(
+        "-o",
+        "--output",
+        metavar="FILE",
+        dest="output",
+        type=str,
+        help='write output to named file; defaults to "flash.uf2" or "flash.bin" where sensible',
+    )
+    parser.add_argument(
+        "-d", "--device", dest="device_path", help="select a device path to flash"
+    )
+    parser.add_argument(
+        "-l", "--list", action="store_true", help="list connected devices"
+    )
+    parser.add_argument(
+        "-c", "--convert", action="store_true", help="do not flash, just convert"
+    )
+    parser.add_argument(
+        "-D", "--deploy", action="store_true", help="just flash, do not convert"
+    )
+    parser.add_argument(
+        "-f",
+        "--family",
+        dest="family",
+        type=str,
+        default="0x0",
+        help="specify familyID - number or name (default: 0x0)",
+    )
+    parser.add_argument(
+        "-C",
+        "--carray",
+        action="store_true",
+        help="convert binary file to a C array, not UF2",
+    )
     args = parser.parse_args()
     appstartaddr = int(args.base, 0)
 
@@ -268,14 +330,17 @@ def main():
         try:
             familyid = int(args.family, 0)
         except ValueError:
-            error("Family ID needs to be a number or one of: " + ", ".join(families.keys()))
+            error(
+                "Family ID needs to be a number or one of: "
+                + ", ".join(families.keys())
+            )
 
     if args.list:
         list_drives()
     else:
         if not args.input:
             error("Need input file")
-        with open(args.input, mode='rb') as f:
+        with open(args.input, mode="rb") as f:
             inpbuf = f.read()
         from_uf2 = is_uf2(inpbuf)
         ext = "uf2"
@@ -291,8 +356,10 @@ def main():
             ext = "h"
         else:
             outbuf = convert_to_uf2(inpbuf)
-        print("Converting to %s, output size: %d, start address: 0x%x" %
-              (ext, len(outbuf), appstartaddr))
+        print(
+            "Converting to %s, output size: %d, start address: 0x%x"
+            % (ext, len(outbuf), appstartaddr)
+        )
         if args.convert or ext != "uf2":
             drives = []
             if args.output == None:

boards/heltec_mesh_node_t114.json

@@ -0,0 +1,53 @@
+{
+  "build": {
+    "arduino": {
+      "ldscript": "nrf52840_s140_v6.ld"
+    },
+    "core": "nRF5",
+    "cpu": "cortex-m4",
+    "extra_flags": "-DARDUINO_NRF52840_PCA10056 -DNRF52840_XXAA",
+    "f_cpu": "64000000L",
+    "hwids": [
+      ["0x239A", "0x4405"],
+      ["0x239A", "0x0029"],
+      ["0x239A", "0x002A"]
+    ],
+    "usb_product": "HT-n5262",
+    "mcu": "nrf52840",
+    "variant": "heltec_mesh_node_t114",
+    "variants_dir": "variants",
+    "bsp": {
+      "name": "adafruit"
+    },
+    "softdevice": {
+      "sd_flags": "-DS140",
+      "sd_name": "s140",
+      "sd_version": "6.1.1",
+      "sd_fwid": "0x00B6"
+    },
+    "bootloader": {
+      "settings_addr": "0xFF000"
+    }
+  },
+  "connectivity": ["bluetooth"],
+  "debug": {
+    "jlink_device": "nRF52840_xxAA",
+    "onboard_tools": ["jlink"],
+    "svd_path": "nrf52840.svd",
+    "openocd_target": "nrf52840-mdk-rs"
+  },
+  "frameworks": ["arduino"],
+  "name": "Heltec nrf (Adafruit BSP)",
+  "upload": {
+    "maximum_ram_size": 248832,
+    "maximum_size": 815104,
+    "speed": 115200,
+    "protocol": "nrfutil",
+    "protocols": ["jlink", "nrfjprog", "nrfutil", "stlink"],
+    "use_1200bps_touch": true,
+    "require_upload_port": true,
+    "wait_for_upload_port": true
+  },
+  "url": "FIXME",
+  "vendor": "Heltec"
+}

boards/heltec_vision_master_e213.json

@@ -0,0 +1,42 @@
+{
+  "build": {
+    "arduino": {
+      "ldscript": "esp32s3_out.ld",
+      "partitions": "default_8MB.csv"
+    },
+    "core": "esp32",
+    "extra_flags": [
+      "-DBOARD_HAS_PSRAM",
+      "-DARDUINO_USB_CDC_ON_BOOT=1",
+      "-DARDUINO_USB_MODE=0",
+      "-DARDUINO_RUNNING_CORE=1",
+      "-DARDUINO_EVENT_RUNNING_CORE=1"
+    ],
+    "f_cpu": "240000000L",
+    "f_flash": "80000000L",
+    "flash_mode": "qio",
+    "hwids": [
+      ["0x303A", "0x1001"],
+      ["0x303A", "0x0002"]
+    ],
+    "mcu": "esp32s3",
+    "variant": "heltec_vision_master_e213"
+  },
+  "connectivity": ["wifi", "bluetooth", "lora"],
+  "debug": {
+    "openocd_target": "esp32s3.cfg"
+  },
+  "frameworks": ["arduino", "espidf"],
+  "name": "Heltec Vision Master E213",
+  "upload": {
+    "flash_size": "8MB",
+    "maximum_ram_size": 327680,
+    "maximum_size": 8388608,
+    "use_1200bps_touch": true,
+    "wait_for_upload_port": true,
+    "require_upload_port": true,
+    "speed": 921600
+  },
+  "url": "https://heltec.org/project/vision-master-e213/",
+  "vendor": "Heltec"
+}

boards/wio-sdk-wm1110.json

@@ -0,0 +1,51 @@
+{
+  "build": {
+    "arduino": {
+      "ldscript": "nrf52840_s140_v7.ld"
+    },
+    "core": "nRF5",
+    "cpu": "cortex-m4",
+    "extra_flags": "-DARDUINO_WIO_WM1110 -DNRF52840_XXAA",
+    "f_cpu": "64000000L",
+    "mcu": "nrf52840",
+    "variant": "Seeed_WIO_WM1110",
+    "bsp": {
+      "name": "adafruit"
+    },
+    "softdevice": {
+      "sd_flags": "-DS140",
+      "sd_name": "s140",
+      "sd_version": "7.3.0",
+      "sd_fwid": "0x0123"
+    },
+    "bootloader": {
+      "settings_addr": "0xFF000"
+    }
+  },
+  "connectivity": ["bluetooth"],
+  "debug": {
+    "jlink_device": "nRF52840_xxAA",
+    "svd_path": "nrf52840.svd"
+  },
+  "frameworks": ["arduino", "freertos"],
+  "name": "Seeed WIO WM1110",
+  "upload": {
+    "maximum_ram_size": 248832,
+    "maximum_size": 815104,
+    "speed": 115200,
+    "protocol": "nrfutil",
+    "protocols": [
+      "jlink",
+      "nrfjprog",
+      "nrfutil",
+      "stlink",
+      "cmsis-dap",
+      "blackmagic"
+    ],
+    "use_1200bps_touch": true,
+    "require_upload_port": true,
+    "wait_for_upload_port": true
+  },
+  "url": "https://www.seeedstudio.com/Wio-WM1110-Dev-Kit-p-5677.html",
+  "vendor": "Seeed Studio"
+}

boards/wio-t1000-s.json

@@ -0,0 +1,58 @@
+{
+  "build": {
+    "arduino": {
+      "ldscript": "nrf52840_s140_v7.ld"
+    },
+    "core": "nRF5",
+    "cpu": "cortex-m4",
+    "extra_flags": "-DARDUINO_WIO_WM1110 -DNRF52840_XXAA",
+    "f_cpu": "64000000L",
+    "hwids": [
+      ["0x239A", "0x8029"],
+      ["0x239A", "0x0029"],
+      ["0x239A", "0x002A"],
+      ["0x239A", "0x802A"]
+    ],
+    "usb_product": "WIO-BOOT",
+    "mcu": "nrf52840",
+    "variant": "Seeed_WIO_WM1110",
+    "bsp": {
+      "name": "adafruit"
+    },
+    "softdevice": {
+      "sd_flags": "-DS140",
+      "sd_name": "s140",
+      "sd_version": "7.3.0",
+      "sd_fwid": "0x0123"
+    },
+    "bootloader": {
+      "settings_addr": "0xFF000"
+    }
+  },
+  "connectivity": ["bluetooth"],
+  "debug": {
+    "jlink_device": "nRF52840_xxAA",
+    "svd_path": "nrf52840.svd"
+  },
+  "frameworks": ["arduino"],
+  "name": "Seeed WIO WM1110",
+  "upload": {
+    "maximum_ram_size": 248832,
+    "maximum_size": 815104,
+    "speed": 115200,
+    "protocol": "nrfutil",
+    "protocols": [
+      "jlink",
+      "nrfjprog",
+      "nrfutil",
+      "stlink",
+      "cmsis-dap",
+      "blackmagic"
+    ],
+    "use_1200bps_touch": true,
+    "require_upload_port": true,
+    "wait_for_upload_port": true
+  },
+  "url": "https://www.seeedstudio.com/LoRaWAN-Tracker-c-1938.html",
+  "vendor": "Seeed Studio"
+}

boards/wio-tracker-wm1110.json

@@ -0,0 +1,58 @@
+{
+  "build": {
+    "arduino": {
+      "ldscript": "nrf52840_s140_v7.ld"
+    },
+    "core": "nRF5",
+    "cpu": "cortex-m4",
+    "extra_flags": "-DARDUINO_WIO_WM1110 -DNRF52840_XXAA",
+    "f_cpu": "64000000L",
+    "hwids": [
+      ["0x239A", "0x8029"],
+      ["0x239A", "0x0029"],
+      ["0x239A", "0x002A"],
+      ["0x239A", "0x802A"]
+    ],
+    "usb_product": "WIO-BOOT",
+    "mcu": "nrf52840",
+    "variant": "Seeed_WIO_WM1110",
+    "bsp": {
+      "name": "adafruit"
+    },
+    "softdevice": {
+      "sd_flags": "-DS140",
+      "sd_name": "s140",
+      "sd_version": "7.3.0",
+      "sd_fwid": "0x0123"
+    },
+    "bootloader": {
+      "settings_addr": "0xFF000"
+    }
+  },
+  "connectivity": ["bluetooth"],
+  "debug": {
+    "jlink_device": "nRF52840_xxAA",
+    "svd_path": "nrf52840.svd"
+  },
+  "frameworks": ["arduino"],
+  "name": "Seeed WIO WM1110",
+  "upload": {
+    "maximum_ram_size": 248832,
+    "maximum_size": 815104,
+    "speed": 115200,
+    "protocol": "nrfutil",
+    "protocols": [
+      "jlink",
+      "nrfjprog",
+      "nrfutil",
+      "stlink",
+      "cmsis-dap",
+      "blackmagic"
+    ],
+    "use_1200bps_touch": true,
+    "require_upload_port": true,
+    "wait_for_upload_port": true
+  },
+  "url": "https://www.seeedstudio.com/Wio-Tracker-1110-Dev-Board-p-5799.html",
+  "vendor": "Seeed Studio"
+}

boards/wiscore_rak4631.json

@@ -35,7 +35,7 @@
     "svd_path": "nrf52840.svd",
     "openocd_target": "nrf52840-mdk-rs"
   },
-  "frameworks": ["arduino"],
+  "frameworks": ["arduino", "freertos"],
   "name": "WisCore RAK4631 Board",
   "upload": {
     "maximum_ram_size": 248832,

extra_scripts/README.md

@@ -0,0 +1,3 @@
+# extra_scripts
+
+This directory contains special [scripts](https://docs.platformio.org/en/latest/scripting/index.html) that are used to modify the platformio environment in rare cases.

extra_scripts/disable_adafruit_usb.py

@@ -0,0 +1,20 @@
+# trunk-ignore-all(flake8/F821)
+# trunk-ignore-all(ruff/F821)
+
+Import("env")
+
+# NOTE: This is not currently used, but can serve as an example on how to write extra_scripts
+
+print("Current CLI targets", COMMAND_LINE_TARGETS)
+print("Current Build targets", BUILD_TARGETS)
+print("CPP defs", env.get("CPPDEFINES"))
+
+# Adafruit.py in the platformio build tree is a bit naive and always enables their USB stack for building.  We don't want this.
+# So come in after that python script has run and disable it.  This hack avoids us having to fork that big project and send in a PR
+# which might not be accepted. -@geeksville
+
+env["CPPDEFINES"].remove("USBCON")
+env["CPPDEFINES"].remove("USE_TINYUSB")
+
+# Custom actions when building program/firmware
+# env.AddPreAction("buildprog", callback...)

platformio.ini

@@ -29,8 +29,16 @@ default_envs = tbeam
 ;default_envs = meshtastic-dr-dev
 ;default_envs = m5stack-coreink
 ;default_envs = rak4631
+;default_envs = rak2560
 ;default_envs = rak10701
 ;default_envs = wio-e5
+;default_envs = radiomaster_900_bandit_nano
+;default_envs = radiomaster_900_bandit_micro
+;default_envs = heltec_capsule_sensor_v3
+;default_envs = heltec_vision_master_t190
+;default_envs = heltec_vision_master_e213
+;default_envs = heltec_vision_master_e290
+;default_envs = heltec_mesh_node_t114
 
 extra_configs =
   arch/*/*.ini
@@ -70,12 +78,14 @@ build_flags = -Wno-missing-field-initializers
   -DRADIOLIB_EXCLUDE_FSK4
   -DRADIOLIB_EXCLUDE_APRS
   -DRADIOLIB_EXCLUDE_LORAWAN
+  -DMESHTASTIC_EXCLUDE_DROPZONE=1
 
 monitor_speed = 115200
+monitor_filters = direct
 
 lib_deps =
-  jgromes/RadioLib@~6.5.0
-  https://github.com/meshtastic/esp8266-oled-ssd1306.git#ee628ee6c9588d4c56c9e3da35f0fc9448ad54a8 ; ESP8266_SSD1306
+  jgromes/RadioLib@~6.6.0
+  https://github.com/meshtastic/esp8266-oled-ssd1306.git#e16cee124fe26490cb14880c679321ad8ac89c95 ; ESP8266_SSD1306
   mathertel/OneButton@^2.5.0 ; OneButton library for non-blocking button debounce
   https://github.com/meshtastic/arduino-fsm.git#7db3702bf0cfe97b783d6c72595e3f38e0b19159
   https://github.com/meshtastic/TinyGPSPlus.git#71a82db35f3b973440044c476d4bcdc673b104f4
@@ -118,10 +128,7 @@ lib_deps =
   adafruit/Adafruit BMP280 Library@^2.6.8
   adafruit/Adafruit BMP085 Library@^1.2.4
   adafruit/Adafruit BME280 Library@^2.2.2
-  https://github.com/boschsensortec/Bosch-BSEC2-Library#v1.7.2502
-  boschsensortec/BME68x Sensor Library@^1.1.40407
   adafruit/Adafruit MCP9808 Library@^2.0.0
-  https://github.com/KodinLanewave/INA3221@^1.0.0
   adafruit/Adafruit INA260 Library@^1.5.0
   adafruit/Adafruit INA219@^1.2.0
   adafruit/Adafruit SHTC3 Library@^1.0.0
@@ -131,8 +138,21 @@ lib_deps =
   adafruit/Adafruit MPU6050@^2.2.4
   adafruit/Adafruit LIS3DH@^1.2.4
   adafruit/Adafruit AHTX0@^2.0.5
-  lewisxhe/SensorLib@^0.2.0
   adafruit/Adafruit LSM6DS@^4.7.2
-  mprograms/QMC5883LCompass@^1.2.0
   adafruit/Adafruit VEML7700 Library@^2.1.6
-  adafruit/Adafruit SHT4x Library@^1.0.4
+  adafruit/Adafruit SHT4x Library@^1.0.4
+  adafruit/Adafruit TSL2591 Library@^1.4.5
+  sparkfun/SparkFun Qwiic Scale NAU7802 Arduino Library@^1.0.5
+  ClosedCube OPT3001@^1.1.2
+  emotibit/EmotiBit MLX90632@^1.0.8
+  dfrobot/DFRobot_RTU@^1.0.3
+
+
+  https://github.com/boschsensortec/Bosch-BSEC2-Library#v1.7.2502
+  boschsensortec/BME68x Sensor Library@^1.1.40407
+  https://github.com/KodinLanewave/INA3221@^1.0.0
+  lewisxhe/SensorLib@^0.2.0
+  mprograms/QMC5883LCompass@^1.2.0
+  
+
+  https://github.com/meshtastic/DFRobot_LarkWeatherStation#dee914270dc7cb3e43fbf034edd85a63a16a12ee

pyocd.yaml

@@ -0,0 +1,7 @@
+# This is a config file to control pyocd ICE debugger probe options (only used for NRF52 targets with hardware debugging connections)
+# for more info see FIXMEURL
+
+# console or telnet
+semihost_console_type: telnet
+enable_semihosting: True
+telnet_port: 4444

src/AccelerometerThread.h

@@ -14,6 +14,12 @@
 #include <Arduino.h>
 #include <SensorBMA423.hpp>
 #include <Wire.h>
+#ifdef RAK_4631
+#include "Fusion/Fusion.h"
+#include "graphics/Screen.h"
+#include "graphics/ScreenFonts.h"
+#include <Rak_BMX160.h>
+#endif
 
 #define ACCELEROMETER_CHECK_INTERVAL_MS 100
 #define ACCELEROMETER_CLICK_THRESHOLD 40
@@ -50,12 +56,13 @@ class AccelerometerThread : public concurrency::OSThread
             return;
         }
         acceleremoter_type = type;
-
+#ifndef RAK_4631
         if (!config.display.wake_on_tap_or_motion && !config.device.double_tap_as_button_press) {
             LOG_DEBUG("AccelerometerThread disabling due to no interested configurations\n");
             disable();
             return;
         }
+#endif
         init();
     }
 
@@ -87,6 +94,79 @@ class AccelerometerThread : public concurrency::OSThread
                 wakeScreen();
                 return 500;
             }
+#ifdef RAK_4631
+        } else if (acceleremoter_type == ScanI2C::DeviceType::BMX160) {
+            sBmx160SensorData_t magAccel;
+            sBmx160SensorData_t gAccel;
+
+            /* Get a new sensor event */
+            bmx160.getAllData(&magAccel, NULL, &gAccel);
+
+            // expirimental calibrate routine. Limited to between 10 and 30 seconds after boot
+            if (millis() > 12 * 1000 && millis() < 30 * 1000) {
+                if (!showingScreen) {
+                    showingScreen = true;
+                    screen->startAlert((FrameCallback)drawFrameCalibration);
+                }
+                if (magAccel.x > highestX)
+                    highestX = magAccel.x;
+                if (magAccel.x < lowestX)
+                    lowestX = magAccel.x;
+                if (magAccel.y > highestY)
+                    highestY = magAccel.y;
+                if (magAccel.y < lowestY)
+                    lowestY = magAccel.y;
+                if (magAccel.z > highestZ)
+                    highestZ = magAccel.z;
+                if (magAccel.z < lowestZ)
+                    lowestZ = magAccel.z;
+            } else if (showingScreen && millis() >= 30 * 1000) {
+                showingScreen = false;
+                screen->endAlert();
+            }
+
+            int highestRealX = highestX - (highestX + lowestX) / 2;
+
+            magAccel.x -= (highestX + lowestX) / 2;
+            magAccel.y -= (highestY + lowestY) / 2;
+            magAccel.z -= (highestZ + lowestZ) / 2;
+            FusionVector ga, ma;
+            ga.axis.x = -gAccel.x; // default location for the BMX160 is on the rear of the board
+            ga.axis.y = -gAccel.y;
+            ga.axis.z = gAccel.z;
+            ma.axis.x = -magAccel.x;
+            ma.axis.y = -magAccel.y;
+            ma.axis.z = magAccel.z * 3;
+
+            // If we're set to one of the inverted positions
+            if (config.display.compass_orientation > meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270) {
+                ma = FusionAxesSwap(ma, FusionAxesAlignmentNXNYPZ);
+                ga = FusionAxesSwap(ga, FusionAxesAlignmentNXNYPZ);
+            }
+
+            float heading = FusionCompassCalculateHeading(FusionConventionNed, ga, ma);
+
+            switch (config.display.compass_orientation) {
+            case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_0_INVERTED:
+            case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_0:
+                break;
+            case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_90:
+            case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_90_INVERTED:
+                heading += 90;
+                break;
+            case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_180:
+            case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_180_INVERTED:
+                heading += 180;
+                break;
+            case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270:
+            case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270_INVERTED:
+                heading += 270;
+                break;
+            }
+
+            screen->setHeading(heading);
+
+#endif
         } else if (acceleremoter_type == ScanI2C::DeviceType::LSM6DS3 && lsm.shake()) {
             wakeScreen();
             return 500;
@@ -149,6 +229,11 @@ class AccelerometerThread : public concurrency::OSThread
             bmaSensor.enableTiltIRQ();
             // It corresponds to isDoubleClick interrupt
             bmaSensor.enableWakeupIRQ();
+#ifdef RAK_4631
+        } else if (acceleremoter_type == ScanI2C::DeviceType::BMX160 && bmx160.begin()) {
+            bmx160.ODR_Config(BMX160_ACCEL_ODR_100HZ, BMX160_GYRO_ODR_100HZ); // set output data rate
+
+#endif
         } else if (acceleremoter_type == ScanI2C::DeviceType::LSM6DS3 && lsm.begin_I2C(accelerometer_found.address)) {
             LOG_DEBUG("LSM6DS3 initializing\n");
             // Default threshold of 2G, less sensitive options are 4, 8 or 16G
@@ -180,6 +265,33 @@ class AccelerometerThread : public concurrency::OSThread
     Adafruit_LSM6DS3TRC lsm;
     SensorBMA423 bmaSensor;
     bool BMA_IRQ = false;
+#ifdef RAK_4631
+    bool showingScreen = false;
+    RAK_BMX160 bmx160;
+    float highestX = 0, lowestX = 0, highestY = 0, lowestY = 0, highestZ = 0, lowestZ = 0;
+
+    static void drawFrameCalibration(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y)
+    {
+        int x_offset = display->width() / 2;
+        int y_offset = display->height() <= 80 ? 0 : 32;
+        display->setTextAlignment(TEXT_ALIGN_LEFT);
+        display->setFont(FONT_MEDIUM);
+        display->drawString(x, y, "Calibrating\nCompass");
+        int16_t compassX = 0, compassY = 0;
+        uint16_t compassDiam = graphics::Screen::getCompassDiam(display->getWidth(), display->getHeight());
+
+        // coordinates for the center of the compass/circle
+        if (config.display.displaymode == meshtastic_Config_DisplayConfig_DisplayMode_DEFAULT) {
+            compassX = x + display->getWidth() - compassDiam / 2 - 5;
+            compassY = y + display->getHeight() / 2;
+        } else {
+            compassX = x + display->getWidth() - compassDiam / 2 - 5;
+            compassY = y + FONT_HEIGHT_SMALL + (display->getHeight() - FONT_HEIGHT_SMALL) / 2;
+        }
+        display->drawCircle(compassX, compassY, compassDiam / 2);
+        screen->drawCompassNorth(display, compassX, compassY, screen->getHeading() * PI / 180);
+    }
+#endif
 };
 
 #endif

src/BluetoothCommon.cpp

@@ -10,4 +10,8 @@ const uint8_t TORADIO_UUID_16[16u] = {0xe7, 0x01, 0x44, 0x12, 0x66, 0x78, 0xdd,
 const uint8_t FROMRADIO_UUID_16[16u] = {0x02, 0x00, 0x12, 0xac, 0x42, 0x02, 0x78, 0xb8,
                                         0xed, 0x11, 0x93, 0x49, 0x9e, 0xe6, 0x55, 0x2c};
 const uint8_t FROMNUM_UUID_16[16u] = {0x53, 0x44, 0xe3, 0x47, 0x75, 0xaa, 0x70, 0xa6,
-                                      0x66, 0x4f, 0x00, 0xa8, 0x8c, 0xa1, 0x9d, 0xed};
+                                      0x66, 0x4f, 0x00, 0xa8, 0x8c, 0xa1, 0x9d, 0xed};
+const uint8_t LEGACY_LOGRADIO_UUID_16[16u] = {0xe2, 0xf2, 0x1e, 0xbe, 0xc5, 0x15, 0xcf, 0xaa,
+                                              0x6b, 0x43, 0xfa, 0x78, 0x38, 0xd2, 0x6f, 0x6c};
+const uint8_t LOGRADIO_UUID_16[16u] = {0x47, 0x95, 0xDF, 0x8C, 0xDE, 0xE9, 0x44, 0x99,
+                                       0x23, 0x44, 0xE6, 0x06, 0x49, 0x6E, 0x3D, 0x5A};

src/BluetoothCommon.h

@@ -11,10 +11,12 @@
 #define TORADIO_UUID "f75c76d2-129e-4dad-a1dd-7866124401e7"
 #define FROMRADIO_UUID "2c55e69e-4993-11ed-b878-0242ac120002"
 #define FROMNUM_UUID "ed9da18c-a800-4f66-a670-aa7547e34453"
+#define LEGACY_LOGRADIO_UUID "6c6fd238-78fa-436b-aacf-15c5be1ef2e2"
+#define LOGRADIO_UUID "5a3d6e49-06e6-4423-9944-e9de8cdf9547"
 
 // NRF52 wants these constants as byte arrays
 // Generated here https://yupana-engineering.com/online-uuid-to-c-array-converter - but in REVERSE BYTE ORDER
-extern const uint8_t MESH_SERVICE_UUID_16[], TORADIO_UUID_16[16u], FROMRADIO_UUID_16[], FROMNUM_UUID_16[];
+extern const uint8_t MESH_SERVICE_UUID_16[], TORADIO_UUID_16[16u], FROMRADIO_UUID_16[], FROMNUM_UUID_16[], LOGRADIO_UUID_16[];
 
 /// Given a level between 0-100, update the BLE attribute
 void updateBatteryLevel(uint8_t level);

src/ButtonThread.cpp

@@ -41,14 +41,24 @@ ButtonThread::ButtonThread() : OSThread("Button")
     }
 #elif defined(BUTTON_PIN)
     int pin = config.device.button_gpio ? config.device.button_gpio : BUTTON_PIN; // Resolved button pin
+#if defined(HELTEC_CAPSULE_SENSOR_V3)
+    this->userButton = OneButton(pin, false, false);
+#elif defined(BUTTON_ACTIVE_LOW) // change by WayenWeng
+    this->userButton = OneButton(pin, BUTTON_ACTIVE_LOW, BUTTON_ACTIVE_PULLUP);
+#else
     this->userButton = OneButton(pin, true, true);
+#endif
     LOG_DEBUG("Using GPIO%02d for button\n", pin);
 #endif
 
 #ifdef INPUT_PULLUP_SENSE
     // Some platforms (nrf52) have a SENSE variant which allows wake from sleep - override what OneButton did
+#ifdef BUTTON_SENSE_TYPE // change by WayenWeng
+    pinMode(pin, BUTTON_SENSE_TYPE);
+#else
     pinMode(pin, INPUT_PULLUP_SENSE);
 #endif
+#endif
 
 #if defined(BUTTON_PIN) || defined(ARCH_PORTDUINO)
     userButton.attachClick(userButtonPressed);
@@ -177,8 +187,9 @@ int32_t ButtonThread::runOnce()
         case BUTTON_EVENT_LONG_PRESSED: {
             LOG_BUTTON("Long press!\n");
             powerFSM.trigger(EVENT_PRESS);
-            if (screen)
-                screen->startShutdownScreen();
+            if (screen) {
+                screen->startAlert("Shutting down...");
+            }
             playBeep();
             break;
         }
@@ -232,10 +243,10 @@ void ButtonThread::attachButtonInterrupts()
     attachInterrupt(
         config.device.button_gpio ? config.device.button_gpio : BUTTON_PIN,
         []() {
-            BaseType_t higherWake = 0;
-            mainDelay.interruptFromISR(&higherWake);
             ButtonThread::userButton.tick();
             runASAP = true;
+            BaseType_t higherWake = 0;
+            mainDelay.interruptFromISR(&higherWake);
         },
         CHANGE);
 #endif
@@ -318,4 +329,4 @@ void ButtonThread::userButtonPressedLongStop()
     if (millis() > c_holdOffTime) {
         btnEvent = BUTTON_EVENT_LONG_RELEASED;
     }
-}
+}

src/DebugConfiguration.cpp

@@ -26,7 +26,7 @@ SOFTWARE.*/
 
 #include "DebugConfiguration.h"
 
-#if HAS_WIFI || HAS_ETHERNET
+#if HAS_NETWORKING
 
 Syslog::Syslog(UDP &client)
 {

src/DebugConfiguration.h

@@ -1,5 +1,6 @@
-#ifndef SYSLOG_H
-#define SYSLOG_H
+#pragma once
+
+#include "configuration.h"
 
 // DEBUG LED
 #ifndef LED_INVERTED
@@ -25,6 +26,14 @@
 
 #include "SerialConsole.h"
 
+// If defined we will include support for ARM ICE "semihosting" for a virtual
+// console over the JTAG port (to replace the normal serial port)
+// Note: Normally this flag is passed into the gcc commandline by platformio.ini.
+// for an example see env:rak4631_dap.
+// #ifndef USE_SEMIHOSTING
+// #define USE_SEMIHOSTING
+// #endif
+
 #define DEBUG_PORT (*console) // Serial debug port
 
 #ifdef USE_SEGGER
@@ -117,7 +126,7 @@
 #include <WiFi.h>
 #endif // HAS_WIFI
 
-#if HAS_WIFI || HAS_ETHERNET
+#if HAS_NETWORKING
 
 class Syslog
 {
@@ -152,6 +161,4 @@ class Syslog
     bool vlogf(uint16_t pri, const char *appName, const char *fmt, va_list args) __attribute__((format(printf, 3, 0)));
 };
 
-#endif // HAS_ETHERNET || HAS_WIFI
-
-#endif // SYSLOG_H
+#endif // HAS_ETHERNET || HAS_WIFI

src/FSCommon.cpp

@@ -84,6 +84,58 @@ bool renameFile(const char *pathFrom, const char *pathTo)
 #endif
 }
 
+#include <vector>
+
+/**
+ * @brief Get the list of files in a directory.
+ *
+ * This function returns a list of files in a directory. The list includes the full path of each file.
+ *
+ * @param dirname The name of the directory.
+ * @param levels The number of levels of subdirectories to list.
+ * @return A vector of strings containing the full path of each file in the directory.
+ */
+std::vector<meshtastic_FileInfo> getFiles(const char *dirname, uint8_t levels)
+{
+    std::vector<meshtastic_FileInfo> filenames = {};
+#ifdef FSCom
+    File root = FSCom.open(dirname, FILE_O_READ);
+    if (!root)
+        return filenames;
+    if (!root.isDirectory())
+        return filenames;
+
+    File file = root.openNextFile();
+    while (file) {
+        if (file.isDirectory() && !String(file.name()).endsWith(".")) {
+            if (levels) {
+#ifdef ARCH_ESP32
+                std::vector<meshtastic_FileInfo> subDirFilenames = getFiles(file.path(), levels - 1);
+#else
+                std::vector<meshtastic_FileInfo> subDirFilenames = getFiles(file.name(), levels - 1);
+#endif
+                filenames.insert(filenames.end(), subDirFilenames.begin(), subDirFilenames.end());
+                file.close();
+            }
+        } else {
+            meshtastic_FileInfo fileInfo = {"", file.size()};
+#ifdef ARCH_ESP32
+            strcpy(fileInfo.file_name, file.path());
+#else
+            strcpy(fileInfo.file_name, file.name());
+#endif
+            if (!String(fileInfo.file_name).endsWith(".")) {
+                filenames.push_back(fileInfo);
+            }
+            file.close();
+        }
+        file = root.openNextFile();
+    }
+    root.close();
+#endif
+    return filenames;
+}
+
 /**
  * Lists the contents of a directory.
  *

src/FSCommon.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include "configuration.h"
+#include <vector>
 
 // Cross platform filesystem API
 
@@ -49,6 +50,7 @@ using namespace Adafruit_LittleFS_Namespace;
 void fsInit();
 bool copyFile(const char *from, const char *to);
 bool renameFile(const char *pathFrom, const char *pathTo);
+std::vector<meshtastic_FileInfo> getFiles(const char *dirname, uint8_t levels);
 void listDir(const char *dirname, uint8_t levels, bool del);
 void rmDir(const char *dirname);
 void setupSDCard();

src/Fusion/Fusion.h

@@ -0,0 +1,32 @@
+/**
+ * @file Fusion.h
+ * @author Seb Madgwick
+ * @brief Main header file for the Fusion library.  This is the only file that
+ * needs to be included when using the library.
+ */
+
+#ifndef FUSION_H
+#define FUSION_H
+
+//------------------------------------------------------------------------------
+// Includes
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "FusionAhrs.h"
+#include "FusionAxes.h"
+#include "FusionCalibration.h"
+#include "FusionCompass.h"
+#include "FusionConvention.h"
+#include "FusionMath.h"
+#include "FusionOffset.h"
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+//------------------------------------------------------------------------------
+// End of file

src/Fusion/FusionAhrs.c

@@ -0,0 +1,542 @@
+/**
+ * @file FusionAhrs.c
+ * @author Seb Madgwick
+ * @brief AHRS algorithm to combine gyroscope, accelerometer, and magnetometer
+ * measurements into a single measurement of orientation relative to the Earth.
+ */
+
+//------------------------------------------------------------------------------
+// Includes
+
+#include "FusionAhrs.h"
+#include <float.h> // FLT_MAX
+#include <math.h>  // atan2f, cosf, fabsf, powf, sinf
+
+//------------------------------------------------------------------------------
+// Definitions
+
+/**
+ * @brief Initial gain used during the initialisation.
+ */
+#define INITIAL_GAIN (10.0f)
+
+/**
+ * @brief Initialisation period in seconds.
+ */
+#define INITIALISATION_PERIOD (3.0f)
+
+//------------------------------------------------------------------------------
+// Function declarations
+
+static inline FusionVector HalfGravity(const FusionAhrs *const ahrs);
+
+static inline FusionVector HalfMagnetic(const FusionAhrs *const ahrs);
+
+static inline FusionVector Feedback(const FusionVector sensor, const FusionVector reference);
+
+static inline int Clamp(const int value, const int min, const int max);
+
+//------------------------------------------------------------------------------
+// Functions
+
+/**
+ * @brief Initialises the AHRS algorithm structure.
+ * @param ahrs AHRS algorithm structure.
+ */
+void FusionAhrsInitialise(FusionAhrs *const ahrs)
+{
+    const FusionAhrsSettings settings = {
+        .convention = FusionConventionNwu,
+        .gain = 0.5f,
+        .gyroscopeRange = 0.0f,
+        .accelerationRejection = 90.0f,
+        .magneticRejection = 90.0f,
+        .recoveryTriggerPeriod = 0,
+    };
+    FusionAhrsSetSettings(ahrs, &settings);
+    FusionAhrsReset(ahrs);
+}
+
+/**
+ * @brief Resets the AHRS algorithm.  This is equivalent to reinitialising the
+ * algorithm while maintaining the current settings.
+ * @param ahrs AHRS algorithm structure.
+ */
+void FusionAhrsReset(FusionAhrs *const ahrs)
+{
+    ahrs->quaternion = FUSION_IDENTITY_QUATERNION;
+    ahrs->accelerometer = FUSION_VECTOR_ZERO;
+    ahrs->initialising = true;
+    ahrs->rampedGain = INITIAL_GAIN;
+    ahrs->angularRateRecovery = false;
+    ahrs->halfAccelerometerFeedback = FUSION_VECTOR_ZERO;
+    ahrs->halfMagnetometerFeedback = FUSION_VECTOR_ZERO;
+    ahrs->accelerometerIgnored = false;
+    ahrs->accelerationRecoveryTrigger = 0;
+    ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
+    ahrs->magnetometerIgnored = false;
+    ahrs->magneticRecoveryTrigger = 0;
+    ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
+}
+
+/**
+ * @brief Sets the AHRS algorithm settings.
+ * @param ahrs AHRS algorithm structure.
+ * @param settings Settings.
+ */
+void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *const settings)
+{
+    ahrs->settings.convention = settings->convention;
+    ahrs->settings.gain = settings->gain;
+    ahrs->settings.gyroscopeRange = settings->gyroscopeRange == 0.0f ? FLT_MAX : 0.98f * settings->gyroscopeRange;
+    ahrs->settings.accelerationRejection = settings->accelerationRejection == 0.0f
+                                               ? FLT_MAX
+                                               : powf(0.5f * sinf(FusionDegreesToRadians(settings->accelerationRejection)), 2);
+    ahrs->settings.magneticRejection =
+        settings->magneticRejection == 0.0f ? FLT_MAX : powf(0.5f * sinf(FusionDegreesToRadians(settings->magneticRejection)), 2);
+    ahrs->settings.recoveryTriggerPeriod = settings->recoveryTriggerPeriod;
+    ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
+    ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
+    if ((settings->gain == 0.0f) ||
+        (settings->recoveryTriggerPeriod == 0)) { // disable acceleration and magnetic rejection features if gain is zero
+        ahrs->settings.accelerationRejection = FLT_MAX;
+        ahrs->settings.magneticRejection = FLT_MAX;
+    }
+    if (ahrs->initialising == false) {
+        ahrs->rampedGain = ahrs->settings.gain;
+    }
+    ahrs->rampedGainStep = (INITIAL_GAIN - ahrs->settings.gain) / INITIALISATION_PERIOD;
+}
+
+/**
+ * @brief Updates the AHRS algorithm using the gyroscope, accelerometer, and
+ * magnetometer measurements.
+ * @param ahrs AHRS algorithm structure.
+ * @param gyroscope Gyroscope measurement in degrees per second.
+ * @param accelerometer Accelerometer measurement in g.
+ * @param magnetometer Magnetometer measurement in arbitrary units.
+ * @param deltaTime Delta time in seconds.
+ */
+void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer,
+                      const FusionVector magnetometer, const float deltaTime)
+{
+#define Q ahrs->quaternion.element
+
+    // Store accelerometer
+    ahrs->accelerometer = accelerometer;
+
+    // Reinitialise if gyroscope range exceeded
+    if ((fabsf(gyroscope.axis.x) > ahrs->settings.gyroscopeRange) || (fabsf(gyroscope.axis.y) > ahrs->settings.gyroscopeRange) ||
+        (fabsf(gyroscope.axis.z) > ahrs->settings.gyroscopeRange)) {
+        const FusionQuaternion quaternion = ahrs->quaternion;
+        FusionAhrsReset(ahrs);
+        ahrs->quaternion = quaternion;
+        ahrs->angularRateRecovery = true;
+    }
+
+    // Ramp down gain during initialisation
+    if (ahrs->initialising) {
+        ahrs->rampedGain -= ahrs->rampedGainStep * deltaTime;
+        if ((ahrs->rampedGain < ahrs->settings.gain) || (ahrs->settings.gain == 0.0f)) {
+            ahrs->rampedGain = ahrs->settings.gain;
+            ahrs->initialising = false;
+            ahrs->angularRateRecovery = false;
+        }
+    }
+
+    // Calculate direction of gravity indicated by algorithm
+    const FusionVector halfGravity = HalfGravity(ahrs);
+
+    // Calculate accelerometer feedback
+    FusionVector halfAccelerometerFeedback = FUSION_VECTOR_ZERO;
+    ahrs->accelerometerIgnored = true;
+    if (FusionVectorIsZero(accelerometer) == false) {
+
+        // Calculate accelerometer feedback scaled by 0.5
+        ahrs->halfAccelerometerFeedback = Feedback(FusionVectorNormalise(accelerometer), halfGravity);
+
+        // Don't ignore accelerometer if acceleration error below threshold
+        if (ahrs->initialising ||
+            ((FusionVectorMagnitudeSquared(ahrs->halfAccelerometerFeedback) <= ahrs->settings.accelerationRejection))) {
+            ahrs->accelerometerIgnored = false;
+            ahrs->accelerationRecoveryTrigger -= 9;
+        } else {
+            ahrs->accelerationRecoveryTrigger += 1;
+        }
+
+        // Don't ignore accelerometer during acceleration recovery
+        if (ahrs->accelerationRecoveryTrigger > ahrs->accelerationRecoveryTimeout) {
+            ahrs->accelerationRecoveryTimeout = 0;
+            ahrs->accelerometerIgnored = false;
+        } else {
+            ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
+        }
+        ahrs->accelerationRecoveryTrigger = Clamp(ahrs->accelerationRecoveryTrigger, 0, ahrs->settings.recoveryTriggerPeriod);
+
+        // Apply accelerometer feedback
+        if (ahrs->accelerometerIgnored == false) {
+            halfAccelerometerFeedback = ahrs->halfAccelerometerFeedback;
+        }
+    }
+
+    // Calculate magnetometer feedback
+    FusionVector halfMagnetometerFeedback = FUSION_VECTOR_ZERO;
+    ahrs->magnetometerIgnored = true;
+    if (FusionVectorIsZero(magnetometer) == false) {
+
+        // Calculate direction of magnetic field indicated by algorithm
+        const FusionVector halfMagnetic = HalfMagnetic(ahrs);
+
+        // Calculate magnetometer feedback scaled by 0.5
+        ahrs->halfMagnetometerFeedback =
+            Feedback(FusionVectorNormalise(FusionVectorCrossProduct(halfGravity, magnetometer)), halfMagnetic);
+
+        // Don't ignore magnetometer if magnetic error below threshold
+        if (ahrs->initialising ||
+            ((FusionVectorMagnitudeSquared(ahrs->halfMagnetometerFeedback) <= ahrs->settings.magneticRejection))) {
+            ahrs->magnetometerIgnored = false;
+            ahrs->magneticRecoveryTrigger -= 9;
+        } else {
+            ahrs->magneticRecoveryTrigger += 1;
+        }
+
+        // Don't ignore magnetometer during magnetic recovery
+        if (ahrs->magneticRecoveryTrigger > ahrs->magneticRecoveryTimeout) {
+            ahrs->magneticRecoveryTimeout = 0;
+            ahrs->magnetometerIgnored = false;
+        } else {
+            ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
+        }
+        ahrs->magneticRecoveryTrigger = Clamp(ahrs->magneticRecoveryTrigger, 0, ahrs->settings.recoveryTriggerPeriod);
+
+        // Apply magnetometer feedback
+        if (ahrs->magnetometerIgnored == false) {
+            halfMagnetometerFeedback = ahrs->halfMagnetometerFeedback;
+        }
+    }
+
+    // Convert gyroscope to radians per second scaled by 0.5
+    const FusionVector halfGyroscope = FusionVectorMultiplyScalar(gyroscope, FusionDegreesToRadians(0.5f));
+
+    // Apply feedback to gyroscope
+    const FusionVector adjustedHalfGyroscope = FusionVectorAdd(
+        halfGyroscope,
+        FusionVectorMultiplyScalar(FusionVectorAdd(halfAccelerometerFeedback, halfMagnetometerFeedback), ahrs->rampedGain));
+
+    // Integrate rate of change of quaternion
+    ahrs->quaternion = FusionQuaternionAdd(
+        ahrs->quaternion,
+        FusionQuaternionMultiplyVector(ahrs->quaternion, FusionVectorMultiplyScalar(adjustedHalfGyroscope, deltaTime)));
+
+    // Normalise quaternion
+    ahrs->quaternion = FusionQuaternionNormalise(ahrs->quaternion);
+#undef Q
+}
+
+/**
+ * @brief Returns the direction of gravity scaled by 0.5.
+ * @param ahrs AHRS algorithm structure.
+ * @return Direction of gravity scaled by 0.5.
+ */
+static inline FusionVector HalfGravity(const FusionAhrs *const ahrs)
+{
+#define Q ahrs->quaternion.element
+    switch (ahrs->settings.convention) {
+    case FusionConventionNwu:
+    case FusionConventionEnu: {
+        const FusionVector halfGravity = {.axis = {
+                                              .x = Q.x * Q.z - Q.w * Q.y,
+                                              .y = Q.y * Q.z + Q.w * Q.x,
+                                              .z = Q.w * Q.w - 0.5f + Q.z * Q.z,
+                                          }}; // third column of transposed rotation matrix scaled by 0.5
+        return halfGravity;
+    }
+    case FusionConventionNed: {
+        const FusionVector halfGravity = {.axis = {
+                                              .x = Q.w * Q.y - Q.x * Q.z,
+                                              .y = -1.0f * (Q.y * Q.z + Q.w * Q.x),
+                                              .z = 0.5f - Q.w * Q.w - Q.z * Q.z,
+                                          }}; // third column of transposed rotation matrix scaled by -0.5
+        return halfGravity;
+    }
+    }
+    return FUSION_VECTOR_ZERO; // avoid compiler warning
+#undef Q
+}
+
+/**
+ * @brief Returns the direction of the magnetic field scaled by 0.5.
+ * @param ahrs AHRS algorithm structure.
+ * @return Direction of the magnetic field scaled by 0.5.
+ */
+static inline FusionVector HalfMagnetic(const FusionAhrs *const ahrs)
+{
+#define Q ahrs->quaternion.element
+    switch (ahrs->settings.convention) {
+    case FusionConventionNwu: {
+        const FusionVector halfMagnetic = {.axis = {
+                                               .x = Q.x * Q.y + Q.w * Q.z,
+                                               .y = Q.w * Q.w - 0.5f + Q.y * Q.y,
+                                               .z = Q.y * Q.z - Q.w * Q.x,
+                                           }}; // second column of transposed rotation matrix scaled by 0.5
+        return halfMagnetic;
+    }
+    case FusionConventionEnu: {
+        const FusionVector halfMagnetic = {.axis = {
+                                               .x = 0.5f - Q.w * Q.w - Q.x * Q.x,
+                                               .y = Q.w * Q.z - Q.x * Q.y,
+                                               .z = -1.0f * (Q.x * Q.z + Q.w * Q.y),
+                                           }}; // first column of transposed rotation matrix scaled by -0.5
+        return halfMagnetic;
+    }
+    case FusionConventionNed: {
+        const FusionVector halfMagnetic = {.axis = {
+                                               .x = -1.0f * (Q.x * Q.y + Q.w * Q.z),
+                                               .y = 0.5f - Q.w * Q.w - Q.y * Q.y,
+                                               .z = Q.w * Q.x - Q.y * Q.z,
+                                           }}; // second column of transposed rotation matrix scaled by -0.5
+        return halfMagnetic;
+    }
+    }
+    return FUSION_VECTOR_ZERO; // avoid compiler warning
+#undef Q
+}
+
+/**
+ * @brief Returns the feedback.
+ * @param sensor Sensor.
+ * @param reference Reference.
+ * @return Feedback.
+ */
+static inline FusionVector Feedback(const FusionVector sensor, const FusionVector reference)
+{
+    if (FusionVectorDotProduct(sensor, reference) < 0.0f) { // if error is >90 degrees
+        return FusionVectorNormalise(FusionVectorCrossProduct(sensor, reference));
+    }
+    return FusionVectorCrossProduct(sensor, reference);
+}
+
+/**
+ * @brief Returns a value limited to maximum and minimum.
+ * @param value Value.
+ * @param min Minimum value.
+ * @param max Maximum value.
+ * @return Value limited to maximum and minimum.
+ */
+static inline int Clamp(const int value, const int min, const int max)
+{
+    if (value < min) {
+        return min;
+    }
+    if (value > max) {
+        return max;
+    }
+    return value;
+}
+
+/**
+ * @brief Updates the AHRS algorithm using the gyroscope and accelerometer
+ * measurements only.
+ * @param ahrs AHRS algorithm structure.
+ * @param gyroscope Gyroscope measurement in degrees per second.
+ * @param accelerometer Accelerometer measurement in g.
+ * @param deltaTime Delta time in seconds.
+ */
+void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer,
+                                    const float deltaTime)
+{
+
+    // Update AHRS algorithm
+    FusionAhrsUpdate(ahrs, gyroscope, accelerometer, FUSION_VECTOR_ZERO, deltaTime);
+
+    // Zero heading during initialisation
+    if (ahrs->initialising) {
+        FusionAhrsSetHeading(ahrs, 0.0f);
+    }
+}
+
+/**
+ * @brief Updates the AHRS algorithm using the gyroscope, accelerometer, and
+ * heading measurements.
+ * @param ahrs AHRS algorithm structure.
+ * @param gyroscope Gyroscope measurement in degrees per second.
+ * @param accelerometer Accelerometer measurement in g.
+ * @param heading Heading measurement in degrees.
+ * @param deltaTime Delta time in seconds.
+ */
+void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer,
+                                     const float heading, const float deltaTime)
+{
+#define Q ahrs->quaternion.element
+
+    // Calculate roll
+    const float roll = atan2f(Q.w * Q.x + Q.y * Q.z, 0.5f - Q.y * Q.y - Q.x * Q.x);
+
+    // Calculate magnetometer
+    const float headingRadians = FusionDegreesToRadians(heading);
+    const float sinHeadingRadians = sinf(headingRadians);
+    const FusionVector magnetometer = {.axis = {
+                                           .x = cosf(headingRadians),
+                                           .y = -1.0f * cosf(roll) * sinHeadingRadians,
+                                           .z = sinHeadingRadians * sinf(roll),
+                                       }};
+
+    // Update AHRS algorithm
+    FusionAhrsUpdate(ahrs, gyroscope, accelerometer, magnetometer, deltaTime);
+#undef Q
+}
+
+/**
+ * @brief Returns the quaternion describing the sensor relative to the Earth.
+ * @param ahrs AHRS algorithm structure.
+ * @return Quaternion describing the sensor relative to the Earth.
+ */
+FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs)
+{
+    return ahrs->quaternion;
+}
+
+/**
+ * @brief Sets the quaternion describing the sensor relative to the Earth.
+ * @param ahrs AHRS algorithm structure.
+ * @param quaternion Quaternion describing the sensor relative to the Earth.
+ */
+void FusionAhrsSetQuaternion(FusionAhrs *const ahrs, const FusionQuaternion quaternion)
+{
+    ahrs->quaternion = quaternion;
+}
+
+/**
+ * @brief Returns the linear acceleration measurement equal to the accelerometer
+ * measurement with the 1 g of gravity removed.
+ * @param ahrs AHRS algorithm structure.
+ * @return Linear acceleration measurement in g.
+ */
+FusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs)
+{
+#define Q ahrs->quaternion.element
+
+    // Calculate gravity in the sensor coordinate frame
+    const FusionVector gravity = {.axis = {
+                                      .x = 2.0f * (Q.x * Q.z - Q.w * Q.y),
+                                      .y = 2.0f * (Q.y * Q.z + Q.w * Q.x),
+                                      .z = 2.0f * (Q.w * Q.w - 0.5f + Q.z * Q.z),
+                                  }}; // third column of transposed rotation matrix
+
+    // Remove gravity from accelerometer measurement
+    switch (ahrs->settings.convention) {
+    case FusionConventionNwu:
+    case FusionConventionEnu: {
+        return FusionVectorSubtract(ahrs->accelerometer, gravity);
+    }
+    case FusionConventionNed: {
+        return FusionVectorAdd(ahrs->accelerometer, gravity);
+    }
+    }
+    return FUSION_VECTOR_ZERO; // avoid compiler warning
+#undef Q
+}
+
+/**
+ * @brief Returns the Earth acceleration measurement equal to accelerometer
+ * measurement in the Earth coordinate frame with the 1 g of gravity removed.
+ * @param ahrs AHRS algorithm structure.
+ * @return Earth acceleration measurement in g.
+ */
+FusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs)
+{
+#define Q ahrs->quaternion.element
+#define A ahrs->accelerometer.axis
+
+    // Calculate accelerometer measurement in the Earth coordinate frame
+    const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations
+    const float qwqx = Q.w * Q.x;
+    const float qwqy = Q.w * Q.y;
+    const float qwqz = Q.w * Q.z;
+    const float qxqy = Q.x * Q.y;
+    const float qxqz = Q.x * Q.z;
+    const float qyqz = Q.y * Q.z;
+    FusionVector accelerometer = {.axis = {
+                                      .x = 2.0f * ((qwqw - 0.5f + Q.x * Q.x) * A.x + (qxqy - qwqz) * A.y + (qxqz + qwqy) * A.z),
+                                      .y = 2.0f * ((qxqy + qwqz) * A.x + (qwqw - 0.5f + Q.y * Q.y) * A.y + (qyqz - qwqx) * A.z),
+                                      .z = 2.0f * ((qxqz - qwqy) * A.x + (qyqz + qwqx) * A.y + (qwqw - 0.5f + Q.z * Q.z) * A.z),
+                                  }}; // rotation matrix multiplied with the accelerometer
+
+    // Remove gravity from accelerometer measurement
+    switch (ahrs->settings.convention) {
+    case FusionConventionNwu:
+    case FusionConventionEnu:
+        accelerometer.axis.z -= 1.0f;
+        break;
+    case FusionConventionNed:
+        accelerometer.axis.z += 1.0f;
+        break;
+    }
+    return accelerometer;
+#undef Q
+#undef A
+}
+
+/**
+ * @brief Returns the AHRS algorithm internal states.
+ * @param ahrs AHRS algorithm structure.
+ * @return AHRS algorithm internal states.
+ */
+FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahrs)
+{
+    const FusionAhrsInternalStates internalStates = {
+        .accelerationError = FusionRadiansToDegrees(FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfAccelerometerFeedback))),
+        .accelerometerIgnored = ahrs->accelerometerIgnored,
+        .accelerationRecoveryTrigger =
+            ahrs->settings.recoveryTriggerPeriod == 0
+                ? 0.0f
+                : (float)ahrs->accelerationRecoveryTrigger / (float)ahrs->settings.recoveryTriggerPeriod,
+        .magneticError = FusionRadiansToDegrees(FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfMagnetometerFeedback))),
+        .magnetometerIgnored = ahrs->magnetometerIgnored,
+        .magneticRecoveryTrigger = ahrs->settings.recoveryTriggerPeriod == 0
+                                       ? 0.0f
+                                       : (float)ahrs->magneticRecoveryTrigger / (float)ahrs->settings.recoveryTriggerPeriod,
+    };
+    return internalStates;
+}
+
+/**
+ * @brief Returns the AHRS algorithm flags.
+ * @param ahrs AHRS algorithm structure.
+ * @return AHRS algorithm flags.
+ */
+FusionAhrsFlags FusionAhrsGetFlags(const FusionAhrs *const ahrs)
+{
+    const FusionAhrsFlags flags = {
+        .initialising = ahrs->initialising,
+        .angularRateRecovery = ahrs->angularRateRecovery,
+        .accelerationRecovery = ahrs->accelerationRecoveryTrigger > ahrs->accelerationRecoveryTimeout,
+        .magneticRecovery = ahrs->magneticRecoveryTrigger > ahrs->magneticRecoveryTimeout,
+    };
+    return flags;
+}
+
+/**
+ * @brief Sets the heading of the orientation measurement provided by the AHRS
+ * algorithm.  This function can be used to reset drift in heading when the AHRS
+ * algorithm is being used without a magnetometer.
+ * @param ahrs AHRS algorithm structure.
+ * @param heading Heading angle in degrees.
+ */
+void FusionAhrsSetHeading(FusionAhrs *const ahrs, const float heading)
+{
+#define Q ahrs->quaternion.element
+    const float yaw = atan2f(Q.w * Q.z + Q.x * Q.y, 0.5f - Q.y * Q.y - Q.z * Q.z);
+    const float halfYawMinusHeading = 0.5f * (yaw - FusionDegreesToRadians(heading));
+    const FusionQuaternion rotation = {.element = {
+                                           .w = cosf(halfYawMinusHeading),
+                                           .x = 0.0f,
+                                           .y = 0.0f,
+                                           .z = -1.0f * sinf(halfYawMinusHeading),
+                                       }};
+    ahrs->quaternion = FusionQuaternionMultiply(rotation, ahrs->quaternion);
+#undef Q
+}
+
+//------------------------------------------------------------------------------
+// End of file

src/Fusion/FusionAhrs.h

@@ -0,0 +1,112 @@
+/**
+ * @file FusionAhrs.h
+ * @author Seb Madgwick
+ * @brief AHRS algorithm to combine gyroscope, accelerometer, and magnetometer
+ * measurements into a single measurement of orientation relative to the Earth.
+ */
+
+#ifndef FUSION_AHRS_H
+#define FUSION_AHRS_H
+
+//------------------------------------------------------------------------------
+// Includes
+
+#include "FusionConvention.h"
+#include "FusionMath.h"
+#include <stdbool.h>
+
+//------------------------------------------------------------------------------
+// Definitions
+
+/**
+ * @brief AHRS algorithm settings.
+ */
+typedef struct {
+    FusionConvention convention;
+    float gain;
+    float gyroscopeRange;
+    float accelerationRejection;
+    float magneticRejection;
+    unsigned int recoveryTriggerPeriod;
+} FusionAhrsSettings;
+
+/**
+ * @brief AHRS algorithm structure.  Structure members are used internally and
+ * must not be accessed by the application.
+ */
+typedef struct {
+    FusionAhrsSettings settings;
+    FusionQuaternion quaternion;
+    FusionVector accelerometer;
+    bool initialising;
+    float rampedGain;
+    float rampedGainStep;
+    bool angularRateRecovery;
+    FusionVector halfAccelerometerFeedback;
+    FusionVector halfMagnetometerFeedback;
+    bool accelerometerIgnored;
+    int accelerationRecoveryTrigger;
+    int accelerationRecoveryTimeout;
+    bool magnetometerIgnored;
+    int magneticRecoveryTrigger;
+    int magneticRecoveryTimeout;
+} FusionAhrs;
+
+/**
+ * @brief AHRS algorithm internal states.
+ */
+typedef struct {
+    float accelerationError;
+    bool accelerometerIgnored;
+    float accelerationRecoveryTrigger;
+    float magneticError;
+    bool magnetometerIgnored;
+    float magneticRecoveryTrigger;
+} FusionAhrsInternalStates;
+
+/**
+ * @brief AHRS algorithm flags.
+ */
+typedef struct {
+    bool initialising;
+    bool angularRateRecovery;
+    bool accelerationRecovery;
+    bool magneticRecovery;
+} FusionAhrsFlags;
+
+//------------------------------------------------------------------------------
+// Function declarations
+
+void FusionAhrsInitialise(FusionAhrs *const ahrs);
+
+void FusionAhrsReset(FusionAhrs *const ahrs);
+
+void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *const settings);
+
+void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer,
+                      const FusionVector magnetometer, const float deltaTime);
+
+void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer,
+                                    const float deltaTime);
+
+void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer,
+                                     const float heading, const float deltaTime);
+
+FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs);
+
+void FusionAhrsSetQuaternion(FusionAhrs *const ahrs, const FusionQuaternion quaternion);
+
+FusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs);
+
+FusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs);
+
+FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahrs);
+
+FusionAhrsFlags FusionAhrsGetFlags(const FusionAhrs *const ahrs);
+
+void FusionAhrsSetHeading(FusionAhrs *const ahrs, const float heading);
+
+#endif
+
+//------------------------------------------------------------------------------
+// End of file

src/Fusion/FusionAxes.h

@@ -0,0 +1,188 @@
+/**
+ * @file FusionAxes.h
+ * @author Seb Madgwick
+ * @brief Swaps sensor axes for alignment with the body axes.
+ */
+
+#ifndef FUSION_AXES_H
+#define FUSION_AXES_H
+
+//------------------------------------------------------------------------------
+// Includes
+
+#include "FusionMath.h"
+
+//------------------------------------------------------------------------------
+// Definitions
+
+/**
+ * @brief Axes alignment describing the sensor axes relative to the body axes.
+ * For example, if the body X axis is aligned with the sensor Y axis and the
+ * body Y axis is aligned with sensor X axis but pointing the opposite direction
+ * then alignment is +Y-X+Z.
+ */
+typedef enum {
+    FusionAxesAlignmentPXPYPZ, /* +X+Y+Z */
+    FusionAxesAlignmentPXNZPY, /* +X-Z+Y */
+    FusionAxesAlignmentPXNYNZ, /* +X-Y-Z */
+    FusionAxesAlignmentPXPZNY, /* +X+Z-Y */
+    FusionAxesAlignmentNXPYNZ, /* -X+Y-Z */
+    FusionAxesAlignmentNXPZPY, /* -X+Z+Y */
+    FusionAxesAlignmentNXNYPZ, /* -X-Y+Z */
+    FusionAxesAlignmentNXNZNY, /* -X-Z-Y */
+    FusionAxesAlignmentPYNXPZ, /* +Y-X+Z */
+    FusionAxesAlignmentPYNZNX, /* +Y-Z-X */
+    FusionAxesAlignmentPYPXNZ, /* +Y+X-Z */
+    FusionAxesAlignmentPYPZPX, /* +Y+Z+X */
+    FusionAxesAlignmentNYPXPZ, /* -Y+X+Z */
+    FusionAxesAlignmentNYNZPX, /* -Y-Z+X */
+    FusionAxesAlignmentNYNXNZ, /* -Y-X-Z */
+    FusionAxesAlignmentNYPZNX, /* -Y+Z-X */
+    FusionAxesAlignmentPZPYNX, /* +Z+Y-X */
+    FusionAxesAlignmentPZPXPY, /* +Z+X+Y */
+    FusionAxesAlignmentPZNYPX, /* +Z-Y+X */
+    FusionAxesAlignmentPZNXNY, /* +Z-X-Y */
+    FusionAxesAlignmentNZPYPX, /* -Z+Y+X */
+    FusionAxesAlignmentNZNXPY, /* -Z-X+Y */
+    FusionAxesAlignmentNZNYNX, /* -Z-Y-X */
+    FusionAxesAlignmentNZPXNY, /* -Z+X-Y */
+} FusionAxesAlignment;
+
+//------------------------------------------------------------------------------
+// Inline functions
+
+/**
+ * @brief Swaps sensor axes for alignment with the body axes.
+ * @param sensor Sensor axes.
+ * @param alignment Axes alignment.
+ * @return Sensor axes aligned with the body axes.
+ */
+static inline FusionVector FusionAxesSwap(const FusionVector sensor, const FusionAxesAlignment alignment)
+{
+    FusionVector result;
+    switch (alignment) {
+    case FusionAxesAlignmentPXPYPZ:
+        break;
+    case FusionAxesAlignmentPXNZPY:
+        result.axis.x = +sensor.axis.x;
+        result.axis.y = -sensor.axis.z;
+        result.axis.z = +sensor.axis.y;
+        return result;
+    case FusionAxesAlignmentPXNYNZ:
+        result.axis.x = +sensor.axis.x;
+        result.axis.y = -sensor.axis.y;
+        result.axis.z = -sensor.axis.z;
+        return result;
+    case FusionAxesAlignmentPXPZNY:
+        result.axis.x = +sensor.axis.x;
+        result.axis.y = +sensor.axis.z;
+        result.axis.z = -sensor.axis.y;
+        return result;
+    case FusionAxesAlignmentNXPYNZ:
+        result.axis.x = -sensor.axis.x;
+        result.axis.y = +sensor.axis.y;
+        result.axis.z = -sensor.axis.z;
+        return result;
+    case FusionAxesAlignmentNXPZPY:
+        result.axis.x = -sensor.axis.x;
+        result.axis.y = +sensor.axis.z;
+        result.axis.z = +sensor.axis.y;
+        return result;
+    case FusionAxesAlignmentNXNYPZ:
+        result.axis.x = -sensor.axis.x;
+        result.axis.y = -sensor.axis.y;
+        result.axis.z = +sensor.axis.z;
+        return result;
+    case FusionAxesAlignmentNXNZNY:
+        result.axis.x = -sensor.axis.x;
+        result.axis.y = -sensor.axis.z;
+        result.axis.z = -sensor.axis.y;
+        return result;
+    case FusionAxesAlignmentPYNXPZ:
+        result.axis.x = +sensor.axis.y;
+        result.axis.y = -sensor.axis.x;
+        result.axis.z = +sensor.axis.z;
+        return result;
+    case FusionAxesAlignmentPYNZNX:
+        result.axis.x = +sensor.axis.y;
+        result.axis.y = -sensor.axis.z;
+        result.axis.z = -sensor.axis.x;
+        return result;
+    case FusionAxesAlignmentPYPXNZ:
+        result.axis.x = +sensor.axis.y;
+        result.axis.y = +sensor.axis.x;
+        result.axis.z = -sensor.axis.z;
+        return result;
+    case FusionAxesAlignmentPYPZPX:
+        result.axis.x = +sensor.axis.y;
+        result.axis.y = +sensor.axis.z;
+        result.axis.z = +sensor.axis.x;
+        return result;
+    case FusionAxesAlignmentNYPXPZ:
+        result.axis.x = -sensor.axis.y;
+        result.axis.y = +sensor.axis.x;
+        result.axis.z = +sensor.axis.z;
+        return result;
+    case FusionAxesAlignmentNYNZPX:
+        result.axis.x = -sensor.axis.y;
+        result.axis.y = -sensor.axis.z;
+        result.axis.z = +sensor.axis.x;
+        return result;
+    case FusionAxesAlignmentNYNXNZ:
+        result.axis.x = -sensor.axis.y;
+        result.axis.y = -sensor.axis.x;
+        result.axis.z = -sensor.axis.z;
+        return result;
+    case FusionAxesAlignmentNYPZNX:
+        result.axis.x = -sensor.axis.y;
+        result.axis.y = +sensor.axis.z;
+        result.axis.z = -sensor.axis.x;
+        return result;
+    case FusionAxesAlignmentPZPYNX:
+        result.axis.x = +sensor.axis.z;
+        result.axis.y = +sensor.axis.y;
+        result.axis.z = -sensor.axis.x;
+        return result;
+    case FusionAxesAlignmentPZPXPY:
+        result.axis.x = +sensor.axis.z;
+        result.axis.y = +sensor.axis.x;
+        result.axis.z = +sensor.axis.y;
+        return result;
+    case FusionAxesAlignmentPZNYPX:
+        result.axis.x = +sensor.axis.z;
+        result.axis.y = -sensor.axis.y;
+        result.axis.z = +sensor.axis.x;
+        return result;
+    case FusionAxesAlignmentPZNXNY:
+        result.axis.x = +sensor.axis.z;
+        result.axis.y = -sensor.axis.x;
+        result.axis.z = -sensor.axis.y;
+        return result;
+    case FusionAxesAlignmentNZPYPX:
+        result.axis.x = -sensor.axis.z;
+        result.axis.y = +sensor.axis.y;
+        result.axis.z = +sensor.axis.x;
+        return result;
+    case FusionAxesAlignmentNZNXPY:
+        result.axis.x = -sensor.axis.z;
+        result.axis.y = -sensor.axis.x;
+        result.axis.z = +sensor.axis.y;
+        return result;
+    case FusionAxesAlignmentNZNYNX:
+        result.axis.x = -sensor.axis.z;
+        result.axis.y = -sensor.axis.y;
+        result.axis.z = -sensor.axis.x;
+        return result;
+    case FusionAxesAlignmentNZPXNY:
+        result.axis.x = -sensor.axis.z;
+        result.axis.y = +sensor.axis.x;
+        result.axis.z = -sensor.axis.y;
+        return result;
+    }
+    return sensor; // avoid compiler warning
+}
+
+#endif
+
+//------------------------------------------------------------------------------
+// End of file

src/Fusion/FusionCalibration.h

@@ -0,0 +1,49 @@
+/**
+ * @file FusionCalibration.h
+ * @author Seb Madgwick
+ * @brief Gyroscope, accelerometer, and magnetometer calibration models.
+ */
+
+#ifndef FUSION_CALIBRATION_H
+#define FUSION_CALIBRATION_H
+
+//------------------------------------------------------------------------------
+// Includes
+
+#include "FusionMath.h"
+
+//------------------------------------------------------------------------------
+// Inline functions
+
+/**
+ * @brief Gyroscope and accelerometer calibration model.
+ * @param uncalibrated Uncalibrated measurement.
+ * @param misalignment Misalignment matrix.
+ * @param sensitivity Sensitivity.
+ * @param offset Offset.
+ * @return Calibrated measurement.
+ */
+static inline FusionVector FusionCalibrationInertial(const FusionVector uncalibrated, const FusionMatrix misalignment,
+                                                     const FusionVector sensitivity, const FusionVector offset)
+{
+    return FusionMatrixMultiplyVector(misalignment,
+                                      FusionVectorHadamardProduct(FusionVectorSubtract(uncalibrated, offset), sensitivity));
+}
+
+/**
+ * @brief Magnetometer calibration model.
+ * @param uncalibrated Uncalibrated measurement.
+ * @param softIronMatrix Soft-iron matrix.
+ * @param hardIronOffset Hard-iron offset.
+ * @return Calibrated measurement.
+ */
+static inline FusionVector FusionCalibrationMagnetic(const FusionVector uncalibrated, const FusionMatrix softIronMatrix,
+                                                     const FusionVector hardIronOffset)
+{
+    return FusionMatrixMultiplyVector(softIronMatrix, FusionVectorSubtract(uncalibrated, hardIronOffset));
+}
+
+#endif
+
+//------------------------------------------------------------------------------
+// End of file

src/Fusion/FusionCompass.c

@@ -0,0 +1,51 @@
+/**
+ * @file FusionCompass.c
+ * @author Seb Madgwick
+ * @brief Tilt-compensated compass to calculate the magnetic heading using
+ * accelerometer and magnetometer measurements.
+ */
+
+//------------------------------------------------------------------------------
+// Includes
+
+#include "FusionCompass.h"
+#include "FusionAxes.h"
+#include <math.h> // atan2f
+
+//------------------------------------------------------------------------------
+// Functions
+
+/**
+ * @brief Calculates the magnetic heading.
+ * @param convention Earth axes convention.
+ * @param accelerometer Accelerometer measurement in any calibrated units.
+ * @param magnetometer Magnetometer measurement in any calibrated units.
+ * @return Heading angle in degrees.
+ */
+float FusionCompassCalculateHeading(const FusionConvention convention, const FusionVector accelerometer,
+                                    const FusionVector magnetometer)
+{
+    switch (convention) {
+    case FusionConventionNwu: {
+        const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(accelerometer, magnetometer));
+        const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, accelerometer));
+        return FusionRadiansToDegrees(atan2f(west.axis.x, north.axis.x));
+    }
+    case FusionConventionEnu: {
+        const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(accelerometer, magnetometer));
+        const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, accelerometer));
+        const FusionVector east = FusionVectorMultiplyScalar(west, -1.0f);
+        return FusionRadiansToDegrees(atan2f(north.axis.x, east.axis.x));
+    }
+    case FusionConventionNed: {
+        const FusionVector up = FusionVectorMultiplyScalar(accelerometer, -1.0f);
+        const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(up, magnetometer));
+        const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, up));
+        return FusionRadiansToDegrees(atan2f(west.axis.x, north.axis.x));
+    }
+    }
+    return 0; // avoid compiler warning
+}
+
+//------------------------------------------------------------------------------
+// End of file

src/Fusion/FusionCompass.h

@@ -0,0 +1,26 @@
+/**
+ * @file FusionCompass.h
+ * @author Seb Madgwick
+ * @brief Tilt-compensated compass to calculate the magnetic heading using
+ * accelerometer and magnetometer measurements.
+ */
+
+#ifndef FUSION_COMPASS_H
+#define FUSION_COMPASS_H
+
+//------------------------------------------------------------------------------
+// Includes
+
+#include "FusionConvention.h"
+#include "FusionMath.h"
+
+//------------------------------------------------------------------------------
+// Function declarations
+
+float FusionCompassCalculateHeading(const FusionConvention convention, const FusionVector accelerometer,
+                                    const FusionVector magnetometer);
+
+#endif
+
+//------------------------------------------------------------------------------
+// End of file

src/Fusion/FusionConvention.h

@@ -0,0 +1,25 @@
+/**
+ * @file FusionConvention.h
+ * @author Seb Madgwick
+ * @brief Earth axes convention.
+ */
+
+#ifndef FUSION_CONVENTION_H
+#define FUSION_CONVENTION_H
+
+//------------------------------------------------------------------------------
+// Definitions
+
+/**
+ * @brief Earth axes convention.
+ */
+typedef enum {
+    FusionConventionNwu, /* North-West-Up */
+    FusionConventionEnu, /* East-North-Up */
+    FusionConventionNed, /* North-East-Down */
+} FusionConvention;
+
+#endif
+
+//------------------------------------------------------------------------------
+// End of file

src/Fusion/FusionMath.h

@@ -0,0 +1,503 @@
+/**
+ * @file FusionMath.h
+ * @author Seb Madgwick
+ * @brief Math library.
+ */
+
+#ifndef FUSION_MATH_H
+#define FUSION_MATH_H
+
+//------------------------------------------------------------------------------
+// Includes
+
+#include <math.h> // M_PI, sqrtf, atan2f, asinf
+#include <stdbool.h>
+#include <stdint.h>
+
+//------------------------------------------------------------------------------
+// Definitions
+
+/**
+ * @brief 3D vector.
+ */
+typedef union {
+    float array[3];
+
+    struct {
+        float x;
+        float y;
+        float z;
+    } axis;
+} FusionVector;
+
+/**
+ * @brief Quaternion.
+ */
+typedef union {
+    float array[4];
+
+    struct {
+        float w;
+        float x;
+        float y;
+        float z;
+    } element;
+} FusionQuaternion;
+
+/**
+ * @brief 3x3 matrix in row-major order.
+ * See http://en.wikipedia.org/wiki/Row-major_order
+ */
+typedef union {
+    float array[3][3];
+
+    struct {
+        float xx;
+        float xy;
+        float xz;
+        float yx;
+        float yy;
+        float yz;
+        float zx;
+        float zy;
+        float zz;
+    } element;
+} FusionMatrix;
+
+/**
+ * @brief Euler angles.  Roll, pitch, and yaw correspond to rotations around
+ * X, Y, and Z respectively.
+ */
+typedef union {
+    float array[3];
+
+    struct {
+        float roll;
+        float pitch;
+        float yaw;
+    } angle;
+} FusionEuler;
+
+/**
+ * @brief Vector of zeros.
+ */
+#define FUSION_VECTOR_ZERO ((FusionVector){.array = {0.0f, 0.0f, 0.0f}})
+
+/**
+ * @brief Vector of ones.
+ */
+#define FUSION_VECTOR_ONES ((FusionVector){.array = {1.0f, 1.0f, 1.0f}})
+
+/**
+ * @brief Identity quaternion.
+ */
+#define FUSION_IDENTITY_QUATERNION ((FusionQuaternion){.array = {1.0f, 0.0f, 0.0f, 0.0f}})
+
+/**
+ * @brief Identity matrix.
+ */
+#define FUSION_IDENTITY_MATRIX ((FusionMatrix){.array = {{1.0f, 0.0f, 0.0f}, {0.0f, 1.0f, 0.0f}, {0.0f, 0.0f, 1.0f}}})
+
+/**
+ * @brief Euler angles of zero.
+ */
+#define FUSION_EULER_ZERO ((FusionEuler){.array = {0.0f, 0.0f, 0.0f}})
+
+/**
+ * @brief Pi. May not be defined in math.h.
+ */
+#ifndef M_PI
+#define M_PI (3.14159265358979323846)
+#endif
+
+/**
+ * @brief Include this definition or add as a preprocessor definition to use
+ * normal square root operations.
+ */
+// #define FUSION_USE_NORMAL_SQRT
+
+//------------------------------------------------------------------------------
+// Inline functions - Degrees and radians conversion
+
+/**
+ * @brief Converts degrees to radians.
+ * @param degrees Degrees.
+ * @return Radians.
+ */
+static inline float FusionDegreesToRadians(const float degrees)
+{
+    return degrees * ((float)M_PI / 180.0f);
+}
+
+/**
+ * @brief Converts radians to degrees.
+ * @param radians Radians.
+ * @return Degrees.
+ */
+static inline float FusionRadiansToDegrees(const float radians)
+{
+    return radians * (180.0f / (float)M_PI);
+}
+
+//------------------------------------------------------------------------------
+// Inline functions - Arc sine
+
+/**
+ * @brief Returns the arc sine of the value.
+ * @param value Value.
+ * @return Arc sine of the value.
+ */
+static inline float FusionAsin(const float value)
+{
+    if (value <= -1.0f) {
+        return (float)M_PI / -2.0f;
+    }
+    if (value >= 1.0f) {
+        return (float)M_PI / 2.0f;
+    }
+    return asinf(value);
+}
+
+//------------------------------------------------------------------------------
+// Inline functions - Fast inverse square root
+
+#ifndef FUSION_USE_NORMAL_SQRT
+
+/**
+ * @brief Calculates the reciprocal of the square root.
+ * See https://pizer.wordpress.com/2008/10/12/fast-inverse-square-root/
+ * @param x Operand.
+ * @return Reciprocal of the square root of x.
+ */
+static inline float FusionFastInverseSqrt(const float x)
+{
+
+    typedef union {
+        float f;
+        int32_t i;
+    } Union32;
+
+    Union32 union32 = {.f = x};
+    union32.i = 0x5F1F1412 - (union32.i >> 1);
+    return union32.f * (1.69000231f - 0.714158168f * x * union32.f * union32.f);
+}
+
+#endif
+
+//------------------------------------------------------------------------------
+// Inline functions - Vector operations
+
+/**
+ * @brief Returns true if the vector is zero.
+ * @param vector Vector.
+ * @return True if the vector is zero.
+ */
+static inline bool FusionVectorIsZero(const FusionVector vector)
+{
+    return (vector.axis.x == 0.0f) && (vector.axis.y == 0.0f) && (vector.axis.z == 0.0f);
+}
+
+/**
+ * @brief Returns the sum of two vectors.
+ * @param vectorA Vector A.
+ * @param vectorB Vector B.
+ * @return Sum of two vectors.
+ */
+static inline FusionVector FusionVectorAdd(const FusionVector vectorA, const FusionVector vectorB)
+{
+    const FusionVector result = {.axis = {
+                                     .x = vectorA.axis.x + vectorB.axis.x,
+                                     .y = vectorA.axis.y + vectorB.axis.y,
+                                     .z = vectorA.axis.z + vectorB.axis.z,
+                                 }};
+    return result;
+}
+
+/**
+ * @brief Returns vector B subtracted from vector A.
+ * @param vectorA Vector A.
+ * @param vectorB Vector B.
+ * @return Vector B subtracted from vector A.
+ */
+static inline FusionVector FusionVectorSubtract(const FusionVector vectorA, const FusionVector vectorB)
+{
+    const FusionVector result = {.axis = {
+                                     .x = vectorA.axis.x - vectorB.axis.x,
+                                     .y = vectorA.axis.y - vectorB.axis.y,
+                                     .z = vectorA.axis.z - vectorB.axis.z,
+                                 }};
+    return result;
+}
+
+/**
+ * @brief Returns the sum of the elements.
+ * @param vector Vector.
+ * @return Sum of the elements.
+ */
+static inline float FusionVectorSum(const FusionVector vector)
+{
+    return vector.axis.x + vector.axis.y + vector.axis.z;
+}
+
+/**
+ * @brief Returns the multiplication of a vector by a scalar.
+ * @param vector Vector.
+ * @param scalar Scalar.
+ * @return Multiplication of a vector by a scalar.
+ */
+static inline FusionVector FusionVectorMultiplyScalar(const FusionVector vector, const float scalar)
+{
+    const FusionVector result = {.axis = {
+                                     .x = vector.axis.x * scalar,
+                                     .y = vector.axis.y * scalar,
+                                     .z = vector.axis.z * scalar,
+                                 }};
+    return result;
+}
+
+/**
+ * @brief Calculates the Hadamard product (element-wise multiplication).
+ * @param vectorA Vector A.
+ * @param vectorB Vector B.
+ * @return Hadamard product.
+ */
+static inline FusionVector FusionVectorHadamardProduct(const FusionVector vectorA, const FusionVector vectorB)
+{
+    const FusionVector result = {.axis = {
+                                     .x = vectorA.axis.x * vectorB.axis.x,
+                                     .y = vectorA.axis.y * vectorB.axis.y,
+                                     .z = vectorA.axis.z * vectorB.axis.z,
+                                 }};
+    return result;
+}
+
+/**
+ * @brief Returns the cross product.
+ * @param vectorA Vector A.
+ * @param vectorB Vector B.
+ * @return Cross product.
+ */
+static inline FusionVector FusionVectorCrossProduct(const FusionVector vectorA, const FusionVector vectorB)
+{
+#define A vectorA.axis
+#define B vectorB.axis
+    const FusionVector result = {.axis = {
+                                     .x = A.y * B.z - A.z * B.y,
+                                     .y = A.z * B.x - A.x * B.z,
+                                     .z = A.x * B.y - A.y * B.x,
+                                 }};
+    return result;
+#undef A
+#undef B
+}
+
+/**
+ * @brief Returns the dot product.
+ * @param vectorA Vector A.
+ * @param vectorB Vector B.
+ * @return Dot product.
+ */
+static inline float FusionVectorDotProduct(const FusionVector vectorA, const FusionVector vectorB)
+{
+    return FusionVectorSum(FusionVectorHadamardProduct(vectorA, vectorB));
+}
+
+/**
+ * @brief Returns the vector magnitude squared.
+ * @param vector Vector.
+ * @return Vector magnitude squared.
+ */
+static inline float FusionVectorMagnitudeSquared(const FusionVector vector)
+{
+    return FusionVectorSum(FusionVectorHadamardProduct(vector, vector));
+}
+
+/**
+ * @brief Returns the vector magnitude.
+ * @param vector Vector.
+ * @return Vector magnitude.
+ */
+static inline float FusionVectorMagnitude(const FusionVector vector)
+{
+    return sqrtf(FusionVectorMagnitudeSquared(vector));
+}
+
+/**
+ * @brief Returns the normalised vector.
+ * @param vector Vector.
+ * @return Normalised vector.
+ */
+static inline FusionVector FusionVectorNormalise(const FusionVector vector)
+{
+#ifdef FUSION_USE_NORMAL_SQRT
+    const float magnitudeReciprocal = 1.0f / sqrtf(FusionVectorMagnitudeSquared(vector));
+#else
+    const float magnitudeReciprocal = FusionFastInverseSqrt(FusionVectorMagnitudeSquared(vector));
+#endif
+    return FusionVectorMultiplyScalar(vector, magnitudeReciprocal);
+}
+
+//------------------------------------------------------------------------------
+// Inline functions - Quaternion operations
+
+/**
+ * @brief Returns the sum of two quaternions.
+ * @param quaternionA Quaternion A.
+ * @param quaternionB Quaternion B.
+ * @return Sum of two quaternions.
+ */
+static inline FusionQuaternion FusionQuaternionAdd(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB)
+{
+    const FusionQuaternion result = {.element = {
+                                         .w = quaternionA.element.w + quaternionB.element.w,
+                                         .x = quaternionA.element.x + quaternionB.element.x,
+                                         .y = quaternionA.element.y + quaternionB.element.y,
+                                         .z = quaternionA.element.z + quaternionB.element.z,
+                                     }};
+    return result;
+}
+
+/**
+ * @brief Returns the multiplication of two quaternions.
+ * @param quaternionA Quaternion A (to be post-multiplied).
+ * @param quaternionB Quaternion B (to be pre-multiplied).
+ * @return Multiplication of two quaternions.
+ */
+static inline FusionQuaternion FusionQuaternionMultiply(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB)
+{
+#define A quaternionA.element
+#define B quaternionB.element
+    const FusionQuaternion result = {.element = {
+                                         .w = A.w * B.w - A.x * B.x - A.y * B.y - A.z * B.z,
+                                         .x = A.w * B.x + A.x * B.w + A.y * B.z - A.z * B.y,
+                                         .y = A.w * B.y - A.x * B.z + A.y * B.w + A.z * B.x,
+                                         .z = A.w * B.z + A.x * B.y - A.y * B.x + A.z * B.w,
+                                     }};
+    return result;
+#undef A
+#undef B
+}
+
+/**
+ * @brief Returns the multiplication of a quaternion with a vector.  This is a
+ * normal quaternion multiplication where the vector is treated a
+ * quaternion with a W element value of zero.  The quaternion is post-
+ * multiplied by the vector.
+ * @param quaternion Quaternion.
+ * @param vector Vector.
+ * @return Multiplication of a quaternion with a vector.
+ */
+static inline FusionQuaternion FusionQuaternionMultiplyVector(const FusionQuaternion quaternion, const FusionVector vector)
+{
+#define Q quaternion.element
+#define V vector.axis
+    const FusionQuaternion result = {.element = {
+                                         .w = -Q.x * V.x - Q.y * V.y - Q.z * V.z,
+                                         .x = Q.w * V.x + Q.y * V.z - Q.z * V.y,
+                                         .y = Q.w * V.y - Q.x * V.z + Q.z * V.x,
+                                         .z = Q.w * V.z + Q.x * V.y - Q.y * V.x,
+                                     }};
+    return result;
+#undef Q
+#undef V
+}
+
+/**
+ * @brief Returns the normalised quaternion.
+ * @param quaternion Quaternion.
+ * @return Normalised quaternion.
+ */
+static inline FusionQuaternion FusionQuaternionNormalise(const FusionQuaternion quaternion)
+{
+#define Q quaternion.element
+#ifdef FUSION_USE_NORMAL_SQRT
+    const float magnitudeReciprocal = 1.0f / sqrtf(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z);
+#else
+    const float magnitudeReciprocal = FusionFastInverseSqrt(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z);
+#endif
+    const FusionQuaternion result = {.element = {
+                                         .w = Q.w * magnitudeReciprocal,
+                                         .x = Q.x * magnitudeReciprocal,
+                                         .y = Q.y * magnitudeReciprocal,
+                                         .z = Q.z * magnitudeReciprocal,
+                                     }};
+    return result;
+#undef Q
+}
+
+//------------------------------------------------------------------------------
+// Inline functions - Matrix operations
+
+/**
+ * @brief Returns the multiplication of a matrix with a vector.
+ * @param matrix Matrix.
+ * @param vector Vector.
+ * @return Multiplication of a matrix with a vector.
+ */
+static inline FusionVector FusionMatrixMultiplyVector(const FusionMatrix matrix, const FusionVector vector)
+{
+#define R matrix.element
+    const FusionVector result = {.axis = {
+                                     .x = R.xx * vector.axis.x + R.xy * vector.axis.y + R.xz * vector.axis.z,
+                                     .y = R.yx * vector.axis.x + R.yy * vector.axis.y + R.yz * vector.axis.z,
+                                     .z = R.zx * vector.axis.x + R.zy * vector.axis.y + R.zz * vector.axis.z,
+                                 }};
+    return result;
+#undef R
+}
+
+//------------------------------------------------------------------------------
+// Inline functions - Conversion operations
+
+/**
+ * @brief Converts a quaternion to a rotation matrix.
+ * @param quaternion Quaternion.
+ * @return Rotation matrix.
+ */
+static inline FusionMatrix FusionQuaternionToMatrix(const FusionQuaternion quaternion)
+{
+#define Q quaternion.element
+    const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations
+    const float qwqx = Q.w * Q.x;
+    const float qwqy = Q.w * Q.y;
+    const float qwqz = Q.w * Q.z;
+    const float qxqy = Q.x * Q.y;
+    const float qxqz = Q.x * Q.z;
+    const float qyqz = Q.y * Q.z;
+    const FusionMatrix matrix = {.element = {
+                                     .xx = 2.0f * (qwqw - 0.5f + Q.x * Q.x),
+                                     .xy = 2.0f * (qxqy - qwqz),
+                                     .xz = 2.0f * (qxqz + qwqy),
+                                     .yx = 2.0f * (qxqy + qwqz),
+                                     .yy = 2.0f * (qwqw - 0.5f + Q.y * Q.y),
+                                     .yz = 2.0f * (qyqz - qwqx),
+                                     .zx = 2.0f * (qxqz - qwqy),
+                                     .zy = 2.0f * (qyqz + qwqx),
+                                     .zz = 2.0f * (qwqw - 0.5f + Q.z * Q.z),
+                                 }};
+    return matrix;
+#undef Q
+}
+
+/**
+ * @brief Converts a quaternion to ZYX Euler angles in degrees.
+ * @param quaternion Quaternion.
+ * @return Euler angles in degrees.
+ */
+static inline FusionEuler FusionQuaternionToEuler(const FusionQuaternion quaternion)
+{
+#define Q quaternion.element
+    const float halfMinusQySquared = 0.5f - Q.y * Q.y; // calculate common terms to avoid repeated operations
+    const FusionEuler euler = {.angle = {
+                                   .roll = FusionRadiansToDegrees(atan2f(Q.w * Q.x + Q.y * Q.z, halfMinusQySquared - Q.x * Q.x)),
+                                   .pitch = FusionRadiansToDegrees(FusionAsin(2.0f * (Q.w * Q.y - Q.z * Q.x))),
+                                   .yaw = FusionRadiansToDegrees(atan2f(Q.w * Q.z + Q.x * Q.y, halfMinusQySquared - Q.z * Q.z)),
+                               }};
+    return euler;
+#undef Q
+}
+
+#endif
+
+//------------------------------------------------------------------------------
+// End of file

src/Fusion/FusionOffset.c

@@ -0,0 +1,80 @@
+/**
+ * @file FusionOffset.c
+ * @author Seb Madgwick
+ * @brief Gyroscope offset correction algorithm for run-time calibration of the
+ * gyroscope offset.
+ */
+
+//------------------------------------------------------------------------------
+// Includes
+
+#include "FusionOffset.h"
+#include <math.h> // fabsf
+
+//------------------------------------------------------------------------------
+// Definitions
+
+/**
+ * @brief Cutoff frequency in Hz.
+ */
+#define CUTOFF_FREQUENCY (0.02f)
+
+/**
+ * @brief Timeout in seconds.
+ */
+#define TIMEOUT (5)
+
+/**
+ * @brief Threshold in degrees per second.
+ */
+#define THRESHOLD (3.0f)
+
+//------------------------------------------------------------------------------
+// Functions
+
+/**
+ * @brief Initialises the gyroscope offset algorithm.
+ * @param offset Gyroscope offset algorithm structure.
+ * @param sampleRate Sample rate in Hz.
+ */
+void FusionOffsetInitialise(FusionOffset *const offset, const unsigned int sampleRate)
+{
+    offset->filterCoefficient = 2.0f * (float)M_PI * CUTOFF_FREQUENCY * (1.0f / (float)sampleRate);
+    offset->timeout = TIMEOUT * sampleRate;
+    offset->timer = 0;
+    offset->gyroscopeOffset = FUSION_VECTOR_ZERO;
+}
+
+/**
+ * @brief Updates the gyroscope offset algorithm and returns the corrected
+ * gyroscope measurement.
+ * @param offset Gyroscope offset algorithm structure.
+ * @param gyroscope Gyroscope measurement in degrees per second.
+ * @return Corrected gyroscope measurement in degrees per second.
+ */
+FusionVector FusionOffsetUpdate(FusionOffset *const offset, FusionVector gyroscope)
+{
+
+    // Subtract offset from gyroscope measurement
+    gyroscope = FusionVectorSubtract(gyroscope, offset->gyroscopeOffset);
+
+    // Reset timer if gyroscope not stationary
+    if ((fabsf(gyroscope.axis.x) > THRESHOLD) || (fabsf(gyroscope.axis.y) > THRESHOLD) || (fabsf(gyroscope.axis.z) > THRESHOLD)) {
+        offset->timer = 0;
+        return gyroscope;
+    }
+
+    // Increment timer while gyroscope stationary
+    if (offset->timer < offset->timeout) {
+        offset->timer++;
+        return gyroscope;
+    }
+
+    // Adjust offset if timer has elapsed
+    offset->gyroscopeOffset =
+        FusionVectorAdd(offset->gyroscopeOffset, FusionVectorMultiplyScalar(gyroscope, offset->filterCoefficient));
+    return gyroscope;
+}
+
+//------------------------------------------------------------------------------
+// End of file

src/Fusion/FusionOffset.h

@@ -0,0 +1,40 @@
+/**
+ * @file FusionOffset.h
+ * @author Seb Madgwick
+ * @brief Gyroscope offset correction algorithm for run-time calibration of the
+ * gyroscope offset.
+ */
+
+#ifndef FUSION_OFFSET_H
+#define FUSION_OFFSET_H
+
+//------------------------------------------------------------------------------
+// Includes
+
+#include "FusionMath.h"
+
+//------------------------------------------------------------------------------
+// Definitions
+
+/**
+ * @brief Gyroscope offset algorithm structure.  Structure members are used
+ * internally and must not be accessed by the application.
+ */
+typedef struct {
+    float filterCoefficient;
+    unsigned int timeout;
+    unsigned int timer;
+    FusionVector gyroscopeOffset;
+} FusionOffset;
+
+//------------------------------------------------------------------------------
+// Function declarations
+
+void FusionOffsetInitialise(FusionOffset *const offset, const unsigned int sampleRate);
+
+FusionVector FusionOffsetUpdate(FusionOffset *const offset, FusionVector gyroscope);
+
+#endif
+
+//------------------------------------------------------------------------------
+// End of file

src/GPSStatus.h

@@ -124,7 +124,7 @@ class GPSStatus : public Status
         if (isDirty) {
             if (hasLock) {
                 // In debug logs, identify position by @timestamp:stage (stage 3 = notify)
-                LOG_DEBUG("New GPS pos@%x:3 lat=%f, lon=%f, alt=%d, pdop=%.2f, track=%.2f, speed=%.2f, sats=%d\n", p.timestamp,
+                LOG_DEBUG("New GPS pos@%x:3 lat=%f lon=%f alt=%d pdop=%.2f track=%.2f speed=%.2f sats=%d\n", p.timestamp,
                           p.latitude_i * 1e-7, p.longitude_i * 1e-7, p.altitude, p.PDOP * 1e-2, p.ground_track * 1e-5,
                           p.ground_speed * 1e-2, p.sats_in_view);
             } else {

src/Power.cpp

@@ -27,7 +27,7 @@
 #if defined(DEBUG_HEAP_MQTT) && !MESHTASTIC_EXCLUDE_MQTT
 #include "mqtt/MQTT.h"
 #include "target_specific.h"
-#if !MESTASTIC_EXCLUDE_WIFI
+#if HAS_WIFI
 #include <WiFi.h>
 #endif
 #endif
@@ -75,6 +75,10 @@ INA219Sensor ina219Sensor;
 INA3221Sensor ina3221Sensor;
 #endif
 
+#if HAS_RAKPROT && !defined(ARCH_PORTDUINO)
+RAK9154Sensor rak9154Sensor;
+#endif
+
 #ifdef HAS_PMU
 #include "XPowersAXP192.tpp"
 #include "XPowersAXP2101.tpp"
@@ -145,6 +149,12 @@ class AnalogBatteryLevel : public HasBatteryLevel
      */
     virtual int getBatteryPercent() override
     {
+#if defined(HAS_RAKPROT) && !defined(ARCH_PORTDUINO) && !defined(HAS_PMU)
+        if (hasRAK()) {
+            return rak9154Sensor.getBusBatteryPercent();
+        }
+#endif
+
         float v = getBattVoltage();
 
         if (v < noBatVolt)
@@ -184,6 +194,12 @@ class AnalogBatteryLevel : public HasBatteryLevel
     virtual uint16_t getBattVoltage() override
     {
 
+#if defined(HAS_RAKPROT) && !defined(ARCH_PORTDUINO) && !defined(HAS_PMU)
+        if (hasRAK()) {
+            return getRAKVoltage();
+        }
+#endif
+
 #if HAS_TELEMETRY && !defined(ARCH_PORTDUINO) && !defined(HAS_PMU) && !MESHTASTIC_EXCLUDE_ENVIRONMENTAL_SENSOR
         if (hasINA()) {
             LOG_DEBUG("Using INA on I2C addr 0x%x for device battery voltage\n", config.power.device_battery_ina_address);
@@ -216,12 +232,20 @@ class AnalogBatteryLevel : public HasBatteryLevel
             raw = espAdcRead();
             scaled = esp_adc_cal_raw_to_voltage(raw, adc_characs);
             scaled *= operativeAdcMultiplier;
-#else // block for all other platforms
+#else           // block for all other platforms
+#ifdef ADC_CTRL // enable adc voltage divider when we need to read
+            pinMode(ADC_CTRL, OUTPUT);
+            digitalWrite(ADC_CTRL, ADC_CTRL_ENABLED);
+            delay(10);
+#endif
             for (uint32_t i = 0; i < BATTERY_SENSE_SAMPLES; i++) {
                 raw += analogRead(BATTERY_PIN);
             }
             raw = raw / BATTERY_SENSE_SAMPLES;
             scaled = operativeAdcMultiplier * ((1000 * AREF_VOLTAGE) / pow(2, BATTERY_SENSE_RESOLUTION_BITS)) * raw;
+#ifdef ADC_CTRL // disable adc voltage divider when we need to read
+            digitalWrite(ADC_CTRL, !ADC_CTRL_ENABLED);
+#endif
 #endif
 
             if (!initial_read_done) {
@@ -335,13 +359,20 @@ class AnalogBatteryLevel : public HasBatteryLevel
     virtual bool isVbusIn() override
     {
 #ifdef EXT_PWR_DETECT
+#ifdef HELTEC_CAPSULE_SENSOR_V3
+        // if external powered that pin will be pulled down
+        if (digitalRead(EXT_PWR_DETECT) == LOW) {
+            return true;
+        }
+        // if it's not LOW - check the battery
+#else
         // if external powered that pin will be pulled up
         if (digitalRead(EXT_PWR_DETECT) == HIGH) {
             return true;
         }
         // if it's not HIGH - check the battery
 #endif
-
+#endif
         return getBattVoltage() > chargingVolt;
     }
 
@@ -349,6 +380,11 @@ class AnalogBatteryLevel : public HasBatteryLevel
     /// we can't be smart enough to say 'full'?
     virtual bool isCharging() override
     {
+#if defined(HAS_RAKPROT) && !defined(ARCH_PORTDUINO) && !defined(HAS_PMU)
+        if (hasRAK()) {
+            return (rak9154Sensor.isCharging()) ? OptTrue : OptFalse;
+        }
+#endif
 #ifdef EXT_CHRG_DETECT
         return digitalRead(EXT_CHRG_DETECT) == ext_chrg_detect_value;
 #else
@@ -372,6 +408,18 @@ class AnalogBatteryLevel : public HasBatteryLevel
     float last_read_value = (OCV[NUM_OCV_POINTS - 1] * NUM_CELLS);
     uint32_t last_read_time_ms = 0;
 
+#if defined(HAS_RAKPROT)
+
+    uint16_t getRAKVoltage() { return rak9154Sensor.getBusVoltageMv(); }
+
+    bool hasRAK()
+    {
+        if (!rak9154Sensor.isInitialized())
+            return rak9154Sensor.runOnce() > 0;
+        return rak9154Sensor.isRunning();
+    }
+#endif
+
 #if HAS_TELEMETRY && !MESHTASTIC_EXCLUDE_ENVIRONMENTAL_SENSOR && !defined(ARCH_PORTDUINO)
     uint16_t getINAVoltage()
     {
@@ -401,6 +449,11 @@ class AnalogBatteryLevel : public HasBatteryLevel
             if (!ina260Sensor.isInitialized())
                 return ina260Sensor.runOnce() > 0;
             return ina260Sensor.isRunning();
+        } else if (nodeTelemetrySensorsMap[meshtastic_TelemetrySensorType_INA3221].first ==
+                   config.power.device_battery_ina_address) {
+            if (!ina3221Sensor.isInitialized())
+                return ina3221Sensor.runOnce() > 0;
+            return ina3221Sensor.isRunning();
         }
         return false;
     }
@@ -421,8 +474,12 @@ Power::Power() : OSThread("Power")
 bool Power::analogInit()
 {
 #ifdef EXT_PWR_DETECT
+#ifdef HELTEC_CAPSULE_SENSOR_V3
+    pinMode(EXT_PWR_DETECT, INPUT_PULLUP);
+#else
     pinMode(EXT_PWR_DETECT, INPUT);
 #endif
+#endif
 #ifdef EXT_CHRG_DETECT
     pinMode(EXT_CHRG_DETECT, ext_chrg_detect_mode);
 #endif

src/PowerFSM.cpp

@@ -11,6 +11,7 @@
 #include "Default.h"
 #include "MeshService.h"
 #include "NodeDB.h"
+#include "PowerMon.h"
 #include "configuration.h"
 #include "graphics/Screen.h"
 #include "main.h"
@@ -49,6 +50,7 @@ static bool isPowered()
 static void sdsEnter()
 {
     LOG_DEBUG("Enter state: SDS\n");
+    powerMon->setState(meshtastic_PowerMon_State_CPU_DeepSleep);
     // FIXME - make sure GPS and LORA radio are off first - because we want close to zero current draw
     doDeepSleep(Default::getConfiguredOrDefaultMs(config.power.sds_secs), false);
 }
@@ -68,6 +70,7 @@ static uint32_t secsSlept;
 static void lsEnter()
 {
     LOG_INFO("lsEnter begin, ls_secs=%u\n", config.power.ls_secs);
+    powerMon->clearState(meshtastic_PowerMon_State_Screen_On);
     screen->setOn(false);
     secsSlept = 0; // How long have we been sleeping this time
 
@@ -87,8 +90,10 @@ static void lsIdle()
             // Briefly come out of sleep long enough to blink the led once every few seconds
             uint32_t sleepTime = SLEEP_TIME;
 
+            powerMon->setState(meshtastic_PowerMon_State_CPU_LightSleep);
             setLed(false); // Never leave led on while in light sleep
             esp_sleep_source_t wakeCause2 = doLightSleep(sleepTime * 1000LL);
+            powerMon->clearState(meshtastic_PowerMon_State_CPU_LightSleep);
 
             switch (wakeCause2) {
             case ESP_SLEEP_WAKEUP_TIMER:
@@ -144,6 +149,7 @@ static void lsExit()
 static void nbEnter()
 {
     LOG_DEBUG("Enter state: NB\n");
+    powerMon->clearState(meshtastic_PowerMon_State_BT_On);
     screen->setOn(false);
 #ifdef ARCH_ESP32
     // Only ESP32 should turn off bluetooth
@@ -155,6 +161,8 @@ static void nbEnter()
 
 static void darkEnter()
 {
+    powerMon->clearState(meshtastic_PowerMon_State_BT_On);
+    powerMon->clearState(meshtastic_PowerMon_State_Screen_On);
     setBluetoothEnable(true);
     screen->setOn(false);
 }
@@ -162,6 +170,8 @@ static void darkEnter()
 static void serialEnter()
 {
     LOG_DEBUG("Enter state: SERIAL\n");
+    powerMon->clearState(meshtastic_PowerMon_State_BT_On);
+    powerMon->setState(meshtastic_PowerMon_State_Screen_On);
     setBluetoothEnable(false);
     screen->setOn(true);
     screen->print("Serial connected\n");
@@ -170,6 +180,7 @@ static void serialEnter()
 static void serialExit()
 {
     // Turn bluetooth back on when we leave serial stream API
+    powerMon->setState(meshtastic_PowerMon_State_BT_On);
     setBluetoothEnable(true);
     screen->print("Serial disconnected\n");
 }
@@ -182,6 +193,8 @@ static void powerEnter()
         LOG_INFO("Loss of power in Powered\n");
         powerFSM.trigger(EVENT_POWER_DISCONNECTED);
     } else {
+        powerMon->setState(meshtastic_PowerMon_State_BT_On);
+        powerMon->setState(meshtastic_PowerMon_State_Screen_On);
         screen->setOn(true);
         setBluetoothEnable(true);
         // within enter() the function getState() returns the state we came from
@@ -205,6 +218,8 @@ static void powerIdle()
 
 static void powerExit()
 {
+    powerMon->setState(meshtastic_PowerMon_State_BT_On);
+    powerMon->setState(meshtastic_PowerMon_State_Screen_On);
     screen->setOn(true);
     setBluetoothEnable(true);
 
@@ -216,6 +231,8 @@ static void powerExit()
 static void onEnter()
 {
     LOG_DEBUG("Enter state: ON\n");
+    powerMon->setState(meshtastic_PowerMon_State_BT_On);
+    powerMon->setState(meshtastic_PowerMon_State_Screen_On);
     screen->setOn(true);
     setBluetoothEnable(true);
 }

src/PowerMon.cpp

@@ -0,0 +1,45 @@
+#include "PowerMon.h"
+#include "NodeDB.h"
+
+// Use the 'live' config flag to figure out if we should be showing this message
+static bool is_power_enabled(uint64_t m)
+{
+    return (m & config.power.powermon_enables) ? true : false;
+}
+
+void PowerMon::setState(_meshtastic_PowerMon_State state, const char *reason)
+{
+#ifdef USE_POWERMON
+    auto oldstates = states;
+    states |= state;
+    if (oldstates != states && is_power_enabled(state)) {
+        emitLog(reason);
+    }
+#endif
+}
+
+void PowerMon::clearState(_meshtastic_PowerMon_State state, const char *reason)
+{
+#ifdef USE_POWERMON
+    auto oldstates = states;
+    states &= ~state;
+    if (oldstates != states && is_power_enabled(state)) {
+        emitLog(reason);
+    }
+#endif
+}
+
+void PowerMon::emitLog(const char *reason)
+{
+#ifdef USE_POWERMON
+    // The nrf52 printf doesn't understand 64 bit ints, so if we ever reach that point this function will need to change.
+    LOG_INFO("S:PM:0x%08lx,%s\n", (uint32_t)states, reason);
+#endif
+}
+
+PowerMon *powerMon;
+
+void powerMonInit()
+{
+    powerMon = new PowerMon();
+}

src/PowerMon.h

@@ -0,0 +1,34 @@
+#pragma once
+#include "configuration.h"
+
+#include "meshtastic/powermon.pb.h"
+
+#ifndef MESHTASTIC_EXCLUDE_POWERMON
+#define USE_POWERMON // FIXME turn this only for certain builds
+#endif
+
+/**
+ * The singleton class for monitoring power consumption of device
+ * subsystems/modes.
+ *
+ * For more information see the PowerMon docs.
+ */
+class PowerMon
+{
+    uint64_t states = 0UL;
+
+  public:
+    PowerMon() {}
+
+    // Mark entry/exit of a power consuming state
+    void setState(_meshtastic_PowerMon_State state, const char *reason = "");
+    void clearState(_meshtastic_PowerMon_State state, const char *reason = "");
+
+  private:
+    // Emit the coded log message
+    void emitLog(const char *reason);
+};
+
+extern PowerMon *powerMon;
+
+void powerMonInit();

src/RedirectablePrint.cpp

@@ -3,6 +3,8 @@
 #include "RTC.h"
 #include "concurrency/OSThread.h"
 #include "configuration.h"
+#include "main.h"
+#include "mesh/generated/meshtastic/mesh.pb.h"
 #include <assert.h>
 #include <cstring>
 #include <memory>
@@ -14,12 +16,7 @@
 #include "platform/portduino/PortduinoGlue.h"
 #endif
 
-/**
- * A printer that doesn't go anywhere
- */
-NoopPrint noopPrint;
-
-#if HAS_WIFI || HAS_ETHERNET
+#if HAS_NETWORKING
 extern Syslog syslog;
 #endif
 void RedirectablePrint::rpInit()
@@ -38,7 +35,7 @@ void RedirectablePrint::setDestination(Print *_dest)
 size_t RedirectablePrint::write(uint8_t c)
 {
     // Always send the characters to our segger JTAG debugger
-#ifdef SEGGER_STDOUT_CH
+#ifdef USE_SEGGER
     SEGGER_RTT_PutChar(SEGGER_STDOUT_CH, c);
 #endif
 
@@ -49,7 +46,7 @@ size_t RedirectablePrint::write(uint8_t c)
               // serial port said (which could be zero)
 }
 
-size_t RedirectablePrint::vprintf(const char *format, va_list arg)
+size_t RedirectablePrint::vprintf(const char *logLevel, const char *format, va_list arg)
 {
     va_list copy;
     static char printBuf[160];
@@ -65,25 +62,200 @@ size_t RedirectablePrint::vprintf(const char *format, va_list arg)
         len = sizeof(printBuf) - 1;
         printBuf[sizeof(printBuf) - 2] = '\n';
     }
-
+    for (size_t f = 0; f < len; f++) {
+        if (!std::isprint(static_cast<unsigned char>(printBuf[f])) && printBuf[f] != '\n')
+            printBuf[f] = '#';
+    }
+    if (logLevel != nullptr) {
+        if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_DEBUG) == 0)
+            Print::write("\u001b[34m", 6);
+        if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_INFO) == 0)
+            Print::write("\u001b[32m", 6);
+        if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_WARN) == 0)
+            Print::write("\u001b[33m", 6);
+        if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_ERROR) == 0)
+            Print::write("\u001b[31m", 6);
+    }
     len = Print::write(printBuf, len);
+    Print::write("\u001b[0m", 5);
     return len;
 }
 
-size_t RedirectablePrint::log(const char *logLevel, const char *format, ...)
+void RedirectablePrint::log_to_serial(const char *logLevel, const char *format, va_list arg)
+{
+    size_t r = 0;
+
+    // Cope with 0 len format strings, but look for new line terminator
+    bool hasNewline = *format && format[strlen(format) - 1] == '\n';
+
+    // If we are the first message on a report, include the header
+    if (!isContinuationMessage) {
+        if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_DEBUG) == 0)
+            Print::write("\u001b[34m", 6);
+        if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_INFO) == 0)
+            Print::write("\u001b[32m", 6);
+        if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_WARN) == 0)
+            Print::write("\u001b[33m", 6);
+        if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_ERROR) == 0)
+            Print::write("\u001b[31m", 6);
+        uint32_t rtc_sec = getValidTime(RTCQuality::RTCQualityDevice, true); // display local time on logfile
+        if (rtc_sec > 0) {
+            long hms = rtc_sec % SEC_PER_DAY;
+            // hms += tz.tz_dsttime * SEC_PER_HOUR;
+            // hms -= tz.tz_minuteswest * SEC_PER_MIN;
+            // mod `hms` to ensure in positive range of [0...SEC_PER_DAY)
+            hms = (hms + SEC_PER_DAY) % SEC_PER_DAY;
+
+            // Tear apart hms into h:m:s
+            int hour = hms / SEC_PER_HOUR;
+            int min = (hms % SEC_PER_HOUR) / SEC_PER_MIN;
+            int sec = (hms % SEC_PER_HOUR) % SEC_PER_MIN; // or hms % SEC_PER_MIN
+#ifdef ARCH_PORTDUINO
+            ::printf("%s \u001b[0m| %02d:%02d:%02d %u ", logLevel, hour, min, sec, millis() / 1000);
+#else
+            printf("%s \u001b[0m| %02d:%02d:%02d %u ", logLevel, hour, min, sec, millis() / 1000);
+#endif
+        } else
+#ifdef ARCH_PORTDUINO
+            ::printf("%s \u001b[0m| ??:??:?? %u ", logLevel, millis() / 1000);
+#else
+            printf("%s \u001b[0m| ??:??:?? %u ", logLevel, millis() / 1000);
+#endif
+
+        auto thread = concurrency::OSThread::currentThread;
+        if (thread) {
+            print("[");
+            // printf("%p ", thread);
+            // assert(thread->ThreadName.length());
+            print(thread->ThreadName);
+            print("] ");
+        }
+    }
+    r += vprintf(logLevel, format, arg);
+
+    isContinuationMessage = !hasNewline;
+}
+
+void RedirectablePrint::log_to_syslog(const char *logLevel, const char *format, va_list arg)
+{
+#if HAS_NETWORKING && !defined(ARCH_PORTDUINO)
+    // if syslog is in use, collect the log messages and send them to syslog
+    if (syslog.isEnabled()) {
+        int ll = 0;
+        switch (logLevel[0]) {
+        case 'D':
+            ll = SYSLOG_DEBUG;
+            break;
+        case 'I':
+            ll = SYSLOG_INFO;
+            break;
+        case 'W':
+            ll = SYSLOG_WARN;
+            break;
+        case 'E':
+            ll = SYSLOG_ERR;
+            break;
+        case 'C':
+            ll = SYSLOG_CRIT;
+            break;
+        default:
+            ll = 0;
+        }
+        auto thread = concurrency::OSThread::currentThread;
+        if (thread) {
+            syslog.vlogf(ll, thread->ThreadName.c_str(), format, arg);
+        } else {
+            syslog.vlogf(ll, format, arg);
+        }
+    }
+#endif
+}
+
+void RedirectablePrint::log_to_ble(const char *logLevel, const char *format, va_list arg)
+{
+#if !MESHTASTIC_EXCLUDE_BLUETOOTH
+    if (config.bluetooth.device_logging_enabled && !pauseBluetoothLogging) {
+        bool isBleConnected = false;
+#ifdef ARCH_ESP32
+        isBleConnected = nimbleBluetooth && nimbleBluetooth->isActive() && nimbleBluetooth->isConnected();
+#elif defined(ARCH_NRF52)
+        isBleConnected = nrf52Bluetooth != nullptr && nrf52Bluetooth->isConnected();
+#endif
+        if (isBleConnected) {
+            char *message;
+            size_t initialLen;
+            size_t len;
+            initialLen = strlen(format);
+            message = new char[initialLen + 1];
+            len = vsnprintf(message, initialLen + 1, format, arg);
+            if (len > initialLen) {
+                delete[] message;
+                message = new char[len + 1];
+                vsnprintf(message, len + 1, format, arg);
+            }
+            auto thread = concurrency::OSThread::currentThread;
+            meshtastic_LogRecord logRecord = meshtastic_LogRecord_init_zero;
+            logRecord.level = getLogLevel(logLevel);
+            strcpy(logRecord.message, message);
+            if (thread)
+                strcpy(logRecord.source, thread->ThreadName.c_str());
+            logRecord.time = getValidTime(RTCQuality::RTCQualityDevice, true);
+
+            uint8_t *buffer = new uint8_t[meshtastic_LogRecord_size];
+            size_t size = pb_encode_to_bytes(buffer, meshtastic_LogRecord_size, meshtastic_LogRecord_fields, &logRecord);
+#ifdef ARCH_ESP32
+            nimbleBluetooth->sendLog(buffer, size);
+#elif defined(ARCH_NRF52)
+            nrf52Bluetooth->sendLog(buffer, size);
+#endif
+            delete[] message;
+            delete[] buffer;
+        }
+    }
+#else
+    (void)logLevel;
+    (void)format;
+    (void)arg;
+#endif
+}
+
+meshtastic_LogRecord_Level RedirectablePrint::getLogLevel(const char *logLevel)
+{
+    meshtastic_LogRecord_Level ll = meshtastic_LogRecord_Level_UNSET; // default to unset
+    switch (logLevel[0]) {
+    case 'D':
+        ll = meshtastic_LogRecord_Level_DEBUG;
+        break;
+    case 'I':
+        ll = meshtastic_LogRecord_Level_INFO;
+        break;
+    case 'W':
+        ll = meshtastic_LogRecord_Level_WARNING;
+        break;
+    case 'E':
+        ll = meshtastic_LogRecord_Level_ERROR;
+        break;
+    case 'C':
+        ll = meshtastic_LogRecord_Level_CRITICAL;
+        break;
+    }
+    return ll;
+}
+
+void RedirectablePrint::log(const char *logLevel, const char *format, ...)
 {
 #ifdef ARCH_PORTDUINO
     if (settingsMap[logoutputlevel] < level_debug && strcmp(logLevel, MESHTASTIC_LOG_LEVEL_DEBUG) == 0)
-        return 0;
+        return;
     else if (settingsMap[logoutputlevel] < level_info && strcmp(logLevel, MESHTASTIC_LOG_LEVEL_INFO) == 0)
-        return 0;
+        return;
     else if (settingsMap[logoutputlevel] < level_warn && strcmp(logLevel, MESHTASTIC_LOG_LEVEL_WARN) == 0)
-        return 0;
+        return;
 #endif
     if (moduleConfig.serial.override_console_serial_port && strcmp(logLevel, MESHTASTIC_LOG_LEVEL_DEBUG) == 0) {
-        return 0;
+        return;
     }
-    size_t r = 0;
+
 #ifdef HAS_FREE_RTOS
     if (inDebugPrint != nullptr && xSemaphoreTake(inDebugPrint, portMAX_DELAY) == pdTRUE) {
 #else
@@ -94,81 +266,11 @@ size_t RedirectablePrint::log(const char *logLevel, const char *format, ...)
         va_list arg;
         va_start(arg, format);
 
-        // Cope with 0 len format strings, but look for new line terminator
-        bool hasNewline = *format && format[strlen(format) - 1] == '\n';
-
-        // If we are the first message on a report, include the header
-        if (!isContinuationMessage) {
-            uint32_t rtc_sec = getValidTime(RTCQuality::RTCQualityDevice, true); // display local time on logfile
-            if (rtc_sec > 0) {
-                long hms = rtc_sec % SEC_PER_DAY;
-                // hms += tz.tz_dsttime * SEC_PER_HOUR;
-                // hms -= tz.tz_minuteswest * SEC_PER_MIN;
-                // mod `hms` to ensure in positive range of [0...SEC_PER_DAY)
-                hms = (hms + SEC_PER_DAY) % SEC_PER_DAY;
-
-                // Tear apart hms into h:m:s
-                int hour = hms / SEC_PER_HOUR;
-                int min = (hms % SEC_PER_HOUR) / SEC_PER_MIN;
-                int sec = (hms % SEC_PER_HOUR) % SEC_PER_MIN; // or hms % SEC_PER_MIN
-#ifdef ARCH_PORTDUINO
-                r += ::printf("%s | %02d:%02d:%02d %u ", logLevel, hour, min, sec, millis() / 1000);
-#else
-                r += printf("%s | %02d:%02d:%02d %u ", logLevel, hour, min, sec, millis() / 1000);
-#endif
-            } else
-#ifdef ARCH_PORTDUINO
-                r += ::printf("%s | ??:??:?? %u ", logLevel, millis() / 1000);
-#else
-                r += printf("%s | ??:??:?? %u ", logLevel, millis() / 1000);
-#endif
-
-            auto thread = concurrency::OSThread::currentThread;
-            if (thread) {
-                print("[");
-                // printf("%p ", thread);
-                // assert(thread->ThreadName.length());
-                print(thread->ThreadName);
-                print("] ");
-            }
-        }
-        r += vprintf(format, arg);
-
-#if (HAS_WIFI || HAS_ETHERNET) && !defined(ARCH_PORTDUINO)
-        // if syslog is in use, collect the log messages and send them to syslog
-        if (syslog.isEnabled()) {
-            int ll = 0;
-            switch (logLevel[0]) {
-            case 'D':
-                ll = SYSLOG_DEBUG;
-                break;
-            case 'I':
-                ll = SYSLOG_INFO;
-                break;
-            case 'W':
-                ll = SYSLOG_WARN;
-                break;
-            case 'E':
-                ll = SYSLOG_ERR;
-                break;
-            case 'C':
-                ll = SYSLOG_CRIT;
-                break;
-            default:
-                ll = 0;
-            }
-            auto thread = concurrency::OSThread::currentThread;
-            if (thread) {
-                syslog.vlogf(ll, thread->ThreadName.c_str(), format, arg);
-            } else {
-                syslog.vlogf(ll, format, arg);
-            }
-        }
-#endif
+        log_to_serial(logLevel, format, arg);
+        log_to_syslog(logLevel, format, arg);
+        log_to_ble(logLevel, format, arg);
 
         va_end(arg);
-
-        isContinuationMessage = !hasNewline;
 #ifdef HAS_FREE_RTOS
         xSemaphoreGive(inDebugPrint);
 #else
@@ -176,7 +278,7 @@ size_t RedirectablePrint::log(const char *logLevel, const char *format, ...)
 #endif
     }
 
-    return r;
+    return;
 }
 
 void RedirectablePrint::hexDump(const char *logLevel, unsigned char *buf, uint16_t len)

src/RedirectablePrint.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include "../freertosinc.h"
+#include "mesh/generated/meshtastic/mesh.pb.h"
 #include <Print.h>
 #include <stdarg.h>
 #include <string>
@@ -41,23 +42,21 @@ class RedirectablePrint : public Print
      * log message.  Otherwise we assume more prints will come before the log message ends.  This
      * allows you to call logDebug a few times to build up a single log message line if you wish.
      */
-    size_t log(const char *logLevel, const char *format, ...) __attribute__((format(printf, 3, 4)));
+    void log(const char *logLevel, const char *format, ...) __attribute__((format(printf, 3, 4)));
 
     /** like printf but va_list based */
-    size_t vprintf(const char *format, va_list arg);
+    size_t vprintf(const char *logLevel, const char *format, va_list arg);
 
     void hexDump(const char *logLevel, unsigned char *buf, uint16_t len);
 
     std::string mt_sprintf(const std::string fmt_str, ...);
-};
 
-class NoopPrint : public Print
-{
-  public:
-    virtual size_t write(uint8_t c) { return 1; }
-};
+  protected:
+    /// Subclasses can override if they need to change how we format over the serial port
+    virtual void log_to_serial(const char *logLevel, const char *format, va_list arg);
 
-/**
- * A printer that doesn't go anywhere
- */
-extern NoopPrint noopPrint;
+  private:
+    void log_to_syslog(const char *logLevel, const char *format, va_list arg);
+    void log_to_ble(const char *logLevel, const char *format, va_list arg);
+    meshtastic_LogRecord_Level getLogLevel(const char *logLevel);
+};

src/SerialConsole.cpp

@@ -7,8 +7,12 @@
 #ifdef RP2040_SLOW_CLOCK
 #define Port Serial2
 #else
+#ifdef USER_DEBUG_PORT // change by WayenWeng
+#define Port USER_DEBUG_PORT
+#else
 #define Port Serial
 #endif
+#endif
 // Defaulting to the formerly removed phone_timeout_secs value of 15 minutes
 #define SERIAL_CONNECTION_TIMEOUT (15 * 60) * 1000UL
 
@@ -24,7 +28,7 @@ void consolePrintf(const char *format, ...)
 {
     va_list arg;
     va_start(arg, format);
-    console->vprintf(format, arg);
+    console->vprintf(nullptr, format, arg);
     va_end(arg);
     console->flush();
 }
@@ -34,7 +38,6 @@ SerialConsole::SerialConsole() : StreamAPI(&Port), RedirectablePrint(&Port), con
     assert(!console);
     console = this;
     canWrite = false; // We don't send packets to our port until it has talked to us first
-                      // setDestination(&noopPrint); for testing, try turning off 'all' debug output and see what leaks
 
 #ifdef RP2040_SLOW_CLOCK
     Port.setTX(SERIAL2_TX);
@@ -81,13 +84,40 @@ bool SerialConsole::handleToRadio(const uint8_t *buf, size_t len)
 {
     // only talk to the API once the configuration has been loaded and we're sure the serial port is not disabled.
     if (config.has_lora && config.device.serial_enabled) {
-        // Turn off debug serial printing once the API is activated, because other threads could print and corrupt packets
-        if (!config.device.debug_log_enabled)
-            setDestination(&noopPrint);
+        // Switch to protobufs for log messages
+        usingProtobufs = true;
         canWrite = true;
 
         return StreamAPI::handleToRadio(buf, len);
     } else {
         return false;
     }
+}
+
+void SerialConsole::log_to_serial(const char *logLevel, const char *format, va_list arg)
+{
+    if (usingProtobufs) {
+        meshtastic_LogRecord_Level ll = meshtastic_LogRecord_Level_UNSET; // default to unset
+        switch (logLevel[0]) {
+        case 'D':
+            ll = meshtastic_LogRecord_Level_DEBUG;
+            break;
+        case 'I':
+            ll = meshtastic_LogRecord_Level_INFO;
+            break;
+        case 'W':
+            ll = meshtastic_LogRecord_Level_WARNING;
+            break;
+        case 'E':
+            ll = meshtastic_LogRecord_Level_ERROR;
+            break;
+        case 'C':
+            ll = meshtastic_LogRecord_Level_CRITICAL;
+            break;
+        }
+
+        auto thread = concurrency::OSThread::currentThread;
+        emitLogRecord(ll, thread ? thread->ThreadName.c_str() : "", format, arg);
+    } else
+        RedirectablePrint::log_to_serial(logLevel, format, arg);
 }

src/SerialConsole.h

@@ -8,6 +8,11 @@
  */
 class SerialConsole : public StreamAPI, public RedirectablePrint, private concurrency::OSThread
 {
+    /**
+     * If true we are talking to a smart host and all messages (including log messages) must be framed as protobufs.
+     */
+    bool usingProtobufs = false;
+
   public:
     SerialConsole();
 
@@ -31,10 +36,13 @@ class SerialConsole : public StreamAPI, public RedirectablePrint, private concur
   protected:
     /// Check the current underlying physical link to see if the client is currently connected
     virtual bool checkIsConnected() override;
+
+    /// Possibly switch to protobufs if we see a valid protobuf message
+    virtual void log_to_serial(const char *logLevel, const char *format, va_list arg);
 };
 
 // A simple wrapper to allow non class aware code write to the console
 void consolePrintf(const char *format, ...);
 void consoleInit();
 
-extern SerialConsole *console;
+extern SerialConsole *console;

src/commands.h

@@ -8,13 +8,11 @@ enum class Cmd {
     SET_ON,
     SET_OFF,
     ON_PRESS,
-    START_BLUETOOTH_PIN_SCREEN,
+    START_ALERT_FRAME,
+    STOP_ALERT_FRAME,
     START_FIRMWARE_UPDATE_SCREEN,
-    STOP_BLUETOOTH_PIN_SCREEN,
     STOP_BOOT_SCREEN,
     PRINT,
-    START_SHUTDOWN_SCREEN,
-    START_REBOOT_SCREEN,
     SHOW_PREV_FRAME,
     SHOW_NEXT_FRAME
 };

src/configuration.h

@@ -75,11 +75,17 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #endif
 
 // -----------------------------------------------------------------------------
-// Regulatory overrides for producing regional builds
+// Regulatory overrides
 // -----------------------------------------------------------------------------
 
-// Define if region should override user saved region
-// #define LORA_REGIONCODE meshtastic_Config_LoRaConfig_RegionCode_SG_923
+// Override user saved region, for producing region-locked builds
+// #define REGULATORY_LORA_REGIONCODE meshtastic_Config_LoRaConfig_RegionCode_SG_923
+
+// Total system gain in dBm to subtract from Tx power to remain within regulatory ERP limit for non-licensed operators
+// This value should be set in variant.h and is PA gain + antenna gain (if system ships with an antenna)
+#ifndef REGULATORY_GAIN_LORA
+#define REGULATORY_GAIN_LORA 0
+#endif
 
 // -----------------------------------------------------------------------------
 // Feature toggles
@@ -131,6 +137,12 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #define AHT10_ADDR 0x38
 #define RCWL9620_ADDR 0x57
 #define VEML7700_ADDR 0x10
+#define TSL25911_ADDR 0x29
+#define OPT3001_ADDR 0x45
+#define OPT3001_ADDR_ALT 0x44
+#define MLX90632_ADDR 0x3A
+#define DFROBOT_LARK_ADDR 0x42
+#define NAU7802_ADDR 0x2A
 
 // -----------------------------------------------------------------------------
 // ACCELEROMETER
@@ -139,6 +151,7 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #define LIS3DH_ADR 0x18
 #define BMA423_ADDR 0x19
 #define LSM6DS3_ADDR 0x6A
+#define BMX160_ADDR 0x69
 
 // -----------------------------------------------------------------------------
 // LED
@@ -158,10 +171,6 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 // -----------------------------------------------------------------------------
 // GPS
 // -----------------------------------------------------------------------------
-#ifndef GPS_BAUDRATE
-#define GPS_BAUDRATE 9600
-#endif
-
 #ifndef GPS_THREAD_INTERVAL
 #define GPS_THREAD_INTERVAL 200
 #endif
@@ -172,6 +181,10 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 /* Step #1: offer chance for variant-specific defines */
 #include "variant.h"
 
+#ifndef GPS_BAUDRATE
+#define GPS_BAUDRATE 9600
+#endif
+
 /* Step #2: follow with defines common to the architecture;
    also enable HAS_ option not specifically disabled by variant.h */
 #include "architecture.h"
@@ -229,9 +242,6 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #define HAS_BLUETOOTH 0
 #endif
 
-#include "DebugConfiguration.h"
-#include "RF95Configuration.h"
-
 #ifndef HW_VENDOR
 #error HW_VENDOR must be defined
 #endif
@@ -248,6 +258,7 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #define MESHTASTIC_EXCLUDE_GPS 1
 #define MESHTASTIC_EXCLUDE_SCREEN 1
 #define MESHTASTIC_EXCLUDE_MQTT 1
+#define MESHTASTIC_EXCLUDE_POWERMON 1
 #endif
 
 // Turn off all optional modules
@@ -268,6 +279,7 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #define MESHTASTIC_EXCLUDE_WAYPOINT 1
 #define MESHTASTIC_EXCLUDE_INPUTBROKER 1
 #define MESHTASTIC_EXCLUDE_SERIAL 1
+#define MESHTASTIC_EXCLUDE_POWERSTRESS 1
 #endif
 
 // // Turn off wifi even if HW supports wifi (webserver relies on wifi and is also disabled)
@@ -277,6 +289,9 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #define HAS_WIFI 0
 #endif
 
+// Allow code that needs internet to just check HAS_NETWORKING rather than HAS_WIFI || HAS_ETHERNET
+#define HAS_NETWORKING (HAS_WIFI || HAS_ETHERNET)
+
 // // Turn off Bluetooth
 #ifdef MESHTASTIC_EXCLUDE_BLUETOOTH
 #undef HAS_BLUETOOTH
@@ -295,4 +310,7 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #ifdef MESHTASTIC_EXCLUDE_SCREEN
 #undef HAS_SCREEN
 #define HAS_SCREEN 0
-#endif
+#endif
+
+#include "DebugConfiguration.h"
+#include "RF95Configuration.h"

src/detect/LoRaRadioType.h

@@ -1,5 +1,16 @@
 #pragma once
 
-enum LoRaRadioType { NO_RADIO, STM32WLx_RADIO, SIM_RADIO, RF95_RADIO, SX1262_RADIO, SX1268_RADIO, LLCC68_RADIO, SX1280_RADIO };
+enum LoRaRadioType {
+    NO_RADIO,
+    STM32WLx_RADIO,
+    SIM_RADIO,
+    RF95_RADIO,
+    SX1262_RADIO,
+    SX1268_RADIO,
+    LLCC68_RADIO,
+    SX1280_RADIO,
+    LR1110_RADIO,
+    LR1120_RADIO
+};
 
 extern LoRaRadioType radioType;

src/detect/ScanI2C.cpp

@@ -6,6 +6,7 @@ const ScanI2C::FoundDevice ScanI2C::DEVICE_NONE = ScanI2C::FoundDevice(ScanI2C::
 ScanI2C::ScanI2C() = default;
 
 void ScanI2C::scanPort(ScanI2C::I2CPort port) {}
+void ScanI2C::scanPort(ScanI2C::I2CPort port, uint8_t *address, uint8_t asize) {}
 
 void ScanI2C::setSuppressScreen()
 {
@@ -36,8 +37,8 @@ ScanI2C::FoundDevice ScanI2C::firstKeyboard() const
 
 ScanI2C::FoundDevice ScanI2C::firstAccelerometer() const
 {
-    ScanI2C::DeviceType types[] = {MPU6050, LIS3DH, BMA423, LSM6DS3};
-    return firstOfOrNONE(4, types);
+    ScanI2C::DeviceType types[] = {MPU6050, LIS3DH, BMA423, LSM6DS3, BMX160};
+    return firstOfOrNONE(5, types);
 }
 
 ScanI2C::FoundDevice ScanI2C::find(ScanI2C::DeviceType) const

src/detect/ScanI2C.h

@@ -45,7 +45,13 @@ class ScanI2C
         VEML7700,
         RCWL9620,
         NCP5623,
-        AHT10
+        TSL2591,
+        OPT3001,
+        MLX90632,
+        AHT10,
+        BMX160,
+        DFROBOT_LARK,
+        NAU7802
     } DeviceType;
 
     // typedef uint8_t DeviceAddress;
@@ -82,6 +88,7 @@ class ScanI2C
     ScanI2C();
 
     virtual void scanPort(ScanI2C::I2CPort);
+    virtual void scanPort(ScanI2C::I2CPort, uint8_t *, uint8_t);
 
     /*
      * A bit of a hack, this tells the scanner not to tell later systems there is a screen to avoid enabling it.

src/detect/ScanI2CTwoWire.cpp

@@ -14,6 +14,15 @@
 #define XPOWERS_AXP192_AXP2101_ADDRESS 0x34
 #endif
 
+bool in_array(uint8_t *array, int size, uint8_t lookfor)
+{
+    int i;
+    for (i = 0; i < size; i++)
+        if (lookfor == array[i])
+            return true;
+    return false;
+}
+
 ScanI2C::FoundDevice ScanI2CTwoWire::find(ScanI2C::DeviceType type) const
 {
     concurrency::LockGuard guard((concurrency::Lock *)&lock);
@@ -135,11 +144,11 @@ uint16_t ScanI2CTwoWire::getRegisterValue(const ScanI2CTwoWire::RegisterLocation
         type = T;                                                                                                                \
         break;
 
-void ScanI2CTwoWire::scanPort(I2CPort port)
+void ScanI2CTwoWire::scanPort(I2CPort port, uint8_t *address, uint8_t asize)
 {
     concurrency::LockGuard guard((concurrency::Lock *)&lock);
 
-    LOG_DEBUG("Scanning for i2c devices on port %d\n", port);
+    LOG_DEBUG("Scanning for I2C devices on port %d\n", port);
 
     uint8_t err;
 
@@ -163,6 +172,11 @@ void ScanI2CTwoWire::scanPort(I2CPort port)
 #endif
 
     for (addr.address = 1; addr.address < 127; addr.address++) {
+        if (asize != 0) {
+            if (!in_array(address, asize, addr.address))
+                continue;
+            LOG_DEBUG("Scanning address 0x%x\n", addr.address);
+        }
         i2cBus->beginTransmission(addr.address);
 #ifdef ARCH_PORTDUINO
         if (i2cBus->read() != -1)
@@ -281,8 +295,9 @@ void ScanI2CTwoWire::scanPort(I2CPort port)
                 if (registerValue == 0x5449) {
                     LOG_INFO("INA3221 sensor found at address 0x%x\n", (uint8_t)addr.address);
                     type = INA3221;
-                } else { // Unknown device
-                    LOG_INFO("No INA3221 found at address 0x%x\n", (uint8_t)addr.address);
+                } else {
+                    LOG_INFO("DFRobot Lark weather station found at address 0x%x\n", (uint8_t)addr.address);
+                    type = DFROBOT_LARK;
                 }
                 break;
             case MCP9808_ADDR:
@@ -299,9 +314,12 @@ void ScanI2CTwoWire::scanPort(I2CPort port)
 
             case SHT31_4x_ADDR:
                 registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x89), 2);
-                if (registerValue == 0x11a2) {
+                if (registerValue == 0x11a2 || registerValue == 0x11da) {
                     type = SHT4X;
                     LOG_INFO("SHT4X sensor found\n");
+                } else if (getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x7E), 2) == 0x5449) {
+                    type = OPT3001;
+                    LOG_INFO("OPT3001 light sensor found\n");
                 } else {
                     type = SHT31;
                     LOG_INFO("SHT31 sensor found\n");
@@ -338,16 +356,21 @@ void ScanI2CTwoWire::scanPort(I2CPort port)
 
                 SCAN_SIMPLE_CASE(PMSA0031_ADDR, PMSA0031, "PMSA0031 air quality sensor found\n")
                 SCAN_SIMPLE_CASE(MPU6050_ADDR, MPU6050, "MPU6050 accelerometer found\n");
+                SCAN_SIMPLE_CASE(BMX160_ADDR, BMX160, "BMX160 accelerometer found\n");
                 SCAN_SIMPLE_CASE(BMA423_ADDR, BMA423, "BMA423 accelerometer found\n");
                 SCAN_SIMPLE_CASE(LSM6DS3_ADDR, LSM6DS3, "LSM6DS3 accelerometer found at address 0x%x\n", (uint8_t)addr.address);
                 SCAN_SIMPLE_CASE(TCA9555_ADDR, TCA9555, "TCA9555 I2C expander found\n");
                 SCAN_SIMPLE_CASE(VEML7700_ADDR, VEML7700, "VEML7700 light sensor found\n");
+                SCAN_SIMPLE_CASE(TSL25911_ADDR, TSL2591, "TSL2591 light sensor found\n");
+                SCAN_SIMPLE_CASE(OPT3001_ADDR, OPT3001, "OPT3001 light sensor found\n");
+                SCAN_SIMPLE_CASE(MLX90632_ADDR, MLX90632, "MLX90632 IR temp sensor found\n");
+                SCAN_SIMPLE_CASE(NAU7802_ADDR, NAU7802, "NAU7802 based scale found\n");
 
             default:
                 LOG_INFO("Device found at address 0x%x was not able to be enumerated\n", addr.address);
             }
         } else if (err == 4) {
-            LOG_ERROR("Unknown error at address 0x%x\n", addr);
+            LOG_ERROR("Unknown error at address 0x%x\n", addr.address);
         }
 
         // Check if a type was found for the enumerated device - save, if so
@@ -358,6 +381,11 @@ void ScanI2CTwoWire::scanPort(I2CPort port)
     }
 }
 
+void ScanI2CTwoWire::scanPort(I2CPort port)
+{
+    scanPort(port, nullptr, 0);
+}
+
 TwoWire *ScanI2CTwoWire::fetchI2CBus(ScanI2C::DeviceAddress address) const
 {
     if (address.port == ScanI2C::I2CPort::WIRE) {
@@ -374,4 +402,4 @@ TwoWire *ScanI2CTwoWire::fetchI2CBus(ScanI2C::DeviceAddress address) const
 size_t ScanI2CTwoWire::countDevices() const
 {
     return foundDevices.size();
-}
+}

src/detect/ScanI2CTwoWire.h

@@ -16,6 +16,8 @@ class ScanI2CTwoWire : public ScanI2C
   public:
     void scanPort(ScanI2C::I2CPort) override;
 
+    void scanPort(ScanI2C::I2CPort, uint8_t *, uint8_t) override;
+
     ScanI2C::FoundDevice find(ScanI2C::DeviceType) const override;
 
     TwoWire *fetchI2CBus(ScanI2C::DeviceAddress) const;
@@ -53,4 +55,4 @@ class ScanI2CTwoWire : public ScanI2C
     uint16_t getRegisterValue(const RegisterLocation &, ResponseWidth) const;
 
     DeviceType probeOLED(ScanI2C::DeviceAddress) const;
-};
+};

src/gps/GPS.cpp

@@ -3,11 +3,13 @@
 #include "Default.h"
 #include "GPS.h"
 #include "NodeDB.h"
+#include "PowerMon.h"
 #include "RTC.h"
 
 #include "main.h" // pmu_found
 #include "sleep.h"
 
+#include "GPSUpdateScheduling.h"
 #include "cas.h"
 #include "ubx.h"
 
@@ -29,6 +31,8 @@ HardwareSerial *GPS::_serial_gps = NULL;
 
 GPS *gps = nullptr;
 
+GPSUpdateScheduling scheduling;
+
 /// Multiple GPS instances might use the same serial port (in sequence), but we can
 /// only init that port once.
 static bool didSerialInit;
@@ -38,6 +42,25 @@ uint8_t uBloxProtocolVersion;
 #define GPS_SOL_EXPIRY_MS 5000 // in millis. give 1 second time to combine different sentences. NMEA Frequency isn't higher anyway
 #define NMEA_MSG_GXGSA "GNGSA" // GSA message (GPGSA, GNGSA etc)
 
+// For logging
+const char *getGPSPowerStateString(GPSPowerState state)
+{
+    switch (state) {
+    case GPS_ACTIVE:
+        return "ACTIVE";
+    case GPS_IDLE:
+        return "IDLE";
+    case GPS_SOFTSLEEP:
+        return "SOFTSLEEP";
+    case GPS_HARDSLEEP:
+        return "HARDSLEEP";
+    case GPS_OFF:
+        return "OFF";
+    default:
+        assert(false); // Unhandled enum value..
+    }
+}
+
 void GPS::UBXChecksum(uint8_t *message, size_t length)
 {
     uint8_t CK_A = 0, CK_B = 0;
@@ -377,9 +400,14 @@ bool GPS::setup()
     int msglen = 0;
 
     if (!didSerialInit) {
-#if !defined(GPS_UC6580)
 
         if (tx_gpio && gnssModel == GNSS_MODEL_UNKNOWN) {
+
+            // if GPS_BAUDRATE is specified in variant (i.e. not 9600), skip to the specified rate.
+            if (speedSelect == 0 && GPS_BAUDRATE != serialSpeeds[speedSelect]) {
+                speedSelect = std::find(serialSpeeds, std::end(serialSpeeds), GPS_BAUDRATE) - serialSpeeds;
+            }
+
             LOG_DEBUG("Probing for GPS at %d \n", serialSpeeds[speedSelect]);
             gnssModel = probe(serialSpeeds[speedSelect]);
             if (gnssModel == GNSS_MODEL_UNKNOWN) {
@@ -395,9 +423,6 @@ bool GPS::setup()
         } else {
             gnssModel = GNSS_MODEL_UNKNOWN;
         }
-#else
-        gnssModel = GNSS_MODEL_UC6580;
-#endif
 
         if (gnssModel == GNSS_MODEL_MTK) {
             /*
@@ -753,7 +778,6 @@ bool GPS::setup()
     }
 
     notifyDeepSleepObserver.observe(&notifyDeepSleep);
-    notifyGPSSleepObserver.observe(&notifyGPSSleep);
 
     return true;
 }
@@ -762,88 +786,185 @@ GPS::~GPS()
 {
     // we really should unregister our sleep observer
     notifyDeepSleepObserver.unobserve(&notifyDeepSleep);
-    notifyGPSSleepObserver.observe(&notifyGPSSleep);
 }
 
-void GPS::setGPSPower(bool on, bool standbyOnly, uint32_t sleepTime)
+// Put the GPS hardware into a specified state
+void GPS::setPowerState(GPSPowerState newState, uint32_t sleepTime)
 {
-    LOG_INFO("Setting GPS power=%d\n", on);
-    if (on) {
-        clearBuffer(); // drop any old data waiting in the buffer before re-enabling
-        if (en_gpio)
-            digitalWrite(en_gpio, on ? GPS_EN_ACTIVE : !GPS_EN_ACTIVE); // turn this on if defined, every time
+    // Update the stored GPSPowerstate, and create local copies
+    GPSPowerState oldState = powerState;
+    powerState = newState;
+    LOG_INFO("GPS power state moving from %s to %s\n", getGPSPowerStateString(oldState), getGPSPowerStateString(newState));
+
+    switch (newState) {
+    case GPS_ACTIVE:
+    case GPS_IDLE:
+        if (oldState == GPS_ACTIVE || oldState == GPS_IDLE) // If hardware already awake, no changes needed
+            break;
+        if (oldState != GPS_ACTIVE && oldState != GPS_IDLE) // If hardware just waking now, clear buffer
+            clearBuffer();
+        powerMon->setState(meshtastic_PowerMon_State_GPS_Active); // Report change for power monitoring (during testing)
+        writePinEN(true);                                         // Power (EN pin): on
+        setPowerPMU(true);                                        // Power (PMU): on
+        writePinStandby(false);                                   // Standby (pin): awake (not standby)
+        setPowerUBLOX(true);                                      // Standby (UBLOX): awake
+        break;
+
+    case GPS_SOFTSLEEP:
+        powerMon->clearState(meshtastic_PowerMon_State_GPS_Active); // Report change for power monitoring (during testing)
+        writePinEN(true);                                           // Power (EN pin): on
+        setPowerPMU(true);                                          // Power (PMU): on
+        writePinStandby(true);                                      // Standby (pin): asleep (not awake)
+        setPowerUBLOX(false, sleepTime);                            // Standby (UBLOX): asleep, timed
+        break;
+
+    case GPS_HARDSLEEP:
+        powerMon->clearState(meshtastic_PowerMon_State_GPS_Active); // Report change for power monitoring (during testing)
+        writePinEN(false);                                          // Power (EN pin): off
+        setPowerPMU(false);                                         // Power (PMU): off
+        writePinStandby(true);                                      // Standby (pin): asleep (not awake)
+        setPowerUBLOX(false, sleepTime);                            // Standby (UBLOX): asleep, timed
+        break;
+
+    case GPS_OFF:
+        assert(sleepTime == 0);                                     // This is an indefinite sleep
+        powerMon->clearState(meshtastic_PowerMon_State_GPS_Active); // Report change for power monitoring (during testing)
+        writePinEN(false);                                          // Power (EN pin): off
+        setPowerPMU(false);                                         // Power (PMU): off
+        writePinStandby(true);                                      // Standby (pin): asleep
+        setPowerUBLOX(false, 0);                                    // Standby (UBLOX): asleep, indefinitely
+        break;
     }
-    isInPowersave = !on;
-    if (!standbyOnly && en_gpio != 0 &&
-        !(HW_VENDOR == meshtastic_HardwareModel_RAK4631 && (rotaryEncoderInterruptImpl1 || upDownInterruptImpl1))) {
-        LOG_DEBUG("GPS powerdown using GPS_EN_ACTIVE\n");
-        digitalWrite(en_gpio, on ? GPS_EN_ACTIVE : !GPS_EN_ACTIVE);
+}
+
+// Set power with EN pin, if relevant
+void GPS::writePinEN(bool on)
+{
+    // Abort: if conflict with Canned Messages when using Wisblock(?)
+    if (HW_VENDOR == meshtastic_HardwareModel_RAK4631 && (rotaryEncoderInterruptImpl1 || upDownInterruptImpl1))
         return;
-    }
-#ifdef HAS_PMU // We only have PMUs on the T-Beam, and that board has a tiny battery to save GPS ephemera, so treat as a standby.
-    if (pmu_found && PMU) {
-        uint8_t model = PMU->getChipModel();
-        if (model == XPOWERS_AXP2101) {
-            if (HW_VENDOR == meshtastic_HardwareModel_TBEAM) {
-                // t-beam v1.2 GNSS power channel
-                on ? PMU->enablePowerOutput(XPOWERS_ALDO3) : PMU->disablePowerOutput(XPOWERS_ALDO3);
-            } else if (HW_VENDOR == meshtastic_HardwareModel_LILYGO_TBEAM_S3_CORE) {
-                // t-beam-s3-core GNSS  power channel
-                on ? PMU->enablePowerOutput(XPOWERS_ALDO4) : PMU->disablePowerOutput(XPOWERS_ALDO4);
-            }
-        } else if (model == XPOWERS_AXP192) {
-            // t-beam v1.1 GNSS  power channel
-            on ? PMU->enablePowerOutput(XPOWERS_LDO3) : PMU->disablePowerOutput(XPOWERS_LDO3);
-        }
+
+    // Abort: if pin unset
+    if (!en_gpio)
         return;
-    }
+
+    // Determine new value for the pin
+    bool val = GPS_EN_ACTIVE ? on : !on;
+
+    // Write and log
+    pinMode(en_gpio, OUTPUT);
+    digitalWrite(en_gpio, val);
+#ifdef GPS_EXTRAVERBOSE
+    LOG_DEBUG("Pin EN %s\n", val == HIGH ? "HIGH" : "LOW");
 #endif
+}
+
+// Set the value of the STANDBY pin, if relevant
+// true for standby state, false for awake
+void GPS::writePinStandby(bool standby)
+{
 #ifdef PIN_GPS_STANDBY // Specifically the standby pin for L76B, L76K and clones
-    if (on) {
-        LOG_INFO("Waking GPS\n");
-        pinMode(PIN_GPS_STANDBY, OUTPUT);
-        // Some PCB's use an inverse logic due to a transistor driver
-        // Example for this is the Pico-Waveshare Lora+GPS HAT
+
+// Determine the new value for the pin
+// Normally: active HIGH for awake
 #ifdef PIN_GPS_STANDBY_INVERTED
-        digitalWrite(PIN_GPS_STANDBY, 0);
+    bool val = standby;
 #else
-        digitalWrite(PIN_GPS_STANDBY, 1);
+    bool val = !standby;
 #endif
-        return;
-    } else {
-        LOG_INFO("GPS entering sleep\n");
-        // notifyGPSSleep.notifyObservers(NULL);
-        pinMode(PIN_GPS_STANDBY, OUTPUT);
-#ifdef PIN_GPS_STANDBY_INVERTED
-        digitalWrite(PIN_GPS_STANDBY, 1);
-#else
-        digitalWrite(PIN_GPS_STANDBY, 0);
+
+    // Write and log
+    pinMode(PIN_GPS_STANDBY, OUTPUT);
+    digitalWrite(PIN_GPS_STANDBY, val);
+#ifdef GPS_EXTRAVERBOSE
+    LOG_DEBUG("Pin STANDBY %s\n", val == HIGH ? "HIGH" : "LOW");
 #endif
+#endif
+}
+
+// Enable / Disable GPS with PMU, if present
+void GPS::setPowerPMU(bool on)
+{
+    // We only have PMUs on the T-Beam, and that board has a tiny battery to save GPS ephemera,
+    // so treat as a standby.
+#ifdef HAS_PMU
+    // Abort: if no PMU
+    if (!pmu_found)
         return;
+
+    // Abort: if PMU not initialized
+    if (!PMU)
+        return;
+
+    uint8_t model = PMU->getChipModel();
+    if (model == XPOWERS_AXP2101) {
+        if (HW_VENDOR == meshtastic_HardwareModel_TBEAM) {
+            // t-beam v1.2 GNSS power channel
+            on ? PMU->enablePowerOutput(XPOWERS_ALDO3) : PMU->disablePowerOutput(XPOWERS_ALDO3);
+        } else if (HW_VENDOR == meshtastic_HardwareModel_LILYGO_TBEAM_S3_CORE) {
+            // t-beam-s3-core GNSS  power channel
+            on ? PMU->enablePowerOutput(XPOWERS_ALDO4) : PMU->disablePowerOutput(XPOWERS_ALDO4);
+        }
+    } else if (model == XPOWERS_AXP192) {
+        // t-beam v1.1 GNSS  power channel
+        on ? PMU->enablePowerOutput(XPOWERS_LDO3) : PMU->disablePowerOutput(XPOWERS_LDO3);
     }
+
+#ifdef GPS_EXTRAVERBOSE
+    LOG_DEBUG("PMU %s\n", on ? "on" : "off");
 #endif
-    if (!on) {
-        if (gnssModel == GNSS_MODEL_UBLOX) {
-            uint8_t msglen;
-            LOG_DEBUG("Sleep Time: %i\n", sleepTime);
-            if (strncmp(info.hwVersion, "000A0000", 8) != 0) {
-                for (int i = 0; i < 4; i++) {
-                    gps->_message_PMREQ[0 + i] = sleepTime >> (i * 8); // Encode the sleep time in millis into the packet
-                }
-                msglen = gps->makeUBXPacket(0x02, 0x41, sizeof(_message_PMREQ), gps->_message_PMREQ);
-            } else {
-                for (int i = 0; i < 4; i++) {
-                    gps->_message_PMREQ_10[4 + i] = sleepTime >> (i * 8); // Encode the sleep time in millis into the packet
-                }
-                msglen = gps->makeUBXPacket(0x02, 0x41, sizeof(_message_PMREQ_10), gps->_message_PMREQ_10);
-            }
-            gps->_serial_gps->write(gps->UBXscratch, msglen);
+#endif
+}
+
+// Set UBLOX power, if relevant
+void GPS::setPowerUBLOX(bool on, uint32_t sleepMs)
+{
+    // Abort: if not UBLOX hardware
+    if (gnssModel != GNSS_MODEL_UBLOX)
+        return;
+
+    // If waking
+    if (on) {
+        gps->_serial_gps->write(0xFF);
+        clearBuffer(); // This often returns old data, so drop it
+#ifdef GPS_EXTRAVERBOSE
+        LOG_DEBUG("UBLOX: wake\n");
+#endif
+    }
+
+    // If putting to sleep
+    else {
+        uint8_t msglen;
+
+        // If we're being asked to sleep indefinitely, make *sure* we're awake first, to process the new sleep command
+        if (sleepMs == 0) {
+            setPowerUBLOX(true);
+            delay(500);
         }
-    } else {
-        if (gnssModel == GNSS_MODEL_UBLOX) {
-            gps->_serial_gps->write(0xFF);
-            clearBuffer(); // This often returns old data, so drop it
+
+        // Determine hardware version
+        if (strncmp(info.hwVersion, "000A0000", 8) != 0) {
+            // Encode the sleep time in millis into the packet
+            for (int i = 0; i < 4; i++)
+                gps->_message_PMREQ[0 + i] = sleepMs >> (i * 8);
+
+            // Record the message length
+            msglen = gps->makeUBXPacket(0x02, 0x41, sizeof(_message_PMREQ), gps->_message_PMREQ);
+        } else {
+            // Encode the sleep time in millis into the packet
+            for (int i = 0; i < 4; i++)
+                gps->_message_PMREQ_10[4 + i] = sleepMs >> (i * 8);
+
+            // Record the message length
+            msglen = gps->makeUBXPacket(0x02, 0x41, sizeof(_message_PMREQ_10), gps->_message_PMREQ_10);
         }
+
+        // Send the UBX packet
+        gps->_serial_gps->write(gps->UBXscratch, msglen);
+
+#ifdef GPS_EXTRAVERBOSE
+        LOG_DEBUG("UBLOX: sleep for %dmS\n", sleepMs);
+#endif
     }
 }
 
@@ -856,81 +977,54 @@ void GPS::setConnected()
     }
 }
 
-/**
- * Switch the GPS into a mode where we are actively looking for a lock, or alternatively switch GPS into a low power mode
- *
- * calls sleep/wake
- */
-void GPS::setAwake(bool on)
+// We want a GPS lock. Wake the hardware
+void GPS::up()
 {
-    if (isAwake != on) {
-        LOG_DEBUG("WANT GPS=%d\n", on);
-        isAwake = on;
-        if (!enabled) { // short circuit if the user has disabled GPS
-            setGPSPower(false, false, 0);
-            return;
-        }
+    scheduling.informSearching();
+    setPowerState(GPS_ACTIVE);
+}
 
-        if (on) {
-            lastWakeStartMsec = millis();
-        } else {
-            lastSleepStartMsec = millis();
-            if (GPSCycles == 1) { // Skipping initial lock time, as it will likely be much longer than average
-                averageLockTime = lastSleepStartMsec - lastWakeStartMsec;
-            } else if (GPSCycles > 1) {
-                averageLockTime += ((int32_t)(lastSleepStartMsec - lastWakeStartMsec) - averageLockTime) / (int32_t)GPSCycles;
-            }
-            GPSCycles++;
-            LOG_DEBUG("GPS Lock took %d, average %d\n", (lastSleepStartMsec - lastWakeStartMsec) / 1000, averageLockTime / 1000);
-        }
-        if ((int32_t)getSleepTime() - averageLockTime >
-            15 * 60 * 1000) { // 15 minutes is probably long enough to make a complete poweroff worth it.
-            setGPSPower(on, false, getSleepTime() - averageLockTime);
-            return;
-        } else if ((int32_t)getSleepTime() - averageLockTime > 10000) { // 10 seconds is enough for standby
-#ifdef GPS_UC6580
-            setGPSPower(on, false, getSleepTime() - averageLockTime);
-#else
-            setGPSPower(on, true, getSleepTime() - averageLockTime);
+// We've got a GPS lock. Enter a low power state, potentially.
+void GPS::down()
+{
+    scheduling.informGotLock();
+    uint32_t predictedSearchDuration = scheduling.predictedSearchDurationMs();
+    uint32_t sleepTime = scheduling.msUntilNextSearch();
+    uint32_t updateInterval = Default::getConfiguredOrDefaultMs(config.position.gps_update_interval);
+
+    LOG_DEBUG("%us until next search\n", sleepTime / 1000);
+
+    // If update interval less than 10 seconds, no attempt to sleep
+    if (updateInterval <= 10 * 1000UL)
+        setPowerState(GPS_IDLE);
+
+    else {
+        // Check whether the GPS hardware is capable of GPS_SOFTSLEEP
+        // If not, fallback to GPS_HARDSLEEP instead
+        bool softsleepSupported = false;
+        if (gnssModel == GNSS_MODEL_UBLOX) // U-blox is supported via PMREQ
+            softsleepSupported = true;
+#ifdef PIN_GPS_STANDBY // L76B, L76K and clones have a standby pin
+        softsleepSupported = true;
 #endif
-            return;
-        }
-        if (averageLockTime > 20000) {
-            averageLockTime -= 1000; // eventually want to sleep again.
-        }
-        if (on)
-            setGPSPower(true, true, 0); // make sure we don't have a fallthrough where GPS is stuck off
+
+        // How long does gps_update_interval need to be, for GPS_HARDSLEEP to become more efficient than GPS_SOFTSLEEP?
+        // Heuristic equation. A compromise manually fitted to power observations from U-blox NEO-6M and M10050
+        // https://www.desmos.com/calculator/6gvjghoumr
+        // This is not particularly accurate, but probably an impromevement over a single, fixed threshold
+        uint32_t hardsleepThreshold = (2750 * pow(predictedSearchDuration / 1000, 1.22));
+        LOG_DEBUG("gps_update_interval >= %us needed to justify hardsleep\n", hardsleepThreshold / 1000);
+
+        // If update interval too short: softsleep (if supported by hardware)
+        if (softsleepSupported && updateInterval < hardsleepThreshold)
+            setPowerState(GPS_SOFTSLEEP, sleepTime);
+
+        // If update interval long enough (or softsleep unsupported): hardsleep instead
+        else
+            setPowerState(GPS_HARDSLEEP, sleepTime);
     }
 }
 
-/** Get how long we should stay looking for each acquisition in msecs
- */
-uint32_t GPS::getWakeTime() const
-{
-    uint32_t t = config.position.position_broadcast_secs;
-
-    if (t == UINT32_MAX)
-        return t; // already maxint
-
-    return Default::getConfiguredOrDefaultMs(t, default_broadcast_interval_secs);
-}
-
-/** Get how long we should sleep between aqusition attempts in msecs
- */
-uint32_t GPS::getSleepTime() const
-{
-    uint32_t t = config.position.gps_update_interval;
-
-    // We'll not need the GPS thread to wake up again after first acq. with fixed position.
-    if (config.position.gps_mode != meshtastic_Config_PositionConfig_GpsMode_ENABLED || config.position.fixed_position)
-        t = UINT32_MAX; // Sleep forever now
-
-    if (t == UINT32_MAX)
-        return t; // already maxint
-
-    return Default::getConfiguredOrDefaultMs(t, default_gps_update_interval);
-}
-
 void GPS::publishUpdate()
 {
     if (shouldPublish) {
@@ -979,13 +1073,13 @@ int32_t GPS::runOnce()
         return disable();
     }
 
-    if (whileIdle()) {
+    if (whileActive()) {
         // if we have received valid NMEA claim we are connected
         setConnected();
     } else {
         if ((config.position.gps_mode == meshtastic_Config_PositionConfig_GpsMode_ENABLED) && (gnssModel == GNSS_MODEL_UBLOX)) {
             // reset the GPS on next bootup
-            if (devicestate.did_gps_reset && (millis() - lastWakeStartMsec > 60000) && !hasFlow()) {
+            if (devicestate.did_gps_reset && scheduling.elapsedSearchMs() > 60 * 1000UL && !hasFlow()) {
                 LOG_DEBUG("GPS is not communicating, trying factory reset on next bootup.\n");
                 devicestate.did_gps_reset = false;
                 nodeDB->saveDeviceStateToDisk();
@@ -1000,54 +1094,43 @@ int32_t GPS::runOnce()
         // gps->factoryReset();
     }
 
-    // If we are overdue for an update, turn on the GPS and at least publish the current status
-    uint32_t now = millis();
-    uint32_t timeAsleep = now - lastSleepStartMsec;
+    // If we're due for an update, wake the GPS
+    if (!config.position.fixed_position && powerState != GPS_ACTIVE && scheduling.isUpdateDue())
+        up();
 
-    auto sleepTime = getSleepTime();
-    if (!isAwake && (sleepTime != UINT32_MAX) &&
-        ((timeAsleep > sleepTime) || (isInPowersave && timeAsleep > (sleepTime - averageLockTime)))) {
-        // We now want to be awake - so wake up the GPS
-        setAwake(true);
+    // If we've already set time from the GPS, no need to ask the GPS
+    bool gotTime = (getRTCQuality() >= RTCQualityGPS);
+    if (!gotTime && lookForTime()) { // Note: we count on this && short-circuiting and not resetting the RTC time
+        gotTime = true;
+        shouldPublish = true;
     }
 
-    // While we are awake
-    if (isAwake) {
-        // LOG_DEBUG("looking for location\n");
-        // If we've already set time from the GPS, no need to ask the GPS
-        bool gotTime = (getRTCQuality() >= RTCQualityGPS);
-        if (!gotTime && lookForTime()) { // Note: we count on this && short-circuiting and not resetting the RTC time
-            gotTime = true;
-            shouldPublish = true;
-        }
+    bool gotLoc = lookForLocation();
+    if (gotLoc && !hasValidLocation) { // declare that we have location ASAP
+        LOG_DEBUG("hasValidLocation RISING EDGE\n");
+        hasValidLocation = true;
+        shouldPublish = true;
+    }
 
-        bool gotLoc = lookForLocation();
-        if (gotLoc && !hasValidLocation) { // declare that we have location ASAP
-            LOG_DEBUG("hasValidLocation RISING EDGE\n");
-            hasValidLocation = true;
-            shouldPublish = true;
-        }
+    bool tooLong = scheduling.searchedTooLong();
+    if (tooLong)
+        LOG_WARN("Couldn't publish a valid location: didn't get a GPS lock in time.\n");
 
-        now = millis();
-        auto wakeTime = getWakeTime();
-        bool tooLong = wakeTime != UINT32_MAX && (now - lastWakeStartMsec) > wakeTime;
+    // Once we get a location we no longer desperately want an update
+    // LOG_DEBUG("gotLoc %d, tooLong %d, gotTime %d\n", gotLoc, tooLong, gotTime);
+    if ((gotLoc && gotTime) || tooLong) {
 
-        // Once we get a location we no longer desperately want an update
-        // LOG_DEBUG("gotLoc %d, tooLong %d, gotTime %d\n", gotLoc, tooLong, gotTime);
-        if ((gotLoc && gotTime) || tooLong) {
-
-            if (tooLong) {
-                // we didn't get a location during this ack window, therefore declare loss of lock
-                if (hasValidLocation) {
-                    LOG_DEBUG("hasValidLocation FALLING EDGE (last read: %d)\n", gotLoc);
-                }
-                p = meshtastic_Position_init_default;
-                hasValidLocation = false;
+        if (tooLong) {
+            // we didn't get a location during this ack window, therefore declare loss of lock
+            if (hasValidLocation) {
+                LOG_DEBUG("hasValidLocation FALLING EDGE\n");
             }
-
-            setAwake(false);
-            shouldPublish = true; // publish our update for this just finished acquisition window
+            p = meshtastic_Position_init_default;
+            hasValidLocation = false;
         }
+
+        down();
+        shouldPublish = true; // publish our update for this just finished acquisition window
     }
 
     // If state has changed do a publish
@@ -1058,7 +1141,7 @@ int32_t GPS::runOnce()
 
     // 9600bps is approx 1 byte per msec, so considering our buffer size we never need to wake more often than 200ms
     // if not awake we can run super infrquently (once every 5 secs?) to see if we need to wake.
-    return isAwake ? GPS_THREAD_INTERVAL : 5000;
+    return (powerState == GPS_ACTIVE) ? GPS_THREAD_INTERVAL : 5000;
 }
 
 // clear the GPS rx buffer as quickly as possible
@@ -1073,9 +1156,7 @@ void GPS::clearBuffer()
 int GPS::prepareDeepSleep(void *unused)
 {
     LOG_INFO("GPS deep sleep!\n");
-
-    setAwake(false);
-
+    disable();
     return 0;
 }
 
@@ -1090,6 +1171,9 @@ GnssModel_t GPS::probe(int serialSpeed)
         _serial_gps->updateBaudRate(serialSpeed);
     }
 #endif
+#ifdef GNSS_Airoha // add by WayenWeng
+    return GNSS_MODEL_UNKNOWN;
+#else
 #ifdef GPS_DEBUG
     for (int i = 0; i < 20; i++) {
         getACK("$GP", 200);
@@ -1103,6 +1187,15 @@ GnssModel_t GPS::probe(int serialSpeed)
     _serial_gps->write("$PCAS03,0,0,0,0,0,0,0,0,0,0,,,0,0*02\r\n");
     delay(20);
 
+    // get version information from Unicore UFirebirdII Series
+    // Works for: UC6580, UM620, UM621, UM670A, UM680A, or UM681A
+    _serial_gps->write("$PDTINFO\r\n");
+    delay(750);
+    if (getACK("UC6580", 500) == GNSS_RESPONSE_OK) {
+        LOG_INFO("UC6580 detected, using UC6580 Module\n");
+        return GNSS_MODEL_UC6580;
+    }
+
     // Get version information
     clearBuffer();
     _serial_gps->write("$PCAS06,1*1A\r\n");
@@ -1236,6 +1329,7 @@ GnssModel_t GPS::probe(int serialSpeed)
     }
 
     return GNSS_MODEL_UBLOX;
+#endif // !GNSS_Airoha
 }
 
 GPS *GPS::createGps()
@@ -1271,12 +1365,6 @@ GPS *GPS::createGps()
     new_gps->tx_gpio = _tx_gpio;
     new_gps->en_gpio = _en_gpio;
 
-    if (_en_gpio != 0) {
-        LOG_DEBUG("Setting %d to output.\n", _en_gpio);
-        pinMode(_en_gpio, OUTPUT);
-        digitalWrite(_en_gpio, !GPS_EN_ACTIVE);
-    }
-
 #ifdef PIN_GPS_PPS
     // pulse per second
     pinMode(PIN_GPS_PPS, INPUT);
@@ -1291,7 +1379,8 @@ GPS *GPS::createGps()
     LOG_DEBUG("Using " NMEA_MSG_GXGSA " for 3DFIX and PDOP\n");
 #endif
 
-    new_gps->setGPSPower(true, false, 0);
+    // Make sure the GPS is awake before performing any init.
+    new_gps->up();
 
 #ifdef PIN_GPS_RESET
     pinMode(PIN_GPS_RESET, OUTPUT);
@@ -1299,7 +1388,6 @@ GPS *GPS::createGps()
     delay(10);
     digitalWrite(PIN_GPS_RESET, !GPS_RESET_MODE);
 #endif
-    new_gps->setAwake(true); // Wake GPS power before doing any init
 
     if (_serial_gps) {
 #ifdef ARCH_ESP32
@@ -1315,13 +1403,6 @@ GPS *GPS::createGps()
 #else
         _serial_gps->begin(GPS_BAUDRATE);
 #endif
-
-        /*
-         * T-Beam-S3-Core will be preset to use gps Probe here, and other boards will not be changed first
-         */
-#if defined(GPS_UC6580)
-        _serial_gps->updateBaudRate(115200);
-#endif
     }
     return new_gps;
 }
@@ -1403,6 +1484,11 @@ bool GPS::factoryReset()
  */
 bool GPS::lookForTime()
 {
+#ifdef GNSS_Airoha // add by WayenWeng
+    uint8_t fix = reader.fixQuality();
+    uint32_t now = millis();
+#endif
+
     auto ti = reader.time;
     auto d = reader.date;
     if (ti.isValid() && d.isValid()) { // Note: we don't check for updated, because we'll only be called if needed
@@ -1437,6 +1523,13 @@ The Unix epoch (or Unix time or POSIX time or Unix timestamp) is the number of s
  */
 bool GPS::lookForLocation()
 {
+#ifdef GNSS_Airoha // add by WayenWeng
+    if ((config.position.gps_update_interval * 1000) >= (GPS_FIX_HOLD_TIME * 2)) {
+        uint8_t fix = reader.fixQuality();
+        uint32_t now = millis();
+    }
+#endif
+
     // By default, TinyGPS++ does not parse GPGSA lines, which give us
     //   the 2D/3D fixType (see NMEAGPS.h)
     // At a minimum, use the fixQuality indicator in GPGGA (FIXME?)
@@ -1585,13 +1678,13 @@ bool GPS::hasFlow()
     return reader.passedChecksum() > 0;
 }
 
-bool GPS::whileIdle()
+bool GPS::whileActive()
 {
     unsigned int charsInBuf = 0;
     bool isValid = false;
-    if (!isAwake) {
+    if (powerState != GPS_ACTIVE) {
         clearBuffer();
-        return isAwake;
+        return false;
     }
 #ifdef SERIAL_BUFFER_SIZE
     if (_serial_gps->available() >= SERIAL_BUFFER_SIZE - 1) {
@@ -1622,16 +1715,21 @@ bool GPS::whileIdle()
 }
 void GPS::enable()
 {
+    // Clear the old scheduling info (reset the lock-time prediction)
+    scheduling.reset();
+
     enabled = true;
     setInterval(GPS_THREAD_INTERVAL);
-    setAwake(true);
+
+    scheduling.informSearching();
+    setPowerState(GPS_ACTIVE);
 }
 
 int32_t GPS::disable()
 {
     enabled = false;
     setInterval(INT32_MAX);
-    setAwake(false);
+    setPowerState(GPS_OFF);
 
     return INT32_MAX;
 }
@@ -1640,11 +1738,11 @@ void GPS::toggleGpsMode()
 {
     if (config.position.gps_mode == meshtastic_Config_PositionConfig_GpsMode_ENABLED) {
         config.position.gps_mode = meshtastic_Config_PositionConfig_GpsMode_DISABLED;
-        LOG_DEBUG("Flag set to false for gps power. GpsMode: DISABLED\n");
+        LOG_INFO("User toggled GpsMode. Now DISABLED.\n");
         disable();
     } else if (config.position.gps_mode == meshtastic_Config_PositionConfig_GpsMode_DISABLED) {
         config.position.gps_mode = meshtastic_Config_PositionConfig_GpsMode_ENABLED;
-        LOG_DEBUG("Flag set to true to restore power. GpsMode: ENABLED\n");
+        LOG_INFO("User toggled GpsMode. Now ENABLED\n");
         enable();
     }
 }

src/gps/GPS.h

@@ -38,6 +38,14 @@ typedef enum {
     GNSS_RESPONSE_OK,
 } GPS_RESPONSE;
 
+enum GPSPowerState : uint8_t {
+    GPS_ACTIVE,    // Awake and want a position
+    GPS_IDLE,      // Awake, but not wanting another position yet
+    GPS_SOFTSLEEP, // Physically powered on, but soft-sleeping
+    GPS_HARDSLEEP, // Physically powered off, but scheduled to wake
+    GPS_OFF        // Powered off indefinitely
+};
+
 // Generate a string representation of DOP
 const char *getDOPString(uint32_t dop);
 
@@ -60,14 +68,11 @@ class GPS : private concurrency::OSThread
     uint8_t fixType = 0;      // fix type from GPGSA
 #endif
   private:
-    uint32_t lastWakeStartMsec = 0, lastSleepStartMsec = 0;
     const int serialSpeeds[6] = {9600, 4800, 38400, 57600, 115200, 9600};
 
     uint32_t rx_gpio = 0;
     uint32_t tx_gpio = 0;
     uint32_t en_gpio = 0;
-    int32_t averageLockTime = 0;
-    uint32_t GPSCycles = 0;
 
     int speedSelect = 0;
     int probeTries = 2;
@@ -78,8 +83,6 @@ class GPS : private concurrency::OSThread
      */
     bool hasValidLocation = false; // default to false, until we complete our first read
 
-    bool isAwake = false; // true if we want a location right now
-
     bool isInPowersave = false;
 
     bool shouldPublish = false; // If we've changed GPS state, this will force a publish the next loop()
@@ -89,10 +92,11 @@ class GPS : private concurrency::OSThread
     bool GPSInitFinished = false; // Init thread finished?
     bool GPSInitStarted = false;  // Init thread finished?
 
+    GPSPowerState powerState = GPS_OFF; // GPS_ACTIVE if we want a location right now
+
     uint8_t numSatellites = 0;
 
     CallbackObserver<GPS, void *> notifyDeepSleepObserver = CallbackObserver<GPS, void *>(this, &GPS::prepareDeepSleep);
-    CallbackObserver<GPS, void *> notifyGPSSleepObserver = CallbackObserver<GPS, void *>(this, &GPS::prepareDeepSleep);
 
   public:
     /** If !NULL we will use this serial port to construct our GPS */
@@ -168,7 +172,8 @@ class GPS : private concurrency::OSThread
     // toggle between enabled/disabled
     void toggleGpsMode();
 
-    void setGPSPower(bool on, bool standbyOnly, uint32_t sleepTime);
+    // Change the power state of the GPS - for power saving / shutdown
+    void setPowerState(GPSPowerState newState, uint32_t sleepMs = 0);
 
     /// Returns true if we have acquired GPS lock.
     virtual bool hasLock();
@@ -199,18 +204,18 @@ class GPS : private concurrency::OSThread
 
     GPS_RESPONSE getACKCas(uint8_t class_id, uint8_t msg_id, uint32_t waitMillis);
 
-    /**
-     * Switch the GPS into a mode where we are actively looking for a lock, or alternatively switch GPS into a low power mode
-     *
-     * calls sleep/wake
-     */
-    void setAwake(bool on);
     virtual bool factoryReset();
 
     // Creates an instance of the GPS class.
     // Returns the new instance or null if the GPS is not present.
     static GPS *createGps();
 
+    // Wake the GPS hardware - ready for an update
+    void up();
+
+    // Let the GPS hardware save power between updates
+    void down();
+
   protected:
     /**
      * Perform any processing that should be done only while the GPS is awake and looking for a fix.
@@ -233,7 +238,7 @@ class GPS : private concurrency::OSThread
      *
      * Return true if we received a valid message from the GPS
      */
-    virtual bool whileIdle();
+    virtual bool whileActive();
 
     /**
      * Perform any processing that should be done only while the GPS is awake and looking for a fix.
@@ -260,13 +265,21 @@ class GPS : private concurrency::OSThread
     void UBXChecksum(uint8_t *message, size_t length);
     void CASChecksum(uint8_t *message, size_t length);
 
-    /** Get how long we should stay looking for each aquisition
+    /** Set power with EN pin, if relevant
      */
-    uint32_t getWakeTime() const;
+    void writePinEN(bool on);
 
-    /** Get how long we should sleep between aqusition attempts
+    /** Set the value of the STANDBY pin, if relevant
      */
-    uint32_t getSleepTime() const;
+    void writePinStandby(bool standby);
+
+    /** Set GPS power with PMU, if relevant
+     */
+    void setPowerPMU(bool on);
+
+    /** Set UBLOX power, if relevant
+     */
+    void setPowerUBLOX(bool on, uint32_t sleepMs = 0);
 
     /**
      * Tell users we have new GPS readings
@@ -282,6 +295,8 @@ class GPS : private concurrency::OSThread
     // delay counter to allow more sats before fixed position stops GPS thread
     uint8_t fixeddelayCtr = 0;
 
+    const char *powerStateToString();
+
   protected:
     GnssModel_t gnssModel = GNSS_MODEL_UNKNOWN;
 };

src/gps/GPSUpdateScheduling.cpp

@@ -0,0 +1,118 @@
+#include "GPSUpdateScheduling.h"
+
+#include "Default.h"
+
+// Mark the time when searching for GPS position begins
+void GPSUpdateScheduling::informSearching()
+{
+    searchStartedMs = millis();
+}
+
+// Mark the time when searching for GPS is complete,
+// then update the predicted lock-time
+void GPSUpdateScheduling::informGotLock()
+{
+    searchEndedMs = millis();
+    LOG_DEBUG("Took %us to get lock\n", (searchEndedMs - searchStartedMs) / 1000);
+    updateLockTimePrediction();
+}
+
+// Clear old lock-time prediction data.
+// When re-enabling GPS with user button.
+void GPSUpdateScheduling::reset()
+{
+    searchStartedMs = 0;
+    searchEndedMs = 0;
+    searchCount = 0;
+    predictedMsToGetLock = 0;
+}
+
+// How many milliseconds before we should next search for GPS position
+// Used by GPS hardware directly, to enter timed hardware sleep
+uint32_t GPSUpdateScheduling::msUntilNextSearch()
+{
+    uint32_t now = millis();
+
+    // Target interval (seconds), between GPS updates
+    uint32_t updateInterval = Default::getConfiguredOrDefaultMs(config.position.gps_update_interval, default_gps_update_interval);
+
+    // Check how long until we should start searching, to hopefully hit our target interval
+    uint32_t dueAtMs = searchEndedMs + updateInterval;
+    uint32_t compensatedStart = dueAtMs - predictedMsToGetLock;
+    int32_t remainingMs = compensatedStart - now;
+
+    // If we should have already started (negative value), start ASAP
+    if (remainingMs < 0)
+        remainingMs = 0;
+
+    return (uint32_t)remainingMs;
+}
+
+// How long have we already been searching?
+// Used to abort a search in progress, if it runs unnaceptably long
+uint32_t GPSUpdateScheduling::elapsedSearchMs()
+{
+    // If searching
+    if (searchStartedMs > searchEndedMs)
+        return millis() - searchStartedMs;
+
+    // If not searching - 0ms. We shouldn't really consume this value
+    else
+        return 0;
+}
+
+// Is it now time to begin searching for a GPS position?
+bool GPSUpdateScheduling::isUpdateDue()
+{
+    return (msUntilNextSearch() == 0);
+}
+
+// Have we been searching for a GPS position for too long?
+bool GPSUpdateScheduling::searchedTooLong()
+{
+    uint32_t maxSearchMs =
+        Default::getConfiguredOrDefaultMs(config.position.position_broadcast_secs, default_broadcast_interval_secs);
+
+    // If broadcast interval set to max, no such thing as "too long"
+    if (maxSearchMs == UINT32_MAX)
+        return false;
+
+    // If we've been searching longer than our position broadcast interval: that's too long
+    else if (elapsedSearchMs() > maxSearchMs)
+        return true;
+
+    // Otherwise, not too long yet!
+    else
+        return false;
+}
+
+// Updates the predicted time-to-get-lock, by exponentially smoothing the latest observation
+void GPSUpdateScheduling::updateLockTimePrediction()
+{
+
+    // How long did it take to get GPS lock this time?
+    // Duration between down() calls
+    int32_t lockTime = searchEndedMs - searchStartedMs;
+    if (lockTime < 0)
+        lockTime = 0;
+
+    // Ignore the first lock-time: likely to be long, will skew data
+
+    // Second locktime: likely stable. Use to intialize the smoothing filter
+    if (searchCount == 1)
+        predictedMsToGetLock = lockTime;
+
+    // Third locktime and after: predict using exponential smoothing. Respond slowly to changes
+    else if (searchCount > 1)
+        predictedMsToGetLock = (lockTime * weighting) + (predictedMsToGetLock * (1 - weighting));
+
+    searchCount++; // Only tracked so we can diregard initial lock-times
+
+    LOG_DEBUG("Predicting %us to get next lock\n", predictedMsToGetLock / 1000);
+}
+
+// How long do we expect to spend searching for a lock?
+uint32_t GPSUpdateScheduling::predictedSearchDurationMs()
+{
+    return GPSUpdateScheduling::predictedMsToGetLock;
+}

src/gps/GPSUpdateScheduling.h

@@ -0,0 +1,29 @@
+#pragma once
+
+#include "configuration.h"
+
+// Encapsulates code responsible for the timing of GPS updates
+class GPSUpdateScheduling
+{
+  public:
+    // Marks the time of these events, for calculation use
+    void informSearching();
+    void informGotLock(); // Predicted lock-time is recalculated here
+
+    void reset();           // Reset the prediction - after GPS::disable() / GPS::enable()
+    bool isUpdateDue();     // Is it time to begin searching for a GPS position?
+    bool searchedTooLong(); // Have we been searching for too long?
+
+    uint32_t msUntilNextSearch(); // How long until we need to begin searching for a GPS? Info provided to GPS hardware for sleep
+    uint32_t elapsedSearchMs();   // How long have we been searching so far?
+    uint32_t predictedSearchDurationMs(); // How long do we expect to spend searching for a lock?
+
+  private:
+    void updateLockTimePrediction(); // Called from informGotLock
+    uint32_t searchStartedMs = 0;
+    uint32_t searchEndedMs = 0;
+    uint32_t searchCount = 0;
+    uint32_t predictedMsToGetLock = 0;
+
+    const float weighting = 0.2; // Controls exponential smoothing of lock-times prediction. 20% weighting of "latest lock-time".
+};

src/gps/GeoCoord.cpp

@@ -486,3 +486,91 @@ std::shared_ptr<GeoCoord> GeoCoord::pointAtDistance(double bearing, double range
 
     return std::make_shared<GeoCoord>(double(lat), double(lon), this->getAltitude());
 }
+
+/**
+ * Convert bearing to degrees
+ * @param bearing
+ * The bearing in string format
+ * @return Bearing in degrees
+ */
+uint GeoCoord::bearingToDegrees(const char *bearing)
+{
+    if (strcmp(bearing, "N") == 0)
+        return 0;
+    else if (strcmp(bearing, "NNE") == 0)
+        return 22;
+    else if (strcmp(bearing, "NE") == 0)
+        return 45;
+    else if (strcmp(bearing, "ENE") == 0)
+        return 67;
+    else if (strcmp(bearing, "E") == 0)
+        return 90;
+    else if (strcmp(bearing, "ESE") == 0)
+        return 112;
+    else if (strcmp(bearing, "SE") == 0)
+        return 135;
+    else if (strcmp(bearing, "SSE") == 0)
+        return 157;
+    else if (strcmp(bearing, "S") == 0)
+        return 180;
+    else if (strcmp(bearing, "SSW") == 0)
+        return 202;
+    else if (strcmp(bearing, "SW") == 0)
+        return 225;
+    else if (strcmp(bearing, "WSW") == 0)
+        return 247;
+    else if (strcmp(bearing, "W") == 0)
+        return 270;
+    else if (strcmp(bearing, "WNW") == 0)
+        return 292;
+    else if (strcmp(bearing, "NW") == 0)
+        return 315;
+    else if (strcmp(bearing, "NNW") == 0)
+        return 337;
+    else
+        return 0;
+}
+
+/**
+ * Convert bearing to string
+ * @param degrees
+ * The bearing in degrees
+ * @return Bearing in string format
+ */
+const char *GeoCoord::degreesToBearing(uint degrees)
+{
+    if (degrees >= 348 || degrees < 11)
+        return "N";
+    else if (degrees >= 11 && degrees < 34)
+        return "NNE";
+    else if (degrees >= 34 && degrees < 56)
+        return "NE";
+    else if (degrees >= 56 && degrees < 79)
+        return "ENE";
+    else if (degrees >= 79 && degrees < 101)
+        return "E";
+    else if (degrees >= 101 && degrees < 124)
+        return "ESE";
+    else if (degrees >= 124 && degrees < 146)
+        return "SE";
+    else if (degrees >= 146 && degrees < 169)
+        return "SSE";
+    else if (degrees >= 169 && degrees < 191)
+        return "S";
+    else if (degrees >= 191 && degrees < 214)
+        return "SSW";
+    else if (degrees >= 214 && degrees < 236)
+        return "SW";
+    else if (degrees >= 236 && degrees < 259)
+        return "WSW";
+    else if (degrees >= 259 && degrees < 281)
+        return "W";
+    else if (degrees >= 281 && degrees < 304)
+        return "WNW";
+    else if (degrees >= 304 && degrees < 326)
+        return "NW";
+    else if (degrees >= 326 && degrees < 348)
+        return "NNW";
+    else
+        return "N";
+}

src/gps/GeoCoord.h

@@ -117,6 +117,8 @@ class GeoCoord
     static float bearing(double lat1, double lon1, double lat2, double lon2);
     static float rangeRadiansToMeters(double range_radians);
     static float rangeMetersToRadians(double range_meters);
+    static uint bearingToDegrees(const char *bearing);
+    static const char *degreesToBearing(uint degrees);
 
     // Point to point conversions
     int32_t distanceTo(const GeoCoord &pointB);

src/gps/ubx.h

@@ -319,6 +319,8 @@ const uint8_t GPS::_message_SAVE[] = {
 // As the M10 has no flash, the best we can do to preserve the config is to set it in RAM and BBR.
 // BBR will survive a restart, and power off for a while, but modules with small backup
 // batteries or super caps will not retain the config for a long power off time.
+// for all configurations using sleep / low power modes, V_BCKP needs to be hooked to permanent power for fast aquisition after
+// sleep
 
 // VALSET Commands for M10
 // Please refer to the M10 Protocol Specification:
@@ -327,40 +329,42 @@ const uint8_t GPS::_message_SAVE[] = {
 // and:
 // https://content.u-blox.com/sites/default/files/u-blox-M10-ROM-5.10_ReleaseNotes_UBX-22001426.pdf
 // for interesting insights.
+//
+// Integration manual:
+// https://content.u-blox.com/sites/default/files/documents/SAM-M10Q_IntegrationManual_UBX-22020019.pdf
+// has details on low-power modes
+
 /*
 CFG-PM2 has been replaced by many CFG-PM commands
-OPERATEMODE E1 2 (0 | 1 | 2)
-POSUPDATEPERIOD U4 1000ms for M10 must be >= 5s try 5
-ACQPERIOD U4 10 seems ok for M10 def ok
-GRIDOFFSET U4 0 seems ok for M10 def ok
-ONTIME U2 1 will try 1
-MINACQTIME U1 0 will try 0 def ok
-MAXACQTIME U1 stick with default of 0 def ok
-DONOTENTEROFF L 1 stay at 1
-WAITTIMEFIX  L 1 stay with 1
-UPDATEEPH L 1 changed to 1 for gps rework default is 1
-EXTINTWAKE L 0 no ext ints
-EXTINTBACKUP L 0 no ext ints
-EXTINTINACTIVE L 0 no ext ints
-EXTINTACTIVITY U4 0 no ext ints
-LIMITPEAKCURRENT L 1 stay with 1
-*/
-// CFG-PMS has been removed
+CFG-PMS has been removed
+
+CFG-PM-OPERATEMODE E1 (0 | 1 | 2) -> 1 (PSMOO), because sporadic position updates are required instead of continous tracking <10s
+(PSMCT) CFG-PM-POSUPDATEPERIOD U4 -> 0ms, no self-timed wakup because receiver power mode is controlled via "software standby
+mode" by legacy UBX-RXM-PMREQ request CFG-PM-ACQPERIOD U4 -> 0ms, because receiver power mode is controlled via "software standby
+mode" by legacy UBX-RXM-PMREQ request CFG-PM-ONTIME U4 -> 0ms, optional I guess CFG-PM-EXTINTBACKUP L -> 1, force receiver into
+BACKUP mode when EXTINT (should be connected to GPS_EN_PIN) pin is "low"
+
+This is required because the receiver never enters low power mode if microcontroller is in deep-sleep.
+Maybe the changing UART_RX levels trigger a wakeup but even with UBX-RXM-PMREQ[12] = 0x00 (all external wakeup sources disabled)
+the receivcer remains in aquisition state -> potentially a bug
+
+Workaround: Control the EXTINT pin by the GPS_EN_PIN signal
+
+As mentioned in the M10 operational issues down below, power save won't allow the use of BDS B1C.
+CFG-SIGNAL-BDS_B1C_ENA L -> 0
 
 // Ram layer config message:
-// b5 62 06 8a 26 00 00 01 00 00 01 00 d0 20 02 02 00 d0 40 05 00 00 00 05 00 d0 30 01 00 08 00 d0 10 01 09 00 d0 10 01 10 00 d0
-// 10 01 8b de
+// 01 01 00 00 01 00 D0 20 01 02 00 D0 40 00 00 00 00 03 00 D0 40 00 00 00 00 05 00 D0 30 00 00 0D 00 D0 10 01
 
 // BBR layer config message:
-// b5 62 06 8a 26 00 00 02 00 00 01 00 d0 20 02 02 00 d0 40 05 00 00 00 05 00 d0 30 01 00 08 00 d0 10 01 09 00 d0 10 01 10 00 d0
-// 10 01 8c 03
-
-const uint8_t GPS::_message_VALSET_PM_RAM[] = {0x00, 0x01, 0x00, 0x00, 0x01, 0x00, 0xd0, 0x20, 0x02, 0x02, 0x00, 0xd0, 0x40,
-                                               0x05, 0x00, 0x00, 0x00, 0x05, 0x00, 0xd0, 0x30, 0x01, 0x00, 0x08, 0x00, 0xd0,
-                                               0x10, 0x01, 0x09, 0x00, 0xd0, 0x10, 0x01, 0x10, 0x00, 0xd0, 0x10, 0x01};
-const uint8_t GPS::_message_VALSET_PM_BBR[] = {0x00, 0x02, 0x00, 0x00, 0x01, 0x00, 0xd0, 0x20, 0x02, 0x02, 0x00, 0xd0, 0x40,
-                                               0x05, 0x00, 0x00, 0x00, 0x05, 0x00, 0xd0, 0x30, 0x01, 0x00, 0x08, 0x00, 0xd0,
-                                               0x10, 0x01, 0x09, 0x00, 0xd0, 0x10, 0x01, 0x10, 0x00, 0xd0, 0x10, 0x01};
+// 01 02 00 00 01 00 D0 20 01 02 00 D0 40 00 00 00 00 03 00 D0 40 00 00 00 00 05 00 D0 30 00 00 0D 00 D0 10 01
+*/
+const uint8_t GPS::_message_VALSET_PM_RAM[] = {0x01, 0x01, 0x00, 0x00, 0x0F, 0x00, 0x31, 0x10, 0x00, 0x01, 0x00, 0xD0, 0x20, 0x01,
+                                               0x02, 0x00, 0xD0, 0x40, 0x00, 0x00, 0x00, 0x00, 0x03, 0x00, 0xD0, 0x40, 0x00, 0x00,
+                                               0x00, 0x00, 0x05, 0x00, 0xD0, 0x30, 0x00, 0x00, 0x0D, 0x00, 0xD0, 0x10, 0x01};
+const uint8_t GPS::_message_VALSET_PM_BBR[] = {0x01, 0x02, 0x00, 0x00, 0x0F, 0x00, 0x31, 0x10, 0x00, 0x01, 0x00, 0xD0, 0x20, 0x01,
+                                               0x02, 0x00, 0xD0, 0x40, 0x00, 0x00, 0x00, 0x00, 0x03, 0x00, 0xD0, 0x40, 0x00, 0x00,
+                                               0x00, 0x00, 0x05, 0x00, 0xD0, 0x30, 0x00, 0x00, 0x0D, 0x00, 0xD0, 0x10, 0x01};
 
 /*
 CFG-ITFM replaced by 5 valset messages which can be combined into one for RAM and one for BBR

src/graphics/EInkDisplay2.cpp

@@ -156,7 +156,8 @@ bool EInkDisplay::connect()
         }
     }
 
-#elif defined(HELTEC_WIRELESS_PAPER_V1_0) || defined(HELTEC_WIRELESS_PAPER)
+#elif defined(HELTEC_WIRELESS_PAPER_V1_0) || defined(HELTEC_WIRELESS_PAPER) || defined(HELTEC_VISION_MASTER_E213) ||             \
+    defined(HELTEC_VISION_MASTER_E290)
     {
         // Start HSPI
         hspi = new SPIClass(HSPI);

src/graphics/EInkDisplay2.h

@@ -5,11 +5,6 @@
 #include "GxEPD2_BW.h"
 #include <OLEDDisplay.h>
 
-#if defined(HELTEC_WIRELESS_PAPER_V1_0) || defined(HELTEC_WIRELESS_PAPER)
-// Re-enable SPI after deep sleep: rtc_gpio_hold_dis()
-#include "driver/rtc_io.h"
-#endif
-
 /**
  * An adapter class that allows using the GxEPD2 library as if it was an OLEDDisplay implementation.
  *
@@ -72,7 +67,8 @@ class EInkDisplay : public OLEDDisplay
     GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT> *adafruitDisplay = NULL;
 
     // If display uses HSPI
-#if defined(HELTEC_WIRELESS_PAPER) || defined(HELTEC_WIRELESS_PAPER_V1_0)
+#if defined(HELTEC_WIRELESS_PAPER) || defined(HELTEC_WIRELESS_PAPER_V1_0) || defined(HELTEC_VISION_MASTER_E213) ||               \
+    defined(HELTEC_VISION_MASTER_E290)
     SPIClass *hspi = NULL;
 #endif
 

src/graphics/Screen.cpp

@@ -41,6 +41,7 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #include "mesh/Channels.h"
 #include "mesh/generated/meshtastic/deviceonly.pb.h"
 #include "meshUtils.h"
+#include "modules/AdminModule.h"
 #include "modules/ExternalNotificationModule.h"
 #include "modules/TextMessageModule.h"
 #include "sleep.h"
@@ -75,7 +76,6 @@ namespace graphics
 // A text message frame + debug frame + all the node infos
 FrameCallback *normalFrames;
 static uint32_t targetFramerate = IDLE_FRAMERATE;
-static char btPIN[16] = "888888";
 
 uint32_t logo_timeout = 5000; // 4 seconds for EACH logo
 
@@ -108,15 +108,39 @@ GeoCoord geoCoord;
 static bool heartbeat = false;
 #endif
 
-static uint16_t displayWidth, displayHeight;
-
-#define SCREEN_WIDTH displayWidth
-#define SCREEN_HEIGHT displayHeight
+// Quick access to screen dimensions from static drawing functions
+// DEPRECATED. To-do: move static functions inside Screen class
+#define SCREEN_WIDTH display->getWidth()
+#define SCREEN_HEIGHT display->getHeight()
 
 #include "graphics/ScreenFonts.h"
 
 #define getStringCenteredX(s) ((SCREEN_WIDTH - display->getStringWidth(s)) / 2)
 
+/// Check if the display can render a string (detect special chars; emoji)
+static bool haveGlyphs(const char *str)
+{
+#if defined(OLED_UA) || defined(OLED_RU)
+    // Don't want to make any assumptions about custom language support
+    return true;
+#endif
+
+    // Check each character with the lookup function for the OLED library
+    // We're not really meant to use this directly..
+    bool have = true;
+    for (uint16_t i = 0; i < strlen(str); i++) {
+        uint8_t result = Screen::customFontTableLookup((uint8_t)str[i]);
+        // If font doesn't support a character, it is substituted for ¿
+        if (result == 191 && (uint8_t)str[i] != 191) {
+            have = false;
+            break;
+        }
+    }
+
+    LOG_DEBUG("haveGlyphs=%d\n", have);
+    return have;
+}
+
 /**
  * Draw the icon with extra info printed around the corners
  */
@@ -140,13 +164,15 @@ static void drawIconScreen(const char *upperMsg, OLEDDisplay *display, OLEDDispl
     if (upperMsg)
         display->drawString(x + 0, y + 0, upperMsg);
 
-    // Draw version in upper right
-    char buf[16];
-    snprintf(buf, sizeof(buf), "%s",
-             xstr(APP_VERSION_SHORT)); // Note: we don't bother printing region or now, it makes the string too long
-    display->drawString(x + SCREEN_WIDTH - display->getStringWidth(buf), y + 0, buf);
+    // Draw version and short name in upper right
+    char buf[25];
+    snprintf(buf, sizeof(buf), "%s\n%s", xstr(APP_VERSION_SHORT), haveGlyphs(owner.short_name) ? owner.short_name : "");
+
+    display->setTextAlignment(TEXT_ALIGN_RIGHT);
+    display->drawString(x + SCREEN_WIDTH, y + 0, buf);
     screen->forceDisplay();
-    // FIXME - draw serial # somewhere?
+
+    display->setTextAlignment(TEXT_ALIGN_LEFT); // Restore left align, just to be kind to any other unsuspecting code
 }
 
 static void drawOEMIconScreen(const char *upperMsg, OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y)
@@ -181,14 +207,15 @@ static void drawOEMIconScreen(const char *upperMsg, OLEDDisplay *display, OLEDDi
     if (upperMsg)
         display->drawString(x + 0, y + 0, upperMsg);
 
-    // Draw version in upper right
-    char buf[16];
-    snprintf(buf, sizeof(buf), "%s",
-             xstr(APP_VERSION_SHORT)); // Note: we don't bother printing region or now, it makes the string too long
-    display->drawString(x + SCREEN_WIDTH - display->getStringWidth(buf), y + 0, buf);
+    // Draw version and shortname in upper right
+    char buf[25];
+    snprintf(buf, sizeof(buf), "%s\n%s", xstr(APP_VERSION_SHORT), haveGlyphs(owner.short_name) ? owner.short_name : "");
+
+    display->setTextAlignment(TEXT_ALIGN_RIGHT);
+    display->drawString(x + SCREEN_WIDTH, y + 0, buf);
     screen->forceDisplay();
 
-    // FIXME - draw serial # somewhere?
+    display->setTextAlignment(TEXT_ALIGN_LEFT); // Restore left align, just to be kind to any other unsuspecting code
 }
 
 static void drawOEMBootScreen(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y)
@@ -198,7 +225,7 @@ static void drawOEMBootScreen(OLEDDisplay *display, OLEDDisplayUiState *state, i
     drawOEMIconScreen(region, display, state, x, y);
 }
 
-static void drawFrameText(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y, const char *message)
+void Screen::drawFrameText(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y, const char *message)
 {
     uint16_t x_offset = display->width() / 2;
     display->setTextAlignment(TEXT_ALIGN_CENTER);
@@ -206,20 +233,6 @@ static void drawFrameText(OLEDDisplay *display, OLEDDisplayUiState *state, int16
     display->drawString(x_offset + x, 26 + y, message);
 }
 
-static void drawBootScreen(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y)
-{
-#ifdef ARCH_ESP32
-    if (wakeCause == ESP_SLEEP_WAKEUP_TIMER || wakeCause == ESP_SLEEP_WAKEUP_EXT1) {
-        drawFrameText(display, state, x, y, "Resuming...");
-    } else
-#endif
-    {
-        // Draw region in upper left
-        const char *region = myRegion ? myRegion->name : NULL;
-        drawIconScreen(region, display, state, x, y);
-    }
-}
-
 // Used on boot when a certificate is being created
 static void drawSSLScreen(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y)
 {
@@ -281,12 +294,15 @@ static void drawFunctionOverlay(OLEDDisplay *display, OLEDDisplayUiState *state)
 /// Used on eink displays while in deep sleep
 static void drawDeepSleepScreen(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y)
 {
+
     // Next frame should use full-refresh, and block while running, else device will sleep before async callback
     EINK_ADD_FRAMEFLAG(display, COSMETIC);
     EINK_ADD_FRAMEFLAG(display, BLOCKING);
 
     LOG_DEBUG("Drawing deep sleep screen\n");
-    drawIconScreen("Sleeping...", display, state, x, y);
+
+    // Display displayStr on the screen
+    drawIconScreen("Sleeping", display, state, x, y);
 }
 
 /// Used on eink displays when screen updates are paused
@@ -301,14 +317,15 @@ static void drawScreensaverOverlay(OLEDDisplay *display, OLEDDisplayUiState *sta
     display->setTextAlignment(TEXT_ALIGN_LEFT);
     const char *pauseText = "Screen Paused";
     const char *idText = owner.short_name;
+    const bool useId = haveGlyphs(idText); // This bool is used to hide the idText box if we can't render the short name
     constexpr uint16_t padding = 5;
     constexpr uint8_t dividerGap = 1;
     constexpr uint8_t imprecision = 5; // How far the box origins can drift from center. Combat burn-in.
 
     // Dimensions
-    const uint16_t idTextWidth = display->getStringWidth(idText, strlen(idText));
+    const uint16_t idTextWidth = display->getStringWidth(idText, strlen(idText), true); // "true": handle utf8 chars
     const uint16_t pauseTextWidth = display->getStringWidth(pauseText, strlen(pauseText));
-    const uint16_t boxWidth = padding + idTextWidth + padding + padding + pauseTextWidth + padding;
+    const uint16_t boxWidth = padding + (useId ? idTextWidth + padding + padding : 0) + pauseTextWidth + padding;
     const uint16_t boxHeight = padding + FONT_HEIGHT_SMALL + padding;
 
     // Position
@@ -318,7 +335,7 @@ static void drawScreensaverOverlay(OLEDDisplay *display, OLEDDisplayUiState *sta
     const int16_t boxBottom = boxTop + boxHeight - 1;
     const int16_t idTextLeft = boxLeft + padding;
     const int16_t idTextTop = boxTop + padding;
-    const int16_t pauseTextLeft = boxLeft + padding + idTextWidth + padding + padding;
+    const int16_t pauseTextLeft = boxLeft + (useId ? padding + idTextWidth + padding : 0) + padding;
     const int16_t pauseTextTop = boxTop + padding;
     const int16_t dividerX = boxLeft + padding + idTextWidth + padding;
     const int16_t dividerTop = boxTop + 1 + dividerGap;
@@ -331,12 +348,14 @@ static void drawScreensaverOverlay(OLEDDisplay *display, OLEDDisplayUiState *sta
     display->drawRect(boxLeft, boxTop, boxWidth, boxHeight);
 
     // Draw: Text
-    display->drawString(idTextLeft, idTextTop, idText);
+    if (useId)
+        display->drawString(idTextLeft, idTextTop, idText);
     display->drawString(pauseTextLeft, pauseTextTop, pauseText);
     display->drawString(pauseTextLeft + 1, pauseTextTop, pauseText); // Faux bold
 
     // Draw: divider
-    display->drawLine(dividerX, dividerTop, dividerX, dividerBottom);
+    if (useId)
+        display->drawLine(dividerX, dividerTop, dividerX, dividerBottom);
 }
 #endif
 
@@ -348,7 +367,7 @@ static void drawModuleFrame(OLEDDisplay *display, OLEDDisplayUiState *state, int
     // in the array of "drawScreen" functions; however,
     // the passed-state doesn't quite reflect the "current"
     // screen, so we have to detect it.
-    if (state->frameState == IN_TRANSITION && state->transitionFrameRelationship == INCOMING) {
+    if (state->frameState == IN_TRANSITION && state->transitionFrameRelationship == TransitionRelationship_INCOMING) {
         // if we're transitioning from the end of the frame list back around to the first
         // frame, then we want this to be `0`
         module_frame = state->transitionFrameTarget;
@@ -362,31 +381,6 @@ static void drawModuleFrame(OLEDDisplay *display, OLEDDisplayUiState *state, int
     pi.drawFrame(display, state, x, y);
 }
 
-static void drawFrameBluetooth(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y)
-{
-    int x_offset = display->width() / 2;
-    int y_offset = display->height() <= 80 ? 0 : 32;
-    display->setTextAlignment(TEXT_ALIGN_CENTER);
-    display->setFont(FONT_MEDIUM);
-    display->drawString(x_offset + x, y_offset + y, "Bluetooth");
-
-    display->setFont(FONT_SMALL);
-    y_offset = display->height() == 64 ? y_offset + FONT_HEIGHT_MEDIUM - 4 : y_offset + FONT_HEIGHT_MEDIUM + 5;
-    display->drawString(x_offset + x, y_offset + y, "Enter this code");
-
-    display->setFont(FONT_LARGE);
-    String displayPin(btPIN);
-    String pin = displayPin.substring(0, 3) + " " + displayPin.substring(3, 6);
-    y_offset = display->height() == 64 ? y_offset + FONT_HEIGHT_SMALL - 5 : y_offset + FONT_HEIGHT_SMALL + 5;
-    display->drawString(x_offset + x, y_offset + y, pin);
-
-    display->setFont(FONT_SMALL);
-    String deviceName = "Name: ";
-    deviceName.concat(getDeviceName());
-    y_offset = display->height() == 64 ? y_offset + FONT_HEIGHT_LARGE - 6 : y_offset + FONT_HEIGHT_LARGE + 5;
-    display->drawString(x_offset + x, y_offset + y, deviceName);
-}
-
 static void drawFrameFirmware(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y)
 {
     display->setTextAlignment(TEXT_ALIGN_CENTER);
@@ -1064,45 +1058,8 @@ static void drawTextMessageFrame(OLEDDisplay *display, OLEDDisplayUiState *state
 #endif
 }
 
-/// Draw the last waypoint we received
-static void drawWaypointFrame(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y)
-{
-    static char tempBuf[237];
-
-    meshtastic_MeshPacket &mp = devicestate.rx_waypoint;
-    meshtastic_NodeInfoLite *node = nodeDB->getMeshNode(getFrom(&mp));
-
-    display->setTextAlignment(TEXT_ALIGN_LEFT);
-    display->setFont(FONT_SMALL);
-    if (config.display.displaymode == meshtastic_Config_DisplayConfig_DisplayMode_INVERTED) {
-        display->fillRect(0 + x, 0 + y, x + display->getWidth(), y + FONT_HEIGHT_SMALL);
-        display->setColor(BLACK);
-    }
-
-    uint32_t seconds = sinceReceived(&mp);
-    uint32_t minutes = seconds / 60;
-    uint32_t hours = minutes / 60;
-    uint32_t days = hours / 24;
-
-    if (config.display.heading_bold) {
-        display->drawStringf(1 + x, 0 + y, tempBuf, "%s ago from %s",
-                             screen->drawTimeDelta(days, hours, minutes, seconds).c_str(),
-                             (node && node->has_user) ? node->user.short_name : "???");
-    }
-    display->drawStringf(0 + x, 0 + y, tempBuf, "%s ago from %s", screen->drawTimeDelta(days, hours, minutes, seconds).c_str(),
-                         (node && node->has_user) ? node->user.short_name : "???");
-
-    display->setColor(WHITE);
-    meshtastic_Waypoint scratch;
-    memset(&scratch, 0, sizeof(scratch));
-    if (pb_decode_from_bytes(mp.decoded.payload.bytes, mp.decoded.payload.size, &meshtastic_Waypoint_msg, &scratch)) {
-        snprintf(tempBuf, sizeof(tempBuf), "Received waypoint: %s", scratch.name);
-        display->drawStringMaxWidth(0 + x, 0 + y + FONT_HEIGHT_SMALL, x + display->getWidth(), tempBuf);
-    }
-}
-
 /// Draw a series of fields in a column, wrapping to multiple columns if needed
-static void drawColumns(OLEDDisplay *display, int16_t x, int16_t y, const char **fields)
+void Screen::drawColumns(OLEDDisplay *display, int16_t x, int16_t y, const char **fields)
 {
     // The coordinates define the left starting point of the text
     display->setTextAlignment(TEXT_ALIGN_LEFT);
@@ -1282,56 +1239,13 @@ static void drawGPScoordinates(OLEDDisplay *display, int16_t x, int16_t y, const
     }
 }
 #endif
-namespace
-{
-
-/// A basic 2D point class for drawing
-class Point
-{
-  public:
-    float x, y;
-
-    Point(float _x, float _y) : x(_x), y(_y) {}
-
-    /// Apply a rotation around zero (standard rotation matrix math)
-    void rotate(float radian)
-    {
-        float cos = cosf(radian), sin = sinf(radian);
-        float rx = x * cos + y * sin, ry = -x * sin + y * cos;
-
-        x = rx;
-        y = ry;
-    }
-
-    void translate(int16_t dx, int dy)
-    {
-        x += dx;
-        y += dy;
-    }
-
-    void scale(float f)
-    {
-        // We use -f here to counter the flip that happens
-        // on the y axis when drawing and rotating on screen
-        x *= f;
-        y *= -f;
-    }
-};
-
-} // namespace
-
-static void drawLine(OLEDDisplay *d, const Point &p1, const Point &p2)
-{
-    d->drawLine(p1.x, p1.y, p2.x, p2.y);
-}
-
 /**
  * Given a recent lat/lon return a guess of the heading the user is walking on.
  *
  * We keep a series of "after you've gone 10 meters, what is your heading since
  * the last reference point?"
  */
-static float estimatedHeading(double lat, double lon)
+float Screen::estimatedHeading(double lat, double lon)
 {
     static double oldLat, oldLon;
     static float b;
@@ -1355,38 +1269,13 @@ static float estimatedHeading(double lat, double lon)
     return b;
 }
 
-static uint16_t getCompassDiam(OLEDDisplay *display)
-{
-    uint16_t diam = 0;
-    uint16_t offset = 0;
-
-    if (config.display.displaymode != meshtastic_Config_DisplayConfig_DisplayMode_DEFAULT)
-        offset = FONT_HEIGHT_SMALL;
-
-    // get the smaller of the 2 dimensions and subtract 20
-    if (display->getWidth() > (display->getHeight() - offset)) {
-        diam = display->getHeight() - offset;
-        // if 2/3 of the other size would be smaller, use that
-        if (diam > (display->getWidth() * 2 / 3)) {
-            diam = display->getWidth() * 2 / 3;
-        }
-    } else {
-        diam = display->getWidth();
-        if (diam > ((display->getHeight() - offset) * 2 / 3)) {
-            diam = (display->getHeight() - offset) * 2 / 3;
-        }
-    }
-
-    return diam - 20;
-};
-
 /// We will skip one node - the one for us, so we just blindly loop over all
 /// nodes
 static size_t nodeIndex;
 static int8_t prevFrame = -1;
 
 // Draw the arrow pointing to a node's location
-static void drawNodeHeading(OLEDDisplay *display, int16_t compassX, int16_t compassY, float headingRadian)
+void Screen::drawNodeHeading(OLEDDisplay *display, int16_t compassX, int16_t compassY, uint16_t compassDiam, float headingRadian)
 {
     Point tip(0.0f, 0.5f), tail(0.0f, -0.5f); // pointing up initially
     float arrowOffsetX = 0.2f, arrowOffsetY = 0.2f;
@@ -1396,16 +1285,45 @@ static void drawNodeHeading(OLEDDisplay *display, int16_t compassX, int16_t comp
 
     for (int i = 0; i < 4; i++) {
         arrowPoints[i]->rotate(headingRadian);
-        arrowPoints[i]->scale(getCompassDiam(display) * 0.6);
+        arrowPoints[i]->scale(compassDiam * 0.6);
         arrowPoints[i]->translate(compassX, compassY);
     }
-    drawLine(display, tip, tail);
-    drawLine(display, leftArrow, tip);
-    drawLine(display, rightArrow, tip);
+    display->drawLine(tip.x, tip.y, tail.x, tail.y);
+    display->drawLine(leftArrow.x, leftArrow.y, tip.x, tip.y);
+    display->drawLine(rightArrow.x, rightArrow.y, tip.x, tip.y);
 }
 
-// Draw north
-static void drawCompassNorth(OLEDDisplay *display, int16_t compassX, int16_t compassY, float myHeading)
+// Get a string representation of the time passed since something happened
+void Screen::getTimeAgoStr(uint32_t agoSecs, char *timeStr, uint8_t maxLength)
+{
+    // Use an absolute timestamp in some cases.
+    // Particularly useful with E-Ink displays. Static UI, fewer refreshes.
+    uint8_t timestampHours, timestampMinutes;
+    int32_t daysAgo;
+    bool useTimestamp = deltaToTimestamp(agoSecs, &timestampHours, &timestampMinutes, &daysAgo);
+
+    if (agoSecs < 120) // last 2 mins?
+        snprintf(timeStr, maxLength, "%u seconds ago", agoSecs);
+    // -- if suitable for timestamp --
+    else if (useTimestamp && agoSecs < 15 * SECONDS_IN_MINUTE) // Last 15 minutes
+        snprintf(timeStr, maxLength, "%u minutes ago", agoSecs / SECONDS_IN_MINUTE);
+    else if (useTimestamp && daysAgo == 0) // Today
+        snprintf(timeStr, maxLength, "Last seen: %02u:%02u", (unsigned int)timestampHours, (unsigned int)timestampMinutes);
+    else if (useTimestamp && daysAgo == 1) // Yesterday
+        snprintf(timeStr, maxLength, "Seen yesterday");
+    else if (useTimestamp && daysAgo > 1) // Last six months (capped by deltaToTimestamp method)
+        snprintf(timeStr, maxLength, "%li days ago", (long)daysAgo);
+    // -- if using time delta instead --
+    else if (agoSecs < 120 * 60) // last 2 hrs
+        snprintf(timeStr, maxLength, "%u minutes ago", agoSecs / 60);
+    // Only show hours ago if it's been less than 6 months. Otherwise, we may have bad data.
+    else if ((agoSecs / 60 / 60) < (hours_in_month * 6))
+        snprintf(timeStr, maxLength, "%u hours ago", agoSecs / 60 / 60);
+    else
+        snprintf(timeStr, maxLength, "unknown age");
+}
+
+void Screen::drawCompassNorth(OLEDDisplay *display, int16_t compassX, int16_t compassY, float myHeading)
 {
     // If north is supposed to be at the top of the compass we want rotation to be +0
     if (config.display.compass_north_top)
@@ -1415,19 +1333,43 @@ static void drawCompassNorth(OLEDDisplay *display, int16_t compassX, int16_t com
     Point N3(-0.04f, 0.55f), N4(0.04f, 0.55f);
     Point *rosePoints[] = {&N1, &N2, &N3, &N4};
 
+    uint16_t compassDiam = Screen::getCompassDiam(SCREEN_WIDTH, SCREEN_HEIGHT);
+
     for (int i = 0; i < 4; i++) {
         // North on compass will be negative of heading
         rosePoints[i]->rotate(-myHeading);
-        rosePoints[i]->scale(getCompassDiam(display));
+        rosePoints[i]->scale(compassDiam);
         rosePoints[i]->translate(compassX, compassY);
     }
-    drawLine(display, N1, N3);
-    drawLine(display, N2, N4);
-    drawLine(display, N1, N4);
+    display->drawLine(N1.x, N1.y, N3.x, N3.y);
+    display->drawLine(N2.x, N2.y, N4.x, N4.y);
+    display->drawLine(N1.x, N1.y, N4.x, N4.y);
 }
 
-/// Convert an integer GPS coords to a floating point
-#define DegD(i) (i * 1e-7)
+uint16_t Screen::getCompassDiam(uint32_t displayWidth, uint32_t displayHeight)
+{
+    uint16_t diam = 0;
+    uint16_t offset = 0;
+
+    if (config.display.displaymode != meshtastic_Config_DisplayConfig_DisplayMode_DEFAULT)
+        offset = FONT_HEIGHT_SMALL;
+
+    // get the smaller of the 2 dimensions and subtract 20
+    if (displayWidth > (displayHeight - offset)) {
+        diam = displayHeight - offset;
+        // if 2/3 of the other size would be smaller, use that
+        if (diam > (displayWidth * 2 / 3)) {
+            diam = displayWidth * 2 / 3;
+        }
+    } else {
+        diam = displayWidth;
+        if (diam > ((displayHeight - offset) * 2 / 3)) {
+            diam = (displayHeight - offset) * 2 / 3;
+        }
+    }
+
+    return diam - 20;
+};
 
 static void drawNodeInfo(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y)
 {
@@ -1467,34 +1409,8 @@ static void drawNodeInfo(OLEDDisplay *display, OLEDDisplayUiState *state, int16_
         snprintf(signalStr, sizeof(signalStr), "Signal: %d%%", clamp((int)((node->snr + 10) * 5), 0, 100));
     }
 
-    uint32_t agoSecs = sinceLastSeen(node);
     static char lastStr[20];
-
-    // Use an absolute timestamp in some cases.
-    // Particularly useful with E-Ink displays. Static UI, fewer refreshes.
-    uint8_t timestampHours, timestampMinutes;
-    int32_t daysAgo;
-    bool useTimestamp = deltaToTimestamp(agoSecs, &timestampHours, &timestampMinutes, &daysAgo);
-
-    if (agoSecs < 120) // last 2 mins?
-        snprintf(lastStr, sizeof(lastStr), "%u seconds ago", agoSecs);
-    // -- if suitable for timestamp --
-    else if (useTimestamp && agoSecs < 15 * SECONDS_IN_MINUTE) // Last 15 minutes
-        snprintf(lastStr, sizeof(lastStr), "%u minutes ago", agoSecs / SECONDS_IN_MINUTE);
-    else if (useTimestamp && daysAgo == 0) // Today
-        snprintf(lastStr, sizeof(lastStr), "Last seen: %02u:%02u", (unsigned int)timestampHours, (unsigned int)timestampMinutes);
-    else if (useTimestamp && daysAgo == 1) // Yesterday
-        snprintf(lastStr, sizeof(lastStr), "Seen yesterday");
-    else if (useTimestamp && daysAgo > 1) // Last six months (capped by deltaToTimestamp method)
-        snprintf(lastStr, sizeof(lastStr), "%li days ago", (long)daysAgo);
-    // -- if using time delta instead --
-    else if (agoSecs < 120 * 60) // last 2 hrs
-        snprintf(lastStr, sizeof(lastStr), "%u minutes ago", agoSecs / 60);
-    // Only show hours ago if it's been less than 6 months. Otherwise, we may have bad data.
-    else if ((agoSecs / 60 / 60) < (hours_in_month * 6))
-        snprintf(lastStr, sizeof(lastStr), "%u hours ago", agoSecs / 60 / 60);
-    else
-        snprintf(lastStr, sizeof(lastStr), "unknown age");
+    screen->getTimeAgoStr(sinceLastSeen(node), lastStr, sizeof(lastStr));
 
     static char distStr[20];
     if (config.display.units == meshtastic_Config_DisplayConfig_DisplayUnits_IMPERIAL) {
@@ -1505,21 +1421,26 @@ static void drawNodeInfo(OLEDDisplay *display, OLEDDisplayUiState *state, int16_
     meshtastic_NodeInfoLite *ourNode = nodeDB->getMeshNode(nodeDB->getNodeNum());
     const char *fields[] = {username, lastStr, signalStr, distStr, NULL};
     int16_t compassX = 0, compassY = 0;
+    uint16_t compassDiam = Screen::getCompassDiam(SCREEN_WIDTH, SCREEN_HEIGHT);
 
     // coordinates for the center of the compass/circle
     if (config.display.displaymode == meshtastic_Config_DisplayConfig_DisplayMode_DEFAULT) {
-        compassX = x + SCREEN_WIDTH - getCompassDiam(display) / 2 - 5;
+        compassX = x + SCREEN_WIDTH - compassDiam / 2 - 5;
         compassY = y + SCREEN_HEIGHT / 2;
     } else {
-        compassX = x + SCREEN_WIDTH - getCompassDiam(display) / 2 - 5;
+        compassX = x + SCREEN_WIDTH - compassDiam / 2 - 5;
         compassY = y + FONT_HEIGHT_SMALL + (SCREEN_HEIGHT - FONT_HEIGHT_SMALL) / 2;
     }
     bool hasNodeHeading = false;
 
-    if (ourNode && hasValidPosition(ourNode)) {
+    if (ourNode && (hasValidPosition(ourNode) || screen->hasHeading())) {
         const meshtastic_PositionLite &op = ourNode->position;
-        float myHeading = estimatedHeading(DegD(op.latitude_i), DegD(op.longitude_i));
-        drawCompassNorth(display, compassX, compassY, myHeading);
+        float myHeading;
+        if (screen->hasHeading())
+            myHeading = (screen->getHeading()) * PI / 180; // gotta convert compass degrees to Radians
+        else
+            myHeading = screen->estimatedHeading(DegD(op.latitude_i), DegD(op.longitude_i));
+        screen->drawCompassNorth(display, compassX, compassY, myHeading);
 
         if (hasValidPosition(node)) {
             // display direction toward node
@@ -1546,7 +1467,7 @@ static void drawNodeInfo(OLEDDisplay *display, OLEDDisplayUiState *state, int16_
             // If the top of the compass is not a static north we need adjust bearingToOther based on heading
             if (!config.display.compass_north_top)
                 bearingToOther -= myHeading;
-            drawNodeHeading(display, compassX, compassY, bearingToOther);
+            screen->drawNodeHeading(display, compassX, compassY, compassDiam, bearingToOther);
         }
     }
     if (!hasNodeHeading) {
@@ -1556,15 +1477,19 @@ static void drawNodeInfo(OLEDDisplay *display, OLEDDisplayUiState *state, int16_
         // hasValidPosition(node));
         display->drawString(compassX - FONT_HEIGHT_SMALL / 4, compassY - FONT_HEIGHT_SMALL / 2, "?");
     }
-    display->drawCircle(compassX, compassY, getCompassDiam(display) / 2);
+    display->drawCircle(compassX, compassY, compassDiam / 2);
 
     if (config.display.displaymode == meshtastic_Config_DisplayConfig_DisplayMode_INVERTED) {
         display->setColor(BLACK);
     }
     // Must be after distStr is populated
-    drawColumns(display, x, y, fields);
+    screen->drawColumns(display, x, y, fields);
 }
 
+#if defined(ESP_PLATFORM) && defined(USE_ST7789)
+SPIClass SPI1(HSPI);
+#endif
+
 Screen::Screen(ScanI2C::DeviceAddress address, meshtastic_Config_DisplayConfig_OledType screenType, OLEDDISPLAY_GEOMETRY geometry)
     : concurrency::OSThread("Screen"), address_found(address), model(screenType), geometry(geometry), cmdQueue(32)
 {
@@ -1572,6 +1497,13 @@ Screen::Screen(ScanI2C::DeviceAddress address, meshtastic_Config_DisplayConfig_O
 #if defined(USE_SH1106) || defined(USE_SH1107) || defined(USE_SH1107_128_64)
     dispdev = new SH1106Wire(address.address, -1, -1, geometry,
                              (address.port == ScanI2C::I2CPort::WIRE1) ? HW_I2C::I2C_TWO : HW_I2C::I2C_ONE);
+#elif defined(USE_ST7789)
+#ifdef ESP_PLATFORM
+    dispdev = new ST7789Spi(&SPI1, ST7789_RESET, ST7789_RS, ST7789_NSS, GEOMETRY_RAWMODE, TFT_WIDTH, TFT_HEIGHT, ST7789_SDA,
+                            ST7789_MISO, ST7789_SCK);
+#else
+    dispdev = new ST7789Spi(&SPI1, ST7789_RESET, ST7789_RS, ST7789_NSS, GEOMETRY_RAWMODE, TFT_WIDTH, TFT_HEIGHT);
+#endif
 #elif defined(USE_SSD1306)
     dispdev = new SSD1306Wire(address.address, -1, -1, geometry,
                               (address.port == ScanI2C::I2CPort::WIRE1) ? HW_I2C::I2C_TWO : HW_I2C::I2C_ONE);
@@ -1650,7 +1582,14 @@ void Screen::handleSetOn(bool on, FrameCallback einkScreensaver)
 #endif
 
             dispdev->displayOn();
-
+#ifdef USE_ST7789
+#ifdef ESP_PLATFORM
+            analogWrite(VTFT_LEDA, BRIGHTNESS_DEFAULT);
+#else
+            pinMode(VTFT_LEDA, OUTPUT);
+            digitalWrite(VTFT_LEDA, TFT_BACKLIGHT_ON);
+#endif
+#endif
             enabled = true;
             setInterval(0); // Draw ASAP
             runASAP = true;
@@ -1661,6 +1600,12 @@ void Screen::handleSetOn(bool on, FrameCallback einkScreensaver)
 #endif
             LOG_INFO("Turning off screen\n");
             dispdev->displayOff();
+
+#ifdef USE_ST7789
+            pinMode(VTFT_LEDA, OUTPUT);
+            digitalWrite(VTFT_LEDA, !TFT_BACKLIGHT_ON);
+#endif
+
 #ifdef T_WATCH_S3
             PMU->disablePowerOutput(XPOWERS_ALDO2);
 #endif
@@ -1710,9 +1655,19 @@ void Screen::setup()
 
     // Add frames.
     EINK_ADD_FRAMEFLAG(dispdev, DEMAND_FAST);
-    static FrameCallback bootFrames[] = {drawBootScreen};
-    static const int bootFrameCount = sizeof(bootFrames) / sizeof(bootFrames[0]);
-    ui->setFrames(bootFrames, bootFrameCount);
+    alertFrames[0] = [this](OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y) -> void {
+#ifdef ARCH_ESP32
+        if (wakeCause == ESP_SLEEP_WAKEUP_TIMER || wakeCause == ESP_SLEEP_WAKEUP_EXT1) {
+            drawFrameText(display, state, x, y, "Resuming...");
+        } else
+#endif
+        {
+            // Draw region in upper left
+            const char *region = myRegion ? myRegion->name : NULL;
+            drawIconScreen(region, display, state, x, y);
+        }
+    };
+    ui->setFrames(alertFrames, 1);
     // No overlays.
     ui->setOverlays(nullptr, 0);
 
@@ -1771,6 +1726,7 @@ void Screen::setup()
     powerStatusObserver.observe(&powerStatus->onNewStatus);
     gpsStatusObserver.observe(&gpsStatus->onNewStatus);
     nodeStatusObserver.observe(&nodeStatus->onNewStatus);
+    adminMessageObserver.observe(adminModule);
     if (textMessageModule)
         textMessageObserver.observe(textMessageModule);
     if (inputBroker)
@@ -1885,13 +1841,22 @@ int32_t Screen::runOnce()
         case Cmd::SHOW_NEXT_FRAME:
             handleShowNextFrame();
             break;
-        case Cmd::START_BLUETOOTH_PIN_SCREEN:
-            handleStartBluetoothPinScreen(cmd.bluetooth_pin);
+        case Cmd::START_ALERT_FRAME: {
+            showingBootScreen = false; // this should avoid the edge case where an alert triggers before the boot screen goes away
+            showingNormalScreen = false;
+            alertFrames[0] = alertFrame;
+#ifdef USE_EINK
+            EINK_ADD_FRAMEFLAG(dispdev, DEMAND_FAST); // Use fast-refresh for next frame, no skip please
+            EINK_ADD_FRAMEFLAG(dispdev, BLOCKING);    // Edge case: if this frame is promoted to COSMETIC, wait for update
+            handleSetOn(true); // Ensure power-on to receive deep-sleep screensaver (PowerFSM should handle?)
+#endif
+            setFrameImmediateDraw(alertFrames);
             break;
+        }
         case Cmd::START_FIRMWARE_UPDATE_SCREEN:
             handleStartFirmwareUpdateScreen();
             break;
-        case Cmd::STOP_BLUETOOTH_PIN_SCREEN:
+        case Cmd::STOP_ALERT_FRAME:
         case Cmd::STOP_BOOT_SCREEN:
             EINK_ADD_FRAMEFLAG(dispdev, COSMETIC); // E-Ink: Explicitly use full-refresh for next frame
             setFrames();
@@ -1900,12 +1865,6 @@ int32_t Screen::runOnce()
             handlePrint(cmd.print_text);
             free(cmd.print_text);
             break;
-        case Cmd::START_SHUTDOWN_SCREEN:
-            handleShutdownScreen();
-            break;
-        case Cmd::START_REBOOT_SCREEN:
-            handleRebootScreen();
-            break;
         default:
             LOG_ERROR("Invalid screen cmd\n");
         }
@@ -1996,9 +1955,6 @@ void Screen::setWelcomeFrames()
 /// Determine which screensaver frame to use, then set the FrameCallback
 void Screen::setScreensaverFrames(FrameCallback einkScreensaver)
 {
-    // Remember current frame, restore position at power-on
-    uint8_t frameNumber = ui->getUiState()->currentFrame;
-
     // Retain specified frame / overlay callback beyond scope of this method
     static FrameCallback screensaverFrame;
     static OverlayCallback screensaverOverlay;
@@ -2036,9 +1992,8 @@ void Screen::setScreensaverFrames(FrameCallback einkScreensaver)
 #endif
 
     // Prepare now for next frame, shown when display wakes
-    ui->setOverlays(NULL, 0);       // Clear overlay
-    setFrames();                    // Return to normal display updates
-    ui->switchToFrame(frameNumber); // Attempt to return to same frame after power-on
+    ui->setOverlays(NULL, 0);  // Clear overlay
+    setFrames(FOCUS_PRESERVE); // Return to normal display updates, showing same frame as before screensaver, ideally
 
     // Pick a refresh method, for when display wakes
 #ifdef EINK_HASQUIRK_GHOSTING
@@ -2049,9 +2004,13 @@ void Screen::setScreensaverFrames(FrameCallback einkScreensaver)
 }
 #endif
 
-// restore our regular frame list
-void Screen::setFrames()
+// Regenerate the normal set of frames, focusing a specific frame if requested
+// Called when a frame should be added / removed, or custom frames should be cleared
+void Screen::setFrames(FrameFocus focus)
 {
+    uint8_t originalPosition = ui->getUiState()->currentFrame;
+    FramesetInfo fsi; // Location of specific frames, for applying focus parameter
+
     LOG_DEBUG("showing standard frames\n");
     showingNormalScreen = true;
 
@@ -2071,7 +2030,7 @@ void Screen::setFrames()
     LOG_DEBUG("Total frame count: %d\n", totalFrameCount);
 #endif
 
-    // We don't show the node info our our node (if we have it yet - we should)
+    // We don't show the node info of our node (if we have it yet - we should)
     size_t numMeshNodes = nodeDB->getNumMeshNodes();
     if (numMeshNodes > 0)
         numMeshNodes--;
@@ -2085,27 +2044,36 @@ void Screen::setFrames()
     // is the same offset into the moduleFrames vector
     // so that we can invoke the module's callback
     for (auto i = moduleFrames.begin(); i != moduleFrames.end(); ++i) {
-        normalFrames[numframes++] = drawModuleFrame;
+        // Draw the module frame, using the hack described above
+        normalFrames[numframes] = drawModuleFrame;
+
+        // Check if the module being drawn has requested focus
+        // We will honor this request later, if setFrames was triggered by a UIFrameEvent
+        MeshModule *m = *i;
+        if (m->isRequestingFocus())
+            fsi.positions.focusedModule = numframes;
+
+        numframes++;
     }
 
     LOG_DEBUG("Added modules.  numframes: %d\n", numframes);
 
     // If we have a critical fault, show it first
-    if (error_code)
+    fsi.positions.fault = numframes;
+    if (error_code) {
         normalFrames[numframes++] = drawCriticalFaultFrame;
+        focus = FOCUS_FAULT; // Change our "focus" parameter, to ensure we show the fault frame
+    }
 
 #ifdef T_WATCH_S3
     normalFrames[numframes++] = screen->digitalWatchFace ? &Screen::drawDigitalClockFrame : &Screen::drawAnalogClockFrame;
 #endif
 
     // If we have a text message - show it next, unless it's a phone message and we aren't using any special modules
+    fsi.positions.textMessage = numframes;
     if (devicestate.has_rx_text_message && shouldDrawMessage(&devicestate.rx_text_message)) {
         normalFrames[numframes++] = drawTextMessageFrame;
     }
-    // If we have a waypoint - show it next, unless it's a phone message and we aren't using any special modules
-    if (devicestate.has_rx_waypoint && shouldDrawMessage(&devicestate.rx_waypoint)) {
-        normalFrames[numframes++] = drawWaypointFrame;
-    }
 
     // then all the nodes
     // We only show a few nodes in our scrolling list - because meshes with many nodes would have too many screens
@@ -2117,11 +2085,14 @@ void Screen::setFrames()
     //
     // Since frames are basic function pointers, we have to use a helper to
     // call a method on debugInfo object.
+    fsi.positions.log = numframes;
     normalFrames[numframes++] = &Screen::drawDebugInfoTrampoline;
 
     // call a method on debugInfoScreen object (for more details)
+    fsi.positions.settings = numframes;
     normalFrames[numframes++] = &Screen::drawDebugInfoSettingsTrampoline;
 
+    fsi.positions.wifi = numframes;
 #if HAS_WIFI && !defined(ARCH_PORTDUINO)
     if (isWifiAvailable()) {
         // call a method on debugInfoScreen object (for more details)
@@ -2129,6 +2100,7 @@ void Screen::setFrames()
     }
 #endif
 
+    fsi.frameCount = numframes; // Total framecount is used to apply FOCUS_PRESERVE
     LOG_DEBUG("Finished building frames. numframes: %d\n", numframes);
 
     ui->setFrames(normalFrames, numframes);
@@ -2142,20 +2114,58 @@ void Screen::setFrames()
     prevFrame = -1; // Force drawNodeInfo to pick a new node (because our list
                     // just changed)
 
+    // Focus on a specific frame, in the frame set we just created
+    switch (focus) {
+    case FOCUS_DEFAULT:
+        ui->switchToFrame(0); // First frame
+        break;
+    case FOCUS_FAULT:
+        ui->switchToFrame(fsi.positions.fault);
+        break;
+    case FOCUS_TEXTMESSAGE:
+        ui->switchToFrame(fsi.positions.textMessage);
+        break;
+    case FOCUS_MODULE:
+        // Whichever frame was marked by MeshModule::requestFocus(), if any
+        // If no module requested focus, will show the first frame instead
+        ui->switchToFrame(fsi.positions.focusedModule);
+        break;
+
+    case FOCUS_PRESERVE:
+        // If we can identify which type of frame "originalPosition" was, can move directly to it in the new frameset
+        FramesetInfo &oldFsi = this->framesetInfo;
+        if (originalPosition == oldFsi.positions.log)
+            ui->switchToFrame(fsi.positions.log);
+        else if (originalPosition == oldFsi.positions.settings)
+            ui->switchToFrame(fsi.positions.settings);
+        else if (originalPosition == oldFsi.positions.wifi)
+            ui->switchToFrame(fsi.positions.wifi);
+
+        // If frame count has decreased
+        else if (fsi.frameCount < oldFsi.frameCount) {
+            uint8_t numDropped = oldFsi.frameCount - fsi.frameCount;
+            // Move n frames backwards
+            if (numDropped <= originalPosition)
+                ui->switchToFrame(originalPosition - numDropped);
+            // Unless that would put us "out of bounds" (< 0)
+            else
+                ui->switchToFrame(0);
+        }
+
+        // If we're not sure exactly which frame we were on, at least return to the same frame number
+        // (node frames; module frames)
+        else
+            ui->switchToFrame(originalPosition);
+
+        break;
+    }
+
+    // Store the info about this frameset, for future setFrames calls
+    this->framesetInfo = fsi;
+
     setFastFramerate(); // Draw ASAP
 }
 
-void Screen::handleStartBluetoothPinScreen(uint32_t pin)
-{
-    LOG_DEBUG("showing bluetooth screen\n");
-    showingNormalScreen = false;
-    EINK_ADD_FRAMEFLAG(dispdev, DEMAND_FAST); // E-Ink: Explicitly use fast-refresh for next frame
-
-    static FrameCallback frames[] = {drawFrameBluetooth};
-    snprintf(btPIN, sizeof(btPIN), "%06u", pin);
-    setFrameImmediateDraw(frames);
-}
-
 void Screen::setFrameImmediateDraw(FrameCallback *drawFrames)
 {
     ui->disableAllIndicators();
@@ -2163,41 +2173,6 @@ void Screen::setFrameImmediateDraw(FrameCallback *drawFrames)
     setFastFramerate();
 }
 
-void Screen::handleShutdownScreen()
-{
-    LOG_DEBUG("showing shutdown screen\n");
-    showingNormalScreen = false;
-#ifdef USE_EINK
-    EINK_ADD_FRAMEFLAG(dispdev, DEMAND_FAST); // Use fast-refresh for next frame, no skip please
-    EINK_ADD_FRAMEFLAG(dispdev, BLOCKING);    // Edge case: if this frame is promoted to COSMETIC, wait for update
-    handleSetOn(true);                        // Ensure power-on to receive deep-sleep screensaver (PowerFSM should handle?)
-#endif
-
-    auto frame = [](OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y) -> void {
-        drawFrameText(display, state, x, y, "Shutting down...");
-    };
-    static FrameCallback frames[] = {frame};
-
-    setFrameImmediateDraw(frames);
-}
-
-void Screen::handleRebootScreen()
-{
-    LOG_DEBUG("showing reboot screen\n");
-    showingNormalScreen = false;
-#ifdef USE_EINK
-    EINK_ADD_FRAMEFLAG(dispdev, DEMAND_FAST); // Use fast-refresh for next frame, no skip please
-    EINK_ADD_FRAMEFLAG(dispdev, BLOCKING);    // Edge case: if this frame is promoted to COSMETIC, wait for update
-    handleSetOn(true);                        // Power-on to show rebooting screen (PowerFSM should handle?)
-#endif
-
-    auto frame = [](OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y) -> void {
-        drawFrameText(display, state, x, y, "Rebooting...");
-    };
-    static FrameCallback frames[] = {frame};
-    setFrameImmediateDraw(frames);
-}
-
 void Screen::handleStartFirmwareUpdateScreen()
 {
     LOG_DEBUG("showing firmware screen\n");
@@ -2214,7 +2189,7 @@ void Screen::blink()
     uint8_t count = 10;
     dispdev->setBrightness(254);
     while (count > 0) {
-        dispdev->fillRect(0, 0, SCREEN_WIDTH, SCREEN_HEIGHT);
+        dispdev->fillRect(0, 0, dispdev->getWidth(), dispdev->getHeight());
         dispdev->display();
         delay(50);
         dispdev->clear();
@@ -2642,7 +2617,7 @@ int Screen::handleStatusUpdate(const meshtastic::Status *arg)
     switch (arg->getStatusType()) {
     case STATUS_TYPE_NODE:
         if (showingNormalScreen && nodeStatus->getLastNumTotal() != nodeStatus->getNumTotal()) {
-            setFrames(); // Regen the list of screens
+            setFrames(FOCUS_PRESERVE); // Regen the list of screen frames (returning to same frame, if possible)
         }
         nodeDB->updateGUI = false;
         break;
@@ -2654,23 +2629,33 @@ int Screen::handleStatusUpdate(const meshtastic::Status *arg)
 int Screen::handleTextMessage(const meshtastic_MeshPacket *packet)
 {
     if (showingNormalScreen) {
-        setFrames(); // Regen the list of screens (will show new text message)
+        // Outgoing message
+        if (packet->from == 0)
+            setFrames(FOCUS_PRESERVE); // Return to same frame (quietly hiding the rx text message frame)
+
+        // Incoming message
+        else
+            setFrames(FOCUS_TEXTMESSAGE); // Focus on the new message
     }
 
     return 0;
 }
 
+// Triggered by MeshModules
 int Screen::handleUIFrameEvent(const UIFrameEvent *event)
 {
     if (showingNormalScreen) {
-        if (event->frameChanged) {
-            setFrames(); // Regen the list of screens (will show new text message)
-        } else if (event->needRedraw) {
+        // Regenerate the frameset, potentially honoring a module's internal requestFocus() call
+        if (event->action == UIFrameEvent::Action::REGENERATE_FRAMESET)
+            setFrames(FOCUS_MODULE);
+
+        // Regenerate the frameset, while attempting to maintain focus on the current frame
+        else if (event->action == UIFrameEvent::Action::REGENERATE_FRAMESET_BACKGROUND)
+            setFrames(FOCUS_PRESERVE);
+
+        // Don't regenerate the frameset, just re-draw whatever is on screen ASAP
+        else if (event->action == UIFrameEvent::Action::REDRAW_ONLY)
             setFastFramerate();
-            // TODO: We might also want switch to corresponding frame,
-            //       but we don't know the exact frame number.
-            // ui->switchToFrame(0);
-        }
     }
 
     return 0;
@@ -2705,7 +2690,25 @@ int Screen::handleInputEvent(const InputEvent *event)
     return 0;
 }
 
+int Screen::handleAdminMessage(const meshtastic_AdminMessage *arg)
+{
+    // Note: only selected admin messages notify this observer
+    // If you wish to handle a new type of message, you should modify AdminModule.cpp first
+
+    switch (arg->which_payload_variant) {
+    // Node removed manually (i.e. via app)
+    case meshtastic_AdminMessage_remove_by_nodenum_tag:
+        setFrames(FOCUS_PRESERVE);
+        break;
+
+    // Default no-op, in case the admin message observable gets used by other classes in future
+    default:
+        break;
+    }
+    return 0;
+}
+
 } // namespace graphics
 #else
 graphics::Screen::Screen(ScanI2C::DeviceAddress, meshtastic_Config_DisplayConfig_OledType, OLEDDISPLAY_GEOMETRY) {}
-#endif // HAS_SCREEN
+#endif // HAS_SCREEN

src/graphics/Screen.h

@@ -21,11 +21,13 @@ class Screen
     void print(const char *) {}
     void doDeepSleep() {}
     void forceDisplay(bool forceUiUpdate = false) {}
-    void startBluetoothPinScreen(uint32_t pin) {}
-    void stopBluetoothPinScreen() {}
-    void startRebootScreen() {}
-    void startShutdownScreen() {}
     void startFirmwareUpdateScreen() {}
+    void increaseBrightness() {}
+    void decreaseBrightness() {}
+    void setFunctionSymbal(std::string) {}
+    void removeFunctionSymbal(std::string) {}
+    void startAlert(const char *) {}
+    void endAlert() {}
 };
 } // namespace graphics
 #else
@@ -34,6 +36,8 @@ class Screen
 #include <OLEDDisplayUi.h>
 
 #include "../configuration.h"
+#include "gps/GeoCoord.h"
+#include "graphics/ScreenFonts.h"
 
 #ifdef USE_ST7567
 #include <ST7567Wire.h>
@@ -41,6 +45,8 @@ class Screen
 #include <SH1106Wire.h>
 #elif defined(USE_SSD1306)
 #include <SSD1306Wire.h>
+#elif defined(USE_ST7789)
+#include <ST7789Spi.h>
 #else
 // the SH1106/SSD1306 variant is auto-detected
 #include <AutoOLEDWire.h>
@@ -82,6 +88,46 @@ class Screen
 #define SEGMENT_WIDTH 16
 #define SEGMENT_HEIGHT 4
 
+/// Convert an integer GPS coords to a floating point
+#define DegD(i) (i * 1e-7)
+
+namespace
+{
+/// A basic 2D point class for drawing
+class Point
+{
+  public:
+    float x, y;
+
+    Point(float _x, float _y) : x(_x), y(_y) {}
+
+    /// Apply a rotation around zero (standard rotation matrix math)
+    void rotate(float radian)
+    {
+        float cos = cosf(radian), sin = sinf(radian);
+        float rx = x * cos + y * sin, ry = -x * sin + y * cos;
+
+        x = rx;
+        y = ry;
+    }
+
+    void translate(int16_t dx, int dy)
+    {
+        x += dx;
+        y += dy;
+    }
+
+    void scale(float f)
+    {
+        // We use -f here to counter the flip that happens
+        // on the y axis when drawing and rotating on screen
+        x *= f;
+        y *= -f;
+    }
+};
+
+} // namespace
+
 namespace graphics
 {
 
@@ -127,9 +173,11 @@ class Screen : public concurrency::OSThread
     CallbackObserver<Screen, const meshtastic_MeshPacket *> textMessageObserver =
         CallbackObserver<Screen, const meshtastic_MeshPacket *>(this, &Screen::handleTextMessage);
     CallbackObserver<Screen, const UIFrameEvent *> uiFrameEventObserver =
-        CallbackObserver<Screen, const UIFrameEvent *>(this, &Screen::handleUIFrameEvent);
+        CallbackObserver<Screen, const UIFrameEvent *>(this, &Screen::handleUIFrameEvent); // Sent by Mesh Modules
     CallbackObserver<Screen, const InputEvent *> inputObserver =
         CallbackObserver<Screen, const InputEvent *>(this, &Screen::handleInputEvent);
+    CallbackObserver<Screen, const meshtastic_AdminMessage *> adminMessageObserver =
+        CallbackObserver<Screen, const meshtastic_AdminMessage *>(this, &Screen::handleAdminMessage);
 
   public:
     explicit Screen(ScanI2C::DeviceAddress, meshtastic_Config_DisplayConfig_OledType, OLEDDISPLAY_GEOMETRY);
@@ -166,20 +214,49 @@ class Screen : public concurrency::OSThread
 
     void blink();
 
+    void drawFrameText(OLEDDisplay *, OLEDDisplayUiState *, int16_t, int16_t, const char *);
+
+    void getTimeAgoStr(uint32_t agoSecs, char *timeStr, uint8_t maxLength);
+
+    // Draw north
+    void drawCompassNorth(OLEDDisplay *display, int16_t compassX, int16_t compassY, float myHeading);
+
+    static uint16_t getCompassDiam(uint32_t displayWidth, uint32_t displayHeight);
+
+    float estimatedHeading(double lat, double lon);
+
+    void drawNodeHeading(OLEDDisplay *display, int16_t compassX, int16_t compassY, uint16_t compassDiam, float headingRadian);
+
+    void drawColumns(OLEDDisplay *display, int16_t x, int16_t y, const char **fields);
+
     /// Handle button press, trackball or swipe action)
     void onPress() { enqueueCmd(ScreenCmd{.cmd = Cmd::ON_PRESS}); }
     void showPrevFrame() { enqueueCmd(ScreenCmd{.cmd = Cmd::SHOW_PREV_FRAME}); }
     void showNextFrame() { enqueueCmd(ScreenCmd{.cmd = Cmd::SHOW_NEXT_FRAME}); }
 
-    /// Starts showing the Bluetooth PIN screen.
-    //
-    // Switches over to a static frame showing the Bluetooth pairing screen
-    // with the PIN.
-    void startBluetoothPinScreen(uint32_t pin)
+    // generic alert start
+    void startAlert(FrameCallback _alertFrame)
+    {
+        alertFrame = _alertFrame;
+        ScreenCmd cmd;
+        cmd.cmd = Cmd::START_ALERT_FRAME;
+        enqueueCmd(cmd);
+    }
+
+    void startAlert(const char *_alertMessage)
+    {
+        startAlert([_alertMessage](OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y) -> void {
+            uint16_t x_offset = display->width() / 2;
+            display->setTextAlignment(TEXT_ALIGN_CENTER);
+            display->setFont(FONT_MEDIUM);
+            display->drawString(x_offset + x, 26 + y, _alertMessage);
+        });
+    }
+
+    void endAlert()
     {
         ScreenCmd cmd;
-        cmd.cmd = Cmd::START_BLUETOOTH_PIN_SCREEN;
-        cmd.bluetooth_pin = pin;
+        cmd.cmd = Cmd::STOP_ALERT_FRAME;
         enqueueCmd(cmd);
     }
 
@@ -190,20 +267,17 @@ class Screen : public concurrency::OSThread
         enqueueCmd(cmd);
     }
 
-    void startShutdownScreen()
+    // Function to allow the AccelerometerThread to set the heading if a sensor provides it
+    // Mutex needed?
+    void setHeading(long _heading)
     {
-        ScreenCmd cmd;
-        cmd.cmd = Cmd::START_SHUTDOWN_SCREEN;
-        enqueueCmd(cmd);
+        hasCompass = true;
+        compassHeading = _heading;
     }
 
-    void startRebootScreen()
-    {
-        ScreenCmd cmd;
-        cmd.cmd = Cmd::START_REBOOT_SCREEN;
-        enqueueCmd(cmd);
-    }
+    bool hasHeading() { return hasCompass; }
 
+    long getHeading() { return compassHeading; }
     // functions for display brightness
     void increaseBrightness();
     void decreaseBrightness();
@@ -211,9 +285,6 @@ class Screen : public concurrency::OSThread
     void setFunctionSymbal(std::string sym);
     void removeFunctionSymbal(std::string sym);
 
-    /// Stops showing the bluetooth PIN screen.
-    void stopBluetoothPinScreen() { enqueueCmd(ScreenCmd{.cmd = Cmd::STOP_BLUETOOTH_PIN_SCREEN}); }
-
     /// Stops showing the boot screen.
     void stopBootScreen() { enqueueCmd(ScreenCmd{.cmd = Cmd::STOP_BOOT_SCREEN}); }
 
@@ -325,6 +396,7 @@ class Screen : public concurrency::OSThread
     int handleTextMessage(const meshtastic_MeshPacket *arg);
     int handleUIFrameEvent(const UIFrameEvent *arg);
     int handleInputEvent(const InputEvent *arg);
+    int handleAdminMessage(const meshtastic_AdminMessage *arg);
 
     /// Used to force (super slow) eink displays to draw critical frames
     void forceDisplay(bool forceUiUpdate = false);
@@ -347,7 +419,13 @@ class Screen : public concurrency::OSThread
 
     bool isAUTOOled = false;
 
+    // Screen dimensions (for convenience)
+    // Defined during Screen::setup
+    uint16_t displayWidth = 0;
+    uint16_t displayHeight = 0;
+
   private:
+    FrameCallback alertFrames[1];
     struct ScreenCmd {
         Cmd cmd;
         union {
@@ -373,13 +451,36 @@ class Screen : public concurrency::OSThread
     void handleOnPress();
     void handleShowNextFrame();
     void handleShowPrevFrame();
-    void handleStartBluetoothPinScreen(uint32_t pin);
     void handlePrint(const char *text);
     void handleStartFirmwareUpdateScreen();
-    void handleShutdownScreen();
-    void handleRebootScreen();
-    /// Rebuilds our list of frames (screens) to default ones.
-    void setFrames();
+
+    // Info collected by setFrames method.
+    // Index location of specific frames. Used to apply the FrameFocus parameter of setFrames
+    struct FramesetInfo {
+        struct FramePositions {
+            uint8_t fault = 0;
+            uint8_t textMessage = 0;
+            uint8_t focusedModule = 0;
+            uint8_t log = 0;
+            uint8_t settings = 0;
+            uint8_t wifi = 0;
+        } positions;
+
+        uint8_t frameCount = 0;
+    } framesetInfo;
+
+    // Which frame we want to be displayed, after we regen the frameset by calling setFrames
+    enum FrameFocus : uint8_t {
+        FOCUS_DEFAULT,  // No specific frame
+        FOCUS_PRESERVE, // Return to the previous frame
+        FOCUS_FAULT,
+        FOCUS_TEXTMESSAGE,
+        FOCUS_MODULE, // Note: target module should call requestFocus(), otherwise no info about which module to focus
+    };
+
+    // Regenerate the normal set of frames, focusing a specific frame if requested
+    // Call when a frame should be added / removed, or custom frames should be cleared
+    void setFrames(FrameFocus focus = FOCUS_DEFAULT);
 
     /// Try to start drawing ASAP
     void setFastFramerate();
@@ -415,6 +516,9 @@ class Screen : public concurrency::OSThread
     bool digitalWatchFace = true;
 #endif
 
+    /// callback for current alert frame
+    FrameCallback alertFrame;
+
     /// Queue of commands to execute in doTask.
     TypedQueue<ScreenCmd> cmdQueue;
     /// Whether we are using a display
@@ -428,6 +532,8 @@ class Screen : public concurrency::OSThread
     // Implementation to Adjust Brightness
     uint8_t brightness = BRIGHTNESS_DEFAULT; // H = 254, MH = 192, ML = 130 L = 103
 
+    bool hasCompass = false;
+    float compassHeading;
     /// Holds state for debug information
     DebugInfo debugInfo;
 
@@ -439,4 +545,5 @@ class Screen : public concurrency::OSThread
 };
 
 } // namespace graphics
+
 #endif

src/graphics/ScreenFonts.h

@@ -28,8 +28,8 @@
 #define FONT_LARGE ArialMT_Plain_24  // Height: 28
 #endif
 
-#define fontHeight(font) ((font)[1] + 1) // height is position 1
+#define _fontHeight(font) ((font)[1] + 1) // height is position 1
 
-#define FONT_HEIGHT_SMALL fontHeight(FONT_SMALL)
-#define FONT_HEIGHT_MEDIUM fontHeight(FONT_MEDIUM)
-#define FONT_HEIGHT_LARGE fontHeight(FONT_LARGE)
+#define FONT_HEIGHT_SMALL _fontHeight(FONT_SMALL)
+#define FONT_HEIGHT_MEDIUM _fontHeight(FONT_MEDIUM)
+#define FONT_HEIGHT_LARGE _fontHeight(FONT_LARGE)

src/graphics/TFTDisplay.cpp

@@ -117,8 +117,16 @@ class LGFX : public lgfx::LGFX_Device
 static LGFX *tft = nullptr;
 
 #elif defined(RAK14014)
+#include <RAK14014_FT6336U.h>
 #include <TFT_eSPI.h>
 TFT_eSPI *tft = nullptr;
+FT6336U ft6336u;
+
+static uint8_t _rak14014_touch_int = false; // TP interrupt generation flag.
+static void rak14014_tpIntHandle(void)
+{
+    _rak14014_touch_int = true;
+}
 
 #elif defined(ST7789_CS)
 #include <LovyanGFX.hpp> // Graphics and font library for ST7735 driver chip
@@ -642,8 +650,12 @@ void TFTDisplay::sendCommand(uint8_t com)
 
 void TFTDisplay::setDisplayBrightness(uint8_t _brightness)
 {
+#ifdef RAK14014
+    // todo
+#else
     tft->setBrightness(_brightness);
     LOG_DEBUG("Brightness is set to value: %i \n", _brightness);
+#endif
 }
 
 void TFTDisplay::flipScreenVertically()
@@ -657,6 +669,7 @@ void TFTDisplay::flipScreenVertically()
 bool TFTDisplay::hasTouch(void)
 {
 #ifdef RAK14014
+    return true;
 #elif !defined(M5STACK)
     return tft->touch() != nullptr;
 #else
@@ -667,6 +680,15 @@ bool TFTDisplay::hasTouch(void)
 bool TFTDisplay::getTouch(int16_t *x, int16_t *y)
 {
 #ifdef RAK14014
+    if (_rak14014_touch_int) {
+        _rak14014_touch_int = false;
+        /* The X and Y axes have to be switched */
+        *y = ft6336u.read_touch1_x();
+        *x = TFT_HEIGHT - ft6336u.read_touch1_y();
+        return true;
+    } else {
+        return false;
+    }
 #elif !defined(M5STACK)
     return tft->getTouch(x, y);
 #else
@@ -716,7 +738,10 @@ bool TFTDisplay::connect()
 #elif defined(RAK14014)
     tft->setRotation(1);
     tft->setSwapBytes(true);
-//    tft->fillScreen(TFT_BLACK);
+    //    tft->fillScreen(TFT_BLACK);
+    ft6336u.begin();
+    pinMode(SCREEN_TOUCH_INT, INPUT_PULLUP);
+    attachInterrupt(digitalPinToInterrupt(SCREEN_TOUCH_INT), rak14014_tpIntHandle, FALLING);
 #elif defined(T_DECK) || defined(PICOMPUTER_S3) || defined(CHATTER_2)
     tft->setRotation(1); // T-Deck has the TFT in landscape
 #elif defined(T_WATCH_S3)

src/input/InputBroker.cpp

@@ -1,7 +1,7 @@
 #include "InputBroker.h"
 #include "PowerFSM.h" // needed for event trigger
 
-InputBroker *inputBroker;
+InputBroker *inputBroker = nullptr;
 
 InputBroker::InputBroker(){};
 

src/input/cardKbI2cImpl.cpp

@@ -1,5 +1,7 @@
 #include "cardKbI2cImpl.h"
 #include "InputBroker.h"
+#include "detect/ScanI2CTwoWire.h"
+#include "main.h"
 
 CardKbI2cImpl *cardKbI2cImpl;
 
@@ -7,10 +9,52 @@ CardKbI2cImpl::CardKbI2cImpl() : KbI2cBase("cardKB") {}
 
 void CardKbI2cImpl::init()
 {
+#ifndef ARCH_PORTDUINO
+    if (cardkb_found.address == 0x00) {
+        LOG_DEBUG("Rescanning for I2C keyboard\n");
+        uint8_t i2caddr_scan[] = {CARDKB_ADDR, TDECK_KB_ADDR, BBQ10_KB_ADDR};
+        uint8_t i2caddr_asize = 3;
+        auto i2cScanner = std::unique_ptr<ScanI2CTwoWire>(new ScanI2CTwoWire());
+
+#if defined(I2C_SDA1)
+        i2cScanner->scanPort(ScanI2C::I2CPort::WIRE1, i2caddr_scan, i2caddr_asize);
+#endif
+        i2cScanner->scanPort(ScanI2C::I2CPort::WIRE, i2caddr_scan, i2caddr_asize);
+        auto kb_info = i2cScanner->firstKeyboard();
+
+        if (kb_info.type != ScanI2C::DeviceType::NONE) {
+            cardkb_found = kb_info.address;
+            switch (kb_info.type) {
+            case ScanI2C::DeviceType::RAK14004:
+                kb_model = 0x02;
+                break;
+            case ScanI2C::DeviceType::CARDKB:
+                kb_model = 0x00;
+                break;
+            case ScanI2C::DeviceType::TDECKKB:
+                // assign an arbitrary value to distinguish from other models
+                kb_model = 0x10;
+                break;
+            case ScanI2C::DeviceType::BBQ10KB:
+                // assign an arbitrary value to distinguish from other models
+                kb_model = 0x11;
+                break;
+            default:
+                // use this as default since it's also just zero
+                LOG_WARN("kb_info.type is unknown(0x%02x), setting kb_model=0x00\n", kb_info.type);
+                kb_model = 0x00;
+            }
+        }
+        if (cardkb_found.address == 0x00) {
+            disable();
+            return;
+        }
+    }
+#else
     if (cardkb_found.address == 0x00) {
         disable();
         return;
     }
-
+#endif
     inputBroker->registerSource(this);
 }

src/input/cardKbI2cImpl.h

@@ -1,6 +1,5 @@
 #pragma once
 #include "kbI2cBase.h"
-#include "main.h"
 
 /**
  * @brief The idea behind this class to have static methods for the event handlers.

src/input/kbI2cBase.cpp

@@ -1,6 +1,7 @@
 #include "kbI2cBase.h"
 #include "configuration.h"
 #include "detect/ScanI2C.h"
+#include "detect/ScanI2CTwoWire.h"
 
 extern ScanI2C::DeviceAddress cardkb_found;
 extern uint8_t kb_model;
@@ -29,11 +30,6 @@ uint8_t read_from_14004(TwoWire *i2cBus, uint8_t reg, uint8_t *data, uint8_t len
 
 int32_t KbI2cBase::runOnce()
 {
-    if (cardkb_found.address == 0x00) {
-        // Input device is not detected.
-        return INT32_MAX;
-    }
-
     if (!i2cBus) {
         switch (cardkb_found.port) {
         case ScanI2C::WIRE1:

src/main.cpp

@@ -6,6 +6,7 @@
 #include "MeshService.h"
 #include "NodeDB.h"
 #include "PowerFSM.h"
+#include "PowerMon.h"
 #include "ReliableRouter.h"
 #include "airtime.h"
 #include "buzz.h"
@@ -41,13 +42,13 @@
 #endif
 #if !MESHTASTIC_EXCLUDE_BLUETOOTH
 #include "nimble/NimbleBluetooth.h"
-NimbleBluetooth *nimbleBluetooth;
+NimbleBluetooth *nimbleBluetooth = nullptr;
 #endif
 #endif
 
 #ifdef ARCH_NRF52
 #include "NRF52Bluetooth.h"
-NRF52Bluetooth *nrf52Bluetooth;
+NRF52Bluetooth *nrf52Bluetooth = nullptr;
 #endif
 
 #if HAS_WIFI
@@ -65,6 +66,8 @@ NRF52Bluetooth *nrf52Bluetooth;
 #endif
 
 #include "LLCC68Interface.h"
+#include "LR1110Interface.h"
+#include "LR1120Interface.h"
 #include "RF95Interface.h"
 #include "SX1262Interface.h"
 #include "SX1268Interface.h"
@@ -92,23 +95,23 @@ NRF52Bluetooth *nrf52Bluetooth;
 #include "ButtonThread.h"
 #endif
 
+#include "AmbientLightingThread.h"
 #include "PowerFSMThread.h"
 
 #if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_ENVIRONMENTAL_SENSOR
 #include "AccelerometerThread.h"
-#include "AmbientLightingThread.h"
-AccelerometerThread *accelerometerThread;
+AccelerometerThread *accelerometerThread = nullptr;
 #endif
 
 #ifdef HAS_I2S
 #include "AudioThread.h"
-AudioThread *audioThread;
+AudioThread *audioThread = nullptr;
 #endif
 
 using namespace concurrency;
 
 // We always create a screen object, but we only init it if we find the hardware
-graphics::Screen *screen;
+graphics::Screen *screen = nullptr;
 
 // Global power status
 meshtastic::PowerStatus *powerStatus = new meshtastic::PowerStatus();
@@ -152,6 +155,7 @@ bool isVibrating = false;
 bool eink_found = true;
 
 uint32_t serialSinceMsec;
+bool pauseBluetoothLogging = false;
 
 bool pmu_found;
 
@@ -170,7 +174,7 @@ const char *getDeviceName()
     static char name[20];
     snprintf(name, sizeof(name), "%02x%02x", dmac[4], dmac[5]);
     // if the shortname exists and is NOT the new default of ab3c, use it for BLE name.
-    if ((owner.short_name != NULL) && (strcmp(owner.short_name, name) != 0)) {
+    if (strcmp(owner.short_name, name) != 0) {
         snprintf(name, sizeof(name), "%s_%02x%02x", owner.short_name, dmac[4], dmac[5]);
     } else {
         snprintf(name, sizeof(name), "Meshtastic_%02x%02x", dmac[4], dmac[5]);
@@ -211,6 +215,14 @@ __attribute__((weak, noinline)) bool loopCanSleep()
     return true;
 }
 
+/**
+ * Print info as a structured log message (for automated log processing)
+ */
+void printInfo()
+{
+    LOG_INFO("S:B:%d,%s\n", HW_VENDOR, optstr(APP_VERSION));
+}
+
 void setup()
 {
     concurrency::hasBeenSetup = true;
@@ -218,7 +230,7 @@ void setup()
         meshtastic_Config_DisplayConfig_OledType::meshtastic_Config_DisplayConfig_OledType_OLED_AUTO;
     OLEDDISPLAY_GEOMETRY screen_geometry = GEOMETRY_128_64;
 
-#ifdef SEGGER_STDOUT_CH
+#ifdef USE_SEGGER
     auto mode = false ? SEGGER_RTT_MODE_BLOCK_IF_FIFO_FULL : SEGGER_RTT_MODE_NO_BLOCK_TRIM;
 #ifdef NRF52840_XXAA
     auto buflen = 4096; // this board has a fair amount of ram
@@ -231,6 +243,7 @@ void setup()
 #ifdef DEBUG_PORT
     consoleInit(); // Set serial baud rate and init our mesh console
 #endif
+    powerMonInit();
 
     serialSinceMsec = millis();
 
@@ -239,7 +252,7 @@ void setup()
     initDeepSleep();
 
     // power on peripherals
-#if defined(TTGO_T_ECHO) && defined(PIN_POWER_EN)
+#if defined(PIN_POWER_EN)
     pinMode(PIN_POWER_EN, OUTPUT);
     digitalWrite(PIN_POWER_EN, HIGH);
     // digitalWrite(PIN_POWER_EN1, INPUT);
@@ -262,6 +275,9 @@ void setup()
     digitalWrite(VEXT_ENABLE_V05, 1); // turn on the lora antenna boost
     digitalWrite(ST7735_BL_V05, 1);   // turn on display backligth
     LOG_DEBUG("HELTEC Detect Tracker V1.1\n");
+#elif defined(VEXT_ENABLE) && defined(VEXT_ON_VALUE)
+    pinMode(VEXT_ENABLE, OUTPUT);
+    digitalWrite(VEXT_ENABLE, VEXT_ON_VALUE); // turn on the display power
 #elif defined(VEXT_ENABLE)
     pinMode(VEXT_ENABLE, OUTPUT);
     digitalWrite(VEXT_ENABLE, 0); // turn on the display power
@@ -292,6 +308,13 @@ void setup()
     digitalWrite(RESET_OLED, 1);
 #endif
 
+#ifdef PERIPHERAL_WARMUP_MS
+    // Some peripherals may require additional time to stabilize after power is connected
+    // e.g. I2C on Heltec Vision Master
+    LOG_INFO("Waiting for peripherals to stabilize\n");
+    delay(PERIPHERAL_WARMUP_MS);
+#endif
+
 #ifdef BUTTON_PIN
 #ifdef ARCH_ESP32
 
@@ -419,10 +442,6 @@ void setup()
     auto i2cCount = i2cScanner->countDevices();
     if (i2cCount == 0) {
         LOG_INFO("No I2C devices found\n");
-        Wire.end();
-#ifdef I2C_SDA1
-        Wire1.end();
-#endif
     } else {
         LOG_INFO("%i I2C devices found\n", i2cCount);
     }
@@ -533,8 +552,12 @@ void setup()
     SCANNER_TO_SENSORS_MAP(ScanI2C::DeviceType::PMSA0031, meshtastic_TelemetrySensorType_PMSA003I)
     SCANNER_TO_SENSORS_MAP(ScanI2C::DeviceType::RCWL9620, meshtastic_TelemetrySensorType_RCWL9620)
     SCANNER_TO_SENSORS_MAP(ScanI2C::DeviceType::VEML7700, meshtastic_TelemetrySensorType_VEML7700)
+    SCANNER_TO_SENSORS_MAP(ScanI2C::DeviceType::TSL2591, meshtastic_TelemetrySensorType_TSL25911FN)
+    SCANNER_TO_SENSORS_MAP(ScanI2C::DeviceType::OPT3001, meshtastic_TelemetrySensorType_OPT3001)
+    SCANNER_TO_SENSORS_MAP(ScanI2C::DeviceType::MLX90632, meshtastic_TelemetrySensorType_MLX90632)
     SCANNER_TO_SENSORS_MAP(ScanI2C::DeviceType::SHT4X, meshtastic_TelemetrySensorType_SHT4X)
     SCANNER_TO_SENSORS_MAP(ScanI2C::DeviceType::AHT10, meshtastic_TelemetrySensorType_AHT10)
+    SCANNER_TO_SENSORS_MAP(ScanI2C::DeviceType::DFROBOT_LARK, meshtastic_TelemetrySensorType_DFROBOT_LARK)
 
     i2cScanner.reset();
 
@@ -550,7 +573,7 @@ void setup()
 #endif
 
     // Hello
-    LOG_INFO("Meshtastic hwvendor=%d, swver=%s\n", HW_VENDOR, optstr(APP_VERSION));
+    printInfo();
 
 #ifdef ARCH_ESP32
     esp32Setup();
@@ -700,7 +723,8 @@ void setup()
 
 // Don't call screen setup until after nodedb is setup (because we need
 // the current region name)
-#if defined(ST7735_CS) || defined(USE_EINK) || defined(ILI9341_DRIVER) || defined(ST7789_CS) || defined(HX8357_CS)
+#if defined(ST7735_CS) || defined(USE_EINK) || defined(ILI9341_DRIVER) || defined(ST7789_CS) || defined(HX8357_CS) ||            \
+    defined(USE_ST7789)
     screen->setup();
 #elif defined(ARCH_PORTDUINO)
     if (screen_found.port != ScanI2C::I2CPort::NO_I2C || settingsMap[displayPanel]) {
@@ -879,6 +903,32 @@ void setup()
     }
 #endif
 
+#if defined(USE_LR1110)
+    if (!rIf) {
+        rIf = new LR1110Interface(RadioLibHAL, LR1110_SPI_NSS_PIN, LR1110_IRQ_PIN, LR1110_NRESER_PIN, LR1110_BUSY_PIN);
+        if (!rIf->init()) {
+            LOG_WARN("Failed to find LR1110 radio\n");
+            delete rIf;
+            rIf = NULL;
+        } else {
+            LOG_INFO("LR1110 Radio init succeeded, using LR1110 radio\n");
+        }
+    }
+#endif
+
+#if defined(USE_LR1120)
+    if (!rIf) {
+        rIf = new LR1120Interface(RadioLibHAL, LR1120_SPI_NSS_PIN, LR1120_IRQ_PIN, LR1120_NRESER_PIN, LR1120_BUSY_PIN);
+        if (!rIf->init()) {
+            LOG_WARN("Failed to find LR1120 radio\n");
+            delete rIf;
+            rIf = NULL;
+        } else {
+            LOG_INFO("LR1120 Radio init succeeded, using LR1120 radio\n");
+        }
+    }
+#endif
+
 #if defined(USE_SX1280)
     if (!rIf) {
         rIf = new SX1280Interface(RadioLibHAL, SX128X_CS, SX128X_DIO1, SX128X_RESET, SX128X_BUSY);
@@ -901,7 +951,7 @@ void setup()
         nodeDB->saveToDisk(SEGMENT_CONFIG);
         if (!rIf->reconfigure()) {
             LOG_WARN("Reconfigure failed, rebooting\n");
-            screen->startRebootScreen();
+            screen->startAlert("Rebooting...");
             rebootAtMsec = millis() + 5000;
         }
     }

src/main.h

@@ -53,6 +53,9 @@ extern Adafruit_DRV2605 drv;
 extern AudioThread *audioThread;
 #endif
 
+// Global Screen singleton.
+extern graphics::Screen *screen;
+
 #if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_ENVIRONMENTAL_SENSOR
 #include "AccelerometerThread.h"
 extern AccelerometerThread *accelerometerThread;
@@ -62,9 +65,6 @@ extern bool isVibrating;
 
 extern int TCPPort; // set by Portduino
 
-// Global Screen singleton.
-extern graphics::Screen *screen;
-
 // extern Observable<meshtastic::PowerStatus> newPowerStatus; //TODO: move this to main-esp32.cpp somehow or a helper class
 
 // extern meshtastic::PowerStatus *powerStatus;
@@ -85,6 +85,8 @@ extern uint32_t serialSinceMsec;
 // This will suppress the current delay and instead try to run ASAP.
 extern bool runASAP;
 
+extern bool pauseBluetoothLogging;
+
 void nrf52Setup(), esp32Setup(), nrf52Loop(), esp32Loop(), rp2040Setup(), clearBonds(), enterDfuMode();
 
 meshtastic_DeviceMetadata getDeviceMetadata();

src/mesh/Channels.cpp

@@ -277,6 +277,17 @@ const char *Channels::getName(size_t chIndex)
     return channelName;
 }
 
+bool Channels::isDefaultChannel(const meshtastic_Channel &ch)
+{
+    if (ch.settings.psk.size == 1 && ch.settings.psk.bytes[0] == 1) {
+        const char *presetName = DisplayFormatters::getModemPresetDisplayName(config.lora.modem_preset, false);
+        // Check if the name is the default derived from the modem preset
+        if (strcmp(ch.settings.name, presetName) == 0)
+            return true;
+    }
+    return false;
+}
+
 bool Channels::hasDefaultChannel()
 {
     // If we don't use a preset or the default frequency slot, or we override the frequency, we don't have a default channel
@@ -285,13 +296,8 @@ bool Channels::hasDefaultChannel()
     // Check if any of the channels are using the default name and PSK
     for (size_t i = 0; i < getNumChannels(); i++) {
         const auto &ch = getByIndex(i);
-        if (ch.settings.psk.size == 1 && ch.settings.psk.bytes[0] == 1) {
-            const char *name = getName(i);
-            const char *presetName = DisplayFormatters::getModemPresetDisplayName(config.lora.modem_preset, false);
-            // Check if the name is the default derived from the modem preset
-            if (strcmp(name, presetName) == 0)
-                return true;
-        }
+        if (isDefaultChannel(ch))
+            return true;
     }
     return false;
 }
@@ -324,4 +330,4 @@ bool Channels::decryptForHash(ChannelIndex chIndex, ChannelHash channelHash)
 int16_t Channels::setActiveByIndex(ChannelIndex channelIndex)
 {
     return setCrypto(channelIndex);
-}
+}

src/mesh/Channels.h

@@ -83,6 +83,9 @@ class Channels
      */
     int16_t setActiveByIndex(ChannelIndex channelIndex);
 
+    // Returns true if the channel has the default name and PSK
+    bool isDefaultChannel(const meshtastic_Channel &ch);
+
     // Returns true if we can be reached via a channel with the default settings given a region and modem preset
     bool hasDefaultChannel();
 
@@ -126,4 +129,4 @@ class Channels
 };
 
 /// Singleton channel table
-extern Channels channels;
+extern Channels channels;

src/mesh/Default.cpp

@@ -1,12 +1,5 @@
 #include "Default.h"
 
-uint32_t Default::getConfiguredOrDefaultMs(uint32_t configuredInterval)
-{
-    if (configuredInterval > 0)
-        return configuredInterval * 1000;
-    return default_broadcast_interval_secs * 1000;
-}
-
 uint32_t Default::getConfiguredOrDefaultMs(uint32_t configuredInterval, uint32_t defaultInterval)
 {
     if (configuredInterval > 0)
@@ -14,10 +7,37 @@ uint32_t Default::getConfiguredOrDefaultMs(uint32_t configuredInterval, uint32_t
     return defaultInterval * 1000;
 }
 
+uint32_t Default::getConfiguredOrDefaultMs(uint32_t configuredInterval)
+{
+    if (configuredInterval > 0)
+        return configuredInterval * 1000;
+    return default_broadcast_interval_secs * 1000;
+}
+
 uint32_t Default::getConfiguredOrDefault(uint32_t configured, uint32_t defaultValue)
 {
     if (configured > 0)
         return configured;
-
     return defaultValue;
+}
+/**
+ * Calculates the scaled value of the configured or default value in ms based on the number of online nodes.
+ *
+ * For example a default of 30 minutes (1800 seconds * 1000) would yield:
+ *   45 nodes = 2475 * 1000
+ *   60 nodes = 4500 * 1000
+ *   75 nodes = 6525 * 1000
+ *   90 nodes = 8550 * 1000
+ * @param configured The configured value.
+ * @param defaultValue The default value.
+ * @param numOnlineNodes The number of online nodes.
+ * @return The scaled value of the configured or default value.
+ */
+uint32_t Default::getConfiguredOrDefaultMsScaled(uint32_t configured, uint32_t defaultValue, uint32_t numOnlineNodes)
+{
+    // If we are a router, we don't scale the value. It's already significantly higher.
+    if (config.device.role == meshtastic_Config_DeviceConfig_Role_ROUTER)
+        return getConfiguredOrDefaultMs(configured, defaultValue);
+
+    return getConfiguredOrDefaultMs(configured, defaultValue) * congestionScalingCoefficient(numOnlineNodes);
 }

src/mesh/Default.h

@@ -5,6 +5,7 @@
 #define ONE_MINUTE_MS 60 * 1000
 
 #define default_gps_update_interval IF_ROUTER(ONE_DAY, 2 * 60)
+#define default_telemetry_broadcast_interval_secs IF_ROUTER(ONE_DAY / 2, 30 * 60)
 #define default_broadcast_interval_secs IF_ROUTER(ONE_DAY / 2, 15 * 60)
 #define default_wait_bluetooth_secs IF_ROUTER(1, 60)
 #define default_sds_secs IF_ROUTER(ONE_DAY, UINT32_MAX) // Default to forever super deep sleep
@@ -28,4 +29,17 @@ class Default
     static uint32_t getConfiguredOrDefaultMs(uint32_t configuredInterval);
     static uint32_t getConfiguredOrDefaultMs(uint32_t configuredInterval, uint32_t defaultInterval);
     static uint32_t getConfiguredOrDefault(uint32_t configured, uint32_t defaultValue);
+    static uint32_t getConfiguredOrDefaultMsScaled(uint32_t configured, uint32_t defaultValue, uint32_t numOnlineNodes);
+
+  private:
+    static float congestionScalingCoefficient(int numOnlineNodes)
+    {
+        if (numOnlineNodes <= 40) {
+            return 1.0; // No scaling for 40 or fewer nodes
+        } else {
+            // Sscaling based on number of nodes over 40
+            int nodesOverForty = (numOnlineNodes - 40);
+            return 1.0 + (nodesOverForty * 0.075); // Each number of online node scales by 0.075
+        }
+    }
 };

src/mesh/FloodingRouter.cpp

@@ -20,9 +20,8 @@ ErrorCode FloodingRouter::send(meshtastic_MeshPacket *p)
 bool FloodingRouter::shouldFilterReceived(const meshtastic_MeshPacket *p)
 {
     if (wasSeenRecently(p)) { // Note: this will also add a recent packet record
-        printPacket("Ignoring incoming msg, because we've already seen it", p);
+        printPacket("Ignoring incoming msg we've already seen", p);
         if (config.device.role != meshtastic_Config_DeviceConfig_Role_ROUTER &&
-            config.device.role != meshtastic_Config_DeviceConfig_Role_ROUTER_CLIENT &&
             config.device.role != meshtastic_Config_DeviceConfig_Role_REPEATER) {
             // cancel rebroadcast of this message *if* there was already one, unless we're a router/repeater!
             Router::cancelSending(p->from, p->id);

src/mesh/InterfacesTemplates.cpp

@@ -1,3 +1,5 @@
+#include "LR11x0Interface.cpp"
+#include "LR11x0Interface.h"
 #include "SX126xInterface.cpp"
 #include "SX126xInterface.h"
 #include "SX128xInterface.cpp"
@@ -10,6 +12,8 @@ template class SX126xInterface<SX1262>;
 template class SX126xInterface<SX1268>;
 template class SX126xInterface<LLCC68>;
 template class SX128xInterface<SX1280>;
+template class LR11x0Interface<LR1110>;
+template class LR11x0Interface<LR1120>;
 #ifdef ARCH_STM32WL
 template class SX126xInterface<STM32WLx>;
 #endif

src/mesh/LR1110Interface.cpp

@@ -0,0 +1,9 @@
+#include "LR1110Interface.h"
+#include "configuration.h"
+#include "error.h"
+
+LR1110Interface::LR1110Interface(LockingArduinoHal *hal, RADIOLIB_PIN_TYPE cs, RADIOLIB_PIN_TYPE irq, RADIOLIB_PIN_TYPE rst,
+                                 RADIOLIB_PIN_TYPE busy)
+    : LR11x0Interface(hal, cs, irq, rst, busy)
+{
+}

src/mesh/LR1110Interface.h

@@ -0,0 +1,13 @@
+#pragma once
+
+#include "LR11x0Interface.h"
+
+/**
+ * Our adapter for LR1110 radios
+ */
+class LR1110Interface : public LR11x0Interface<LR1110>
+{
+  public:
+    LR1110Interface(LockingArduinoHal *hal, RADIOLIB_PIN_TYPE cs, RADIOLIB_PIN_TYPE irq, RADIOLIB_PIN_TYPE rst,
+                    RADIOLIB_PIN_TYPE busy);
+};

src/mesh/LR1120Interface.cpp

@@ -0,0 +1,9 @@
+#include "LR1120Interface.h"
+#include "configuration.h"
+#include "error.h"
+
+LR1120Interface::LR1120Interface(LockingArduinoHal *hal, RADIOLIB_PIN_TYPE cs, RADIOLIB_PIN_TYPE irq, RADIOLIB_PIN_TYPE rst,
+                                 RADIOLIB_PIN_TYPE busy)
+    : LR11x0Interface(hal, cs, irq, rst, busy)
+{
+}

src/mesh/LR1120Interface.h

@@ -0,0 +1,13 @@
+#pragma once
+
+#include "LR11x0Interface.h"
+
+/**
+ * Our adapter for LR1120 wideband radios
+ */
+class LR1120Interface : public LR11x0Interface<LR1120>
+{
+  public:
+    LR1120Interface(LockingArduinoHal *hal, RADIOLIB_PIN_TYPE cs, RADIOLIB_PIN_TYPE irq, RADIOLIB_PIN_TYPE rst,
+                    RADIOLIB_PIN_TYPE busy);
+};

src/mesh/LR11x0Interface.cpp

@@ -0,0 +1,300 @@
+#include "LR11x0Interface.h"
+#include "configuration.h"
+#include "error.h"
+#include "mesh/NodeDB.h"
+#ifdef ARCH_PORTDUINO
+#include "PortduinoGlue.h"
+#endif
+
+// Particular boards might define a different max power based on what their hardware can do, default to max power output if not
+// specified (may be dangerous if using external PA and LR11x0 power config forgotten)
+#ifndef LR11X0_MAX_POWER
+#define LR11X0_MAX_POWER 22
+#endif
+
+template <typename T>
+LR11x0Interface<T>::LR11x0Interface(LockingArduinoHal *hal, RADIOLIB_PIN_TYPE cs, RADIOLIB_PIN_TYPE irq, RADIOLIB_PIN_TYPE rst,
+                                    RADIOLIB_PIN_TYPE busy)
+    : RadioLibInterface(hal, cs, irq, rst, busy, &lora), lora(&module)
+{
+    LOG_WARN("LR11x0Interface(cs=%d, irq=%d, rst=%d, busy=%d)\n", cs, irq, rst, busy);
+}
+
+/// Initialise the Driver transport hardware and software.
+/// Make sure the Driver is properly configured before calling init().
+/// \return true if initialisation succeeded.
+template <typename T> bool LR11x0Interface<T>::init()
+{
+#ifdef LR11X0_POWER_EN
+    pinMode(LR11X0_POWER_EN, OUTPUT);
+    digitalWrite(LR11X0_POWER_EN, HIGH);
+#endif
+
+// FIXME: correct logic to default to not using TCXO if no voltage is specified for LR11x0_DIO3_TCXO_VOLTAGE
+#if !defined(LR11X0_DIO3_TCXO_VOLTAGE)
+    float tcxoVoltage =
+        0; // "TCXO reference voltage to be set on DIO3. Defaults to 1.6 V, set to 0 to skip." per
+           // https://github.com/jgromes/RadioLib/blob/690a050ebb46e6097c5d00c371e961c1caa3b52e/src/modules/LR11x0/LR11x0.h#L471C26-L471C104
+    // (DIO3 is free to be used as an IRQ)
+    LOG_DEBUG("LR11X0_DIO3_TCXO_VOLTAGE not defined, not using DIO3 as TCXO reference voltage\n");
+#else
+    float tcxoVoltage = LR11X0_DIO3_TCXO_VOLTAGE;
+    LOG_DEBUG("LR11X0_DIO3_TCXO_VOLTAGE defined, using DIO3 as TCXO reference voltage at %f V\n", LR11X0_DIO3_TCXO_VOLTAGE);
+    // (DIO3 is not free to be used as an IRQ)
+#endif
+
+    RadioLibInterface::init();
+
+    if (power > LR11X0_MAX_POWER) // Clamp power to maximum defined level
+        power = LR11X0_MAX_POWER;
+
+    limitPower();
+
+    // set RF switch configuration for Wio WM1110
+    // Wio WM1110 uses DIO5 and DIO6 for RF switching
+    // NOTE: other boards may be different. If you are
+    // using a different board, you may need to wrap
+    // this in a conditional.
+
+    static const uint32_t rfswitch_dio_pins[] = {RADIOLIB_LR11X0_DIO5, RADIOLIB_LR11X0_DIO6, RADIOLIB_NC, RADIOLIB_NC,
+                                                 RADIOLIB_NC};
+
+    static const Module::RfSwitchMode_t rfswitch_table[] = {
+        // mode                  DIO5  DIO6
+        {LR11x0::MODE_STBY, {LOW, LOW}},  {LR11x0::MODE_RX, {HIGH, LOW}},
+        {LR11x0::MODE_TX, {HIGH, HIGH}},  {LR11x0::MODE_TX_HP, {LOW, HIGH}},
+        {LR11x0::MODE_TX_HF, {LOW, LOW}}, {LR11x0::MODE_GNSS, {LOW, LOW}},
+        {LR11x0::MODE_WIFI, {LOW, LOW}},  END_OF_MODE_TABLE,
+    };
+
+// We need to do this before begin() call
+#ifdef LR11X0_DIO_AS_RF_SWITCH
+    LOG_DEBUG("Setting DIO RF switch\n");
+    bool dioAsRfSwitch = true;
+#elif defined(ARCH_PORTDUINO)
+    bool dioAsRfSwitch = false;
+    if (settingsMap[dio2_as_rf_switch]) {
+        LOG_DEBUG("Setting DIO RF switch\n");
+        dioAsRfSwitch = true;
+    }
+#else
+    bool dioAsRfSwitch = false;
+#endif
+
+    if (dioAsRfSwitch)
+        lora.setRfSwitchTable(rfswitch_dio_pins, rfswitch_table);
+
+    int res = lora.begin(getFreq(), bw, sf, cr, syncWord, power, preambleLength, tcxoVoltage);
+    // \todo Display actual typename of the adapter, not just `LR11x0`
+    LOG_INFO("LR11x0 init result %d\n", res);
+    if (res == RADIOLIB_ERR_CHIP_NOT_FOUND)
+        return false;
+
+    LOG_INFO("Frequency set to %f\n", getFreq());
+    LOG_INFO("Bandwidth set to %f\n", bw);
+    LOG_INFO("Power output set to %d\n", power);
+
+    if (res == RADIOLIB_ERR_NONE)
+        res = lora.setCRC(2);
+
+    // FIXME: May want to set depending on a definition, currently all LR1110 variant files use the DC-DC regulator option
+    if (res == RADIOLIB_ERR_NONE)
+        res = lora.setRegulatorDCDC();
+
+    if (res == RADIOLIB_ERR_NONE) {
+        if (config.lora.sx126x_rx_boosted_gain) { // the name is unfortunate but historically accurate
+            res = lora.setRxBoostedGainMode(true);
+            LOG_INFO("Set RX gain to boosted mode; result: %d\n", res);
+        } else {
+            res = lora.setRxBoostedGainMode(false);
+            LOG_INFO("Set RX gain to power saving mode (boosted mode off); result: %d\n", res);
+        }
+    }
+
+    if (res == RADIOLIB_ERR_NONE)
+        startReceive(); // start receiving
+
+    return res == RADIOLIB_ERR_NONE;
+}
+
+template <typename T> bool LR11x0Interface<T>::reconfigure()
+{
+    RadioLibInterface::reconfigure();
+
+    // set mode to standby
+    setStandby();
+
+    // configure publicly accessible settings
+    int err = lora.setSpreadingFactor(sf);
+    if (err != RADIOLIB_ERR_NONE)
+        RECORD_CRITICALERROR(meshtastic_CriticalErrorCode_INVALID_RADIO_SETTING);
+
+    err = lora.setBandwidth(bw);
+    if (err != RADIOLIB_ERR_NONE)
+        RECORD_CRITICALERROR(meshtastic_CriticalErrorCode_INVALID_RADIO_SETTING);
+
+    err = lora.setCodingRate(cr);
+    if (err != RADIOLIB_ERR_NONE)
+        RECORD_CRITICALERROR(meshtastic_CriticalErrorCode_INVALID_RADIO_SETTING);
+
+    // Hmm - seems to lower SNR when the signal levels are high.  Leaving off for now...
+    // TODO: Confirm gain registers are okay now
+    // err = lora.setRxGain(true);
+    // assert(err == RADIOLIB_ERR_NONE);
+
+    err = lora.setSyncWord(syncWord);
+    assert(err == RADIOLIB_ERR_NONE);
+
+    err = lora.setPreambleLength(preambleLength);
+    assert(err == RADIOLIB_ERR_NONE);
+
+    err = lora.setFrequency(getFreq());
+    if (err != RADIOLIB_ERR_NONE)
+        RECORD_CRITICALERROR(meshtastic_CriticalErrorCode_INVALID_RADIO_SETTING);
+
+    if (power > LR11X0_MAX_POWER) // This chip has lower power limits than some
+        power = LR11X0_MAX_POWER;
+
+    err = lora.setOutputPower(power);
+    assert(err == RADIOLIB_ERR_NONE);
+
+    startReceive(); // restart receiving
+
+    return RADIOLIB_ERR_NONE;
+}
+
+template <typename T> void INTERRUPT_ATTR LR11x0Interface<T>::disableInterrupt()
+{
+    lora.clearIrqAction();
+}
+
+template <typename T> void LR11x0Interface<T>::setStandby()
+{
+    checkNotification(); // handle any pending interrupts before we force standby
+
+    int err = lora.standby();
+
+    if (err != RADIOLIB_ERR_NONE) {
+        LOG_DEBUG("LR11x0 standby failed with error %d\n", err);
+    }
+
+    assert(err == RADIOLIB_ERR_NONE);
+
+    isReceiving = false; // If we were receiving, not any more
+    activeReceiveStart = 0;
+    disableInterrupt();
+    completeSending(); // If we were sending, not anymore
+    RadioLibInterface::setStandby();
+}
+
+/**
+ * Add SNR data to received messages
+ */
+template <typename T> void LR11x0Interface<T>::addReceiveMetadata(meshtastic_MeshPacket *mp)
+{
+    // LOG_DEBUG("PacketStatus %x\n", lora.getPacketStatus());
+    mp->rx_snr = lora.getSNR();
+    mp->rx_rssi = lround(lora.getRSSI());
+}
+
+/** We override to turn on transmitter power as needed.
+ */
+template <typename T> void LR11x0Interface<T>::configHardwareForSend()
+{
+    RadioLibInterface::configHardwareForSend();
+}
+
+// For power draw measurements, helpful to force radio to stay sleeping
+// #define SLEEP_ONLY
+
+template <typename T> void LR11x0Interface<T>::startReceive()
+{
+#ifdef SLEEP_ONLY
+    sleep();
+#else
+
+    setStandby();
+
+    lora.setPreambleLength(preambleLength); // Solve RX ack fail after direct message sent.  Not sure why this is needed.
+
+    // We use a 16 bit preamble so this should save some power by letting radio sit in standby mostly.
+    // Furthermore, we need the PREAMBLE_DETECTED and HEADER_VALID IRQ flag to detect whether we are actively receiving
+    int err = lora.startReceive(
+        RADIOLIB_LR11X0_RX_TIMEOUT_INF, RADIOLIB_LR11X0_IRQ_RX_DONE,
+        0); // only RX_DONE IRQ is needed, we'll check for PREAMBLE_DETECTED and HEADER_VALID in isActivelyReceiving
+    assert(err == RADIOLIB_ERR_NONE);
+
+    RadioLibInterface::startReceive();
+
+    // Must be done AFTER, starting transmit, because startTransmit clears (possibly stale) interrupt pending register bits
+    enableInterrupt(isrRxLevel0);
+#endif
+}
+
+/** Is the channel currently active? */
+template <typename T> bool LR11x0Interface<T>::isChannelActive()
+{
+    // check if we can detect a LoRa preamble on the current channel
+    int16_t result;
+
+    setStandby();
+    result = lora.scanChannel();
+    if (result == RADIOLIB_LORA_DETECTED)
+        return true;
+
+    assert(result != RADIOLIB_ERR_WRONG_MODEM);
+
+    return false;
+}
+
+/** Could we send right now (i.e. either not actively receiving or transmitting)? */
+template <typename T> bool LR11x0Interface<T>::isActivelyReceiving()
+{
+    // The IRQ status will be cleared when we start our read operation. Check if we've started a header, but haven't yet
+    // received and handled the interrupt for reading the packet/handling errors.
+
+    uint16_t irq = lora.getIrqStatus();
+    bool detected = (irq & (RADIOLIB_LR11X0_IRQ_SYNC_WORD_HEADER_VALID | RADIOLIB_LR11X0_IRQ_PREAMBLE_DETECTED));
+    // Handle false detections
+    if (detected) {
+        uint32_t now = millis();
+        if (!activeReceiveStart) {
+            activeReceiveStart = now;
+        } else if ((now - activeReceiveStart > 2 * preambleTimeMsec) && !(irq & RADIOLIB_LR11X0_IRQ_SYNC_WORD_HEADER_VALID)) {
+            // The HEADER_VALID flag should be set by now if it was really a packet, so ignore PREAMBLE_DETECTED flag
+            activeReceiveStart = 0;
+            LOG_DEBUG("Ignore false preamble detection.\n");
+            return false;
+        } else if (now - activeReceiveStart > maxPacketTimeMsec) {
+            // We should have gotten an RX_DONE IRQ by now if it was really a packet, so ignore HEADER_VALID flag
+            activeReceiveStart = 0;
+            LOG_DEBUG("Ignore false header detection.\n");
+            return false;
+        }
+    }
+
+    // if (detected) LOG_DEBUG("rx detected\n");
+    return detected;
+}
+
+template <typename T> bool LR11x0Interface<T>::sleep()
+{
+    // Not keeping config is busted - next time nrf52 board boots lora sending fails  tcxo related? - see datasheet
+    // \todo Display actual typename of the adapter, not just `LR11x0`
+    LOG_DEBUG("LR11x0 entering sleep mode (FIXME, don't keep config)\n");
+    setStandby(); // Stop any pending operations
+
+    // turn off TCXO if it was powered
+    // FIXME - this isn't correct
+    // lora.setTCXO(0);
+
+    // put chipset into sleep mode (we've already disabled interrupts by now)
+    bool keepConfig = true;
+    lora.sleep(keepConfig, 0); // Note: we do not keep the config, full reinit will be needed
+
+#ifdef LR11X0_POWER_EN
+    digitalWrite(LR11X0_POWER_EN, LOW);
+#endif
+
+    return true;
+}

src/mesh/LR11x0Interface.h

@@ -0,0 +1,71 @@
+#pragma once
+
+#include "RadioLibInterface.h"
+
+/**
+ * \brief Adapter for LR11x0 radio family. Implements common logic for child classes.
+ * \tparam T RadioLib module type for LR11x0: SX1262, SX1268.
+ */
+template <class T> class LR11x0Interface : public RadioLibInterface
+{
+  public:
+    LR11x0Interface(LockingArduinoHal *hal, RADIOLIB_PIN_TYPE cs, RADIOLIB_PIN_TYPE irq, RADIOLIB_PIN_TYPE rst,
+                    RADIOLIB_PIN_TYPE busy);
+
+    /// Initialise the Driver transport hardware and software.
+    /// Make sure the Driver is properly configured before calling init().
+    /// \return true if initialisation succeeded.
+    virtual bool init() override;
+
+    /// Apply any radio provisioning changes
+    /// Make sure the Driver is properly configured before calling init().
+    /// \return true if initialisation succeeded.
+    virtual bool reconfigure() override;
+
+    /// Prepare hardware for sleep.  Call this _only_ for deep sleep, not needed for light sleep.
+    virtual bool sleep() override;
+
+    bool isIRQPending() override { return lora.getIrqStatus() != 0; }
+
+  protected:
+    /**
+     * Specific module instance
+     */
+    T lora;
+
+    /**
+     * Glue functions called from ISR land
+     */
+    virtual void disableInterrupt() override;
+
+    /**
+     * Enable a particular ISR callback glue function
+     */
+    virtual void enableInterrupt(void (*callback)()) { lora.setIrqAction(callback); }
+
+    /** can we detect a LoRa preamble on the current channel? */
+    virtual bool isChannelActive() override;
+
+    /** are we actively receiving a packet (only called during receiving state) */
+    virtual bool isActivelyReceiving() override;
+
+    /**
+     * Start waiting to receive a message
+     */
+    virtual void startReceive() override;
+
+    /**
+     *  We override to turn on transmitter power as needed.
+     */
+    virtual void configHardwareForSend() override;
+
+    /**
+     * Add SNR data to received messages
+     */
+    virtual void addReceiveMetadata(meshtastic_MeshPacket *mp) override;
+
+    virtual void setStandby() override;
+
+  private:
+    uint32_t activeReceiveStart = 0;
+};