Merge pull request #41 from sensebox/feat/auto-detect-acc-sensor

auto detect acceleration sensor

Author: felixerdy

senseBox-bike-atrai-v2/platformio.ini

@@ -23,4 +23,4 @@ lib_deps =
 	adafruit/Adafruit MPU6050@^2.2.6
 	sensebox/SenseBoxBLE@^1.1.0
 	adafruit/Adafruit NeoPixel@^1.12.2
-	; tanakamasayuki/TensorFlowLite_ESP32@^1.0.0
+	invensenseinc/ICM42670P@^1.0.7

senseBox-bike-atrai-v2/src/sensors/AccelerationSensor/AccelerationSensor.cpp

@@ -4,29 +4,34 @@
 AccelerationSensor::AccelerationSensor() : BaseSensor("accelerationSensorTask", 2048, 0) {}
 
 String surfaceClassificationUUID = "B944AF10F4954560968F2F0D18CAB521";
-// String accUUID = "B944AF10F4954560968F2F0D18CAB522";
 String anomalyUUID = "B944AF10F4954560968F2F0D18CAB523";
 int surfaceClassificationCharacteristic = 0;
 int anomalyCharacteristic = 0;
 
-Adafruit_MPU6050 mpu;
-
 void AccelerationSensor::initSensor()
 {
-
-  if (!mpu.begin(0x68, &Wire1))
-  {
-    Serial.println("MPU6050 Chip wurde nicht gefunden");
-    return;
-  }
-  else
+  // Try initializing MPU6050 first
+  if (mpu.begin(0x68, &Wire1))
   {
     Serial.println("MPU6050 Found!");
     mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
     mpu.setGyroRange(MPU6050_RANGE_500_DEG);
     mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
-    delay(100);
-  };
+    activeSensor = MPU6050;
+  }
+  else if (icm.begin() == 0)
+  {
+    // If MPU6050 fails, try ICM42670P
+    Serial.println("ICM42670P Found!");
+    icm.startAccel(21, 8); // Accel ODR = 100 Hz, Full Scale Range = 16G
+    icm.startGyro(21, 500); // Gyro ODR = 100 Hz, Full Scale Range = 2000 dps
+    activeSensor = ICM42670X;
+  }
+  else
+  {
+    Serial.println("No compatible acceleration sensor found");
+    return;
+  }
 
   surfaceClassificationCharacteristic = BLEModule::createCharacteristic(surfaceClassificationUUID.c_str());
   anomalyCharacteristic = BLEModule::createCharacteristic(anomalyUUID.c_str());
@@ -46,25 +51,44 @@ float prevAccTime = millis();
 bool AccelerationSensor::readSensorData()
 {
   bool classified = false;
-  sensors_event_t a, g, temp;
+  
 
-  mpu.getEvent(&a, &g, &temp);
-
-  buffer[ix++] = a.acceleration.x;
-  buffer[ix++] = a.acceleration.y;
-  buffer[ix++] = a.acceleration.z;
-  buffer[ix++] = g.gyro.x;
-  buffer[ix++] = g.gyro.y;
-  buffer[ix++] = g.gyro.z;
+  if (activeSensor == MPU6050)
+  {
+    sensors_event_t a, g, temp;
+    mpu.getEvent(&a, &g, &temp);
+    buffer[ix++] = a.acceleration.x;
+    buffer[ix++] = a.acceleration.y;
+    buffer[ix++] = a.acceleration.z;
+    buffer[ix++] = g.gyro.x;
+    buffer[ix++] = g.gyro.y;
+    buffer[ix++] = g.gyro.z;
+  }
+  else if (activeSensor == ICM42670X)
+  {
+    inv_imu_sensor_event_t imu_event;
+    icm.getDataFromRegisters(imu_event);
+    buffer[ix++] = (imu_event.accel[0]*9.81)/4096.0;
+    buffer[ix++] = (imu_event.accel[1]*9.81)/4096.0;
+    buffer[ix++] = (imu_event.accel[2]*9.81)/4096.0;
+    buffer[ix++] = (imu_event.gyro[0]*2.0)/6550.0;
+    buffer[ix++] = (imu_event.gyro[1]*2.0)/6550.0;
+    buffer[ix++] = (imu_event.gyro[2]*2.0)/6550.0;
+  }
+  else
+  {
+    return false; // No sensor active
+  }
 
   Serial.println(millis() - prevAccTime);
   prevAccTime = millis();
 
-  // one second inverval
+  // one second interval
   if (EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE <= ix)
   {
     classified = true;
-    // Turn the raw buffer in a signal which we can the classify
+
+    // Turn the raw buffer in a signal which we can classify
     signal_t signal;
     int err = numpy::signal_from_buffer(buffer, EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE, &signal);
     if (err != 0)
@@ -76,7 +100,6 @@ bool AccelerationSensor::readSensorData()
 
     // Run the classifier
     ei_impulse_result_t result = {};
-
     err = run_classifier(&signal, &result, false);
     if (err != EI_IMPULSE_OK)
     {
@@ -95,12 +118,10 @@ bool AccelerationSensor::readSensorData()
 
     if (sendBLE)
     {
-
       notifyBLE(probAsphalt, probCompact, probPaving, probSett, probStanding, anomaly);
     }
 
     ix = 0;
-
     buffer[EI_CLASSIFIER_DSP_INPUT_FRAME_SIZE] = {};
   }
 

senseBox-bike-atrai-v2/src/sensors/AccelerationSensor/AccelerationSensor.h

@@ -3,6 +3,7 @@
 
 #include "../BaseSensor.h"
 #include <Adafruit_MPU6050.h>
+#include "ICM42670P.h" // Include for ICM42670P
 
 class AccelerationSensor : public BaseSensor
 {
@@ -13,6 +14,18 @@ class AccelerationSensor : public BaseSensor
 protected:
   void initSensor() override;
   void notifyBLE(float probAsphalt, float probCompact, float probPaving, float probSett, float probStanding, float anomaly);
+
+private:
+  enum SensorType
+  {
+    NONE,
+    MPU6050,
+    ICM42670X // ICM42670X is a placeholder for the ICM42670P
+  };
+
+  SensorType activeSensor = NONE; // Track which sensor is active
+  Adafruit_MPU6050 mpu;
+  ICM42670 icm = ICM42670(Wire1, 0); // Instantiate an ICM42670 with LSB address set to 0
 };
 
-#endif // ACCELERATION_SENSOR_H
+#endif // ACCELERATION_SENSOR_H