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 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

208
src/motionDetection/MotionDetection.cpp
View File

@ -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();
};

2
src/motionDetection/MotionDetection.h
View File

@ -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;