diff --git a/linux/acs/AcsBoardPolling.cpp b/linux/acs/AcsBoardPolling.cpp index 727e8405..b8539a8f 100644 --- a/linux/acs/AcsBoardPolling.cpp +++ b/linux/acs/AcsBoardPolling.cpp @@ -591,7 +591,6 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) { gyro.performStartup = false; gyro.replyResult = returnvalue::OK; } - sif::debug << "hello world 2" << std::endl; // Read regular registers std::memcpy(cmdBuf.data(), adis1650x::BURST_READ_ENABLE.data(), adis1650x::BURST_READ_ENABLE.size()); diff --git a/mission/acs/GyrAdis1650XHandler.cpp b/mission/acs/GyrAdis1650XHandler.cpp index 6f447178..fe10c214 100644 --- a/mission/acs/GyrAdis1650XHandler.cpp +++ b/mission/acs/GyrAdis1650XHandler.cpp @@ -15,7 +15,7 @@ GyrAdis1650XHandler::GyrAdis1650XHandler(object_id_t objectId, object_id_t devic } void GyrAdis1650XHandler::doStartUp() { - if (internalState != InternalState::STARTUP) { + if (internalState == InternalState::NONE) { internalState = InternalState::STARTUP; commandExecuted = false; } @@ -24,12 +24,13 @@ void GyrAdis1650XHandler::doStartUp() { if (not commandExecuted) { warningSwitch = true; breakCountdown.setTimeout(adis1650x::START_UP_TIME); + updatePeriodicReply(true, adis1650x::REPLY); commandExecuted = true; } - updatePeriodicReply(true, adis1650x::REPLY); } if (internalState == InternalState::STARTUP_DONE) { setMode(MODE_ON); + commandExecuted = false; internalState = InternalState::NONE; } } @@ -62,7 +63,6 @@ ReturnValue_t GyrAdis1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_ } *id = adis1650x::REQUEST; adisRequest.cfg.decRateReg = adis1650x::DEC_RATE; - internalState = InternalState::STARTUP_DONE; return preparePeriodicRequest(acs::SimpleSensorMode::NORMAL); } case (InternalState::SHUTDOWN): { @@ -93,6 +93,9 @@ ReturnValue_t GyrAdis1650XHandler::scanForReply(const uint8_t *start, size_t rem getMode() == _MODE_POWER_DOWN) { return IGNORE_FULL_PACKET; } + if (internalState == InternalState::STARTUP) { + internalState = InternalState::STARTUP_DONE; + } *foundLen = remainingSize; if (remainingSize != sizeof(acs::Adis1650XReply)) { return returnvalue::FAILED;