Merge pull request 'ACS Board: Better reply result handling' (#588) from possible_fixes_acs_brd_polling into develop

Reviewed-on: #588
Reviewed-by: Marius Eggert <eggertm@irs.uni-stuttgart.de>

CHANGELOG.md

@@ -24,6 +24,8 @@ will consitute of a breaking change warranting a new major release:
   SD card switch from 0 to 1 and vice-versa works without errors from persistent TM stores now.
 - Add a way for the SUS polling to detect broken or off devices by checking the retrieved
   temperature for the all-ones value (0x0fff).
+- Better reply result handling for the ACS board devices.
+- ADIS1650X initial timeout handling now performed in device handler.
 
 ## Changed
 

linux/acs/AcsBoardPolling.cpp

@@ -113,7 +113,8 @@ 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.countdown.setTimeout(adis1650x::START_UP_TIME);
+        // The initial countdown is handled by the device handler now.
+        // adis.countdown.setTimeout(adis1650x::START_UP_TIME);
         if (adis.type == adis1650x::Type::ADIS16507) {
           adis.ownReply.data.accelScaling = adis1650x::ACCELEROMETER_RANGE_16507;
         } else if (adis.type == adis1650x::Type::ADIS16505) {
@@ -127,6 +128,7 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
         adis.ownReply.cfgWasSet = false;
         adis.ownReply.dataWasSet = false;
       }
+      adis.replyResult = returnvalue::FAILED;
       adis.mode = req->mode;
     }
     return returnvalue::OK;
@@ -145,6 +147,7 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
       } else {
         gyro.ownReply.cfgWasSet = false;
       }
+      gyro.replyResult = returnvalue::FAILED;
       gyro.mode = req->mode;
     }
     return returnvalue::OK;
@@ -163,6 +166,7 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
         mgm.ownReply.dataWasSet = false;
         mgm.ownReply.temperatureWasSet = false;
       }
+      mgm.replyResult = returnvalue::FAILED;
       mgm.mode = req->mode;
     }
     return returnvalue::OK;
@@ -180,6 +184,7 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
       } else {
         mgm.ownReply.dataWasRead = false;
       }
+      mgm.replyResult = returnvalue::FAILED;
       mgm.mode = req->mode;
     }
     return returnvalue::OK;
@@ -309,18 +314,18 @@ void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) {
       std::memcpy(cmdBuf.data() + 1, l3g.sensorCfg, 5);
       result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), 6);
       if (result != returnvalue::OK) {
-        l3g.replyResult = returnvalue::OK;
+        l3g.replyResult = result;
       }
       // Ignore useless reply and red config
       cmdBuf[0] = l3gd20h::CTRL_REG_1 | l3gd20h::AUTO_INCREMENT_MASK | l3gd20h::READ_MASK;
       std::memset(cmdBuf.data() + 1, 0, 5);
       result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), 6);
       if (result != returnvalue::OK) {
-        l3g.replyResult = returnvalue::OK;
+        l3g.replyResult = result;
       }
       result = spiComIF.readReceivedMessage(l3g.cookie, &rawReply, &dummy);
       if (result != returnvalue::OK) {
-        l3g.replyResult = returnvalue::OK;
+        l3g.replyResult = result;
       }
       MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
       // Cross check configuration as verification that communication is working
@@ -331,6 +336,7 @@ void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) {
           return;
         }
       }
+      l3g.replyResult = returnvalue::OK;
       l3g.performStartup = false;
       l3g.ownReply.cfgWasSet = true;
       l3g.ownReply.sensitivity = l3gd20h::ctrlReg4ToSensitivity(l3g.sensorCfg[3]);
@@ -357,6 +363,7 @@ void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) {
         return;
       }
     }
+    l3g.replyResult = returnvalue::OK;
     l3g.ownReply.statusReg = rawReply[l3gd20h::STATUS_IDX];
     l3g.ownReply.angVelocities[0] = (rawReply[l3gd20h::OUT_X_H] << 8) | rawReply[l3gd20h::OUT_X_L];
     l3g.ownReply.angVelocities[1] = (rawReply[l3gd20h::OUT_Y_H] << 8) | rawReply[l3gd20h::OUT_Y_L];
@@ -443,20 +450,15 @@ ReturnValue_t AcsBoardPolling::readAdisCfg(SpiCookie& cookie, size_t transferLen
 void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {
   ReturnValue_t result;
   acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
-  bool cdHasTimedOut = false;
   bool mustPerformStartup = false;
   {
     MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
     mode = gyro.mode;
-    cdHasTimedOut = gyro.countdown.hasTimedOut();
     mustPerformStartup = gyro.performStartup;
   }
   if (mode == acs::SimpleSensorMode::OFF) {
     return;
   }
-  if (not cdHasTimedOut) {
-    return;
-  }
   if (mustPerformStartup) {
     uint8_t regList[6];
     // Read configuration
@@ -495,6 +497,7 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {
     gyro.ownReply.cfg.prodId = prodId;
     gyro.ownReply.data.sensitivity = adis1650x::rangMdlToSensitivity(gyro.ownReply.cfg.rangMdl);
     gyro.performStartup = false;
+    gyro.replyResult = returnvalue::OK;
   }
   // Read regular registers
   std::memcpy(cmdBuf.data(), adis1650x::BURST_READ_ENABLE.data(),
@@ -533,6 +536,7 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {
   }
 
   MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
+  gyro.replyResult = returnvalue::OK;
   gyro.ownReply.dataWasSet = true;
   gyro.ownReply.cfg.diagStat = rawReply[2] << 8 | rawReply[3];
   gyro.ownReply.data.angVelocities[0] = (rawReply[4] << 8) | rawReply[5];
@@ -590,6 +594,7 @@ void AcsBoardPolling::mgmLis3Handler(MgmLis3& mgm) {
       }
       // Done here. We can always read back config and data during periodic handling
       mgm.performStartup = false;
+      mgm.replyResult = returnvalue::OK;
     }
     cmdBuf[0] = mgmLis3::readCommand(mgmLis3::CTRL_REG1, true);
     std::memset(cmdBuf.data() + 1, 0, mgmLis3::NR_OF_DATA_AND_CFG_REGISTERS);
@@ -607,7 +612,7 @@ void AcsBoardPolling::mgmLis3Handler(MgmLis3& mgm) {
     // Verify communication by re-checking config
     if (rawReply[1] != mgm.cfg[0] or rawReply[2] != mgm.cfg[1] or rawReply[3] != mgm.cfg[2] or
         rawReply[4] != mgm.cfg[3] or rawReply[5] != mgm.cfg[4]) {
-      mgm.replyResult = result;
+      mgm.replyResult = returnvalue::FAILED;
       return;
     }
     {
@@ -634,6 +639,7 @@ void AcsBoardPolling::mgmLis3Handler(MgmLis3& mgm) {
       return;
     }
     MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
+    mgm.replyResult = returnvalue::OK;
     mgm.ownReply.temperatureWasSet = true;
     mgm.ownReply.temperatureRaw = (rawReply[2] << 8) | rawReply[1];
   }
@@ -704,6 +710,7 @@ void AcsBoardPolling::mgmRm3100Handler(MgmRm3100& mgm) {
         return;
       }
       mgm.performStartup = false;
+      mgm.replyResult = returnvalue::OK;
     }
     // Regular read operation
     cmdBuf[0] = mgmRm3100::MEASUREMENT_REG_START | mgmRm3100::READ_MASK;
@@ -725,6 +732,7 @@ void AcsBoardPolling::mgmRm3100Handler(MgmRm3100& mgm) {
       mgm.ownReply.scaleFactors[idx] = 1.0 / mgmRm3100::DEFAULT_GAIN;
     }
     mgm.ownReply.dataWasRead = true;
+    mgm.replyResult = returnvalue::OK;
     // Bitshift trickery to account for 24 bit signed value.
     mgm.ownReply.mgmValuesRaw[0] =
         ((rawReply[1] << 24) | (rawReply[2] << 16) | (rawReply[3] << 8)) >> 8;

linux/acs/SusPolling.cpp

@@ -96,7 +96,7 @@ ReturnValue_t SusPolling::readReceivedMessage(CookieIF* cookie, uint8_t** buffer
   if (susIdx < 0) {
     return FAILED;
   }
-  if(susDevs[susIdx].replyResult != returnvalue::OK) {
+  if (susDevs[susIdx].replyResult != returnvalue::OK) {
     return susDevs[susIdx].replyResult;
   }
   MutexGuard mg(ipcLock);
@@ -170,7 +170,7 @@ ReturnValue_t SusPolling::handleSusPolling() {
       susDevs[idx].ownReply.tempRaw = ((rawReply[0] & 0x0f) << 8) | rawReply[1];
       // Reply is all ones. Sensor is probably off or faulty when
       // it should not be.
-      if(susDevs[idx].ownReply.tempRaw == 0x0fff) {
+      if (susDevs[idx].ownReply.tempRaw == 0x0fff) {
         susDevs[idx].replyResult = returnvalue::FAILED;
       } else {
         susDevs[idx].replyResult = returnvalue::OK;

mission/acs/GyrAdis1650XHandler.cpp

@@ -56,6 +56,9 @@ ReturnValue_t GyrAdis1650XHandler::buildNormalDeviceCommand(DeviceCommandId_t *i
 ReturnValue_t GyrAdis1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
   switch (internalState) {
     case (InternalState::STARTUP): {
+      if (breakCountdown.isBusy()) {
+        return NOTHING_TO_SEND;
+      }
       *id = adis1650x::REQUEST;
       return preparePeriodicRequest(acs::SimpleSensorMode::NORMAL);
     }