WIP: Read/Write from Registerbanks

src/motionDetection/MotionDetection.cpp

@@ -12,8 +12,8 @@ void MotionDetection::begin(void){
     handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); 
     digitalWrite(34,LOW);
     // set Accel and Gyroscop to Low Noise
+    handler->transfer(PWR_MGMT0);
     handler->transfer(0x1F);
-    handler->transfer(0x0F);
     //busy Wait for startup 
     delayMicroseconds(250);
     //set Gyroconfig
@@ -22,8 +22,14 @@ void MotionDetection::begin(void){
     //set Gyro Filter
     handler->transfer(0x23);
     handler->transfer(0x37);
+    //Disable FIFO Bypass
+    handler->transfer(FIFO_CONFIG1);
+    handler->transfer(0x00);
     digitalWrite(34,HIGH);
     handler->endTransaction();
+    this->writeToRegisterBank(MREG1,FIFO_CONFIG5,0x23);
+    //Enable Gyro and Acceldata in FIFO
+
 };
 void MotionDetection::end(void){
     handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0));
@@ -71,7 +77,7 @@ uint8_t MotionDetection::cmdWrite(uint8_t reg){
     return (CMD_WRITE | (reg & ADDR_MASK));
 };
 
-int8_t MotionDetection::readRegister(uint8_t reg){
+uint8_t MotionDetection::readRegister(uint8_t reg){
     handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0));
     digitalWrite(34,LOW);
     uint8_t result;
@@ -80,4 +86,99 @@ int8_t MotionDetection::readRegister(uint8_t reg){
      digitalWrite(34,HIGH);
     handler->endTransaction(); 
     return result;
+};
+
+uint8_t MotionDetection::readFromRegisterBank(registerBank bank,uint8_t reg){
+    uint8_t result = 0;
+    handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0));
+    digitalWrite(34,LOW);
+    switch(bank){
+        case(MREG1):
+            result = handler->transfer(BLK_SEL_R);
+            result = handler->transfer(0x00);
+            break;
+        case(MREG2):
+            result = handler->transfer(BLK_SEL_R);
+            result = handler->transfer(0x28);
+            break;
+        case(MREG3):
+            result = handler->transfer(BLK_SEL_R);
+            result = handler->transfer(0x50);
+            break;
+    }
+    result = handler->transfer(MADDR_R);
+    result = handler->transfer(reg);
+    digitalWrite(34,HIGH);
+    handler->endTransaction(); 
+    delayMicroseconds(10);
+     result=this->readRegister(M_R);
+    delayMicroseconds(10);
+    this->resetRegisterBankAccess();
+    return result;
+};
+
+void MotionDetection::writeToRegisterBank(registerBank bank, uint8_t reg, uint8_t value){
+    while((this->readRegister(MCLK_RDY))&0x08!=0x08){
+        Serial.println("CLK not rdy");
+        delay(100);
+    }
+    Serial.print("MADDR_W: ");
+    Serial.println(readRegister(MADDR_W)); 
+    handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0));
+    digitalWrite(34,LOW);
+    switch(bank){
+        case(MREG1):
+            handler->transfer(BLK_SEL_W);
+            handler->transfer(0x00);
+            break;
+        case(MREG2):
+            handler->transfer(BLK_SEL_W);
+            handler->transfer(0x28);
+            break;
+        case(MREG3):
+            handler->transfer(BLK_SEL_W);
+            handler->transfer(0x50);
+            break;
+    }
+    handler->transfer(MADDR_W);
+    handler->transfer(reg);
+    //handler->transfer(reg);
+    handler->transfer(M_W);
+    handler->transfer(value);
+    digitalWrite(34,HIGH);
+    handler->endTransaction();
+    Serial.print("MADDR_W: ");
+    Serial.println(readRegister(MADDR_W));
+    delayMicroseconds(10);
+    this->resetRegisterBankAccess();
+
+};
+
+void MotionDetection::resetRegisterBankAccess(){
+    handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0));
+    digitalWrite(34,LOW);
+    handler->transfer(BLK_SEL_R);
+    handler->transfer(0x00);
+    handler->transfer(BLK_SEL_W);
+    handler->transfer(0x00);
+    handler->transfer(MADDR_R);
+    handler->transfer(0x00);
+    handler->transfer(MADDR_W);
+    handler->transfer(0x00);
+    digitalWrite(34,HIGH);
+    handler->endTransaction();
+};
+
+void MotionDetection::testFIFO(){
+    this->writeToRegisterBank(MREG1,FIFO_CONFIG5,0x23);
+    //Serial.println(this->readFromRegisterBank(MREG1,0x47));
+    //uint16_t fifocount = this->readRegister(FIFO_COUNTH)<<8;
+    //fifocount |= this->readRegister(FIFO_COUNTL);
+    //Serial.print("FiFo Count: ");
+    //Serial.println(fifocount);
+
+    Serial.print("INT_CONFIG1: ");
+    Serial.println(readFromRegisterBank(MREG1,0x05));
+    Serial.print("FiFoData: ");
+    Serial.println(readRegister(0x3F));
 };

src/motionDetection/MotionDetection.h

@@ -12,6 +12,7 @@
 #define MotionDetection_h
 #include <SPI.h>
 #include <Arduino.h>
+#include "IMU_CMDs.h"
 struct IMUResult{
     int16_t x;
     int16_t y;
@@ -21,40 +22,18 @@ struct IMUResult{
 
 class MotionDetection{
 protected:
-  
+    enum registerBank{MREG1,MREG2,MREG3};
-    static const uint8_t CMD_READ =      0x80;
-    static const uint8_t CMD_WRITE =     0x00;
-    static const uint8_t ADDR_MASK =     0x7F;
-
-    //Registers
-    static const uint8_t REG_TEMP_LOW =  0x0A;
-    static const uint8_t REG_TEMP_HIGH = 0X09;
-
-    static const uint8_t ACCEL_DATA_X_HIGH = 0x0B;
-    static const uint8_t ACCEL_DATA_X_LOW =  0x0C;
-    static const uint8_t ACCEL_DATA_Y_HIGH = 0x0D;
-    static const uint8_t ACCEL_DATA_Y_LOW =  0x0E;
-    static const uint8_t ACCEL_DATA_Z_HIGH = 0x0F;
-    static const uint8_t ACCEL_DATA_Z_LOW =  0x10;
-
-    static const uint8_t GYRO_DATA_X_HIGH =  0x11;
-    static const uint8_t GYRO_DATA_X_LOW =   0x12;
-    static const uint8_t GYRO_DATA_Y_HIGH =  0x13;
-    static const uint8_t GYRO_DATA_Y_LOW =   0x14;
-    static const uint8_t GYRO_DATA_Z_HIGH =  0x15;
-    static const uint8_t GYRO_DATA_Z_LOW =   0x16;
-
-    static const uint8_t PWR_MGMT0 =         0x1F;
-    static const uint8_t WHO_AM_I =          0x75;
-
     static const uint frequency = 10000000;
+    uint8_t readFromRegisterBank(registerBank bank,uint8_t reg);
+    void writeToRegisterBank(registerBank bank, uint8_t reg, uint8_t value);
+    void resetRegisterBankAccess();
 
     uint16_t cmdRead(uint8_t regHigh,uint8_t regLow);
     uint16_t cmdWrite(uint8_t regHigh,uint8_t regLow);
     uint8_t cmdRead(uint8_t reg);
     uint8_t cmdWrite(uint8_t reg);
 
-    int8_t readRegister(uint8_t reg);
+    uint8_t readRegister(uint8_t reg);
     int16_t readDoubleRegister(uint8_t lowerReg);
 
     SPIClass * handler = NULL;
@@ -106,5 +85,7 @@ public:
      * @return the value of the whoami register of the ICM-42670 
      */
     int8_t getWhoAmI();
+
+    void testFIFO();
 };
 #endif //MotionDetection