From 841202897b48ded0d7640643fa9461c09ff30a10 Mon Sep 17 00:00:00 2001 From: hhaupt Date: Thu, 13 Jun 2024 04:17:12 +0200 Subject: [PATCH] Fixed FiFo access --- src/motionDetection/MotionDetection.cpp | 141 ++++++------------------ src/motionDetection/MotionDetection.h | 1 + 2 files changed, 35 insertions(+), 107 deletions(-) diff --git a/src/motionDetection/MotionDetection.cpp b/src/motionDetection/MotionDetection.cpp index cdc4ab7..03c650c 100644 --- a/src/motionDetection/MotionDetection.cpp +++ b/src/motionDetection/MotionDetection.cpp @@ -8,40 +8,21 @@ void MotionDetection::begin(void){ pinMode(34,OUTPUT); digitalWrite(34,HIGH); handler->begin(36,37,35,34); - - //handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); - //digitalWrite(34,LOW); // set Accel and Gyroscop to Low Noise this->writeRegister(PWR_MGMT0,0x1F); - //handler->transfer(PWR_MGMT0); - //handler->transfer(0x1F); //busy Wait for startup delayMicroseconds(250); + //set accelconfig + this->writeRegister(0x21,0x05); //set Gyroconfig this->writeRegister(0x20,0x25); - //handler->transfer(0x20); - //handler->transfer(0x25); //set Gyro Filter 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(); //Enable Gyro and Acceldata in FIFO this->initFIFO(); }; void MotionDetection::end(void){ - //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(); }; IMUResult MotionDetection::getAcceleration(){ IMUResult result; @@ -86,37 +67,25 @@ uint8_t MotionDetection::readRegister(uint8_t reg){ uint8_t result; result = handler->transfer(cmdRead(reg)); result = handler->transfer(0x00); - digitalWrite(34,HIGH); + 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): this->writeRegister(BLK_SEL_R,0x00); - //result = handler->transfer(BLK_SEL_R); - //result = handler->transfer(0x00); break; case(MREG2): this->writeRegister(BLK_SEL_R,0x28); - //result = handler->transfer(BLK_SEL_R); - //result = handler->transfer(0x28); break; case(MREG3): this->writeRegister(BLK_SEL_R,0x50); - //result = handler->transfer(BLK_SEL_R); - //result = handler->transfer(0x50); break; } 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); delayMicroseconds(10); @@ -132,117 +101,75 @@ void MotionDetection::writeToRegisterBank(registerBank bank, uint8_t reg, uint8_ 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); //set Idle Bit this->writeRegister(PWR_MGMT0,result|0x10); - //handler->transfer(PWR_MGMT0); - //handler->transfer(result|0x10); switch(bank){ case(MREG1): this->writeRegister(BLK_SEL_W,0x00); - // handler->transfer(BLK_SEL_W); - // handler->transfer(0x00); break; case(MREG2): this->writeRegister(BLK_SEL_W,0x28); - //handler->transfer(BLK_SEL_W); - //handler->transfer(0x28); break; case(MREG3): this->writeRegister(BLK_SEL_W,0x50); - //handler->transfer(BLK_SEL_W); - //handler->transfer(0x50); break; } this->writeRegister(MADDR_W,reg); - //handler->transfer(MADDR_W); - //handler->transfer(reg); 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)); this->resetRegisterBankAccess(); - }; void MotionDetection::resetRegisterBankAccess(){ - //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,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)); + uint16_t fifocount; + while(fifocount < 20){ + fifocount = 0; + fifocount = (this->readRegister(FIFO_COUNTH)<<8); + fifocount |= this->readRegister(FIFO_COUNTL); + Serial.print("FiFo Count: "); + Serial.println(fifocount); + } Serial.print("FiFoData: "); - Serial.println(readRegister(0x3F)); -}; + handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); + digitalWrite(34,LOW); + handler->transfer(cmdRead(0x3f)); + handler->transfer(buf,8*fifocount); + //delayMicroseconds(10); + digitalWrite(34,HIGH); + handler->endTransaction(); + Serial.println("============="); + + writeRegister(0x02,0x04); + delayMicroseconds(10); + fifocount = 0; + fifocount = (this->readRegister(FIFO_COUNTH)<<8); + fifocount |= this->readRegister(FIFO_COUNTL); + Serial.print("FiFo Count: "); + Serial.println(fifocount); +} void MotionDetection::initFIFO(){ - - //handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); - //digitalWrite(34,LOW); - + delay(60); //set INTF_CONFIG0 FIFO_COUNT_REC_RECORD und Little Endian - this->writeRegister(INTF_CONFIG0,0x70); - //handler->transfer(INTF_CONFIG0); - //handler->transfer(0x70); + this->writeRegister(INTF_CONFIG0,0x60); //set FIFO_CONFIG1 to Mode Snapshot and BYPASS Off - this->writeRegister(FIFO_CONFIG1,0x02); - //handler->transfer(FIFO_CONFIG1); - //handler->transfer(0x02); + this->writeRegister(FIFO_CONFIG1,0x00); //set TMST_CONFIG1_MREG1 TMST_CONFIIG1_TMST_EN - //digitalWrite(34,HIGH); - //handler->endTransaction(); - this->writeToRegisterBank(MREG1,TMST_CONFIG1,0x03); + this->writeToRegisterBank(MREG1,TMST_CONFIG1,0x00); //set FiFO config 5 GYRO_EN,TMST_FSYNC, ACCEL_EN, WM_GT_TH_EN - this->writeToRegisterBank(MREG1,FIFO_CONFIG5,0x27); + this->writeToRegisterBank(MREG1,FIFO_CONFIG5,0x21); //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){ @@ -250,7 +177,7 @@ void MotionDetection::writeRegister(uint8_t reg, uint8_t value){ digitalWrite(34,LOW); handler->transfer(reg); handler->transfer(value); - delayMicroseconds(10); digitalWrite(34,HIGH); + delayMicroseconds(10); handler->endTransaction(); }; \ No newline at end of file diff --git a/src/motionDetection/MotionDetection.h b/src/motionDetection/MotionDetection.h index f668d79..3f5664f 100644 --- a/src/motionDetection/MotionDetection.h +++ b/src/motionDetection/MotionDetection.h @@ -24,6 +24,7 @@ class MotionDetection{ protected: enum registerBank{MREG1,MREG2,MREG3}; static const uint frequency = 10000000; + int8_t* buf = new int8_t[16*200]; uint8_t readFromRegisterBank(registerBank bank,uint8_t reg); void writeToRegisterBank(registerBank bank, uint8_t reg, uint8_t value); void resetRegisterBankAccess();