eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 121 Wiki Activity

str img loader wip

Browse Source
This commit is contained in:
Jakob Meier
2021-12-02 08:05:33 +01:00
parent 8c649b3e70
commit a7ab2bb93a
11 changed files with 526 additions and 214 deletions
Download Patch File Download Diff File Expand all files Collapse all files

381
mission/devices/StarTrackerHandler.cpp
View File

@ -20,17 +20,83 @@ StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF,
if (strImageLoader == nullptr) {
sif::error << "StarTrackerHandler: Invalid str image loader" << std::endl;
}
eventQueue = QueueFactory::instance()->createMessageQueue(EventMessage::EVENT_MESSAGE_SIZE * 5);
slipInit();
}
StarTrackerHandler::~StarTrackerHandler() {
DeviceHandlerBase::initialize();
}
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 != HasReturnvaluesIF::RETURN_OK) {
return result;
}
result = manager->subscribeToAllEventsFrom(eventQueue->getId(), objects::STR_IMG_LOADER);
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;
}
strImageLoader->setComIF(communicationInterface);
strImageLoader->setComCookie(comCookie);
}
ReturnValue_t StarTrackerHandler::initialize() {
ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
const uint8_t* data, size_t size) {
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): {
strImageLoader->startImageUpload();
imageLoaderExecuting = true;
return EXECUTION_FINISHED;
}
case(StarTracker::DOWNLOAD_IMAGE): {
strImageLoader->startImageDownload();
imageLoaderExecuting = true;
return EXECUTION_FINISHED;
}
default:
break;
}
return DeviceHandlerBase::executeAction();
}
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() {
@ -89,7 +155,9 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi
return RETURN_OK;
}
case (StarTracker::UPLOAD_IMAGE): {
result = prepareImageUploadCommand(commandData, commandDataLen);
std::string uploadImage = std::string(reinterpret_cast<const char*>(commandData),
commandDataLen);
strImageLoader->startImageUpload(uploadImage);
return result;
}
case (StarTracker::REQ_POWER): {
@ -129,8 +197,7 @@ void StarTrackerHandler::fillCommandAndReplyMap() {
StarTracker::MAX_FRAME_SIZE * 2 + 2);
this->insertInCommandAndReplyMap(StarTracker::REQ_TIME, 3, &timeSet,
StarTracker::MAX_FRAME_SIZE * 2 + 2);
this->insertInCommandAndReplyMap(StarTracker::UPLOAD_IMAGE, 3, nullptr,
StarTracker::MAX_FRAME_SIZE * 2 + 2);
this->insertInCommandMap(StarTracker::UPLOAD_IMAGE);
this->insertInCommandAndReplyMap(StarTracker::REQ_POWER, 3, &powerSet,
StarTracker::MAX_FRAME_SIZE * 2 + 2);
this->insertInCommandAndReplyMap(StarTracker::REQ_INTERFACE, 3, &interfaceSet,
@ -151,52 +218,36 @@ ReturnValue_t StarTrackerHandler::scanForReply(const uint8_t *start, size_t rema
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);
size_t bytesLeft = 0;
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;
}
result = dataLinkLayer.decodeFrame(start, remainingSize, &bytesLeft);
remainingSize = *bytesLeft;
switch(result) {
case ArcsecDatalinkLayer::DEC_IN_PROGRESS: {
// Need a second doSendRead pass to reaa in whole packet
return IGNORE_REPLY_DATA;
}
case RETURN_OK: {
break;
}
default:
return result;
}
switch (decodedFrame[0]) {
switch (dataLinkLayer.getReplyFrameType()) {
case TMTC_ACTIONREPLY: {
*foundLen = bytePos;
*foundLen = remainingsize - bytesLeft;
result = scanForActionReply(foundId);
break;
}
case TMTC_SETPARAMREPLY: {
*foundLen = bytePos;
*foundLen = remainingsize - bytesLeft;
result = scanForSetParameterReply(foundId);
break;
}
case TMTC_TELEMETRYREPLYA:
case TMTC_TELEMETRYREPLY: {
*foundLen = bytePos;
*foundLen = remainingsize - bytesLeft;
result = scanForTmReply(foundId);
break;
}
@ -238,10 +289,6 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, con
result = handleInterfaceTm();
break;
}
case (StarTracker::UPLOAD_IMAGE): {
result = handleUploadImageReply();
break;
}
case (StarTracker::REQ_POWER): {
result = handlePowerTm();
break;
@ -352,11 +399,17 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l
}
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::scanForActionReply(DeviceCommandId_t *foundId) {
switch (decodedFrame[1]) {
const uint8_t* reply = dataLinkLayer.getReply();
switch (*reply) {
case (StarTracker::ID::PING): {
*foundId = StarTracker::PING_REQUEST;
break;
@ -378,7 +431,8 @@ ReturnValue_t StarTrackerHandler::scanForActionReply(DeviceCommandId_t *foundId)
}
ReturnValue_t StarTrackerHandler::scanForSetParameterReply(DeviceCommandId_t *foundId) {
switch (decodedFrame[1]) {
const uint8_t* reply = dataLinkLayer.getReply();
switch (*reply) {
case (StarTracker::ID::SUBSCRIBE): {
*foundId = StarTracker::SUBSCRIBE_TO_TM;
break;
@ -392,7 +446,8 @@ ReturnValue_t StarTrackerHandler::scanForSetParameterReply(DeviceCommandId_t *fo
}
ReturnValue_t StarTrackerHandler::scanForTmReply(DeviceCommandId_t *foundId) {
switch (decodedFrame[1]) {
const uint8_t* reply = dataLinkLayer.getReply();
switch (*reply) {
case (StarTracker::ID::VERSION): {
*foundId = StarTracker::REQ_VERSION;
break;
@ -418,7 +473,7 @@ ReturnValue_t StarTrackerHandler::scanForTmReply(DeviceCommandId_t *foundId) {
break;
}
default: {
sif::debug << "StarTrackerHandler::scanForReply: Reply contains invalid reply id"
sif::debug << "StarTrackerHandler::scanForTmReply: Reply contains invalid reply id"
<< std::endl;
return RETURN_FAILED;
break;
@ -427,31 +482,37 @@ ReturnValue_t StarTrackerHandler::scanForTmReply(DeviceCommandId_t *foundId) {
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::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);
uint32_t encLength = 0;
arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
rawPacket = encBuffer;
rawPacketLen = encLength;
dataLinkLayer.encodeFrame(commandBuffer, length);
rawPacket = dataLinkLayer.getEncodedFrame();
rawPacketLen = dataLinkLayer.getEncodedLength();
}
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;
dataLinkLayer.encodeFrame(commandBuffer, length);
rawPacket = dataLinkLayer.getEncodedFrame();
rawPacketLen = dataLinkLayer.getEncodedLength();
}
void StarTrackerHandler::preparePingRequest() {
@ -467,64 +528,37 @@ void StarTrackerHandler::preparePingRequest() {
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;
dataLinkLayer.encodeFrame(commandBuffer, length);
rawPacket = dataLinkLayer.getEncodedFrame();
rawPacketLen = dataLinkLayer.getEncodedLength();
}
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;
}
ReturnValue_t StarTrackerHandler::prepareImageUploadCommand(const uint8_t* commandData,
size_t commandDataLen) {
if (commandDataLen != UPLOAD_COMMAND_LEN) {
return INVALID_UPLOAD_COMMAND;
}
uint32_t length = 0;
uint32_t position = deserializeUint32(commandData);
if (position > MAX_POSITION) {
return MAX_POSITION;
}
rememberUploadPosition = position;
struct UploadActionRequest uploadRequest;
uploadRequest.position = position;
std::memcpy(uploadRequest.data, commandData + 4, commandDataLen - 4);
arc_pack_upload_action_req(&uploadRequest, commandBuffer, &length);
uint32_t encLength = 0;
arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
rawPacket = encBuffer;
rawPacketLen = encLength;
return RETURN_OK;
dataLinkLayer.encodeFrame(commandBuffer, length);
rawPacket = dataLinkLayer.getEncodedFrame();
rawPacketLen = dataLinkLayer.getEncodedLength();
}
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;
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);
uint32_t encLength = 0;
arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
rawPacket = encBuffer;
rawPacketLen = encLength;
dataLinkLayer.encodeFrame(commandBuffer, length);
rawPacket = dataLinkLayer.getEncodedFrame();
rawPacketLen = dataLinkLayer.getEncodedLength();
}
void StarTrackerHandler::prepareSubscriptionCommand(const uint8_t* tmId) {
uint32_t encLength = 0;
uint32_t length = 18;
commandBuffer[0] = TMTC_SETPARAMREQ;
commandBuffer[1] = StarTracker::ID::SUBSCRIBE;
@ -545,9 +579,9 @@ void StarTrackerHandler::prepareSubscriptionCommand(const uint8_t* tmId) {
commandBuffer[15] = 0;
commandBuffer[16] = 0;
commandBuffer[17] = 0;
arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
rawPacket = encBuffer;
rawPacketLen = encLength;
dataLinkLayer.encodeFrame(commandBuffer, length);
rawPacket = dataLinkLayer.getEncodedFrame();
rawPacketLen = dataLinkLayer.getEncodedLength();
}
void StarTrackerHandler::prepareSolutionRequest() {
@ -562,29 +596,30 @@ void StarTrackerHandler::prepareSolutionRequest() {
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;
dataLinkLayer.encodeFrame(commandBuffer, length);
rawPacket = dataLinkLayer.getEncodedFrame();
rawPacketLen = dataLinkLayer.getEncodedLength();
}
ReturnValue_t StarTrackerHandler::handleSetParamReply() {
uint8_t status = *(decodedFrame + STATUS_OFFSET);
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>(*(decodedFrame + PARAMETER_ID_OFFSET)) << std::endl;
static_cast<unsigned int>(*(reply + PARAMETER_ID_OFFSET)) << std::endl;
return SET_PARAM_FAILED;
}
return RETURN_OK;
}
ReturnValue_t StarTrackerHandler::handleActionReply() {
uint8_t status = *(decodedFrame + STATUS_OFFSET);
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>(*(decodedFrame + ACTION_ID_OFFSET))
<< static_cast<unsigned int>(*(reply + ACTION_ID_OFFSET))
<< " and status "<< static_cast<unsigned int>(status) << std::endl;
return ACTION_FAILED;
}
@ -594,8 +629,9 @@ ReturnValue_t StarTrackerHandler::handleActionReply() {
ReturnValue_t StarTrackerHandler::handlePingReply() {
ReturnValue_t result = RETURN_OK;
uint32_t pingId = 0;
uint8_t status = *(decodedFrame + 2);
const uint8_t* buffer = decodedFrame + 3;
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
@ -609,22 +645,6 @@ ReturnValue_t StarTrackerHandler::handlePingReply() {
return result;
}
ReturnValue_t StarTrackerHandler::handleUploadImageReply() {
ReturnValue_t result = RETURN_OK;
result = handleActionReply();
if (result != RETURN_OK) {
return result;
}
// Position seems to be always 0 (independent of sent position)
// uint32_t position = deserializeUint32(decodedFrame + ACTION_DATA_OFFSET);
// if (position != rememberUploadPosition) {
// sif::warning << "StarTrackerHandler::handleUploadImageReply: Invalid position"
// << std::endl;
// return UPLOAD_IMAGE_FAILED;
// }
return result;
}
ReturnValue_t StarTrackerHandler::handleTimeTm() {
ReturnValue_t result = RETURN_OK;
result = timeSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
@ -642,11 +662,12 @@ ReturnValue_t StarTrackerHandler::handleTimeTm() {
result = VERSION_REQ_FAILED;
return result;
}
const uint8_t* reply = dataLinkLayer.getReply();
timeSet.ticks = ticks;
timeSet.time = time;
timeSet.runTime = deserializeUint32(decodedFrame + offset);
timeSet.runTime = deserializeUint32(reply + offset);
offset += sizeof(uint32_t);
timeSet.unixTime = deserializeUint64(decodedFrame + offset);
timeSet.unixTime = deserializeUint64(reply + offset);
result = timeSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
if (result != RETURN_OK) {
return result;
@ -674,13 +695,14 @@ ReturnValue_t StarTrackerHandler::handleVersionTm() {
if (result != RETURN_OK) {
return result;
}
const uint8_t* reply = dataLinkLayer.getReply();
versionSet.ticks = ticks;
versionSet.time = time;
versionSet.program = *(decodedFrame + offset);
versionSet.program = *(reply + offset);
offset += 1;
versionSet.major = *(decodedFrame + offset);
versionSet.major = *(reply + offset);
offset += 1;
versionSet.minor = *(decodedFrame + offset);
versionSet.minor = *(reply + offset);
result = versionSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
if (result != RETURN_OK) {
return result;
@ -708,12 +730,13 @@ ReturnValue_t StarTrackerHandler::handleInterfaceTm() {
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(decodedFrame + offset);
interfaceSet.frameCount = deserializeUint32(reply + offset);
offset += size;
interfaceSet.checksumerrorCount = deserializeUint32(decodedFrame + offset);
interfaceSet.checksumerrorCount = deserializeUint32(reply + offset);
result = interfaceSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
if (result != RETURN_OK) {
return result;
@ -741,55 +764,56 @@ ReturnValue_t StarTrackerHandler::handlePowerTm() {
if (result != RETURN_OK) {
return result;
}
const uint8_t* reply = dataLinkLayer.getReply();
powerSet.ticks= ticks;
powerSet.time= time;
float value = 0;
std::memcpy(&value, decodedFrame + offset, sizeof(value));
std::memcpy(&value, reply + offset, sizeof(value));
powerSet.mcuCurrent = value;
offset += 4;
std::memcpy(&value, decodedFrame + offset, sizeof(value));
std::memcpy(&value, reply + offset, sizeof(value));
powerSet.mcuVoltage = value;
offset += 4;
std::memcpy(&value, decodedFrame + offset, sizeof(value));
std::memcpy(&value, reply + offset, sizeof(value));
powerSet.fpgaCoreCurrent = value;
offset += 4;
std::memcpy(&value, decodedFrame + offset, sizeof(value));
std::memcpy(&value, reply + offset, sizeof(value));
powerSet.fpgaCoreVoltage = value;
offset += 4;
std::memcpy(&value, decodedFrame + offset, sizeof(value));
std::memcpy(&value, reply + offset, sizeof(value));
powerSet.fpga18Current = value;
offset += 4;
std::memcpy(&value, decodedFrame + offset, sizeof(value));
std::memcpy(&value, reply + offset, sizeof(value));
powerSet.fpga18Voltage = value;
offset += 4;
std::memcpy(&value, decodedFrame + offset, sizeof(value));
std::memcpy(&value, reply + offset, sizeof(value));
powerSet.fpga25Current = value;
offset += 4;
std::memcpy(&value, decodedFrame + offset, sizeof(value));
std::memcpy(&value, reply + offset, sizeof(value));
powerSet.fpga25Voltage = value;
offset += 4;
std::memcpy(&value, decodedFrame + offset, sizeof(value));
std::memcpy(&value, reply + offset, sizeof(value));
powerSet.cmv21Current = value;
offset += 4;
std::memcpy(&value, decodedFrame + offset, sizeof(value));
std::memcpy(&value, reply + offset, sizeof(value));
powerSet.cmv21Voltage = value;
offset += 4;
std::memcpy(&value, decodedFrame + offset, sizeof(value));
std::memcpy(&value, reply + offset, sizeof(value));
powerSet.cmvPixCurrent= value;
offset += 4;
std::memcpy(&value, decodedFrame + offset, sizeof(value));
std::memcpy(&value, reply + offset, sizeof(value));
powerSet.cmvPixVoltage = value;
offset += 4;
std::memcpy(&value, decodedFrame + offset, sizeof(value));
std::memcpy(&value, reply + offset, sizeof(value));
powerSet.cmv33Current= value;
offset += 4;
std::memcpy(&value, decodedFrame + offset, sizeof(value));
std::memcpy(&value, reply + offset, sizeof(value));
powerSet.cmv33Voltage = value;
offset += 4;
std::memcpy(&value, decodedFrame + offset, sizeof(value));
std::memcpy(&value, reply + offset, sizeof(value));
powerSet.cmvResCurrent= value;
offset += 4;
std::memcpy(&value, decodedFrame + offset, sizeof(value));
std::memcpy(&value, reply + offset, sizeof(value));
powerSet.cmvResVoltage = value;
result = powerSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
if (result != RETURN_OK) {
@ -818,64 +842,65 @@ ReturnValue_t StarTrackerHandler::handleSolutionTm() {
result = TEMPERATURE_REQ_FAILED;
return result;
}
const uint8_t* reply = dataLinkLayer.getReply();
solutionSet.ticks= ticks;
solutionSet.time= time;
float word = 0;
std::memcpy(&word, decodedFrame + offset, sizeof(float));
std::memcpy(&word, reply + offset, sizeof(float));
solutionSet.caliQw = word;
offset += sizeof(float);
std::memcpy(&word, decodedFrame + offset, sizeof(float));
std::memcpy(&word, reply + offset, sizeof(float));
solutionSet.caliQx = word;
offset += sizeof(float);
std::memcpy(&word, decodedFrame + offset, sizeof(float));
std::memcpy(&word, reply + offset, sizeof(float));
solutionSet.caliQy = word;
offset += sizeof(float);
std::memcpy(&word, decodedFrame + offset, sizeof(float));
std::memcpy(&word, reply + offset, sizeof(float));
solutionSet.caliQz = word;
offset += sizeof(float);
std::memcpy(&word, decodedFrame + offset, sizeof(float));
std::memcpy(&word, reply + offset, sizeof(float));
solutionSet.trackConfidence = word;
offset += sizeof(float);
std::memcpy(&word, decodedFrame + offset, sizeof(float));
std::memcpy(&word, reply + offset, sizeof(float));
solutionSet.trackQw = word;
offset += sizeof(float);
std::memcpy(&word, decodedFrame + offset, sizeof(float));
std::memcpy(&word, reply + offset, sizeof(float));
solutionSet.trackQx = word;
offset += sizeof(float);
std::memcpy(&word, decodedFrame + offset, sizeof(float));
std::memcpy(&word, reply + offset, sizeof(float));
solutionSet.trackQy = word;
offset += sizeof(float);
std::memcpy(&word, decodedFrame + offset, sizeof(float));
std::memcpy(&word, reply + offset, sizeof(float));
solutionSet.trackQz = word;
offset += sizeof(float);
solutionSet.trackRemoved = *(decodedFrame + offset);
solutionSet.trackRemoved = *(reply + offset);
offset += sizeof(uint8_t);
solutionSet.starsCentroided = *(decodedFrame + offset);
solutionSet.starsCentroided = *(reply + offset);
offset += sizeof(uint8_t);
solutionSet.starsMatchedDatabase = *(decodedFrame + offset);
solutionSet.starsMatchedDatabase = *(reply + offset);
offset += sizeof(float);
std::memcpy(&word, decodedFrame + offset, sizeof(float));
std::memcpy(&word, reply + offset, sizeof(float));
solutionSet.lisaQw = word;
offset += sizeof(float);
std::memcpy(&word, decodedFrame + offset, sizeof(float));
std::memcpy(&word, reply + offset, sizeof(float));
solutionSet.lisaQx = word;
offset += sizeof(float);
std::memcpy(&word, decodedFrame + offset, sizeof(float));
std::memcpy(&word, reply + offset, sizeof(float));
solutionSet.lisaQy = word;
offset += sizeof(float);
std::memcpy(&word, decodedFrame + offset, sizeof(float));
std::memcpy(&word, reply + offset, sizeof(float));
solutionSet.lisaQz = word;
offset += sizeof(float);
std::memcpy(&word, decodedFrame + offset, sizeof(float));
std::memcpy(&word, reply + offset, sizeof(float));
solutionSet.lisaPercentageClose = word;
offset += sizeof(float);
solutionSet.lisaNrClose = *(decodedFrame + offset);
solutionSet.lisaNrClose = *(reply + offset);
offset += sizeof(uint8_t);
solutionSet.isTrustWorthy = *(decodedFrame + offset);
solutionSet.isTrustWorthy = *(reply + offset);
offset += sizeof(uint8_t);
solutionSet.stableCount = *(decodedFrame + offset);
solutionSet.stableCount = *(reply + offset);
offset += sizeof(uint32_t);
solutionSet.stableCount = *(decodedFrame + offset);
solutionSet.stableCount = *(reply + offset);
result = solutionSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
if (result != RETURN_OK) {
return result;
@ -903,13 +928,14 @@ ReturnValue_t StarTrackerHandler::handleTemperatureTm() {
if (result != RETURN_OK) {
return result;
}
const uint8_t* reply = dataLinkLayer.getReply();
temperatureSet.ticks= ticks;
temperatureSet.time= time;
float temperature = 0;
std::memcpy(&temperature, decodedFrame + offset, sizeof(temperature));
std::memcpy(&temperature, reply + offset, sizeof(temperature));
temperatureSet.mcuTemperature = temperature;
offset += sizeof(float);
std::memcpy(&temperature, decodedFrame + offset, sizeof(temperature));
std::memcpy(&temperature, reply + offset, sizeof(temperature));
temperatureSet.cmosTemperature = temperature;
result = temperatureSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
if (result != RETURN_OK) {
@ -922,9 +948,10 @@ ReturnValue_t StarTrackerHandler::handleTemperatureTm() {
}
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);
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) {

52
mission/devices/StarTrackerHandler.h
View File

@ -7,6 +7,8 @@
#include <thirdparty/arcsec_star_tracker/common/SLIP.h>
#include <fsfw/datapool/PoolReadGuard.h>
#include <bsp_q7s/devices/StrImageLoader.h>
#include <bsp_q7s/devices/ArcsecDataLinkLayer.h>
#include <fsfw/events/HasEventsIF.h>
/**
* @brief This is the device handler for the star tracker from arcsec.
@ -35,6 +37,15 @@ public:
ReturnValue_t initialize() override;
/**
* @brief Overwrite this function from DHB to handle commands executed by the str image
* loader task.
*/
ReturnValue_t executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
const uint8_t* data, size_t size) override;
void performOperationHook() override;
protected:
void doStartUp() override;
void doShutDown() override;
@ -64,7 +75,9 @@ private:
//! [EXPORT] : [COMMENT] Received reply is too short
static const ReturnValue_t REPLY_TOO_SHORT = MAKE_RETURN_CODE(0xB0);
//! [EXPORT] : [COMMENT] Received reply with invalid CRC
static const ReturnValue_t CRC_FAILURE = MAKE_RETURN_CODE(0xB0);
static const ReturnValue_t CRC_FAILURE = MAKE_RETURN_CODE(0xB1);
//! [EXPORT] : [COMMENT] Image loader executing
static const ReturnValue_t IMAGE_LOADER_EXECUTING = MAKE_RETURN_CODE(0xB2);
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::STR_HANDLER;
@ -95,13 +108,13 @@ private:
static const size_t UPLOAD_COMMAND_LEN = 1028;
// Max valid position value in upload image command
static const uint16_t MAX_POSITION= 4095;
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;
static const uint8_t ACTION_DATA_OFFSET = 3;
static const uint8_t STATUS_OFFSET = 1;
static const uint8_t TICKS_OFFSET = 2;
static const uint8_t TIME_OFFSET = 6;
static const uint8_t TM_DATA_FIELD_OFFSET = 14;
static const uint8_t PARAMETER_ID_OFFSET = 0;
static const uint8_t ACTION_ID_OFFSET = 0;
static const uint8_t ACTION_DATA_OFFSET = 2;
// Ping request will reply ping with this ID (data field)
static const uint32_t PING_ID = 0x55;
@ -113,6 +126,10 @@ private:
// Pointer to object responsible for uploading and downloading images to/from the star tracker
StrImageLoader* strImageLoader = nullptr;
MessageQueueIF* eventQueue = nullptr;
ArcsecDatalinkLayer dataLinkLayer;
StarTracker::TemperatureSet temperatureSet;
StarTracker::VersionSet versionSet;
StarTracker::PowerSet powerSet;
@ -121,13 +138,6 @@ private:
StarTracker::SolutionSet solutionSet;
uint8_t commandBuffer[StarTracker::MAX_FRAME_SIZE];
uint8_t rxBuffer[StarTracker::MAX_FRAME_SIZE];
uint8_t decodedFrame[StarTracker::MAX_FRAME_SIZE];
/** Size of buffer derived from the egse source code */
uint8_t encBuffer[StarTracker::MAX_FRAME_SIZE * 2 + 2];
slip_decode_state slipInfo;
enum class InternalState {
TEMPERATURE_REQUEST
@ -135,7 +145,7 @@ private:
InternalState internalState = InternalState::TEMPERATURE_REQUEST;
uint32_t rememberUploadPosition = 0;
bool imageLoaderExecuting = false;
/**
* @brief This function initializes the serial link ip protocol struct slipInfo.
@ -172,11 +182,6 @@ private:
*/
void prepareInterfaceRequest();
/**
* @brief Fills the command buffer with data to upload part of an image.
*/
ReturnValue_t prepareImageUploadCommand(const uint8_t* commandData, size_t commandDataLen);
/**
* @brief Fills the command buffer with data to request the power telemetry packet.
*/
@ -217,11 +222,6 @@ private:
ReturnValue_t handlePingReply();
/**
* @brief Handles reply to upload image command
*/
ReturnValue_t handleUploadImageReply();
/**
* @brief Fills the time set with the data of the time request reply.
*/

1
mission/devices/devicedefinitions/StarTrackerDefinitions.h
View File

@ -91,6 +91,7 @@ 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 DOWNLOAD_IMAGE = 9;
static const DeviceCommandId_t UPLOAD_IMAGE = 10;
static const DeviceCommandId_t REQ_POWER = 11;
static const DeviceCommandId_t SUBSCRIBE_TO_TM = 18;