Update EM7180_MPU6500_AK8963C_BMP280.ino

Fixed MPU6500 init function

EM7180_MPU6500_AK8963C_BMP280.ino

@@ -1148,27 +1148,30 @@ void initMPU6500()
   writeByte(MPU6500_ADDRESS, SMPLRT_DIV, 0x04);  // Use a 200 Hz rate; a rate consistent with the filter update rate 
                                     // determined inset in CONFIG above
  
- // Set gyroscope full scale range
+// Set gyroscope full scale range
  // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3
-  uint8_t c = readByte(MPU6500_ADDRESS, GYRO_CONFIG);
-//  writeRegister(GYRO_CONFIG, c & ~0xE0); // Clear self-test bits [7:5] 
-  writeByte(MPU6500_ADDRESS, GYRO_CONFIG, c & ~0x02); // Clear Fchoice bits [1:0] 
-  writeByte(MPU6500_ADDRESS, GYRO_CONFIG, c & ~0x18); // Clear AFS bits [4:3]
-  writeByte(MPU6500_ADDRESS, GYRO_CONFIG, c | Gscale << 3); // Set full scale range for the gyro
- // writeRegister(GYRO_CONFIG, c | 0x00); // Set Fchoice for the gyro to 11 by writing its inverse to bits 1:0 of GYRO_CONFIG
+  uint8_t c = readByte(MPU6500_ADDRESS, GYRO_CONFIG); // get current GYRO_CONFIG register value
+ // c = c & ~0xE0; // Clear self-test bits [7:5] 
+  c = c & ~0x02; // Clear Fchoice bits [1:0] 
+  c = c & ~0x18; // Clear AFS bits [4:3]
+  c = c | Gscale << 3; // Set full scale range for the gyro
+ // c =| 0x00; // Set Fchoice for the gyro to 11 by writing its inverse to bits 1:0 of GYRO_CONFIG
+  writeByte(MPU6500_ADDRESS, GYRO_CONFIG, c ); // Write new GYRO_CONFIG value to register
   
  // Set accelerometer full-scale range configuration
-  c = readByte(MPU6500_ADDRESS, ACCEL_CONFIG);
-//  writeRegister(ACCEL_CONFIG, c & ~0xE0); // Clear self-test bits [7:5] 
-  writeByte(MPU6500_ADDRESS, ACCEL_CONFIG, c & ~0x18); // Clear AFS bits [4:3]
-  writeByte(MPU6500_ADDRESS, ACCEL_CONFIG, c | Ascale << 3); // Set full scale range for the accelerometer 
+  c = readByte(MPU6500_ADDRESS, ACCEL_CONFIG); // get current ACCEL_CONFIG register value
+ // c = c & ~0xE0; // Clear self-test bits [7:5] 
+  c = c & ~0x18;  // Clear AFS bits [4:3]
+  c = c | Ascale << 3; // Set full scale range for the accelerometer 
+  writeByte(MPU6500_ADDRESS, ACCEL_CONFIG, c); // Write new ACCEL_CONFIG register value
 
  // Set accelerometer sample rate configuration
  // It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for
  // accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz
-  c = readByte(MPU6500_ADDRESS, ACCEL_CONFIG2);
-  writeByte(MPU6500_ADDRESS, ACCEL_CONFIG2, c & ~0x0F); // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0])  
-  writeByte(MPU6500_ADDRESS, ACCEL_CONFIG2, c | 0x03); // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz
+  c = readByte(MPU6500_ADDRESS, ACCEL_CONFIG2); // get current ACCEL_CONFIG2 register value
+  c = c & ~0x0F; // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0])  
+  c = c | 0x03;  // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz
+  writeByte(MPU6500_ADDRESS, ACCEL_CONFIG2, c); // Write new ACCEL_CONFIG2 register value
 
  // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates, 
  // but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting