adis handler continued

mission/devices/GyroADIS16507Handler.cpp

@@ -6,6 +6,8 @@
 #include "fsfw_hal/linux/spi/SpiComIF.h"
 #endif
 
+#include <unistd.h>
+
 GyroADIS16507Handler::GyroADIS16507Handler(object_id_t objectId,
         object_id_t deviceCommunication, CookieIF * comCookie):
             DeviceHandlerBase(objectId, deviceCommunication, comCookie), primaryDataset(this),
@@ -79,6 +81,8 @@ ReturnValue_t GyroADIS16507Handler::buildCommandFromCommand(DeviceCommandId_t de
 }
 
 void GyroADIS16507Handler::fillCommandAndReplyMap() {
+    insertInCommandAndReplyMap(ADIS16507::READ_SENSOR_DATA, 1, &primaryDataset);
+    insertInCommandAndReplyMap(ADIS16507::READ_OUT_CONFIG, 1, &configDataset);
 }
 
 ReturnValue_t GyroADIS16507Handler::scanForReply(const uint8_t *start, size_t remainingSize,
@@ -112,6 +116,35 @@ 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): {
+
+        }
+        }
 
     }
 
@@ -147,6 +180,28 @@ ReturnValue_t GyroADIS16507Handler::initializeLocalDataPool(localpool::DataPool
     return HasReturnvaluesIF::RETURN_OK;
 }
 
+GyroADIS16507Handler::BurstModes GyroADIS16507Handler::getBurstMode() {
+    configDataset.mscCtrlReg.read();
+    uint16_t currentCtrlReg = configDataset.mscCtrlReg.value;
+    configDataset.mscCtrlReg.commit();
+    if((currentCtrlReg & ADIS16507::BURST_32_BIT) == ADIS16507::BURST_32_BIT) {
+        if((currentCtrlReg & ADIS16507::BURST_SEL_BIT) == ADIS16507::BURST_SEL_BIT) {
+            return BurstModes::BURST_32_BURST_SEL_1;
+        }
+        else {
+            return BurstModes::BURST_32_BURST_SEL_0;
+        }
+    }
+    else {
+        if((currentCtrlReg & ADIS16507::BURST_SEL_BIT) == ADIS16507::BURST_SEL_BIT) {
+            return BurstModes::BURST_16_BURST_SEL_1;
+        }
+        else {
+            return BurstModes::BURST_16_BURST_SEL_0;
+        }
+    }
+}
+
 #if OBSW_ADIS16507_LINUX_COM_IF == 1
 
 ReturnValue_t GyroADIS16507Handler::spiSendCallback(SpiComIF *comIf, SpiCookie *cookie,
@@ -163,7 +218,7 @@ ReturnValue_t GyroADIS16507Handler::spiSendCallback(SpiComIF *comIf, SpiCookie *
         return comIf->performRegularSendOperation(cookie, sendData, sendLen);
     }
     case(ADIS16507::READ_OUT_CONFIG): {
-
+        usleep(ADIS16507::STALL_TIME_MICROSECONDS);
     }
     }
     return HasReturnvaluesIF::RETURN_OK;

mission/devices/GyroADIS16507Handler.h

@@ -42,11 +42,20 @@ private:
         IDLE
     };
 
+    enum class BurstModes {
+        BURST_16_BURST_SEL_0,
+        BURST_16_BURST_SEL_1,
+        BURST_32_BURST_SEL_0,
+        BURST_32_BURST_SEL_1
+    };
+
     InternalState internalState = InternalState::STARTUP;
     bool commandExecuted = false;
 
     void prepareReadCommand(uint8_t* regList, size_t len);
 
+    BurstModes getBurstMode();
+
 #if OBSW_ADIS16507_LINUX_COM_IF == 1
     static ReturnValue_t spiSendCallback(SpiComIF* comIf, SpiCookie *cookie,
             const uint8_t *sendData, size_t sendLen, void* args);

mission/devices/devicedefinitions/GyroADIS16507Definitions.h

@@ -12,6 +12,8 @@ static constexpr size_t MAXIMUM_REPLY_SIZE = 64;
 static constexpr uint32_t GYRO_RANGE = 125;
 static constexpr uint32_t ACCELEROMETER_RANGE = 392;
 
+static constexpr uint32_t STALL_TIME_MICROSECONDS = 16;
+
 static constexpr uint16_t PROD_ID = 16507;
 
 static constexpr std::array<uint8_t, 2> BURST_READ_ENABLE = {0x68, 0x00};
@@ -34,6 +36,11 @@ static constexpr DeviceCommandId_t RESET_SENSOR_CONFIGURATION = 30;
 static constexpr DeviceCommandId_t SW_RESET = 31;
 static constexpr DeviceCommandId_t PRINT_CURRENT_CONFIGURATION = 32;
 
+static constexpr uint16_t BURST_32_BIT = 1 << 9;
+static constexpr uint16_t BURST_SEL_BIT = 1 << 8;
+static constexpr uint16_t LIN_ACCEL_COMPENSATION_BIT = 1 << 7;
+static constexpr uint16_t POINT_PERCUSSION_COMPENSATION_BIT = 1 << 6;
+
 static constexpr size_t CONFIG_READOUT_SIZE = 8;
 static constexpr size_t SENSOR_READOUT_SIZE = 22;