adis handler continued

mission/devices/GyroADIS16507Handler.cpp

@@ -1,3 +1,4 @@
+#include <fsfw/action/HasActionsIF.h>
 #include <fsfw/datapool/PoolReadGuard.h>
 #include <fsfw_hal/linux/utility.h>
 
@@ -14,7 +15,7 @@
 GyroADIS16507Handler::GyroADIS16507Handler(object_id_t objectId,
         object_id_t deviceCommunication, CookieIF * comCookie):
         DeviceHandlerBase(objectId, deviceCommunication, comCookie), primaryDataset(this),
-        configDataset(this), breakCountdown(0) {
+        configDataset(this), breakCountdown() {
 #if ADIS16507_DEBUG == 1
     debugDivider = new PeriodicOperationDivider(5);
 #endif
@@ -29,17 +30,29 @@ GyroADIS16507Handler::GyroADIS16507Handler(object_id_t objectId,
 
 void GyroADIS16507Handler::doStartUp() {
     if(internalState == InternalState::STARTUP) {
+        if(not commandExecuted) {
+            breakCountdown.setTimeout(ADIS16507::START_UP_TIME);
+            commandExecuted = true;
+        }
+        if(breakCountdown.hasTimedOut()) {
+            internalState = InternalState::CONFIG;
+        }
+    }
+
+    if(internalState == InternalState::CONFIG) {
         if(commandExecuted) {
             commandExecuted = false;
             internalState = InternalState::IDLE;
         }
     }
+
     if(internalState == InternalState::IDLE) {
         setMode(MODE_NORMAL);
     }
 }
 
 void GyroADIS16507Handler::doShutDown() {
+    commandExecuted = false;
 }
 
 ReturnValue_t GyroADIS16507Handler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
@@ -80,6 +93,10 @@ ReturnValue_t GyroADIS16507Handler::buildCommandFromCommand(DeviceCommandId_t de
         break;
     }
     case(ADIS16507::READ_SENSOR_DATA): {
+        if(breakCountdown.isBusy()) {
+            // A glob command is pending and sensor data can't be read currently
+            return NO_REPLY_EXPECTED;
+        }
         std::memcpy(commandBuffer.data(), ADIS16507::BURST_READ_ENABLE.data(),
                 ADIS16507::BURST_READ_ENABLE.size());
         std::memset(commandBuffer.data() + 2, 0, 10 * 2);
@@ -88,23 +105,48 @@ ReturnValue_t GyroADIS16507Handler::buildCommandFromCommand(DeviceCommandId_t de
         break;
     }
     case(ADIS16507::SELF_TEST_SENSORS): {
+        if(breakCountdown.isBusy()) {
+            // Another glob command is pending
+            return HasActionsIF::IS_BUSY;
+        }
         prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::SENSOR_SELF_TEST, 0x00);
+        breakCountdown.setTimeout(ADIS16507::SELF_TEST_BREAK);
         break;
     }
     case(ADIS16507::SELF_TEST_MEMORY): {
+        if(breakCountdown.isBusy()) {
+            // Another glob command is pending
+            return HasActionsIF::IS_BUSY;
+        }
         prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::FLASH_MEMORY_TEST, 0x00);
+        breakCountdown.setTimeout(ADIS16507::FLASH_MEMORY_TEST_BREAK);
         break;
     }
     case(ADIS16507::UPDATE_NV_CONFIGURATION): {
+        if(breakCountdown.isBusy()) {
+            // Another glob command is pending
+            return HasActionsIF::IS_BUSY;
+        }
         prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::FLASH_MEMORY_UPDATE, 0x00);
+        breakCountdown.setTimeout(ADIS16507::FLASH_MEMORY_UPDATE_BREAK);
         break;
     }
     case(ADIS16507::RESET_SENSOR_CONFIGURATION): {
+        if(breakCountdown.isBusy()) {
+            // Another glob command is pending
+            return HasActionsIF::IS_BUSY;
+        }
         prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::FACTORY_CALIBRATION, 0x00);
+        breakCountdown.setTimeout(ADIS16507::FACTORY_CALIBRATION_BREAK);
         break;
     }
     case(ADIS16507::SW_RESET): {
+        if(breakCountdown.isBusy()) {
+            // Another glob command is pending
+            return HasActionsIF::IS_BUSY;
+        }
         prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::SOFTWARE_RESET, 0x00);
+        breakCountdown.setTimeout(ADIS16507::SW_RESET_BREAK);
         break;
     }
     case(ADIS16507::PRINT_CURRENT_CONFIGURATION): {
@@ -127,6 +169,10 @@ void GyroADIS16507Handler::fillCommandAndReplyMap() {
     insertInCommandAndReplyMap(ADIS16507::READ_OUT_CONFIG, 1, &configDataset);
     insertInCommandAndReplyMap(ADIS16507::SELF_TEST_SENSORS, 1);
     insertInCommandAndReplyMap(ADIS16507::SELF_TEST_MEMORY, 1);
+    insertInCommandAndReplyMap(ADIS16507::UPDATE_NV_CONFIGURATION, 1);
+    insertInCommandAndReplyMap(ADIS16507::RESET_SENSOR_CONFIGURATION, 1);
+    insertInCommandAndReplyMap(ADIS16507::SW_RESET, 1);
+    insertInCommandAndReplyMap(ADIS16507::PRINT_CURRENT_CONFIGURATION, 1);
 }
 
 ReturnValue_t GyroADIS16507Handler::scanForReply(const uint8_t *start, size_t remainingSize,
@@ -265,6 +311,8 @@ void GyroADIS16507Handler::prepareWriteCommand(uint8_t startReg, uint8_t valueOn
     commandBuffer[1] = valueOne;
     commandBuffer[2] = secondReg;
     commandBuffer[3] = valueTwo;
+    this->rawPacketLen = 4;
+    this->rawPacket = commandBuffer.data();
 }
 
 void GyroADIS16507Handler::prepareReadCommand(uint8_t *regList, size_t len) {

mission/devices/GyroADIS16507Handler.h

@@ -40,6 +40,7 @@ private:
 
     enum class InternalState {
         STARTUP,
+        CONFIG,
         IDLE
     };
 

mission/devices/devicedefinitions/GyroADIS16507Definitions.h

@@ -19,6 +19,13 @@ static constexpr uint16_t PROD_ID = 16507;
 
 static constexpr std::array<uint8_t, 2> BURST_READ_ENABLE = {0x68, 0x00};
 
+static constexpr dur_millis_t START_UP_TIME = 310;
+static constexpr dur_millis_t SW_RESET_BREAK = 255;
+static constexpr dur_millis_t FACTORY_CALIBRATION_BREAK = 136;
+static constexpr dur_millis_t FLASH_MEMORY_UPDATE_BREAK = 70;
+static constexpr dur_millis_t FLASH_MEMORY_TEST_BREAK = 30;
+static constexpr dur_millis_t SELF_TEST_BREAK = 24;
+
 static constexpr uint8_t DIAG_STAT_REG = 0x02;
 static constexpr uint8_t FILTER_CTRL_REG = 0x5c;
 static constexpr uint8_t MSC_CTRL_REG = 0x60;