diff --git a/src/motionDetection/MotionDetection.cpp b/src/motionDetection/MotionDetection.cpp index e22fe65..d0e99c1 100644 --- a/src/motionDetection/MotionDetection.cpp +++ b/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)); }; \ No newline at end of file diff --git a/src/motionDetection/MotionDetection.h b/src/motionDetection/MotionDetection.h index 6d9bd5a..7f76b5d 100644 --- a/src/motionDetection/MotionDetection.h +++ b/src/motionDetection/MotionDetection.h @@ -12,6 +12,7 @@ #define MotionDetection_h #include #include +#include "IMU_CMDs.h" struct IMUResult{ int16_t x; int16_t y; @@ -21,40 +22,18 @@ struct IMUResult{ class MotionDetection{ protected: - - 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; - + enum registerBank{MREG1,MREG2,MREG3}; 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 \ No newline at end of file