linux/acs/AcsBoardPolling.cpp

@@ -113,6 +113,7 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
     if (req->mode != adis.mode) {
       if (req->mode == acs::SimpleSensorMode::NORMAL) {
         adis.type = req->type;
+        adis.decRate = req->cfg.decRateReg;
         // The initial countdown is handled by the device handler now.
         // adis.countdown.setTimeout(adis1650x::START_UP_TIME);
         if (adis.type == adis1650x::Type::ADIS16507) {
@@ -376,6 +377,81 @@ void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) {
   }
 }
 
+ReturnValue_t AcsBoardPolling::writeAdisReg(SpiCookie& cookie) {
+  ReturnValue_t result = returnvalue::OK;
+  int retval = 0;
+  // Prepare transfer
+  int fileDescriptor = 0;
+  std::string device = spiComIF.getSpiDev();
+  UnixFileGuard fileHelper(device, fileDescriptor, O_RDWR, "SpiComIF::sendMessage");
+  if (fileHelper.getOpenResult() != returnvalue::OK) {
+    return spi::OPENING_FILE_FAILED;
+  }
+  spi::SpiModes spiMode = spi::SpiModes::MODE_0;
+  uint32_t spiSpeed = 0;
+  cookie.getSpiParameters(spiMode, spiSpeed, nullptr);
+  spiComIF.setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
+  cookie.assignWriteBuffer(cmdBuf.data());
+  cookie.setTransferSize(2);
+
+  gpioId_t gpioId = cookie.getChipSelectPin();
+  MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
+  uint32_t timeoutMs = 0;
+  MutexIF* mutex = spiComIF.getCsMutex();
+  cookie.getMutexParams(timeoutType, timeoutMs);
+  if (mutex == nullptr) {
+    sif::warning << "GyroADIS16507Handler::spiSendCallback: "
+                    "Mutex or GPIO interface invalid"
+                 << std::endl;
+    return returnvalue::FAILED;
+  }
+
+  size_t idx = 0;
+  spi_ioc_transfer* transferStruct = cookie.getTransferStructHandle();
+  uint64_t origTx = transferStruct->tx_buf;
+  uint64_t origRx = transferStruct->rx_buf;
+  for (idx = 0; idx < 2; idx++) {
+    result = mutex->lockMutex(timeoutType, timeoutMs);
+    if (result != returnvalue::OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+      sif::error << "AcsBoardPolling: Failed to lock mutex" << std::endl;
+#endif
+      return result;
+    }
+    // Pull SPI CS low. For now, no support for active high given
+    if (gpioId != gpio::NO_GPIO) {
+      gpioIF.pullLow(gpioId);
+    }
+
+    // Execute transfer
+    // Initiate a full duplex SPI transfer.
+    retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), cookie.getTransferStructHandle());
+    if (retval < 0) {
+      utility::handleIoctlError("SpiComIF::sendMessage: ioctl error.");
+      result = spi::FULL_DUPLEX_TRANSFER_FAILED;
+    }
+#if FSFW_HAL_SPI_WIRETAPPING == 1
+    comIf->performSpiWiretapping(cookie);
+#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */
+
+    if (gpioId != gpio::NO_GPIO) {
+      gpioIF.pullHigh(gpioId);
+    }
+    mutex->unlockMutex();
+
+    idx += 2;
+    transferStruct->tx_buf += 2;
+    transferStruct->rx_buf += 2;
+    if (idx < 4) {
+      usleep(adis1650x::STALL_TIME_MICROSECONDS);
+    }
+  }
+  transferStruct->tx_buf = origTx;
+  transferStruct->rx_buf = origRx;
+  cookie.setTransferSize(0);
+  return returnvalue::OK;
+}
+
 ReturnValue_t AcsBoardPolling::readAdisCfg(SpiCookie& cookie, size_t transferLen) {
   ReturnValue_t result = returnvalue::OK;
   int retval = 0;
@@ -455,15 +531,20 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {
   ReturnValue_t result;
   acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
   bool mustPerformStartup = false;
+  uint16_t decRate = 0;
   {
     MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
     mode = gyro.mode;
+    decRate = gyro.decRate;
     mustPerformStartup = gyro.performStartup;
   }
   if (mode == acs::SimpleSensorMode::OFF) {
     return;
   }
   if (mustPerformStartup) {
+    adis1650x::prepareWriteRegCommand(adis1650x::DEC_RATE_REG, decRate, cmdBuf.data(),
+                                      cmdBuf.size());
+    writeAdisReg(*gyro.cookie);
     uint8_t regList[6];
     // Read configuration
     regList[0] = adis1650x::DIAG_STAT_REG;
@@ -491,13 +572,19 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {
       gyro.replyResult = returnvalue::FAILED;
       return;
     }
+    uint16_t decRateReadBack = (rawReply[10] << 8) | rawReply[11];
+    if (decRateReadBack != decRate) {
+      sif::warning << "AcsPollingTask: DEC rate configuration failed. Read " << decRateReadBack
+                   << ", expected " << decRate << std::endl;
+      gyro.replyResult = returnvalue::FAILED;
+    }
     MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
     gyro.ownReply.cfgWasSet = true;
     gyro.ownReply.cfg.diagStat = (rawReply[2] << 8) | rawReply[3];
     gyro.ownReply.cfg.filterSetting = (rawReply[4] << 8) | rawReply[5];
     gyro.ownReply.cfg.rangMdl = (rawReply[6] << 8) | rawReply[7];
     gyro.ownReply.cfg.mscCtrlReg = (rawReply[8] << 8) | rawReply[9];
-    gyro.ownReply.cfg.decRateReg = (rawReply[10] << 8) | rawReply[11];
+    gyro.ownReply.cfg.decRateReg = decRateReadBack;
     gyro.ownReply.cfg.prodId = prodId;
     gyro.ownReply.data.sensitivity = adis1650x::rangMdlToSensitivity(gyro.ownReply.cfg.rangMdl);
     gyro.performStartup = false;

linux/acs/AcsBoardPolling.h

@@ -37,6 +37,7 @@ class AcsBoardPolling : public SystemObject,
 
   struct GyroAdis : public DevBase {
     adis1650x::Type type;
+    uint16_t decRate;
     Countdown countdown;
     acs::Adis1650XReply ownReply;
     acs::Adis1650XReply readerReply;
@@ -84,6 +85,8 @@ class AcsBoardPolling : public SystemObject,
   void gyroAdisHandler(GyroAdis& gyro);
   void mgmLis3Handler(MgmLis3& mgm);
   void mgmRm3100Handler(MgmRm3100& mgm);
+  // This fumction configures the register as specified on p.21 of the datasheet.
+  ReturnValue_t writeAdisReg(SpiCookie& cookie);
   // Special readout: 16us stall time between small 2 byte transfers.
   ReturnValue_t readAdisCfg(SpiCookie& spiCookie, size_t transferLen);
 };

mission/acs/GyrAdis1650XHandler.cpp

@@ -26,11 +26,11 @@ void GyrAdis1650XHandler::doStartUp() {
       breakCountdown.setTimeout(adis1650x::START_UP_TIME);
       commandExecuted = true;
     }
-    if (breakCountdown.hasTimedOut()) {
-      updatePeriodicReply(true, adis1650x::REPLY);
-      setMode(MODE_ON);
-      internalState = InternalState::NONE;
-    }
+  }
+  if (internalState == InternalState::STARTUP_DONE) {
+    updatePeriodicReply(true, adis1650x::REPLY);
+    setMode(MODE_ON);
+    internalState = InternalState::NONE;
   }
 }
 
@@ -61,6 +61,8 @@ ReturnValue_t GyrAdis1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_
         return NOTHING_TO_SEND;
       }
       *id = adis1650x::REQUEST;
+      adisRequest.cfg.decRateReg = adis1650x::DEC_RATE;
+      internalState = InternalState::STARTUP_DONE;
       return preparePeriodicRequest(acs::SimpleSensorMode::NORMAL);
     }
     case (InternalState::SHUTDOWN): {

mission/acs/GyrAdis1650XHandler.h

@@ -48,7 +48,7 @@ class GyrAdis1650XHandler : public DeviceHandlerBase {
 
   bool warningSwitch = true;
 
-  enum class InternalState { NONE, STARTUP, SHUTDOWN };
+  enum class InternalState { NONE, STARTUP, STARTUP_DONE, SHUTDOWN };
 
   InternalState internalState = InternalState::STARTUP;
   bool commandExecuted = false;

mission/acs/acsBoardPolling.h

@@ -8,11 +8,6 @@
 
 namespace acs {
 
-struct Adis1650XRequest {
-  SimpleSensorMode mode;
-  adis1650x::Type type;
-};
-
 struct Adis1650XConfig {
   uint16_t diagStat;
   uint16_t filterSetting;
@@ -22,6 +17,12 @@ struct Adis1650XConfig {
   uint16_t prodId;
 };
 
+struct Adis1650XRequest {
+  SimpleSensorMode mode;
+  adis1650x::Type type;
+  Adis1650XConfig cfg;
+};
+
 struct Adis1650XData {
   double sensitivity = 0.0;
   // Angular velocities in all axes (X, Y and Z)

mission/acs/gyroAdisHelpers.cpp

@@ -52,3 +52,14 @@ double adis1650x::rangMdlToSensitivity(uint16_t rangMdl) {
     }
   }
 }
+
+void adis1650x::prepareWriteRegCommand(uint8_t regStart, uint16_t val, uint8_t* outBuf,
+                                       size_t maxLen) {
+  if (maxLen < 4) {
+    return;
+  }
+  outBuf[0] = regStart | adis1650x::WRITE_MASK;
+  outBuf[1] = val & 0Xff;
+  outBuf[2] = (regStart + 1) | adis1650x::WRITE_MASK;
+  outBuf[3] = (val >> 8) & 0xff;
+}

mission/acs/gyroAdisHelpers.h

@@ -16,6 +16,8 @@ enum class BurstModes {
 };
 
 size_t prepareReadCommand(const uint8_t* regList, size_t len, uint8_t* outBuf, size_t maxLen);
+void prepareWriteRegCommand(uint8_t regStart, uint16_t val, uint8_t* outBuf, size_t maxLen);
+
 BurstModes burstModeFromMscCtrl(uint16_t mscCtrl);
 double rangMdlToSensitivity(uint16_t rangMdl);