From 4068b8bb01b9d2bc3dc2fee816dc9359cbb97a21 Mon Sep 17 00:00:00 2001
From: ztimson <zaktimson@gmail.com>
Date: Wed, 30 Oct 2024 11:22:19 -0400
Subject: [PATCH] Added IMU

---
 src/bno080.js       | 71 +++++++++++++++++++++++++++++++++++++++++++++
 src/sensor-suite.js |  9 ++++--
 2 files changed, 78 insertions(+), 2 deletions(-)
 create mode 100644 src/bno080.js

diff --git a/src/bno080.js b/src/bno080.js
new file mode 100644
index 0000000..8f79864
--- /dev/null
+++ b/src/bno080.js
@@ -0,0 +1,71 @@
+import i2c from 'i2c-bus';
+
+export async function imu(address = 0x4A) {  // BNO080 default I2C address
+    const i2cBus = await i2c.openPromisified(1);
+
+    // Register addresses for accelerometer, gyroscope, and magnetometer data (assumes configuration is done)
+    const ACCEL_X_LSB = 0x08;
+    const ACCEL_X_MSB = 0x09;
+    const ACCEL_Y_LSB = 0x0A;
+    const ACCEL_Y_MSB = 0x0B;
+    const ACCEL_Z_LSB = 0x0C;
+    const ACCEL_Z_MSB = 0x0D;
+
+    const GYRO_X_LSB = 0x10;
+    const GYRO_X_MSB = 0x11;
+    const GYRO_Y_LSB = 0x12;
+    const GYRO_Y_MSB = 0x13;
+    const GYRO_Z_LSB = 0x14;
+    const GYRO_Z_MSB = 0x15;
+
+    const MAG_X_LSB = 0x16;  // Hypothetical register for magnetometer X LSB
+    const MAG_X_MSB = 0x17;  // Hypothetical register for magnetometer X MSB
+    const MAG_Y_LSB = 0x18;  // Hypothetical register for magnetometer Y LSB
+    const MAG_Y_MSB = 0x19;  // Hypothetical register for magnetometer Y MSB
+    const MAG_Z_LSB = 0x1A;  // Hypothetical register for magnetometer Z LSB
+    const MAG_Z_MSB = 0x1B;  // Hypothetical register for magnetometer Z MSB
+
+    // Read data from the sensor registers
+    const data = await Promise.all([
+        i2cBus.readByte(address, ACCEL_X_LSB),
+        i2cBus.readByte(address, ACCEL_X_MSB),
+        i2cBus.readByte(address, ACCEL_Y_LSB),
+        i2cBus.readByte(address, ACCEL_Y_MSB),
+        i2cBus.readByte(address, ACCEL_Z_LSB),
+        i2cBus.readByte(address, ACCEL_Z_MSB),
+        i2cBus.readByte(address, GYRO_X_LSB),
+        i2cBus.readByte(address, GYRO_X_MSB),
+        i2cBus.readByte(address, GYRO_Y_LSB),
+        i2cBus.readByte(address, GYRO_Y_MSB),
+        i2cBus.readByte(address, GYRO_Z_LSB),
+        i2cBus.readByte(address, GYRO_Z_MSB),
+        i2cBus.readByte(address, MAG_X_LSB),
+        i2cBus.readByte(address, MAG_X_MSB),
+        i2cBus.readByte(address, MAG_Y_LSB),
+        i2cBus.readByte(address, MAG_Y_MSB),
+        i2cBus.readByte(address, MAG_Z_LSB),
+        i2cBus.readByte(address, MAG_Z_MSB),
+    ]);
+
+    // Convert accelerometer data
+    const accelX = ((data[1] << 8) | data[0]) / 1000;
+    const accelY = ((data[3] << 8) | data[2]) / 1000;
+    const accelZ = ((data[5] << 8) | data[4]) / 1000;
+
+    // Convert gyroscope data
+    const gyroX = ((data[7] << 8) | data[6]) / 1000;
+    const gyroY = ((data[9] << 8) | data[8]) / 1000;
+    const gyroZ = ((data[11] << 8) | data[10]) / 1000;
+
+    // Convert magnetometer data
+    const magX = ((data[13] << 8) | data[12]) / 1000;  // Hypothetical conversion factor
+    const magY = ((data[15] << 8) | data[14]) / 1000;
+    const magZ = ((data[17] << 8) | data[16]) / 1000;
+
+    await i2cBus.close();
+    return {
+        acceleration: { x: accelX, y: accelY, z: accelZ },
+        gyroscope: { x: gyroX, y: gyroY, z: gyroZ },
+        magnetometer: { x: magX, y: magY, z: magZ },
+    };
+}
diff --git a/src/sensor-suite.js b/src/sensor-suite.js
index c13ee5a..5dbfdef 100644
--- a/src/sensor-suite.js
+++ b/src/sensor-suite.js
@@ -1,6 +1,7 @@
 import {adjustedInterval, sleep} from '@ztimson/utils';
 import {bms} from './bms.js';
 import {bme} from './bme280.js';
+import {imu} from './bno080.js';
 
 export default class SensorSuite {
     data = {
@@ -25,11 +26,15 @@ export default class SensorSuite {
     }
 
     async start() {
+        // Movement
+        this.intervals.push(adjustedInterval(async () => {
+            this.data.movement = await this.statusWrapper(imu(), 'imu');
+        }, 1000));
+        // Environment
         this.intervals.push(adjustedInterval(async () => {
             this.data.environment = await this.statusWrapper(bme(), 'bme280');
-            if(this.data.environment?.pressure != null)
-                this.data.environment.altitude = SensorSuite.#hPaToAltitude(this.data.environment.pressure);
         }, 1000));
+        // Battery
         await sleep(500); // Offset reading sensor data
         this.intervals.push(adjustedInterval(async () => {
             this.data.battery = await this.statusWrapper(bms(), 'bms');