adis handler continued

fsfw_hal

@@ -1 +1 @@
-Subproject commit d801319c12713d08cbdbc571ee2a922ce2f0c851
+Subproject commit 73a427d7e0605519aecfa53fa289b94e0143394f

linux/fsfwconfig/OBSWConfig.h

@@ -38,6 +38,9 @@ debugging. */
 #define DEBUG_RAD_SENSOR                1
 #define DEBUG_SUS                       1
 
+// Leave at one as the BSP is linux. Used by the ADIS16507 device handler
+#define OBSW_ADIS16507_LINUX_COM_IF     1
+
 #include "OBSWVersion.h"
 
 /* Can be used to switch device to NORMAL mode immediately */

mission/devices/GyroADIS16507Handler.cpp

@@ -1,8 +1,21 @@
+#include <fsfw/datapool/PoolReadGuard.h>
 #include "GyroADIS16507Handler.h"
 
+#if OBSW_ADIS16507_LINUX_COM_IF == 1
+#include "fsfw_hal/linux/spi/SpiCookie.h"
+#include "fsfw_hal/linux/spi/SpiComIF.h"
+#endif
+
 GyroADIS16507Handler::GyroADIS16507Handler(object_id_t objectId,
         object_id_t deviceCommunication, CookieIF * comCookie):
-            DeviceHandlerBase(objectId, deviceCommunication, comCookie) {
+            DeviceHandlerBase(objectId, deviceCommunication, comCookie), primaryDataset(this),
+            configDataset(this) {
+#if OBSW_ADIS16507_LINUX_COM_IF == 1
+    SpiCookie* cookie = dynamic_cast<SpiCookie*>(comCookie);
+    if(cookie != nullptr) {
+        cookie->setCallbackMode(&spiSendCallback, this);
+    }
+#endif
 }
 
 void GyroADIS16507Handler::doStartUp() {
@@ -18,15 +31,50 @@ void GyroADIS16507Handler::doShutDown() {
 }
 
 ReturnValue_t GyroADIS16507Handler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
-    return HasReturnvaluesIF::RETURN_OK;
+    *id = ADIS16507::READ_SENSOR_DATA;
+    return buildCommandFromCommand(*id, nullptr, 0);
 }
 
 ReturnValue_t GyroADIS16507Handler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
+    switch(internalState) {
+    case(InternalState::STARTUP): {
+        *id = ADIS16507::READ_OUT_CONFIG;
+        buildCommandFromCommand(*id, nullptr, 0);
+        break;
+    }
+    default: {
+        /* Might be a configuration error. */
+        sif::debug << "GyroADIS16507Handler::buildTransitionDeviceCommand: "
+                "Unknown internal state!" << std::endl;
+        return HasReturnvaluesIF::RETURN_OK;
+    }
+    }
     return HasReturnvaluesIF::RETURN_OK;
 }
 
 ReturnValue_t GyroADIS16507Handler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
         const uint8_t *commandData, size_t commandDataLen) {
+    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;
+        prepareReadCommand(regList, sizeof(regList));
+        this->rawPacket = commandBuffer.data();
+        break;
+    }
+    case(ADIS16507::READ_SENSOR_DATA): {
+        std::memcpy(commandBuffer.data(), ADIS16507::BURST_READ_ENABLE.data(),
+                ADIS16507::BURST_READ_ENABLE.size());
+        std::memset(commandBuffer.data() + 2, 0, 10 * 2);
+        this->rawPacketLen = ADIS16507::SENSOR_READOUT_SIZE;
+        this->rawPacket = commandBuffer.data();
+        break;
+    }
+    }
     return HasReturnvaluesIF::RETURN_OK;
 }
 
@@ -35,17 +83,90 @@ void GyroADIS16507Handler::fillCommandAndReplyMap() {
 
 ReturnValue_t GyroADIS16507Handler::scanForReply(const uint8_t *start, size_t remainingSize,
         DeviceCommandId_t *foundId, size_t *foundLen) {
+    /* For SPI, the ID will always be the one of the last sent command. */
+    *foundId = this->getPendingCommand();
+    *foundLen = this->rawPacketLen;
+
     return HasReturnvaluesIF::RETURN_OK;
 }
 
 ReturnValue_t GyroADIS16507Handler::interpretDeviceReply(DeviceCommandId_t id,
         const uint8_t *packet) {
-    return HasReturnvaluesIF::RETURN_OK;
+    switch(id) {
-}
+    case(ADIS16507::READ_OUT_CONFIG): {
+        PoolReadGuard rg(&configDataset);
+        uint16_t readProdId = packet[6] << 8 | packet[7];
+        if (readProdId != ADIS16507::PROD_ID) {
+#if OBSW_VERBOSE_LEVEL >= 1
+            sif::debug << "GyroADIS16507Handler::interpretDeviceReply: Invalid product ID!"
+                    << std::endl;
+#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];
+        if(internalState == InternalState::STARTUP) {
+            commandExecuted = true;
+        }
+        break;
+    }
+    case(ADIS16507::READ_SENSOR_DATA): {
 
-void GyroADIS16507Handler::setNormalDatapoolEntriesInvalid() {
+    }
+
+    }
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 uint32_t GyroADIS16507Handler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
     return 5000;
 }
+
+void GyroADIS16507Handler::prepareReadCommand(uint8_t *regList, size_t len) {
+    for(size_t idx = 0; idx < len; idx++) {
+        commandBuffer[idx * 2] = regList[idx];
+        commandBuffer[idx * 2 + 1] = 0x00;
+    }
+    commandBuffer[len * 2] = 0x00;
+    commandBuffer[len * 2 + 1] = 0x00;
+}
+
+ReturnValue_t GyroADIS16507Handler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
+        LocalDataPoolManager &poolManager) {
+    localDataPoolMap.emplace(ADIS16507::ANG_VELOC_X, new PoolEntry<double>({0.0}));
+    localDataPoolMap.emplace(ADIS16507::ANG_VELOC_Y, new PoolEntry<double>({0.0}));
+    localDataPoolMap.emplace(ADIS16507::ANG_VELOC_Z, new PoolEntry<double>({0.0}));
+    localDataPoolMap.emplace(ADIS16507::ACCELERATION_X, new PoolEntry<double>({0.0}));
+    localDataPoolMap.emplace(ADIS16507::ACCELERATION_Y, new PoolEntry<double>({0.0}));
+    localDataPoolMap.emplace(ADIS16507::ACCELERATION_Z, new PoolEntry<double>({0.0}));
+
+    localDataPoolMap.emplace(ADIS16507::FILTER_SETTINGS, new PoolEntry<uint8_t>());
+    localDataPoolMap.emplace(ADIS16507::MSC_CTRL_REGISTER, new PoolEntry<uint16_t>());
+    localDataPoolMap.emplace(ADIS16507::DEC_RATE_REGISTER, new PoolEntry<uint16_t>());
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+#if OBSW_ADIS16507_LINUX_COM_IF == 1
+
+ReturnValue_t GyroADIS16507Handler::spiSendCallback(SpiComIF *comIf, SpiCookie *cookie,
+        const uint8_t *sendData, size_t sendLen, void *args) {
+    GyroADIS16507Handler* handler = reinterpret_cast<GyroADIS16507Handler*>(args);
+    if(handler == nullptr) {
+        sif::error << "GyroADIS16507Handler::spiSendCallback: Passed handler pointer is invalid!"
+                << std::endl;
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+    DeviceCommandId_t currentCommand = handler->getPendingCommand();
+    switch(currentCommand) {
+    case(ADIS16507::READ_SENSOR_DATA): {
+        return comIf->performRegularSendOperation(cookie, sendData, sendLen);
+    }
+    case(ADIS16507::READ_OUT_CONFIG): {
+
+    }
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+#endif /* OBSW_ADIS16507_LINUX_COM_IF == 1 */

mission/devices/GyroADIS16507Handler.h

@@ -1,12 +1,19 @@
 #ifndef MISSION_DEVICES_GYROADIS16507HANDLER_H_
 #define MISSION_DEVICES_GYROADIS16507HANDLER_H_
 
-#include <fsfw/devicehandlers/DeviceHandlerBase.h>
+#include "OBSWConfig.h"
+#include "fsfw/devicehandlers/DeviceHandlerBase.h"
 #include "devicedefinitions/GyroADIS16507Definitions.h"
 
+#if OBSW_ADIS16507_LINUX_COM_IF == 1
+class SpiComIF;
+class SpiCookie;
+#endif
+
 class GyroADIS16507Handler: public DeviceHandlerBase {
 public:
-    GyroADIS16507Handler(object_id_t objectId, object_id_t deviceCommunication, CookieIF * comCookie);
+    GyroADIS16507Handler(object_id_t objectId, object_id_t deviceCommunication,
+            CookieIF * comCookie);
 
     /* DeviceHandlerBase abstract function implementation */
     void doStartUp() override;
@@ -20,10 +27,15 @@ public:
             DeviceCommandId_t *foundId, size_t *foundLen) override;
     ReturnValue_t interpretDeviceReply(DeviceCommandId_t id,
             const uint8_t *packet) override;
-    void setNormalDatapoolEntriesInvalid() override;
     uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override;
+    ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
+            LocalDataPoolManager &poolManager) override;
 
 private:
+    std::array<uint8_t, 32> commandBuffer;
+
+    AdisGyroPrimaryDataset primaryDataset;
+    AdisGyroConfigDataset configDataset;
 
     enum class InternalState {
         STARTUP,
@@ -32,6 +44,14 @@ private:
 
     InternalState internalState = InternalState::STARTUP;
     bool commandExecuted = false;
+
+    void prepareReadCommand(uint8_t* regList, size_t len);
+
+#if OBSW_ADIS16507_LINUX_COM_IF == 1
+    static ReturnValue_t spiSendCallback(SpiComIF* comIf, SpiCookie *cookie,
+            const uint8_t *sendData, size_t sendLen, void* args);
+#endif
+
 };
 
 

mission/devices/GyroL3GD20Handler.cpp

@@ -65,8 +65,8 @@ ReturnValue_t GyroHandlerL3GD20H::buildTransitionDeviceCommand(DeviceCommandId_t
 	}
 	default:
 		/* Might be a configuration error. */
-		sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown "
+		sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown internal state!" <<
-				<< "internal state!" << std::endl;
+		        std::endl;
 		return HasReturnvaluesIF::RETURN_OK;
 	}
 	return HasReturnvaluesIF::RETURN_OK;

mission/devices/devicedefinitions/GyroADIS16507Definitions.h

@@ -1,11 +1,122 @@
 #ifndef MISSION_DEVICES_DEVICEDEFINITIONS_GYROADIS16507DEFINITIONS_H_
 #define MISSION_DEVICES_DEVICEDEFINITIONS_GYROADIS16507DEFINITIONS_H_
 
+#include "fsfw/datapoollocal/StaticLocalDataSet.h"
+
 #include <cstddef>
 
 namespace ADIS16507 {
 
 static constexpr size_t MAXIMUM_REPLY_SIZE = 64;
+
+static constexpr uint32_t GYRO_RANGE = 125;
+static constexpr uint32_t ACCELEROMETER_RANGE = 392;
+
+static constexpr uint16_t PROD_ID = 16507;
+
+static constexpr std::array<uint8_t, 2> BURST_READ_ENABLE = {0x68, 0x00};
+
+static constexpr uint8_t DIAG_STAT_REG = 0x02;
+static constexpr uint8_t FILTER_CTRL_REG = 0x5c;
+static constexpr uint8_t MSC_CTRL_REG = 0x60;
+static constexpr uint8_t DEC_RATE_REG = 0x64;
+static constexpr uint8_t GLOB_CMD = 0x68;
+static constexpr uint8_t PROD_ID_REG = 0x72;
+
+static constexpr DeviceCommandId_t READ_SENSOR_DATA = 0;
+static constexpr DeviceCommandId_t READ_OUT_CONFIG = 1;
+static constexpr DeviceCommandId_t SELF_TEST_SENSORS = 2;
+static constexpr DeviceCommandId_t SELF_TEST_MEMORY = 3;
+static constexpr DeviceCommandId_t UPDATE_NV_CONFIGURATION = 4;
+static constexpr DeviceCommandId_t SELECT_BURST_READ_MODE = 5;
+
+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 size_t CONFIG_READOUT_SIZE = 8;
+static constexpr size_t SENSOR_READOUT_SIZE = 22;
+
+static constexpr uint32_t ADIS_DATASET_ID = READ_SENSOR_DATA;
+static constexpr uint32_t ADIS_CFG_DATASET_ID = READ_OUT_CONFIG;
+
+enum PrimaryPoolIds: lp_id_t {
+    ANG_VELOC_X,
+    ANG_VELOC_Y,
+    ANG_VELOC_Z,
+    ACCELERATION_X,
+    ACCELERATION_Y,
+    ACCELERATION_Z,
+    TEMPERATURE
+};
+
+enum ConfigPoolIds: lp_id_t {
+    FILTER_SETTINGS,
+    MSC_CTRL_REGISTER,
+    DEC_RATE_REGISTER,
+};
+
+enum FilterSettings: uint8_t {
+    NO_FILTER = 0,
+    TWO_TAPS = 1,
+    FOUR_TAPS = 2,
+    EIGHT_TAPS = 3,
+    SIXTEEN_TAPS = 4,
+    THIRTYTWO_TAPS = 5,
+    SIXTYFOUR_TAPS = 6
+};
+
 }
 
+class AdisGyroPrimaryDataset: public StaticLocalDataSet<7 * sizeof(float)> {
+public:
+
+    /** Constructor  for data users like controllers */
+    AdisGyroPrimaryDataset(object_id_t adisId):
+        StaticLocalDataSet(sid_t(adisId, ADIS16507::ADIS_DATASET_ID)) {
+        setAllVariablesReadOnly();
+    }
+
+    /* Angular velocities in degrees per second (DPS) */
+    lp_var_t<double> angVelocX = lp_var_t<double>(sid.objectId,
+            ADIS16507::ANG_VELOC_X, this);
+    lp_var_t<double> angVelocY = lp_var_t<double>(sid.objectId,
+            ADIS16507::ANG_VELOC_Y, this);
+    lp_var_t<double> angVelocZ = lp_var_t<double>(sid.objectId,
+            ADIS16507::ANG_VELOC_Z, this);
+    lp_var_t<double> accelX = lp_var_t<double>(sid.objectId,
+            ADIS16507::ACCELERATION_X, this);
+    lp_var_t<double> accelY = lp_var_t<double>(sid.objectId,
+            ADIS16507::ACCELERATION_Y, this);
+    lp_var_t<double> accelZ = lp_var_t<double>(sid.objectId,
+            ADIS16507::ACCELERATION_Z, this);
+    lp_var_t<float> temperature = lp_var_t<float>(sid.objectId,
+            ADIS16507::TEMPERATURE, this);
+private:
+
+    friend class GyroADIS16507Handler;
+    /** Constructor for the data creator */
+    AdisGyroPrimaryDataset(HasLocalDataPoolIF* hkOwner):
+            StaticLocalDataSet(hkOwner, ADIS16507::ADIS_DATASET_ID) {}
+};
+
+class AdisGyroConfigDataset: public StaticLocalDataSet<32> {
+public:
+
+    /** Constructor  for data users like controllers */
+    AdisGyroConfigDataset(object_id_t adisId):
+        StaticLocalDataSet(sid_t(adisId, ADIS16507::ADIS_DATASET_ID)) {
+        setAllVariablesReadOnly();
+    }
+
+    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);
+private:
+    friend class GyroADIS16507Handler;
+    /** Constructor for the data creator */
+    AdisGyroConfigDataset(HasLocalDataPoolIF* hkOwner):
+            StaticLocalDataSet(hkOwner, ADIS16507::ADIS_CFG_DATASET_ID) {}
+};
+
 #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GYROADIS16507DEFINITIONS_H_ */

mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h

@@ -142,7 +142,7 @@ enum MgmPoolIds: lp_id_t {
     TEMPERATURE_CELCIUS
 };
 
-class MgmPrimaryDataset: public StaticLocalDataSet<3 * sizeof(float)> {
+class MgmPrimaryDataset: public StaticLocalDataSet<4 * sizeof(float)> {
 public:
     MgmPrimaryDataset(HasLocalDataPoolIF* hkOwner):
         StaticLocalDataSet(hkOwner, MGM_DATA_SET_ID) {}