eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

MEKF Fixes #843

Merged
meggert merged 13 commits from mekf-fix into main 2024-02-29 13:08:25 +01:00
Conversation 0 Commits 13 Files Changed 7 +171 -80
15 changed files with 171 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit c6a308b308 - Show all commits

4
CHANGELOG.md
View File

@ -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`.

16
linux/acs/StrComHandler.cpp
View File

@ -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<char*>(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<char*>(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) {
}

4
mission/acs/str/ArcsecDatalinkLayer.cpp
View File

@ -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;
}

51
mission/acs/str/StarTrackerHandler.cpp
View File

@ -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<uint64_t>(tv.tv_sec) * 1000 * 1000) + (static_cast<uint64_t>(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<startracker::FirmwareTarget>(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<uint32_t>(9));
localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_D, new PoolEntry<uint32_t>(9));
localDataPoolMap.emplace(startracker::TICKS_BLOB_STATS, new PoolEntry<uint32_t>());
localDataPoolMap.emplace(startracker::TIME_BLOB_STATS, new PoolEntry<uint64_t>());
localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_NOISE, new PoolEntry<uint8_t>(16));
localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_THOLD, new PoolEntry<uint8_t>(16));
localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_LVALID, new PoolEntry<uint8_t>(16));
localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_OFLOW, new PoolEntry<uint8_t>(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<uint8_t>(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: {

2
mission/acs/str/StarTrackerHandler.h
View File

@ -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();

30
mission/acs/str/strHelpers.h
View File

@ -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<uint8_t, 16>(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<uint32_t> ticks = lp_var_t<uint32_t>(sid.objectId, PoolIds::TICKS_BLOB_STATS, this);
lp_var_t<uint64_t> time = lp_var_t<uint64_t>(sid.objectId, PoolIds::TIME_BLOB_STATS, this);
lp_vec_t<uint8_t, 16> noise =
lp_vec_t<uint8_t, 16>(sid.objectId, PoolIds::BLOB_STATS_NOISE, this);
lp_vec_t<uint8_t, 16> thold =
lp_vec_t<uint8_t, 16>(sid.objectId, PoolIds::BLOB_STATS_THOLD, this);
lp_vec_t<uint8_t, 16> lvalid =
lp_vec_t<uint8_t, 16>(sid.objectId, PoolIds::BLOB_STATS_LVALID, this);
lp_vec_t<uint8_t, 16> oflow =
lp_vec_t<uint8_t, 16>(sid.objectId, PoolIds::BLOB_STATS_OFLOW, this);
};
class ContrastSet : public StaticLocalDataSet<8> {
public:
ContrastSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, CONTRAST_SET_ID) {}

42
mission/controller/AcsController.cpp
View File

@ -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<double>::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<double>::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<double>::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<double>::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<double>::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;

2
mission/controller/AcsController.h
View File

@ -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

24
mission/controller/acs/Guidance.cpp
View File

@ -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;

3
mission/controller/acs/Guidance.h
View File

@ -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;

44
mission/controller/acs/control/PtgCtrl.cpp
View File

@ -1,11 +1,5 @@
#include "PtgCtrl.h"
#include <fsfw/globalfunctions/constants.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/globalfunctions/sign.h>
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<double>::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<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
// calculate angular momentum of the satellite
double angMomentumSat[3] = {0, 0, 0};
MatrixOperations<double>::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<double>::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<double>::subtract(speedRws, refSpeedRws, speedRws, 4);
// convert speed from 10 RPM to 1 RPM
VectorOperations<double>::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<double>::add(angMomentumSat, angMomentumRw, angMomentumTotal, 3);
// calculating momentum error
double deltaAngMomentum[3] = {0, 0, 0};
VectorOperations<double>::subtract(angMomentumTotal, pointingLawParameters->desatMomentumRef,
deltaAngMomentum, 3);
VectorOperations<double>::subtract(angMomentumRw, pointingLawParameters->desatMomentumRef,
angMomentumTotal, 3);
// resulting magnetic dipole command
double crossAngMomentumMagField[3] = {0, 0, 0};
VectorOperations<double>::cross(deltaAngMomentum, magFieldBT, crossAngMomentumMagField);
VectorOperations<double>::cross(angMomentumTotal, magFieldBT, crossAngMomentumMagField);
double factor =
pointingLawParameters->deSatGainFactor / VectorOperations<double>::norm(magFieldBT, 3);
VectorOperations<double>::mulScalar(crossAngMomentumMagField, factor, mgtDpDes, 3);

18
mission/controller/acs/control/PtgCtrl.h
View File

@ -1,11 +1,16 @@
#ifndef PTGCTRL_H_
#define PTGCTRL_H_
#include <math.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/globalfunctions/sign.h>
#include <mission/acs/defs.h>
#include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/acs/SensorValues.h>
#include <stdio.h>
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
#include <cmath>
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

7
mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
View File

@ -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,

2
thirdparty/sagittactl vendored

@ -1 +1 @@
Subproject commit 20f32a2f291d00869c693066de02c0257dec7be4
Subproject commit a558693675dae09aff7ef74242c25f826584c46a

2
tmtc

@ -1 +1 @@
Subproject commit 771199e542350f498a5604fa78004155af7b0efe
Subproject commit 6ade001d3d3b3b9c0d2838aa94c3112aaf486a61