diff --git a/adafruit_bno055.py b/adafruit_bno055.py
index 97af7a8..a9d76af 100644
--- a/adafruit_bno055.py
+++ b/adafruit_bno055.py
@@ -197,6 +197,14 @@ def mode(self):
         """
         return self._read_register(_MODE_REGISTER)
 
+    @mode.setter
+    def mode(self, new_mode):
+        self._write_register(_MODE_REGISTER, CONFIG_MODE)  # Empirically necessary
+        time.sleep(0.02)  # Datasheet table 3.6
+        if new_mode != CONFIG_MODE:
+            self._write_register(_MODE_REGISTER, new_mode)
+            time.sleep(0.01)  # Table 3.6
+
     @property
     def calibration_status(self):
         """Tuple containing sys, gyro, accel, and mag calibration data."""
@@ -213,10 +221,6 @@ def calibrated(self):
         sys, gyro, accel, mag = self.calibration_status
         return sys == gyro == accel == mag == 0x03
 
-    @mode.setter
-    def mode(self, new_mode):
-        self._write_register(_MODE_REGISTER, new_mode)
-
     @property
     def external_crystal(self):
         """Switches the use of external crystal on or off."""