diff --git a/Sming/Libraries/MPU6050/MPU6050.cpp b/Sming/Libraries/MPU6050/MPU6050.cpp
index d6cf680d01..f09aa7653e 100644
--- a/Sming/Libraries/MPU6050/MPU6050.cpp
+++ b/Sming/Libraries/MPU6050/MPU6050.cpp
@@ -43,9 +43,8 @@ THE SOFTWARE.
 /** Default constructor, uses default I2C address.
  * @see MPU6050_DEFAULT_ADDRESS
  */
-MPU6050::MPU6050()
+MPU6050::MPU6050() : devAddr{MPU6050_DEFAULT_ADDRESS}
 {
-	devAddr = MPU6050_DEFAULT_ADDRESS;
 }
 
 /** Specific address constructor.
@@ -54,9 +53,8 @@ MPU6050::MPU6050()
  * @see MPU6050_ADDRESS_AD0_LOW
  * @see MPU6050_ADDRESS_AD0_HIGH
  */
-MPU6050::MPU6050(uint8_t address)
+MPU6050::MPU6050(uint8_t address) : devAddr{address}
 {
-	devAddr = address;
 }
 
 /** Power on and prepare for general usage.
@@ -3315,10 +3313,9 @@ void MPU6050::readMemoryBlock(uint8_t* data, uint16_t dataSize, uint8_t bank, ui
 {
 	setMemoryBank(bank);
 	setMemoryStartAddress(address);
-	uint8_t chunkSize;
 	for(uint16_t i = 0; i < dataSize;) {
 		// determine correct chunk size according to bank position and data size
-		chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;
+		uint8_t chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;
 
 		// make sure we don't go past the data size
 		if(i + chunkSize > dataSize)
@@ -3351,18 +3348,17 @@ bool MPU6050::writeMemoryBlock(const uint8_t* data, uint16_t dataSize, uint8_t b
 {
 	setMemoryBank(bank);
 	setMemoryStartAddress(address);
-	uint8_t chunkSize;
 	uint8_t* verifyBuffer = 0;
 	uint8_t* progBuffer = 0;
 	uint16_t i;
 	uint8_t j;
 	if(verify)
-		verifyBuffer = (uint8_t*)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE);
+		verifyBuffer = static_cast<uint8_t*>(malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE));
 	if(useProgMem)
-		progBuffer = (uint8_t*)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE);
+		progBuffer = static_cast<uint8_t*>(malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE));
 	for(i = 0; i < dataSize;) {
 		// determine correct chunk size according to bank position and data size
-		chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;
+		uint8_t chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;
 
 		// make sure we don't go past the data size
 		if(i + chunkSize > dataSize)
@@ -3378,7 +3374,7 @@ bool MPU6050::writeMemoryBlock(const uint8_t* data, uint16_t dataSize, uint8_t b
 				progBuffer[j] = pgm_read_byte(data + i + j);
 		} else {
 			// write the chunk of data as specified
-			progBuffer = (uint8_t*)data + i;
+			progBuffer = const_cast<uint8_t*>(data) + i;
 		}
 
 		I2Cdev::writeBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer);
@@ -3440,7 +3436,7 @@ bool MPU6050::writeDMPConfigurationSet(const uint8_t* data, uint16_t dataSize, b
 	uint8_t success, special;
 	uint16_t i, j;
 	if(useProgMem) {
-		progBuffer = (uint8_t*)malloc(8); // assume 8-byte blocks, realloc later if necessary
+		progBuffer = static_cast<uint8_t*>(malloc(8)); // assume 8-byte blocks, realloc later if necessary
 	}
 
 	// config set data is a long string of blocks with the following structure:
@@ -3468,11 +3464,11 @@ bool MPU6050::writeDMPConfigurationSet(const uint8_t* data, uint16_t dataSize, b
       Serial.println(length);*/
 			if(useProgMem) {
 				if(sizeof(progBuffer) < length)
-					progBuffer = (uint8_t*)realloc(progBuffer, length);
+					progBuffer = static_cast<uint8_t*>(realloc(progBuffer, length));
 				for(j = 0; j < length; j++)
 					progBuffer[j] = pgm_read_byte(data + i + j);
 			} else {
-				progBuffer = (uint8_t*)data + i;
+				progBuffer = const_cast<uint8_t*>(data) + i;
 			}
 			success = writeMemoryBlock(progBuffer, length, bank, offset, true);
 			i += length;
@@ -3596,7 +3592,6 @@ void MPU6050::PID(uint8_t ReadAddress, float kP, float kI, uint8_t Loops)
 	int16_t BitZero[3];
 	uint8_t shift = (SaveAddress == 0x77) ? 3 : 2;
 	float Error, PTerm, ITerm[3];
-	int16_t eSample;
 	uint32_t eSum;
 	for(int i = 0; i < 3; i++) {
 		I2Cdev::readWord(devAddr, SaveAddress + (i * shift),
@@ -3610,7 +3605,7 @@ void MPU6050::PID(uint8_t ReadAddress, float kP, float kI, uint8_t Loops)
 		}
 	}
 	for(int L = 0; L < Loops; L++) {
-		eSample = 0;
+		int16_t eSample{0};
 		for(int c = 0; c < 100; c++) { // 100 PI Calculations
 			eSum = 0;
 			for(int i = 0; i < 3; i++) {
diff --git a/Sming/Libraries/MPU6050/MPU6050.h b/Sming/Libraries/MPU6050/MPU6050.h
index bba931d1d3..cc2fe0e1e7 100644
--- a/Sming/Libraries/MPU6050/MPU6050.h
+++ b/Sming/Libraries/MPU6050/MPU6050.h
@@ -45,7 +45,7 @@ THE SOFTWARE.
 // http://forum.arduino.cc/index.php?&topic=141571.msg1062899#msg1062899s
 
 #undef pgm_read_byte
-#define pgm_read_byte(addr) (*(const unsigned char*)(addr))
+#define pgm_read_byte(addr) (*static_cast<const unsigned char*>(addr))
 
 //#define PROGMEM /* empty */
 //#define pgm_read_byte(x) (*(x))
@@ -831,7 +831,7 @@ class MPU6050
 
 private:
 	uint8_t devAddr;
-	uint8_t buffer[14];
+	uint8_t buffer[14] = {0};
 };
 
 #endif /* _MPU6050_H_ */
diff --git a/Sming/Libraries/MPU6050/README.md b/Sming/Libraries/MPU6050/README.md
index 45243771e6..68c01c3f30 100644
--- a/Sming/Libraries/MPU6050/README.md
+++ b/Sming/Libraries/MPU6050/README.md
@@ -3,5 +3,6 @@
 Based on code from [jrowberg/i2cdevlib](https://github.com/jrowberg/i2cdevlib/tree/master/ESP32_ESP-IDF/components/MPU6050). Most of the code is the same, except:
 
 -   Removed MPU6050::ReadRegister function due to incompatibility. It is also not used anywhere in the original code.
+-   MPU6050_6Axis_MotionApps20.h and MPU6050_9Axis_MotionApps41.h are not included due to deps to freeRTOS. helper_3dmath.h is also not included since it is only used in the above mentioned files.
 -   Removed map function in favor of the Sming built-in one.
--   Adapted include path and applied clangformat
+-   Adapted include path, coding style and applied clangformat