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 - The `PTG_CTRL_NO_ATTITUDE_INFORMATION` will now actually trigger a fallback into safe mode
and is triggered by the `AcsController` now. and is triggered by the `AcsController` now.
- Fixed a corner case, in which an invalid speed command could be sent to the `RwHandler`. - 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. - Fixed bugs within the `MEKF` and simplified the code.
## Changed ## 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 also limits the rotation for the reference target quaternion to prevent spikes in required
rotation rates. rotation rates.
- Updated QUEST and Sun Vector Params to new values. - 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 ## Added
- Updated STR handler to unlock and allow using the secondary firmware slot. - 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 - Added new action command to update the standard deviations within the `MEKF` from the
`AcsParameters`. `AcsParameters`.

16
linux/acs/StrComHandler.cpp
View File

@ -270,7 +270,7 @@ ReturnValue_t StrComHandler::performImageDownload() {
file.close(); file.close();
return returnvalue::OK; 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); result = sendAndRead(size, downloadReq.position);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) { if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) {
@ -349,7 +349,7 @@ ReturnValue_t StrComHandler::performImageUpload() {
} }
file.seekg(uploadReq.position * SIZE_IMAGE_PART, file.beg); file.seekg(uploadReq.position * SIZE_IMAGE_PART, file.beg);
file.read(reinterpret_cast<char*>(uploadReq.data), SIZE_IMAGE_PART); 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); result = sendAndRead(size, uploadReq.position);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
return returnvalue::FAILED; return returnvalue::FAILED;
@ -375,7 +375,7 @@ ReturnValue_t StrComHandler::performImageUpload() {
file.seekg(fullChunks * SIZE_IMAGE_PART, file.beg); file.seekg(fullChunks * SIZE_IMAGE_PART, file.beg);
file.read(reinterpret_cast<char*>(uploadReq.data), remainder); file.read(reinterpret_cast<char*>(uploadReq.data), remainder);
file.close(); 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); result = sendAndRead(size, uploadReq.position);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
return returnvalue::FAILED; return returnvalue::FAILED;
@ -445,7 +445,7 @@ ReturnValue_t StrComHandler::performFlashWrite() {
bytesWrittenInRegion = 0; bytesWrittenInRegion = 0;
} }
req.address = bytesWrittenInRegion; 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); result = sendAndRead(size, req.address);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
return result; return result;
@ -488,7 +488,7 @@ ReturnValue_t StrComHandler::performFlashWrite() {
req.length = remainingBytes; req.length = remainingBytes;
totalBytesWritten += CHUNK_SIZE; totalBytesWritten += CHUNK_SIZE;
bytesWrittenInRegion += remainingBytes; 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); result = sendAndRead(size, req.address);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
return result; return result;
@ -536,7 +536,7 @@ ReturnValue_t StrComHandler::performFlashRead() {
} else { } else {
req.length = CHUNK_SIZE; 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); result = sendAndRead(size, req.address);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) { 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++) { for (uint32_t idx = from; idx < to; idx++) {
unlockReq.region = idx; unlockReq.region = idx;
unlockReq.code = startracker::region_secrets::SECRETS[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); result = sendAndRead(size, unlockReq.region);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
return result; return result;
@ -764,7 +764,7 @@ ReturnValue_t StrComHandler::unlockAndEraseRegions(uint32_t from, uint32_t to) {
return result; return result;
} }
eraseReq.region = idx; 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); result = sendAndRead(size, eraseReq.region);
if (result != returnvalue::OK) { 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; size_t encodedDataSize = 0;
slip_error_t slipError = slip_error_t slipError =
slip_decode_frame(decodedRxFrame, &rxFrameSize, rxAnalysisBuffer + startIdx, 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); decodeRingBuf.deleteData(idx + 1);
switch (slipError) { switch (slipError) {
case (SLIP_OK): { 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, void ArcsecDatalinkLayer::encodeFrame(const uint8_t* data, size_t length, const uint8_t** txFrame,
size_t& size) { 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) { if (txFrame != nullptr) {
*txFrame = txEncoded; *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), centroidSet(this),
centroidsSet(this), centroidsSet(this),
contrastSet(this), contrastSet(this),
blobStatsSet(this),
strHelper(strHelper), strHelper(strHelper),
powerSwitch(powerSwitch), powerSwitch(powerSwitch),
sdCardIF(sdCardIF), sdCardIF(sdCardIF),
@ -572,7 +573,7 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi
Clock::getClock(&tv); Clock::getClock(&tv);
setTimeRequest.unixTime = setTimeRequest.unixTime =
(static_cast<uint64_t>(tv.tv_sec) * 1000 * 1000) + (static_cast<uint64_t>(tv.tv_usec)); (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; size_t serLen = 0;
// Time in milliseconds. Manual serialization because arcsec API ignores endianness. // Time in milliseconds. Manual serialization because arcsec API ignores endianness.
SerializeAdapter::serialize(&setTimeRequest.unixTime, commandBuffer + 2, &serLen, SerializeAdapter::serialize(&setTimeRequest.unixTime, commandBuffer + 2, &serLen,
@ -597,6 +598,10 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi
prepareRequestContrastTm(); prepareRequestContrastTm();
return returnvalue::OK; return returnvalue::OK;
} }
case (startracker::REQ_BLOB_STATS): {
prepareRequestBlobStatsTm();
return returnvalue::OK;
}
case (startracker::BOOT): { case (startracker::BOOT): {
prepareBootCommand(static_cast<startracker::FirmwareTarget>(firmwareTargetRaw)); prepareBootCommand(static_cast<startracker::FirmwareTarget>(firmwareTargetRaw));
return returnvalue::OK; return returnvalue::OK;
@ -885,6 +890,8 @@ void StarTrackerHandler::fillCommandAndReplyMap() {
startracker::MAX_FRAME_SIZE * 2 + 2); startracker::MAX_FRAME_SIZE * 2 + 2);
this->insertInCommandAndReplyMap(startracker::REQ_CONTRAST, 3, &contrastSet, this->insertInCommandAndReplyMap(startracker::REQ_CONTRAST, 3, &contrastSet,
startracker::MAX_FRAME_SIZE * 2 + 2); 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) { ReturnValue_t StarTrackerHandler::isModeCombinationValid(Mode_t mode, Submode_t submode) {
@ -1082,6 +1089,12 @@ void StarTrackerHandler::resetSecondaryTmSet() {
histogramSet.setValidity(false, true); histogramSet.setValidity(false, true);
} }
} }
{
PoolReadGuard pg(&blobStatsSet);
if (pg.getReadResult() == returnvalue::OK) {
histogramSet.setValidity(false, true);
}
}
} }
void StarTrackerHandler::bootBootloader() { void StarTrackerHandler::bootBootloader() {
@ -1230,6 +1243,10 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id,
result = handleTm(packet, histogramSet, "REQ_HISTO"); result = handleTm(packet, histogramSet, "REQ_HISTO");
break; break;
} }
case (startracker::REQ_BLOB_STATS): {
result = handleTm(packet, blobStatsSet, "REQ_BLOB_STATS");
break;
}
case (startracker::SUBSCRIPTION): case (startracker::SUBSCRIPTION):
case (startracker::LOGLEVEL): case (startracker::LOGLEVEL):
case (startracker::LOGSUBSCRIPTION): 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_C, new PoolEntry<uint32_t>(9));
localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_D, 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( poolManager.subscribeForDiagPeriodicPacket(
subdp::DiagnosticsHkPeriodicParams(temperatureSet.getSid(), false, 10.0)); subdp::DiagnosticsHkPeriodicParams(temperatureSet.getSid(), false, 10.0));
poolManager.subscribeForRegularPeriodicPacket( poolManager.subscribeForRegularPeriodicPacket(
@ -1659,6 +1683,8 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l
subdp::RegularHkPeriodicParams(centroidsSet.getSid(), false, 10.0)); subdp::RegularHkPeriodicParams(centroidsSet.getSid(), false, 10.0));
poolManager.subscribeForRegularPeriodicPacket( poolManager.subscribeForRegularPeriodicPacket(
subdp::RegularHkPeriodicParams(contrastSet.getSid(), false, 10.0)); subdp::RegularHkPeriodicParams(contrastSet.getSid(), false, 10.0));
poolManager.subscribeForRegularPeriodicPacket(
subdp::RegularHkPeriodicParams(blobStatsSet.getSid(), false, 10.0));
return returnvalue::OK; return returnvalue::OK;
} }
@ -1905,6 +1931,10 @@ ReturnValue_t StarTrackerHandler::scanForTmReply(uint8_t replyId, DeviceCommandI
*foundId = startracker::REQ_BLOB; *foundId = startracker::REQ_BLOB;
break; break;
} }
case (startracker::ID::BLOB_STATS): {
*foundId = startracker::REQ_BLOB_STATS;
break;
}
case (startracker::ID::BLOBS): { case (startracker::ID::BLOBS): {
*foundId = startracker::REQ_BLOBS; *foundId = startracker::REQ_BLOBS;
break; break;
@ -1989,7 +2019,7 @@ ReturnValue_t StarTrackerHandler::executeFlashReadCommand(const uint8_t* command
void StarTrackerHandler::prepareBootCommand(startracker::FirmwareTarget target) { void StarTrackerHandler::prepareBootCommand(startracker::FirmwareTarget target) {
uint32_t length = 0; uint32_t length = 0;
struct BootActionRequest bootRequest = {static_cast<uint8_t>(target)}; 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; rawPacket = commandBuffer;
rawPacketLen = length; rawPacketLen = length;
} }
@ -2022,7 +2052,7 @@ ReturnValue_t StarTrackerHandler::prepareChecksumCommand(const uint8_t* commandD
return result; return result;
} }
uint32_t rawCmdLength = 0; uint32_t rawCmdLength = 0;
arc_pack_checksum_action_req(&req, commandBuffer, &rawCmdLength); prv_arc_pack_checksum_action_req(&req, commandBuffer, &rawCmdLength);
rawPacket = commandBuffer; rawPacket = commandBuffer;
rawPacketLen = rawCmdLength; rawPacketLen = rawCmdLength;
checksumCmd.rememberRegion = req.region; checksumCmd.rememberRegion = req.region;
@ -2041,7 +2071,7 @@ void StarTrackerHandler::prepareTimeRequest() {
void StarTrackerHandler::preparePingRequest() { void StarTrackerHandler::preparePingRequest() {
uint32_t length = 0; uint32_t length = 0;
struct PingActionRequest pingRequest = {PING_ID}; struct PingActionRequest pingRequest = {PING_ID};
arc_pack_ping_action_req(&pingRequest, commandBuffer, &length); prv_arc_pack_ping_action_req(&pingRequest, commandBuffer, &length);
rawPacket = commandBuffer; rawPacket = commandBuffer;
rawPacketLen = length; rawPacketLen = length;
} }
@ -2070,7 +2100,7 @@ void StarTrackerHandler::preparePowerRequest() {
void StarTrackerHandler::prepareSwitchToBootloaderCmd() { void StarTrackerHandler::prepareSwitchToBootloaderCmd() {
uint32_t length = 0; uint32_t length = 0;
struct RebootActionRequest rebootReq {}; struct RebootActionRequest rebootReq {};
arc_pack_reboot_action_req(&rebootReq, commandBuffer, &length); prv_arc_pack_reboot_action_req(&rebootReq, commandBuffer, &length);
rawPacket = commandBuffer; rawPacket = commandBuffer;
rawPacketLen = length; rawPacketLen = length;
} }
@ -2079,7 +2109,7 @@ void StarTrackerHandler::prepareTakeImageCommand(const uint8_t* commandData) {
uint32_t length = 0; uint32_t length = 0;
struct CameraActionRequest camReq; struct CameraActionRequest camReq;
camReq.actionid = *commandData; camReq.actionid = *commandData;
arc_pack_camera_action_req(&camReq, commandBuffer, &length); prv_arc_pack_camera_action_req(&camReq, commandBuffer, &length);
rawPacket = commandBuffer; rawPacket = commandBuffer;
rawPacketLen = length; rawPacketLen = length;
} }
@ -2145,6 +2175,14 @@ ReturnValue_t StarTrackerHandler::prepareRequestCentroidTm() {
return returnvalue::OK; 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() { ReturnValue_t StarTrackerHandler::prepareRequestCentroidsTm() {
uint32_t length = 0; uint32_t length = 0;
arc_tm_pack_centroids_req(commandBuffer, &length); 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_MATCHED_CENTROIDS:
case startracker::REQ_BLOB: case startracker::REQ_BLOB:
case startracker::REQ_BLOBS: case startracker::REQ_BLOBS:
case startracker::REQ_BLOB_STATS:
case startracker::REQ_CENTROID: case startracker::REQ_CENTROID:
case startracker::REQ_CENTROIDS: case startracker::REQ_CENTROIDS:
case startracker::REQ_CONTRAST: { case startracker::REQ_CONTRAST: {

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

@ -216,6 +216,7 @@ class StarTrackerHandler : public DeviceHandlerBase {
startracker::CentroidSet centroidSet; startracker::CentroidSet centroidSet;
startracker::CentroidsSet centroidsSet; startracker::CentroidsSet centroidsSet;
startracker::ContrastSet contrastSet; startracker::ContrastSet contrastSet;
startracker::BlobStatsSet blobStatsSet;
// Pointer to object responsible for uploading and downloading images to/from the star tracker // Pointer to object responsible for uploading and downloading images to/from the star tracker
StrComHandler* strHelper = nullptr; StrComHandler* strHelper = nullptr;
@ -470,6 +471,7 @@ class StarTrackerHandler : public DeviceHandlerBase {
ReturnValue_t prepareRequestCentroidTm(); ReturnValue_t prepareRequestCentroidTm();
ReturnValue_t prepareRequestCentroidsTm(); ReturnValue_t prepareRequestCentroidsTm();
ReturnValue_t prepareRequestContrastTm(); ReturnValue_t prepareRequestContrastTm();
ReturnValue_t prepareRequestBlobStatsTm();
ReturnValue_t prepareRequestTrackingParams(); ReturnValue_t prepareRequestTrackingParams();
ReturnValue_t prepareRequestValidationParams(); ReturnValue_t prepareRequestValidationParams();
ReturnValue_t prepareRequestAlgoParams(); ReturnValue_t prepareRequestAlgoParams();

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

@ -328,6 +328,13 @@ enum PoolIds : lp_id_t {
CONTRAST_B, CONTRAST_B,
CONTRAST_C, CONTRAST_C,
CONTRAST_D, 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; 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 RESET_SECONDARY_TM_SET = 96;
static constexpr DeviceCommandId_t READ_SECONDARY_TM_SET = 97; static constexpr DeviceCommandId_t READ_SECONDARY_TM_SET = 97;
static constexpr DeviceCommandId_t RELOAD_JSON_CFG_FILE = 100; 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 DeviceCommandId_t NONE = 0xFFFFFFFF;
static const uint32_t VERSION_SET_ID = REQ_VERSION; 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 CENTROID_SET_ID = REQ_CENTROID;
static const uint32_t CENTROIDS_SET_ID = REQ_CENTROIDS; static const uint32_t CENTROIDS_SET_ID = REQ_CENTROIDS;
static const uint32_t CONTRAST_SET_ID = REQ_CONTRAST; 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 */ /** Max size of unencoded frame */
static const size_t MAX_FRAME_SIZE = 1200; 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 CENTROIDS = 37;
static constexpr uint8_t AUTO_BLOB = 39; static constexpr uint8_t AUTO_BLOB = 39;
static constexpr uint8_t MATCHED_CENTROIDS = 40; static constexpr uint8_t MATCHED_CENTROIDS = 40;
static constexpr uint8_t BLOB_STATS = 49;
} // namespace ID } // namespace ID
namespace Program { namespace Program {
@ -1558,6 +1569,23 @@ class CentroidsSet : public StaticLocalDataSet<10> {
lp_vec_t<uint8_t, 16>(sid.objectId, PoolIds::CENTROIDS_MAGNITUDES, this); 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> { class ContrastSet : public StaticLocalDataSet<8> {
public: public:
ContrastSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, CONTRAST_SET_ID) {} 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; bool allRwAvailable = true;
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; 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 (result == acsctrl::MULTIPLE_RW_UNAVAILABLE) {
if (multipleRwUnavailableCounter >= if (multipleRwUnavailableCounter >=
acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) { acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) {
@ -437,10 +437,10 @@ void AcsController::performPointingCtrl() {
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
&acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value, allRwAvailable, &rwAvail, &acsParameters.idleModeControllerParameters,
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes); sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
break; break;
case acs::PTG_TARGET: case acs::PTG_TARGET:
@ -460,10 +460,10 @@ void AcsController::performPointingCtrl() {
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, allRwAvailable, &rwAvail, &acsParameters.targetModeControllerParameters,
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes); sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
break; break;
case acs::PTG_TARGET_GS: case acs::PTG_TARGET_GS:
@ -480,10 +480,10 @@ void AcsController::performPointingCtrl() {
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
&acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, allRwAvailable, &rwAvail, &acsParameters.gsTargetModeControllerParameters,
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes); sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
break; break;
case acs::PTG_NADIR: case acs::PTG_NADIR:
@ -502,10 +502,10 @@ void AcsController::performPointingCtrl() {
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
&acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value, allRwAvailable, &rwAvail, &acsParameters.nadirModeControllerParameters,
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes); sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
break; break;
case acs::PTG_INERTIAL: case acs::PTG_INERTIAL:
@ -524,10 +524,10 @@ void AcsController::performPointingCtrl() {
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4); VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
ptgCtrl.ptgDesaturation( ptgCtrl.ptgDesaturation(
&acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value, allRwAvailable, &rwAvail, &acsParameters.inertialModeControllerParameters,
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
sensorValues.rw4Set.currSpeed.value, mgtDpDes); sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
break; break;
default: default:
sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl; 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}; int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
int16_t cmdDipoleMtqs[3] = {0, 0, 0}; int16_t cmdDipoleMtqs[3] = {0, 0, 0};
acsctrl::RwAvail rwAvail;
#if OBSW_THREAD_TRACING == 1 #if OBSW_THREAD_TRACING == 1
uint32_t opCounter = 0; uint32_t opCounter = 0;
#endif #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, ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
double *rwPseudoInv) { double *rwPseudoInv, acsctrl::RwAvail *rwAvail) {
bool rw1valid = (sensorValues->rw1Set.state.value and sensorValues->rw1Set.state.isValid()); rwAvail->rw1avail = (sensorValues->rw1Set.state.value and sensorValues->rw1Set.state.isValid());
bool rw2valid = (sensorValues->rw2Set.state.value and sensorValues->rw2Set.state.isValid()); rwAvail->rw2avail = (sensorValues->rw2Set.state.value and sensorValues->rw2Set.state.isValid());
bool rw3valid = (sensorValues->rw3Set.state.value and sensorValues->rw3Set.state.isValid()); rwAvail->rw3avail = (sensorValues->rw3Set.state.value and sensorValues->rw3Set.state.isValid());
bool rw4valid = (sensorValues->rw4Set.state.value and sensorValues->rw4Set.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)); std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double));
return returnvalue::OK; 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, std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW1,
12 * sizeof(double)); 12 * sizeof(double));
return acsctrl::SINGLE_RW_UNAVAILABLE; 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, std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW2,
12 * sizeof(double)); 12 * sizeof(double));
return acsctrl::SINGLE_RW_UNAVAILABLE; 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, std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW3,
12 * sizeof(double)); 12 * sizeof(double));
return acsctrl::SINGLE_RW_UNAVAILABLE; 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, std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW4,
12 * sizeof(double)); 12 * sizeof(double));
return acsctrl::SINGLE_RW_UNAVAILABLE; 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 targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3],
double &errorAngle); double &errorAngle);
ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv); ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv,
acsctrl::RwAvail *rwAvail);
private: private:
const AcsParameters *acsParameters; const AcsParameters *acsParameters;

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

@ -1,11 +1,5 @@
#include "PtgCtrl.h" #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(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
PtgCtrl::~PtgCtrl() {} PtgCtrl::~PtgCtrl() {}
@ -68,9 +62,9 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
// Inverse of gainMatrix // Inverse of gainMatrix
double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
gainMatrixInverse[0][0] = 1 / gainMatrix[0][0]; gainMatrixInverse[0][0] = 1. / gainMatrix[0][0];
gainMatrixInverse[1][1] = 1 / gainMatrix[1][1]; gainMatrixInverse[1][1] = 1. / gainMatrix[1][1];
gainMatrixInverse[2][2] = 1 / gainMatrix[2][2]; gainMatrixInverse[2][2] = 1. / gainMatrix[2][2];
double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiply( MatrixOperations<double>::multiply(
@ -146,9 +140,10 @@ void PtgCtrl::ptgNullspace(const bool allRwAvabilable,
4); 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 *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) { const int32_t speedRw2, const int32_t speedRw3, double *mgtDpDes) {
if (not magFieldBValid or not pointingLawParameters->desatOn) { if (not magFieldBValid or not pointingLawParameters->desatOn) {
return; return;
@ -162,17 +157,24 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP
double magFieldBT[3] = {0, 0, 0}; double magFieldBT[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3); 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 // calculate angular momentum of the reaction wheels with respect to the nullspace RW speed
// relocate RW speed zero to nullspace RW speed // relocate RW speed zero to nullspace RW speed
double refSpeedRws[4] = {0, 0, 0, 0}; double refSpeedRws[4] = {0, 0, 0, 0};
VectorOperations<double>::mulScalar(acsParameters->rwMatrices.nullspaceVector, VectorOperations<double>::mulScalar(acsParameters->rwMatrices.nullspaceVector,
pointingLawParameters->nullspaceSpeed, refSpeedRws, 4); 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); VectorOperations<double>::subtract(speedRws, refSpeedRws, speedRws, 4);
// convert speed from 10 RPM to 1 RPM // convert speed from 10 RPM to 1 RPM
VectorOperations<double>::mulScalar(speedRws, 1e-1, speedRws, 4); VectorOperations<double>::mulScalar(speedRws, 1e-1, speedRws, 4);
// convert to rad/s // convert to rad/s
@ -188,16 +190,12 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP
// calculate total angular momentum // calculate total angular momentum
double angMomentumTotal[3] = {0, 0, 0}; double angMomentumTotal[3] = {0, 0, 0};
VectorOperations<double>::add(angMomentumSat, angMomentumRw, angMomentumTotal, 3); VectorOperations<double>::subtract(angMomentumRw, pointingLawParameters->desatMomentumRef,
angMomentumTotal, 3);
// calculating momentum error
double deltaAngMomentum[3] = {0, 0, 0};
VectorOperations<double>::subtract(angMomentumTotal, pointingLawParameters->desatMomentumRef,
deltaAngMomentum, 3);
// resulting magnetic dipole command // resulting magnetic dipole command
double crossAngMomentumMagField[3] = {0, 0, 0}; double crossAngMomentumMagField[3] = {0, 0, 0};
VectorOperations<double>::cross(deltaAngMomentum, magFieldBT, crossAngMomentumMagField); VectorOperations<double>::cross(angMomentumTotal, magFieldBT, crossAngMomentumMagField);
double factor = double factor =
pointingLawParameters->deSatGainFactor / VectorOperations<double>::norm(magFieldBT, 3); pointingLawParameters->deSatGainFactor / VectorOperations<double>::norm(magFieldBT, 3);
VectorOperations<double>::mulScalar(crossAngMomentumMagField, factor, mgtDpDes, 3); VectorOperations<double>::mulScalar(crossAngMomentumMagField, factor, mgtDpDes, 3);

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

@ -1,11 +1,16 @@
#ifndef PTGCTRL_H_ #ifndef PTGCTRL_H_
#define 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/acs/defs.h>
#include <mission/controller/acs/AcsParameters.h> #include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/acs/SensorValues.h> #include <mission/controller/acs/SensorValues.h>
#include <stdio.h> #include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
#include <cmath>
class PtgCtrl { class PtgCtrl {
/* /*
@ -38,10 +43,11 @@ class PtgCtrl {
const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
const int32_t speedRw3, double *rwTrqNs); const int32_t speedRw3, double *rwTrqNs);
void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, void ptgDesaturation(const bool allRwAvabilable, const acsctrl::RwAvail *rwAvail,
const double *magFieldB, const bool magFieldBValid, const double *satRate, AcsParameters::PointingLawParameters *pointingLawParameters,
const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, const double *magFieldB, const bool magFieldBValid, const int32_t speedRw0,
const int32_t speedRw3, double *mgtDpDes); 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 /* @brief: Commands the stiction torque in case wheel speed is to low
* torqueCommand modified torque after anti-stiction * 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. //! [EXPORT] : [COMMENT] Multiple RWs have failed.
static constexpr ReturnValue_t MULTIPLE_RW_UNAVAILABLE = MAKE_RETURN_CODE(0xA4); 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 { enum SetIds : uint32_t {
MGM_SENSOR_DATA, MGM_SENSOR_DATA,
MGM_PROCESSED_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