mission/devices/StarTrackerHandler.cpp

@@ -2,7 +2,6 @@
 #include "OBSWConfig.h"
 
 #include <fsfw/globalfunctions/CRC.h>
-#include <fsfw/datapool/PoolReadGuard.h>
 
 extern "C" {
     #include <thirdparty/arcsec_star_tracker/client/generated/telemetry.h>
@@ -12,7 +11,8 @@ extern "C" {
 
 StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF,
         CookieIF * comCookie) :
-        DeviceHandlerBase(objectId, comIF, comCookie), temperatureSet(this), versionSet(this) {
+        DeviceHandlerBase(objectId, comIF, comCookie), temperatureSet(this), versionSet(this), powerSet(
+                this), interfaceSet(this), timeSet(this), solutionSet(this) {
     if (comCookie == NULL) {
         sif::error << "StarTrackerHandler: Invalid com cookie" << std::endl;
     }
@@ -61,14 +61,38 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi
         preparePingRequest();
         return RETURN_OK;
     }
+    case (StarTracker::REQ_TIME): {
+        prepareTimeRequest();
+        return RETURN_OK;
+    }
+    case (StarTracker::BOOT): {
+        prepareBootCommand();
+        return RETURN_OK;
+    }
     case (StarTracker::REQ_VERSION): {
         prepareVersionRequest();
         return RETURN_OK;
     }
+    case (StarTracker::REQ_INTERFACE): {
+        prepareInterfaceRequest();
+        return RETURN_OK;
+    }
+    case (StarTracker::REQ_POWER): {
+        preparePowerRequest();
+        return RETURN_OK;
+    }
     case (StarTracker::REBOOT): {
         prepareRebootCommand();
         return RETURN_OK;
     }
+    case (StarTracker::SUBSCRIBE_TO_TM): {
+        prepareSubscriptionCommand(commandData);
+        return RETURN_OK;
+    }
+    case (StarTracker::REQ_SOLUTION): {
+        prepareSolutionRequest();
+        return RETURN_OK;
+    }
     case (StarTracker::REQ_TEMPERATURE): {
         prepareTemperatureRequest();
         return RETURN_OK;
@@ -82,26 +106,44 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi
 void StarTrackerHandler::fillCommandAndReplyMap() {
     /** Reply lengths are unknown because of the slip encoding. Thus always maximum reply size
       * is specified */
-	this->insertInCommandAndReplyMap(StarTracker::PING_REQUEST, 1, nullptr,
+	this->insertInCommandAndReplyMap(StarTracker::PING_REQUEST, 3, nullptr,
 			StarTracker::MAX_FRAME_SIZE * 2 + 2);
-	this->insertInCommandAndReplyMap(StarTracker::REQ_VERSION, 1, nullptr,
+	this->insertInCommandAndReplyMap(StarTracker::BOOT, 3, nullptr,
 			StarTracker::MAX_FRAME_SIZE * 2 + 2);
+	this->insertInCommandAndReplyMap(StarTracker::REQ_VERSION, 3, &versionSet,
+			StarTracker::MAX_FRAME_SIZE * 2 + 2);
+	this->insertInCommandAndReplyMap(StarTracker::REQ_TIME, 3, &timeSet,
+			StarTracker::MAX_FRAME_SIZE * 2 + 2);
+	this->insertInCommandAndReplyMap(StarTracker::REQ_POWER, 3, &powerSet,
+			StarTracker::MAX_FRAME_SIZE * 2 + 2);
+	this->insertInCommandAndReplyMap(StarTracker::REQ_INTERFACE, 3, &interfaceSet,
+			StarTracker::MAX_FRAME_SIZE * 2 + 2);
+	// Reboot has no reply. Star tracker reboots immediately
 	this->insertInCommandMap(StarTracker::REBOOT);
-	this->insertInCommandAndReplyMap(StarTracker::REQ_TEMPERATURE, 1, &temperatureSet,
+    this->insertInCommandAndReplyMap(StarTracker::SUBSCRIBE_TO_TM, 3, nullptr,
+            StarTracker::MAX_FRAME_SIZE * 2 + 2);
+	this->insertInCommandAndReplyMap(StarTracker::REQ_SOLUTION, 3, &solutionSet,
+			StarTracker::MAX_FRAME_SIZE * 2 + 2);
+	this->insertInCommandAndReplyMap(StarTracker::REQ_TEMPERATURE, 3, &temperatureSet,
 			StarTracker::MAX_FRAME_SIZE * 2 + 2);
 }
 
 ReturnValue_t StarTrackerHandler::scanForReply(const uint8_t *start, size_t remainingSize,
         DeviceCommandId_t *foundId, size_t *foundLen) {
 
+    ReturnValue_t result = RETURN_OK;
     uint32_t decodedLength = 0;
-
+    size_t bytePos = 0;
-    for (size_t idx = 0; idx < remainingSize; idx++) {
+    for (bytePos = 0; bytePos < remainingSize; bytePos++) {
-        enum arc_dec_result decResult = arc_transport_decode_body(*(start + idx), &slipInfo,
+        enum arc_dec_result decResult = arc_transport_decode_body(*(start + bytePos), &slipInfo,
                 decodedFrame, &decodedLength);
 
         switch (decResult) {
         case ARC_DEC_INPROGRESS: {
+            if (bytePos == remainingSize - 1) {
+                // second doSendread() required to read whole packet
+                return IGNORE_FULL_PACKET;
+            }
             continue;
         }
         case ARC_DEC_ASYNC: {
@@ -124,31 +166,30 @@ ReturnValue_t StarTrackerHandler::scanForReply(const uint8_t *start, size_t rema
         }
     }
 
-    switch (decodedFrame[1]) {
+    switch (decodedFrame[0]) {
-    case (static_cast<uint8_t>(StarTracker::PING_REQUEST)): {
+    case TMTC_ACTIONREPLY: {
-        *foundLen = decodedLength;
+        *foundLen = bytePos;
-        *foundId = StarTracker::PING_REQUEST;
+        result = scanForActionReply(foundId);
         break;
     }
-    case (static_cast<uint8_t>(StarTracker::REQ_VERSION)): {
+    case TMTC_SETPARAMREPLY: {
-        *foundLen = decodedLength;
+        *foundLen = bytePos;
-        *foundId = StarTracker::PING_REQUEST;
+        result = scanForSetParameterReply(foundId);
         break;
     }
-    case (static_cast<uint8_t>(StarTracker::REQ_TEMPERATURE)): {
+    case TMTC_TELEMETRYREPLYA:
-        *foundLen = decodedLength;
+    case TMTC_TELEMETRYREPLY: {
-        *foundId = StarTracker::REQ_TEMPERATURE;
+        *foundLen = bytePos;
+        result = scanForTmReply(foundId);
         break;
     }
     default: {
-        sif::debug << "StarTrackerHandler::scanForReply: Reply contains invalid reply id"
+        sif::debug << "StarTrackerHandler::scanForReply: Reply has invalid type id" << std::endl;
-                << std::endl;
+        result = RETURN_FAILED;
-        return RETURN_FAILED;
-        break;
     }
     }
 
-    return RETURN_OK;
+    return result;
 }
 
 ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {
@@ -156,14 +197,38 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, con
     ReturnValue_t result = RETURN_OK;
 
     switch (id) {
+    case (StarTracker::SUBSCRIBE_TO_TM): {
+        result = handleSetParamReply();
+        break;
+    }
+    case (StarTracker::REQ_TIME): {
+        result = handleTimeTm();
+        break;
+    }
     case (StarTracker::PING_REQUEST): {
         result = handlePingReply();
         break;
     }
+    case (StarTracker::BOOT): {
+        result = handleActionReply();
+        break;
+    }
     case (StarTracker::REQ_VERSION): {
         result = handleVersionTm();
         break;
     }
+    case (StarTracker::REQ_INTERFACE): {
+        result = handleInterfaceTm();
+        break;
+    }
+    case (StarTracker::REQ_POWER): {
+        result = handlePowerTm();
+        break;
+    }
+    case (StarTracker::REQ_SOLUTION): {
+        handleSolutionTm();
+        break;
+    }
     case (StarTracker::REQ_TEMPERATURE): {
         handleTemperatureTm();
         break;
@@ -171,13 +236,14 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, con
     default: {
         sif::debug << "StarTrackerHandler::interpretDeviceReply: Unknown device reply id:" << id
                 << std::endl;
-        return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
+        result = DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
     }
     }
 
     return result;
 }
 
+
 void StarTrackerHandler::setNormalDatapoolEntriesInvalid() {
 
 }
@@ -189,16 +255,78 @@ uint32_t StarTrackerHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo
 ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
         LocalDataPoolManager& poolManager) {
 
+    localDataPoolMap.emplace(StarTracker::TICKS_TIME_SET, new PoolEntry<uint32_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::TIME_TIME_SET, new PoolEntry<uint64_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::RUN_TIME, new PoolEntry<uint32_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::UNIX_TIME, new PoolEntry<uint64_t>( { 0 }));
+
     localDataPoolMap.emplace(StarTracker::TICKS_VERSION_SET, new PoolEntry<uint32_t>( { 0 }));
     localDataPoolMap.emplace(StarTracker::TIME_VERSION_SET, new PoolEntry<uint64_t>( { 0 }));
     localDataPoolMap.emplace(StarTracker::PROGRAM, new PoolEntry<uint8_t>( { 0 }));
     localDataPoolMap.emplace(StarTracker::MAJOR, new PoolEntry<uint8_t>( { 0 }));
     localDataPoolMap.emplace(StarTracker::MINOR, new PoolEntry<uint8_t>( { 0 }));
+
+    localDataPoolMap.emplace(StarTracker::TICKS_INTERFACE_SET, new PoolEntry<uint32_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::TIME_INTERFACE_SET, new PoolEntry<uint64_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::FRAME_COUNT, new PoolEntry<uint32_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::CHECKSUM_ERROR_COUNT, new PoolEntry<uint32_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::SET_PARAM_COUNT, new PoolEntry<uint32_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::SET_PARAM_REPLY_COUNT, new PoolEntry<uint32_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::PARAM_REQUEST_COUNT, new PoolEntry<uint32_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::PARAM_REPLY_COUNT, new PoolEntry<uint32_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::REQ_TM_COUNT, new PoolEntry<uint32_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::TM_REPLY_COUNT, new PoolEntry<uint32_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::ACTION_REQ_COUNT, new PoolEntry<uint32_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::ACTION_REPLY_COUNT, new PoolEntry<uint32_t>( { 0 }));
+
+    localDataPoolMap.emplace(StarTracker::TICKS_POWER_SET, new PoolEntry<uint32_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::TIME_POWER_SET, new PoolEntry<uint64_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::MCU_CURRENT, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::MCU_VOLTAGE, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::FPGA_CORE_CURRENT, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::FPGA_CORE_VOLTAGE, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::FPGA_18_CURRENT, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::FPGA_18_VOLTAGE, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::FPGA_25_CURRENT, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::FPGA_25_VOLTAGE, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::CMV_21_CURRENT, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::CMV_21_VOLTAGE, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::CMV_PIX_CURRENT, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::CMV_PIX_VOLTAGE, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::CMV_33_CURRENT, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::CMV_33_VOLTAGE, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::CMV_RES_CURRENT, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::CMV_RES_VOLTAGE, new PoolEntry<float>( { 0 }));
+
     localDataPoolMap.emplace(StarTracker::TICKS_TEMPERATURE_SET, new PoolEntry<uint32_t>( { 0 }));
     localDataPoolMap.emplace(StarTracker::TIME_TEMPERATURE_SET, new PoolEntry<uint64_t>( { 0 }));
     localDataPoolMap.emplace(StarTracker::MCU_TEMPERATURE, new PoolEntry<float>( { 0 }));
     localDataPoolMap.emplace(StarTracker::CMOS_TEMPERATURE, new PoolEntry<float>( { 0 }));
 
+    localDataPoolMap.emplace(StarTracker::TICKS_SOLUTION_SET, new PoolEntry<uint32_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::TIME_SOLUTION_SET, new PoolEntry<uint64_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::CALI_QW, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::CALI_QX, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::CALI_QY, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::CALI_QZ, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::TRACK_CONFIDENCE, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::TRACK_QW, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::TRACK_QX, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::TRACK_QY, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::TRACK_QZ, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::TRACK_REMOVED, new PoolEntry<uint8_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::STARS_CENTROIDED, new PoolEntry<uint8_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::STARS_MATCHED_DATABASE, new PoolEntry<uint8_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::LISA_QW, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::LISA_QX, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::LISA_QY, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::LISA_QZ, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::LISA_PERC_CLOSE, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::LISA_NR_CLOSE, new PoolEntry<uint8_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::TRUST_WORTHY, new PoolEntry<uint8_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::STABLE_COUNT, new PoolEntry<uint32_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::SOLUTION_STRATEGY, new PoolEntry<uint8_t>( { 0 }));
+
     return RETURN_OK;
 }
 
@@ -206,6 +334,74 @@ size_t StarTrackerHandler::getNextReplyLength(DeviceCommandId_t commandId){
     return StarTracker::MAX_FRAME_SIZE;
 }
 
+ReturnValue_t StarTrackerHandler::scanForActionReply(DeviceCommandId_t *foundId) {
+    switch (decodedFrame[1]) {
+    case (StarTracker::ID::PING): {
+        *foundId = StarTracker::PING_REQUEST;
+        break;
+    }
+    case (StarTracker::ID::BOOT): {
+        *foundId = StarTracker::BOOT;
+        break;
+    }
+    default:
+        sif::debug << "StarTrackerHandler::scanForSetParameterReply: Unknown parameter reply id"
+            << std::endl;
+        return RETURN_FAILED;
+    }
+    return RETURN_OK;
+}
+
+ReturnValue_t StarTrackerHandler::scanForSetParameterReply(DeviceCommandId_t *foundId) {
+    switch (decodedFrame[1]) {
+    case (StarTracker::ID::SUBSCRIBE): {
+        *foundId = StarTracker::SUBSCRIBE_TO_TM;
+        break;
+    }
+    default:
+        sif::debug << "StarTrackerHandler::scanForSetParameterReply: Unknown parameter reply id"
+            << std::endl;
+        return RETURN_FAILED;
+    }
+    return RETURN_OK;
+}
+
+ReturnValue_t StarTrackerHandler::scanForTmReply(DeviceCommandId_t *foundId) {
+    switch (decodedFrame[1]) {
+        case (StarTracker::ID::VERSION): {
+            *foundId = StarTracker::REQ_VERSION;
+            break;
+        }
+        case (StarTracker::ID::INTERFACE): {
+            *foundId = StarTracker::REQ_INTERFACE;
+            break;
+        }
+        case (StarTracker::ID::POWER): {
+            *foundId = StarTracker::REQ_POWER;
+            break;
+        }
+        case (StarTracker::ID::TEMPERATURE): {
+            *foundId = StarTracker::REQ_TEMPERATURE;
+            break;
+        }
+        case (StarTracker::ID::TIME): {
+            *foundId = StarTracker::REQ_TIME;
+            break;
+        }
+        case (StarTracker::ID::SOLUTION): {
+            *foundId = StarTracker::REQ_SOLUTION;
+            break;
+        }
+        default: {
+            sif::debug << "StarTrackerHandler::scanForReply: Reply contains invalid reply id"
+                    << std::endl;
+            return RETURN_FAILED;
+            break;
+        }
+    }
+    return RETURN_OK;
+}
+
 void StarTrackerHandler::slipInit() {
     slipInfo.buffer = rxBuffer;
     slipInfo.maxlength = StarTracker::MAX_FRAME_SIZE;
@@ -214,6 +410,25 @@ void StarTrackerHandler::slipInit() {
     slipInfo.prev_state = SLIP_COMPLETE;
 }
 
+void StarTrackerHandler::prepareBootCommand() {
+    uint32_t length = 0;
+    struct BootActionRequest bootRequest = {BOOT_REGION_ID};
+    arc_pack_boot_action_req(&bootRequest, commandBuffer, &length);
+    uint32_t encLength = 0;
+    arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
+    rawPacket = encBuffer;
+    rawPacketLen = encLength;
+}
+
+void StarTrackerHandler::prepareTimeRequest() {
+    uint32_t length = 0;
+    arc_tm_pack_time_req(commandBuffer, &length);
+    uint32_t encLength = 0;
+    arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
+    rawPacket = encBuffer;
+    rawPacketLen = encLength;
+}
+
 void StarTrackerHandler::preparePingRequest() {
     uint32_t length = 0;
     struct PingActionRequest pingRequest = {PING_ID};
@@ -233,6 +448,24 @@ void StarTrackerHandler::prepareVersionRequest() {
     rawPacketLen = encLength;
 }
 
+void StarTrackerHandler::prepareInterfaceRequest() {
+    uint32_t length = 0;
+    arc_tm_pack_interface_req(commandBuffer, &length);
+    uint32_t encLength = 0;
+    arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
+    rawPacket = encBuffer;
+    rawPacketLen = encLength;
+}
+
+void StarTrackerHandler::preparePowerRequest() {
+    uint32_t length = 0;
+    arc_tm_pack_power_req(commandBuffer, &length);
+    uint32_t encLength = 0;
+    arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
+    rawPacket = encBuffer;
+    rawPacketLen = encLength;
+}
+
 void StarTrackerHandler::prepareRebootCommand() {
     uint32_t length = 0;
     struct RebootActionRequest rebootReq;
@@ -243,6 +476,42 @@ void StarTrackerHandler::prepareRebootCommand() {
     rawPacketLen = encLength;
 }
 
+void StarTrackerHandler::prepareSubscriptionCommand(const uint8_t* tmId) {
+    uint32_t encLength = 0;
+    uint32_t length = 18;
+    commandBuffer[0] = TMTC_SETPARAMREQ;
+    commandBuffer[1] = StarTracker::ID::SUBSCRIBE;
+    // Fill all other fields with invalid tm id
+    commandBuffer[2] = *tmId;
+    commandBuffer[3] = 0;
+    commandBuffer[4] = 0;
+    commandBuffer[5] = 0;
+    commandBuffer[6] = 0;
+    commandBuffer[7] = 0;
+    commandBuffer[8] = 0;
+    commandBuffer[9] = 0;
+    commandBuffer[10] = 0;
+    commandBuffer[11] = 0;
+    commandBuffer[12] = 0;
+    commandBuffer[13] = 0;
+    commandBuffer[14] = 0;
+    commandBuffer[15] = 0;
+    commandBuffer[16] = 0;
+    commandBuffer[17] = 0;
+    arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
+    rawPacket = encBuffer;
+    rawPacketLen = encLength;
+}
+
+void StarTrackerHandler::prepareSolutionRequest() {
+    uint32_t length = 0;
+    arc_tm_pack_solution_req(commandBuffer, &length);
+    uint32_t encLength = 0;
+    arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
+    rawPacket = encBuffer;
+    rawPacketLen = encLength;
+}
+
 void StarTrackerHandler::prepareTemperatureRequest() {
     uint32_t length = 0;
     arc_tm_pack_temperature_req(commandBuffer, &length);
@@ -252,6 +521,29 @@ void StarTrackerHandler::prepareTemperatureRequest() {
     rawPacketLen = encLength;
 }
 
+ReturnValue_t StarTrackerHandler::handleSetParamReply() {
+    uint8_t status = *(decodedFrame + STATUS_OFFSET);
+    if (status != StarTracker::STATUS_OK) {
+        sif::warning << "StarTrackerHandler::handleSetParamReply: Failed to execute parameter set "
+                " command with parameter ID" <<
+                static_cast<unsigned int>(*(decodedFrame + PARAMETER_ID_OFFSET)) << std::endl;
+        return SET_PARAM_FAILED;
+    }
+    return RETURN_OK;
+}
+
+ReturnValue_t StarTrackerHandler::handleActionReply() {
+    uint8_t status = *(decodedFrame + STATUS_OFFSET);
+    if (status != StarTracker::STATUS_OK) {
+        sif::warning << "StarTrackerHandler::handleActionReply: Failed to execute action "
+                << " command with action ID "
+                << static_cast<unsigned int>(*(decodedFrame + ACTION_ID_OFFSET))
+                << " and status "<< static_cast<unsigned int>(status) << std::endl;
+        return ACTION_FAILED;
+    }
+    return RETURN_OK;
+}
+
 ReturnValue_t StarTrackerHandler::handlePingReply() {
     ReturnValue_t result = RETURN_OK;
     uint32_t pingId = 0;
@@ -269,70 +561,380 @@ ReturnValue_t StarTrackerHandler::handlePingReply() {
     return result;
 }
 
+ReturnValue_t StarTrackerHandler::handleTimeTm() {
+    ReturnValue_t result = RETURN_OK;
+    PoolReadGuard rg(&timeSet);
+    uint32_t offset = TM_DATA_FIELD_OFFSET;
+    uint8_t status = 0;
+    uint32_t ticks = 0;
+    uint64_t time = 0;
+    getTmHeaderData(&status, &ticks, &time);
+    if(status != StarTracker::STATUS_OK) {
+        sif::warning << "StarTrackerHandler::handleVersionTm: Reply error: "
+                << static_cast<unsigned int>(status) << std::endl;
+        result = VERSION_REQ_FAILED;
+        return result;
+    }
+    timeSet.ticks = ticks;
+    timeSet.time = time;
+    timeSet.runTime = deserializeUint32(decodedFrame + offset);
+    offset += sizeof(uint32_t);
+    timeSet.unixTime = deserializeUint64(decodedFrame + offset);
+#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
+    sif::info << "StarTrackerHandler::handleTimeTm: Ticks: "
+            << timeSet.ticks << std::endl;
+    sif::info << "StarTrackerHandler::handleTimeTm: Time (time stamp): "
+            << timeSet.time << " us" << std::endl;
+    sif::info << "StarTrackerHandler::handleTimeTm: Run Time: "
+            << timeSet.runTime << " ms" << std::endl;
+    sif::info << "StarTrackerHandler::handleTimeTm: Unix Time: "
+            << timeSet.unixTime << " s" << std::endl;
+#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
+    return result;
+}
+
 ReturnValue_t StarTrackerHandler::handleVersionTm() {
     ReturnValue_t result = RETURN_OK;
     PoolReadGuard rg(&versionSet);
-    uint32_t offset = 2;
+    uint32_t offset = TM_DATA_FIELD_OFFSET;
-    uint8_t status = *(decodedFrame + offset);
+    uint8_t status = 0;
+    uint32_t ticks = 0;
+    uint64_t time = 0;
+    getTmHeaderData(&status, &ticks, &time);
+    if(status != StarTracker::STATUS_OK) {
+        sif::warning << "StarTrackerHandler::handleVersionTm: Reply error: "
+                << static_cast<unsigned int>(status) << std::endl;
+        result = VERSION_REQ_FAILED;
+        return result;
+    }
+    versionSet.ticks = ticks;
+    versionSet.time = time;
+    versionSet.program = *(decodedFrame + offset);
     offset += 1;
+    versionSet.major = *(decodedFrame + offset);
+    offset += 1;
+    versionSet.minor = *(decodedFrame + offset);
+#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
+    sif::info << "StarTrackerHandler::handleVersionTm: Ticks: "
+            << versionSet.ticks << std::endl;
+    sif::info << "StarTrackerHandler::handleVersionTm: Unix Time: "
+            << versionSet.time << " us" << std::endl;
+    sif::info << "StarTrackerHandler::handleVersionTm: Program: "
+            << static_cast<unsigned int>(versionSet.program.value) << std::endl;
+    sif::info << "StarTrackerHandler::handleVersionTm: Major: "
+            << static_cast<unsigned int>(versionSet.major.value) << std::endl;
+    sif::info << "StarTrackerHandler::handleVersionTm: Minor: "
+            << static_cast<unsigned int>(versionSet.minor.value) << std::endl;
+#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
+    return result;
+}
+
+ReturnValue_t StarTrackerHandler::handleInterfaceTm() {
+    ReturnValue_t result = RETURN_OK;
+    PoolReadGuard rg(&interfaceSet);
+    uint32_t offset = TM_DATA_FIELD_OFFSET;
+    uint8_t status = 0;
+    uint32_t ticks = 0;
+    uint64_t time = 0;
+    getTmHeaderData(&status, &ticks, &time);
+    if(status != StarTracker::STATUS_OK) {
+        sif::warning << "StarTrackerHandler::handleInterfaceTm: Reply error: "
+                << static_cast<unsigned int>(status) << std::endl;
+        result = INTERFACE_REQ_FAILED;
+        return result;
+    }
+    interfaceSet.ticks = ticks;
+    interfaceSet.time = time;
+    size_t size = sizeof(uint32_t);
+    interfaceSet.frameCount = deserializeUint32(decodedFrame + offset);
+    offset += size;
+    interfaceSet.checksumerrorCount = deserializeUint32(decodedFrame + offset);
+    offset += size;
+    interfaceSet.setParamCount = deserializeUint32(decodedFrame + offset);
+    offset += size;
+    interfaceSet.setParamReplyCount = deserializeUint32(decodedFrame + offset);
+    offset += size;
+    interfaceSet.paramRequestCount = deserializeUint32(decodedFrame + offset);
+    offset += size;
+    interfaceSet.paramReplyCount = deserializeUint32(decodedFrame + offset);
+    offset += size;
+    interfaceSet.tmReplyCount = deserializeUint32(decodedFrame + offset);
+    offset += size;
+    interfaceSet.actionReqCount = deserializeUint32(decodedFrame + offset);
+    offset += size;
+    interfaceSet.actionReplyCount = deserializeUint32(decodedFrame + offset);
+
+#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
+    sif::info << "StarTrackerHandler::handleInterfaceTm: Ticks: "
+            << interfaceSet.ticks << std::endl;
+    sif::info << "StarTrackerHandler::handleInterfaceTm: Time: "
+            << interfaceSet.time << " us" << std::endl;
+    sif::info << "StarTrackerHandler::handleInterfaceTm: Frame Count: "
+            << interfaceSet.frameCount << std::endl;
+    sif::info << "StarTrackerHandler::handleInterfaceTm: Checksum Error Count: "
+            << interfaceSet.checksumerrorCount << std::endl;
+    sif::info << "StarTrackerHandler::handleInterfaceTm: Set Param Count: "
+            << interfaceSet.setParamCount << std::endl;
+    sif::info << "StarTrackerHandler::handleInterfaceTm: Set Param Reply Count: "
+            << interfaceSet.setParamReplyCount << std::endl;
+    sif::info << "StarTrackerHandler::handleInterfaceTm: Param Request Count: "
+            << interfaceSet.paramRequestCount << std::endl;
+    sif::info << "StarTrackerHandler::handleInterfaceTm: Param Reply Count: "
+            << interfaceSet.paramReplyCount << std::endl;
+    sif::info << "StarTrackerHandler::handleInterfaceTm: Req TM Count: "
+            << interfaceSet.reqTmCount << std::endl;
+    sif::info << "StarTrackerHandler::handleInterfaceTm: Telemetry Reply Count: "
+            << interfaceSet.tmReplyCount << std::endl;
+    sif::info << "StarTrackerHandler::handleInterfaceTm: Action Request Count: "
+            << interfaceSet.actionReqCount << std::endl;
+    sif::info << "StarTrackerHandler::handleInterfaceTm: Action Reply Count: "
+            << interfaceSet.actionReplyCount << std::endl;
+#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
+    return result;
+}
+
+ReturnValue_t StarTrackerHandler::handlePowerTm() {
+    ReturnValue_t result = RETURN_OK;
+    PoolReadGuard rg(&powerSet);
+    uint32_t offset = TM_DATA_FIELD_OFFSET;
+    uint8_t status = 0;
+    uint32_t ticks = 0;
+    uint64_t time = 0;
+    getTmHeaderData(&status, &ticks, &time);
+    if(status != StarTracker::STATUS_OK) {
+        sif::warning << "StarTrackerHandler::handlePowerTm: Reply error: "
+                << static_cast<unsigned int>(status) << std::endl;
+        result = POWER_REQ_FAILED;
+        return result;
+    }
+    powerSet.ticks= ticks;
+    powerSet.time= time;
+    float value = 0;
+    std::memcpy(&value, decodedFrame + offset, sizeof(value));
+    powerSet.mcuCurrent = value;
+    offset += 4;
+    std::memcpy(&value, decodedFrame + offset, sizeof(value));
+    powerSet.mcuVoltage = value;
+    offset += 4;
+    std::memcpy(&value, decodedFrame + offset, sizeof(value));
+    powerSet.fpgaCoreCurrent = value;
+    offset += 4;
+    std::memcpy(&value, decodedFrame + offset, sizeof(value));
+    powerSet.fpgaCoreVoltage = value;
+    offset += 4;
+    std::memcpy(&value, decodedFrame + offset, sizeof(value));
+    powerSet.fpga18Current = value;
+    offset += 4;
+    std::memcpy(&value, decodedFrame + offset, sizeof(value));
+    powerSet.fpga18Voltage = value;
+    offset += 4;
+    std::memcpy(&value, decodedFrame + offset, sizeof(value));
+    powerSet.fpga25Current = value;
+    offset += 4;
+    std::memcpy(&value, decodedFrame + offset, sizeof(value));
+    powerSet.fpga25Voltage = value;
+    offset += 4;
+    std::memcpy(&value, decodedFrame + offset, sizeof(value));
+    powerSet.cmv21Current = value;
+    offset += 4;
+    std::memcpy(&value, decodedFrame + offset, sizeof(value));
+    powerSet.cmv21Voltage = value;
+    offset += 4;
+    std::memcpy(&value, decodedFrame + offset, sizeof(value));
+    powerSet.cmvPixCurrent= value;
+    offset += 4;
+    std::memcpy(&value, decodedFrame + offset, sizeof(value));
+    powerSet.cmvPixVoltage = value;
+    offset += 4;
+    std::memcpy(&value, decodedFrame + offset, sizeof(value));
+    powerSet.cmv33Current= value;
+    offset += 4;
+    std::memcpy(&value, decodedFrame + offset, sizeof(value));
+    powerSet.cmv33Voltage = value;
+    offset += 4;
+    std::memcpy(&value, decodedFrame + offset, sizeof(value));
+    powerSet.cmvResCurrent= value;
+    offset += 4;
+    std::memcpy(&value, decodedFrame + offset, sizeof(value));
+    powerSet.cmvResVoltage = value;
+#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
+    sif::info << "StarTrackerHandler::handlePowerTm: Ticks: "
+            << powerSet.ticks << std::endl;
+    sif::info << "StarTrackerHandler::handlePowerTm: Time: "
+            << powerSet.time << " us" << std::endl;
+    sif::info << "StarTrackerHandler::handlePowerTm: MCU Current: "
+            << powerSet.mcuCurrent << " A" << std::endl;
+    sif::info << "StarTrackerHandler::handlePowerTm: MCU Voltage: "
+            << powerSet.mcuVoltage << " V" << std::endl;
+    sif::info << "StarTrackerHandler::handlePowerTm: FPGA Core current: "
+            << powerSet.fpgaCoreCurrent << " A" << std::endl;
+    sif::info << "StarTrackerHandler::handlePowerTm: FPGA Core voltage: "
+            << powerSet.fpgaCoreVoltage << " V" << std::endl;
+    sif::info << "StarTrackerHandler::handlePowerTm: FPGA 18 current: "
+            << powerSet.fpga18Current << " A" << std::endl;
+    sif::info << "StarTrackerHandler::handlePowerTm: FPGA 18 voltage: "
+            << powerSet.fpga18Voltage << " V" << std::endl;
+    sif::info << "StarTrackerHandler::handlePowerTm: FPGA 25 current: "
+            << powerSet.fpga25Current << " A" << std::endl;
+    sif::info << "StarTrackerHandler::handlePowerTm: FPGA 25 voltage: "
+            << powerSet.fpga25Voltage << " V" << std::endl;
+    sif::info << "StarTrackerHandler::handlePowerTm: CMV 21 current: "
+            << powerSet.cmv21Current << " A" << std::endl;
+    sif::info << "StarTrackerHandler::handlePowerTm: CMV 21 voltage: "
+            << powerSet.cmv21Voltage << " V" << std::endl;
+    sif::info << "StarTrackerHandler::handlePowerTm: CMV Pix current: "
+            << powerSet.cmvPixCurrent << " A" << std::endl;
+    sif::info << "StarTrackerHandler::handlePowerTm: CMV Pix voltage: "
+            << powerSet.cmvPixVoltage << " V" << std::endl;
+    sif::info << "StarTrackerHandler::handlePowerTm: CMV 33 current: "
+            << powerSet.cmv33Current << " A" << std::endl;
+    sif::info << "StarTrackerHandler::handlePowerTm: CMV 33 voltage: "
+            << powerSet.cmv33Voltage << " V" << std::endl;
+    sif::info << "StarTrackerHandler::handlePowerTm: CMV Res current: "
+            << powerSet.cmvResCurrent << " A" << std::endl;
+    sif::info << "StarTrackerHandler::handlePowerTm: CMV Res voltage: "
+            << powerSet.cmvResVoltage << " V" << std::endl;
+#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
+    return result;
+}
+
+ReturnValue_t StarTrackerHandler::handleSolutionTm() {
+    ReturnValue_t result = RETURN_OK;
+    result = solutionSet.read(MutexIF::TimeoutType::WAITING, 20);
+    if (result != RETURN_OK) {
+        return result;
+    }
+    uint32_t offset = TM_DATA_FIELD_OFFSET;
+    uint8_t status = 0;
+    uint32_t ticks = 0;
+    uint64_t time = 0;
+    getTmHeaderData(&status, &ticks, &time);
     if(status != StarTracker::STATUS_OK) {
         sif::warning << "StarTrackerHandler::handleTemperatureTm: Reply error: "
                 << static_cast<unsigned int>(status) << std::endl;
-        result = TEMPERATURE_REQUEST_FAILED;
+        result = TEMPERATURE_REQ_FAILED;
+        return result;
+    }
+    solutionSet.ticks= ticks;
+    solutionSet.time= time;
+    float word = 0;
+    std::memcpy(&word, decodedFrame + offset, sizeof(float));
+    solutionSet.caliQw = word;
+    offset += sizeof(float);
+    std::memcpy(&word, decodedFrame + offset, sizeof(float));
+    solutionSet.caliQx = word;
+    offset += sizeof(float);
+    std::memcpy(&word, decodedFrame + offset, sizeof(float));
+    solutionSet.caliQy = word;
+    offset += sizeof(float);
+    std::memcpy(&word, decodedFrame + offset, sizeof(float));
+    solutionSet.caliQz = word;
+    offset += sizeof(float);
+    std::memcpy(&word, decodedFrame + offset, sizeof(float));
+    solutionSet.trackConfidence = word;
+    offset += sizeof(float);
+    std::memcpy(&word, decodedFrame + offset, sizeof(float));
+    solutionSet.trackQw = word;
+    offset += sizeof(float);
+    std::memcpy(&word, decodedFrame + offset, sizeof(float));
+    solutionSet.trackQx = word;
+    offset += sizeof(float);
+    std::memcpy(&word, decodedFrame + offset, sizeof(float));
+    solutionSet.trackQy = word;
+    offset += sizeof(float);
+    std::memcpy(&word, decodedFrame + offset, sizeof(float));
+    solutionSet.trackQz = word;
+    offset += sizeof(float);
+    solutionSet.trackRemoved = *(decodedFrame + offset);
+    offset += sizeof(uint8_t);
+    solutionSet.starsCentroided = *(decodedFrame + offset);
+    offset += sizeof(uint8_t);
+    solutionSet.starsMatchedDatabase = *(decodedFrame + offset);
+    offset += sizeof(float);
+    std::memcpy(&word, decodedFrame + offset, sizeof(float));
+    solutionSet.lisaQw = word;
+    offset += sizeof(float);
+    std::memcpy(&word, decodedFrame + offset, sizeof(float));
+    solutionSet.lisaQx = word;
+    offset += sizeof(float);
+    std::memcpy(&word, decodedFrame + offset, sizeof(float));
+    solutionSet.lisaQy = word;
+    offset += sizeof(float);
+    std::memcpy(&word, decodedFrame + offset, sizeof(float));
+    solutionSet.lisaQz = word;
+    offset += sizeof(float);
+    std::memcpy(&word, decodedFrame + offset, sizeof(float));
+    solutionSet.lisaPercentageClose = word;
+    offset += sizeof(float);
+    solutionSet.lisaNrClose = *(decodedFrame + offset);
+    offset += sizeof(uint8_t);
+    solutionSet.isTrustWorthy = *(decodedFrame + offset);
+    offset += sizeof(uint8_t);
+    solutionSet.stableCount = *(decodedFrame + offset);
+    offset += sizeof(uint32_t);
+    solutionSet.stableCount = *(decodedFrame + offset);
+    result = solutionSet.commit(MutexIF::TimeoutType::WAITING, 20);
+    if (result != RETURN_OK) {
         return result;
     }
-    versionSet.program = (*decodedFrame + offset);
-    offset += 1;
-    versionSet.major = (*decodedFrame + offset);
-    offset += 1;
-    versionSet.minor = (*decodedFrame + offset);
 #if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
-    sif::info << "StarTrackerHandler::handleVersionTm: Program: "
+    solutionSet.printSet();
-            << versionSet.program << std::endl;
+#endif
-    sif::info << "StarTrackerHandler::handleVersionTm: Major: "
-            << versionSet.major << std::endl;
-    sif::info << "StarTrackerHandler::handleVersionTm: Minor: "
-            << versionSet.minor << std::endl;
-#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
     return result;
 }
 
 ReturnValue_t StarTrackerHandler::handleTemperatureTm() {
     ReturnValue_t result = RETURN_OK;
     PoolReadGuard rg(&temperatureSet);
-    uint32_t offset = 2;
+    uint32_t offset = TM_DATA_FIELD_OFFSET;
-    uint8_t status = *(decodedFrame + offset);
+    uint8_t status = 0;
-    offset += 1;
+    uint32_t ticks = 0;
+    uint64_t time = 0;
+    getTmHeaderData(&status, &ticks, &time);
     if(status != StarTracker::STATUS_OK) {
         sif::warning << "StarTrackerHandler::handleTemperatureTm: Reply error: "
                 << static_cast<unsigned int>(status) << std::endl;
-        result = TEMPERATURE_REQUEST_FAILED;
+        result = TEMPERATURE_REQ_FAILED;
         return result;
     }
-    const uint8_t* buffer = decodedFrame + offset;
+    temperatureSet.ticks= ticks;
-    size_t size = sizeof(temperatureSet.ticks);
+    temperatureSet.time= time;
-    SerializeAdapter::deSerialize(&temperatureSet.ticks, &buffer, &size, SerializeIF::Endianness::LITTLE);
+    float temperature = 0;
-    offset += size;
+    std::memcpy(&temperature, decodedFrame + offset, sizeof(temperature));
-    buffer = decodedFrame + offset;
+    temperatureSet.mcuTemperature = temperature;
-    size = sizeof(temperatureSet.time);
+    offset += sizeof(temperature);
-    SerializeAdapter::deSerialize(&temperatureSet.time, &buffer, &size, SerializeIF::Endianness::LITTLE);
+    std::memcpy(&temperature, decodedFrame + offset, sizeof(temperature));
-    offset += size;
+    temperatureSet.cmosTemperature = temperature;
-    temperatureSet.mcuTemperature = *(decodedFrame + offset) << 24
-            | *(decodedFrame + offset + 1) << 16 | *(decodedFrame + offset + 2) << 8
-            | *(decodedFrame + offset + 3);
-    offset += sizeof(temperatureSet.mcuTemperature);
-    temperatureSet.mcuTemperature = *(decodedFrame + offset) << 24
-            | *(decodedFrame + offset + 1) << 16 | *(decodedFrame + offset + 2) << 8
-            | *(decodedFrame + offset + 3);
 #if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
-    sif::info << "StarTrackerHandler::handleTemperatureTm: Ticks: "
+    solutionSet.printSet();
-            << temperatureSet.ticks << std::endl;
-    sif::info << "StarTrackerHandler::handleTemperatureTm: Time: "
-            << temperatureSet.time << std::endl;
-    sif::info << "StarTrackerHandler::handleTemperatureTm: MCU Temperature: "
-            << temperatureSet.mcuTemperature << " °C" << std::endl;
-    sif::info << "StarTrackerHandler::handleTemperatureTm: CMOS Temperature: "
-            << temperatureSet.mcuTemperature << " °C" << std::endl;
 #endif
     return result;
 }
+
+void StarTrackerHandler::getTmHeaderData(uint8_t* status, uint32_t* ticks, uint64_t* time) {
+    *status = *(decodedFrame + STATUS_OFFSET);
+    *ticks = deserializeUint32(decodedFrame + TICKS_OFFSET);
+    *time = deserializeUint64(decodedFrame + TIME_OFFSET);
+}
+
+uint32_t StarTrackerHandler::deserializeUint32(uint8_t* buffer) {
+    uint32_t word = 0;
+    word = *(buffer + 3) << 24
+        | *(buffer + 2) << 16
+        | *(buffer + 1) << 8
+        | *(buffer);
+    return word;
+}
+
+uint64_t StarTrackerHandler::deserializeUint64(uint8_t* buffer) {
+    uint64_t word = 0;
+    word = static_cast<uint64_t>(*(buffer + 7)) << 56
+        | static_cast<uint64_t>(*(buffer + 6)) << 48
+        | static_cast<uint64_t>(*(buffer + 5)) << 40
+        | static_cast<uint64_t>(*(buffer + 4)) << 32
+        | static_cast<uint64_t>(*(buffer + 3)) << 24
+        | static_cast<uint64_t>(*(buffer + 2)) << 16
+        | static_cast<uint64_t>(*(buffer + 1)) << 8
+        | static_cast<uint64_t>(*(buffer));
+    return word;
+}

mission/devices/StarTrackerHandler.h

@@ -5,6 +5,7 @@
 #include <fsfw/src/fsfw/serialize/SerializeAdapter.h>
 #include <mission/devices/devicedefinitions/StarTrackerDefinitions.h>
 #include <thirdparty/arcsec_star_tracker/common/SLIP.h>
+#include <fsfw/datapool/PoolReadGuard.h>
 
 /**
  * @brief	This is the device handler for the star tracker from arcsec.
@@ -63,16 +64,38 @@ private:
 
     static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::STR_HANDLER;
 
-    //! [EXPORT] : [COMMENT] Status in tm reply not ok
+    //! [EXPORT] : [COMMENT] Status in temperature reply signals error
-    static const ReturnValue_t TEMPERATURE_REQUEST_FAILED = MAKE_RETURN_CODE(0xA0);
+    static const ReturnValue_t TEMPERATURE_REQ_FAILED = MAKE_RETURN_CODE(0xA0);
     //! [EXPORT] : [COMMENT] Ping command failed
     static const ReturnValue_t PING_FAILED = MAKE_RETURN_CODE(0xA1);
+    //! [EXPORT] : [COMMENT] Status in version reply signals error
+    static const ReturnValue_t VERSION_REQ_FAILED = MAKE_RETURN_CODE(0xA3);
+    //! [EXPORT] : [COMMENT] Status in interface reply signals error
+    static const ReturnValue_t INTERFACE_REQ_FAILED = MAKE_RETURN_CODE(0xA4);
+    //! [EXPORT] : [COMMENT] Status in power reply signals error
+    static const ReturnValue_t POWER_REQ_FAILED = MAKE_RETURN_CODE(0xA5);
+    //! [EXPORT] : [COMMENT] Status of reply to parameter set command signals error
+    static const ReturnValue_t SET_PARAM_FAILED = MAKE_RETURN_CODE(0xA6);
+    //! [EXPORT] : [COMMENT] Status of reply to action command signals error
+    static const ReturnValue_t ACTION_FAILED = MAKE_RETURN_CODE(0xA7);
+
+    static const uint8_t STATUS_OFFSET = 2;
+    static const uint8_t TICKS_OFFSET = 3;
+    static const uint8_t TIME_OFFSET = 7;
+    static const uint8_t TM_DATA_FIELD_OFFSET = 15;
+    static const uint8_t PARAMETER_ID_OFFSET = 1;
+    static const uint8_t ACTION_ID_OFFSET = 1;
 
     // Ping request will reply ping with this ID (data field)
     static const uint32_t PING_ID = 0x55;
+    static const uint32_t BOOT_REGION_ID = 1;
 
     StarTracker::TemperatureSet temperatureSet;
     StarTracker::VersionSet versionSet;
+    StarTracker::PowerSet powerSet;
+    StarTracker::InterfaceSet interfaceSet;
+    StarTracker::TimeSet timeSet;
+    StarTracker::SolutionSet solutionSet;
 
 	uint8_t commandBuffer[StarTracker::MAX_FRAME_SIZE];
 	uint8_t rxBuffer[StarTracker::MAX_FRAME_SIZE];
@@ -94,27 +117,86 @@ private:
 	 */
 	void slipInit();
 
+	ReturnValue_t scanForActionReply(DeviceCommandId_t *foundId);
+	ReturnValue_t scanForSetParameterReply(DeviceCommandId_t *foundId);
+	ReturnValue_t scanForTmReply(DeviceCommandId_t *foundId);
+
 	/**
 	 * @brief   Fills command buffer with data to ping the star tracker
 	 */
 	void preparePingRequest();
 
+	/**
+	 * @brief   Fills command buffer with data to request the time telemetry.
+	 */
+	void prepareTimeRequest();
+
+	/**
+	 * @brief   Fills command buffer with data to boot image (works only when star tracker is
+	 *          in bootloader mode).
+	 */
+	void prepareBootCommand();
+
 	/**
 	 * @brief   Fills command buffer with data to request the version telemetry packet
 	 */
 	void prepareVersionRequest();
 
+	/**
+	 * @brief   Fills the command buffer with data to request the interface telemetry packet.
+	 */
+	void prepareInterfaceRequest();
+
+	/**
+	 * @brief   Fills the command buffer with data to request the power telemetry packet.
+	 */
+	void preparePowerRequest();
+
 	/**
 	 * @brief   Fills command buffer with data to reboot star tracker.
 	 */
 	void prepareRebootCommand();
 
+	/**
+	 * @brief   Fills command buffer with data to subscribe to a telemetry packet.
+	 *
+	 * @param tmId  The ID of the telemetry packet to subscribe to
+	 */
+	void prepareSubscriptionCommand(const uint8_t* tmId);
+
+	/**
+     * @brief   Fills command buffer with data to request solution telemtry packet (contains
+     *          attitude information)
+     */
+	void prepareSolutionRequest();
+
 	/**
      * @brief   Fills command buffer with data to request temperature from star tracker
      */
 	void prepareTemperatureRequest();
 
+	/**
+	 * @brief   Default function to handle action replies
+	 */
+	ReturnValue_t handleActionReply();
+
+	/**
+	 * @brief   Handles all set parameter replies
+	 */
+	ReturnValue_t handleSetParamReply();
+
 	ReturnValue_t handlePingReply();
+
+	/**
+	 * @brief   Fills the time set with the data of the time request reply.
+	 */
+	ReturnValue_t handleTimeTm();
+
+	/**
+	 * @brief   Handles reply data of solution request.
+	 */
+	ReturnValue_t handleSolutionTm();
+
 	/**
 	 * @brief   This function handles the telemetry reply of a temperature request.
 	 */
@@ -124,6 +206,43 @@ private:
 	 * @brief   This function handles the telemetry reply of a version request.
 	 */
 	ReturnValue_t handleVersionTm();
+
+	/**
+	 * @brief   Handles reply to request interface telemetry command.
+	 */
+	ReturnValue_t handleInterfaceTm();
+
+	/**
+	 * @brief   Handles reply to request power telemetry command.
+	 */
+	ReturnValue_t handlePowerTm();
+
+	/**
+	 * @brief   Extracts the header data of a received telemetry frame
+	 */
+	void getTmHeaderData(uint8_t* status, uint32_t* ticks, uint64_t* time);
+
+	/**
+	 * @brief   This function deserializes 8 bytes into a 32 bit unsigned integer.
+	 *
+	 * @param buffer Pointer to buffer holding the data to deserialize
+	 *
+	 * @return  The 32-bit unsigned integer.
+	 *
+	 * @note    Deserialization will be performed in little endian byte order
+	 */
+	uint32_t deserializeUint32(uint8_t* buffer);
+
+	/**
+	 * @brief   This function deserializes 8 bytes into a 64 bit unsigned integer.
+	 *
+	 * @param buffer Pointer to buffer holding the data to deserialize
+	 *
+	 * @return  The 64-bit unsigned integer.
+	 *
+	 * @note    Deserialization will be performed in little endian byte order
+	 */
+	uint64_t deserializeUint64(uint8_t* buffer);
 };
 
 #endif /* MISSION_DEVICES_STARTRACKERHANDLER_H_ */

mission/devices/devicedefinitions/StarTrackerDefinitions.h

@@ -5,6 +5,7 @@
 #include <fsfw/datapoollocal/LocalPoolVariable.h>
 #include <fsfw/devicehandlers/DeviceHandlerIF.h>
 #include "objects/systemObjectList.h"
+#include <fsfw/datapool/PoolReadGuard.h>
 
 namespace StarTracker {
 
@@ -14,32 +15,117 @@ static const uint8_t ADDRESS = 33;
 static const uint8_t STATUS_OK = 0;
 
 enum PoolIds: lp_id_t {
+    TICKS_TIME_SET,
+    TIME_TIME_SET,
+    RUN_TIME,
+    UNIX_TIME,
     TICKS_VERSION_SET,
     TIME_VERSION_SET,
+    PROGRAM,
+    MAJOR,
+    MINOR,
+    TICKS_INTERFACE_SET,
+    TIME_INTERFACE_SET,
+    FRAME_COUNT,
+    CHECKSUM_ERROR_COUNT,
+    SET_PARAM_COUNT,
+    SET_PARAM_REPLY_COUNT,
+    PARAM_REQUEST_COUNT,
+    PARAM_REPLY_COUNT,
+    REQ_TM_COUNT,
+    TM_REPLY_COUNT,
+    ACTION_REQ_COUNT,
+    ACTION_REPLY_COUNT,
+    TICKS_POWER_SET,
+    TIME_POWER_SET,
+    MCU_CURRENT,
+    MCU_VOLTAGE,
+    FPGA_CORE_CURRENT,
+    FPGA_CORE_VOLTAGE,
+    FPGA_18_CURRENT,
+    FPGA_18_VOLTAGE,
+    FPGA_25_CURRENT,
+    FPGA_25_VOLTAGE,
+    CMV_21_CURRENT,
+    CMV_21_VOLTAGE,
+    CMV_PIX_CURRENT,
+    CMV_PIX_VOLTAGE,
+    CMV_33_CURRENT,
+    CMV_33_VOLTAGE,
+    CMV_RES_CURRENT,
+    CMV_RES_VOLTAGE,
     TICKS_TEMPERATURE_SET,
     TIME_TEMPERATURE_SET,
     MCU_TEMPERATURE,
     CMOS_TEMPERATURE,
-    PROGRAM,
+
-    MAJOR,
+    TICKS_SOLUTION_SET,
-    MINOR
+    TIME_SOLUTION_SET,
+    CALI_QW,
+    CALI_QX,
+    CALI_QY,
+    CALI_QZ,
+    TRACK_CONFIDENCE,
+    TRACK_QW,
+    TRACK_QX,
+    TRACK_QY,
+    TRACK_QZ,
+    TRACK_REMOVED,
+    STARS_CENTROIDED,
+    STARS_MATCHED_DATABASE,
+    LISA_QW,
+    LISA_QX,
+    LISA_QY,
+    LISA_QZ,
+    LISA_PERC_CLOSE,
+    LISA_NR_CLOSE,
+    TRUST_WORTHY,
+    STABLE_COUNT,
+    SOLUTION_STRATEGY
 };
 
-
-
 static const DeviceCommandId_t PING_REQUEST = 0;
+// Boots image (works only in bootloader mode)
+static const DeviceCommandId_t BOOT = 1;
 static const DeviceCommandId_t REQ_VERSION = 2;
+static const DeviceCommandId_t REQ_INTERFACE = 3;
+static const DeviceCommandId_t REQ_TIME = 4;
 static const DeviceCommandId_t REBOOT = 7;
+static const DeviceCommandId_t REQ_POWER = 11;
+static const DeviceCommandId_t SUBSCRIBE_TO_TM = 18;
+static const DeviceCommandId_t REQ_SOLUTION = 24;
 static const DeviceCommandId_t REQ_TEMPERATURE = 25;
 
 static const uint32_t VERSION_SET_ID = REQ_VERSION;
+static const uint32_t INTERFACE_SET_ID = REQ_INTERFACE;
+static const uint32_t POWER_SET_ID = REQ_POWER;
 static const uint32_t TEMPERATURE_SET_ID = REQ_TEMPERATURE;
+static const uint32_t TIME_SET_ID = REQ_TIME;
+static const uint32_t SOLUTION_SET_ID = REQ_SOLUTION;
 
 /** Max size of unencoded frame */
 static const size_t MAX_FRAME_SIZE = 1200;
 
 static const uint8_t TEMPERATURE_SET_ENTRIES = 4;
-static const uint8_t VERSION_SET_ENTRIES = 3;
+static const uint8_t VERSION_SET_ENTRIES = 5;
+static const uint8_t INTERFACE_SET_ENTRIES = 12;
+static const uint8_t POWER_SET_ENTRIES = 18;
+static const uint8_t TIME_SET_ENTRIES = 4;
+static const uint8_t SOLUTION_SET_ENTRIES = 23;
+
+// Action, parameter and telemetry IDs
+namespace ID {
+    static const uint8_t PING = 0;
+    static const uint8_t BOOT = 1;
+    static const uint8_t VERSION = 2;
+    static const uint8_t INTERFACE = 3;
+    static const uint8_t REBOOT = 7;
+    static const uint8_t POWER = 11;
+    static const uint8_t SUBSCRIBE = 18;
+    static const uint8_t SOLUTION = 24;
+    static const uint8_t TEMPERATURE = 25;
+    static const uint8_t TIME = 1;
+}
 
 /**
  * @brief   This dataset can be used to store the temperature of a reaction wheel.
@@ -97,7 +183,262 @@ public:
             PoolIds::MINOR, this);
 };
 
+/**
+ * @brief   Dataset to store the interface telemtry data.
+ */
+class InterfaceSet:
+        public StaticLocalDataSet<INTERFACE_SET_ENTRIES> {
+public:
+
+    InterfaceSet(HasLocalDataPoolIF* owner):
+        StaticLocalDataSet(owner, REQ_INTERFACE) {
+    }
+
+    InterfaceSet(object_id_t objectId):
+        StaticLocalDataSet(sid_t(objectId, REQ_INTERFACE)) {
+    }
+
+    // Ticks is time reference generated by interanl counter of the star tracker
+    lp_var_t<uint32_t> ticks = lp_var_t<uint32_t>(sid.objectId,
+            PoolIds::TICKS_INTERFACE_SET, this);
+    /** Unix time in microseconds */
+    lp_var_t<uint64_t> time = lp_var_t<uint64_t>(sid.objectId,
+            PoolIds::TIME_INTERFACE_SET, this);
+    lp_var_t<uint32_t> frameCount = lp_var_t<uint32_t>(sid.objectId,
+            PoolIds::FRAME_COUNT, this);
+    lp_var_t<uint32_t> checksumerrorCount = lp_var_t<uint32_t>(sid.objectId,
+            PoolIds::CHECKSUM_ERROR_COUNT, this);
+    lp_var_t<uint32_t> setParamCount = lp_var_t<uint32_t>(sid.objectId,
+            PoolIds::SET_PARAM_COUNT, this);
+    lp_var_t<uint32_t> setParamReplyCount = lp_var_t<uint32_t>(sid.objectId,
+            PoolIds::SET_PARAM_REPLY_COUNT, this);
+    lp_var_t<uint32_t> paramRequestCount = lp_var_t<uint32_t>(sid.objectId,
+            PoolIds::PARAM_REQUEST_COUNT, this);
+    lp_var_t<uint32_t> paramReplyCount = lp_var_t<uint32_t>(sid.objectId,
+            PoolIds::PARAM_REPLY_COUNT, this);
+    lp_var_t<uint32_t> reqTmCount = lp_var_t<uint32_t>(sid.objectId,
+            PoolIds::REQ_TM_COUNT, this);
+    lp_var_t<uint32_t> tmReplyCount = lp_var_t<uint32_t>(sid.objectId,
+            PoolIds::TM_REPLY_COUNT, this);
+    lp_var_t<uint32_t> actionReqCount = lp_var_t<uint32_t>(sid.objectId,
+            PoolIds::ACTION_REQ_COUNT, this);
+    lp_var_t<uint32_t> actionReplyCount = lp_var_t<uint32_t>(sid.objectId,
+            PoolIds::ACTION_REPLY_COUNT, this);
+};
+
+/**
+ * @brief   Dataset to store the data of the power telemetry reply.
+ */
+class PowerSet:
+        public StaticLocalDataSet<POWER_SET_ENTRIES> {
+public:
+
+    PowerSet(HasLocalDataPoolIF* owner):
+        StaticLocalDataSet(owner, REQ_INTERFACE) {
+    }
+
+    PowerSet(object_id_t objectId):
+        StaticLocalDataSet(sid_t(objectId, REQ_INTERFACE)) {
+    }
+
+    // Ticks is time reference generated by interanl counter of the star tracker
+    lp_var_t<uint32_t> ticks = lp_var_t<uint32_t>(sid.objectId,
+            PoolIds::TICKS_POWER_SET, this);
+    /** Unix time in microseconds */
+    lp_var_t<uint64_t> time = lp_var_t<uint64_t>(sid.objectId,
+            PoolIds::TIME_POWER_SET, this);
+    lp_var_t<float> mcuCurrent = lp_var_t<float>(sid.objectId,
+            PoolIds::MCU_CURRENT, this);
+    lp_var_t<float> mcuVoltage = lp_var_t<float>(sid.objectId,
+            PoolIds::MCU_VOLTAGE, this);
+    lp_var_t<float> fpgaCoreCurrent = lp_var_t<float>(sid.objectId,
+            PoolIds::FPGA_CORE_CURRENT, this);
+    lp_var_t<float> fpgaCoreVoltage = lp_var_t<float>(sid.objectId,
+            PoolIds::FPGA_CORE_VOLTAGE, this);
+    lp_var_t<float> fpga18Current = lp_var_t<float>(sid.objectId,
+            PoolIds::FPGA_18_CURRENT, this);
+    lp_var_t<float> fpga18Voltage = lp_var_t<float>(sid.objectId,
+            PoolIds::FPGA_18_VOLTAGE, this);
+    lp_var_t<float> fpga25Current = lp_var_t<float>(sid.objectId,
+            PoolIds::FPGA_25_CURRENT, this);
+    lp_var_t<float> fpga25Voltage = lp_var_t<float>(sid.objectId,
+            PoolIds::FPGA_25_VOLTAGE, this);
+    lp_var_t<float> cmv21Current = lp_var_t<float>(sid.objectId,
+            PoolIds::CMV_21_CURRENT, this);
+    lp_var_t<float> cmv21Voltage = lp_var_t<float>(sid.objectId,
+            PoolIds::CMV_21_VOLTAGE, this);
+    lp_var_t<float> cmvPixCurrent = lp_var_t<float>(sid.objectId,
+            PoolIds::CMV_PIX_CURRENT, this);
+    lp_var_t<float> cmvPixVoltage = lp_var_t<float>(sid.objectId,
+            PoolIds::CMV_PIX_VOLTAGE, this);
+    lp_var_t<float> cmv33Current = lp_var_t<float>(sid.objectId,
+            PoolIds::CMV_33_CURRENT, this);
+    lp_var_t<float> cmv33Voltage = lp_var_t<float>(sid.objectId,
+            PoolIds::CMV_33_VOLTAGE, this);
+    lp_var_t<float> cmvResCurrent = lp_var_t<float>(sid.objectId,
+            PoolIds::CMV_RES_CURRENT, this);
+    lp_var_t<float> cmvResVoltage = lp_var_t<float>(sid.objectId,
+            PoolIds::CMV_RES_VOLTAGE, this);
+};
+
+/**
+ * @brief   Data set to store the time telemetry packet.
+ */
+class TimeSet:
+        public StaticLocalDataSet<TIME_SET_ENTRIES> {
+public:
+
+    TimeSet(HasLocalDataPoolIF* owner):
+        StaticLocalDataSet(owner, TIME_SET_ID) {
+    }
+
+    TimeSet(object_id_t objectId):
+        StaticLocalDataSet(sid_t(objectId, TIME_SET_ID)) {
+    }
+
+    lp_var_t<uint32_t> ticks = lp_var_t<uint32_t>(sid.objectId,
+                PoolIds::TICKS_TIME_SET, this);
+    /** Unix time in microseconds */
+    lp_var_t<uint64_t> time = lp_var_t<uint64_t>(sid.objectId,
+            PoolIds::TIME_TIME_SET, this);
+    // Number of milliseconds since processor start-up
+    lp_var_t<uint32_t> runTime = lp_var_t<uint32_t>(sid.objectId,
+            PoolIds::RUN_TIME, this);
+    // Unix time in seconds?? --> maybe typo in datasheet. Seems to be microseconds
+    lp_var_t<uint64_t> unixTime = lp_var_t<uint64_t>(sid.objectId,
+            PoolIds::UNIX_TIME, this);
+};
+
+/**
+ * @brief   The solution dataset is the main dataset of the star tracker and contains the
+ *          attitude information.
+ */
+class SolutionSet:
+        public StaticLocalDataSet<SOLUTION_SET_ENTRIES> {
+public:
+
+    SolutionSet(HasLocalDataPoolIF* owner):
+        StaticLocalDataSet(owner, SOLUTION_SET_ID) {
+    }
+
+    SolutionSet(object_id_t objectId):
+        StaticLocalDataSet(sid_t(objectId, SOLUTION_SET_ID)) {
+    }
+
+    // Ticks timestamp
+    lp_var_t<uint32_t> ticks = lp_var_t<uint32_t>(sid.objectId,
+                PoolIds::TICKS_SOLUTION_SET, this);
+    /// Unix time stamp
+    lp_var_t<uint64_t> time = lp_var_t<uint64_t>(sid.objectId,
+            PoolIds::TIME_SOLUTION_SET, this);
+    // Calibrated quaternion (takes into account the mounting quaternion), typically same as
+    // track q values
+    lp_var_t<float> caliQw = lp_var_t<float>(sid.objectId,
+            PoolIds::CALI_QW, this);
+    lp_var_t<float> caliQx = lp_var_t<float>(sid.objectId,
+            PoolIds::CALI_QX, this);
+    lp_var_t<float> caliQy = lp_var_t<float>(sid.objectId,
+            PoolIds::CALI_QY, this);
+    lp_var_t<float> caliQz = lp_var_t<float>(sid.objectId,
+            PoolIds::CALI_QZ, this);
+    // The lower this value the more confidence that the star tracker solution is correct
+    lp_var_t<float> trackConfidence = lp_var_t<float>(sid.objectId,
+            PoolIds::TRACK_CONFIDENCE, this);
+    // Estimated attitude of spacecraft
+    lp_var_t<float> trackQw = lp_var_t<float>(sid.objectId,
+                PoolIds::TRACK_QW, this);
+    lp_var_t<float> trackQx = lp_var_t<float>(sid.objectId,
+            PoolIds::TRACK_QX, this);
+    lp_var_t<float> trackQy = lp_var_t<float>(sid.objectId,
+            PoolIds::TRACK_QY, this);
+    lp_var_t<float> trackQz = lp_var_t<float>(sid.objectId,
+            PoolIds::TRACK_QZ, this);
+    // Number of stars removed from tracking solution
+    lp_var_t<uint8_t> trackRemoved = lp_var_t<uint8_t>(sid.objectId,
+            PoolIds::TRACK_REMOVED, this);
+    // Number of stars for which a valid centroid was found
+    lp_var_t<uint8_t> starsCentroided = lp_var_t<uint8_t>(sid.objectId,
+            PoolIds::STARS_CENTROIDED, this);
+    // Number of stars that matched to a database star
+    lp_var_t<uint8_t> starsMatchedDatabase = lp_var_t<uint8_t>(sid.objectId,
+            PoolIds::STARS_MATCHED_DATABASE, this);
+    // Result of LISA (lost in space algorithm), searches for stars without prior knowledge of
+    // attitude
+    lp_var_t<float> lisaQw = lp_var_t<float>(sid.objectId,
+            PoolIds::LISA_QW, this);
+    lp_var_t<float> lisaQx = lp_var_t<float>(sid.objectId,
+            PoolIds::LISA_QX, this);
+    lp_var_t<float> lisaQy = lp_var_t<float>(sid.objectId,
+            PoolIds::LISA_QY, this);
+    lp_var_t<float> lisaQz = lp_var_t<float>(sid.objectId,
+            PoolIds::LISA_QZ, this);
+    // Percentage of close stars in LISA solution
+    lp_var_t<float> lisaPercentageClose = lp_var_t<float>(sid.objectId,
+            PoolIds::LISA_PERC_CLOSE, this);
+    // Number of close stars in LISA solution
+    lp_var_t<uint8_t> lisaNrClose = lp_var_t<uint8_t>(sid.objectId,
+            PoolIds::LISA_NR_CLOSE, this);
+    // Gives a combined overview of the validation parameters (1 for valid solution, otherwise 0)
+    lp_var_t<uint8_t> isTrustWorthy = lp_var_t<uint8_t>(sid.objectId,
+            PoolIds::TRUST_WORTHY, this);
+    // Number of times the validation criteria was met
+    lp_var_t<uint32_t> stableCount = lp_var_t<uint32_t>(sid.objectId,
+            PoolIds::STABLE_COUNT, this);
+    // Shows the autonomous mode used to obtain the star tracker attitude
+    lp_var_t<uint8_t> solutionStrategy = lp_var_t<uint8_t>(sid.objectId,
+            PoolIds::SOLUTION_STRATEGY, this);
+
+    void printSet() {
+        PoolReadGuard rg(this);
+        sif::info << "SolutionSet::printSet: Ticks: "
+            << this->ticks << std::endl;
+        sif::info << "SolutionSet::printSet: Time: "
+            << this->time << std::endl;
+        sif::info << "SolutionSet::printSet: Calibrated quaternion Qw: "
+            << this->caliQw << std::endl;
+        sif::info << "SolutionSet::printSet: Calibrated quaternion Qx: "
+            << this->caliQx << std::endl;
+        sif::info << "SolutionSet::printSet: Calibrated quaternion Qy: "
+            << this->caliQy << std::endl;
+        sif::info << "SolutionSet::printSet: Calibrated quaternion Qz: "
+            << this->caliQz << std::endl;
+        sif::info << "SolutionSet::printSet: Track confidence: "
+            << this->trackConfidence << std::endl;
+        sif::info << "SolutionSet::printSet: Track Qw: "
+            << this->trackQw << std::endl;
+        sif::info << "SolutionSet::printSet: Track Qx: "
+            << this->trackQx << std::endl;
+        sif::info << "SolutionSet::printSet: Track Qy: "
+            << this->trackQy << std::endl;
+        sif::info << "SolutionSet::printSet: Track Qz: "
+            << this->trackQz << std::endl;
+        sif::info << "SolutionSet::printSet: Track removed: "
+            << static_cast<uint8_t>(this->trackRemoved) << std::endl;
+        sif::info << "SolutionSet::printSet: Number of stars centroided: "
+            << static_cast<uint8_t>(this->starsCentroided) << std::endl;
+        sif::info << "SolutionSet::printSet: Number of stars matched database: "
+            << static_cast<uint8_t>(this->starsMatchedDatabase) << std::endl;
+        sif::info << "SolutionSet::printSet: LISA Qw: "
+            << this->lisaQw << std::endl;
+        sif::info << "SolutionSet::printSet: LISA Qx: "
+            << this->lisaQx << std::endl;
+        sif::info << "SolutionSet::printSet: LISA Qy: "
+            << this->lisaQy << std::endl;
+        sif::info << "SolutionSet::printSet: LISA Qz: "
+            << this->lisaQz << std::endl;
+        sif::info << "SolutionSet::printSet: LISA Percentage close: "
+            << this->lisaPercentageClose << std::endl;
+        sif::info << "SolutionSet::printSet: LISA number of close stars: "
+            << static_cast<uint8_t>(this->lisaNrClose) << std::endl;
+        sif::info << "SolutionSet::printSet: Is trust worthy: "
+            << static_cast<uint8_t>(this->isTrustWorthy) << std::endl;
+        sif::info << "SolutionSet::printSet: Stable count: "
+            << this->stableCount << std::endl;
+        sif::info << "SolutionSet::printSet: Solution strategy: "
+            << static_cast<uint8_t>(this->solutionStrategy) << std::endl;
+    }
+};
+
 }
 
 #endif /* MISSION_STARTRACKER_DEFINITIONS_H_ */
-