diff --git a/CHANGELOG.md b/CHANGELOG.md index 2f68f5ad..3727efef 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,18 @@ 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. + +## Changed + +- Bugfixes for STR mode transitions: Booting to mode ON with submode FIRMWARE now works properly. + Furthermore, the submode in the NORMAL mode now should be 0 instead of some ON mode submode. +- 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 diff --git a/fsfw b/fsfw index 43fd0b2f..227524a2 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 43fd0b2f59c3aeb2d3f4db10cfad56ee3709d68d +Subproject commit 227524a21da755d125bcb1a5ff67bcbc452f8cf9 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/linux/devices/startracker/StarTrackerHandler.cpp b/linux/devices/startracker/StarTrackerHandler.cpp index 0edf6de1..61b77fe2 100644 --- a/linux/devices/startracker/StarTrackerHandler.cpp +++ b/linux/devices/startracker/StarTrackerHandler.cpp @@ -268,18 +268,18 @@ void StarTrackerHandler::doStartUp() { default: return; } - setMode(_MODE_TO_ON, SUBMODE_BOOTLOADER); + startupState = StartupState::DONE; + internalState = InternalState::IDLE; + setMode(_MODE_TO_ON); } void StarTrackerHandler::doShutDown() { // If the star tracker is shutdown also stop all running processes in the image loader task strHelper->stopProcess(); - setMode(_MODE_POWER_DOWN); -} - -void StarTrackerHandler::doOffActivity() { - startupState = StartupState::IDLE; internalState = InternalState::IDLE; + startupState = StartupState::IDLE; + bootState = FwBootState::NONE; + setMode(_MODE_POWER_DOWN); } ReturnValue_t StarTrackerHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { @@ -302,81 +302,103 @@ ReturnValue_t StarTrackerHandler::buildNormalDeviceCommand(DeviceCommandId_t* id ReturnValue_t StarTrackerHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { switch (internalState) { - case InternalState::BOOT: - *id = startracker::BOOT; - bootCountdown.setTimeout(BOOT_TIMEOUT); - internalState = InternalState::BOOT_DELAY; - return buildCommandFromCommand(*id, nullptr, 0); - case InternalState::REQ_VERSION: - internalState = InternalState::VERIFY_BOOT; - // Again read program to check if firmware boot was successful - *id = startracker::REQ_VERSION; - return buildCommandFromCommand(*id, nullptr, 0); - case InternalState::LOGLEVEL: - internalState = InternalState::WAIT_FOR_EXECUTION; - *id = startracker::LOGLEVEL; - return buildCommandFromCommand(*id, reinterpret_cast(paramJsonFile.c_str()), - paramJsonFile.size()); - case InternalState::LIMITS: - internalState = InternalState::WAIT_FOR_EXECUTION; - *id = startracker::LIMITS; - return buildCommandFromCommand(*id, reinterpret_cast(paramJsonFile.c_str()), - paramJsonFile.size()); - case InternalState::TRACKING: - internalState = InternalState::WAIT_FOR_EXECUTION; - *id = startracker::TRACKING; - return buildCommandFromCommand(*id, reinterpret_cast(paramJsonFile.c_str()), - paramJsonFile.size()); - case InternalState::MOUNTING: - internalState = InternalState::WAIT_FOR_EXECUTION; - *id = startracker::MOUNTING; - return buildCommandFromCommand(*id, reinterpret_cast(paramJsonFile.c_str()), - paramJsonFile.size()); - case InternalState::IMAGE_PROCESSOR: - internalState = InternalState::WAIT_FOR_EXECUTION; - *id = startracker::IMAGE_PROCESSOR; - return buildCommandFromCommand(*id, reinterpret_cast(paramJsonFile.c_str()), - paramJsonFile.size()); - case InternalState::CAMERA: - internalState = InternalState::WAIT_FOR_EXECUTION; - *id = startracker::CAMERA; - return buildCommandFromCommand(*id, reinterpret_cast(paramJsonFile.c_str()), - paramJsonFile.size()); - case InternalState::CENTROIDING: - internalState = InternalState::WAIT_FOR_EXECUTION; - *id = startracker::CENTROIDING; - return buildCommandFromCommand(*id, reinterpret_cast(paramJsonFile.c_str()), - paramJsonFile.size()); - case InternalState::LISA: - internalState = InternalState::WAIT_FOR_EXECUTION; - *id = startracker::LISA; - return buildCommandFromCommand(*id, reinterpret_cast(paramJsonFile.c_str()), - paramJsonFile.size()); - case InternalState::MATCHING: - internalState = InternalState::WAIT_FOR_EXECUTION; - *id = startracker::MATCHING; - return buildCommandFromCommand(*id, reinterpret_cast(paramJsonFile.c_str()), - paramJsonFile.size()); - case InternalState::VALIDATION: - internalState = InternalState::WAIT_FOR_EXECUTION; - *id = startracker::VALIDATION; - return buildCommandFromCommand(*id, reinterpret_cast(paramJsonFile.c_str()), - paramJsonFile.size()); - case InternalState::ALGO: - internalState = InternalState::WAIT_FOR_EXECUTION; - *id = startracker::ALGO; - return buildCommandFromCommand(*id, reinterpret_cast(paramJsonFile.c_str()), - paramJsonFile.size()); - case InternalState::LOG_SUBSCRIPTION: - internalState = InternalState::WAIT_FOR_EXECUTION; - *id = startracker::LOGSUBSCRIPTION; - return buildCommandFromCommand(*id, reinterpret_cast(paramJsonFile.c_str()), - paramJsonFile.size()); - case InternalState::DEBUG_CAMERA: - internalState = InternalState::WAIT_FOR_EXECUTION; - *id = startracker::DEBUG_CAMERA; - return buildCommandFromCommand(*id, reinterpret_cast(paramJsonFile.c_str()), - paramJsonFile.size()); + case InternalState::BOOT_FIRMWARE: { + if (bootState == FwBootState::WAIT_FOR_EXECUTION or bootState == FwBootState::VERIFY_BOOT) { + return NOTHING_TO_SEND; + } + if (bootState == FwBootState::NONE) { + *id = startracker::BOOT; + bootCountdown.setTimeout(BOOT_TIMEOUT); + bootState = FwBootState::BOOT_DELAY; + return buildCommandFromCommand(*id, nullptr, 0); + } + if (bootState == FwBootState::BOOT_DELAY) { + if (bootCountdown.isBusy()) { + return NOTHING_TO_SEND; + } + bootState = FwBootState::REQ_VERSION; + } + switch (bootState) { + case (FwBootState::REQ_VERSION): { + bootState = FwBootState::VERIFY_BOOT; + // Again read program to check if firmware boot was successful + *id = startracker::REQ_VERSION; + return buildCommandFromCommand(*id, nullptr, 0); + } + case (FwBootState::LOGLEVEL): { + bootState = FwBootState::WAIT_FOR_EXECUTION; + *id = startracker::LOGLEVEL; + return buildCommandFromCommand( + *id, reinterpret_cast(paramJsonFile.c_str()), paramJsonFile.size()); + } + case (FwBootState::LIMITS): { + bootState = FwBootState::WAIT_FOR_EXECUTION; + *id = startracker::LIMITS; + return buildCommandFromCommand( + *id, reinterpret_cast(paramJsonFile.c_str()), paramJsonFile.size()); + } + case (FwBootState::TRACKING): { + bootState = FwBootState::WAIT_FOR_EXECUTION; + *id = startracker::TRACKING; + return buildCommandFromCommand( + *id, reinterpret_cast(paramJsonFile.c_str()), paramJsonFile.size()); + } + case FwBootState::MOUNTING: + bootState = FwBootState::WAIT_FOR_EXECUTION; + *id = startracker::MOUNTING; + return buildCommandFromCommand( + *id, reinterpret_cast(paramJsonFile.c_str()), paramJsonFile.size()); + case FwBootState::IMAGE_PROCESSOR: + bootState = FwBootState::WAIT_FOR_EXECUTION; + *id = startracker::IMAGE_PROCESSOR; + return buildCommandFromCommand( + *id, reinterpret_cast(paramJsonFile.c_str()), paramJsonFile.size()); + case FwBootState::CAMERA: + bootState = FwBootState::WAIT_FOR_EXECUTION; + *id = startracker::CAMERA; + return buildCommandFromCommand( + *id, reinterpret_cast(paramJsonFile.c_str()), paramJsonFile.size()); + case FwBootState::CENTROIDING: + bootState = FwBootState::WAIT_FOR_EXECUTION; + *id = startracker::CENTROIDING; + return buildCommandFromCommand( + *id, reinterpret_cast(paramJsonFile.c_str()), paramJsonFile.size()); + case FwBootState::LISA: + bootState = FwBootState::WAIT_FOR_EXECUTION; + *id = startracker::LISA; + return buildCommandFromCommand( + *id, reinterpret_cast(paramJsonFile.c_str()), paramJsonFile.size()); + case FwBootState::MATCHING: + bootState = FwBootState::WAIT_FOR_EXECUTION; + *id = startracker::MATCHING; + return buildCommandFromCommand( + *id, reinterpret_cast(paramJsonFile.c_str()), paramJsonFile.size()); + case FwBootState::VALIDATION: + bootState = FwBootState::WAIT_FOR_EXECUTION; + *id = startracker::VALIDATION; + return buildCommandFromCommand( + *id, reinterpret_cast(paramJsonFile.c_str()), paramJsonFile.size()); + case FwBootState::ALGO: + bootState = FwBootState::WAIT_FOR_EXECUTION; + *id = startracker::ALGO; + return buildCommandFromCommand( + *id, reinterpret_cast(paramJsonFile.c_str()), paramJsonFile.size()); + case FwBootState::LOG_SUBSCRIPTION: + bootState = FwBootState::WAIT_FOR_EXECUTION; + *id = startracker::LOGSUBSCRIPTION; + return buildCommandFromCommand( + *id, reinterpret_cast(paramJsonFile.c_str()), paramJsonFile.size()); + case FwBootState::DEBUG_CAMERA: + bootState = FwBootState::WAIT_FOR_EXECUTION; + *id = startracker::DEBUG_CAMERA; + return buildCommandFromCommand( + *id, reinterpret_cast(paramJsonFile.c_str()), paramJsonFile.size()); + default: { + sif::error << "STR: Unexpected boot state" << (int)bootState << std::endl; + return NOTHING_TO_SEND; + } + } + } case InternalState::BOOT_BOOTLOADER: internalState = InternalState::BOOTLOADER_CHECK; *id = startracker::SWITCH_TO_BOOTLOADER_PROGRAM; @@ -707,6 +729,15 @@ void StarTrackerHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) { void StarTrackerHandler::doOnTransition(Submode_t subModeFrom) { uint8_t dhbSubmode = getSubmode(); + // We hide that the transition to submode firmware actually goes through the submode bootloader. + // This is because the startracker always starts in bootloader mode but we want to allow direct + // transitions to firmware mode. + if (startupState == StartupState::DONE) { + subModeFrom = SUBMODE_BOOTLOADER; + } + if (dhbSubmode == SUBMODE_NONE) { + bootFirmware(MODE_ON); + } if (dhbSubmode == SUBMODE_BOOTLOADER && subModeFrom == SUBMODE_FIRMWARE) { bootBootloader(); } else if (dhbSubmode == SUBMODE_FIRMWARE && subModeFrom == SUBMODE_FIRMWARE) { @@ -736,19 +767,23 @@ void StarTrackerHandler::doNormalTransition(Mode_t modeFrom, Submode_t subModeFr void StarTrackerHandler::bootFirmware(Mode_t toMode) { switch (internalState) { case InternalState::IDLE: - internalState = InternalState::BOOT; + sif::info << "STR: Booting to firmware mode" << std::endl; + internalState = InternalState::BOOT_FIRMWARE; break; - case InternalState::BOOT_DELAY: - if (bootCountdown.hasTimedOut()) { - internalState = InternalState::REQ_VERSION; - } + case InternalState::BOOT_FIRMWARE: break; case InternalState::FAILED_FIRMWARE_BOOT: internalState = InternalState::IDLE; break; case InternalState::DONE: - setMode(toMode); + if (toMode == MODE_NORMAL) { + setMode(toMode, 0); + } else { + setMode(toMode, SUBMODE_FIRMWARE); + } + sif::info << "STR: Firmware boot success" << std::endl; internalState = InternalState::IDLE; + startupState = StartupState::IDLE; break; default: return; @@ -776,10 +811,11 @@ void StarTrackerHandler::setUpJsonCfgs(JsonConfigs& cfgs, const char* paramJsonF void StarTrackerHandler::bootBootloader() { if (internalState == InternalState::IDLE) { internalState = InternalState::BOOT_BOOTLOADER; - } else if (internalState == InternalState::BOOTING_BOOTLOADER_FAILED) { + } else if (internalState == InternalState::FAILED_BOOTLOADER_BOOT) { internalState = InternalState::IDLE; } else if (internalState == InternalState::DONE) { internalState = InternalState::IDLE; + startupState = StartupState::IDLE; setMode(MODE_ON); } } @@ -1934,7 +1970,7 @@ ReturnValue_t StarTrackerHandler::checkProgram() { if (startupState == StartupState::WAIT_CHECK_PROGRAM) { startupState = StartupState::DONE; } - if (internalState == InternalState::VERIFY_BOOT) { + if (bootState == FwBootState::VERIFY_BOOT) { sif::warning << "StarTrackerHandler::checkProgram: Failed to boot firmware" << std::endl; // Device handler will run into timeout and fall back to transition source mode triggerEvent(BOOTING_FIRMWARE_FAILED_EVENT); @@ -1947,11 +1983,11 @@ ReturnValue_t StarTrackerHandler::checkProgram() { if (startupState == StartupState::WAIT_CHECK_PROGRAM) { startupState = StartupState::BOOT_BOOTLOADER; } - if (internalState == InternalState::VERIFY_BOOT) { - internalState = InternalState::LOGLEVEL; + if (bootState == FwBootState::VERIFY_BOOT) { + bootState = FwBootState::LOGLEVEL; } else if (internalState == InternalState::BOOTLOADER_CHECK) { triggerEvent(BOOTING_BOOTLOADER_FAILED_EVENT); - internalState = InternalState::BOOTING_BOOTLOADER_FAILED; + internalState = InternalState::FAILED_BOOTLOADER_BOOT; } break; default: @@ -2025,54 +2061,55 @@ ReturnValue_t StarTrackerHandler::handleActionReplySet(LocalPoolDataSetBase& dat void StarTrackerHandler::handleStartup(const uint8_t* parameterId) { switch (*parameterId) { case (startracker::ID::LOG_LEVEL): { - internalState = InternalState::LIMITS; + bootState = FwBootState::LIMITS; break; } case (startracker::ID::LIMITS): { - internalState = InternalState::TRACKING; + bootState = FwBootState::TRACKING; break; } case (startracker::ID::TRACKING): { - internalState = InternalState::MOUNTING; + bootState = FwBootState::MOUNTING; break; } case (startracker::ID::MOUNTING): { - internalState = InternalState::IMAGE_PROCESSOR; + bootState = FwBootState::IMAGE_PROCESSOR; break; } case (startracker::ID::IMAGE_PROCESSOR): { - internalState = InternalState::CAMERA; + bootState = FwBootState::CAMERA; break; } case (startracker::ID::CAMERA): { - internalState = InternalState::CENTROIDING; + bootState = FwBootState::CENTROIDING; break; } case (startracker::ID::CENTROIDING): { - internalState = InternalState::LISA; + bootState = FwBootState::LISA; break; } case (startracker::ID::LISA): { - internalState = InternalState::MATCHING; + bootState = FwBootState::MATCHING; break; } case (startracker::ID::MATCHING): { - internalState = InternalState::VALIDATION; + bootState = FwBootState::VALIDATION; break; } case (startracker::ID::VALIDATION): { - internalState = InternalState::ALGO; + bootState = FwBootState::ALGO; break; } case (startracker::ID::ALGO): { - internalState = InternalState::LOG_SUBSCRIPTION; + bootState = FwBootState::LOG_SUBSCRIPTION; break; } case (startracker::ID::LOG_SUBSCRIPTION): { - internalState = InternalState::DEBUG_CAMERA; + bootState = FwBootState::DEBUG_CAMERA; break; } case (startracker::ID::DEBUG_CAMERA): { + bootState = FwBootState::NONE; internalState = InternalState::DONE; break; } diff --git a/linux/devices/startracker/StarTrackerHandler.h b/linux/devices/startracker/StarTrackerHandler.h index 11cf7fc3..a70ff8cf 100644 --- a/linux/devices/startracker/StarTrackerHandler.h +++ b/linux/devices/startracker/StarTrackerHandler.h @@ -60,7 +60,6 @@ class StarTrackerHandler : public DeviceHandlerBase { protected: void doStartUp() override; void doShutDown() override; - void doOffActivity() override; ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t* id) override; ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t* id) override; void fillCommandAndReplyMap() override; @@ -247,14 +246,31 @@ class StarTrackerHandler : public DeviceHandlerBase { NormalState normalState = NormalState::TEMPERATURE_REQUEST; + enum class StartupState { + IDLE, + CHECK_PROGRAM, + WAIT_CHECK_PROGRAM, + BOOT_BOOTLOADER, + WAIT_JCFG, + DONE + }; + StartupState startupState = StartupState::IDLE; + enum class InternalState { IDLE, - BOOT, + BOOT_FIRMWARE, + DONE, + FAILED_FIRMWARE_BOOT, + BOOT_BOOTLOADER, + BOOTLOADER_CHECK, + FAILED_BOOTLOADER_BOOT + }; + + enum class FwBootState { + NONE, + BOOT_DELAY, REQ_VERSION, VERIFY_BOOT, - STARTUP_CHECK, - BOOT_DELAY, - FIRMWARE_CHECK, LOGLEVEL, LIMITS, TRACKING, @@ -270,26 +286,11 @@ class StarTrackerHandler : public DeviceHandlerBase { LOG_SUBSCRIPTION, DEBUG_CAMERA, WAIT_FOR_EXECUTION, - DONE, - FAILED_FIRMWARE_BOOT, - BOOT_BOOTLOADER, - BOOTLOADER_CHECK, - BOOTING_BOOTLOADER_FAILED }; + FwBootState bootState = FwBootState::NONE; InternalState internalState = InternalState::IDLE; - enum class StartupState { - IDLE, - CHECK_PROGRAM, - WAIT_CHECK_PROGRAM, - BOOT_BOOTLOADER, - WAIT_JCFG, - DONE - }; - - StartupState startupState = StartupState::IDLE; - bool strHelperExecuting = false; const power::Switch_t powerSwitch = power::NO_SWITCH; 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 */ 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; diff --git a/mission/system/objects/DualLaneAssemblyBase.cpp b/mission/system/objects/DualLaneAssemblyBase.cpp index ca969dbb..22728457 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); @@ -75,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,6 +129,13 @@ void DualLaneAssemblyBase::handleModeReached() { // 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(); } } @@ -229,6 +247,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 {