some bugfixes for adis datasets

bsp_linux_board/ObjectFactory.cpp

@@ -123,6 +123,11 @@ void ObjectFactory::produce(){
 #endif /* RPI_TEST_ACS_BOARD == 1 */
 
 #if RPI_TEST_ADIS16507 == 1
+    GpioCookie* gpioCookieAcsBoard = new GpioCookie();
+    gpio::createRpiGpioConfig(gpioCookieAcsBoard, gpioIds::GYRO_0_ADIS_CS, gpio::GYRO_0_BCM_PIN,
+            "GYRO_0_ADIS", gpio::Direction::OUT, 1);
+    gpioIF->addGpios(gpioCookieAcsBoard);
+
     std::string spiDev = "/dev/spidev0.0";
     SpiCookie* spiCookie = new SpiCookie(addresses::GYRO_0_ADIS, gpioIds::GYRO_0_ADIS_CS, spiDev,
             ADIS16507::MAXIMUM_REPLY_SIZE, spi::DEFAULT_ADIS16507_MODE, spi::DEFAULT_ADIS16507_SPEED,

fsfw_hal

@@ -1 +1 @@
-Subproject commit 2893da047bc695fcb02b0f9567cef0366a37793a
+Subproject commit c2b4231802bcc25e1a2588ab4d12c8ecf64d2c47

linux/fsfwconfig/FSFWConfig.h

@@ -68,4 +68,6 @@ static constexpr uint8_t FSFW_CSB_FIFO_DEPTH = 6;
 static constexpr size_t FSFW_PRINT_BUFFER_SIZE = 124;
 }
 
+#define FSFW_HAL_LINUX_SPI_WIRETAPPING  0
+
 #endif /* CONFIG_FSFWCONFIG_H_ */

linux/fsfwconfig/OBSWConfig.h

@@ -21,8 +21,6 @@ debugging. */
 #define OBSW_ADD_TEST_CODE              1
 #define OBSW_ADD_TEST_PST               1
 
-#define FSFW_HAL_LINUX_SPI_WIRETAPPING  0
-
 #define TEST_LIBGPIOD                   0
 #define TEST_RADIATION_SENSOR_HANDLER   1
 #define TEST_SUS_HANDLER                1
@@ -37,6 +35,8 @@ debugging. */
 #define ACU_DEBUG                       0
 #define SYRLINKS_DEBUG                  0
 #define IMQT_DEBUG  				    0
+#define ADIS16507_DEBUG                 1
+#define L3GD20_GYRO_DEBUG               0
 #define DEBUG_RAD_SENSOR                1
 #define DEBUG_SUS                       1
 

mission/devices/GyroADIS16507Handler.cpp

@@ -15,6 +15,10 @@ GyroADIS16507Handler::GyroADIS16507Handler(object_id_t objectId,
         object_id_t deviceCommunication, CookieIF * comCookie):
         DeviceHandlerBase(objectId, deviceCommunication, comCookie), primaryDataset(this),
         configDataset(this) {
+#if ADIS16507_DEBUG == 1
+    debugDivider = new PeriodicOperationDivider(5);
+#endif
+
 #if OBSW_ADIS16507_LINUX_COM_IF == 1
     SpiCookie* cookie = dynamic_cast<SpiCookie*>(comCookie);
     if(cookie != nullptr) {
@@ -30,6 +34,9 @@ void GyroADIS16507Handler::doStartUp() {
             internalState = InternalState::IDLE;
         }
     }
+    if(internalState == InternalState::IDLE) {
+        setMode(MODE_NORMAL);
+    }
 }
 
 void GyroADIS16507Handler::doShutDown() {
@@ -103,7 +110,7 @@ ReturnValue_t GyroADIS16507Handler::interpretDeviceReply(DeviceCommandId_t id,
     switch(id) {
     case(ADIS16507::READ_OUT_CONFIG): {
         PoolReadGuard rg(&configDataset);
-        uint16_t readProdId = packet[8] << 8 | packet[9];
+        uint16_t readProdId = packet[10] << 8 | packet[11];
         if (readProdId != ADIS16507::PROD_ID) {
 #if OBSW_VERBOSE_LEVEL >= 1
             sif::debug << "GyroADIS16507Handler::interpretDeviceReply: Invalid product ID!"
@@ -111,10 +118,10 @@ ReturnValue_t GyroADIS16507Handler::interpretDeviceReply(DeviceCommandId_t id,
 #endif
             return HasReturnvaluesIF::RETURN_FAILED;
         }
-        configDataset.diagStatReg.value = packet[0] << 8 | packet[1];
+        configDataset.diagStatReg.value = packet[2] << 8 | packet[3];
-        configDataset.filterSetting.value =  packet[2] << 8 | packet[3];
+        configDataset.filterSetting.value =  packet[4] << 8 | packet[5];
-        configDataset.mscCtrlReg.value = packet[4] << 8 | packet[5];
+        configDataset.mscCtrlReg.value = packet[6] << 8 | packet[7];
-        configDataset.decRateReg.value = packet[6] << 8 | packet[7];
+        configDataset.decRateReg.value = packet[8] << 8 | packet[9];
         configDataset.setValidity(true, true);
         if(internalState == InternalState::STARTUP) {
             commandExecuted = true;
@@ -162,33 +169,44 @@ ReturnValue_t GyroADIS16507Handler::handleSensorData(const uint8_t *packet) {
         }
         configDataset.diagStatReg.commit();
 
-        PoolReadGuard pg(&primaryDataset);
+        {
-        int16_t angVelocXRaw = packet[4] << 8 | packet[5];
+            PoolReadGuard pg(&primaryDataset);
-        primaryDataset.angVelocX.value = static_cast<float>(angVelocXRaw) / INT16_MAX *
+            int16_t angVelocXRaw = packet[4] << 8 | packet[5];
-                ADIS16507::GYRO_RANGE;
+            primaryDataset.angVelocX.value = static_cast<float>(angVelocXRaw) / INT16_MAX *
-        int16_t angVelocYRaw = packet[6] << 8 | packet[7];
+                    ADIS16507::GYRO_RANGE;
-        primaryDataset.angVelocY.value = static_cast<float>(angVelocYRaw) / INT16_MAX *
+            int16_t angVelocYRaw = packet[6] << 8 | packet[7];
-                ADIS16507::GYRO_RANGE;
+            primaryDataset.angVelocY.value = static_cast<float>(angVelocYRaw) / INT16_MAX *
-        int16_t angVelocZRaw = packet[8] << 8 | packet[9];
+                    ADIS16507::GYRO_RANGE;
-        primaryDataset.angVelocZ.value = static_cast<float>(angVelocZRaw) / INT16_MAX *
+            int16_t angVelocZRaw = packet[8] << 8 | packet[9];
-                ADIS16507::GYRO_RANGE;
+            primaryDataset.angVelocZ.value = static_cast<float>(angVelocZRaw) / INT16_MAX *
+                    ADIS16507::GYRO_RANGE;
 
-        int16_t accelXRaw = packet[10] << 8 | packet[11];
+            int16_t accelXRaw = packet[10] << 8 | packet[11];
-        primaryDataset.accelX.value = static_cast<float>(accelXRaw) / INT16_MAX *
+            primaryDataset.accelX.value = static_cast<float>(accelXRaw) / INT16_MAX *
-                ADIS16507::GYRO_RANGE;
+                    ADIS16507::GYRO_RANGE;
-        int16_t accelYRaw = packet[12] << 8 | packet[13];
+            int16_t accelYRaw = packet[12] << 8 | packet[13];
-        primaryDataset.accelY.value = static_cast<float>(accelYRaw) / INT16_MAX *
+            primaryDataset.accelY.value = static_cast<float>(accelYRaw) / INT16_MAX *
-                ADIS16507::GYRO_RANGE;
+                    ADIS16507::GYRO_RANGE;
-        int16_t accelZRaw = packet[14] << 8 | packet[15];
+            int16_t accelZRaw = packet[14] << 8 | packet[15];
-        primaryDataset.accelZ.value = static_cast<float>(accelZRaw) / INT16_MAX *
+            primaryDataset.accelZ.value = static_cast<float>(accelZRaw) / INT16_MAX *
-                ADIS16507::GYRO_RANGE;
+                    ADIS16507::GYRO_RANGE;
 
-        int16_t temperatureRaw = packet[16] << 8 | packet[17];
+            int16_t temperatureRaw = packet[16] << 8 | packet[17];
-        primaryDataset.temperature.value = static_cast<float>(temperatureRaw) * 0.1;
+            primaryDataset.temperature.value = static_cast<float>(temperatureRaw) * 0.1;
+            // Ignore data counter for now
+            primaryDataset.setValidity(true, true);
+        }
 
-        // Ignore data counter for now
+#if ADIS16507_DEBUG == 1
+        if(debugDivider->checkAndIncrement()) {
+            sif::info << "GyroADIS16507Handler: Angular velocities in degrees per second:" <<
+                    std::endl;
+            sif::info << "X: " << primaryDataset.angVelocX.value << " \xC2\xB0" << std::endl;
+            sif::info << "Y: " << primaryDataset.angVelocY.value << " \xC2\xB0" << std::endl;
+            sif::info << "Z: " << primaryDataset.angVelocZ.value << " \xC2\xB0" << std::endl;
+        }
+#endif
 
-        primaryDataset.setValidity(true, true);
         break;
     }
     case(BurstModes::BURST_32_BURST_SEL_0): {
@@ -219,7 +237,9 @@ ReturnValue_t GyroADIS16507Handler::initializeLocalDataPool(localpool::DataPool
     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::TEMPERATURE, new PoolEntry<float>({0.0}));
 
+    localDataPoolMap.emplace(ADIS16507::DIAG_STAT_REGISTER, new PoolEntry<uint16_t>());
     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>());
@@ -294,17 +314,20 @@ ReturnValue_t GyroADIS16507Handler::spiSendCallback(SpiComIF *comIf, SpiCookie *
 #endif
         }
 
+        if(gpioId != gpio::NO_GPIO) {
+            result = mutex->lockMutex(timeoutType, timeoutMs);
+            if (result != RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+                sif::error << "SpiComIF::sendMessage: Failed to lock mutex" << std::endl;
+#endif
+                return result;
+            }
+        }
+
         size_t idx = 0;
         while(idx < sendLen) {
             // Pull SPI CS low. For now, no support for active high given
             if(gpioId != gpio::NO_GPIO) {
-                result = mutex->lockMutex(timeoutType, timeoutMs);
-                if (result != RETURN_OK) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-                    sif::error << "SpiComIF::sendMessage: Failed to lock mutex" << std::endl;
-#endif
-                    return result;
-                }
                 gpioIF->pullLow(gpioId);
             }
 
@@ -321,21 +344,19 @@ ReturnValue_t GyroADIS16507Handler::spiSendCallback(SpiComIF *comIf, SpiCookie *
 
             if(gpioId != gpio::NO_GPIO) {
                 gpioIF->pullHigh(gpioId);
-                result = mutex->unlockMutex();
-                if (result != RETURN_OK) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-                    sif::error << "SpiComIF::getSendSuccess: Failed to unlock mutex" << std::endl;
-#endif
-                    return result;
-                }
             }
 
             idx += 2;
             if(idx < sendLen) {
                 usleep(ADIS16507::STALL_TIME_MICROSECONDS);
             }
-            sendData += 2;
+            spi_ioc_transfer* transferStruct = cookie->getTransferStructHandle();
-            cookie->getTransferStructHandle()->rx_buf += 2;
+            transferStruct->tx_buf += 2;
+            transferStruct->rx_buf += 2;
+        }
+
+        if(gpioId != gpio::NO_GPIO) {
+            mutex->unlockMutex();
         }
     }
     }

mission/devices/GyroADIS16507Handler.h

@@ -1,6 +1,7 @@
 #ifndef MISSION_DEVICES_GYROADIS16507HANDLER_H_
 #define MISSION_DEVICES_GYROADIS16507HANDLER_H_
 
+#include <fsfw/globalfunctions/PeriodicOperationDivider.h>
 #include "OBSWConfig.h"
 #include "fsfw/devicehandlers/DeviceHandlerBase.h"
 #include "devicedefinitions/GyroADIS16507Definitions.h"
@@ -61,6 +62,10 @@ private:
             const uint8_t *sendData, size_t sendLen, void* args);
 #endif
 
+#if ADIS16507_DEBUG == 1
+    PeriodicOperationDivider* debugDivider;
+#endif
+
     ReturnValue_t handleSensorData(const uint8_t* packet);
 };
 

mission/devices/GyroL3GD20Handler.cpp

@@ -7,7 +7,7 @@ GyroHandlerL3GD20H::GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceC
 		CookieIF *comCookie):
 		DeviceHandlerBase(objectId, deviceCommunication, comCookie),
 		dataset(this) {
-#if OBSW_VERBOSE_LEVEL >= 1
+#if L3GD20_GYRO_DEBUG == 1
     debugDivider = new PeriodicOperationDivider(5);
 #endif
 }
@@ -197,7 +197,7 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id,
 
 		int8_t temperaturOffset = (-1) * packet[L3GD20H::TEMPERATURE_IDX];
 		float temperature = 25.0 + temperaturOffset;
-#if OBSW_VERBOSE_LEVEL >= 1
+#if L3GD20_GYRO_DEBUG == 1
         if(debugDivider->checkAndIncrement()) {
             /* Set terminal to utf-8 if there is an issue with micro printout. */
 #if FSFW_CPP_OSTREAM_ENABLED == 1

mission/devices/GyroL3GD20Handler.h

@@ -70,7 +70,7 @@ private:
 
 	float scaleFactor = static_cast<float>(L3GD20H::RANGE_DPS_00) / INT16_MAX;
 
-#if OBSW_VERBOSE_LEVEL >= 1
+#if L3GD20_GYRO_DEBUG == 1
     PeriodicOperationDivider* debugDivider = nullptr;
 #endif
 };

mission/devices/devicedefinitions/GyroADIS16507Definitions.h

@@ -41,8 +41,8 @@ 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 CONFIG_READOUT_SIZE = 10 + 2;
-static constexpr size_t SENSOR_READOUT_SIZE = 22;
+static constexpr size_t SENSOR_READOUT_SIZE = 20 + 2;
 
 static constexpr uint32_t ADIS_DATASET_ID = READ_SENSOR_DATA;
 static constexpr uint32_t ADIS_CFG_DATASET_ID = READ_OUT_CONFIG;
@@ -54,10 +54,7 @@ enum PrimaryPoolIds: lp_id_t {
     ACCELERATION_X,
     ACCELERATION_Y,
     ACCELERATION_Z,
-    TEMPERATURE
+    TEMPERATURE,
-};
-
-enum ConfigPoolIds: lp_id_t {
     DIAG_STAT_REGISTER,
     FILTER_SETTINGS,
     MSC_CTRL_REGISTER,
@@ -113,11 +110,12 @@ public:
 
     /** Constructor  for data users like controllers */
     AdisGyroConfigDataset(object_id_t adisId):
-        StaticLocalDataSet(sid_t(adisId, ADIS16507::ADIS_DATASET_ID)) {
+        StaticLocalDataSet(sid_t(adisId, ADIS16507::ADIS_CFG_DATASET_ID)) {
         setAllVariablesReadOnly();
     }
 
-    lp_var_t<uint16_t> diagStatReg = lp_var_t<uint16_t>(sid.objectId, ADIS16507::DIAG_STAT_REG);
+    lp_var_t<uint16_t> diagStatReg = lp_var_t<uint16_t>(sid.objectId,
+            ADIS16507::DIAG_STAT_REGISTER);
     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);