From 2b1e2626cb067f77d28c4bbb7e799e2502e68706 Mon Sep 17 00:00:00 2001
From: Jan-Henrik Bruhn <hi@jhbruhn.de>
Date: Fri, 14 Feb 2025 22:06:03 +0100
Subject: [PATCH] Implement Tilt Sensor support

---
 firmware/include/MonomeSerialDevice.h |  2 +-
 firmware/src/MonomeSerialDevice.cpp   | 31 +++++++++-----------
 firmware/src/firmware.cpp             | 41 +++++++++++++--------------
 3 files changed, 34 insertions(+), 40 deletions(-)

diff --git a/firmware/include/MonomeSerialDevice.h b/firmware/include/MonomeSerialDevice.h
index b8b44b0..6c07e26 100644
--- a/firmware/include/MonomeSerialDevice.h
+++ b/firmware/include/MonomeSerialDevice.h
@@ -74,7 +74,7 @@ public:
   void refreshArc();
 
   void setTiltActive(uint8_t sensor, bool active);
-  void sendTiltEvent(uint8_t sensor, int16_t x, int16_t y, int16_t z);
+  void sendTiltEvent(uint8_t sensor, int8_t xh,int8_t xl,int8_t yh,int8_t yl,int8_t zh,int8_t zl);
 
 
   bool active;
diff --git a/firmware/src/MonomeSerialDevice.cpp b/firmware/src/MonomeSerialDevice.cpp
index a0f677f..c8e0762 100644
--- a/firmware/src/MonomeSerialDevice.cpp
+++ b/firmware/src/MonomeSerialDevice.cpp
@@ -43,20 +43,17 @@ void MonomeSerialDevice::setTiltActive(uint8_t sensor, bool active) {
   }
 }
 
-void MonomeSerialDevice::sendTiltEvent(uint8_t sensor, int16_t x, int16_t y, int16_t z) {
+void MonomeSerialDevice::sendTiltEvent(uint8_t sensor, int8_t xh,int8_t xl,int8_t yh,int8_t yl,int8_t zh,int8_t zl) {
     if (sensor < 4 && tiltActive[sensor]) {
-        lastTiltX[sensor] = x;
-        lastTiltY[sensor] = y;
-        lastTiltZ[sensor] = z;
 
         Serial.write((uint8_t)0x81);  // tiltイベントのプレフィックス
         Serial.write(sensor);
-        Serial.write((uint8_t)(x >> 8));
-        Serial.write((uint8_t)(x & 0xFF));
-        Serial.write((uint8_t)(y >> 8));
-        Serial.write((uint8_t)(y & 0xFF));
-        Serial.write((uint8_t)(z >> 8));
-        Serial.write((uint8_t)(z & 0xFF));
+        Serial.write((int8_t)xh);
+        Serial.write((int8_t)xl);
+        Serial.write((int8_t)yh);
+        Serial.write((int8_t)yl);
+        Serial.write((int8_t)zh);
+        Serial.write((int8_t)zl);
     }
 }
 
@@ -637,15 +634,13 @@ void MonomeSerialDevice::processSerial() {
       break;
 
     case 0x81:  // tilt data request
-      {
-        uint8_t sensor = Serial.read();
-        if (sensor < 4 && tiltActive[sensor]) {
-          // recent data send
-          sendTiltEvent(sensor, lastTiltX[sensor], lastTiltY[sensor], lastTiltZ[sensor]);
-        }
-      }
+      readX = Serial.read();
+      setTiltActive(readX, true);
+      break;
+    case 0x82:  // tilt data request
+      readX = Serial.read();
+      setTiltActive(readX, false);
       break;
-
     // 0x90 variable 64 LED ring
     case 0x90:
       //pattern:  /prefix/ring/set n x a
diff --git a/firmware/src/firmware.cpp b/firmware/src/firmware.cpp
index c7eccef..a3aac7b 100644
--- a/firmware/src/firmware.cpp
+++ b/firmware/src/firmware.cpp
@@ -42,6 +42,8 @@ int gridRotation = DEFAULT_ROTATION;
 bool flipHorizontal = DEFAULT_FLIP_HORIZONTAL;
 bool flipVertical = DEFAULT_FLIP_VERTICAL;
 
+bool hasTiltSensor = false;
+
 void sendTiltData();
 void updateLEDMatrix();
 void scanButtonMatrix();
@@ -141,23 +143,17 @@ void setup() {
     pixels.begin();
 
     // Initialize MPU-6050
-    /*Wire.setSCL(21);
+    Wire.setSCL(21);
     Wire.setSDA(20);
-    Wire.begin();*/
+    Wire.begin();
 
-    /*if (!mpu.begin()) {
-        Serial.println("Failed to find MPU6050 chip");
-        while (1) {
-            delay(10);
-        }
+    if (mpu.begin()) {
+        hasTiltSensor = true;
+        Serial.println("MPU6050 Found!");
+
+        mpu.setAccelerometerRange(MPU6050_RANGE_16_G);
+        mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
     }
-    Serial.println("MPU6050 Found!");
-
-    mpu.setGyroRange(MPU6050_RANGE_500_DEG);
-    mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
-
-    // Set tilt sensor 0 as active
-    mdp.setTiltActive(0, true);*/
 }
 
 void loop() {
@@ -174,9 +170,9 @@ void loop() {
     }
 
     // Send tilt data every 100ms
-    if (currentMillis - lastTiltCheck >= 100) {
+    if (hasTiltSensor && currentMillis - lastTiltCheck >= 100) {
         lastTiltCheck = currentMillis;
-        //sendTiltData();
+        sendTiltData();
     }
 }
 
@@ -214,10 +210,13 @@ void sendTiltData() {
     sensors_event_t a, g, temp;
     mpu.getEvent(&a, &g, &temp);
 
-    // Scale gyro data to 16-bit integer range
-    int16_t x = (int16_t)(g.gyro.x * 1000);
-    int16_t y = (int16_t)(g.gyro.y * 1000);
-    int16_t z = (int16_t)(g.gyro.z * 1000);
+    // TODO: Include Rotation here?
+    int16_t axis[3];
+    axis[0] = (int16_t)(a.acceleration.x * 4) + 128;
+    axis[1] = (int16_t)(-a.acceleration.y * 4) + 128;
+    axis[2] = (int16_t)(a.acceleration.z * 4) + 128;
 
-    mdp.sendTiltEvent(0, x, y, z);
+    int8_t *axisbytes = (int8_t *)axis;
+
+    mdp.sendTiltEvent(0,axisbytes[0],axisbytes[1],axisbytes[2],axisbytes[3],axisbytes[4],axisbytes[5]);
 }
\ No newline at end of file