diff --git a/src/motionDetection/IMU_CMDs.h b/src/motionDetection/IMU_CMDs.h index cdbe607..ab420c7 100644 --- a/src/motionDetection/IMU_CMDs.h +++ b/src/motionDetection/IMU_CMDs.h @@ -28,6 +28,8 @@ #define PWR_MGMT0 0x1F #define WHO_AM_I 0x75 +#define INTF_CONFIG0 0x35 + #define BLK_SEL_W 0x79 #define BLK_SEL_R 0x7C #define MADDR_W 0x7A @@ -39,8 +41,9 @@ #define FIFO_COUNTL 0x3E #define FIFO_DATA 0x3F #define FIFO_CONFIG1 0x28 - +#define FIFO_CONFIG2 0x29 //MREG1 #define FIFO_CONFIG5 0x01 +#define TMST_CONFIG1 0x00 #endif \ No newline at end of file diff --git a/src/motionDetection/MotionDetection.cpp b/src/motionDetection/MotionDetection.cpp index d0e99c1..cdc4ab7 100644 --- a/src/motionDetection/MotionDetection.cpp +++ b/src/motionDetection/MotionDetection.cpp @@ -9,36 +9,39 @@ void MotionDetection::begin(void){ digitalWrite(34,HIGH); handler->begin(36,37,35,34); - handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); - digitalWrite(34,LOW); + //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); + this->writeRegister(PWR_MGMT0,0x1F); + //handler->transfer(PWR_MGMT0); + //handler->transfer(0x1F); //busy Wait for startup delayMicroseconds(250); //set Gyroconfig - handler->transfer(0x20); - handler->transfer(0x25); + this->writeRegister(0x20,0x25); + //handler->transfer(0x20); + //handler->transfer(0x25); //set Gyro Filter - handler->transfer(0x23); - handler->transfer(0x37); + this->writeRegister(0x23,0x37); + //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); + //handler->transfer(FIFO_CONFIG1); + //handler->transfer(0x00); + //digitalWrite(34,HIGH); + //handler->endTransaction(); //Enable Gyro and Acceldata in FIFO - + this->initFIFO(); }; void MotionDetection::end(void){ - handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); - digitalWrite(34,LOW); - handler->transfer(cmdWrite(PWR_MGMT0)); + //handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); + //digitalWrite(34,LOW); + this->writeRegister(PWR_MGMT0,0x00); + //handler->transfer(cmdWrite(PWR_MGMT0)); //turn Accel and Gyroscope off - handler->transfer(0x00); - digitalWrite(34,HIGH); - handler->end(); + //handler->transfer(0x00); + //digitalWrite(34,HIGH); + //handler->end(); }; IMUResult MotionDetection::getAcceleration(){ IMUResult result; @@ -90,28 +93,32 @@ uint8_t MotionDetection::readRegister(uint8_t reg){ uint8_t MotionDetection::readFromRegisterBank(registerBank bank,uint8_t reg){ uint8_t result = 0; - handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); - digitalWrite(34,LOW); + //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); + this->writeRegister(BLK_SEL_R,0x00); + //result = handler->transfer(BLK_SEL_R); + //result = handler->transfer(0x00); break; case(MREG2): - result = handler->transfer(BLK_SEL_R); - result = handler->transfer(0x28); + this->writeRegister(BLK_SEL_R,0x28); + //result = handler->transfer(BLK_SEL_R); + //result = handler->transfer(0x28); break; case(MREG3): - result = handler->transfer(BLK_SEL_R); - result = handler->transfer(0x50); + this->writeRegister(BLK_SEL_R,0x50); + //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(); + this->writeRegister(MADDR_R,reg); + //result = handler->transfer(MADDR_R); + //result = handler->transfer(reg); + //digitalWrite(34,HIGH); + //handler->endTransaction(); delayMicroseconds(10); - result=this->readRegister(M_R); + result=this->readRegister(M_R); delayMicroseconds(10); this->resetRegisterBankAccess(); return result; @@ -122,63 +129,128 @@ void MotionDetection::writeToRegisterBank(registerBank bank, uint8_t reg, uint8_ Serial.println("CLK not rdy"); delay(100); } + uint8_t result = this->readRegister(PWR_MGMT0); Serial.print("MADDR_W: "); Serial.println(readRegister(MADDR_W)); - handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); - digitalWrite(34,LOW); + //handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); + //digitalWrite(34,LOW); + //set Idle Bit + this->writeRegister(PWR_MGMT0,result|0x10); + //handler->transfer(PWR_MGMT0); + //handler->transfer(result|0x10); switch(bank){ case(MREG1): - handler->transfer(BLK_SEL_W); - handler->transfer(0x00); + this->writeRegister(BLK_SEL_W,0x00); + // handler->transfer(BLK_SEL_W); + // handler->transfer(0x00); break; case(MREG2): - handler->transfer(BLK_SEL_W); - handler->transfer(0x28); + this->writeRegister(BLK_SEL_W,0x28); + //handler->transfer(BLK_SEL_W); + //handler->transfer(0x28); break; case(MREG3): - handler->transfer(BLK_SEL_W); - handler->transfer(0x50); + this->writeRegister(BLK_SEL_W,0x50); + //handler->transfer(BLK_SEL_W); + //handler->transfer(0x50); break; } - handler->transfer(MADDR_W); - handler->transfer(reg); + this->writeRegister(MADDR_W,reg); + //handler->transfer(MADDR_W); //handler->transfer(reg); - handler->transfer(M_W); - handler->transfer(value); - digitalWrite(34,HIGH); - handler->endTransaction(); + this->writeRegister(M_W,value); + //handler->transfer(M_W); + //handler->transfer(value); + delayMicroseconds(10); + this->writeRegister(PWR_MGMT0,result&0xEF); + //handler->transfer(PWR_MGMT0); + //handler->transfer(result&0xEF); + //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(); + //handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); + //digitalWrite(34,LOW); + this->writeRegister(BLK_SEL_R,0x00); + //handler->transfer(BLK_SEL_R); + //handler->transfer(0x00); + this->writeRegister(BLK_SEL_W,0x00); + //handler->transfer(BLK_SEL_W); + //handler->transfer(0x00); + this->writeRegister(MADDR_R,0x00); + //handler->transfer(MADDR_R); + //handler->transfer(0x00); + this->writeRegister(MADDR_W,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); + //this->writeToRegisterBank(MREG1,FIFO_CONFIG5,0x23); + Serial.println(this->readFromRegisterBank(MREG1,FIFO_CONFIG5)); + uint16_t fifocount = this->readRegister(FIFO_COUNTH)<<8; + fifocount |= this->readRegister(FIFO_COUNTL); + Serial.print("FiFo Count: "); + Serial.println(fifocount); + uint16_t fifocount = this->readRegister(FIFO_COUNTH)<<8; + fifocount |= this->readRegister(FIFO_COUNTL); + Serial.print("FiFo Count: "); + Serial.println(fifocount); + 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("INT_CONFIG1: "); + //Serial.println(readFromRegisterBank(MREG1,0x05)); Serial.print("FiFoData: "); Serial.println(readRegister(0x3F)); +}; + +void MotionDetection::initFIFO(){ + + //handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); + //digitalWrite(34,LOW); + + //set INTF_CONFIG0 FIFO_COUNT_REC_RECORD und Little Endian + this->writeRegister(INTF_CONFIG0,0x70); + //handler->transfer(INTF_CONFIG0); + //handler->transfer(0x70); + //set FIFO_CONFIG1 to Mode Snapshot and BYPASS Off + this->writeRegister(FIFO_CONFIG1,0x02); + //handler->transfer(FIFO_CONFIG1); + //handler->transfer(0x02); + //set TMST_CONFIG1_MREG1 TMST_CONFIIG1_TMST_EN + //digitalWrite(34,HIGH); + //handler->endTransaction(); + this->writeToRegisterBank(MREG1,TMST_CONFIG1,0x03); + //set FiFO config 5 GYRO_EN,TMST_FSYNC, ACCEL_EN, WM_GT_TH_EN + this->writeToRegisterBank(MREG1,FIFO_CONFIG5,0x27); + //set FOF_CONFIG2 0x1 (INT triggerd each packaged) + //handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); + //digitalWrite(34,LOW); + this->writeRegister(FIFO_CONFIG2,0x0A); + //handler->transfer(FIFO_CONFIG2); + //handler->transfer(0x0A); + //disable Data Read Interrupt + //digitalWrite(34,HIGH); + //handler->endTransaction(); + +}; + +void MotionDetection::writeRegister(uint8_t reg, uint8_t value){ + handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); + digitalWrite(34,LOW); + handler->transfer(reg); + handler->transfer(value); + delayMicroseconds(10); + digitalWrite(34,HIGH); + handler->endTransaction(); }; \ No newline at end of file diff --git a/src/motionDetection/MotionDetection.h b/src/motionDetection/MotionDetection.h index 7f76b5d..f668d79 100644 --- a/src/motionDetection/MotionDetection.h +++ b/src/motionDetection/MotionDetection.h @@ -35,6 +35,8 @@ protected: uint8_t readRegister(uint8_t reg); int16_t readDoubleRegister(uint8_t lowerReg); + void writeRegister(uint8_t reg, uint8_t value); + void initFIFO(); SPIClass * handler = NULL;