Squashed commit of the following:

commit e2e1880 Merge: f7fc1ce 86915b1 Author: kounocom <meia@kouno.xyz> Date: Fri Oct 11 18:10:11 2024 +0300 Merge branch 'icm45686-firmware' of https://github.com/gorbit99/SlimeVR-Tracker-ESP into icm45686-firmware commit 86915b1 Author: gorbit99 <gorbitgames@gmail.com> Date: Mon Oct 7 08:27:51 2024 +0200 Fix incorrect number in IMU name commit 4abf06f Author: gorbit99 <gorbitgames@gmail.com> Date: Sat Oct 5 04:46:42 2024 +0200 Remove unnecessary changes commit 4680248 Author: gorbit99 <gorbitgames@gmail.com> Date: Wed Oct 2 09:02:21 2024 +0200 ICM45686 implementation commit f7fc1ce Author: kounocom <meia@kouno.xyz> Date: Wed Oct 2 08:00:31 2024 +0300 Make sensor reset work (lol) commit 6eb1ef9 Author: kounocom <meia@kouno.xyz> Date: Wed Oct 2 07:39:19 2024 +0300 Use correct sensitivity commit b84096c Merge: 6515014 73faa41 Author: gorbit99 <gorbitgames@gmail.com> Date: Wed Oct 2 03:59:36 2024 +0200 Merge branch 'icm45686-firmware' of https://github.com/gorbit99/SlimeVR-Tracker-ESP into icm45686-firmware commit 6515014 Author: gorbit99 <gorbitgames@gmail.com> Date: Wed Oct 2 03:59:24 2024 +0200 Hires mode maybe? commit 73faa41 Author: kounocom <meia@kouno.xyz> Date: Wed Oct 2 03:11:08 2024 +0300 Sleep more commit 1aae55a Author: kounocom <meia@kouno.xyz> Date: Wed Oct 2 03:00:55 2024 +0300 Use 8KB FIFO commit bc65001 Author: kounocom <meia@kouno.xyz> Date: Wed Oct 2 02:50:59 2024 +0300 Use clock commit eec8ac6 Author: gorbit99 <gorbitgames@gmail.com> Date: Tue Oct 1 23:56:46 2024 +0200 Actually enable FIFO this time commit db71a05 Author: kounocom <meia@kouno.xyz> Date: Mon Sep 30 17:03:19 2024 +0300 change to 4g, fill in missing stuff elsewhere commit 8a63326 Author: gorbit99 <gorbitgames@gmail.com> Date: Wed Sep 25 21:09:51 2024 +0200 In theory this works? commit dbce4cb Author: Butterscotch! <bscotchvanilla@gmail.com> Date: Wed Sep 25 12:02:37 2024 -0400 Add dependabot & update workflows (SlimeVR#344) * Add dependabot & update workflows * Specify python-version & use pip cache * Add requirements.txt * Cache PIP generally instead & remove requirements commit 8b00064 Author: Ondrej Hruska <11602729+ZycaR@users.noreply.github.com> Date: Sun Sep 1 10:46:47 2024 -0700 GET.INFO Serial Command Should Returns Battery Voltage & Level (in Percentages) (SlimeVR#338) * GET.INFO serial command return also battery voltage & level in percents * move public headers together
kounocom · Oct 11, 2024 · 7f8c981 · 7f8c981
1 parent 05622fd
commit 7f8c981
Show file tree

Hide file tree

Showing 11 changed files with 706 additions and 389 deletions.
diff --git a/.github/dependabot.yml b/.github/dependabot.yml
@@ -0,0 +1,8 @@
+version: 2
+updates:
+
+  - package-ecosystem: "github-actions"
+    directory: "/"
+    schedule:
+      # Check for updates to GitHub Actions every week
+      interval: "weekly"
diff --git a/.github/workflows/actions.yml b/.github/workflows/actions.yml
@@ -9,21 +9,29 @@ jobs:
     runs-on: ubuntu-20.04
 
     steps:
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v4
+      - uses: actions/cache@v4
+        with:
+          path: |
+            ~/.cache/pip
+            ~/.platformio/.cache
+          key: ${{ runner.os }}-pio
 
       - name: Set up Python
-        uses: actions/setup-python@v1
+        uses: actions/setup-python@v5
+        with:
+          python-version: "3.12"
 
-      - name: Install pio and its dependencies
+      - name: Install PlatformIO and its dependencies
         run: |
           python -m pip install --upgrade pip
-          pip install platformio
+          pip install --upgrade platformio
 
       - name: Run builds
         run: python ./ci/build.py
 
       - name: Upload binaries
-        uses: actions/upload-artifact@v2
+        uses: actions/upload-artifact@v4
         with:
           name: binaries
           path: ./build/*.bin
diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml
@@ -1,6 +1,6 @@
 name: Releases
 
-on: 
+on:
   push:
     tags:
     - '*'
@@ -12,9 +12,9 @@ jobs:
     permissions:
       contents: write
     steps:
-    - uses: actions/checkout@v2
+    - uses: actions/checkout@v4
     - uses: ncipollo/release-action@v1
       with:
         artifacts: "./build/*.bin"
         draft: true
-        token: ${{ secrets.GITHUB_TOKEN }}
+        token: ${{ secrets.GITHUB_TOKEN }}
diff --git a/src/GlobalVars.h b/src/GlobalVars.h
@@ -32,6 +32,7 @@
 #include "network/manager.h"
 #include "sensors/SensorManager.h"
 #include "status/StatusManager.h"
+#include "batterymonitor.h"
 
 extern Timer<> globalTimer;
 extern SlimeVR::LEDManager ledManager;
@@ -40,5 +41,6 @@ extern SlimeVR::Configuration::Configuration configuration;
 extern SlimeVR::Sensors::SensorManager sensorManager;
 extern SlimeVR::Network::Manager networkManager;
 extern SlimeVR::Network::Connection networkConnection;
+extern BatteryMonitor battery;
 
 #endif
diff --git a/src/batterymonitor.h b/src/batterymonitor.h
@@ -72,6 +72,9 @@ class BatteryMonitor
     void Setup();
     void Loop();
 
+    float getVoltage() const { return voltage; }
+    float getLevel() const { return level; }
+
 private:
     unsigned long last_battery_sample = 0;
 #if BATTERY_MONITOR == BAT_MCP3021 || BATTERY_MONITOR == BAT_INTERNAL_MCP3021

diff --git a/src/consts.h b/src/consts.h
@@ -42,6 +42,7 @@ enum class ImuID {
     LSM6DSV,
     LSM6DSO,
     LSM6DSR,
+    ICM45686,
     Empty = 255
 };
 
@@ -62,6 +63,7 @@ enum class ImuID {
 #define IMU_LSM6DSO SoftFusionLSM6DSO
 #define IMU_LSM6DSR SoftFusionLSM6DSR
 #define IMU_MPU6050_SF SoftFusionMPU6050
+#define IMU_ICM45686 SoftFusionICM45686
 
 #define IMU_DEV_RESERVED 250 // Reserved, should not be used in any release firmware
 

diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp
@@ -38,6 +38,7 @@
 #include "softfusion/drivers/lsm6dso.h"
 #include "softfusion/drivers/lsm6dsr.h"
 #include "softfusion/drivers/mpu6050.h"
+#include "softfusion/drivers/icm45686.h"
 
 #include "softfusion/i2cimpl.h"
 
@@ -56,6 +57,7 @@ namespace SlimeVR
         using SoftFusionLSM6DSO = SoftFusionSensor<SoftFusion::Drivers::LSM6DSO, SoftFusion::I2CImpl>;
         using SoftFusionLSM6DSR = SoftFusionSensor<SoftFusion::Drivers::LSM6DSR, SoftFusion::I2CImpl>;
         using SoftFusionMPU6050 = SoftFusionSensor<SoftFusion::Drivers::MPU6050, SoftFusion::I2CImpl>;
+        using SoftFusionICM45686 = SoftFusionSensor<SoftFusion::Drivers::ICM45686, SoftFusion::I2CImpl>;
 
         // TODO Make it more generic in the future and move another place (abstract sensor interface)
         void SensorManager::swapI2C(uint8_t sclPin, uint8_t sdaPin)

diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp
@@ -101,6 +101,8 @@ const char * getIMUNameByType(ImuID imuType) {
             return "LSM6DSO";
         case ImuID::LSM6DSR:
             return "LSM6DSR";
+        case ImuID::ICM45686:
+            return "ICM45686";
         case ImuID::Unknown:
         case ImuID::Empty:
             return "UNKNOWN";

diff --git a/src/sensors/softfusion/drivers/icm45686.h b/src/sensors/softfusion/drivers/icm45686.h
@@ -0,0 +1,205 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2024 Gorbit99 & SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+
+#pragma once
+
+#include <algorithm>
+#include <array>
+#include <cstdint>
+
+namespace SlimeVR::Sensors::SoftFusion::Drivers {
+
+// Driver uses acceleration range at 32g
+// and gyroscope range at 4000dps
+// using high resolution mode
+// Uses 32.768kHz clock
+// Gyroscope ODR = 409.6Hz, accel ODR = 102.4Hz
+// Timestamps reading not used, as they're useless (constant predefined increment)
+
+template <typename I2CImpl>
+struct ICM45686 {
+	static constexpr uint8_t Address = 0x68;
+	static constexpr auto Name = "ICM-45686";
+	static constexpr auto Type = ImuID::ICM45686;
+
+	static constexpr float GyrTs = 1.0 / 409.6;
+	static constexpr float AccTs = 1.0 / 102.4;
+
+	static constexpr float MagTs = 1.0 / 100;
+
+	static constexpr float GyroSensitivity = 131.072f;
+	static constexpr float AccelSensitivity = 16384.0f;
+
+	static constexpr bool Uses32BitSensorData = true;
+
+	I2CImpl i2c;
+	SlimeVR::Logging::Logger& logger;
+	ICM45686(I2CImpl i2c, SlimeVR::Logging::Logger& logger)
+		: i2c(i2c)
+		, logger(logger) {}
+
+	struct Regs {
+		struct WhoAmI {
+			static constexpr uint8_t reg = 0x72;
+			static constexpr uint8_t value = 0xe9;
+		};
+		static constexpr uint8_t TempData = 0x0c;
+
+		struct DeviceConfig {
+			static constexpr uint8_t reg = 0x7f;
+			static constexpr uint8_t valueSwReset = 0b11;
+		};
+
+		struct Pin9Config {
+			static constexpr uint8_t reg = 0x31;
+			static constexpr uint8_t value = 0b00000110;  // pin 9 to clkin
+		};
+
+		struct RtcConfig {
+			static constexpr uint8_t reg = 0x26;
+			static constexpr uint8_t value = 0b00100011;  // enable RTC
+		};
+
+		struct GyroConfig {
+			static constexpr uint8_t reg = 0x1c;
+			static constexpr uint8_t value
+				= (0b0000 << 4) | 0b0111;  // 4000dps, odr=409.6Hz
+		};
+
+		struct AccelConfig {
+			static constexpr uint8_t reg = 0x1b;
+			static constexpr uint8_t value
+				= (0b000 << 4) | 0b1001;  // 32g, odr = 102.4Hz
+		};
+
+		struct FifoConfig0 {
+			static constexpr uint8_t reg = 0x1d;
+			static constexpr uint8_t value
+				= (0b01 << 6) | (0b011111);  // stream to FIFO mode, FIFO depth
+											 // 8k bytes <-- this disables all APEX
+											 // features, but we don't need them
+		};
+
+		struct FifoConfig3 {
+			static constexpr uint8_t reg = 0x21;
+			static constexpr uint8_t value = (0b1 << 0) | (0b1 << 1) | (0b1 << 2)
+										   | (0b1 << 3);  // enable FIFO,
+														  // enable accel,
+														  // enable gyro,
+														  // enable hires mode
+		};
+
+		struct PwrMgmt0 {
+			static constexpr uint8_t reg = 0x10;
+			static constexpr uint8_t value
+				= 0b11 | (0b11 << 2);  // accel in low noise mode, gyro in low noise
+		};
+
+		static constexpr uint8_t FifoCount = 0x12;
+		static constexpr uint8_t FifoData = 0x14;
+	};
+
+#pragma pack(push, 1)
+	struct FifoEntryAligned {
+		union {
+			struct {
+				int16_t accel[3];
+				int16_t gyro[3];
+				uint16_t temp;
+				uint16_t timestamp;
+				uint8_t lsb[3];
+			} part;
+			uint8_t raw[19];
+		};
+	};
+#pragma pack(pop)
+
+	static constexpr size_t FullFifoEntrySize = sizeof(FifoEntryAligned) + 1;
+
+	bool initialize() {
+		// perform initialization step
+		i2c.writeReg(Regs::DeviceConfig::reg, Regs::DeviceConfig::valueSwReset);
+		delay(35);
+		i2c.writeReg(Regs::Pin9Config::reg, Regs::Pin9Config::value);
+		i2c.writeReg(Regs::RtcConfig::reg, Regs::RtcConfig::value);
+		i2c.writeReg(Regs::GyroConfig::reg, Regs::GyroConfig::value);
+		i2c.writeReg(Regs::AccelConfig::reg, Regs::AccelConfig::value);
+		i2c.writeReg(Regs::FifoConfig0::reg, Regs::FifoConfig0::value);
+		i2c.writeReg(Regs::FifoConfig3::reg, Regs::FifoConfig3::value);
+		i2c.writeReg(Regs::PwrMgmt0::reg, Regs::PwrMgmt0::value);
+		delay(1);
+
+		return true;
+	}
+
+	float getDirectTemp() const {
+		const auto value = static_cast<int16_t>(i2c.readReg16(Regs::TempData));
+		float result = ((float)value / 132.48f) + 25.0f;
+		return result;
+	}
+
+	template <typename AccelCall, typename GyroCall>
+	void bulkRead(AccelCall&& processAccelSample, GyroCall&& processGyroSample) {
+		const auto fifo_packets = i2c.readReg16(Regs::FifoCount);
+		const auto fifo_bytes = fifo_packets * sizeof(FullFifoEntrySize);
+
+		std::array<uint8_t, FullFifoEntrySize * 8> read_buffer;  // max 8 readings
+		const auto bytes_to_read = std::min(
+									   static_cast<size_t>(read_buffer.size()),
+									   static_cast<size_t>(fifo_bytes)
+								   )
+								 / FullFifoEntrySize * FullFifoEntrySize;
+		i2c.readBytes(Regs::FifoData, bytes_to_read, read_buffer.data());
+		for (auto i = 0u; i < bytes_to_read; i += FullFifoEntrySize) {
+			FifoEntryAligned entry;
+			memcpy(
+				entry.raw,
+				&read_buffer[i + 0x1],
+				sizeof(FifoEntryAligned)
+			);  // skip fifo header
+			const int32_t gyroData[3]{
+				static_cast<int32_t>(entry.part.gyro[0]) << 4
+					| (entry.part.lsb[0] & 0xf),
+				static_cast<int32_t>(entry.part.gyro[1]) << 4
+					| (entry.part.lsb[1] & 0xf),
+				static_cast<int32_t>(entry.part.gyro[2]) << 4
+					| (entry.part.lsb[2] & 0xf),
+			};
+			processGyroSample(gyroData, GyrTs);
+
+			if (entry.part.accel[0] != -32768) {
+				const int32_t accelData[3]{
+					static_cast<int32_t>(entry.part.accel[0]) << 4
+						| (static_cast<int32_t>(entry.part.lsb[0]) & 0xf0 >> 4),
+					static_cast<int32_t>(entry.part.accel[1]) << 4
+						| (static_cast<int32_t>(entry.part.lsb[1]) & 0xf0 >> 4),
+					static_cast<int32_t>(entry.part.accel[2]) << 4
+						| (static_cast<int32_t>(entry.part.lsb[2]) & 0xf0 >> 4),
+				};
+				processAccelSample(accelData, AccTs);
+			}
+		}
+	}
+};
+
+}  // namespace SlimeVR::Sensors::SoftFusion::Drivers