1192 lines
43 KiB
C++
1192 lines
43 KiB
C++
#include <fstream>
|
|
|
|
#include "StarTrackerHandler.h"
|
|
#include "OBSWConfig.h"
|
|
#include "StarTrackerJsonCommands.h"
|
|
|
|
#include <fsfw/globalfunctions/CRC.h>
|
|
#include <fsfw/ipc/QueueFactory.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, StrImageLoader* strImageLoader) :
|
|
DeviceHandlerBase(objectId, comIF, comCookie), temperatureSet(this), versionSet(this), powerSet(
|
|
this), interfaceSet(this), timeSet(this), solutionSet(this), strImageLoader(
|
|
strImageLoader) {
|
|
if (comCookie == nullptr) {
|
|
sif::error << "StarTrackerHandler: Invalid com cookie" << std::endl;
|
|
}
|
|
if (strImageLoader == nullptr) {
|
|
sif::error << "StarTrackerHandler: Invalid str image loader" << std::endl;
|
|
}
|
|
eventQueue = QueueFactory::instance()->createMessageQueue(EventMessage::EVENT_MESSAGE_SIZE * 5);
|
|
}
|
|
|
|
StarTrackerHandler::~StarTrackerHandler() {
|
|
}
|
|
|
|
ReturnValue_t StarTrackerHandler::initialize() {
|
|
ReturnValue_t result = RETURN_OK;
|
|
result = DeviceHandlerBase::initialize();
|
|
if (result != RETURN_OK) {
|
|
return result;
|
|
}
|
|
|
|
EventManagerIF* manager = ObjectManager::instance()->get<EventManagerIF>(
|
|
objects::EVENT_MANAGER);
|
|
if (manager == nullptr) {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "StarTrackerHandler::initialize: Invalid event manager" << std::endl;
|
|
#endif
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;;
|
|
}
|
|
result = manager->registerListener(eventQueue->getId());
|
|
if (result != RETURN_OK) {
|
|
return result;
|
|
}
|
|
result = manager->subscribeToEventRange(eventQueue->getId(),
|
|
event::getEventId(StrImageLoader::IMAGE_UPLOAD_FAILED),
|
|
event::getEventId(StrImageLoader::IMAGE_DOWNLOAD_SUCCESSFUL));
|
|
if (result != RETURN_OK) {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::warning << "StarTrackerHandler::initialize: Failed to subscribe to events form image"
|
|
" loader" << std::endl;
|
|
#endif
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
}
|
|
|
|
result = strImageLoader->setComIF(communicationInterface);
|
|
if (result != RETURN_OK) {
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
}
|
|
strImageLoader->setComCookie(comCookie);
|
|
return RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
|
|
const uint8_t* data, size_t size) {
|
|
|
|
ReturnValue_t result = RETURN_OK;
|
|
|
|
if (actionId == StarTracker::STOP_IMAGE_LOADER) {
|
|
strImageLoader->stopProcess();
|
|
return EXECUTION_FINISHED;
|
|
}
|
|
|
|
if (imageLoaderExecuting == true) {
|
|
return IMAGE_LOADER_EXECUTING;
|
|
}
|
|
// Intercept image loader commands which do not follow the common DHB communication flow
|
|
switch(actionId) {
|
|
case(StarTracker::UPLOAD_IMAGE): {
|
|
if (size > MAX_PATH_SIZE) {
|
|
return FILE_PATH_TOO_LONG;
|
|
}
|
|
result = strImageLoader->startImageUpload(
|
|
std::string(reinterpret_cast<const char*>(data), size));
|
|
if (result != RETURN_OK) {
|
|
return result;
|
|
}
|
|
imageLoaderExecuting = true;
|
|
return EXECUTION_FINISHED;
|
|
}
|
|
case(StarTracker::DOWNLOAD_IMAGE): {
|
|
strImageLoader->startImageDownload(std::string(reinterpret_cast<const char*>(data), size));
|
|
imageLoaderExecuting = true;
|
|
return EXECUTION_FINISHED;
|
|
}
|
|
default:
|
|
break;
|
|
}
|
|
return DeviceHandlerBase::executeAction(actionId, commandedBy, data, size);
|
|
}
|
|
|
|
void StarTrackerHandler::performOperationHook() {
|
|
EventMessage event;
|
|
for (ReturnValue_t result = eventQueue->receiveMessage(&event);
|
|
result == RETURN_OK; result = eventQueue->receiveMessage(&event)) {
|
|
switch (event.getMessageId()) {
|
|
case EventMessage::EVENT_MESSAGE:
|
|
handleEvent(&event);
|
|
break;
|
|
default:
|
|
sif::debug << "CCSDSHandler::checkEvents: Did not subscribe to this event message"
|
|
<< std::endl;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
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) {
|
|
ReturnValue_t result = RETURN_OK;
|
|
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::TAKE_IMAGE): {
|
|
prepareTakeImageCommand(commandData);
|
|
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;
|
|
}
|
|
case (StarTracker::RESET_ERROR): {
|
|
prepareErrorResetRequest();
|
|
return RETURN_OK;
|
|
}
|
|
case (StarTracker::LIMITS): {
|
|
Limits limits;
|
|
result = prepareParamCommand(commandData, commandDataLen, limits);
|
|
return result;
|
|
}
|
|
case (StarTracker::MOUNTING): {
|
|
Mounting mounting;
|
|
result = prepareParamCommand(commandData, commandDataLen, mounting);
|
|
return result;
|
|
}
|
|
case (StarTracker::CAMERA): {
|
|
Camera camera;
|
|
result = prepareParamCommand(commandData, commandDataLen, camera);
|
|
return result;
|
|
}
|
|
case (StarTracker::BLOB): {
|
|
Blob blob;
|
|
result = prepareParamCommand(commandData, commandDataLen, blob);
|
|
return result;
|
|
}
|
|
case (StarTracker::CENTROIDING): {
|
|
Centroiding centroiding;
|
|
result = prepareParamCommand(commandData, commandDataLen, centroiding);
|
|
return result;
|
|
}
|
|
case (StarTracker::LISA): {
|
|
Lisa lisa;
|
|
result = prepareParamCommand(commandData, commandDataLen, lisa);
|
|
return result;
|
|
}
|
|
case (StarTracker::MATCHING): {
|
|
Matching matching;
|
|
result = prepareParamCommand(commandData, commandDataLen, matching);
|
|
return result;
|
|
}
|
|
case (StarTracker::VALIDATION): {
|
|
Validation validation;
|
|
result = prepareParamCommand(commandData, commandDataLen, validation);
|
|
return result;
|
|
}
|
|
case (StarTracker::ALGO): {
|
|
Algo algo;
|
|
result = prepareParamCommand(commandData, commandDataLen, algo);
|
|
return result;
|
|
}
|
|
case (StarTracker::TRACKING): {
|
|
Tracking tracking;
|
|
result = prepareParamCommand(commandData, commandDataLen, tracking);
|
|
return result;
|
|
}
|
|
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->insertInCommandMap(StarTracker::UPLOAD_IMAGE);
|
|
this->insertInCommandMap(StarTracker::DOWNLOAD_IMAGE);
|
|
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);
|
|
this->insertInCommandAndReplyMap(StarTracker::LIMITS, 3, nullptr,
|
|
StarTracker::MAX_FRAME_SIZE * 2 + 2);
|
|
this->insertInCommandAndReplyMap(StarTracker::MOUNTING, 3, nullptr,
|
|
StarTracker::MAX_FRAME_SIZE * 2 + 2);
|
|
this->insertInCommandAndReplyMap(StarTracker::CAMERA, 3, nullptr,
|
|
StarTracker::MAX_FRAME_SIZE * 2 + 2);
|
|
this->insertInCommandAndReplyMap(StarTracker::BLOB, 3, nullptr,
|
|
StarTracker::MAX_FRAME_SIZE * 2 + 2);
|
|
this->insertInCommandAndReplyMap(StarTracker::CENTROIDING, 3, nullptr,
|
|
StarTracker::MAX_FRAME_SIZE * 2 + 2);
|
|
this->insertInCommandAndReplyMap(StarTracker::LISA, 3, nullptr,
|
|
StarTracker::MAX_FRAME_SIZE * 2 + 2);
|
|
this->insertInCommandAndReplyMap(StarTracker::MATCHING, 3, nullptr,
|
|
StarTracker::MAX_FRAME_SIZE * 2 + 2);
|
|
this->insertInCommandAndReplyMap(StarTracker::TRACKING, 3, nullptr,
|
|
StarTracker::MAX_FRAME_SIZE * 2 + 2);
|
|
this->insertInCommandAndReplyMap(StarTracker::VALIDATION, 3, nullptr,
|
|
StarTracker::MAX_FRAME_SIZE * 2 + 2);
|
|
this->insertInCommandAndReplyMap(StarTracker::ALGO, 3, nullptr,
|
|
StarTracker::MAX_FRAME_SIZE * 2 + 2);
|
|
this->insertInCommandAndReplyMap(StarTracker::TAKE_IMAGE, 3, nullptr,
|
|
StarTracker::MAX_FRAME_SIZE * 2 + 2);
|
|
this->insertInCommandAndReplyMap(StarTracker::RESET_ERROR, 3, nullptr,
|
|
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;
|
|
size_t bytesLeft = 0;
|
|
|
|
result = dataLinkLayer.decodeFrame(start, remainingSize, &bytesLeft);
|
|
switch(result) {
|
|
case ArcsecDatalinkLayer::DEC_IN_PROGRESS: {
|
|
remainingSize = bytesLeft;
|
|
// Need a second doSendRead pass to reaa in whole packet
|
|
return IGNORE_REPLY_DATA;
|
|
}
|
|
case RETURN_OK: {
|
|
break;
|
|
}
|
|
default:
|
|
remainingSize = bytesLeft;
|
|
return result;
|
|
}
|
|
|
|
switch (dataLinkLayer.getReplyFrameType()) {
|
|
case TMTC_ACTIONREPLY: {
|
|
*foundLen = remainingSize - bytesLeft;
|
|
result = scanForActionReply(foundId);
|
|
break;
|
|
}
|
|
case TMTC_SETPARAMREPLY: {
|
|
*foundLen = remainingSize - bytesLeft;
|
|
result = scanForParameterReply(foundId);
|
|
break;
|
|
}
|
|
case TMTC_TELEMETRYREPLYA:
|
|
case TMTC_TELEMETRYREPLY: {
|
|
*foundLen = remainingSize - bytesLeft;
|
|
result = scanForTmReply(foundId);
|
|
break;
|
|
}
|
|
default: {
|
|
sif::debug << "StarTrackerHandler::scanForReply: Reply has invalid type id" << std::endl;
|
|
result = RETURN_FAILED;
|
|
}
|
|
}
|
|
|
|
remainingSize = bytesLeft;
|
|
|
|
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::RESET_ERROR): {
|
|
result = handleActionReply();
|
|
break;
|
|
}
|
|
case (StarTracker::TAKE_IMAGE): {
|
|
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;
|
|
}
|
|
case (StarTracker::LIMITS):
|
|
case (StarTracker::MOUNTING):
|
|
case (StarTracker::CAMERA):
|
|
case (StarTracker::BLOB):
|
|
case (StarTracker::CENTROIDING):
|
|
case (StarTracker::LISA):
|
|
case (StarTracker::MATCHING):
|
|
case (StarTracker::TRACKING):
|
|
case (StarTracker::VALIDATION):
|
|
case (StarTracker::ALGO): {
|
|
result = handleSetParamReply();
|
|
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){
|
|
// Prevent DHB from polling UART during upload command. Because UART is used by image loader
|
|
// task
|
|
if (commandId == StarTracker::UPLOAD_IMAGE) {
|
|
return 0;
|
|
}
|
|
return StarTracker::MAX_FRAME_SIZE;
|
|
}
|
|
|
|
ReturnValue_t StarTrackerHandler::doSendReadHook() {
|
|
if(imageLoaderExecuting) {
|
|
return RETURN_FAILED;
|
|
}
|
|
return RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t StarTrackerHandler::scanForActionReply(DeviceCommandId_t *foundId) {
|
|
const uint8_t* reply = dataLinkLayer.getReply();
|
|
switch (*reply) {
|
|
case (StarTracker::ID::PING): {
|
|
*foundId = StarTracker::PING_REQUEST;
|
|
break;
|
|
}
|
|
case (StarTracker::ID::BOOT): {
|
|
*foundId = StarTracker::BOOT;
|
|
break;
|
|
}
|
|
case (StarTracker::ID::TAKE_IMAGE): {
|
|
*foundId = StarTracker::TAKE_IMAGE;
|
|
break;
|
|
}
|
|
case (StarTracker::ID::UPLOAD_IMAGE): {
|
|
*foundId = StarTracker::UPLOAD_IMAGE;
|
|
break;
|
|
}
|
|
case (StarTracker::ID::ERROR_RESET): {
|
|
*foundId = StarTracker::RESET_ERROR;
|
|
break;
|
|
}
|
|
default:
|
|
sif::warning << "StarTrackerHandler::scanForParameterReply: Unknown parameter reply id"
|
|
<< std::endl;
|
|
return RETURN_FAILED;
|
|
}
|
|
return RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t StarTrackerHandler::scanForParameterReply(DeviceCommandId_t *foundId) {
|
|
const uint8_t* reply = dataLinkLayer.getReply();
|
|
switch (*reply) {
|
|
case (StarTracker::ID::SUBSCRIBE): {
|
|
*foundId = StarTracker::SUBSCRIBE_TO_TM;
|
|
break;
|
|
}
|
|
case (StarTracker::ID::LIMITS): {
|
|
*foundId = StarTracker::LIMITS;
|
|
break;
|
|
}
|
|
case (StarTracker::ID::MOUNTING): {
|
|
*foundId = StarTracker::MOUNTING;
|
|
break;
|
|
}
|
|
case (StarTracker::ID::CAMERA): {
|
|
*foundId = StarTracker::CAMERA;
|
|
break;
|
|
}
|
|
case (StarTracker::ID::BLOB): {
|
|
*foundId = StarTracker::BLOB;
|
|
break;
|
|
}
|
|
case (StarTracker::ID::CENTROIDING): {
|
|
*foundId = StarTracker::CENTROIDING;
|
|
break;
|
|
}
|
|
case (StarTracker::ID::LISA): {
|
|
*foundId = StarTracker::LISA;
|
|
break;
|
|
}
|
|
case (StarTracker::ID::MATCHING): {
|
|
*foundId = StarTracker::MATCHING;
|
|
break;
|
|
}
|
|
case (StarTracker::ID::TRACKING): {
|
|
*foundId = StarTracker::TRACKING;
|
|
break;
|
|
}
|
|
case (StarTracker::ID::VALIDATION): {
|
|
*foundId = StarTracker::VALIDATION;
|
|
break;
|
|
}
|
|
case (StarTracker::ID::ALGO): {
|
|
*foundId = StarTracker::ALGO;
|
|
break;
|
|
}
|
|
default:
|
|
sif::debug << "StarTrackerHandler::scanForParameterReply: Unknown parameter reply id"
|
|
<< std::endl;
|
|
return RETURN_FAILED;
|
|
}
|
|
return RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t StarTrackerHandler::scanForTmReply(DeviceCommandId_t *foundId) {
|
|
const uint8_t* reply = dataLinkLayer.getReply();
|
|
switch (*reply) {
|
|
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::scanForTmReply: Reply contains invalid reply id"
|
|
<< std::endl;
|
|
return RETURN_FAILED;
|
|
break;
|
|
}
|
|
}
|
|
return RETURN_OK;
|
|
}
|
|
|
|
void StarTrackerHandler::handleEvent(EventMessage* eventMessage) {
|
|
object_id_t objectId = eventMessage->getReporter();
|
|
switch(objectId){
|
|
case objects::STR_IMG_LOADER: {
|
|
// All events from image loader signal either that the operation was successful or that it
|
|
// failed
|
|
imageLoaderExecuting = false;
|
|
break;
|
|
}
|
|
default:
|
|
sif::debug << "StarTrackerHandler::handleEvent: Did not subscribe to this event"
|
|
<< std::endl;
|
|
break;
|
|
}
|
|
}
|
|
|
|
void StarTrackerHandler::prepareBootCommand() {
|
|
uint32_t length = 0;
|
|
struct BootActionRequest bootRequest = {BOOT_REGION_ID};
|
|
arc_pack_boot_action_req(&bootRequest, commandBuffer, &length);
|
|
dataLinkLayer.encodeFrame(commandBuffer, length);
|
|
rawPacket = dataLinkLayer.getEncodedFrame();
|
|
rawPacketLen = dataLinkLayer.getEncodedLength();
|
|
}
|
|
|
|
void StarTrackerHandler::prepareTimeRequest() {
|
|
uint32_t length = 0;
|
|
arc_tm_pack_time_req(commandBuffer, &length);
|
|
dataLinkLayer.encodeFrame(commandBuffer, length);
|
|
rawPacket = dataLinkLayer.getEncodedFrame();
|
|
rawPacketLen = dataLinkLayer.getEncodedLength();
|
|
}
|
|
|
|
void StarTrackerHandler::preparePingRequest() {
|
|
uint32_t length = 0;
|
|
struct PingActionRequest pingRequest = {PING_ID};
|
|
arc_pack_ping_action_req(&pingRequest, commandBuffer, &length);
|
|
dataLinkLayer.encodeFrame(commandBuffer, length);
|
|
rawPacket = dataLinkLayer.getEncodedFrame();
|
|
rawPacketLen = dataLinkLayer.getEncodedLength();
|
|
}
|
|
|
|
void StarTrackerHandler::prepareVersionRequest() {
|
|
uint32_t length = 0;
|
|
arc_tm_pack_version_req(commandBuffer, &length);
|
|
dataLinkLayer.encodeFrame(commandBuffer, length);
|
|
rawPacket = dataLinkLayer.getEncodedFrame();
|
|
rawPacketLen = dataLinkLayer.getEncodedLength();
|
|
}
|
|
|
|
void StarTrackerHandler::prepareInterfaceRequest() {
|
|
uint32_t length = 0;
|
|
arc_tm_pack_interface_req(commandBuffer, &length);
|
|
dataLinkLayer.encodeFrame(commandBuffer, length);
|
|
rawPacket = dataLinkLayer.getEncodedFrame();
|
|
rawPacketLen = dataLinkLayer.getEncodedLength();
|
|
}
|
|
|
|
void StarTrackerHandler::preparePowerRequest() {
|
|
uint32_t length = 0;
|
|
arc_tm_pack_power_req(commandBuffer, &length);
|
|
dataLinkLayer.encodeFrame(commandBuffer, length);
|
|
rawPacket = dataLinkLayer.getEncodedFrame();
|
|
rawPacketLen = dataLinkLayer.getEncodedLength();
|
|
}
|
|
|
|
void StarTrackerHandler::prepareRebootCommand() {
|
|
uint32_t length = 0;
|
|
struct RebootActionRequest rebootReq;
|
|
arc_pack_reboot_action_req(&rebootReq, commandBuffer, &length);
|
|
dataLinkLayer.encodeFrame(commandBuffer, length);
|
|
rawPacket = dataLinkLayer.getEncodedFrame();
|
|
rawPacketLen = dataLinkLayer.getEncodedLength();
|
|
}
|
|
|
|
void StarTrackerHandler::prepareTakeImageCommand(const uint8_t* commandData) {
|
|
uint32_t length = 0;
|
|
struct CameraActionRequest camReq;
|
|
camReq.actionid = *commandData;
|
|
arc_pack_camera_action_req(&camReq, commandBuffer, &length);
|
|
dataLinkLayer.encodeFrame(commandBuffer, length);
|
|
rawPacket = dataLinkLayer.getEncodedFrame();
|
|
rawPacketLen = dataLinkLayer.getEncodedLength();
|
|
}
|
|
|
|
void StarTrackerHandler::prepareSubscriptionCommand(const uint8_t* tmId) {
|
|
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;
|
|
dataLinkLayer.encodeFrame(commandBuffer, length);
|
|
rawPacket = dataLinkLayer.getEncodedFrame();
|
|
rawPacketLen = dataLinkLayer.getEncodedLength();
|
|
}
|
|
|
|
void StarTrackerHandler::prepareSolutionRequest() {
|
|
uint32_t length = 0;
|
|
arc_tm_pack_solution_req(commandBuffer, &length);
|
|
dataLinkLayer.encodeFrame(commandBuffer, length);
|
|
rawPacket = dataLinkLayer.getEncodedFrame();
|
|
rawPacketLen = dataLinkLayer.getEncodedLength();
|
|
}
|
|
|
|
void StarTrackerHandler::prepareTemperatureRequest() {
|
|
uint32_t length = 0;
|
|
arc_tm_pack_temperature_req(commandBuffer, &length);
|
|
dataLinkLayer.encodeFrame(commandBuffer, length);
|
|
rawPacket = dataLinkLayer.getEncodedFrame();
|
|
rawPacketLen = dataLinkLayer.getEncodedLength();
|
|
}
|
|
|
|
void StarTrackerHandler::prepareErrorResetRequest() {
|
|
uint32_t length = 0;
|
|
struct ResetErrorSignalActionRequest req;
|
|
arc_pack_reseterrorsignal_action_req(&req, commandBuffer, &length);
|
|
dataLinkLayer.encodeFrame(commandBuffer, length);
|
|
rawPacket = dataLinkLayer.getEncodedFrame();
|
|
rawPacketLen = dataLinkLayer.getEncodedLength();
|
|
}
|
|
|
|
ReturnValue_t StarTrackerHandler::prepareParamCommand(const uint8_t* commandData,
|
|
size_t commandDataLen, ArcsecJsonParamBase& paramSet) {
|
|
ReturnValue_t result = RETURN_OK;
|
|
if (commandDataLen > MAX_PATH_SIZE) {
|
|
return FILE_PATH_TOO_LONG;
|
|
}
|
|
std::string fullName(reinterpret_cast<const char*>(commandData), commandDataLen);
|
|
|
|
result = paramSet.create(fullName, commandBuffer);
|
|
if (result != RETURN_OK) {
|
|
sif::warning << "StarTrackerHandler::prepareParamCommand: Failed to create parameter "
|
|
"command" << std::endl;
|
|
return result;
|
|
}
|
|
dataLinkLayer.encodeFrame(commandBuffer, paramSet.getSize());
|
|
rawPacket = dataLinkLayer.getEncodedFrame();
|
|
rawPacketLen = dataLinkLayer.getEncodedLength();
|
|
return RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t StarTrackerHandler::handleSetParamReply() {
|
|
const uint8_t* reply = dataLinkLayer.getReply();
|
|
uint8_t status = *(reply + STATUS_OFFSET);
|
|
if (status != StarTracker::STATUS_OK) {
|
|
sif::warning << "StarTrackerHandler::handleSetParamReply: Failed to execute parameter set "
|
|
" command with parameter ID" <<
|
|
static_cast<unsigned int>(*(reply + PARAMETER_ID_OFFSET)) << std::endl;
|
|
return SET_PARAM_FAILED;
|
|
}
|
|
return RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t StarTrackerHandler::handleActionReply() {
|
|
const uint8_t* reply = dataLinkLayer.getReply();
|
|
uint8_t status = *(reply + STATUS_OFFSET);
|
|
if (status != StarTracker::STATUS_OK) {
|
|
sif::warning << "StarTrackerHandler::handleActionReply: Failed to execute action "
|
|
<< " command with action ID "
|
|
<< static_cast<unsigned int>(*(reply + 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;
|
|
const uint8_t* reply = dataLinkLayer.getReply();
|
|
uint8_t status = *(reply + 2);
|
|
const uint8_t* buffer = reply + 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) {
|
|
sif::warning << "StarTrackerHandler::handlePingReply: Ping failed" << std::endl;
|
|
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;
|
|
}
|
|
const uint8_t* reply = dataLinkLayer.getReply();
|
|
timeSet.ticks = ticks;
|
|
timeSet.time = time;
|
|
timeSet.runTime = deserializeUint32(reply + offset);
|
|
offset += sizeof(uint32_t);
|
|
timeSet.unixTime = deserializeUint64(reply + 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;
|
|
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;
|
|
}
|
|
result = versionSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
|
if (result != RETURN_OK) {
|
|
return result;
|
|
}
|
|
const uint8_t* reply = dataLinkLayer.getReply();
|
|
versionSet.ticks = ticks;
|
|
versionSet.time = time;
|
|
versionSet.program = *(reply + offset);
|
|
offset += 1;
|
|
versionSet.major = *(reply + offset);
|
|
offset += 1;
|
|
versionSet.minor = *(reply + offset);
|
|
result = versionSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
|
if (result != RETURN_OK) {
|
|
return result;
|
|
}
|
|
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
|
|
versionSet.printSet();
|
|
#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
|
|
return result;
|
|
}
|
|
|
|
ReturnValue_t StarTrackerHandler::handleInterfaceTm() {
|
|
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::handleInterfaceTm: Reply error: "
|
|
<< static_cast<unsigned int>(status) << std::endl;
|
|
result = INTERFACE_REQ_FAILED;
|
|
return result;
|
|
}
|
|
result = interfaceSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
|
if (result != RETURN_OK) {
|
|
return result;
|
|
}
|
|
const uint8_t* reply = dataLinkLayer.getReply();
|
|
interfaceSet.ticks = ticks;
|
|
interfaceSet.time = time;
|
|
size_t size = sizeof(uint32_t);
|
|
interfaceSet.frameCount = deserializeUint32(reply + offset);
|
|
offset += size;
|
|
interfaceSet.checksumerrorCount = deserializeUint32(reply + offset);
|
|
result = interfaceSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
|
if (result != RETURN_OK) {
|
|
return result;
|
|
}
|
|
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
|
|
interfaceSet.printSet();
|
|
#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
|
|
return result;
|
|
}
|
|
|
|
ReturnValue_t StarTrackerHandler::handlePowerTm() {
|
|
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::handlePowerTm: Reply error: "
|
|
<< static_cast<unsigned int>(status) << std::endl;
|
|
result = POWER_REQ_FAILED;
|
|
return result;
|
|
}
|
|
result = powerSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
|
if (result != RETURN_OK) {
|
|
return result;
|
|
}
|
|
const uint8_t* reply = dataLinkLayer.getReply();
|
|
powerSet.ticks= ticks;
|
|
powerSet.time= time;
|
|
float value = 0;
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
powerSet.mcuCurrent = value;
|
|
offset += 4;
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
powerSet.mcuVoltage = value;
|
|
offset += 4;
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
powerSet.fpgaCoreCurrent = value;
|
|
offset += 4;
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
powerSet.fpgaCoreVoltage = value;
|
|
offset += 4;
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
powerSet.fpga18Current = value;
|
|
offset += 4;
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
powerSet.fpga18Voltage = value;
|
|
offset += 4;
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
powerSet.fpga25Current = value;
|
|
offset += 4;
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
powerSet.fpga25Voltage = value;
|
|
offset += 4;
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
powerSet.cmv21Current = value;
|
|
offset += 4;
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
powerSet.cmv21Voltage = value;
|
|
offset += 4;
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
powerSet.cmvPixCurrent= value;
|
|
offset += 4;
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
powerSet.cmvPixVoltage = value;
|
|
offset += 4;
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
powerSet.cmv33Current= value;
|
|
offset += 4;
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
powerSet.cmv33Voltage = value;
|
|
offset += 4;
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
powerSet.cmvResCurrent= value;
|
|
offset += 4;
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
powerSet.cmvResVoltage = value;
|
|
result = powerSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
|
if (result != RETURN_OK) {
|
|
return result;
|
|
}
|
|
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
|
|
powerSet.printSet();
|
|
#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;
|
|
}
|
|
const uint8_t* reply = dataLinkLayer.getReply();
|
|
solutionSet.ticks= ticks;
|
|
solutionSet.time= time;
|
|
float word = 0;
|
|
std::memcpy(&word, reply + offset, sizeof(float));
|
|
solutionSet.caliQw = word;
|
|
offset += sizeof(float);
|
|
std::memcpy(&word, reply + offset, sizeof(float));
|
|
solutionSet.caliQx = word;
|
|
offset += sizeof(float);
|
|
std::memcpy(&word, reply + offset, sizeof(float));
|
|
solutionSet.caliQy = word;
|
|
offset += sizeof(float);
|
|
std::memcpy(&word, reply + offset, sizeof(float));
|
|
solutionSet.caliQz = word;
|
|
offset += sizeof(float);
|
|
std::memcpy(&word, reply + offset, sizeof(float));
|
|
solutionSet.trackConfidence = word;
|
|
offset += sizeof(float);
|
|
std::memcpy(&word, reply + offset, sizeof(float));
|
|
solutionSet.trackQw = word;
|
|
offset += sizeof(float);
|
|
std::memcpy(&word, reply + offset, sizeof(float));
|
|
solutionSet.trackQx = word;
|
|
offset += sizeof(float);
|
|
std::memcpy(&word, reply + offset, sizeof(float));
|
|
solutionSet.trackQy = word;
|
|
offset += sizeof(float);
|
|
std::memcpy(&word, reply + offset, sizeof(float));
|
|
solutionSet.trackQz = word;
|
|
offset += sizeof(float);
|
|
solutionSet.trackRemoved = *(reply + offset);
|
|
offset += sizeof(uint8_t);
|
|
solutionSet.starsCentroided = *(reply + offset);
|
|
offset += sizeof(uint8_t);
|
|
solutionSet.starsMatchedDatabase = *(reply + offset);
|
|
offset += sizeof(float);
|
|
std::memcpy(&word, reply + offset, sizeof(float));
|
|
solutionSet.lisaQw = word;
|
|
offset += sizeof(float);
|
|
std::memcpy(&word, reply + offset, sizeof(float));
|
|
solutionSet.lisaQx = word;
|
|
offset += sizeof(float);
|
|
std::memcpy(&word, reply + offset, sizeof(float));
|
|
solutionSet.lisaQy = word;
|
|
offset += sizeof(float);
|
|
std::memcpy(&word, reply + offset, sizeof(float));
|
|
solutionSet.lisaQz = word;
|
|
offset += sizeof(float);
|
|
std::memcpy(&word, reply + offset, sizeof(float));
|
|
solutionSet.lisaPercentageClose = word;
|
|
offset += sizeof(float);
|
|
solutionSet.lisaNrClose = *(reply + offset);
|
|
offset += sizeof(uint8_t);
|
|
solutionSet.isTrustWorthy = *(reply + offset);
|
|
offset += sizeof(uint8_t);
|
|
solutionSet.stableCount = *(reply + offset);
|
|
offset += sizeof(uint32_t);
|
|
solutionSet.stableCount = *(reply + 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;
|
|
}
|
|
const uint8_t* reply = dataLinkLayer.getReply();
|
|
temperatureSet.ticks= ticks;
|
|
temperatureSet.time= time;
|
|
float temperature = 0;
|
|
std::memcpy(&temperature, reply + offset, sizeof(temperature));
|
|
temperatureSet.mcuTemperature = temperature;
|
|
offset += sizeof(float);
|
|
std::memcpy(&temperature, reply + 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) {
|
|
const uint8_t* reply = dataLinkLayer.getReply();
|
|
*status = *(reply + STATUS_OFFSET);
|
|
*ticks = deserializeUint32(reply + TICKS_OFFSET);
|
|
*time = deserializeUint64(reply + TIME_OFFSET);
|
|
}
|
|
|
|
uint32_t StarTrackerHandler::deserializeUint32(const uint8_t* buffer) {
|
|
uint32_t word = 0;
|
|
word = *(buffer + 3) << 24
|
|
| *(buffer + 2) << 16
|
|
| *(buffer + 1) << 8
|
|
| *(buffer);
|
|
return word;
|
|
}
|
|
|
|
uint64_t StarTrackerHandler::deserializeUint64(const 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;
|
|
}
|