From 2f9cdefc967f65de03f388e45b78880d542cf99f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 26 Feb 2023 19:09:12 +0100 Subject: [PATCH] some more bugfixes --- linux/devices/AcsBoardPolling.cpp | 8 ++++- mission/devices/GyrL3gCustomHandler.cpp | 39 +++---------------------- 2 files changed, 11 insertions(+), 36 deletions(-) diff --git a/linux/devices/AcsBoardPolling.cpp b/linux/devices/AcsBoardPolling.cpp index 9803dfe9..7a5376da 100644 --- a/linux/devices/AcsBoardPolling.cpp +++ b/linux/devices/AcsBoardPolling.cpp @@ -253,11 +253,17 @@ void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) { } cmdBuf[0] = l3gd20h::READ_START | l3gd20h::AUTO_INCREMENT_MASK | l3gd20h::READ_MASK; std::memset(cmdBuf.data() + 1, 0, l3gd20h::READ_LEN); - result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), 6); + result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), l3gd20h::READ_LEN + 1); if (result != returnvalue::OK) { l3g.replyResult = returnvalue::FAILED; return; } + result = spiComIF.readReceivedMessage(l3g.cookie, &rawReply, &dummy); + if (result != returnvalue::OK) { + l3g.replyResult = returnvalue::FAILED; + return; + } + MutexGuard mg(ipcLock); // The regular read function always returns the full sensor config as well. Use that // to verify communications. for (uint8_t idx = 0; idx < 5; idx++) { diff --git a/mission/devices/GyrL3gCustomHandler.cpp b/mission/devices/GyrL3gCustomHandler.cpp index 0af32d0c..f1fb31ab 100644 --- a/mission/devices/GyrL3gCustomHandler.cpp +++ b/mission/devices/GyrL3gCustomHandler.cpp @@ -116,11 +116,11 @@ ReturnValue_t GyrL3gCustomHandler::interpretDeviceReply(DeviceCommandId_t id, if (internalState == InternalState::STARTUP) { commandExecuted = true; } - PoolReadGuard readSet(&dataset); + PoolReadGuard pg(&dataset); dataset.setValidity(true, true); - dataset.angVelocX = reply->angVelocities[0]; - dataset.angVelocY = reply->angVelocities[1]; - dataset.angVelocZ = reply->angVelocities[2]; + dataset.angVelocX = static_cast(reply->angVelocities[0]) * reply->sensitivity; + dataset.angVelocY = static_cast(reply->angVelocities[1]) * reply->sensitivity; + dataset.angVelocZ = static_cast(reply->angVelocities[2]) * reply->sensitivity; if (std::abs(dataset.angVelocX.value) > absLimitX) { dataset.angVelocX.setValid(false); } @@ -132,34 +132,6 @@ ReturnValue_t GyrL3gCustomHandler::interpretDeviceReply(DeviceCommandId_t id, } dataset.temperature = 25.0 - reply->tempOffsetRaw; } - // PoolReadGuard readSet(&dataset); - // if (readSet.getReadResult() == returnvalue::OK) { - // if (std::abs(angVelocX) < this->absLimitX) { - // dataset.angVelocX = angVelocX; - // dataset.angVelocX.setValid(true); - // } else { - // dataset.angVelocX.setValid(false); - // } - // - // if (std::abs(angVelocY) < this->absLimitY) { - // dataset.angVelocY = angVelocY; - // dataset.angVelocY.setValid(true); - // } else { - // dataset.angVelocY.setValid(false); - // } - // - // if (std::abs(angVelocZ) < this->absLimitZ) { - // dataset.angVelocZ = angVelocZ; - // dataset.angVelocZ.setValid(true); - // } else { - // dataset.angVelocZ.setValid(false); - // } - // - // dataset.temperature = temperature; - // dataset.temperature.setValid(true); - // } - // break; - // } return returnvalue::OK; } @@ -183,9 +155,6 @@ ReturnValue_t GyrL3gCustomHandler::initializeLocalDataPool(localpool::DataPool & void GyrL3gCustomHandler::fillCommandAndReplyMap() { insertInCommandMap(l3gd20h::REQUEST); insertInReplyMap(l3gd20h::REPLY, 5, nullptr, 0, true); - // insertInCommandAndReplyMap(l3gd20h::READ_REGS, 1, &dataset); - // insertInCommandAndReplyMap(l3gd20h::CONFIGURE_CTRL_REGS, 1); - // insertInCommandAndReplyMap(l3gd20h::READ_CTRL_REGS, 1); } void GyrL3gCustomHandler::modeChanged() { internalState = InternalState::NONE; }