diff --git a/CHANGELOG.md b/CHANGELOG.md index 421b98cb..1d7f262c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -29,6 +29,7 @@ will consitute of a breaking change warranting a new major release: - The `PTG_CTRL_NO_ATTITUDE_INFORMATION` will now actually trigger a fallback into safe mode and is triggered by the `AcsController` now. - Fixed a corner case, in which an invalid speed command could be sent to the `RwHandler`. +- Fixed calculation of desaturation torque for faulty RWs. - Fixed bugs within the `MEKF` and simplified the code. ## Changed @@ -43,10 +44,13 @@ will consitute of a breaking change warranting a new major release: also limits the rotation for the reference target quaternion to prevent spikes in required rotation rates. - Updated QUEST and Sun Vector Params to new values. +- Removed the satellites's angular momentum from desaturation calculation. +- Bumped internal `sagittactl` library to v11.11. ## Added - Updated STR handler to unlock and allow using the secondary firmware slot. +- STR handling for new BlobStats TM set. - Added new action command to update the standard deviations within the `MEKF` from the `AcsParameters`. diff --git a/linux/acs/StrComHandler.cpp b/linux/acs/StrComHandler.cpp index 9c5e045f..65a1b5a4 100644 --- a/linux/acs/StrComHandler.cpp +++ b/linux/acs/StrComHandler.cpp @@ -270,7 +270,7 @@ ReturnValue_t StrComHandler::performImageDownload() { file.close(); return returnvalue::OK; } - arc_pack_download_action_req(&downloadReq, cmdBuf.data(), &size); + prv_arc_pack_download_action_req(&downloadReq, cmdBuf.data(), &size); result = sendAndRead(size, downloadReq.position); if (result != returnvalue::OK) { if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) { @@ -349,7 +349,7 @@ ReturnValue_t StrComHandler::performImageUpload() { } file.seekg(uploadReq.position * SIZE_IMAGE_PART, file.beg); file.read(reinterpret_cast(uploadReq.data), SIZE_IMAGE_PART); - arc_pack_upload_action_req(&uploadReq, cmdBuf.data(), &size); + prv_arc_pack_upload_action_req(&uploadReq, cmdBuf.data(), &size); result = sendAndRead(size, uploadReq.position); if (result != returnvalue::OK) { return returnvalue::FAILED; @@ -375,7 +375,7 @@ ReturnValue_t StrComHandler::performImageUpload() { file.seekg(fullChunks * SIZE_IMAGE_PART, file.beg); file.read(reinterpret_cast(uploadReq.data), remainder); file.close(); - arc_pack_upload_action_req(&uploadReq, cmdBuf.data(), &size); + prv_arc_pack_upload_action_req(&uploadReq, cmdBuf.data(), &size); result = sendAndRead(size, uploadReq.position); if (result != returnvalue::OK) { return returnvalue::FAILED; @@ -445,7 +445,7 @@ ReturnValue_t StrComHandler::performFlashWrite() { bytesWrittenInRegion = 0; } req.address = bytesWrittenInRegion; - arc_pack_write_action_req(&req, cmdBuf.data(), &size); + prv_arc_pack_write_action_req(&req, cmdBuf.data(), &size); result = sendAndRead(size, req.address); if (result != returnvalue::OK) { return result; @@ -488,7 +488,7 @@ ReturnValue_t StrComHandler::performFlashWrite() { req.length = remainingBytes; totalBytesWritten += CHUNK_SIZE; bytesWrittenInRegion += remainingBytes; - arc_pack_write_action_req(&req, cmdBuf.data(), &size); + prv_arc_pack_write_action_req(&req, cmdBuf.data(), &size); result = sendAndRead(size, req.address); if (result != returnvalue::OK) { return result; @@ -536,7 +536,7 @@ ReturnValue_t StrComHandler::performFlashRead() { } else { req.length = CHUNK_SIZE; } - arc_pack_read_action_req(&req, cmdBuf.data(), &size); + prv_arc_pack_read_action_req(&req, cmdBuf.data(), &size); result = sendAndRead(size, req.address); if (result != returnvalue::OK) { if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) { @@ -752,7 +752,7 @@ ReturnValue_t StrComHandler::unlockAndEraseRegions(uint32_t from, uint32_t to) { for (uint32_t idx = from; idx < to; idx++) { unlockReq.region = idx; unlockReq.code = startracker::region_secrets::SECRETS[idx]; - arc_pack_unlock_action_req(&unlockReq, cmdBuf.data(), &size); + prv_arc_pack_unlock_action_req(&unlockReq, cmdBuf.data(), &size); result = sendAndRead(size, unlockReq.region); if (result != returnvalue::OK) { return result; @@ -764,7 +764,7 @@ ReturnValue_t StrComHandler::unlockAndEraseRegions(uint32_t from, uint32_t to) { return result; } eraseReq.region = idx; - arc_pack_erase_action_req(&eraseReq, cmdBuf.data(), &size); + prv_arc_pack_erase_action_req(&eraseReq, cmdBuf.data(), &size); result = sendAndRead(size, eraseReq.region); if (result != returnvalue::OK) { } diff --git a/mission/acs/str/ArcsecDatalinkLayer.cpp b/mission/acs/str/ArcsecDatalinkLayer.cpp index c1082fc7..9d5d26e3 100644 --- a/mission/acs/str/ArcsecDatalinkLayer.cpp +++ b/mission/acs/str/ArcsecDatalinkLayer.cpp @@ -32,7 +32,7 @@ ReturnValue_t ArcsecDatalinkLayer::checkRingBufForFrame(const uint8_t** decodedF size_t encodedDataSize = 0; slip_error_t slipError = slip_decode_frame(decodedRxFrame, &rxFrameSize, rxAnalysisBuffer + startIdx, - idx - startIdx + 1, &encodedDataSize, ARC_DEF_SAGITTA_SLIP_ID); + idx - startIdx + 1, &encodedDataSize, ARC_DEF_SAGITTA_SLIP_ID, 0); decodeRingBuf.deleteData(idx + 1); switch (slipError) { case (SLIP_OK): { @@ -76,7 +76,7 @@ void ArcsecDatalinkLayer::reset() { decodeRingBuf.clear(); } void ArcsecDatalinkLayer::encodeFrame(const uint8_t* data, size_t length, const uint8_t** txFrame, size_t& size) { - slip_encode_frame(data, length, txEncoded, &size, ARC_DEF_SAGITTA_SLIP_ID); + slip_encode_frame(data, length, txEncoded, sizeof(txEncoded), &size, ARC_DEF_SAGITTA_SLIP_ID); if (txFrame != nullptr) { *txFrame = txEncoded; } diff --git a/mission/acs/str/StarTrackerHandler.cpp b/mission/acs/str/StarTrackerHandler.cpp index a88014ab..d8409e21 100644 --- a/mission/acs/str/StarTrackerHandler.cpp +++ b/mission/acs/str/StarTrackerHandler.cpp @@ -63,6 +63,7 @@ StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF, centroidSet(this), centroidsSet(this), contrastSet(this), + blobStatsSet(this), strHelper(strHelper), powerSwitch(powerSwitch), sdCardIF(sdCardIF), @@ -572,7 +573,7 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi Clock::getClock(&tv); setTimeRequest.unixTime = (static_cast(tv.tv_sec) * 1000 * 1000) + (static_cast(tv.tv_usec)); - arc_pack_settime_action_req(&setTimeRequest, commandBuffer, &rawPacketLen); + prv_arc_pack_settime_action_req(&setTimeRequest, commandBuffer, &rawPacketLen); size_t serLen = 0; // Time in milliseconds. Manual serialization because arcsec API ignores endianness. SerializeAdapter::serialize(&setTimeRequest.unixTime, commandBuffer + 2, &serLen, @@ -597,6 +598,10 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi prepareRequestContrastTm(); return returnvalue::OK; } + case (startracker::REQ_BLOB_STATS): { + prepareRequestBlobStatsTm(); + return returnvalue::OK; + } case (startracker::BOOT): { prepareBootCommand(static_cast(firmwareTargetRaw)); return returnvalue::OK; @@ -885,6 +890,8 @@ void StarTrackerHandler::fillCommandAndReplyMap() { startracker::MAX_FRAME_SIZE * 2 + 2); this->insertInCommandAndReplyMap(startracker::REQ_CONTRAST, 3, &contrastSet, startracker::MAX_FRAME_SIZE * 2 + 2); + this->insertInCommandAndReplyMap(startracker::REQ_BLOB_STATS, 3, &blobStatsSet, + startracker::MAX_FRAME_SIZE * 2 + 2); } ReturnValue_t StarTrackerHandler::isModeCombinationValid(Mode_t mode, Submode_t submode) { @@ -1082,6 +1089,12 @@ void StarTrackerHandler::resetSecondaryTmSet() { histogramSet.setValidity(false, true); } } + { + PoolReadGuard pg(&blobStatsSet); + if (pg.getReadResult() == returnvalue::OK) { + histogramSet.setValidity(false, true); + } + } } void StarTrackerHandler::bootBootloader() { @@ -1230,6 +1243,10 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, result = handleTm(packet, histogramSet, "REQ_HISTO"); break; } + case (startracker::REQ_BLOB_STATS): { + result = handleTm(packet, blobStatsSet, "REQ_BLOB_STATS"); + break; + } case (startracker::SUBSCRIPTION): case (startracker::LOGLEVEL): case (startracker::LOGSUBSCRIPTION): @@ -1631,6 +1648,13 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_C, new PoolEntry(9)); localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_D, new PoolEntry(9)); + localDataPoolMap.emplace(startracker::TICKS_BLOB_STATS, new PoolEntry()); + localDataPoolMap.emplace(startracker::TIME_BLOB_STATS, new PoolEntry()); + localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_NOISE, new PoolEntry(16)); + localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_THOLD, new PoolEntry(16)); + localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_LVALID, new PoolEntry(16)); + localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_OFLOW, new PoolEntry(16)); + poolManager.subscribeForDiagPeriodicPacket( subdp::DiagnosticsHkPeriodicParams(temperatureSet.getSid(), false, 10.0)); poolManager.subscribeForRegularPeriodicPacket( @@ -1659,6 +1683,8 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l subdp::RegularHkPeriodicParams(centroidsSet.getSid(), false, 10.0)); poolManager.subscribeForRegularPeriodicPacket( subdp::RegularHkPeriodicParams(contrastSet.getSid(), false, 10.0)); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(blobStatsSet.getSid(), false, 10.0)); return returnvalue::OK; } @@ -1905,6 +1931,10 @@ ReturnValue_t StarTrackerHandler::scanForTmReply(uint8_t replyId, DeviceCommandI *foundId = startracker::REQ_BLOB; break; } + case (startracker::ID::BLOB_STATS): { + *foundId = startracker::REQ_BLOB_STATS; + break; + } case (startracker::ID::BLOBS): { *foundId = startracker::REQ_BLOBS; break; @@ -1989,7 +2019,7 @@ ReturnValue_t StarTrackerHandler::executeFlashReadCommand(const uint8_t* command void StarTrackerHandler::prepareBootCommand(startracker::FirmwareTarget target) { uint32_t length = 0; struct BootActionRequest bootRequest = {static_cast(target)}; - arc_pack_boot_action_req(&bootRequest, commandBuffer, &length); + prv_arc_pack_boot_action_req(&bootRequest, commandBuffer, &length); rawPacket = commandBuffer; rawPacketLen = length; } @@ -2022,7 +2052,7 @@ ReturnValue_t StarTrackerHandler::prepareChecksumCommand(const uint8_t* commandD return result; } uint32_t rawCmdLength = 0; - arc_pack_checksum_action_req(&req, commandBuffer, &rawCmdLength); + prv_arc_pack_checksum_action_req(&req, commandBuffer, &rawCmdLength); rawPacket = commandBuffer; rawPacketLen = rawCmdLength; checksumCmd.rememberRegion = req.region; @@ -2041,7 +2071,7 @@ void StarTrackerHandler::prepareTimeRequest() { void StarTrackerHandler::preparePingRequest() { uint32_t length = 0; struct PingActionRequest pingRequest = {PING_ID}; - arc_pack_ping_action_req(&pingRequest, commandBuffer, &length); + prv_arc_pack_ping_action_req(&pingRequest, commandBuffer, &length); rawPacket = commandBuffer; rawPacketLen = length; } @@ -2070,7 +2100,7 @@ void StarTrackerHandler::preparePowerRequest() { void StarTrackerHandler::prepareSwitchToBootloaderCmd() { uint32_t length = 0; struct RebootActionRequest rebootReq {}; - arc_pack_reboot_action_req(&rebootReq, commandBuffer, &length); + prv_arc_pack_reboot_action_req(&rebootReq, commandBuffer, &length); rawPacket = commandBuffer; rawPacketLen = length; } @@ -2079,7 +2109,7 @@ void StarTrackerHandler::prepareTakeImageCommand(const uint8_t* commandData) { uint32_t length = 0; struct CameraActionRequest camReq; camReq.actionid = *commandData; - arc_pack_camera_action_req(&camReq, commandBuffer, &length); + prv_arc_pack_camera_action_req(&camReq, commandBuffer, &length); rawPacket = commandBuffer; rawPacketLen = length; } @@ -2145,6 +2175,14 @@ ReturnValue_t StarTrackerHandler::prepareRequestCentroidTm() { return returnvalue::OK; } +ReturnValue_t StarTrackerHandler::prepareRequestBlobStatsTm() { + uint32_t length = 0; + arc_tm_pack_blobstats_req(commandBuffer, &length); + rawPacket = commandBuffer; + rawPacketLen = length; + return returnvalue::OK; +} + ReturnValue_t StarTrackerHandler::prepareRequestCentroidsTm() { uint32_t length = 0; arc_tm_pack_centroids_req(commandBuffer, &length); @@ -2894,6 +2932,7 @@ ReturnValue_t StarTrackerHandler::checkCommand(ActionId_t actionId) { case startracker::REQ_MATCHED_CENTROIDS: case startracker::REQ_BLOB: case startracker::REQ_BLOBS: + case startracker::REQ_BLOB_STATS: case startracker::REQ_CENTROID: case startracker::REQ_CENTROIDS: case startracker::REQ_CONTRAST: { diff --git a/mission/acs/str/StarTrackerHandler.h b/mission/acs/str/StarTrackerHandler.h index f6611ed6..acad969f 100644 --- a/mission/acs/str/StarTrackerHandler.h +++ b/mission/acs/str/StarTrackerHandler.h @@ -216,6 +216,7 @@ class StarTrackerHandler : public DeviceHandlerBase { startracker::CentroidSet centroidSet; startracker::CentroidsSet centroidsSet; startracker::ContrastSet contrastSet; + startracker::BlobStatsSet blobStatsSet; // Pointer to object responsible for uploading and downloading images to/from the star tracker StrComHandler* strHelper = nullptr; @@ -470,6 +471,7 @@ class StarTrackerHandler : public DeviceHandlerBase { ReturnValue_t prepareRequestCentroidTm(); ReturnValue_t prepareRequestCentroidsTm(); ReturnValue_t prepareRequestContrastTm(); + ReturnValue_t prepareRequestBlobStatsTm(); ReturnValue_t prepareRequestTrackingParams(); ReturnValue_t prepareRequestValidationParams(); ReturnValue_t prepareRequestAlgoParams(); diff --git a/mission/acs/str/strHelpers.h b/mission/acs/str/strHelpers.h index e73a58af..ebbc80c9 100644 --- a/mission/acs/str/strHelpers.h +++ b/mission/acs/str/strHelpers.h @@ -328,6 +328,13 @@ enum PoolIds : lp_id_t { CONTRAST_B, CONTRAST_C, CONTRAST_D, + + TICKS_BLOB_STATS, + TIME_BLOB_STATS, + BLOB_STATS_NOISE, + BLOB_STATS_THOLD, + BLOB_STATS_LVALID, + BLOB_STATS_OFLOW, }; static const DeviceCommandId_t PING_REQUEST = 0; @@ -394,7 +401,9 @@ static constexpr DeviceCommandId_t ADD_SECONDARY_TM_TO_NORMAL_MODE = 95; static constexpr DeviceCommandId_t RESET_SECONDARY_TM_SET = 96; static constexpr DeviceCommandId_t READ_SECONDARY_TM_SET = 97; static constexpr DeviceCommandId_t RELOAD_JSON_CFG_FILE = 100; -static const DeviceCommandId_t FIRMWARE_UPDATE_BACKUP = 101; +static constexpr DeviceCommandId_t FIRMWARE_UPDATE_BACKUP = 101; +static constexpr DeviceCommandId_t REQ_BLOB_STATS = 102; + static const DeviceCommandId_t NONE = 0xFFFFFFFF; static const uint32_t VERSION_SET_ID = REQ_VERSION; @@ -426,6 +435,7 @@ static const uint32_t BLOBS_SET_ID = REQ_BLOBS; static const uint32_t CENTROID_SET_ID = REQ_CENTROID; static const uint32_t CENTROIDS_SET_ID = REQ_CENTROIDS; static const uint32_t CONTRAST_SET_ID = REQ_CONTRAST; +static const uint32_t BLOB_STATS_SET_ID = REQ_BLOB_STATS; /** Max size of unencoded frame */ static const size_t MAX_FRAME_SIZE = 1200; @@ -492,6 +502,7 @@ static constexpr uint8_t CENTROID = 26; static constexpr uint8_t CENTROIDS = 37; static constexpr uint8_t AUTO_BLOB = 39; static constexpr uint8_t MATCHED_CENTROIDS = 40; +static constexpr uint8_t BLOB_STATS = 49; } // namespace ID namespace Program { @@ -1558,6 +1569,23 @@ class CentroidsSet : public StaticLocalDataSet<10> { lp_vec_t(sid.objectId, PoolIds::CENTROIDS_MAGNITUDES, this); }; +class BlobStatsSet : public StaticLocalDataSet<6> { + public: + BlobStatsSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, BLOB_STATS_SET_ID) {} + + // Data received from the Centroids Telemetry Set (ID 49) + lp_var_t ticks = lp_var_t(sid.objectId, PoolIds::TICKS_BLOB_STATS, this); + lp_var_t time = lp_var_t(sid.objectId, PoolIds::TIME_BLOB_STATS, this); + lp_vec_t noise = + lp_vec_t(sid.objectId, PoolIds::BLOB_STATS_NOISE, this); + lp_vec_t thold = + lp_vec_t(sid.objectId, PoolIds::BLOB_STATS_THOLD, this); + lp_vec_t lvalid = + lp_vec_t(sid.objectId, PoolIds::BLOB_STATS_LVALID, this); + lp_vec_t oflow = + lp_vec_t(sid.objectId, PoolIds::BLOB_STATS_OFLOW, this); +}; + class ContrastSet : public StaticLocalDataSet<8> { public: ContrastSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, CONTRAST_SET_ID) {} diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 4682517b..f9a04c46 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -400,7 +400,7 @@ void AcsController::performPointingCtrl() { bool allRwAvailable = true; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); + ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv, &rwAvail); if (result == acsctrl::MULTIPLE_RW_UNAVAILABLE) { if (multipleRwUnavailableCounter >= acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) { @@ -437,10 +437,10 @@ void AcsController::performPointingCtrl() { VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( - &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, - sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, - sensorValues.rw4Set.currSpeed.value, mgtDpDes); + allRwAvailable, &rwAvail, &acsParameters.idleModeControllerParameters, + mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); break; case acs::PTG_TARGET: @@ -460,10 +460,10 @@ void AcsController::performPointingCtrl() { VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( - &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, - sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, - sensorValues.rw4Set.currSpeed.value, mgtDpDes); + allRwAvailable, &rwAvail, &acsParameters.targetModeControllerParameters, + mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); break; case acs::PTG_TARGET_GS: @@ -480,10 +480,10 @@ void AcsController::performPointingCtrl() { VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( - &acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, - sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, - sensorValues.rw4Set.currSpeed.value, mgtDpDes); + allRwAvailable, &rwAvail, &acsParameters.gsTargetModeControllerParameters, + mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); break; case acs::PTG_NADIR: @@ -502,10 +502,10 @@ void AcsController::performPointingCtrl() { VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( - &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, - sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, - sensorValues.rw4Set.currSpeed.value, mgtDpDes); + allRwAvailable, &rwAvail, &acsParameters.nadirModeControllerParameters, + mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); break; case acs::PTG_INERTIAL: @@ -524,10 +524,10 @@ void AcsController::performPointingCtrl() { VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( - &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, - sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, - sensorValues.rw4Set.currSpeed.value, mgtDpDes); + allRwAvailable, &rwAvail, &acsParameters.inertialModeControllerParameters, + mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); break; default: sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl; diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index f4146039..86100547 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -89,6 +89,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes int32_t cmdSpeedRws[4] = {0, 0, 0, 0}; int16_t cmdDipoleMtqs[3] = {0, 0, 0}; + acsctrl::RwAvail rwAvail; + #if OBSW_THREAD_TRACING == 1 uint32_t opCounter = 0; #endif diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 4f7727f2..f9182cbd 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -327,28 +327,32 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do } ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, - double *rwPseudoInv) { - bool rw1valid = (sensorValues->rw1Set.state.value and sensorValues->rw1Set.state.isValid()); - bool rw2valid = (sensorValues->rw2Set.state.value and sensorValues->rw2Set.state.isValid()); - bool rw3valid = (sensorValues->rw3Set.state.value and sensorValues->rw3Set.state.isValid()); - bool rw4valid = (sensorValues->rw4Set.state.value and sensorValues->rw4Set.state.isValid()); + double *rwPseudoInv, acsctrl::RwAvail *rwAvail) { + rwAvail->rw1avail = (sensorValues->rw1Set.state.value and sensorValues->rw1Set.state.isValid()); + rwAvail->rw2avail = (sensorValues->rw2Set.state.value and sensorValues->rw2Set.state.isValid()); + rwAvail->rw3avail = (sensorValues->rw3Set.state.value and sensorValues->rw3Set.state.isValid()); + rwAvail->rw4avail = (sensorValues->rw4Set.state.value and sensorValues->rw4Set.state.isValid()); - if (rw1valid and rw2valid and rw3valid and rw4valid) { + if (rwAvail->rw1avail and rwAvail->rw2avail and rwAvail->rw3avail and rwAvail->rw4avail) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double)); return returnvalue::OK; - } else if (not rw1valid and rw2valid and rw3valid and rw4valid) { + } else if (not rwAvail->rw1avail and rwAvail->rw2avail and rwAvail->rw3avail and + rwAvail->rw4avail) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW1, 12 * sizeof(double)); return acsctrl::SINGLE_RW_UNAVAILABLE; - } else if (rw1valid and not rw2valid and rw3valid and rw4valid) { + } else if (rwAvail->rw1avail and not rwAvail->rw2avail and rwAvail->rw3avail and + rwAvail->rw4avail) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW2, 12 * sizeof(double)); return acsctrl::SINGLE_RW_UNAVAILABLE; - } else if (rw1valid and rw2valid and not rw3valid and rw4valid) { + } else if (rwAvail->rw1avail and rwAvail->rw2avail and not rwAvail->rw3avail and + rwAvail->rw4avail) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW3, 12 * sizeof(double)); return acsctrl::SINGLE_RW_UNAVAILABLE; - } else if (rw1valid and rw2valid and rw3valid and not rw4valid) { + } else if (rwAvail->rw1avail and rwAvail->rw2avail and rwAvail->rw3avail and + not rwAvail->rw4avail) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW4, 12 * sizeof(double)); return acsctrl::SINGLE_RW_UNAVAILABLE; diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index a914ecfe..aa771d8d 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -44,7 +44,8 @@ class Guidance { double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3], double &errorAngle); - ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv); + ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv, + acsctrl::RwAvail *rwAvail); private: const AcsParameters *acsParameters; diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 95b16a3c..a334b3a7 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -1,11 +1,5 @@ #include "PtgCtrl.h" -#include -#include -#include -#include -#include - PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } PtgCtrl::~PtgCtrl() {} @@ -68,9 +62,9 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters // Inverse of gainMatrix double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - gainMatrixInverse[0][0] = 1 / gainMatrix[0][0]; - gainMatrixInverse[1][1] = 1 / gainMatrix[1][1]; - gainMatrixInverse[2][2] = 1 / gainMatrix[2][2]; + gainMatrixInverse[0][0] = 1. / gainMatrix[0][0]; + gainMatrixInverse[1][1] = 1. / gainMatrix[1][1]; + gainMatrixInverse[2][2] = 1. / gainMatrix[2][2]; double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MatrixOperations::multiply( @@ -146,9 +140,10 @@ void PtgCtrl::ptgNullspace(const bool allRwAvabilable, 4); } -void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, +void PtgCtrl::ptgDesaturation(const bool allRwAvailable, const acsctrl::RwAvail *rwAvail, + AcsParameters::PointingLawParameters *pointingLawParameters, const double *magFieldB, const bool magFieldBValid, - const double *satRate, const int32_t speedRw0, const int32_t speedRw1, + const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, const int32_t speedRw3, double *mgtDpDes) { if (not magFieldBValid or not pointingLawParameters->desatOn) { return; @@ -162,17 +157,24 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP double magFieldBT[3] = {0, 0, 0}; VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); - // calculate angular momentum of the satellite - double angMomentumSat[3] = {0, 0, 0}; - MatrixOperations::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), satRate, - angMomentumSat, 3, 3, 1); - // calculate angular momentum of the reaction wheels with respect to the nullspace RW speed // relocate RW speed zero to nullspace RW speed double refSpeedRws[4] = {0, 0, 0, 0}; VectorOperations::mulScalar(acsParameters->rwMatrices.nullspaceVector, pointingLawParameters->nullspaceSpeed, refSpeedRws, 4); + if (not allRwAvailable) { + if (not rwAvail->rw1avail) { + refSpeedRws[0] = 0.0; + } else if (not rwAvail->rw2avail) { + refSpeedRws[1] = 0.0; + } else if (not rwAvail->rw3avail) { + refSpeedRws[2] = 0.0; + } else if (not rwAvail->rw4avail) { + refSpeedRws[3] = 0.0; + } + } VectorOperations::subtract(speedRws, refSpeedRws, speedRws, 4); + // convert speed from 10 RPM to 1 RPM VectorOperations::mulScalar(speedRws, 1e-1, speedRws, 4); // convert to rad/s @@ -188,16 +190,12 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP // calculate total angular momentum double angMomentumTotal[3] = {0, 0, 0}; - VectorOperations::add(angMomentumSat, angMomentumRw, angMomentumTotal, 3); - - // calculating momentum error - double deltaAngMomentum[3] = {0, 0, 0}; - VectorOperations::subtract(angMomentumTotal, pointingLawParameters->desatMomentumRef, - deltaAngMomentum, 3); + VectorOperations::subtract(angMomentumRw, pointingLawParameters->desatMomentumRef, + angMomentumTotal, 3); // resulting magnetic dipole command double crossAngMomentumMagField[3] = {0, 0, 0}; - VectorOperations::cross(deltaAngMomentum, magFieldBT, crossAngMomentumMagField); + VectorOperations::cross(angMomentumTotal, magFieldBT, crossAngMomentumMagField); double factor = pointingLawParameters->deSatGainFactor / VectorOperations::norm(magFieldBT, 3); VectorOperations::mulScalar(crossAngMomentumMagField, factor, mgtDpDes, 3); diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index 66402b0d..daeff4a2 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -1,11 +1,16 @@ #ifndef PTGCTRL_H_ #define PTGCTRL_H_ -#include +#include +#include +#include +#include #include #include #include -#include +#include + +#include class PtgCtrl { /* @@ -38,10 +43,11 @@ class PtgCtrl { const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, const int32_t speedRw3, double *rwTrqNs); - void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, - const double *magFieldB, const bool magFieldBValid, const double *satRate, - const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, - const int32_t speedRw3, double *mgtDpDes); + void ptgDesaturation(const bool allRwAvabilable, const acsctrl::RwAvail *rwAvail, + AcsParameters::PointingLawParameters *pointingLawParameters, + const double *magFieldB, const bool magFieldBValid, const int32_t speedRw0, + const int32_t speedRw1, const int32_t speedRw2, const int32_t speedRw3, + double *mgtDpDes); /* @brief: Commands the stiction torque in case wheel speed is to low * torqueCommand modified torque after anti-stiction diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index c830ffac..61489db2 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -21,6 +21,13 @@ static constexpr ReturnValue_t SINGLE_RW_UNAVAILABLE = MAKE_RETURN_CODE(0xA3); //! [EXPORT] : [COMMENT] Multiple RWs have failed. static constexpr ReturnValue_t MULTIPLE_RW_UNAVAILABLE = MAKE_RETURN_CODE(0xA4); +struct RwAvail { + bool rw1avail = false; + bool rw2avail = false; + bool rw3avail = false; + bool rw4avail = false; +}; + enum SetIds : uint32_t { MGM_SENSOR_DATA, MGM_PROCESSED_DATA, diff --git a/thirdparty/sagittactl b/thirdparty/sagittactl index 20f32a2f..a5586936 160000 --- a/thirdparty/sagittactl +++ b/thirdparty/sagittactl @@ -1 +1 @@ -Subproject commit 20f32a2f291d00869c693066de02c0257dec7be4 +Subproject commit a558693675dae09aff7ef74242c25f826584c46a diff --git a/tmtc b/tmtc index 771199e5..6ade001d 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 771199e542350f498a5604fa78004155af7b0efe +Subproject commit 6ade001d3d3b3b9c0d2838aa94c3112aaf486a61