Skip to content

Commit

Permalink
Add files via upload
Browse files Browse the repository at this point in the history
  • Loading branch information
@wollewald
wollewald authored Oct 4, 2022
1 parent 2da7ce4 commit 9fc065c
Show file tree
Hide file tree
Showing 4 changed files with 38 additions and 53 deletions.
45 changes: 19 additions & 26 deletions src/MPU6500_WE.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,13 +83,13 @@ float constexpr MPU6500_WE::WHO_AM_I_CODE ;

/************ Constructors ************/

MPU6500_WE::MPU6500_WE(int addr)
MPU6500_WE::MPU6500_WE(uint8_t addr)
: MPU6500_WE(&Wire, addr)
{
// intentionally empty
}

MPU6500_WE::MPU6500_WE(TwoWire *w, int addr)
MPU6500_WE::MPU6500_WE(TwoWire *w, uint8_t addr)
: _wire(w)
, i2cAddress(addr)
{
Expand Down Expand Up @@ -268,10 +268,11 @@ void MPU6500_WE::setSPIClockSpeed(unsigned long clock){
/************* x,y,z results *************/

xyzFloat MPU6500_WE::getAccRawValues(){
uint64_t const xyzDataReg = readMPU9250Register3x16(REGISTER_ACCEL_OUT);
int16_t const xRaw = static_cast<int16_t>((xyzDataReg >> 32) & 0xFFFF);
int16_t const yRaw = static_cast<int16_t>((xyzDataReg >> 16) & 0xFFFF);
int16_t const zRaw = static_cast<int16_t>(xyzDataReg & 0xFFFF);
uint8_t rawData[6];
readMPU9250Register3x16(REGISTER_ACCEL_OUT, rawData);
int16_t const xRaw = static_cast<int16_t>((rawData[0] << 8) | rawData[1]);
int16_t const yRaw = static_cast<int16_t>((rawData[2] << 8) | rawData[3]);
int16_t const zRaw = static_cast<int16_t>((rawData[4] << 8) | rawData[5]);
return xyzFloat{static_cast<float>(xRaw), static_cast<float>(yRaw), static_cast<float>(zRaw)};
}

Expand Down Expand Up @@ -302,8 +303,6 @@ xyzFloat MPU6500_WE::getGValuesFromFifo(){
return accRawVal * (static_cast<float>(accRangeFactor) / 16384.0f);
}



float MPU6500_WE::getResultantG(xyzFloat gVal){
float resultant = 0.0;
resultant = sqrt(sq(gVal.x) + sq(gVal.y) + sq(gVal.z));
Expand All @@ -318,10 +317,11 @@ float MPU6500_WE::getTemperature(){
}

xyzFloat MPU6500_WE::getGyrRawValues(){
uint64_t xyzDataReg = readMPU9250Register3x16(REGISTER_GYRO_OUT);
int16_t xRaw = (int16_t)((xyzDataReg >> 32) & 0xFFFF);
int16_t yRaw = (int16_t)((xyzDataReg >> 16) & 0xFFFF);
int16_t zRaw = (int16_t)(xyzDataReg & 0xFFFF);
uint8_t rawData[6];
readMPU9250Register3x16(REGISTER_GYRO_OUT, rawData);
int16_t const xRaw = static_cast<int16_t>((rawData[0] << 8) | rawData[1]);
int16_t const yRaw = static_cast<int16_t>((rawData[2] << 8) | rawData[3]);
int16_t const zRaw = static_cast<int16_t>((rawData[4] << 8) | rawData[5]);
return xyzFloat{static_cast<float>(xRaw), static_cast<float>(yRaw), static_cast<float>(zRaw)};
}

Expand Down Expand Up @@ -678,7 +678,7 @@ uint8_t MPU6500_WE::readMPU9250Register8(uint8_t reg){
_wire->beginTransmission(i2cAddress);
_wire->write(reg);
_wire->endTransmission(false);
_wire->requestFrom(i2cAddress,1);
_wire->requestFrom(i2cAddress,(uint8_t)1);
if(_wire->available()){
regValue = _wire->read();
}
Expand All @@ -703,7 +703,7 @@ int16_t MPU6500_WE::readMPU9250Register16(uint8_t reg){
_wire->beginTransmission(i2cAddress);
_wire->write(reg);
_wire->endTransmission(false);
_wire->requestFrom(i2cAddress,2);
_wire->requestFrom(i2cAddress,(uint8_t)2);
if(_wire->available()){
MSByte = _wire->read();
LSByte = _wire->read();
Expand All @@ -723,18 +723,15 @@ int16_t MPU6500_WE::readMPU9250Register16(uint8_t reg){
return regValue;
}

uint64_t MPU6500_WE::readMPU9250Register3x16(uint8_t reg){
uint8_t mpu9250Triple[6];
uint64_t regValue = 0;

void MPU6500_WE::readMPU9250Register3x16(uint8_t reg, uint8_t *buf){
if(!useSPI){
_wire->beginTransmission(i2cAddress);
_wire->write(reg);
_wire->endTransmission(false);
_wire->requestFrom(i2cAddress,6);
_wire->requestFrom(i2cAddress,(uint8_t)6);
if(_wire->available()){
for(int i=0; i<6; i++){
mpu9250Triple[i] = _wire->read();
buf[i] = _wire->read();
}
}
}
Expand All @@ -744,15 +741,11 @@ uint64_t MPU6500_WE::readMPU9250Register3x16(uint8_t reg){
digitalWrite(csPin, LOW);
_spi->transfer(reg);
for(int i=0; i<6; i++){
mpu9250Triple[i] = _spi->transfer(0x00);
buf[i] = _spi->transfer(0x00);
}
digitalWrite(csPin, HIGH);
_spi->endTransaction();
}

regValue = ((uint64_t) mpu9250Triple[0]<<40) + ((uint64_t) mpu9250Triple[1]<<32) +((uint64_t) mpu9250Triple[2]<<24) +
+ ((uint64_t) mpu9250Triple[3]<<16) + ((uint64_t) mpu9250Triple[4]<<8) + (uint64_t) mpu9250Triple[5];
return regValue;
}

xyzFloat MPU6500_WE::readMPU9250xyzValFromFifo(){
Expand All @@ -762,7 +755,7 @@ xyzFloat MPU6500_WE::readMPU9250xyzValFromFifo(){
_wire->beginTransmission(i2cAddress);
_wire->write(REGISTER_FIFO_R_W);
_wire->endTransmission(false);
_wire->requestFrom(i2cAddress,6);
_wire->requestFrom(i2cAddress,(uint8_t)6);
if(_wire->available()){
for(int i=0; i<6; i++){
fifoTriple[i] = _wire->read();
Expand Down
8 changes: 4 additions & 4 deletions src/MPU6500_WE.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -228,8 +228,8 @@ class MPU6500_WE

/* Constructors */

MPU6500_WE(int const addr);
MPU6500_WE(TwoWire * const w = &Wire, int const addr = 0x68);
MPU6500_WE(uint8_t const addr);
MPU6500_WE(TwoWire * const w = &Wire, uint8_t const addr = 0x68);
/* MPU6500_WE(int const cs, bool spi); */
MPU6500_WE(SPIClass * const s, int const cs, bool spi);

Expand Down Expand Up @@ -318,13 +318,13 @@ class MPU6500_WE
void writeMPU9250Register(uint8_t reg, uint8_t val);
uint8_t readMPU9250Register8(uint8_t reg);
int16_t readMPU9250Register16(uint8_t reg);
uint64_t readMPU9250Register3x16(uint8_t reg);
void readMPU9250Register3x16(uint8_t reg, uint8_t *buf);
xyzFloat readMPU9250xyzValFromFifo();

TwoWire * const _wire = &Wire;
SPIClass * const _spi = &SPI;
SPISettings mySPISettings;
int const i2cAddress = 0x68;
uint8_t const i2cAddress = 0x68;
int const csPin = 10;
bool useSPI = false;

Expand Down
32 changes: 12 additions & 20 deletions src/MPU9250_WE.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ uint8_t constexpr MPU9250_WE::MAGNETOMETER_WHO_AM_I_CODE ;

/************ Constructors ************/

MPU9250_WE::MPU9250_WE(int addr)
MPU9250_WE::MPU9250_WE(uint8_t addr)
: MPU6500_WE(addr)
{
// intentionally empty
Expand All @@ -61,7 +61,7 @@ MPU9250_WE::MPU9250_WE()
// intentionally empty
}

MPU9250_WE::MPU9250_WE(TwoWire *w, int addr)
MPU9250_WE::MPU9250_WE(TwoWire *w, uint8_t addr)
: MPU6500_WE(w, addr)
{
// intentionally empty
Expand All @@ -88,12 +88,12 @@ bool MPU9250_WE::init(){
/************* x,y,z results *************/

xyzFloat MPU9250_WE::getMagValues(){
xyzFloat magVal;

uint64_t const xyzDataReg = readAK8963Data();
int16_t xRaw = (int16_t)((xyzDataReg >> 32) & 0xFFFF);
int16_t yRaw = (int16_t)((xyzDataReg >> 16) & 0xFFFF);
int16_t zRaw = (int16_t)(xyzDataReg & 0xFFFF);
xyzFloat magVal = {0.0, 0.0, 0.0};
uint8_t rawData[6];
readAK8963Data(rawData);
int16_t xRaw = (int16_t)((rawData[1] << 8) | rawData[0]);
int16_t yRaw = (int16_t)((rawData[3] << 8) | rawData[2]);
int16_t zRaw = (int16_t)((rawData[5] << 8) | rawData[4]);

float constexpr scaleFactor = 4912.0 / 32760.0;

Expand Down Expand Up @@ -185,18 +185,15 @@ uint8_t MPU9250_WE::readAK8963Register8(uint8_t reg){
return regVal;
}

uint64_t MPU9250_WE::readAK8963Data(){
uint8_t magByte[6];
uint64_t regValue = 0;

void MPU9250_WE::readAK8963Data(uint8_t *buf){
if(!useSPI){
_wire->beginTransmission(i2cAddress);
_wire->write(MPU9250_EXT_SLV_SENS_DATA_00);
_wire->endTransmission(false);
_wire->requestFrom(i2cAddress,6);
_wire->requestFrom(i2cAddress,(uint8_t)6);
if(_wire->available()){
for(int i=0; i<6; i++){
magByte[i] = _wire->read();
buf[i] = _wire->read();
}
}
}
Expand All @@ -206,16 +203,11 @@ uint64_t MPU9250_WE::readAK8963Data(){
digitalWrite(csPin, LOW);
_spi->transfer(reg);
for(int i=0; i<6; i++){
magByte[i] = _spi->transfer(0x00);
buf[i] = _spi->transfer(0x00);
}
digitalWrite(csPin, HIGH);
_spi->endTransaction();
}

regValue = ((uint64_t) magByte[1]<<40) + ((uint64_t) magByte[0]<<32) +((uint64_t) magByte[3]<<24) +
+ ((uint64_t) magByte[2]<<16) + ((uint64_t) magByte[5]<<8) + (uint64_t) magByte[4];

return regValue;
}

void MPU9250_WE::setMagnetometer16Bit(){
Expand Down
6 changes: 3 additions & 3 deletions src/MPU9250_WE.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -160,9 +160,9 @@ class MPU9250_WE : public MPU6500_WE

/* Constructors */

MPU9250_WE(int addr);
MPU9250_WE(uint8_t addr);
MPU9250_WE();
MPU9250_WE(TwoWire *w, int addr);
MPU9250_WE(TwoWire *w, uint8_t addr);
MPU9250_WE(TwoWire *w);
MPU9250_WE(SPIClass *s, int cs, bool spi);

Expand All @@ -187,7 +187,7 @@ class MPU9250_WE : public MPU6500_WE
void resetMagnetometer();
void writeAK8963Register(uint8_t reg, uint8_t val);
uint8_t readAK8963Register8(uint8_t reg);
uint64_t readAK8963Data();
void readAK8963Data(uint8_t *buf);
void setMagnetometer16Bit();
uint8_t getStatus2Register();

Expand Down

0 comments on commit 9fc065c

Please sign in to comment.