diff --git a/mission/devices/GyroADIS16507Handler.cpp b/mission/devices/GyroADIS16507Handler.cpp index 88973f97..97426e81 100644 --- a/mission/devices/GyroADIS16507Handler.cpp +++ b/mission/devices/GyroADIS16507Handler.cpp @@ -1,3 +1,4 @@ +#include #include #include @@ -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) { diff --git a/mission/devices/GyroADIS16507Handler.h b/mission/devices/GyroADIS16507Handler.h index a9a62567..653f56e7 100644 --- a/mission/devices/GyroADIS16507Handler.h +++ b/mission/devices/GyroADIS16507Handler.h @@ -40,6 +40,7 @@ private: enum class InternalState { STARTUP, + CONFIG, IDLE }; diff --git a/mission/devices/devicedefinitions/GyroADIS16507Definitions.h b/mission/devices/devicedefinitions/GyroADIS16507Definitions.h index 9c675a2e..a7970b81 100644 --- a/mission/devices/devicedefinitions/GyroADIS16507Definitions.h +++ b/mission/devices/devicedefinitions/GyroADIS16507Definitions.h @@ -19,6 +19,13 @@ static constexpr uint16_t PROD_ID = 16507; static constexpr std::array 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;