phillip/dezibot
1
0
Fork 0
mirror of https://gitlab.dit.htwk-leipzig.de/phillip.kuehne/dezibot.git synced 2025-05-21 20:11:46 +02:00
Code Issues Packages Projects Releases Wiki Activity

Fixed FiFo access

Browse Source
This commit is contained in:
hhaupt 2024-06-13 04:17:12 +02:00
parent a058b3f205
commit 841202897b
2 changed files with 35 additions and 107 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

141
src/motionDetection/MotionDetection.cpp
View File

@ -8,40 +8,21 @@ void MotionDetection::begin(void){
pinMode(34,OUTPUT); pinMode(34,OUTPUT);
digitalWrite(34,HIGH); digitalWrite(34,HIGH);
handler->begin(36,37,35,34); handler->begin(36,37,35,34);
//handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0));
//digitalWrite(34,LOW);
// set Accel and Gyroscop to Low Noise // set Accel and Gyroscop to Low Noise
this->writeRegister(PWR_MGMT0,0x1F); this->writeRegister(PWR_MGMT0,0x1F);
//handler->transfer(PWR_MGMT0);
//handler->transfer(0x1F);
//busy Wait for startup //busy Wait for startup
delayMicroseconds(250); delayMicroseconds(250);
//set accelconfig
this->writeRegister(0x21,0x05);
//set Gyroconfig //set Gyroconfig
this->writeRegister(0x20,0x25); this->writeRegister(0x20,0x25);
//handler->transfer(0x20);
//handler->transfer(0x25);
//set Gyro Filter //set Gyro Filter
this->writeRegister(0x23,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();
//Enable Gyro and Acceldata in FIFO //Enable Gyro and Acceldata in FIFO
this->initFIFO(); this->initFIFO();
}; };
void MotionDetection::end(void){ void MotionDetection::end(void){
//handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0));
//digitalWrite(34,LOW);
this->writeRegister(PWR_MGMT0,0x00); 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 MotionDetection::getAcceleration(){
IMUResult result; IMUResult result;
@ -86,37 +67,25 @@ uint8_t MotionDetection::readRegister(uint8_t reg){
uint8_t result; uint8_t result;
result = handler->transfer(cmdRead(reg)); result = handler->transfer(cmdRead(reg));
result = handler->transfer(0x00); result = handler->transfer(0x00);
digitalWrite(34,HIGH); digitalWrite(34,HIGH);
handler->endTransaction(); handler->endTransaction();
return result; return result;
}; };
uint8_t MotionDetection::readFromRegisterBank(registerBank bank,uint8_t reg){ uint8_t MotionDetection::readFromRegisterBank(registerBank bank,uint8_t reg){
uint8_t result = 0; uint8_t result = 0;
//handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0));
//digitalWrite(34,LOW);
switch(bank){ switch(bank){
case(MREG1): case(MREG1):
this->writeRegister(BLK_SEL_R,0x00); this->writeRegister(BLK_SEL_R,0x00);
//result = handler->transfer(BLK_SEL_R);
//result = handler->transfer(0x00);
break; break;
case(MREG2): case(MREG2):
this->writeRegister(BLK_SEL_R,0x28); this->writeRegister(BLK_SEL_R,0x28);
//result = handler->transfer(BLK_SEL_R);
//result = handler->transfer(0x28);
break; break;
case(MREG3): case(MREG3):
this->writeRegister(BLK_SEL_R,0x50); this->writeRegister(BLK_SEL_R,0x50);
//result = handler->transfer(BLK_SEL_R);
//result = handler->transfer(0x50);
break; break;
} }
this->writeRegister(MADDR_R,reg); this->writeRegister(MADDR_R,reg);
//result = handler->transfer(MADDR_R);
//result = handler->transfer(reg);
//digitalWrite(34,HIGH);
//handler->endTransaction();
delayMicroseconds(10); delayMicroseconds(10);
result=this->readRegister(M_R); result=this->readRegister(M_R);
delayMicroseconds(10); delayMicroseconds(10);
@ -132,117 +101,75 @@ void MotionDetection::writeToRegisterBank(registerBank bank, uint8_t reg, uint8_
uint8_t result = this->readRegister(PWR_MGMT0); uint8_t result = this->readRegister(PWR_MGMT0);
Serial.print("MADDR_W: "); Serial.print("MADDR_W: ");
Serial.println(readRegister(MADDR_W)); Serial.println(readRegister(MADDR_W));
//handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0));
//digitalWrite(34,LOW);
//set Idle Bit //set Idle Bit
this->writeRegister(PWR_MGMT0,result|0x10); this->writeRegister(PWR_MGMT0,result|0x10);
//handler->transfer(PWR_MGMT0);
//handler->transfer(result|0x10);
switch(bank){ switch(bank){
case(MREG1): case(MREG1):
this->writeRegister(BLK_SEL_W,0x00); this->writeRegister(BLK_SEL_W,0x00);
// handler->transfer(BLK_SEL_W);
// handler->transfer(0x00);
break; break;
case(MREG2): case(MREG2):
this->writeRegister(BLK_SEL_W,0x28); this->writeRegister(BLK_SEL_W,0x28);
//handler->transfer(BLK_SEL_W);
//handler->transfer(0x28);
break; break;
case(MREG3): case(MREG3):
this->writeRegister(BLK_SEL_W,0x50); this->writeRegister(BLK_SEL_W,0x50);
//handler->transfer(BLK_SEL_W);
//handler->transfer(0x50);
break; break;
} }
this->writeRegister(MADDR_W,reg); this->writeRegister(MADDR_W,reg);
//handler->transfer(MADDR_W);
//handler->transfer(reg);
this->writeRegister(M_W,value); this->writeRegister(M_W,value);
//handler->transfer(M_W);
//handler->transfer(value);
delayMicroseconds(10); delayMicroseconds(10);
this->writeRegister(PWR_MGMT0,result&0xEF); this->writeRegister(PWR_MGMT0,result&0xEF);
//handler->transfer(PWR_MGMT0);
//handler->transfer(result&0xEF);
//digitalWrite(34,HIGH);
//handler->endTransaction();
Serial.print("MADDR_W: "); Serial.print("MADDR_W: ");
Serial.println(readRegister(MADDR_W)); Serial.println(readRegister(MADDR_W));
this->resetRegisterBankAccess(); this->resetRegisterBankAccess();
}; };
void MotionDetection::resetRegisterBankAccess(){ void MotionDetection::resetRegisterBankAccess(){
//handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0));
//digitalWrite(34,LOW);
this->writeRegister(BLK_SEL_R,0x00); this->writeRegister(BLK_SEL_R,0x00);
//handler->transfer(BLK_SEL_R);
//handler->transfer(0x00);
this->writeRegister(BLK_SEL_W,0x00); this->writeRegister(BLK_SEL_W,0x00);
//handler->transfer(BLK_SEL_W);
//handler->transfer(0x00);
this->writeRegister(MADDR_R,0x00); this->writeRegister(MADDR_R,0x00);
//handler->transfer(MADDR_R);
//handler->transfer(0x00);
this->writeRegister(MADDR_W,0x00); this->writeRegister(MADDR_W,0x00);
//handler->transfer(MADDR_W);
//handler->transfer(0x00);
//digitalWrite(34,HIGH);
//handler->endTransaction();
}; };
void MotionDetection::testFIFO(){ void MotionDetection::testFIFO(){
//this->writeToRegisterBank(MREG1,FIFO_CONFIG5,0x23); uint16_t fifocount;
Serial.println(this->readFromRegisterBank(MREG1,FIFO_CONFIG5)); while(fifocount < 20){
uint16_t fifocount = this->readRegister(FIFO_COUNTH)<<8; fifocount = 0;
fifocount |= this->readRegister(FIFO_COUNTL); fifocount = (this->readRegister(FIFO_COUNTH)<<8);
Serial.print("FiFo Count: "); fifocount |= this->readRegister(FIFO_COUNTL);
Serial.println(fifocount); Serial.print("FiFo Count: ");
uint16_t fifocount = this->readRegister(FIFO_COUNTH)<<8; Serial.println(fifocount);
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("FiFoData: "); 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(){ void MotionDetection::initFIFO(){
delay(60);
//handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0));
//digitalWrite(34,LOW);
//set INTF_CONFIG0 FIFO_COUNT_REC_RECORD und Little Endian //set INTF_CONFIG0 FIFO_COUNT_REC_RECORD und Little Endian
this->writeRegister(INTF_CONFIG0,0x70); this->writeRegister(INTF_CONFIG0,0x60);
//handler->transfer(INTF_CONFIG0);
//handler->transfer(0x70);
//set FIFO_CONFIG1 to Mode Snapshot and BYPASS Off //set FIFO_CONFIG1 to Mode Snapshot and BYPASS Off
this->writeRegister(FIFO_CONFIG1,0x02); this->writeRegister(FIFO_CONFIG1,0x00);
//handler->transfer(FIFO_CONFIG1);
//handler->transfer(0x02);
//set TMST_CONFIG1_MREG1 TMST_CONFIIG1_TMST_EN //set TMST_CONFIG1_MREG1 TMST_CONFIIG1_TMST_EN
//digitalWrite(34,HIGH); this->writeToRegisterBank(MREG1,TMST_CONFIG1,0x00);
//handler->endTransaction();
this->writeToRegisterBank(MREG1,TMST_CONFIG1,0x03);
//set FiFO config 5 GYRO_EN,TMST_FSYNC, ACCEL_EN, WM_GT_TH_EN //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) //set FOF_CONFIG2 0x1 (INT triggerd each packaged)
//handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0));
//digitalWrite(34,LOW);
this->writeRegister(FIFO_CONFIG2,0x0A); 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){ 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); digitalWrite(34,LOW);
handler->transfer(reg); handler->transfer(reg);
handler->transfer(value); handler->transfer(value);
delayMicroseconds(10);
digitalWrite(34,HIGH); digitalWrite(34,HIGH);
delayMicroseconds(10);
handler->endTransaction(); handler->endTransaction();
}; };

1
src/motionDetection/MotionDetection.h
View File

@ -24,6 +24,7 @@ class MotionDetection{
protected: protected:
enum registerBank{MREG1,MREG2,MREG3}; enum registerBank{MREG1,MREG2,MREG3};
static const uint frequency = 10000000; static const uint frequency = 10000000;
int8_t* buf = new int8_t[16*200];
uint8_t readFromRegisterBank(registerBank bank,uint8_t reg); uint8_t readFromRegisterBank(registerBank bank,uint8_t reg);
void writeToRegisterBank(registerBank bank, uint8_t reg, uint8_t value); void writeToRegisterBank(registerBank bank, uint8_t reg, uint8_t value);
void resetRegisterBankAccess(); void resetRegisterBankAccess();