From 7cf3247cbd96a291839841f66163a196fc222503 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 20 Mar 2023 13:08:38 +0100 Subject: [PATCH] seems to work now --- linux/devices/AcsBoardPolling.cpp | 162 ++++++++++++------------ mission/devices/GyrAdis1650XHandler.cpp | 5 +- 2 files changed, 84 insertions(+), 83 deletions(-) diff --git a/linux/devices/AcsBoardPolling.cpp b/linux/devices/AcsBoardPolling.cpp index 307ddebb..adcbbf19 100644 --- a/linux/devices/AcsBoardPolling.cpp +++ b/linux/devices/AcsBoardPolling.cpp @@ -451,95 +451,99 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) { cdHasTimedOut = gyro.countdown.hasTimedOut(); mustPerformStartup = gyro.performStartup; } - if (mode == acs::SimpleSensorMode::NORMAL and cdHasTimedOut) { - if (mustPerformStartup) { - uint8_t regList[6]; - // Read configuration - regList[0] = adis1650x::DIAG_STAT_REG; - regList[1] = adis1650x::FILTER_CTRL_REG; - regList[2] = adis1650x::RANG_MDL_REG; - regList[3] = adis1650x::MSC_CTRL_REG; - regList[4] = adis1650x::DEC_RATE_REG; - regList[5] = adis1650x::PROD_ID_REG; - size_t transferLen = - adis1650x::prepareReadCommand(regList, sizeof(regList), cmdBuf.data(), cmdBuf.size()); - result = readAdisCfg(*gyro.cookie, transferLen); - if (result != returnvalue::OK) { - gyro.replyResult = result; - return; - } - result = spiComIF.readReceivedMessage(gyro.cookie, &rawReply, &dummy); - if (result != returnvalue::OK or rawReply == nullptr) { - gyro.replyResult = result; - return; - } - uint16_t prodId = (rawReply[12] << 8) | rawReply[13]; - if (((gyro.type == adis1650x::Type::ADIS16505) and (prodId != adis1650x::PROD_ID_16505)) or - ((gyro.type == adis1650x::Type::ADIS16507) and (prodId != adis1650x::PROD_ID_16507))) { - sif::warning << "AcsPollingTask: Invalid ADIS product ID " << prodId << std::endl; - gyro.replyResult = returnvalue::FAILED; - return; - } - 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.prodId = prodId; - gyro.ownReply.data.sensitivity = adis1650x::rangMdlToSensitivity(gyro.ownReply.cfg.rangMdl); - gyro.performStartup = false; - } - // Read regular registers - std::memcpy(cmdBuf.data(), adis1650x::BURST_READ_ENABLE.data(), - adis1650x::BURST_READ_ENABLE.size()); - std::memset(cmdBuf.data() + 2, 0, 10 * 2); - result = spiComIF.sendMessage(gyro.cookie, cmdBuf.data(), adis1650x::SENSOR_READOUT_SIZE); + if (mode == acs::SimpleSensorMode::OFF) { + return; + } + if (not cdHasTimedOut) { + return; + } + if (mustPerformStartup) { + uint8_t regList[6]; + // Read configuration + regList[0] = adis1650x::DIAG_STAT_REG; + regList[1] = adis1650x::FILTER_CTRL_REG; + regList[2] = adis1650x::RANG_MDL_REG; + regList[3] = adis1650x::MSC_CTRL_REG; + regList[4] = adis1650x::DEC_RATE_REG; + regList[5] = adis1650x::PROD_ID_REG; + size_t transferLen = + adis1650x::prepareReadCommand(regList, sizeof(regList), cmdBuf.data(), cmdBuf.size()); + result = readAdisCfg(*gyro.cookie, transferLen); if (result != returnvalue::OK) { - gyro.replyResult = returnvalue::FAILED; + gyro.replyResult = result; return; } result = spiComIF.readReceivedMessage(gyro.cookie, &rawReply, &dummy); if (result != returnvalue::OK or rawReply == nullptr) { + gyro.replyResult = result; + return; + } + uint16_t prodId = (rawReply[12] << 8) | rawReply[13]; + if (((gyro.type == adis1650x::Type::ADIS16505) and (prodId != adis1650x::PROD_ID_16505)) or + ((gyro.type == adis1650x::Type::ADIS16507) and (prodId != adis1650x::PROD_ID_16507))) { + sif::warning << "AcsPollingTask: Invalid ADIS product ID " << prodId << std::endl; gyro.replyResult = returnvalue::FAILED; return; } - uint16_t checksum = (rawReply[20] << 8) | rawReply[21]; - - // Now verify the read checksum with the expected checksum according to datasheet p. 20 - uint16_t calcChecksum = 0; - for (size_t idx = 2; idx < 20; idx++) { - calcChecksum += rawReply[idx]; - } - if (checksum != calcChecksum) { - sif::warning << "AcsPollingTask: Invalid ADIS reply checksum" << std::endl; - gyro.replyResult = returnvalue::FAILED; - return; - } - - auto burstMode = adis1650x::burstModeFromMscCtrl(gyro.ownReply.cfg.mscCtrlReg); - if (burstMode != adis1650x::BurstModes::BURST_16_BURST_SEL_0) { - sif::error << "GyroADIS1650XHandler::interpretDeviceReply: Analysis for select burst mode" - " not implemented!" - << std::endl; - gyro.replyResult = returnvalue::FAILED; - return; - } - MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); - gyro.ownReply.dataWasSet = true; - gyro.ownReply.cfg.diagStat = rawReply[2] << 8 | rawReply[3]; - gyro.ownReply.data.angVelocities[0] = (rawReply[4] << 8) | rawReply[5]; - gyro.ownReply.data.angVelocities[1] = (rawReply[6] << 8) | rawReply[7]; - gyro.ownReply.data.angVelocities[2] = (rawReply[8] << 8) | rawReply[9]; - - gyro.ownReply.data.accelerations[0] = (rawReply[10] << 8) | rawReply[11]; - gyro.ownReply.data.accelerations[1] = (rawReply[12] << 8) | rawReply[13]; - gyro.ownReply.data.accelerations[2] = (rawReply[14] << 8) | rawReply[15]; - - gyro.ownReply.data.temperatureRaw = (rawReply[16] << 8) | rawReply[17]; + 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.prodId = prodId; + gyro.ownReply.data.sensitivity = adis1650x::rangMdlToSensitivity(gyro.ownReply.cfg.rangMdl); + gyro.performStartup = false; } + // Read regular registers + std::memcpy(cmdBuf.data(), adis1650x::BURST_READ_ENABLE.data(), + adis1650x::BURST_READ_ENABLE.size()); + std::memset(cmdBuf.data() + 2, 0, 10 * 2); + result = spiComIF.sendMessage(gyro.cookie, cmdBuf.data(), adis1650x::SENSOR_READOUT_SIZE); + if (result != returnvalue::OK) { + gyro.replyResult = returnvalue::FAILED; + return; + } + result = spiComIF.readReceivedMessage(gyro.cookie, &rawReply, &dummy); + if (result != returnvalue::OK or rawReply == nullptr) { + gyro.replyResult = returnvalue::FAILED; + return; + } + uint16_t checksum = (rawReply[20] << 8) | rawReply[21]; + + // Now verify the read checksum with the expected checksum according to datasheet p. 20 + uint16_t calcChecksum = 0; + for (size_t idx = 2; idx < 20; idx++) { + calcChecksum += rawReply[idx]; + } + if (checksum != calcChecksum) { + sif::warning << "AcsPollingTask: Invalid ADIS reply checksum" << std::endl; + gyro.replyResult = returnvalue::FAILED; + return; + } + + auto burstMode = adis1650x::burstModeFromMscCtrl(gyro.ownReply.cfg.mscCtrlReg); + if (burstMode != adis1650x::BurstModes::BURST_16_BURST_SEL_0) { + sif::error << "GyroADIS1650XHandler::interpretDeviceReply: Analysis for select burst mode" + " not implemented!" + << std::endl; + gyro.replyResult = returnvalue::FAILED; + return; + } + + MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); + gyro.ownReply.dataWasSet = true; + gyro.ownReply.cfg.diagStat = rawReply[2] << 8 | rawReply[3]; + gyro.ownReply.data.angVelocities[0] = (rawReply[4] << 8) | rawReply[5]; + gyro.ownReply.data.angVelocities[1] = (rawReply[6] << 8) | rawReply[7]; + gyro.ownReply.data.angVelocities[2] = (rawReply[8] << 8) | rawReply[9]; + + gyro.ownReply.data.accelerations[0] = (rawReply[10] << 8) | rawReply[11]; + gyro.ownReply.data.accelerations[1] = (rawReply[12] << 8) | rawReply[13]; + gyro.ownReply.data.accelerations[2] = (rawReply[14] << 8) | rawReply[15]; + + gyro.ownReply.data.temperatureRaw = (rawReply[16] << 8) | rawReply[17]; } void AcsBoardPolling::mgmLis3Handler(MgmLis3& mgm) { diff --git a/mission/devices/GyrAdis1650XHandler.cpp b/mission/devices/GyrAdis1650XHandler.cpp index 73e9a0cd..7d576238 100644 --- a/mission/devices/GyrAdis1650XHandler.cpp +++ b/mission/devices/GyrAdis1650XHandler.cpp @@ -65,10 +65,7 @@ ReturnValue_t GyrAdis1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_ } case (InternalState::SHUTDOWN): { *id = adis1650x::REQUEST; - acs::Adis1650XRequest *request = reinterpret_cast(cmdBuf.data()); - request->mode = acs::SimpleSensorMode::OFF; - request->type = adisType; - return returnvalue::OK; + return preparePeriodicRequest(acs::SimpleSensorMode::OFF); } default: { return NOTHING_TO_SEND;