948 lines
37 KiB
C++
948 lines
37 KiB
C++
#include "StarTrackerHandler.h"
|
|
#include "OBSWConfig.h"
|
|
|
|
#include <fsfw/globalfunctions/CRC.h>
|
|
|
|
extern "C" {
|
|
#include <thirdparty/arcsec_star_tracker/client/generated/telemetry.h>
|
|
#include <thirdparty/arcsec_star_tracker/client/generated/actionreq.h>
|
|
#include "common/misc.h"
|
|
}
|
|
|
|
StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF,
|
|
CookieIF * comCookie) :
|
|
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;
|
|
}
|
|
slipInit();
|
|
}
|
|
|
|
StarTrackerHandler::~StarTrackerHandler() {
|
|
}
|
|
|
|
void StarTrackerHandler::doStartUp() {
|
|
|
|
#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1
|
|
// setMode(MODE_NORMAL);
|
|
setMode(_MODE_TO_ON);
|
|
#else
|
|
setMode(_MODE_TO_ON);
|
|
#endif
|
|
}
|
|
|
|
void StarTrackerHandler::doShutDown() {
|
|
setMode(_MODE_POWER_DOWN);
|
|
}
|
|
|
|
ReturnValue_t StarTrackerHandler::buildNormalDeviceCommand(DeviceCommandId_t * id) {
|
|
switch (internalState) {
|
|
case InternalState::TEMPERATURE_REQUEST:
|
|
*id = StarTracker::REQ_TEMPERATURE;
|
|
break;
|
|
default:
|
|
sif::debug << "StarTrackerHandler::buildNormalDeviceCommand: Invalid internal step"
|
|
<< std::endl;
|
|
break;
|
|
}
|
|
return buildCommandFromCommand(*id, NULL, 0);
|
|
}
|
|
|
|
ReturnValue_t StarTrackerHandler::buildTransitionDeviceCommand(DeviceCommandId_t * id) {
|
|
return NOTHING_TO_SEND;
|
|
}
|
|
|
|
ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
|
|
const uint8_t * commandData, size_t commandDataLen) {
|
|
|
|
switch (deviceCommand) {
|
|
case (StarTracker::PING_REQUEST): {
|
|
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;
|
|
}
|
|
default:
|
|
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
|
|
}
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
}
|
|
|
|
void StarTrackerHandler::fillCommandAndReplyMap() {
|
|
/** Reply lengths are unknown because of the slip encoding. Thus always maximum reply size
|
|
* is specified */
|
|
this->insertInCommandAndReplyMap(StarTracker::PING_REQUEST, 3, nullptr,
|
|
StarTracker::MAX_FRAME_SIZE * 2 + 2);
|
|
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::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 (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: {
|
|
sif::debug << "StarTrackerHandler::scanForReply: Received asynchronous tm" << std::endl;
|
|
/** No asynchronous replies are expected as of now */
|
|
return RETURN_FAILED;
|
|
}
|
|
case ARC_DEC_ERROR_FRAME_SHORT:
|
|
return REPLY_TOO_SHORT;
|
|
case ARC_DEC_ERROR_CHECKSUM:
|
|
return CRC_FAILURE;
|
|
case ARC_DEC_SYNC: {
|
|
/** Reset length of SLIP struct for next frame */
|
|
slipInfo.length = 0;
|
|
break;
|
|
}
|
|
default:
|
|
sif::debug << "StarTrackerHandler::scanForReply: Unknown result code" << std::endl;
|
|
break;
|
|
}
|
|
}
|
|
|
|
switch (decodedFrame[0]) {
|
|
case TMTC_ACTIONREPLY: {
|
|
*foundLen = bytePos;
|
|
result = scanForActionReply(foundId);
|
|
break;
|
|
}
|
|
case TMTC_SETPARAMREPLY: {
|
|
*foundLen = bytePos;
|
|
result = scanForSetParameterReply(foundId);
|
|
break;
|
|
}
|
|
case TMTC_TELEMETRYREPLYA:
|
|
case TMTC_TELEMETRYREPLY: {
|
|
*foundLen = bytePos;
|
|
result = scanForTmReply(foundId);
|
|
break;
|
|
}
|
|
default: {
|
|
sif::debug << "StarTrackerHandler::scanForReply: Reply has invalid type id" << std::endl;
|
|
result = RETURN_FAILED;
|
|
}
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {
|
|
|
|
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;
|
|
}
|
|
default: {
|
|
sif::debug << "StarTrackerHandler::interpretDeviceReply: Unknown device reply id:" << id
|
|
<< std::endl;
|
|
result = DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
|
|
}
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
|
|
void StarTrackerHandler::setNormalDatapoolEntriesInvalid() {
|
|
|
|
}
|
|
|
|
uint32_t StarTrackerHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
|
|
return 5000;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
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;
|
|
slipInfo.length = 0;
|
|
slipInfo.unescape_next = 0;
|
|
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};
|
|
arc_pack_ping_action_req(&pingRequest, commandBuffer, &length);
|
|
uint32_t encLength = 0;
|
|
arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
|
|
rawPacket = encBuffer;
|
|
rawPacketLen = encLength;
|
|
}
|
|
|
|
void StarTrackerHandler::prepareVersionRequest() {
|
|
uint32_t length = 0;
|
|
arc_tm_pack_version_req(commandBuffer, &length);
|
|
uint32_t encLength = 0;
|
|
arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
|
|
rawPacket = encBuffer;
|
|
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;
|
|
arc_pack_reboot_action_req(&rebootReq, commandBuffer, &length);
|
|
uint32_t encLength = 0;
|
|
arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
|
|
rawPacket = encBuffer;
|
|
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);
|
|
uint32_t encLength = 0;
|
|
arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
|
|
rawPacket = encBuffer;
|
|
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;
|
|
uint8_t status = *(decodedFrame + 2);
|
|
const uint8_t* buffer = decodedFrame + 3;
|
|
size_t size = sizeof(pingId);
|
|
SerializeAdapter::deSerialize(&pingId, &buffer, &size, SerializeIF::Endianness::LITTLE);
|
|
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
|
|
sif::info << "Ping status: "<< static_cast<unsigned int>(status) << std::endl;
|
|
sif::info << "Ping id: 0x"<< std::hex << pingId << std::endl;
|
|
#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
|
|
if (status != StarTracker::STATUS_OK || pingId != PING_ID) {
|
|
result = PING_FAILED;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
ReturnValue_t StarTrackerHandler::handleTimeTm() {
|
|
ReturnValue_t result = RETURN_OK;
|
|
result = timeSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
|
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::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);
|
|
result = timeSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
|
if (result != RETURN_OK) {
|
|
return result;
|
|
}
|
|
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
|
|
timeSet.printSet();
|
|
#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 = 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;
|
|
}
|
|
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(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
|
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::handleSolutioneTm: Reply error: "
|
|
<< static_cast<unsigned int>(status) << std::endl;
|
|
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(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
|
if (result != RETURN_OK) {
|
|
return result;
|
|
}
|
|
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
|
|
solutionSet.printSet();
|
|
#endif
|
|
return result;
|
|
}
|
|
|
|
ReturnValue_t StarTrackerHandler::handleTemperatureTm() {
|
|
ReturnValue_t result = RETURN_OK;
|
|
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_REQ_FAILED;
|
|
return result;
|
|
}
|
|
result = temperatureSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
|
if (result != RETURN_OK) {
|
|
return result;
|
|
}
|
|
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;
|
|
result = temperatureSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
|
if (result != RETURN_OK) {
|
|
return result;
|
|
}
|
|
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
|
|
temperatureSet.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<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;
|
|
}
|