From 5ea129d2529b11376544236ab4624c0975a6a7fa Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Sat, 27 Nov 2021 19:40:52 +0100 Subject: [PATCH] star tracker solution telemetry --- mission/devices/StarTrackerHandler.cpp | 734 ++++++++++++++++-- mission/devices/StarTrackerHandler.h | 123 ++- .../StarTrackerDefinitions.h | 355 ++++++++- 3 files changed, 1137 insertions(+), 75 deletions(-) diff --git a/mission/devices/StarTrackerHandler.cpp b/mission/devices/StarTrackerHandler.cpp index 10805f7e..ce69d41d 100644 --- a/mission/devices/StarTrackerHandler.cpp +++ b/mission/devices/StarTrackerHandler.cpp @@ -2,7 +2,6 @@ #include "OBSWConfig.h" #include -#include extern "C" { #include @@ -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; - - for (size_t idx = 0; idx < remainingSize; idx++) { - enum arc_dec_result decResult = arc_transport_decode_body(*(start + idx), &slipInfo, + size_t bytePos = 0; + for (bytePos = 0; bytePos < remainingSize; bytePos++) { + 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]) { - case (static_cast(StarTracker::PING_REQUEST)): { - *foundLen = decodedLength; - *foundId = StarTracker::PING_REQUEST; + switch (decodedFrame[0]) { + case TMTC_ACTIONREPLY: { + *foundLen = bytePos; + result = scanForActionReply(foundId); break; } - case (static_cast(StarTracker::REQ_VERSION)): { - *foundLen = decodedLength; - *foundId = StarTracker::PING_REQUEST; + case TMTC_SETPARAMREPLY: { + *foundLen = bytePos; + result = scanForSetParameterReply(foundId); break; } - case (static_cast(StarTracker::REQ_TEMPERATURE)): { - *foundLen = decodedLength; - *foundId = StarTracker::REQ_TEMPERATURE; + case TMTC_TELEMETRYREPLYA: + case TMTC_TELEMETRYREPLY: { + *foundLen = bytePos; + result = scanForTmReply(foundId); break; } default: { - sif::debug << "StarTrackerHandler::scanForReply: Reply contains invalid reply id" - << std::endl; - return RETURN_FAILED; - break; + sif::debug << "StarTrackerHandler::scanForReply: Reply has invalid type id" << std::endl; + result = RETURN_FAILED; } } - 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( { 0 })); + localDataPoolMap.emplace(StarTracker::TIME_TIME_SET, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::RUN_TIME, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::UNIX_TIME, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::TICKS_VERSION_SET, new PoolEntry( { 0 })); localDataPoolMap.emplace(StarTracker::TIME_VERSION_SET, new PoolEntry( { 0 })); localDataPoolMap.emplace(StarTracker::PROGRAM, new PoolEntry( { 0 })); localDataPoolMap.emplace(StarTracker::MAJOR, new PoolEntry( { 0 })); localDataPoolMap.emplace(StarTracker::MINOR, new PoolEntry( { 0 })); + + localDataPoolMap.emplace(StarTracker::TICKS_INTERFACE_SET, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::TIME_INTERFACE_SET, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::FRAME_COUNT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CHECKSUM_ERROR_COUNT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::SET_PARAM_COUNT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::SET_PARAM_REPLY_COUNT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::PARAM_REQUEST_COUNT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::PARAM_REPLY_COUNT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::REQ_TM_COUNT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::TM_REPLY_COUNT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::ACTION_REQ_COUNT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::ACTION_REPLY_COUNT, new PoolEntry( { 0 })); + + localDataPoolMap.emplace(StarTracker::TICKS_POWER_SET, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::TIME_POWER_SET, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::MCU_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::MCU_VOLTAGE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::FPGA_CORE_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::FPGA_CORE_VOLTAGE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::FPGA_18_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::FPGA_18_VOLTAGE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::FPGA_25_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::FPGA_25_VOLTAGE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CMV_21_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CMV_21_VOLTAGE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CMV_PIX_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CMV_PIX_VOLTAGE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CMV_33_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CMV_33_VOLTAGE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CMV_RES_CURRENT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CMV_RES_VOLTAGE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::TICKS_TEMPERATURE_SET, new PoolEntry( { 0 })); localDataPoolMap.emplace(StarTracker::TIME_TEMPERATURE_SET, new PoolEntry( { 0 })); localDataPoolMap.emplace(StarTracker::MCU_TEMPERATURE, new PoolEntry( { 0 })); localDataPoolMap.emplace(StarTracker::CMOS_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::TICKS_SOLUTION_SET, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::TIME_SOLUTION_SET, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CALI_QW, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CALI_QX, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CALI_QY, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CALI_QZ, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::TRACK_CONFIDENCE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::TRACK_QW, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::TRACK_QX, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::TRACK_QY, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::TRACK_QZ, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::TRACK_REMOVED, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::STARS_CENTROIDED, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::STARS_MATCHED_DATABASE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::LISA_QW, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::LISA_QX, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::LISA_QY, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::LISA_QZ, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::LISA_PERC_CLOSE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::LISA_NR_CLOSE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::TRUST_WORTHY, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::STABLE_COUNT, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::SOLUTION_STRATEGY, new PoolEntry( { 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(*(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(*(decodedFrame + ACTION_ID_OFFSET)) + << " and status "<< static_cast(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(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; - uint8_t status = *(decodedFrame + offset); + 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(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(versionSet.program.value) << std::endl; + sif::info << "StarTrackerHandler::handleVersionTm: Major: " + << static_cast(versionSet.major.value) << std::endl; + sif::info << "StarTrackerHandler::handleVersionTm: Minor: " + << static_cast(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(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(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(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: " - << versionSet.program << std::endl; - 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 */ + solutionSet.printSet(); +#endif return result; } ReturnValue_t StarTrackerHandler::handleTemperatureTm() { ReturnValue_t result = RETURN_OK; PoolReadGuard rg(&temperatureSet); - uint32_t offset = 2; - uint8_t status = *(decodedFrame + offset); - offset += 1; + 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(status) << std::endl; - result = TEMPERATURE_REQUEST_FAILED; + result = TEMPERATURE_REQ_FAILED; return result; } - const uint8_t* buffer = decodedFrame + offset; - size_t size = sizeof(temperatureSet.ticks); - SerializeAdapter::deSerialize(&temperatureSet.ticks, &buffer, &size, SerializeIF::Endianness::LITTLE); - offset += size; - buffer = decodedFrame + offset; - size = sizeof(temperatureSet.time); - SerializeAdapter::deSerialize(&temperatureSet.time, &buffer, &size, SerializeIF::Endianness::LITTLE); - offset += size; - 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); + temperatureSet.ticks= ticks; + temperatureSet.time= time; + float temperature = 0; + std::memcpy(&temperature, decodedFrame + offset, sizeof(temperature)); + temperatureSet.mcuTemperature = temperature; + offset += sizeof(temperature); + std::memcpy(&temperature, decodedFrame + offset, sizeof(temperature)); + temperatureSet.cmosTemperature = temperature; #if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 - sif::info << "StarTrackerHandler::handleTemperatureTm: Ticks: " - << 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; + solutionSet.printSet(); #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(*(buffer + 7)) << 56 + | static_cast(*(buffer + 6)) << 48 + | static_cast(*(buffer + 5)) << 40 + | static_cast(*(buffer + 4)) << 32 + | static_cast(*(buffer + 3)) << 24 + | static_cast(*(buffer + 2)) << 16 + | static_cast(*(buffer + 1)) << 8 + | static_cast(*(buffer)); + return word; +} diff --git a/mission/devices/StarTrackerHandler.h b/mission/devices/StarTrackerHandler.h index 89bd7097..a4389296 100644 --- a/mission/devices/StarTrackerHandler.h +++ b/mission/devices/StarTrackerHandler.h @@ -5,6 +5,7 @@ #include #include #include +#include /** * @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 - static const ReturnValue_t TEMPERATURE_REQUEST_FAILED = MAKE_RETURN_CODE(0xA0); + //! [EXPORT] : [COMMENT] Status in temperature reply signals error + 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_ */ diff --git a/mission/devices/devicedefinitions/StarTrackerDefinitions.h b/mission/devices/devicedefinitions/StarTrackerDefinitions.h index 6772abfe..ca671d8e 100644 --- a/mission/devices/devicedefinitions/StarTrackerDefinitions.h +++ b/mission/devices/devicedefinitions/StarTrackerDefinitions.h @@ -5,6 +5,7 @@ #include #include #include "objects/systemObjectList.h" +#include 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, - MINOR + + TICKS_SOLUTION_SET, + 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 { +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 ticks = lp_var_t(sid.objectId, + PoolIds::TICKS_INTERFACE_SET, this); + /** Unix time in microseconds */ + lp_var_t time = lp_var_t(sid.objectId, + PoolIds::TIME_INTERFACE_SET, this); + lp_var_t frameCount = lp_var_t(sid.objectId, + PoolIds::FRAME_COUNT, this); + lp_var_t checksumerrorCount = lp_var_t(sid.objectId, + PoolIds::CHECKSUM_ERROR_COUNT, this); + lp_var_t setParamCount = lp_var_t(sid.objectId, + PoolIds::SET_PARAM_COUNT, this); + lp_var_t setParamReplyCount = lp_var_t(sid.objectId, + PoolIds::SET_PARAM_REPLY_COUNT, this); + lp_var_t paramRequestCount = lp_var_t(sid.objectId, + PoolIds::PARAM_REQUEST_COUNT, this); + lp_var_t paramReplyCount = lp_var_t(sid.objectId, + PoolIds::PARAM_REPLY_COUNT, this); + lp_var_t reqTmCount = lp_var_t(sid.objectId, + PoolIds::REQ_TM_COUNT, this); + lp_var_t tmReplyCount = lp_var_t(sid.objectId, + PoolIds::TM_REPLY_COUNT, this); + lp_var_t actionReqCount = lp_var_t(sid.objectId, + PoolIds::ACTION_REQ_COUNT, this); + lp_var_t actionReplyCount = lp_var_t(sid.objectId, + PoolIds::ACTION_REPLY_COUNT, this); +}; + +/** + * @brief Dataset to store the data of the power telemetry reply. + */ +class PowerSet: + public StaticLocalDataSet { +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 ticks = lp_var_t(sid.objectId, + PoolIds::TICKS_POWER_SET, this); + /** Unix time in microseconds */ + lp_var_t time = lp_var_t(sid.objectId, + PoolIds::TIME_POWER_SET, this); + lp_var_t mcuCurrent = lp_var_t(sid.objectId, + PoolIds::MCU_CURRENT, this); + lp_var_t mcuVoltage = lp_var_t(sid.objectId, + PoolIds::MCU_VOLTAGE, this); + lp_var_t fpgaCoreCurrent = lp_var_t(sid.objectId, + PoolIds::FPGA_CORE_CURRENT, this); + lp_var_t fpgaCoreVoltage = lp_var_t(sid.objectId, + PoolIds::FPGA_CORE_VOLTAGE, this); + lp_var_t fpga18Current = lp_var_t(sid.objectId, + PoolIds::FPGA_18_CURRENT, this); + lp_var_t fpga18Voltage = lp_var_t(sid.objectId, + PoolIds::FPGA_18_VOLTAGE, this); + lp_var_t fpga25Current = lp_var_t(sid.objectId, + PoolIds::FPGA_25_CURRENT, this); + lp_var_t fpga25Voltage = lp_var_t(sid.objectId, + PoolIds::FPGA_25_VOLTAGE, this); + lp_var_t cmv21Current = lp_var_t(sid.objectId, + PoolIds::CMV_21_CURRENT, this); + lp_var_t cmv21Voltage = lp_var_t(sid.objectId, + PoolIds::CMV_21_VOLTAGE, this); + lp_var_t cmvPixCurrent = lp_var_t(sid.objectId, + PoolIds::CMV_PIX_CURRENT, this); + lp_var_t cmvPixVoltage = lp_var_t(sid.objectId, + PoolIds::CMV_PIX_VOLTAGE, this); + lp_var_t cmv33Current = lp_var_t(sid.objectId, + PoolIds::CMV_33_CURRENT, this); + lp_var_t cmv33Voltage = lp_var_t(sid.objectId, + PoolIds::CMV_33_VOLTAGE, this); + lp_var_t cmvResCurrent = lp_var_t(sid.objectId, + PoolIds::CMV_RES_CURRENT, this); + lp_var_t cmvResVoltage = lp_var_t(sid.objectId, + PoolIds::CMV_RES_VOLTAGE, this); +}; + +/** + * @brief Data set to store the time telemetry packet. + */ +class TimeSet: + public StaticLocalDataSet { +public: + + TimeSet(HasLocalDataPoolIF* owner): + StaticLocalDataSet(owner, TIME_SET_ID) { + } + + TimeSet(object_id_t objectId): + StaticLocalDataSet(sid_t(objectId, TIME_SET_ID)) { + } + + lp_var_t ticks = lp_var_t(sid.objectId, + PoolIds::TICKS_TIME_SET, this); + /** Unix time in microseconds */ + lp_var_t time = lp_var_t(sid.objectId, + PoolIds::TIME_TIME_SET, this); + // Number of milliseconds since processor start-up + lp_var_t runTime = lp_var_t(sid.objectId, + PoolIds::RUN_TIME, this); + // Unix time in seconds?? --> maybe typo in datasheet. Seems to be microseconds + lp_var_t unixTime = lp_var_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 { +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 ticks = lp_var_t(sid.objectId, + PoolIds::TICKS_SOLUTION_SET, this); + /// Unix time stamp + lp_var_t time = lp_var_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 caliQw = lp_var_t(sid.objectId, + PoolIds::CALI_QW, this); + lp_var_t caliQx = lp_var_t(sid.objectId, + PoolIds::CALI_QX, this); + lp_var_t caliQy = lp_var_t(sid.objectId, + PoolIds::CALI_QY, this); + lp_var_t caliQz = lp_var_t(sid.objectId, + PoolIds::CALI_QZ, this); + // The lower this value the more confidence that the star tracker solution is correct + lp_var_t trackConfidence = lp_var_t(sid.objectId, + PoolIds::TRACK_CONFIDENCE, this); + // Estimated attitude of spacecraft + lp_var_t trackQw = lp_var_t(sid.objectId, + PoolIds::TRACK_QW, this); + lp_var_t trackQx = lp_var_t(sid.objectId, + PoolIds::TRACK_QX, this); + lp_var_t trackQy = lp_var_t(sid.objectId, + PoolIds::TRACK_QY, this); + lp_var_t trackQz = lp_var_t(sid.objectId, + PoolIds::TRACK_QZ, this); + // Number of stars removed from tracking solution + lp_var_t trackRemoved = lp_var_t(sid.objectId, + PoolIds::TRACK_REMOVED, this); + // Number of stars for which a valid centroid was found + lp_var_t starsCentroided = lp_var_t(sid.objectId, + PoolIds::STARS_CENTROIDED, this); + // Number of stars that matched to a database star + lp_var_t starsMatchedDatabase = lp_var_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 lisaQw = lp_var_t(sid.objectId, + PoolIds::LISA_QW, this); + lp_var_t lisaQx = lp_var_t(sid.objectId, + PoolIds::LISA_QX, this); + lp_var_t lisaQy = lp_var_t(sid.objectId, + PoolIds::LISA_QY, this); + lp_var_t lisaQz = lp_var_t(sid.objectId, + PoolIds::LISA_QZ, this); + // Percentage of close stars in LISA solution + lp_var_t lisaPercentageClose = lp_var_t(sid.objectId, + PoolIds::LISA_PERC_CLOSE, this); + // Number of close stars in LISA solution + lp_var_t lisaNrClose = lp_var_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 isTrustWorthy = lp_var_t(sid.objectId, + PoolIds::TRUST_WORTHY, this); + // Number of times the validation criteria was met + lp_var_t stableCount = lp_var_t(sid.objectId, + PoolIds::STABLE_COUNT, this); + // Shows the autonomous mode used to obtain the star tracker attitude + lp_var_t solutionStrategy = lp_var_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(this->trackRemoved) << std::endl; + sif::info << "SolutionSet::printSet: Number of stars centroided: " + << static_cast(this->starsCentroided) << std::endl; + sif::info << "SolutionSet::printSet: Number of stars matched database: " + << static_cast(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(this->lisaNrClose) << std::endl; + sif::info << "SolutionSet::printSet: Is trust worthy: " + << static_cast(this->isTrustWorthy) << std::endl; + sif::info << "SolutionSet::printSet: Stable count: " + << this->stableCount << std::endl; + sif::info << "SolutionSet::printSet: Solution strategy: " + << static_cast(this->solutionStrategy) << std::endl; + } +}; + } #endif /* MISSION_STARTRACKER_DEFINITIONS_H_ */ -