Added i2cScan and ICM-42670P whoami

src/main.cpp

@@ -1,3 +1,6 @@
+// #define MAG
+#define IMU
+
 #include <Arduino.h>
 #include <AsyncElegantOTA.h>
 
@@ -15,22 +18,22 @@
 #include <ESPAsyncWebServer.h>
 #include <SPIFFSEditor.h>
 #include <Arduino_JSON.h>
+#ifdef MAG
 #include <Adafruit_Sensor.h>
 #include <Adafruit_BusIO_Register.h>
-// #include <Adafruit_MMC56x3.h>
-#include <ICM42670P.h>
-
+#include <Adafruit_MMC56x3.h>
+#endif
 
 #include <Preferences.h>
 Preferences settings;
 
-#define LED_PIN 2
+#define LED_PIN 8
 
 /* Assign a unique ID to this sensor at the same time */
-// Adafruit_MMC5603 mag = Adafruit_MMC5603(12345);
-
-/* ICM42670P*/
-ICM42670P imu(Wire,0);
+#ifdef MAG
+Adafruit_MMC5603 mag = Adafruit_MMC5603(0);
+String magData;
+#endif
 
 //*********************************************************
 // Web server variable declarations
@@ -74,6 +77,8 @@ void onWsEvent(AsyncWebSocket * server, AsyncWebSocketClient * client, AwsEventT
 void initSPIFFS();
 bool initWiFi();
 String getLEDState();
+bool initIMU();
+void i2cScan();
 // ----------------------------------------------
 void portScan(){
 byte error, address;
@@ -123,40 +128,29 @@ void setup() {
   while (!Serial)
         delay(10); // will pause Zero, Leonardo, etc until serial console opens
 
-    if (!Wire.begin(4,5)) {
-        while(1){
-            Serial.println("WIRE NOT INIT");
-            delay(500);
-        }
-    }
-  
-  portScan();
-
-/* Initialise the sensor */
-    // Serial.println("Adafruit_MMC5603 Magnetometer Calibration");
-    // Serial.println("");
-  // if (!mag.begin(MMC56X3_DEFAULT_ADDRESS, &Wire)) {  // I2C mode
-  //   /* There was a problem detecting the MMC5603 ... check your connections */
-  //   Serial.println("Ooops, no MMC5603 detected ... Check your wiring!");
-  //   while (1) delay(10);
-  // }
-
-
- // Initializing the ICM42670P
- int ret = imu.begin();
-  if (ret != 0) {
-    Serial.print("ICM42670P initialization failed: ");
-    Serial.println(ret);
-    while(1);
+  if (!Wire.begin(4,5,400000)) {
+      while(1){
+          Serial.println("WIRE NOT INIT");
+          delay(500);
+      }
   }
-  // Accel ODR = 100 Hz and Full Scale Range = 16G
-  imu.startAccel(100,16);
-  // Gyro ODR = 100 Hz and Full Scale Range = 2000 dps
-  imu.startGyro(100,2000);
-  // Wait IMU to start
-  delay(100);
 
+  i2cScan();
+  Serial.println("init IMU");
+  Serial.println("");
+  initIMU();
+
+
+/* Initialise the mag sensor */
+#ifdef MAG
+  if (!mag.begin(MMC56X3_DEFAULT_ADDRESS, &Wire)) {  // I2C mode
+    /* There was a problem detecting the MMC5603 ... check your connections */
+    Serial.println("Ooops, no MMC5603 detected ... Check your wiring!");
+    while (1) delay(10);
+  }
+#endif
 //*****************************************
+
 //Init OTA
   AsyncElegantOTA.begin(&server);    // Start AsyncElegantOTA
 
@@ -357,31 +351,31 @@ server.addHandler(new SPIFFSEditor(SPIFFS,http_username,http_password));
     server.begin();
 
 }
+
+#ifdef MAG
+String readMAG() {
+    sensors_event_t event;
+    mag.getEvent(&event);
+    magData = "Mag: X";
+    magData += String(event.magnetic.x);
+    magData += " Y";
+    magData += String(event.magnetic.x);
+    magData += " Z";
+    magData += String(event.magnetic.x);
+    magData += "\n";
+    return magData;
+}
+#endif
+
 void loop() {
-  inv_imu_sensor_event_t imu_event;
-
-  // Get last event
-  imu.getDataFromRegisters(&imu_event);
-
-  // Format data for Serial Plotter
-  Serial.print("AccelX:");
-  Serial.println(imu_event.accel[0]);
-  Serial.print("AccelY:");
-  Serial.println(imu_event.accel[1]);
-  Serial.print("AccelZ:");
-  Serial.println(imu_event.accel[2]);
-  Serial.print("GyroX:");
-  Serial.println(imu_event.gyro[0]);
-  Serial.print("GyroY:");
-  Serial.println(imu_event.gyro[1]);
-  Serial.print("GyroZ:");
-  Serial.println(imu_event.gyro[2]);
-  Serial.print("Temperature:");
-  Serial.println(imu_event.temperature);
-
-  // Run @ ODR 100Hz
-  delay(100);
-    }
+#ifdef MAG
+  Serial.println(readMAG());
+#endif
+  digitalWrite(LED_PIN,HIGH);
+  delay(500);
+  digitalWrite(LED_PIN,LOW);
+  delay(500);
+}
 
 // functions
 void onWsEvent(AsyncWebSocket * server, AsyncWebSocketClient * client, AwsEventType type, void * arg, uint8_t *data, size_t len){
@@ -558,3 +552,58 @@ String getLEDState(const String& var) {
   }
   return String();
 }
+
+bool initIMU(){
+  bool isOK=false;
+  int out = 0;
+  Wire.beginTransmission(0x68);
+  Wire.write(0x75);
+  Wire.endTransmission();
+  Wire.requestFrom(0x68,1);
+  if(Wire.available()){
+    out = Wire.read();
+    isOK = true;
+  }
+  Serial.println(out);
+  return isOK;
+}
+
+void i2cScan(){
+ byte error, address;
+  int nDevices;
+
+  Serial.println("Scanning...");
+
+  nDevices = 0;
+  for(address = 100; address < 127; address++ )
+  {
+    Serial.println(address);
+    // The i2c_scanner uses the return value of
+    // the Write.endTransmisstion to see if
+    // a device did acknowledge to the address.
+    Wire.beginTransmission(address);
+    error = Wire.endTransmission();
+
+    if (error == 0)
+    {
+      Serial.print("I2C device found at address 0x");
+      if (address<16)
+        Serial.print("0");
+      Serial.print(address,HEX);
+      Serial.println("  !");
+
+      nDevices++;
+    }
+    else if (error==4)
+    {
+      Serial.print("Unknown error at address 0x");
+      if (address<16)
+        Serial.print("0");
+      Serial.println(address,HEX);
+    }
+  }
+  if (nDevices == 0)
+    Serial.println("No I2C devices found\n");
+  else
+    Serial.println("done\n");  
+}