diff --git a/mission/devices/GyroADIS16507Handler.cpp b/mission/devices/GyroADIS16507Handler.cpp
index 08a0b3ef..0e0cec00 100644
--- a/mission/devices/GyroADIS16507Handler.cpp
+++ b/mission/devices/GyroADIS16507Handler.cpp
@@ -116,42 +116,47 @@ ReturnValue_t GyroADIS16507Handler::interpretDeviceReply(DeviceCommandId_t id,
         break;
     }
     case(ADIS16507::READ_SENSOR_DATA): {
-        BurstModes burstMode = getBurstMode();
-        switch(burstMode) {
-        case(BurstModes::BURST_16_BURST_SEL_1):
-        case(BurstModes::BURST_32_BURST_SEL_1): {
-            sif::warning << "GyroADIS16507Handler::interpretDeviceReply: Analysis with BURST_SEL1"
-                    " not implemented!" << std::endl;
-            return HasReturnvaluesIF::RETURN_OK;
-        }
-        case(BurstModes::BURST_16_BURST_SEL_0): {
-            uint16_t checksum = packet[20] << 8 | packet[21];
-            /* Now verify the read checksum with the expected checksum
-            according to datasheet p. 20 */
-            uint16_t calcChecksum = 0;
-            for(size_t idx = 2; idx < 22; idx ++) {
-                calcChecksum += packet[idx];
-            }
-            if(checksum != calcChecksum) {
-#if OBSW_VERBOSE_LEVEL >= 1
-                sif::warning << "GyroADIS16507Handler::interpretDeviceReply: "
-                        "Invalid checksum detected!" << std::endl;
-#endif
-                return HasReturnvaluesIF::RETURN_FAILED;
-            }
-
-        }
-        case(BurstModes::BURST_32_BURST_SEL_0): {
-
-        }
-        }
-
+        return handleSensorData(packet);
     }
 
     }
     return HasReturnvaluesIF::RETURN_OK;
 }
 
+
+ReturnValue_t GyroADIS16507Handler::handleSensorData(const uint8_t *packet) {
+    BurstModes burstMode = getBurstMode();
+    switch(burstMode) {
+    case(BurstModes::BURST_16_BURST_SEL_1):
+    case(BurstModes::BURST_32_BURST_SEL_1): {
+        sif::warning << "GyroADIS16507Handler::interpretDeviceReply: Analysis with BURST_SEL1"
+                " not implemented!" << std::endl;
+        return HasReturnvaluesIF::RETURN_OK;
+    }
+    case(BurstModes::BURST_16_BURST_SEL_0): {
+        uint16_t checksum = packet[20] << 8 | packet[21];
+        /* Now verify the read checksum with the expected checksum
+        according to datasheet p. 20 */
+        uint16_t calcChecksum = 0;
+        for(size_t idx = 2; idx < 22; idx ++) {
+            calcChecksum += packet[idx];
+        }
+        if(checksum != calcChecksum) {
+#if OBSW_VERBOSE_LEVEL >= 1
+            sif::warning << "GyroADIS16507Handler::interpretDeviceReply: "
+                    "Invalid checksum detected!" << std::endl;
+#endif
+            return HasReturnvaluesIF::RETURN_FAILED;
+        }
+        break;
+    }
+    case(BurstModes::BURST_32_BURST_SEL_0): {
+        break;
+    }
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
 uint32_t GyroADIS16507Handler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
     return 5000;
 }
diff --git a/mission/devices/GyroADIS16507Handler.h b/mission/devices/GyroADIS16507Handler.h
index dc5cf189..45e9f153 100644
--- a/mission/devices/GyroADIS16507Handler.h
+++ b/mission/devices/GyroADIS16507Handler.h
@@ -61,6 +61,7 @@ private:
             const uint8_t *sendData, size_t sendLen, void* args);
 #endif
 
+    ReturnValue_t handleSensorData(const uint8_t* packet);
 };