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

implemented access to registerbanks, fixed wrong write access

Browse Source
This commit is contained in:
hhaupt 2024-06-12 20:26:38 +02:00
parent 00c76166f8
commit a058b3f205
3 changed files with 146 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/motionDetection/IMU_CMDs.h
View File

@ -28,6 +28,8 @@
#define PWR_MGMT0 0x1F #define PWR_MGMT0 0x1F
#define WHO_AM_I 0x75 #define WHO_AM_I 0x75
#define INTF_CONFIG0 0x35
#define BLK_SEL_W 0x79 #define BLK_SEL_W 0x79
#define BLK_SEL_R 0x7C #define BLK_SEL_R 0x7C
#define MADDR_W 0x7A #define MADDR_W 0x7A
@ -39,8 +41,9 @@
#define FIFO_COUNTL 0x3E #define FIFO_COUNTL 0x3E
#define FIFO_DATA 0x3F #define FIFO_DATA 0x3F
#define FIFO_CONFIG1 0x28 #define FIFO_CONFIG1 0x28
#define FIFO_CONFIG2 0x29
//MREG1 //MREG1
#define FIFO_CONFIG5 0x01 #define FIFO_CONFIG5 0x01
#define TMST_CONFIG1 0x00
#endif #endif

208
src/motionDetection/MotionDetection.cpp
View File

@ -9,36 +9,39 @@ void MotionDetection::begin(void){
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)); //handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0));
digitalWrite(34,LOW); //digitalWrite(34,LOW);
// set Accel and Gyroscop to Low Noise // set Accel and Gyroscop to Low Noise
handler->transfer(PWR_MGMT0); this->writeRegister(PWR_MGMT0,0x1F);
handler->transfer(0x1F); //handler->transfer(PWR_MGMT0);
//handler->transfer(0x1F);
//busy Wait for startup //busy Wait for startup
delayMicroseconds(250); delayMicroseconds(250);
//set Gyroconfig //set Gyroconfig
handler->transfer(0x20); this->writeRegister(0x20,0x25);
handler->transfer(0x25); //handler->transfer(0x20);
//handler->transfer(0x25);
//set Gyro Filter //set Gyro Filter
handler->transfer(0x23); this->writeRegister(0x23,0x37);
handler->transfer(0x37); //handler->transfer(0x23);
//handler->transfer(0x37);
//Disable FIFO Bypass //Disable FIFO Bypass
handler->transfer(FIFO_CONFIG1); //handler->transfer(FIFO_CONFIG1);
handler->transfer(0x00); //handler->transfer(0x00);
digitalWrite(34,HIGH); //digitalWrite(34,HIGH);
handler->endTransaction(); //handler->endTransaction();
this->writeToRegisterBank(MREG1,FIFO_CONFIG5,0x23);
//Enable Gyro and Acceldata in FIFO //Enable Gyro and Acceldata in FIFO
this->initFIFO();
}; };
void MotionDetection::end(void){ void MotionDetection::end(void){
handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); //handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0));
digitalWrite(34,LOW); //digitalWrite(34,LOW);
handler->transfer(cmdWrite(PWR_MGMT0)); this->writeRegister(PWR_MGMT0,0x00);
//handler->transfer(cmdWrite(PWR_MGMT0));
//turn Accel and Gyroscope off //turn Accel and Gyroscope off
handler->transfer(0x00); //handler->transfer(0x00);
digitalWrite(34,HIGH); //digitalWrite(34,HIGH);
handler->end(); //handler->end();
}; };
IMUResult MotionDetection::getAcceleration(){ IMUResult MotionDetection::getAcceleration(){
IMUResult result; IMUResult result;
@ -90,28 +93,32 @@ uint8_t MotionDetection::readRegister(uint8_t reg){
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)); //handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0));
digitalWrite(34,LOW); //digitalWrite(34,LOW);
switch(bank){ switch(bank){
case(MREG1): case(MREG1):
result = handler->transfer(BLK_SEL_R); this->writeRegister(BLK_SEL_R,0x00);
result = handler->transfer(0x00); //result = handler->transfer(BLK_SEL_R);
//result = handler->transfer(0x00);
break; break;
case(MREG2): case(MREG2):
result = handler->transfer(BLK_SEL_R); this->writeRegister(BLK_SEL_R,0x28);
result = handler->transfer(0x28); //result = handler->transfer(BLK_SEL_R);
//result = handler->transfer(0x28);
break; break;
case(MREG3): case(MREG3):
result = handler->transfer(BLK_SEL_R); this->writeRegister(BLK_SEL_R,0x50);
result = handler->transfer(0x50); //result = handler->transfer(BLK_SEL_R);
//result = handler->transfer(0x50);
break; break;
} }
result = handler->transfer(MADDR_R); this->writeRegister(MADDR_R,reg);
result = handler->transfer(reg); //result = handler->transfer(MADDR_R);
digitalWrite(34,HIGH); //result = handler->transfer(reg);
handler->endTransaction(); //digitalWrite(34,HIGH);
//handler->endTransaction();
delayMicroseconds(10); delayMicroseconds(10);
result=this->readRegister(M_R); result=this->readRegister(M_R);
delayMicroseconds(10); delayMicroseconds(10);
this->resetRegisterBankAccess(); this->resetRegisterBankAccess();
return result; return result;
@ -122,63 +129,128 @@ void MotionDetection::writeToRegisterBank(registerBank bank, uint8_t reg, uint8_
Serial.println("CLK not rdy"); Serial.println("CLK not rdy");
delay(100); delay(100);
} }
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)); //handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0));
digitalWrite(34,LOW); //digitalWrite(34,LOW);
//set Idle Bit
this->writeRegister(PWR_MGMT0,result|0x10);
//handler->transfer(PWR_MGMT0);
//handler->transfer(result|0x10);
switch(bank){ switch(bank){
case(MREG1): case(MREG1):
handler->transfer(BLK_SEL_W); this->writeRegister(BLK_SEL_W,0x00);
handler->transfer(0x00); // handler->transfer(BLK_SEL_W);
// handler->transfer(0x00);
break; break;
case(MREG2): case(MREG2):
handler->transfer(BLK_SEL_W); this->writeRegister(BLK_SEL_W,0x28);
handler->transfer(0x28); //handler->transfer(BLK_SEL_W);
//handler->transfer(0x28);
break; break;
case(MREG3): case(MREG3):
handler->transfer(BLK_SEL_W); this->writeRegister(BLK_SEL_W,0x50);
handler->transfer(0x50); //handler->transfer(BLK_SEL_W);
//handler->transfer(0x50);
break; break;
} }
handler->transfer(MADDR_W); this->writeRegister(MADDR_W,reg);
handler->transfer(reg); //handler->transfer(MADDR_W);
//handler->transfer(reg); //handler->transfer(reg);
handler->transfer(M_W); this->writeRegister(M_W,value);
handler->transfer(value); //handler->transfer(M_W);
digitalWrite(34,HIGH); //handler->transfer(value);
handler->endTransaction(); 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.print("MADDR_W: ");
Serial.println(readRegister(MADDR_W)); Serial.println(readRegister(MADDR_W));
delayMicroseconds(10);
this->resetRegisterBankAccess(); this->resetRegisterBankAccess();
}; };
void MotionDetection::resetRegisterBankAccess(){ void MotionDetection::resetRegisterBankAccess(){
handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); //handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0));
digitalWrite(34,LOW); //digitalWrite(34,LOW);
handler->transfer(BLK_SEL_R); this->writeRegister(BLK_SEL_R,0x00);
handler->transfer(0x00); //handler->transfer(BLK_SEL_R);
handler->transfer(BLK_SEL_W); //handler->transfer(0x00);
handler->transfer(0x00); this->writeRegister(BLK_SEL_W,0x00);
handler->transfer(MADDR_R); //handler->transfer(BLK_SEL_W);
handler->transfer(0x00); //handler->transfer(0x00);
handler->transfer(MADDR_W); this->writeRegister(MADDR_R,0x00);
handler->transfer(0x00); //handler->transfer(MADDR_R);
digitalWrite(34,HIGH); //handler->transfer(0x00);
handler->endTransaction(); 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); //this->writeToRegisterBank(MREG1,FIFO_CONFIG5,0x23);
//Serial.println(this->readFromRegisterBank(MREG1,0x47)); Serial.println(this->readFromRegisterBank(MREG1,FIFO_CONFIG5));
//uint16_t fifocount = this->readRegister(FIFO_COUNTH)<<8; uint16_t fifocount = this->readRegister(FIFO_COUNTH)<<8;
//fifocount |= this->readRegister(FIFO_COUNTL); fifocount |= this->readRegister(FIFO_COUNTL);
//Serial.print("FiFo Count: "); Serial.print("FiFo Count: ");
//Serial.println(fifocount); 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.print("INT_CONFIG1: ");
Serial.println(readFromRegisterBank(MREG1,0x05)); //Serial.println(readFromRegisterBank(MREG1,0x05));
Serial.print("FiFoData: "); Serial.print("FiFoData: ");
Serial.println(readRegister(0x3F)); 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();
}; };

2
src/motionDetection/MotionDetection.h
View File

@ -35,6 +35,8 @@ protected:
uint8_t readRegister(uint8_t reg); uint8_t readRegister(uint8_t reg);
int16_t readDoubleRegister(uint8_t lowerReg); int16_t readDoubleRegister(uint8_t lowerReg);
void writeRegister(uint8_t reg, uint8_t value);
void initFIFO();
SPIClass * handler = NULL; SPIClass * handler = NULL;