diff --git a/CHANGELOG.md b/CHANGELOG.md index 6a7f07cd..1a2af1ca 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -65,6 +65,8 @@ will consitute of a breaking change warranting a new major release: - IMTQ HK sets - IMTQ dummy now handles power switch - Added some new ACS parameters +- Enabled decimation filter for the ADIS GYRs +- Enabled second low-pass filter for L3GD20H GYRs ## Fixed @@ -101,6 +103,7 @@ will consitute of a breaking change warranting a new major release: quaternion. - Various fixes for the pointing modes of the `ACS Controller`. All modes should work now as intended. +- The variance for the ADIS GYRs now represents the used `-3` version and not the `-1` version # [v2.0.5] 2023-05-11 diff --git a/fsfw b/fsfw index 5eb9ee8b..3a702295 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 5eb9ee8bc1e6f8640cbea46febd11e4b92241881 +Subproject commit 3a70229510c017099ff2d43533e19d41f2030d81 diff --git a/linux/acs/AcsBoardPolling.cpp b/linux/acs/AcsBoardPolling.cpp index e525bf53..b8539a8f 100644 --- a/linux/acs/AcsBoardPolling.cpp +++ b/linux/acs/AcsBoardPolling.cpp @@ -113,6 +113,7 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send if (req->mode != adis.mode) { if (req->mode == acs::SimpleSensorMode::NORMAL) { adis.type = req->type; + adis.decRate = req->cfg.decRateReg; // The initial countdown is handled by the device handler now. // adis.countdown.setTimeout(adis1650x::START_UP_TIME); if (adis.type == adis1650x::Type::ADIS16507) { @@ -376,6 +377,80 @@ void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) { } } +ReturnValue_t AcsBoardPolling::writeAdisReg(SpiCookie& cookie) { + ReturnValue_t result = returnvalue::OK; + int retval = 0; + // Prepare transfer + int fileDescriptor = 0; + std::string device = spiComIF.getSpiDev(); + UnixFileGuard fileHelper(device, fileDescriptor, O_RDWR, "SpiComIF::sendMessage"); + if (fileHelper.getOpenResult() != returnvalue::OK) { + return spi::OPENING_FILE_FAILED; + } + spi::SpiModes spiMode = spi::SpiModes::MODE_0; + uint32_t spiSpeed = 0; + cookie.getSpiParameters(spiMode, spiSpeed, nullptr); + spiComIF.setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed); + cookie.assignWriteBuffer(cmdBuf.data()); + cookie.setTransferSize(2); + + gpioId_t gpioId = cookie.getChipSelectPin(); + MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING; + uint32_t timeoutMs = 0; + MutexIF* mutex = spiComIF.getCsMutex(); + cookie.getMutexParams(timeoutType, timeoutMs); + if (mutex == nullptr) { + sif::warning << "GyroADIS16507Handler::spiSendCallback: " + "Mutex or GPIO interface invalid" + << std::endl; + return returnvalue::FAILED; + } + + size_t idx = 0; + spi_ioc_transfer* transferStruct = cookie.getTransferStructHandle(); + uint64_t origTx = transferStruct->tx_buf; + uint64_t origRx = transferStruct->rx_buf; + for (idx = 0; idx < 4; idx += 2) { + result = mutex->lockMutex(timeoutType, timeoutMs); + if (result != returnvalue::OK) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "AcsBoardPolling: Failed to lock mutex" << std::endl; +#endif + return result; + } + // Pull SPI CS low. For now, no support for active high given + if (gpioId != gpio::NO_GPIO) { + gpioIF.pullLow(gpioId); + } + + // Execute transfer + // Initiate a full duplex SPI transfer. + retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), cookie.getTransferStructHandle()); + if (retval < 0) { + utility::handleIoctlError("SpiComIF::sendMessage: ioctl error."); + result = spi::FULL_DUPLEX_TRANSFER_FAILED; + } +#if FSFW_HAL_SPI_WIRETAPPING == 1 + comIf->performSpiWiretapping(cookie); +#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */ + + if (gpioId != gpio::NO_GPIO) { + gpioIF.pullHigh(gpioId); + } + mutex->unlockMutex(); + + transferStruct->tx_buf += 2; + transferStruct->rx_buf += 2; + if (idx < 4) { + usleep(adis1650x::STALL_TIME_MICROSECONDS); + } + } + transferStruct->tx_buf = origTx; + transferStruct->rx_buf = origRx; + cookie.setTransferSize(0); + return returnvalue::OK; +} + ReturnValue_t AcsBoardPolling::readAdisCfg(SpiCookie& cookie, size_t transferLen) { ReturnValue_t result = returnvalue::OK; int retval = 0; @@ -455,15 +530,20 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) { ReturnValue_t result; acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF; bool mustPerformStartup = false; + uint16_t decRate = 0; { MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); mode = gyro.mode; + decRate = gyro.decRate; mustPerformStartup = gyro.performStartup; } if (mode == acs::SimpleSensorMode::OFF) { return; } if (mustPerformStartup) { + adis1650x::prepareWriteRegCommand(adis1650x::DEC_RATE_REG, decRate, cmdBuf.data(), + cmdBuf.size()); + writeAdisReg(*gyro.cookie); uint8_t regList[6]; // Read configuration regList[0] = adis1650x::DIAG_STAT_REG; @@ -491,13 +571,21 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) { gyro.replyResult = returnvalue::FAILED; return; } + uint16_t decRateReadBack = (rawReply[10] << 8) | rawReply[11]; + sif::debug << "AcsPollingTask: DEC rate configuration. Read " << decRateReadBack + << ", expected " << decRate << std::endl; + if (decRateReadBack != decRate) { + sif::warning << "AcsPollingTask: DEC rate configuration failed. Read " << decRateReadBack + << ", expected " << decRate << std::endl; + gyro.replyResult = returnvalue::FAILED; + } 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.decRateReg = decRateReadBack; gyro.ownReply.cfg.prodId = prodId; gyro.ownReply.data.sensitivity = adis1650x::rangMdlToSensitivity(gyro.ownReply.cfg.rangMdl); gyro.performStartup = false; diff --git a/linux/acs/AcsBoardPolling.h b/linux/acs/AcsBoardPolling.h index 794c9c47..5b4d0390 100644 --- a/linux/acs/AcsBoardPolling.h +++ b/linux/acs/AcsBoardPolling.h @@ -37,6 +37,7 @@ class AcsBoardPolling : public SystemObject, struct GyroAdis : public DevBase { adis1650x::Type type; + uint16_t decRate; Countdown countdown; acs::Adis1650XReply ownReply; acs::Adis1650XReply readerReply; @@ -84,6 +85,8 @@ class AcsBoardPolling : public SystemObject, void gyroAdisHandler(GyroAdis& gyro); void mgmLis3Handler(MgmLis3& mgm); void mgmRm3100Handler(MgmRm3100& mgm); + // This fumction configures the register as specified on p.21 of the datasheet. + ReturnValue_t writeAdisReg(SpiCookie& cookie); // Special readout: 16us stall time between small 2 byte transfers. ReturnValue_t readAdisCfg(SpiCookie& spiCookie, size_t transferLen); }; diff --git a/mission/acs/GyrAdis1650XHandler.cpp b/mission/acs/GyrAdis1650XHandler.cpp index c61187ce..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,13 +24,14 @@ void GyrAdis1650XHandler::doStartUp() { if (not commandExecuted) { warningSwitch = true; breakCountdown.setTimeout(adis1650x::START_UP_TIME); + updatePeriodicReply(true, adis1650x::REPLY); commandExecuted = true; } - if (breakCountdown.hasTimedOut()) { - updatePeriodicReply(true, adis1650x::REPLY); - setMode(MODE_ON); - internalState = InternalState::NONE; - } + } + if (internalState == InternalState::STARTUP_DONE) { + setMode(MODE_ON); + commandExecuted = false; + internalState = InternalState::NONE; } } @@ -61,6 +62,7 @@ ReturnValue_t GyrAdis1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_ return NOTHING_TO_SEND; } *id = adis1650x::REQUEST; + adisRequest.cfg.decRateReg = adis1650x::DEC_RATE; return preparePeriodicRequest(acs::SimpleSensorMode::NORMAL); } case (InternalState::SHUTDOWN): { @@ -91,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; diff --git a/mission/acs/GyrAdis1650XHandler.h b/mission/acs/GyrAdis1650XHandler.h index 5d906f61..308d472b 100644 --- a/mission/acs/GyrAdis1650XHandler.h +++ b/mission/acs/GyrAdis1650XHandler.h @@ -48,7 +48,7 @@ class GyrAdis1650XHandler : public DeviceHandlerBase { bool warningSwitch = true; - enum class InternalState { NONE, STARTUP, SHUTDOWN }; + enum class InternalState { NONE, STARTUP, STARTUP_DONE, SHUTDOWN }; InternalState internalState = InternalState::STARTUP; bool commandExecuted = false; diff --git a/mission/acs/acsBoardPolling.h b/mission/acs/acsBoardPolling.h index 90f0f19c..9f794fb0 100644 --- a/mission/acs/acsBoardPolling.h +++ b/mission/acs/acsBoardPolling.h @@ -8,11 +8,6 @@ namespace acs { -struct Adis1650XRequest { - SimpleSensorMode mode; - adis1650x::Type type; -}; - struct Adis1650XConfig { uint16_t diagStat; uint16_t filterSetting; @@ -22,6 +17,12 @@ struct Adis1650XConfig { uint16_t prodId; }; +struct Adis1650XRequest { + SimpleSensorMode mode; + adis1650x::Type type; + Adis1650XConfig cfg; +}; + struct Adis1650XData { double sensitivity = 0.0; // Angular velocities in all axes (X, Y and Z) diff --git a/mission/acs/gyroAdisHelpers.cpp b/mission/acs/gyroAdisHelpers.cpp index 0f41b058..5c1446cc 100644 --- a/mission/acs/gyroAdisHelpers.cpp +++ b/mission/acs/gyroAdisHelpers.cpp @@ -52,3 +52,14 @@ double adis1650x::rangMdlToSensitivity(uint16_t rangMdl) { } } } + +void adis1650x::prepareWriteRegCommand(uint8_t regStart, uint16_t val, uint8_t* outBuf, + size_t maxLen) { + if (maxLen < 4) { + return; + } + outBuf[0] = regStart | adis1650x::WRITE_MASK; + outBuf[1] = val & 0xff; + outBuf[2] = (regStart + 1) | adis1650x::WRITE_MASK; + outBuf[3] = (val >> 8) & 0xff; +} diff --git a/mission/acs/gyroAdisHelpers.h b/mission/acs/gyroAdisHelpers.h index 09f0aa1e..f360db76 100644 --- a/mission/acs/gyroAdisHelpers.h +++ b/mission/acs/gyroAdisHelpers.h @@ -16,6 +16,8 @@ enum class BurstModes { }; size_t prepareReadCommand(const uint8_t* regList, size_t len, uint8_t* outBuf, size_t maxLen); +void prepareWriteRegCommand(uint8_t regStart, uint16_t val, uint8_t* outBuf, size_t maxLen); + BurstModes burstModeFromMscCtrl(uint16_t mscCtrl); double rangMdlToSensitivity(uint16_t rangMdl); @@ -92,6 +94,9 @@ static constexpr size_t SENSOR_READOUT_SIZE = 20 + 2; static constexpr uint32_t ADIS_DATASET_ID = READ_SENSOR_DATA; static constexpr uint32_t ADIS_CFG_DATASET_ID = READ_OUT_CONFIG; +static constexpr uint16_t FILT_CTRL = 0x0000; +static constexpr uint16_t DEC_RATE = 0x00C7; + enum GlobCmds : uint8_t { FACTORY_CALIBRATION = 0b0000'0010, SENSOR_SELF_TEST = 0b0000'0100, diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 9c91399c..15725f2f 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -781,9 +781,9 @@ class AcsParameters : public HasParametersIF { /* var = sigma^2, sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is * assumed to be equal for the same class of sensors */ - float gyr02variance[3] = {pow(3.0e-3, 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms - pow(3.0e-3, 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms - pow(4.3e-3, 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms + float gyr02variance[3] = {pow(4.6e-3, 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms + pow(4.6e-3, 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms + pow(6.1e-3, 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms float gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)}; uint8_t preferAdis = false; float gyrFilterWeight = 0.6;