From 5890e639feb5dbe22bd6dab9d9ba977be9dcc009 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 20 Mar 2023 10:17:41 +0100 Subject: [PATCH 1/6] bugfix for dual to single side transition --- mission/system/objects/DualLaneAssemblyBase.cpp | 12 ++++++++++-- mission/system/objects/DualLaneAssemblyBase.h | 1 + 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/mission/system/objects/DualLaneAssemblyBase.cpp b/mission/system/objects/DualLaneAssemblyBase.cpp index ca969dbb..582ba886 100644 --- a/mission/system/objects/DualLaneAssemblyBase.cpp +++ b/mission/system/objects/DualLaneAssemblyBase.cpp @@ -34,10 +34,17 @@ void DualLaneAssemblyBase::performChildOperation() { } void DualLaneAssemblyBase::startTransition(Mode_t mode, Submode_t submode) { - // doStartTransition(mode, submode); using namespace duallane; pwrStateMachine.reset(); if (mode != MODE_OFF) { + // Special exception: A transition from dual side to single mode must be handled like + // going OFF. + if ((this->mode == MODE_ON or this->mode == DeviceHandlerIF::MODE_NORMAL) and + this->submode == DUAL_MODE and submode != DUAL_MODE) { + dualToSingleSideTransition = true; + AssemblyBase::startTransition(mode, submode); + return; + } // If anything other than MODE_OFF is commanded, perform power state machine first // Cache the target modes, required by power state machine pwrStateMachine.start(mode, submode); @@ -111,7 +118,7 @@ ReturnValue_t DualLaneAssemblyBase::isModeCombinationValid(Mode_t mode, Submode_ void DualLaneAssemblyBase::handleModeReached() { using namespace duallane; - if (targetMode == MODE_OFF) { + if (targetMode == MODE_OFF or dualToSingleSideTransition) { pwrStateMachine.start(targetMode, targetSubmode); // Now we can switch off the power. After that, the AssemblyBase::handleModeReached function // will be called @@ -229,6 +236,7 @@ void DualLaneAssemblyBase::finishModeOp() { pwrStateMachine.reset(); powerRetryCounter = 0; tryingOtherSide = false; + dualToSingleSideTransition = false; dualModeErrorSwitch = true; } diff --git a/mission/system/objects/DualLaneAssemblyBase.h b/mission/system/objects/DualLaneAssemblyBase.h index a8a2f521..929ef3f9 100644 --- a/mission/system/objects/DualLaneAssemblyBase.h +++ b/mission/system/objects/DualLaneAssemblyBase.h @@ -31,6 +31,7 @@ class DualLaneAssemblyBase : public AssemblyBase, public ConfirmsFailuresIF { uint8_t powerRetryCounter = 0; bool tryingOtherSide = false; bool dualModeErrorSwitch = true; + bool dualToSingleSideTransition = false; duallane::Submodes defaultSubmode = duallane::Submodes::A_SIDE; enum RecoveryCustomStates { From 74bb27e61f87d70a5a3245276312c6fccead4cf5 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 20 Mar 2023 11:12:19 +0100 Subject: [PATCH 2/6] some more fixes --- .../system/objects/DualLaneAssemblyBase.cpp | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/mission/system/objects/DualLaneAssemblyBase.cpp b/mission/system/objects/DualLaneAssemblyBase.cpp index 582ba886..22728457 100644 --- a/mission/system/objects/DualLaneAssemblyBase.cpp +++ b/mission/system/objects/DualLaneAssemblyBase.cpp @@ -82,9 +82,13 @@ ReturnValue_t DualLaneAssemblyBase::pwrStateMachineWrapper() { // Will be called for transitions to MODE_OFF, where everything is done after power switching finishModeOp(); } else if (opCode == OpCodes::TO_NOT_OFF_DONE) { - // Will be called for transitions from MODE_OFF to anything else, where the mode still has - // to be commanded after power switching - AssemblyBase::startTransition(targetMode, targetSubmode); + if (dualToSingleSideTransition) { + finishModeOp(); + } else { + // Will be called for transitions from MODE_OFF to anything else, where the mode still has + // to be commanded after power switching + AssemblyBase::startTransition(targetMode, targetSubmode); + } } else if (opCode == OpCodes::TIMEOUT_OCCURED) { if (powerRetryCounter == 0) { powerRetryCounter++; @@ -118,13 +122,20 @@ ReturnValue_t DualLaneAssemblyBase::isModeCombinationValid(Mode_t mode, Submode_ void DualLaneAssemblyBase::handleModeReached() { using namespace duallane; - if (targetMode == MODE_OFF or dualToSingleSideTransition) { + if (targetMode == MODE_OFF) { pwrStateMachine.start(targetMode, targetSubmode); // Now we can switch off the power. After that, the AssemblyBase::handleModeReached function // will be called // Ignore failures for now. pwrStateMachineWrapper(); } else { + // For dual to single side transition, devices should be logically off, but the switch + // handling still needs to be done. + if (dualToSingleSideTransition) { + pwrStateMachine.start(targetMode, targetSubmode); + pwrStateMachineWrapper(); + return; + } finishModeOp(); } } From 7cf3247cbd96a291839841f66163a196fc222503 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 20 Mar 2023 13:08:38 +0100 Subject: [PATCH 3/6] 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; From 22f8d256ddd3395c106ee9102908fab692404fd9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 20 Mar 2023 13:09:28 +0100 Subject: [PATCH 4/6] changelog --- CHANGELOG.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 2f68f5ad..e5ca13ae 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,11 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## Fixed + +- Fixed transition for dual power lane assemblies: When going from dual side submode to single side + submode, perform logical commanding first, similarly to when going to OFF mode. + # [v1.38.0] 2023-03-17 eive-tmtc: v2.19.2 From f9e04ed9a2d0077ba8cc15718f067cd547dbb10a Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 20 Mar 2023 15:42:45 +0100 Subject: [PATCH 5/6] updated gyr bias --- mission/controller/acs/AcsParameters.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index fca8ed8e..5b185ea4 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -768,10 +768,10 @@ class AcsParameters : public HasParametersIF { double gyr2orientationMatrix[3][3] = {{0, 0, -1}, {0, -1, 0}, {-1, 0, 0}}; double gyr3orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}}; - double gyr0bias[3] = {0.06318149743589743, 0.4283235025641024, -0.16383500000000004}; - double gyr1bias[3] = {-0.12855128205128205, 1.6737307692307695, 1.031724358974359}; - double gyr2bias[3] = {0.15039212820512823, 0.7094475589743591, -0.22298363589743594}; - double gyr3bias[3] = {0.0021730769230769217, -0.6655897435897435, 0.034096153846153845}; + double gyr0bias[3] = {0.0, 0.4, -0.1}; + double gyr1bias[3] = {0.0956745283018868, 2.0854575471698116, 1.2505990566037737}; + double gyr2bias[3] = {0.1, 0.7, -0.2}; + double gyr3bias[3] = {-0.10721698113207549, -0.6111650943396226, 0.1716462264150944}; /* 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 */ From 11c54e270f96db00ceea885f5a26d540f730c9f4 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 20 Mar 2023 15:50:50 +0100 Subject: [PATCH 6/6] changelog --- CHANGELOG.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index e5ca13ae..6c7c5f65 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -21,6 +21,11 @@ will consitute of a breaking change warranting a new major release: - Fixed transition for dual power lane assemblies: When going from dual side submode to single side submode, perform logical commanding first, similarly to when going to OFF mode. +## Changed + +- Updated GYR bias values to newest measurements. This also corrects the ADIS values to always + consit of just one digit. + # [v1.38.0] 2023-03-17 eive-tmtc: v2.19.2