Merge pull request #69 from Magic-Compass/setFIFORateAndMPUAddr

Set fifo rate and mpu addr
ElectronicCats · Dec 13, 2023 · ce4d7e2 · ce4d7e2
2 parents f531b2b + 2a9b931
commit ce4d7e2
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 7 deletions.
diff --git a/src/MPU6050.h b/src/MPU6050.h
@@ -972,7 +972,7 @@ class MPU6050 {
         // special methods for MotionApps 2.0 implementation
         #ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS20
 
-            uint8_t dmpInitialize();
+            uint8_t dmpInitialize(uint8_t rateDivisor, uint8_t mpuAddr);
             bool dmpPacketAvailable();
 
             uint8_t dmpSetFIFORate(uint8_t fifoRate);

diff --git a/src/MPU6050_6Axis_MotionApps20.h b/src/MPU6050_6Axis_MotionApps20.h
@@ -275,7 +275,7 @@ const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = {
 #endif
 
 // I Simplified this:
-uint8_t MPU6050::dmpInitialize() {
+uint8_t MPU6050::dmpInitialize(uint8_t rateDivisor = MPU6050_DMP_FIFO_RATE_DIVISOR, uint8_t mpuAddr = 0x68) {
 	// reset device
 	DEBUG_PRINTLN(F("\n\nResetting MPU6050..."));
 	reset();
@@ -309,8 +309,8 @@ uint8_t MPU6050::dmpInitialize() {
 	setSlaveAddress(0, 0x7F);
 	DEBUG_PRINTLN(F("Disabling I2C Master mode..."));
 	setI2CMasterModeEnabled(false);
-	DEBUG_PRINTLN(F("Setting slave 0 address to 0x68 (self)..."));
-	setSlaveAddress(0, 0x68);
+	DEBUG_PRINTLN(F("Setting slave 0 address to self..."));
+	setSlaveAddress(0, mpuAddr);
 	DEBUG_PRINTLN(F("Resetting I2C Master control..."));
 	resetI2CMaster();
 	delay(20);
@@ -340,7 +340,7 @@ uint8_t MPU6050::dmpInitialize() {
 	DEBUG_PRINTLN(F("Success! DMP code written and verified."));
 
 	// Set the FIFO Rate Divisor int the DMP Firmware Memory
-	unsigned char dmpUpdate[] = {0x00, MPU6050_DMP_FIFO_RATE_DIVISOR};
+	unsigned char dmpUpdate[] = {0x00, rateDivisor};
 	writeMemoryBlock(dmpUpdate, 0x02, 0x02, 0x16); // Lets write the dmpUpdate data to the Firmware image, we have 2 bytes to write in bank 0x02 with the Offset 0x16
 
 	//write start address MSB into register

diff --git a/src/MPU6050_6Axis_MotionApps_V6_12.h b/src/MPU6050_6Axis_MotionApps_V6_12.h
@@ -351,7 +351,7 @@ const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = {
 
 // this is the most basic initialization I can create. with the intent that we access the register bytes as few times as needed to get the job done.
 // for detailed descriptins of all registers and there purpose google "MPU-6000/MPU-6050 Register Map and Descriptions"
-uint8_t MPU6050::dmpInitialize() { // Lets get it over with fast Write everything once and set it up necely
+uint8_t MPU6050::dmpInitialize(uint8_t rateDivisor = 0x04, uint8_t mpuAddr = 0x68) { // Lets get it over with fast Write everything once and set it up necely
 	uint8_t val;
 	uint16_t ival;
   // Reset procedure per instructions in the "MPU-6000/MPU-6050 Register Map and Descriptions" page 41
@@ -365,7 +365,7 @@ uint8_t MPU6050::dmpInitialize() { // Lets get it over with fast Write everythin
 	I2Cdev::writeBytes(devAddr,0x1C, 1, &(val = 0x00)); // 0000 0000 ACCEL_CONFIG: 0 =  Accel Full Scale Select: 2g
 	I2Cdev::writeBytes(devAddr,0x37, 1, &(val = 0x80)); // 1001 0000 INT_PIN_CFG: ACTL The logic level for int pin is active low. and interrupt status bits are cleared on any read
 	I2Cdev::writeBytes(devAddr,0x6B, 1, &(val = 0x01)); // 0000 0001 PWR_MGMT_1: Clock Source Select PLL_X_gyro
-	I2Cdev::writeBytes(devAddr,0x19, 1, &(val = 0x04)); // 0000 0100 SMPLRT_DIV: Divides the internal sample rate 400Hz ( Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV))
+	I2Cdev::writeBytes(devAddr,0x19, 1, &(val = rateDivisor)); // 0000 0100 SMPLRT_DIV: Divides the internal sample rate 400Hz ( Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV))
 	I2Cdev::writeBytes(devAddr,0x1A, 1, &(val = 0x01)); // 0000 0001 CONFIG: Digital Low Pass Filter (DLPF) Configuration 188HZ  //Im betting this will be the beat
 	if (!writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) return 1; // Loads the DMP image into the MPU6050 Memory // Should Never Fail
 	I2Cdev::writeWords(devAddr, 0x70, 1, &(ival = 0x0400)); // DMP Program Start Address