diff --git a/mission/devices/GyroADIS16507Handler.cpp b/mission/devices/GyroADIS16507Handler.cpp
index 0e0cec00..9bef3232 100644
--- a/mission/devices/GyroADIS16507Handler.cpp
+++ b/mission/devices/GyroADIS16507Handler.cpp
@@ -59,11 +59,12 @@ ReturnValue_t GyroADIS16507Handler::buildCommandFromCommand(DeviceCommandId_t de
     switch(deviceCommand) {
     case(ADIS16507::READ_OUT_CONFIG): {
         this->rawPacketLen = ADIS16507::CONFIG_READOUT_SIZE;
-        uint8_t regList[4];
-        regList[0] = ADIS16507::FILTER_CTRL_REG;
-        regList[1] = ADIS16507::MSC_CTRL_REG;
-        regList[2] = ADIS16507::DEC_RATE_REG;
-        regList[3] = ADIS16507::PROD_ID_REG;
+        uint8_t regList[5];
+        regList[0] = ADIS16507::DIAG_STAT_REG;
+        regList[1] = ADIS16507::FILTER_CTRL_REG;
+        regList[2] = ADIS16507::MSC_CTRL_REG;
+        regList[3] = ADIS16507::DEC_RATE_REG;
+        regList[4] = ADIS16507::PROD_ID_REG;
         prepareReadCommand(regList, sizeof(regList));
         this->rawPacket = commandBuffer.data();
         break;
@@ -99,7 +100,7 @@ ReturnValue_t GyroADIS16507Handler::interpretDeviceReply(DeviceCommandId_t id,
     switch(id) {
     case(ADIS16507::READ_OUT_CONFIG): {
         PoolReadGuard rg(&configDataset);
-        uint16_t readProdId = packet[6] << 8 | packet[7];
+        uint16_t readProdId = packet[8] << 8 | packet[9];
         if (readProdId != ADIS16507::PROD_ID) {
 #if OBSW_VERBOSE_LEVEL >= 1
             sif::debug << "GyroADIS16507Handler::interpretDeviceReply: Invalid product ID!"
@@ -107,9 +108,11 @@ ReturnValue_t GyroADIS16507Handler::interpretDeviceReply(DeviceCommandId_t id,
 #endif
             return HasReturnvaluesIF::RETURN_FAILED;
         }
-        configDataset.filterSetting.value =  packet[0] << 8 | packet[1];
-        configDataset.mscCtrlReg.value = packet[2] << 8 | packet[3];
-        configDataset.decRateReg.value = packet[4] << 8 | packet[5];
+        configDataset.diagStatReg.value = packet[0] << 8 | packet[1];
+        configDataset.filterSetting.value =  packet[2] << 8 | packet[3];
+        configDataset.mscCtrlReg.value = packet[4] << 8 | packet[5];
+        configDataset.decRateReg.value = packet[6] << 8 | packet[7];
+        configDataset.setValidity(true, true);
         if(internalState == InternalState::STARTUP) {
             commandExecuted = true;
         }
@@ -148,6 +151,41 @@ ReturnValue_t GyroADIS16507Handler::handleSensorData(const uint8_t *packet) {
 #endif
             return HasReturnvaluesIF::RETURN_FAILED;
         }
+
+        ReturnValue_t result = configDataset.diagStatReg.read();
+        if(result == HasReturnvaluesIF::RETURN_OK) {
+            configDataset.diagStatReg.value = packet[2] << 8 | packet[3];
+            configDataset.diagStatReg.setValid(true);
+        }
+        configDataset.diagStatReg.commit();
+
+        PoolReadGuard pg(&primaryDataset);
+        int16_t angVelocXRaw = packet[4] << 8 | packet[5];
+        primaryDataset.angVelocX.value = static_cast<float>(angVelocXRaw) / INT16_MAX *
+                ADIS16507::GYRO_RANGE;
+        int16_t angVelocYRaw = packet[6] << 8 | packet[7];
+        primaryDataset.angVelocY.value = static_cast<float>(angVelocYRaw) / INT16_MAX *
+                ADIS16507::GYRO_RANGE;
+        int16_t angVelocZRaw = packet[8] << 8 | packet[9];
+        primaryDataset.angVelocZ.value = static_cast<float>(angVelocZRaw) / INT16_MAX *
+                ADIS16507::GYRO_RANGE;
+
+        int16_t accelXRaw = packet[10] << 8 | packet[11];
+        primaryDataset.accelX.value = static_cast<float>(accelXRaw) / INT16_MAX *
+                ADIS16507::GYRO_RANGE;
+        int16_t accelYRaw = packet[12] << 8 | packet[13];
+        primaryDataset.accelY.value = static_cast<float>(accelYRaw) / INT16_MAX *
+                ADIS16507::GYRO_RANGE;
+        int16_t accelZRaw = packet[14] << 8 | packet[15];
+        primaryDataset.accelZ.value = static_cast<float>(accelZRaw) / INT16_MAX *
+                ADIS16507::GYRO_RANGE;
+
+        int16_t temperatureRaw = packet[16] << 8 | packet[17];
+        primaryDataset.temperature.value = static_cast<float>(temperatureRaw) * 0.1;
+
+        // Ignore data counter for now
+
+        primaryDataset.setValidity(true, true);
         break;
     }
     case(BurstModes::BURST_32_BURST_SEL_0): {
diff --git a/mission/devices/devicedefinitions/GyroADIS16507Definitions.h b/mission/devices/devicedefinitions/GyroADIS16507Definitions.h
index 1a2d25a9..f58196a0 100644
--- a/mission/devices/devicedefinitions/GyroADIS16507Definitions.h
+++ b/mission/devices/devicedefinitions/GyroADIS16507Definitions.h
@@ -58,6 +58,7 @@ enum PrimaryPoolIds: lp_id_t {
 };
 
 enum ConfigPoolIds: lp_id_t {
+    DIAG_STAT_REGISTER,
     FILTER_SETTINGS,
     MSC_CTRL_REGISTER,
     DEC_RATE_REGISTER,
@@ -75,7 +76,7 @@ enum FilterSettings: uint8_t {
 
 }
 
-class AdisGyroPrimaryDataset: public StaticLocalDataSet<7 * sizeof(float)> {
+class AdisGyroPrimaryDataset: public StaticLocalDataSet<8> {
 public:
 
     /** Constructor  for data users like controllers */
@@ -107,7 +108,7 @@ private:
             StaticLocalDataSet(hkOwner, ADIS16507::ADIS_DATASET_ID) {}
 };
 
-class AdisGyroConfigDataset: public StaticLocalDataSet<32> {
+class AdisGyroConfigDataset: public StaticLocalDataSet<5> {
 public:
 
     /** Constructor  for data users like controllers */
@@ -116,6 +117,7 @@ public:
         setAllVariablesReadOnly();
     }
 
+    lp_var_t<uint16_t> diagStatReg = lp_var_t<uint16_t>(sid.objectId, ADIS16507::DIAG_STAT_REG);
     lp_var_t<uint8_t> filterSetting = lp_var_t<uint8_t>(sid.objectId, ADIS16507::FILTER_SETTINGS);
     lp_var_t<uint16_t> mscCtrlReg = lp_var_t<uint16_t>(sid.objectId, ADIS16507::MSC_CTRL_REGISTER);
     lp_var_t<uint16_t> decRateReg = lp_var_t<uint16_t>(sid.objectId, ADIS16507::DEC_RATE_REGISTER);