diff --git a/src/drivers/mpu9250/mpu9250.cpp b/src/drivers/mpu9250/mpu9250.cpp
index 69527a743b..58f98e01e3 100644
--- a/src/drivers/mpu9250/mpu9250.cpp
+++ b/src/drivers/mpu9250/mpu9250.cpp
@@ -1013,6 +1013,17 @@ MPU9250::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
 	write_reg(reg, val);
 }
 
+void
+MPU9250::modify_checked_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
+{
+	uint8_t	val;
+
+	val = read_reg(reg);
+	val &= ~clearbits;
+	val |= setbits;
+	write_checked_reg(reg, val);
+}
+
 void
 MPU9250::write_checked_reg(unsigned reg, uint8_t value)
 {
diff --git a/src/drivers/mpu9250/mpu9250.h b/src/drivers/mpu9250/mpu9250.h
index da3daf8934..2725896f99 100644
--- a/src/drivers/mpu9250/mpu9250.h
+++ b/src/drivers/mpu9250/mpu9250.h
@@ -462,6 +462,17 @@ private:
 	 */
 	void			write_checked_reg(unsigned reg, uint8_t value);
 
+	/**
+	 * Modify a checked register in the mpu
+	 *
+	 * Bits are cleared before bits are set.
+	 *
+	 * @param reg		The register to modify.
+	 * @param clearbits	Bits in the register to clear.
+	 * @param setbits	Bits in the register to set.
+	 */
+	void			modify_checked_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
+
 	/**
 	 * Set the mpu measurement range.
 	 *