star tracker solution telemetry
This commit is contained in:
Jakob Meier
parent
eb0e9c2a41
commit
5ea129d252
@ -2,7 +2,6 @@
|
||||
#include "OBSWConfig.h"
|
||||
|
||||
#include <fsfw/globalfunctions/CRC.h>
|
||||
#include <fsfw/datapool/PoolReadGuard.h>
|
||||
|
||||
extern "C" {
|
||||
#include <thirdparty/arcsec_star_tracker/client/generated/telemetry.h>
|
||||
@ -12,7 +11,8 @@ extern "C" {
|
||||
|
||||
StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF,
|
||||
CookieIF * comCookie) :
|
||||
DeviceHandlerBase(objectId, comIF, comCookie), temperatureSet(this), versionSet(this) {
|
||||
DeviceHandlerBase(objectId, comIF, comCookie), temperatureSet(this), versionSet(this), powerSet(
|
||||
this), interfaceSet(this), timeSet(this), solutionSet(this) {
|
||||
if (comCookie == NULL) {
|
||||
sif::error << "StarTrackerHandler: Invalid com cookie" << std::endl;
|
||||
}
|
||||
@ -61,14 +61,38 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi
|
||||
preparePingRequest();
|
||||
return RETURN_OK;
|
||||
}
|
||||
case (StarTracker::REQ_TIME): {
|
||||
prepareTimeRequest();
|
||||
return RETURN_OK;
|
||||
}
|
||||
case (StarTracker::BOOT): {
|
||||
prepareBootCommand();
|
||||
return RETURN_OK;
|
||||
}
|
||||
case (StarTracker::REQ_VERSION): {
|
||||
prepareVersionRequest();
|
||||
return RETURN_OK;
|
||||
}
|
||||
case (StarTracker::REQ_INTERFACE): {
|
||||
prepareInterfaceRequest();
|
||||
return RETURN_OK;
|
||||
}
|
||||
case (StarTracker::REQ_POWER): {
|
||||
preparePowerRequest();
|
||||
return RETURN_OK;
|
||||
}
|
||||
case (StarTracker::REBOOT): {
|
||||
prepareRebootCommand();
|
||||
return RETURN_OK;
|
||||
}
|
||||
case (StarTracker::SUBSCRIBE_TO_TM): {
|
||||
prepareSubscriptionCommand(commandData);
|
||||
return RETURN_OK;
|
||||
}
|
||||
case (StarTracker::REQ_SOLUTION): {
|
||||
prepareSolutionRequest();
|
||||
return RETURN_OK;
|
||||
}
|
||||
case (StarTracker::REQ_TEMPERATURE): {
|
||||
prepareTemperatureRequest();
|
||||
return RETURN_OK;
|
||||
@ -82,26 +106,44 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi
|
||||
void StarTrackerHandler::fillCommandAndReplyMap() {
|
||||
/** Reply lengths are unknown because of the slip encoding. Thus always maximum reply size
|
||||
* is specified */
|
||||
this->insertInCommandAndReplyMap(StarTracker::PING_REQUEST, 1, nullptr,
|
||||
this->insertInCommandAndReplyMap(StarTracker::PING_REQUEST, 3, nullptr,
|
||||
StarTracker::MAX_FRAME_SIZE * 2 + 2);
|
||||
this->insertInCommandAndReplyMap(StarTracker::REQ_VERSION, 1, nullptr,
|
||||
this->insertInCommandAndReplyMap(StarTracker::BOOT, 3, nullptr,
|
||||
StarTracker::MAX_FRAME_SIZE * 2 + 2);
|
||||
this->insertInCommandAndReplyMap(StarTracker::REQ_VERSION, 3, &versionSet,
|
||||
StarTracker::MAX_FRAME_SIZE * 2 + 2);
|
||||
this->insertInCommandAndReplyMap(StarTracker::REQ_TIME, 3, &timeSet,
|
||||
StarTracker::MAX_FRAME_SIZE * 2 + 2);
|
||||
this->insertInCommandAndReplyMap(StarTracker::REQ_POWER, 3, &powerSet,
|
||||
StarTracker::MAX_FRAME_SIZE * 2 + 2);
|
||||
this->insertInCommandAndReplyMap(StarTracker::REQ_INTERFACE, 3, &interfaceSet,
|
||||
StarTracker::MAX_FRAME_SIZE * 2 + 2);
|
||||
// Reboot has no reply. Star tracker reboots immediately
|
||||
this->insertInCommandMap(StarTracker::REBOOT);
|
||||
this->insertInCommandAndReplyMap(StarTracker::REQ_TEMPERATURE, 1, &temperatureSet,
|
||||
this->insertInCommandAndReplyMap(StarTracker::SUBSCRIBE_TO_TM, 3, nullptr,
|
||||
StarTracker::MAX_FRAME_SIZE * 2 + 2);
|
||||
this->insertInCommandAndReplyMap(StarTracker::REQ_SOLUTION, 3, &solutionSet,
|
||||
StarTracker::MAX_FRAME_SIZE * 2 + 2);
|
||||
this->insertInCommandAndReplyMap(StarTracker::REQ_TEMPERATURE, 3, &temperatureSet,
|
||||
StarTracker::MAX_FRAME_SIZE * 2 + 2);
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::scanForReply(const uint8_t *start, size_t remainingSize,
|
||||
DeviceCommandId_t *foundId, size_t *foundLen) {
|
||||
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
uint32_t decodedLength = 0;
|
||||
|
||||
for (size_t idx = 0; idx < remainingSize; idx++) {
|
||||
enum arc_dec_result decResult = arc_transport_decode_body(*(start + idx), &slipInfo,
|
||||
size_t bytePos = 0;
|
||||
for (bytePos = 0; bytePos < remainingSize; bytePos++) {
|
||||
enum arc_dec_result decResult = arc_transport_decode_body(*(start + bytePos), &slipInfo,
|
||||
decodedFrame, &decodedLength);
|
||||
|
||||
switch (decResult) {
|
||||
case ARC_DEC_INPROGRESS: {
|
||||
if (bytePos == remainingSize - 1) {
|
||||
// second doSendread() required to read whole packet
|
||||
return IGNORE_FULL_PACKET;
|
||||
}
|
||||
continue;
|
||||
}
|
||||
case ARC_DEC_ASYNC: {
|
||||
@ -124,31 +166,30 @@ ReturnValue_t StarTrackerHandler::scanForReply(const uint8_t *start, size_t rema
|
||||
}
|
||||
}
|
||||
|
||||
switch (decodedFrame[1]) {
|
||||
case (static_cast<uint8_t>(StarTracker::PING_REQUEST)): {
|
||||
*foundLen = decodedLength;
|
||||
*foundId = StarTracker::PING_REQUEST;
|
||||
switch (decodedFrame[0]) {
|
||||
case TMTC_ACTIONREPLY: {
|
||||
*foundLen = bytePos;
|
||||
result = scanForActionReply(foundId);
|
||||
break;
|
||||
}
|
||||
case (static_cast<uint8_t>(StarTracker::REQ_VERSION)): {
|
||||
*foundLen = decodedLength;
|
||||
*foundId = StarTracker::PING_REQUEST;
|
||||
case TMTC_SETPARAMREPLY: {
|
||||
*foundLen = bytePos;
|
||||
result = scanForSetParameterReply(foundId);
|
||||
break;
|
||||
}
|
||||
case (static_cast<uint8_t>(StarTracker::REQ_TEMPERATURE)): {
|
||||
*foundLen = decodedLength;
|
||||
*foundId = StarTracker::REQ_TEMPERATURE;
|
||||
case TMTC_TELEMETRYREPLYA:
|
||||
case TMTC_TELEMETRYREPLY: {
|
||||
*foundLen = bytePos;
|
||||
result = scanForTmReply(foundId);
|
||||
break;
|
||||
}
|
||||
default: {
|
||||
sif::debug << "StarTrackerHandler::scanForReply: Reply contains invalid reply id"
|
||||
<< std::endl;
|
||||
return RETURN_FAILED;
|
||||
break;
|
||||
sif::debug << "StarTrackerHandler::scanForReply: Reply has invalid type id" << std::endl;
|
||||
result = RETURN_FAILED;
|
||||
}
|
||||
}
|
||||
|
||||
return RETURN_OK;
|
||||
return result;
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {
|
||||
@ -156,14 +197,38 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, con
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
|
||||
switch (id) {
|
||||
case (StarTracker::SUBSCRIBE_TO_TM): {
|
||||
result = handleSetParamReply();
|
||||
break;
|
||||
}
|
||||
case (StarTracker::REQ_TIME): {
|
||||
result = handleTimeTm();
|
||||
break;
|
||||
}
|
||||
case (StarTracker::PING_REQUEST): {
|
||||
result = handlePingReply();
|
||||
break;
|
||||
}
|
||||
case (StarTracker::BOOT): {
|
||||
result = handleActionReply();
|
||||
break;
|
||||
}
|
||||
case (StarTracker::REQ_VERSION): {
|
||||
result = handleVersionTm();
|
||||
break;
|
||||
}
|
||||
case (StarTracker::REQ_INTERFACE): {
|
||||
result = handleInterfaceTm();
|
||||
break;
|
||||
}
|
||||
case (StarTracker::REQ_POWER): {
|
||||
result = handlePowerTm();
|
||||
break;
|
||||
}
|
||||
case (StarTracker::REQ_SOLUTION): {
|
||||
handleSolutionTm();
|
||||
break;
|
||||
}
|
||||
case (StarTracker::REQ_TEMPERATURE): {
|
||||
handleTemperatureTm();
|
||||
break;
|
||||
@ -171,13 +236,14 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, con
|
||||
default: {
|
||||
sif::debug << "StarTrackerHandler::interpretDeviceReply: Unknown device reply id:" << id
|
||||
<< std::endl;
|
||||
return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
|
||||
result = DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
void StarTrackerHandler::setNormalDatapoolEntriesInvalid() {
|
||||
|
||||
}
|
||||
@ -189,16 +255,78 @@ uint32_t StarTrackerHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo
|
||||
ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
|
||||
LocalDataPoolManager& poolManager) {
|
||||
|
||||
localDataPoolMap.emplace(StarTracker::TICKS_TIME_SET, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::TIME_TIME_SET, new PoolEntry<uint64_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::RUN_TIME, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::UNIX_TIME, new PoolEntry<uint64_t>( { 0 }));
|
||||
|
||||
localDataPoolMap.emplace(StarTracker::TICKS_VERSION_SET, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::TIME_VERSION_SET, new PoolEntry<uint64_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::PROGRAM, new PoolEntry<uint8_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::MAJOR, new PoolEntry<uint8_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::MINOR, new PoolEntry<uint8_t>( { 0 }));
|
||||
|
||||
localDataPoolMap.emplace(StarTracker::TICKS_INTERFACE_SET, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::TIME_INTERFACE_SET, new PoolEntry<uint64_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::FRAME_COUNT, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CHECKSUM_ERROR_COUNT, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::SET_PARAM_COUNT, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::SET_PARAM_REPLY_COUNT, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::PARAM_REQUEST_COUNT, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::PARAM_REPLY_COUNT, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::REQ_TM_COUNT, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::TM_REPLY_COUNT, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::ACTION_REQ_COUNT, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::ACTION_REPLY_COUNT, new PoolEntry<uint32_t>( { 0 }));
|
||||
|
||||
localDataPoolMap.emplace(StarTracker::TICKS_POWER_SET, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::TIME_POWER_SET, new PoolEntry<uint64_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::MCU_CURRENT, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::MCU_VOLTAGE, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::FPGA_CORE_CURRENT, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::FPGA_CORE_VOLTAGE, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::FPGA_18_CURRENT, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::FPGA_18_VOLTAGE, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::FPGA_25_CURRENT, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::FPGA_25_VOLTAGE, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CMV_21_CURRENT, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CMV_21_VOLTAGE, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CMV_PIX_CURRENT, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CMV_PIX_VOLTAGE, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CMV_33_CURRENT, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CMV_33_VOLTAGE, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CMV_RES_CURRENT, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CMV_RES_VOLTAGE, new PoolEntry<float>( { 0 }));
|
||||
|
||||
localDataPoolMap.emplace(StarTracker::TICKS_TEMPERATURE_SET, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::TIME_TEMPERATURE_SET, new PoolEntry<uint64_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::MCU_TEMPERATURE, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CMOS_TEMPERATURE, new PoolEntry<float>( { 0 }));
|
||||
|
||||
localDataPoolMap.emplace(StarTracker::TICKS_SOLUTION_SET, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::TIME_SOLUTION_SET, new PoolEntry<uint64_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CALI_QW, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CALI_QX, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CALI_QY, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CALI_QZ, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::TRACK_CONFIDENCE, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::TRACK_QW, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::TRACK_QX, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::TRACK_QY, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::TRACK_QZ, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::TRACK_REMOVED, new PoolEntry<uint8_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::STARS_CENTROIDED, new PoolEntry<uint8_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::STARS_MATCHED_DATABASE, new PoolEntry<uint8_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::LISA_QW, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::LISA_QX, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::LISA_QY, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::LISA_QZ, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::LISA_PERC_CLOSE, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::LISA_NR_CLOSE, new PoolEntry<uint8_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::TRUST_WORTHY, new PoolEntry<uint8_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::STABLE_COUNT, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::SOLUTION_STRATEGY, new PoolEntry<uint8_t>( { 0 }));
|
||||
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
@ -206,6 +334,74 @@ size_t StarTrackerHandler::getNextReplyLength(DeviceCommandId_t commandId){
|
||||
return StarTracker::MAX_FRAME_SIZE;
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::scanForActionReply(DeviceCommandId_t *foundId) {
|
||||
switch (decodedFrame[1]) {
|
||||
case (StarTracker::ID::PING): {
|
||||
*foundId = StarTracker::PING_REQUEST;
|
||||
break;
|
||||
}
|
||||
case (StarTracker::ID::BOOT): {
|
||||
*foundId = StarTracker::BOOT;
|
||||
break;
|
||||
}
|
||||
default:
|
||||
sif::debug << "StarTrackerHandler::scanForSetParameterReply: Unknown parameter reply id"
|
||||
<< std::endl;
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::scanForSetParameterReply(DeviceCommandId_t *foundId) {
|
||||
switch (decodedFrame[1]) {
|
||||
case (StarTracker::ID::SUBSCRIBE): {
|
||||
*foundId = StarTracker::SUBSCRIBE_TO_TM;
|
||||
break;
|
||||
}
|
||||
default:
|
||||
sif::debug << "StarTrackerHandler::scanForSetParameterReply: Unknown parameter reply id"
|
||||
<< std::endl;
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::scanForTmReply(DeviceCommandId_t *foundId) {
|
||||
switch (decodedFrame[1]) {
|
||||
case (StarTracker::ID::VERSION): {
|
||||
*foundId = StarTracker::REQ_VERSION;
|
||||
break;
|
||||
}
|
||||
case (StarTracker::ID::INTERFACE): {
|
||||
*foundId = StarTracker::REQ_INTERFACE;
|
||||
break;
|
||||
}
|
||||
case (StarTracker::ID::POWER): {
|
||||
*foundId = StarTracker::REQ_POWER;
|
||||
break;
|
||||
}
|
||||
case (StarTracker::ID::TEMPERATURE): {
|
||||
*foundId = StarTracker::REQ_TEMPERATURE;
|
||||
break;
|
||||
}
|
||||
case (StarTracker::ID::TIME): {
|
||||
*foundId = StarTracker::REQ_TIME;
|
||||
break;
|
||||
}
|
||||
case (StarTracker::ID::SOLUTION): {
|
||||
*foundId = StarTracker::REQ_SOLUTION;
|
||||
break;
|
||||
}
|
||||
default: {
|
||||
sif::debug << "StarTrackerHandler::scanForReply: Reply contains invalid reply id"
|
||||
<< std::endl;
|
||||
return RETURN_FAILED;
|
||||
break;
|
||||
}
|
||||
}
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
void StarTrackerHandler::slipInit() {
|
||||
slipInfo.buffer = rxBuffer;
|
||||
slipInfo.maxlength = StarTracker::MAX_FRAME_SIZE;
|
||||
@ -214,6 +410,25 @@ void StarTrackerHandler::slipInit() {
|
||||
slipInfo.prev_state = SLIP_COMPLETE;
|
||||
}
|
||||
|
||||
void StarTrackerHandler::prepareBootCommand() {
|
||||
uint32_t length = 0;
|
||||
struct BootActionRequest bootRequest = {BOOT_REGION_ID};
|
||||
arc_pack_boot_action_req(&bootRequest, commandBuffer, &length);
|
||||
uint32_t encLength = 0;
|
||||
arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
|
||||
rawPacket = encBuffer;
|
||||
rawPacketLen = encLength;
|
||||
}
|
||||
|
||||
void StarTrackerHandler::prepareTimeRequest() {
|
||||
uint32_t length = 0;
|
||||
arc_tm_pack_time_req(commandBuffer, &length);
|
||||
uint32_t encLength = 0;
|
||||
arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
|
||||
rawPacket = encBuffer;
|
||||
rawPacketLen = encLength;
|
||||
}
|
||||
|
||||
void StarTrackerHandler::preparePingRequest() {
|
||||
uint32_t length = 0;
|
||||
struct PingActionRequest pingRequest = {PING_ID};
|
||||
@ -233,6 +448,24 @@ void StarTrackerHandler::prepareVersionRequest() {
|
||||
rawPacketLen = encLength;
|
||||
}
|
||||
|
||||
void StarTrackerHandler::prepareInterfaceRequest() {
|
||||
uint32_t length = 0;
|
||||
arc_tm_pack_interface_req(commandBuffer, &length);
|
||||
uint32_t encLength = 0;
|
||||
arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
|
||||
rawPacket = encBuffer;
|
||||
rawPacketLen = encLength;
|
||||
}
|
||||
|
||||
void StarTrackerHandler::preparePowerRequest() {
|
||||
uint32_t length = 0;
|
||||
arc_tm_pack_power_req(commandBuffer, &length);
|
||||
uint32_t encLength = 0;
|
||||
arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
|
||||
rawPacket = encBuffer;
|
||||
rawPacketLen = encLength;
|
||||
}
|
||||
|
||||
void StarTrackerHandler::prepareRebootCommand() {
|
||||
uint32_t length = 0;
|
||||
struct RebootActionRequest rebootReq;
|
||||
@ -243,6 +476,42 @@ void StarTrackerHandler::prepareRebootCommand() {
|
||||
rawPacketLen = encLength;
|
||||
}
|
||||
|
||||
void StarTrackerHandler::prepareSubscriptionCommand(const uint8_t* tmId) {
|
||||
uint32_t encLength = 0;
|
||||
uint32_t length = 18;
|
||||
commandBuffer[0] = TMTC_SETPARAMREQ;
|
||||
commandBuffer[1] = StarTracker::ID::SUBSCRIBE;
|
||||
// Fill all other fields with invalid tm id
|
||||
commandBuffer[2] = *tmId;
|
||||
commandBuffer[3] = 0;
|
||||
commandBuffer[4] = 0;
|
||||
commandBuffer[5] = 0;
|
||||
commandBuffer[6] = 0;
|
||||
commandBuffer[7] = 0;
|
||||
commandBuffer[8] = 0;
|
||||
commandBuffer[9] = 0;
|
||||
commandBuffer[10] = 0;
|
||||
commandBuffer[11] = 0;
|
||||
commandBuffer[12] = 0;
|
||||
commandBuffer[13] = 0;
|
||||
commandBuffer[14] = 0;
|
||||
commandBuffer[15] = 0;
|
||||
commandBuffer[16] = 0;
|
||||
commandBuffer[17] = 0;
|
||||
arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
|
||||
rawPacket = encBuffer;
|
||||
rawPacketLen = encLength;
|
||||
}
|
||||
|
||||
void StarTrackerHandler::prepareSolutionRequest() {
|
||||
uint32_t length = 0;
|
||||
arc_tm_pack_solution_req(commandBuffer, &length);
|
||||
uint32_t encLength = 0;
|
||||
arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
|
||||
rawPacket = encBuffer;
|
||||
rawPacketLen = encLength;
|
||||
}
|
||||
|
||||
void StarTrackerHandler::prepareTemperatureRequest() {
|
||||
uint32_t length = 0;
|
||||
arc_tm_pack_temperature_req(commandBuffer, &length);
|
||||
@ -252,6 +521,29 @@ void StarTrackerHandler::prepareTemperatureRequest() {
|
||||
rawPacketLen = encLength;
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::handleSetParamReply() {
|
||||
uint8_t status = *(decodedFrame + STATUS_OFFSET);
|
||||
if (status != StarTracker::STATUS_OK) {
|
||||
sif::warning << "StarTrackerHandler::handleSetParamReply: Failed to execute parameter set "
|
||||
" command with parameter ID" <<
|
||||
static_cast<unsigned int>(*(decodedFrame + PARAMETER_ID_OFFSET)) << std::endl;
|
||||
return SET_PARAM_FAILED;
|
||||
}
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::handleActionReply() {
|
||||
uint8_t status = *(decodedFrame + STATUS_OFFSET);
|
||||
if (status != StarTracker::STATUS_OK) {
|
||||
sif::warning << "StarTrackerHandler::handleActionReply: Failed to execute action "
|
||||
<< " command with action ID "
|
||||
<< static_cast<unsigned int>(*(decodedFrame + ACTION_ID_OFFSET))
|
||||
<< " and status "<< static_cast<unsigned int>(status) << std::endl;
|
||||
return ACTION_FAILED;
|
||||
}
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::handlePingReply() {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
uint32_t pingId = 0;
|
||||
@ -269,70 +561,380 @@ ReturnValue_t StarTrackerHandler::handlePingReply() {
|
||||
return result;
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::handleTimeTm() {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
PoolReadGuard rg(&timeSet);
|
||||
uint32_t offset = TM_DATA_FIELD_OFFSET;
|
||||
uint8_t status = 0;
|
||||
uint32_t ticks = 0;
|
||||
uint64_t time = 0;
|
||||
getTmHeaderData(&status, &ticks, &time);
|
||||
if(status != StarTracker::STATUS_OK) {
|
||||
sif::warning << "StarTrackerHandler::handleVersionTm: Reply error: "
|
||||
<< static_cast<unsigned int>(status) << std::endl;
|
||||
result = VERSION_REQ_FAILED;
|
||||
return result;
|
||||
}
|
||||
timeSet.ticks = ticks;
|
||||
timeSet.time = time;
|
||||
timeSet.runTime = deserializeUint32(decodedFrame + offset);
|
||||
offset += sizeof(uint32_t);
|
||||
timeSet.unixTime = deserializeUint64(decodedFrame + offset);
|
||||
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
|
||||
sif::info << "StarTrackerHandler::handleTimeTm: Ticks: "
|
||||
<< timeSet.ticks << std::endl;
|
||||
sif::info << "StarTrackerHandler::handleTimeTm: Time (time stamp): "
|
||||
<< timeSet.time << " us" << std::endl;
|
||||
sif::info << "StarTrackerHandler::handleTimeTm: Run Time: "
|
||||
<< timeSet.runTime << " ms" << std::endl;
|
||||
sif::info << "StarTrackerHandler::handleTimeTm: Unix Time: "
|
||||
<< timeSet.unixTime << " s" << std::endl;
|
||||
#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
|
||||
return result;
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::handleVersionTm() {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
PoolReadGuard rg(&versionSet);
|
||||
uint32_t offset = 2;
|
||||
uint8_t status = *(decodedFrame + offset);
|
||||
uint32_t offset = TM_DATA_FIELD_OFFSET;
|
||||
uint8_t status = 0;
|
||||
uint32_t ticks = 0;
|
||||
uint64_t time = 0;
|
||||
getTmHeaderData(&status, &ticks, &time);
|
||||
if(status != StarTracker::STATUS_OK) {
|
||||
sif::warning << "StarTrackerHandler::handleVersionTm: Reply error: "
|
||||
<< static_cast<unsigned int>(status) << std::endl;
|
||||
result = VERSION_REQ_FAILED;
|
||||
return result;
|
||||
}
|
||||
versionSet.ticks = ticks;
|
||||
versionSet.time = time;
|
||||
versionSet.program = *(decodedFrame + offset);
|
||||
offset += 1;
|
||||
versionSet.major = *(decodedFrame + offset);
|
||||
offset += 1;
|
||||
versionSet.minor = *(decodedFrame + offset);
|
||||
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
|
||||
sif::info << "StarTrackerHandler::handleVersionTm: Ticks: "
|
||||
<< versionSet.ticks << std::endl;
|
||||
sif::info << "StarTrackerHandler::handleVersionTm: Unix Time: "
|
||||
<< versionSet.time << " us" << std::endl;
|
||||
sif::info << "StarTrackerHandler::handleVersionTm: Program: "
|
||||
<< static_cast<unsigned int>(versionSet.program.value) << std::endl;
|
||||
sif::info << "StarTrackerHandler::handleVersionTm: Major: "
|
||||
<< static_cast<unsigned int>(versionSet.major.value) << std::endl;
|
||||
sif::info << "StarTrackerHandler::handleVersionTm: Minor: "
|
||||
<< static_cast<unsigned int>(versionSet.minor.value) << std::endl;
|
||||
#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
|
||||
return result;
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::handleInterfaceTm() {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
PoolReadGuard rg(&interfaceSet);
|
||||
uint32_t offset = TM_DATA_FIELD_OFFSET;
|
||||
uint8_t status = 0;
|
||||
uint32_t ticks = 0;
|
||||
uint64_t time = 0;
|
||||
getTmHeaderData(&status, &ticks, &time);
|
||||
if(status != StarTracker::STATUS_OK) {
|
||||
sif::warning << "StarTrackerHandler::handleInterfaceTm: Reply error: "
|
||||
<< static_cast<unsigned int>(status) << std::endl;
|
||||
result = INTERFACE_REQ_FAILED;
|
||||
return result;
|
||||
}
|
||||
interfaceSet.ticks = ticks;
|
||||
interfaceSet.time = time;
|
||||
size_t size = sizeof(uint32_t);
|
||||
interfaceSet.frameCount = deserializeUint32(decodedFrame + offset);
|
||||
offset += size;
|
||||
interfaceSet.checksumerrorCount = deserializeUint32(decodedFrame + offset);
|
||||
offset += size;
|
||||
interfaceSet.setParamCount = deserializeUint32(decodedFrame + offset);
|
||||
offset += size;
|
||||
interfaceSet.setParamReplyCount = deserializeUint32(decodedFrame + offset);
|
||||
offset += size;
|
||||
interfaceSet.paramRequestCount = deserializeUint32(decodedFrame + offset);
|
||||
offset += size;
|
||||
interfaceSet.paramReplyCount = deserializeUint32(decodedFrame + offset);
|
||||
offset += size;
|
||||
interfaceSet.tmReplyCount = deserializeUint32(decodedFrame + offset);
|
||||
offset += size;
|
||||
interfaceSet.actionReqCount = deserializeUint32(decodedFrame + offset);
|
||||
offset += size;
|
||||
interfaceSet.actionReplyCount = deserializeUint32(decodedFrame + offset);
|
||||
|
||||
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
|
||||
sif::info << "StarTrackerHandler::handleInterfaceTm: Ticks: "
|
||||
<< interfaceSet.ticks << std::endl;
|
||||
sif::info << "StarTrackerHandler::handleInterfaceTm: Time: "
|
||||
<< interfaceSet.time << " us" << std::endl;
|
||||
sif::info << "StarTrackerHandler::handleInterfaceTm: Frame Count: "
|
||||
<< interfaceSet.frameCount << std::endl;
|
||||
sif::info << "StarTrackerHandler::handleInterfaceTm: Checksum Error Count: "
|
||||
<< interfaceSet.checksumerrorCount << std::endl;
|
||||
sif::info << "StarTrackerHandler::handleInterfaceTm: Set Param Count: "
|
||||
<< interfaceSet.setParamCount << std::endl;
|
||||
sif::info << "StarTrackerHandler::handleInterfaceTm: Set Param Reply Count: "
|
||||
<< interfaceSet.setParamReplyCount << std::endl;
|
||||
sif::info << "StarTrackerHandler::handleInterfaceTm: Param Request Count: "
|
||||
<< interfaceSet.paramRequestCount << std::endl;
|
||||
sif::info << "StarTrackerHandler::handleInterfaceTm: Param Reply Count: "
|
||||
<< interfaceSet.paramReplyCount << std::endl;
|
||||
sif::info << "StarTrackerHandler::handleInterfaceTm: Req TM Count: "
|
||||
<< interfaceSet.reqTmCount << std::endl;
|
||||
sif::info << "StarTrackerHandler::handleInterfaceTm: Telemetry Reply Count: "
|
||||
<< interfaceSet.tmReplyCount << std::endl;
|
||||
sif::info << "StarTrackerHandler::handleInterfaceTm: Action Request Count: "
|
||||
<< interfaceSet.actionReqCount << std::endl;
|
||||
sif::info << "StarTrackerHandler::handleInterfaceTm: Action Reply Count: "
|
||||
<< interfaceSet.actionReplyCount << std::endl;
|
||||
#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
|
||||
return result;
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::handlePowerTm() {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
PoolReadGuard rg(&powerSet);
|
||||
uint32_t offset = TM_DATA_FIELD_OFFSET;
|
||||
uint8_t status = 0;
|
||||
uint32_t ticks = 0;
|
||||
uint64_t time = 0;
|
||||
getTmHeaderData(&status, &ticks, &time);
|
||||
if(status != StarTracker::STATUS_OK) {
|
||||
sif::warning << "StarTrackerHandler::handlePowerTm: Reply error: "
|
||||
<< static_cast<unsigned int>(status) << std::endl;
|
||||
result = POWER_REQ_FAILED;
|
||||
return result;
|
||||
}
|
||||
powerSet.ticks= ticks;
|
||||
powerSet.time= time;
|
||||
float value = 0;
|
||||
std::memcpy(&value, decodedFrame + offset, sizeof(value));
|
||||
powerSet.mcuCurrent = value;
|
||||
offset += 4;
|
||||
std::memcpy(&value, decodedFrame + offset, sizeof(value));
|
||||
powerSet.mcuVoltage = value;
|
||||
offset += 4;
|
||||
std::memcpy(&value, decodedFrame + offset, sizeof(value));
|
||||
powerSet.fpgaCoreCurrent = value;
|
||||
offset += 4;
|
||||
std::memcpy(&value, decodedFrame + offset, sizeof(value));
|
||||
powerSet.fpgaCoreVoltage = value;
|
||||
offset += 4;
|
||||
std::memcpy(&value, decodedFrame + offset, sizeof(value));
|
||||
powerSet.fpga18Current = value;
|
||||
offset += 4;
|
||||
std::memcpy(&value, decodedFrame + offset, sizeof(value));
|
||||
powerSet.fpga18Voltage = value;
|
||||
offset += 4;
|
||||
std::memcpy(&value, decodedFrame + offset, sizeof(value));
|
||||
powerSet.fpga25Current = value;
|
||||
offset += 4;
|
||||
std::memcpy(&value, decodedFrame + offset, sizeof(value));
|
||||
powerSet.fpga25Voltage = value;
|
||||
offset += 4;
|
||||
std::memcpy(&value, decodedFrame + offset, sizeof(value));
|
||||
powerSet.cmv21Current = value;
|
||||
offset += 4;
|
||||
std::memcpy(&value, decodedFrame + offset, sizeof(value));
|
||||
powerSet.cmv21Voltage = value;
|
||||
offset += 4;
|
||||
std::memcpy(&value, decodedFrame + offset, sizeof(value));
|
||||
powerSet.cmvPixCurrent= value;
|
||||
offset += 4;
|
||||
std::memcpy(&value, decodedFrame + offset, sizeof(value));
|
||||
powerSet.cmvPixVoltage = value;
|
||||
offset += 4;
|
||||
std::memcpy(&value, decodedFrame + offset, sizeof(value));
|
||||
powerSet.cmv33Current= value;
|
||||
offset += 4;
|
||||
std::memcpy(&value, decodedFrame + offset, sizeof(value));
|
||||
powerSet.cmv33Voltage = value;
|
||||
offset += 4;
|
||||
std::memcpy(&value, decodedFrame + offset, sizeof(value));
|
||||
powerSet.cmvResCurrent= value;
|
||||
offset += 4;
|
||||
std::memcpy(&value, decodedFrame + offset, sizeof(value));
|
||||
powerSet.cmvResVoltage = value;
|
||||
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
|
||||
sif::info << "StarTrackerHandler::handlePowerTm: Ticks: "
|
||||
<< powerSet.ticks << std::endl;
|
||||
sif::info << "StarTrackerHandler::handlePowerTm: Time: "
|
||||
<< powerSet.time << " us" << std::endl;
|
||||
sif::info << "StarTrackerHandler::handlePowerTm: MCU Current: "
|
||||
<< powerSet.mcuCurrent << " A" << std::endl;
|
||||
sif::info << "StarTrackerHandler::handlePowerTm: MCU Voltage: "
|
||||
<< powerSet.mcuVoltage << " V" << std::endl;
|
||||
sif::info << "StarTrackerHandler::handlePowerTm: FPGA Core current: "
|
||||
<< powerSet.fpgaCoreCurrent << " A" << std::endl;
|
||||
sif::info << "StarTrackerHandler::handlePowerTm: FPGA Core voltage: "
|
||||
<< powerSet.fpgaCoreVoltage << " V" << std::endl;
|
||||
sif::info << "StarTrackerHandler::handlePowerTm: FPGA 18 current: "
|
||||
<< powerSet.fpga18Current << " A" << std::endl;
|
||||
sif::info << "StarTrackerHandler::handlePowerTm: FPGA 18 voltage: "
|
||||
<< powerSet.fpga18Voltage << " V" << std::endl;
|
||||
sif::info << "StarTrackerHandler::handlePowerTm: FPGA 25 current: "
|
||||
<< powerSet.fpga25Current << " A" << std::endl;
|
||||
sif::info << "StarTrackerHandler::handlePowerTm: FPGA 25 voltage: "
|
||||
<< powerSet.fpga25Voltage << " V" << std::endl;
|
||||
sif::info << "StarTrackerHandler::handlePowerTm: CMV 21 current: "
|
||||
<< powerSet.cmv21Current << " A" << std::endl;
|
||||
sif::info << "StarTrackerHandler::handlePowerTm: CMV 21 voltage: "
|
||||
<< powerSet.cmv21Voltage << " V" << std::endl;
|
||||
sif::info << "StarTrackerHandler::handlePowerTm: CMV Pix current: "
|
||||
<< powerSet.cmvPixCurrent << " A" << std::endl;
|
||||
sif::info << "StarTrackerHandler::handlePowerTm: CMV Pix voltage: "
|
||||
<< powerSet.cmvPixVoltage << " V" << std::endl;
|
||||
sif::info << "StarTrackerHandler::handlePowerTm: CMV 33 current: "
|
||||
<< powerSet.cmv33Current << " A" << std::endl;
|
||||
sif::info << "StarTrackerHandler::handlePowerTm: CMV 33 voltage: "
|
||||
<< powerSet.cmv33Voltage << " V" << std::endl;
|
||||
sif::info << "StarTrackerHandler::handlePowerTm: CMV Res current: "
|
||||
<< powerSet.cmvResCurrent << " A" << std::endl;
|
||||
sif::info << "StarTrackerHandler::handlePowerTm: CMV Res voltage: "
|
||||
<< powerSet.cmvResVoltage << " V" << std::endl;
|
||||
#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
|
||||
return result;
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::handleSolutionTm() {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
result = solutionSet.read(MutexIF::TimeoutType::WAITING, 20);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
uint32_t offset = TM_DATA_FIELD_OFFSET;
|
||||
uint8_t status = 0;
|
||||
uint32_t ticks = 0;
|
||||
uint64_t time = 0;
|
||||
getTmHeaderData(&status, &ticks, &time);
|
||||
if(status != StarTracker::STATUS_OK) {
|
||||
sif::warning << "StarTrackerHandler::handleTemperatureTm: Reply error: "
|
||||
<< static_cast<unsigned int>(status) << std::endl;
|
||||
result = TEMPERATURE_REQUEST_FAILED;
|
||||
result = TEMPERATURE_REQ_FAILED;
|
||||
return result;
|
||||
}
|
||||
solutionSet.ticks= ticks;
|
||||
solutionSet.time= time;
|
||||
float word = 0;
|
||||
std::memcpy(&word, decodedFrame + offset, sizeof(float));
|
||||
solutionSet.caliQw = word;
|
||||
offset += sizeof(float);
|
||||
std::memcpy(&word, decodedFrame + offset, sizeof(float));
|
||||
solutionSet.caliQx = word;
|
||||
offset += sizeof(float);
|
||||
std::memcpy(&word, decodedFrame + offset, sizeof(float));
|
||||
solutionSet.caliQy = word;
|
||||
offset += sizeof(float);
|
||||
std::memcpy(&word, decodedFrame + offset, sizeof(float));
|
||||
solutionSet.caliQz = word;
|
||||
offset += sizeof(float);
|
||||
std::memcpy(&word, decodedFrame + offset, sizeof(float));
|
||||
solutionSet.trackConfidence = word;
|
||||
offset += sizeof(float);
|
||||
std::memcpy(&word, decodedFrame + offset, sizeof(float));
|
||||
solutionSet.trackQw = word;
|
||||
offset += sizeof(float);
|
||||
std::memcpy(&word, decodedFrame + offset, sizeof(float));
|
||||
solutionSet.trackQx = word;
|
||||
offset += sizeof(float);
|
||||
std::memcpy(&word, decodedFrame + offset, sizeof(float));
|
||||
solutionSet.trackQy = word;
|
||||
offset += sizeof(float);
|
||||
std::memcpy(&word, decodedFrame + offset, sizeof(float));
|
||||
solutionSet.trackQz = word;
|
||||
offset += sizeof(float);
|
||||
solutionSet.trackRemoved = *(decodedFrame + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
solutionSet.starsCentroided = *(decodedFrame + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
solutionSet.starsMatchedDatabase = *(decodedFrame + offset);
|
||||
offset += sizeof(float);
|
||||
std::memcpy(&word, decodedFrame + offset, sizeof(float));
|
||||
solutionSet.lisaQw = word;
|
||||
offset += sizeof(float);
|
||||
std::memcpy(&word, decodedFrame + offset, sizeof(float));
|
||||
solutionSet.lisaQx = word;
|
||||
offset += sizeof(float);
|
||||
std::memcpy(&word, decodedFrame + offset, sizeof(float));
|
||||
solutionSet.lisaQy = word;
|
||||
offset += sizeof(float);
|
||||
std::memcpy(&word, decodedFrame + offset, sizeof(float));
|
||||
solutionSet.lisaQz = word;
|
||||
offset += sizeof(float);
|
||||
std::memcpy(&word, decodedFrame + offset, sizeof(float));
|
||||
solutionSet.lisaPercentageClose = word;
|
||||
offset += sizeof(float);
|
||||
solutionSet.lisaNrClose = *(decodedFrame + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
solutionSet.isTrustWorthy = *(decodedFrame + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
solutionSet.stableCount = *(decodedFrame + offset);
|
||||
offset += sizeof(uint32_t);
|
||||
solutionSet.stableCount = *(decodedFrame + offset);
|
||||
result = solutionSet.commit(MutexIF::TimeoutType::WAITING, 20);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
versionSet.program = (*decodedFrame + offset);
|
||||
offset += 1;
|
||||
versionSet.major = (*decodedFrame + offset);
|
||||
offset += 1;
|
||||
versionSet.minor = (*decodedFrame + offset);
|
||||
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
|
||||
sif::info << "StarTrackerHandler::handleVersionTm: Program: "
|
||||
<< versionSet.program << std::endl;
|
||||
sif::info << "StarTrackerHandler::handleVersionTm: Major: "
|
||||
<< versionSet.major << std::endl;
|
||||
sif::info << "StarTrackerHandler::handleVersionTm: Minor: "
|
||||
<< versionSet.minor << std::endl;
|
||||
#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
|
||||
solutionSet.printSet();
|
||||
#endif
|
||||
return result;
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::handleTemperatureTm() {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
PoolReadGuard rg(&temperatureSet);
|
||||
uint32_t offset = 2;
|
||||
uint8_t status = *(decodedFrame + offset);
|
||||
offset += 1;
|
||||
uint32_t offset = TM_DATA_FIELD_OFFSET;
|
||||
uint8_t status = 0;
|
||||
uint32_t ticks = 0;
|
||||
uint64_t time = 0;
|
||||
getTmHeaderData(&status, &ticks, &time);
|
||||
if(status != StarTracker::STATUS_OK) {
|
||||
sif::warning << "StarTrackerHandler::handleTemperatureTm: Reply error: "
|
||||
<< static_cast<unsigned int>(status) << std::endl;
|
||||
result = TEMPERATURE_REQUEST_FAILED;
|
||||
result = TEMPERATURE_REQ_FAILED;
|
||||
return result;
|
||||
}
|
||||
const uint8_t* buffer = decodedFrame + offset;
|
||||
size_t size = sizeof(temperatureSet.ticks);
|
||||
SerializeAdapter::deSerialize(&temperatureSet.ticks, &buffer, &size, SerializeIF::Endianness::LITTLE);
|
||||
offset += size;
|
||||
buffer = decodedFrame + offset;
|
||||
size = sizeof(temperatureSet.time);
|
||||
SerializeAdapter::deSerialize(&temperatureSet.time, &buffer, &size, SerializeIF::Endianness::LITTLE);
|
||||
offset += size;
|
||||
temperatureSet.mcuTemperature = *(decodedFrame + offset) << 24
|
||||
| *(decodedFrame + offset + 1) << 16 | *(decodedFrame + offset + 2) << 8
|
||||
| *(decodedFrame + offset + 3);
|
||||
offset += sizeof(temperatureSet.mcuTemperature);
|
||||
temperatureSet.mcuTemperature = *(decodedFrame + offset) << 24
|
||||
| *(decodedFrame + offset + 1) << 16 | *(decodedFrame + offset + 2) << 8
|
||||
| *(decodedFrame + offset + 3);
|
||||
temperatureSet.ticks= ticks;
|
||||
temperatureSet.time= time;
|
||||
float temperature = 0;
|
||||
std::memcpy(&temperature, decodedFrame + offset, sizeof(temperature));
|
||||
temperatureSet.mcuTemperature = temperature;
|
||||
offset += sizeof(temperature);
|
||||
std::memcpy(&temperature, decodedFrame + offset, sizeof(temperature));
|
||||
temperatureSet.cmosTemperature = temperature;
|
||||
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
|
||||
sif::info << "StarTrackerHandler::handleTemperatureTm: Ticks: "
|
||||
<< temperatureSet.ticks << std::endl;
|
||||
sif::info << "StarTrackerHandler::handleTemperatureTm: Time: "
|
||||
<< temperatureSet.time << std::endl;
|
||||
sif::info << "StarTrackerHandler::handleTemperatureTm: MCU Temperature: "
|
||||
<< temperatureSet.mcuTemperature << " °C" << std::endl;
|
||||
sif::info << "StarTrackerHandler::handleTemperatureTm: CMOS Temperature: "
|
||||
<< temperatureSet.mcuTemperature << " °C" << std::endl;
|
||||
solutionSet.printSet();
|
||||
#endif
|
||||
return result;
|
||||
}
|
||||
|
||||
void StarTrackerHandler::getTmHeaderData(uint8_t* status, uint32_t* ticks, uint64_t* time) {
|
||||
*status = *(decodedFrame + STATUS_OFFSET);
|
||||
*ticks = deserializeUint32(decodedFrame + TICKS_OFFSET);
|
||||
*time = deserializeUint64(decodedFrame + TIME_OFFSET);
|
||||
}
|
||||
|
||||
uint32_t StarTrackerHandler::deserializeUint32(uint8_t* buffer) {
|
||||
uint32_t word = 0;
|
||||
word = *(buffer + 3) << 24
|
||||
| *(buffer + 2) << 16
|
||||
| *(buffer + 1) << 8
|
||||
| *(buffer);
|
||||
return word;
|
||||
}
|
||||
|
||||
uint64_t StarTrackerHandler::deserializeUint64(uint8_t* buffer) {
|
||||
uint64_t word = 0;
|
||||
word = static_cast<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;
|
||||
}
|
||||
|
||||
@ -5,6 +5,7 @@
|
||||
#include <fsfw/src/fsfw/serialize/SerializeAdapter.h>
|
||||
#include <mission/devices/devicedefinitions/StarTrackerDefinitions.h>
|
||||
#include <thirdparty/arcsec_star_tracker/common/SLIP.h>
|
||||
#include <fsfw/datapool/PoolReadGuard.h>
|
||||
|
||||
/**
|
||||
* @brief This is the device handler for the star tracker from arcsec.
|
||||
@ -63,16 +64,38 @@ private:
|
||||
|
||||
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::STR_HANDLER;
|
||||
|
||||
//! [EXPORT] : [COMMENT] Status in tm reply not ok
|
||||
static const ReturnValue_t TEMPERATURE_REQUEST_FAILED = MAKE_RETURN_CODE(0xA0);
|
||||
//! [EXPORT] : [COMMENT] Status in temperature reply signals error
|
||||
static const ReturnValue_t TEMPERATURE_REQ_FAILED = MAKE_RETURN_CODE(0xA0);
|
||||
//! [EXPORT] : [COMMENT] Ping command failed
|
||||
static const ReturnValue_t PING_FAILED = MAKE_RETURN_CODE(0xA1);
|
||||
//! [EXPORT] : [COMMENT] Status in version reply signals error
|
||||
static const ReturnValue_t VERSION_REQ_FAILED = MAKE_RETURN_CODE(0xA3);
|
||||
//! [EXPORT] : [COMMENT] Status in interface reply signals error
|
||||
static const ReturnValue_t INTERFACE_REQ_FAILED = MAKE_RETURN_CODE(0xA4);
|
||||
//! [EXPORT] : [COMMENT] Status in power reply signals error
|
||||
static const ReturnValue_t POWER_REQ_FAILED = MAKE_RETURN_CODE(0xA5);
|
||||
//! [EXPORT] : [COMMENT] Status of reply to parameter set command signals error
|
||||
static const ReturnValue_t SET_PARAM_FAILED = MAKE_RETURN_CODE(0xA6);
|
||||
//! [EXPORT] : [COMMENT] Status of reply to action command signals error
|
||||
static const ReturnValue_t ACTION_FAILED = MAKE_RETURN_CODE(0xA7);
|
||||
|
||||
static const uint8_t STATUS_OFFSET = 2;
|
||||
static const uint8_t TICKS_OFFSET = 3;
|
||||
static const uint8_t TIME_OFFSET = 7;
|
||||
static const uint8_t TM_DATA_FIELD_OFFSET = 15;
|
||||
static const uint8_t PARAMETER_ID_OFFSET = 1;
|
||||
static const uint8_t ACTION_ID_OFFSET = 1;
|
||||
|
||||
// Ping request will reply ping with this ID (data field)
|
||||
static const uint32_t PING_ID = 0x55;
|
||||
static const uint32_t BOOT_REGION_ID = 1;
|
||||
|
||||
StarTracker::TemperatureSet temperatureSet;
|
||||
StarTracker::VersionSet versionSet;
|
||||
StarTracker::PowerSet powerSet;
|
||||
StarTracker::InterfaceSet interfaceSet;
|
||||
StarTracker::TimeSet timeSet;
|
||||
StarTracker::SolutionSet solutionSet;
|
||||
|
||||
uint8_t commandBuffer[StarTracker::MAX_FRAME_SIZE];
|
||||
uint8_t rxBuffer[StarTracker::MAX_FRAME_SIZE];
|
||||
@ -94,27 +117,86 @@ private:
|
||||
*/
|
||||
void slipInit();
|
||||
|
||||
ReturnValue_t scanForActionReply(DeviceCommandId_t *foundId);
|
||||
ReturnValue_t scanForSetParameterReply(DeviceCommandId_t *foundId);
|
||||
ReturnValue_t scanForTmReply(DeviceCommandId_t *foundId);
|
||||
|
||||
/**
|
||||
* @brief Fills command buffer with data to ping the star tracker
|
||||
*/
|
||||
void preparePingRequest();
|
||||
|
||||
/**
|
||||
* @brief Fills command buffer with data to request the time telemetry.
|
||||
*/
|
||||
void prepareTimeRequest();
|
||||
|
||||
/**
|
||||
* @brief Fills command buffer with data to boot image (works only when star tracker is
|
||||
* in bootloader mode).
|
||||
*/
|
||||
void prepareBootCommand();
|
||||
|
||||
/**
|
||||
* @brief Fills command buffer with data to request the version telemetry packet
|
||||
*/
|
||||
void prepareVersionRequest();
|
||||
|
||||
/**
|
||||
* @brief Fills the command buffer with data to request the interface telemetry packet.
|
||||
*/
|
||||
void prepareInterfaceRequest();
|
||||
|
||||
/**
|
||||
* @brief Fills the command buffer with data to request the power telemetry packet.
|
||||
*/
|
||||
void preparePowerRequest();
|
||||
|
||||
/**
|
||||
* @brief Fills command buffer with data to reboot star tracker.
|
||||
*/
|
||||
void prepareRebootCommand();
|
||||
|
||||
/**
|
||||
* @brief Fills command buffer with data to subscribe to a telemetry packet.
|
||||
*
|
||||
* @param tmId The ID of the telemetry packet to subscribe to
|
||||
*/
|
||||
void prepareSubscriptionCommand(const uint8_t* tmId);
|
||||
|
||||
/**
|
||||
* @brief Fills command buffer with data to request solution telemtry packet (contains
|
||||
* attitude information)
|
||||
*/
|
||||
void prepareSolutionRequest();
|
||||
|
||||
/**
|
||||
* @brief Fills command buffer with data to request temperature from star tracker
|
||||
*/
|
||||
void prepareTemperatureRequest();
|
||||
|
||||
/**
|
||||
* @brief Default function to handle action replies
|
||||
*/
|
||||
ReturnValue_t handleActionReply();
|
||||
|
||||
/**
|
||||
* @brief Handles all set parameter replies
|
||||
*/
|
||||
ReturnValue_t handleSetParamReply();
|
||||
|
||||
ReturnValue_t handlePingReply();
|
||||
|
||||
/**
|
||||
* @brief Fills the time set with the data of the time request reply.
|
||||
*/
|
||||
ReturnValue_t handleTimeTm();
|
||||
|
||||
/**
|
||||
* @brief Handles reply data of solution request.
|
||||
*/
|
||||
ReturnValue_t handleSolutionTm();
|
||||
|
||||
/**
|
||||
* @brief This function handles the telemetry reply of a temperature request.
|
||||
*/
|
||||
@ -124,6 +206,43 @@ private:
|
||||
* @brief This function handles the telemetry reply of a version request.
|
||||
*/
|
||||
ReturnValue_t handleVersionTm();
|
||||
|
||||
/**
|
||||
* @brief Handles reply to request interface telemetry command.
|
||||
*/
|
||||
ReturnValue_t handleInterfaceTm();
|
||||
|
||||
/**
|
||||
* @brief Handles reply to request power telemetry command.
|
||||
*/
|
||||
ReturnValue_t handlePowerTm();
|
||||
|
||||
/**
|
||||
* @brief Extracts the header data of a received telemetry frame
|
||||
*/
|
||||
void getTmHeaderData(uint8_t* status, uint32_t* ticks, uint64_t* time);
|
||||
|
||||
/**
|
||||
* @brief This function deserializes 8 bytes into a 32 bit unsigned integer.
|
||||
*
|
||||
* @param buffer Pointer to buffer holding the data to deserialize
|
||||
*
|
||||
* @return The 32-bit unsigned integer.
|
||||
*
|
||||
* @note Deserialization will be performed in little endian byte order
|
||||
*/
|
||||
uint32_t deserializeUint32(uint8_t* buffer);
|
||||
|
||||
/**
|
||||
* @brief This function deserializes 8 bytes into a 64 bit unsigned integer.
|
||||
*
|
||||
* @param buffer Pointer to buffer holding the data to deserialize
|
||||
*
|
||||
* @return The 64-bit unsigned integer.
|
||||
*
|
||||
* @note Deserialization will be performed in little endian byte order
|
||||
*/
|
||||
uint64_t deserializeUint64(uint8_t* buffer);
|
||||
};
|
||||
|
||||
#endif /* MISSION_DEVICES_STARTRACKERHANDLER_H_ */
|
||||
|
||||
@ -5,6 +5,7 @@
|
||||
#include <fsfw/datapoollocal/LocalPoolVariable.h>
|
||||
#include <fsfw/devicehandlers/DeviceHandlerIF.h>
|
||||
#include "objects/systemObjectList.h"
|
||||
#include <fsfw/datapool/PoolReadGuard.h>
|
||||
|
||||
namespace StarTracker {
|
||||
|
||||
@ -14,32 +15,117 @@ static const uint8_t ADDRESS = 33;
|
||||
static const uint8_t STATUS_OK = 0;
|
||||
|
||||
enum PoolIds: lp_id_t {
|
||||
TICKS_TIME_SET,
|
||||
TIME_TIME_SET,
|
||||
RUN_TIME,
|
||||
UNIX_TIME,
|
||||
TICKS_VERSION_SET,
|
||||
TIME_VERSION_SET,
|
||||
PROGRAM,
|
||||
MAJOR,
|
||||
MINOR,
|
||||
TICKS_INTERFACE_SET,
|
||||
TIME_INTERFACE_SET,
|
||||
FRAME_COUNT,
|
||||
CHECKSUM_ERROR_COUNT,
|
||||
SET_PARAM_COUNT,
|
||||
SET_PARAM_REPLY_COUNT,
|
||||
PARAM_REQUEST_COUNT,
|
||||
PARAM_REPLY_COUNT,
|
||||
REQ_TM_COUNT,
|
||||
TM_REPLY_COUNT,
|
||||
ACTION_REQ_COUNT,
|
||||
ACTION_REPLY_COUNT,
|
||||
TICKS_POWER_SET,
|
||||
TIME_POWER_SET,
|
||||
MCU_CURRENT,
|
||||
MCU_VOLTAGE,
|
||||
FPGA_CORE_CURRENT,
|
||||
FPGA_CORE_VOLTAGE,
|
||||
FPGA_18_CURRENT,
|
||||
FPGA_18_VOLTAGE,
|
||||
FPGA_25_CURRENT,
|
||||
FPGA_25_VOLTAGE,
|
||||
CMV_21_CURRENT,
|
||||
CMV_21_VOLTAGE,
|
||||
CMV_PIX_CURRENT,
|
||||
CMV_PIX_VOLTAGE,
|
||||
CMV_33_CURRENT,
|
||||
CMV_33_VOLTAGE,
|
||||
CMV_RES_CURRENT,
|
||||
CMV_RES_VOLTAGE,
|
||||
TICKS_TEMPERATURE_SET,
|
||||
TIME_TEMPERATURE_SET,
|
||||
MCU_TEMPERATURE,
|
||||
CMOS_TEMPERATURE,
|
||||
PROGRAM,
|
||||
MAJOR,
|
||||
MINOR
|
||||
|
||||
TICKS_SOLUTION_SET,
|
||||
TIME_SOLUTION_SET,
|
||||
CALI_QW,
|
||||
CALI_QX,
|
||||
CALI_QY,
|
||||
CALI_QZ,
|
||||
TRACK_CONFIDENCE,
|
||||
TRACK_QW,
|
||||
TRACK_QX,
|
||||
TRACK_QY,
|
||||
TRACK_QZ,
|
||||
TRACK_REMOVED,
|
||||
STARS_CENTROIDED,
|
||||
STARS_MATCHED_DATABASE,
|
||||
LISA_QW,
|
||||
LISA_QX,
|
||||
LISA_QY,
|
||||
LISA_QZ,
|
||||
LISA_PERC_CLOSE,
|
||||
LISA_NR_CLOSE,
|
||||
TRUST_WORTHY,
|
||||
STABLE_COUNT,
|
||||
SOLUTION_STRATEGY
|
||||
};
|
||||
|
||||
|
||||
|
||||
static const DeviceCommandId_t PING_REQUEST = 0;
|
||||
// Boots image (works only in bootloader mode)
|
||||
static const DeviceCommandId_t BOOT = 1;
|
||||
static const DeviceCommandId_t REQ_VERSION = 2;
|
||||
static const DeviceCommandId_t REQ_INTERFACE = 3;
|
||||
static const DeviceCommandId_t REQ_TIME = 4;
|
||||
static const DeviceCommandId_t REBOOT = 7;
|
||||
static const DeviceCommandId_t REQ_POWER = 11;
|
||||
static const DeviceCommandId_t SUBSCRIBE_TO_TM = 18;
|
||||
static const DeviceCommandId_t REQ_SOLUTION = 24;
|
||||
static const DeviceCommandId_t REQ_TEMPERATURE = 25;
|
||||
|
||||
static const uint32_t VERSION_SET_ID = REQ_VERSION;
|
||||
static const uint32_t INTERFACE_SET_ID = REQ_INTERFACE;
|
||||
static const uint32_t POWER_SET_ID = REQ_POWER;
|
||||
static const uint32_t TEMPERATURE_SET_ID = REQ_TEMPERATURE;
|
||||
static const uint32_t TIME_SET_ID = REQ_TIME;
|
||||
static const uint32_t SOLUTION_SET_ID = REQ_SOLUTION;
|
||||
|
||||
/** Max size of unencoded frame */
|
||||
static const size_t MAX_FRAME_SIZE = 1200;
|
||||
|
||||
static const uint8_t TEMPERATURE_SET_ENTRIES = 4;
|
||||
static const uint8_t VERSION_SET_ENTRIES = 3;
|
||||
static const uint8_t VERSION_SET_ENTRIES = 5;
|
||||
static const uint8_t INTERFACE_SET_ENTRIES = 12;
|
||||
static const uint8_t POWER_SET_ENTRIES = 18;
|
||||
static const uint8_t TIME_SET_ENTRIES = 4;
|
||||
static const uint8_t SOLUTION_SET_ENTRIES = 23;
|
||||
|
||||
// Action, parameter and telemetry IDs
|
||||
namespace ID {
|
||||
static const uint8_t PING = 0;
|
||||
static const uint8_t BOOT = 1;
|
||||
static const uint8_t VERSION = 2;
|
||||
static const uint8_t INTERFACE = 3;
|
||||
static const uint8_t REBOOT = 7;
|
||||
static const uint8_t POWER = 11;
|
||||
static const uint8_t SUBSCRIBE = 18;
|
||||
static const uint8_t SOLUTION = 24;
|
||||
static const uint8_t TEMPERATURE = 25;
|
||||
static const uint8_t TIME = 1;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief This dataset can be used to store the temperature of a reaction wheel.
|
||||
@ -97,7 +183,262 @@ public:
|
||||
PoolIds::MINOR, this);
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Dataset to store the interface telemtry data.
|
||||
*/
|
||||
class InterfaceSet:
|
||||
public StaticLocalDataSet<INTERFACE_SET_ENTRIES> {
|
||||
public:
|
||||
|
||||
InterfaceSet(HasLocalDataPoolIF* owner):
|
||||
StaticLocalDataSet(owner, REQ_INTERFACE) {
|
||||
}
|
||||
|
||||
InterfaceSet(object_id_t objectId):
|
||||
StaticLocalDataSet(sid_t(objectId, REQ_INTERFACE)) {
|
||||
}
|
||||
|
||||
// Ticks is time reference generated by interanl counter of the star tracker
|
||||
lp_var_t<uint32_t> ticks = lp_var_t<uint32_t>(sid.objectId,
|
||||
PoolIds::TICKS_INTERFACE_SET, this);
|
||||
/** Unix time in microseconds */
|
||||
lp_var_t<uint64_t> time = lp_var_t<uint64_t>(sid.objectId,
|
||||
PoolIds::TIME_INTERFACE_SET, this);
|
||||
lp_var_t<uint32_t> frameCount = lp_var_t<uint32_t>(sid.objectId,
|
||||
PoolIds::FRAME_COUNT, this);
|
||||
lp_var_t<uint32_t> checksumerrorCount = lp_var_t<uint32_t>(sid.objectId,
|
||||
PoolIds::CHECKSUM_ERROR_COUNT, this);
|
||||
lp_var_t<uint32_t> setParamCount = lp_var_t<uint32_t>(sid.objectId,
|
||||
PoolIds::SET_PARAM_COUNT, this);
|
||||
lp_var_t<uint32_t> setParamReplyCount = lp_var_t<uint32_t>(sid.objectId,
|
||||
PoolIds::SET_PARAM_REPLY_COUNT, this);
|
||||
lp_var_t<uint32_t> paramRequestCount = lp_var_t<uint32_t>(sid.objectId,
|
||||
PoolIds::PARAM_REQUEST_COUNT, this);
|
||||
lp_var_t<uint32_t> paramReplyCount = lp_var_t<uint32_t>(sid.objectId,
|
||||
PoolIds::PARAM_REPLY_COUNT, this);
|
||||
lp_var_t<uint32_t> reqTmCount = lp_var_t<uint32_t>(sid.objectId,
|
||||
PoolIds::REQ_TM_COUNT, this);
|
||||
lp_var_t<uint32_t> tmReplyCount = lp_var_t<uint32_t>(sid.objectId,
|
||||
PoolIds::TM_REPLY_COUNT, this);
|
||||
lp_var_t<uint32_t> actionReqCount = lp_var_t<uint32_t>(sid.objectId,
|
||||
PoolIds::ACTION_REQ_COUNT, this);
|
||||
lp_var_t<uint32_t> actionReplyCount = lp_var_t<uint32_t>(sid.objectId,
|
||||
PoolIds::ACTION_REPLY_COUNT, this);
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Dataset to store the data of the power telemetry reply.
|
||||
*/
|
||||
class PowerSet:
|
||||
public StaticLocalDataSet<POWER_SET_ENTRIES> {
|
||||
public:
|
||||
|
||||
PowerSet(HasLocalDataPoolIF* owner):
|
||||
StaticLocalDataSet(owner, REQ_INTERFACE) {
|
||||
}
|
||||
|
||||
PowerSet(object_id_t objectId):
|
||||
StaticLocalDataSet(sid_t(objectId, REQ_INTERFACE)) {
|
||||
}
|
||||
|
||||
// Ticks is time reference generated by interanl counter of the star tracker
|
||||
lp_var_t<uint32_t> ticks = lp_var_t<uint32_t>(sid.objectId,
|
||||
PoolIds::TICKS_POWER_SET, this);
|
||||
/** Unix time in microseconds */
|
||||
lp_var_t<uint64_t> time = lp_var_t<uint64_t>(sid.objectId,
|
||||
PoolIds::TIME_POWER_SET, this);
|
||||
lp_var_t<float> mcuCurrent = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::MCU_CURRENT, this);
|
||||
lp_var_t<float> mcuVoltage = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::MCU_VOLTAGE, this);
|
||||
lp_var_t<float> fpgaCoreCurrent = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::FPGA_CORE_CURRENT, this);
|
||||
lp_var_t<float> fpgaCoreVoltage = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::FPGA_CORE_VOLTAGE, this);
|
||||
lp_var_t<float> fpga18Current = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::FPGA_18_CURRENT, this);
|
||||
lp_var_t<float> fpga18Voltage = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::FPGA_18_VOLTAGE, this);
|
||||
lp_var_t<float> fpga25Current = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::FPGA_25_CURRENT, this);
|
||||
lp_var_t<float> fpga25Voltage = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::FPGA_25_VOLTAGE, this);
|
||||
lp_var_t<float> cmv21Current = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::CMV_21_CURRENT, this);
|
||||
lp_var_t<float> cmv21Voltage = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::CMV_21_VOLTAGE, this);
|
||||
lp_var_t<float> cmvPixCurrent = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::CMV_PIX_CURRENT, this);
|
||||
lp_var_t<float> cmvPixVoltage = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::CMV_PIX_VOLTAGE, this);
|
||||
lp_var_t<float> cmv33Current = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::CMV_33_CURRENT, this);
|
||||
lp_var_t<float> cmv33Voltage = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::CMV_33_VOLTAGE, this);
|
||||
lp_var_t<float> cmvResCurrent = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::CMV_RES_CURRENT, this);
|
||||
lp_var_t<float> cmvResVoltage = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::CMV_RES_VOLTAGE, this);
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Data set to store the time telemetry packet.
|
||||
*/
|
||||
class TimeSet:
|
||||
public StaticLocalDataSet<TIME_SET_ENTRIES> {
|
||||
public:
|
||||
|
||||
TimeSet(HasLocalDataPoolIF* owner):
|
||||
StaticLocalDataSet(owner, TIME_SET_ID) {
|
||||
}
|
||||
|
||||
TimeSet(object_id_t objectId):
|
||||
StaticLocalDataSet(sid_t(objectId, TIME_SET_ID)) {
|
||||
}
|
||||
|
||||
lp_var_t<uint32_t> ticks = lp_var_t<uint32_t>(sid.objectId,
|
||||
PoolIds::TICKS_TIME_SET, this);
|
||||
/** Unix time in microseconds */
|
||||
lp_var_t<uint64_t> time = lp_var_t<uint64_t>(sid.objectId,
|
||||
PoolIds::TIME_TIME_SET, this);
|
||||
// Number of milliseconds since processor start-up
|
||||
lp_var_t<uint32_t> runTime = lp_var_t<uint32_t>(sid.objectId,
|
||||
PoolIds::RUN_TIME, this);
|
||||
// Unix time in seconds?? --> maybe typo in datasheet. Seems to be microseconds
|
||||
lp_var_t<uint64_t> unixTime = lp_var_t<uint64_t>(sid.objectId,
|
||||
PoolIds::UNIX_TIME, this);
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief The solution dataset is the main dataset of the star tracker and contains the
|
||||
* attitude information.
|
||||
*/
|
||||
class SolutionSet:
|
||||
public StaticLocalDataSet<SOLUTION_SET_ENTRIES> {
|
||||
public:
|
||||
|
||||
SolutionSet(HasLocalDataPoolIF* owner):
|
||||
StaticLocalDataSet(owner, SOLUTION_SET_ID) {
|
||||
}
|
||||
|
||||
SolutionSet(object_id_t objectId):
|
||||
StaticLocalDataSet(sid_t(objectId, SOLUTION_SET_ID)) {
|
||||
}
|
||||
|
||||
// Ticks timestamp
|
||||
lp_var_t<uint32_t> ticks = lp_var_t<uint32_t>(sid.objectId,
|
||||
PoolIds::TICKS_SOLUTION_SET, this);
|
||||
/// Unix time stamp
|
||||
lp_var_t<uint64_t> time = lp_var_t<uint64_t>(sid.objectId,
|
||||
PoolIds::TIME_SOLUTION_SET, this);
|
||||
// Calibrated quaternion (takes into account the mounting quaternion), typically same as
|
||||
// track q values
|
||||
lp_var_t<float> caliQw = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::CALI_QW, this);
|
||||
lp_var_t<float> caliQx = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::CALI_QX, this);
|
||||
lp_var_t<float> caliQy = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::CALI_QY, this);
|
||||
lp_var_t<float> caliQz = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::CALI_QZ, this);
|
||||
// The lower this value the more confidence that the star tracker solution is correct
|
||||
lp_var_t<float> trackConfidence = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::TRACK_CONFIDENCE, this);
|
||||
// Estimated attitude of spacecraft
|
||||
lp_var_t<float> trackQw = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::TRACK_QW, this);
|
||||
lp_var_t<float> trackQx = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::TRACK_QX, this);
|
||||
lp_var_t<float> trackQy = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::TRACK_QY, this);
|
||||
lp_var_t<float> trackQz = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::TRACK_QZ, this);
|
||||
// Number of stars removed from tracking solution
|
||||
lp_var_t<uint8_t> trackRemoved = lp_var_t<uint8_t>(sid.objectId,
|
||||
PoolIds::TRACK_REMOVED, this);
|
||||
// Number of stars for which a valid centroid was found
|
||||
lp_var_t<uint8_t> starsCentroided = lp_var_t<uint8_t>(sid.objectId,
|
||||
PoolIds::STARS_CENTROIDED, this);
|
||||
// Number of stars that matched to a database star
|
||||
lp_var_t<uint8_t> starsMatchedDatabase = lp_var_t<uint8_t>(sid.objectId,
|
||||
PoolIds::STARS_MATCHED_DATABASE, this);
|
||||
// Result of LISA (lost in space algorithm), searches for stars without prior knowledge of
|
||||
// attitude
|
||||
lp_var_t<float> lisaQw = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::LISA_QW, this);
|
||||
lp_var_t<float> lisaQx = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::LISA_QX, this);
|
||||
lp_var_t<float> lisaQy = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::LISA_QY, this);
|
||||
lp_var_t<float> lisaQz = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::LISA_QZ, this);
|
||||
// Percentage of close stars in LISA solution
|
||||
lp_var_t<float> lisaPercentageClose = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::LISA_PERC_CLOSE, this);
|
||||
// Number of close stars in LISA solution
|
||||
lp_var_t<uint8_t> lisaNrClose = lp_var_t<uint8_t>(sid.objectId,
|
||||
PoolIds::LISA_NR_CLOSE, this);
|
||||
// Gives a combined overview of the validation parameters (1 for valid solution, otherwise 0)
|
||||
lp_var_t<uint8_t> isTrustWorthy = lp_var_t<uint8_t>(sid.objectId,
|
||||
PoolIds::TRUST_WORTHY, this);
|
||||
// Number of times the validation criteria was met
|
||||
lp_var_t<uint32_t> stableCount = lp_var_t<uint32_t>(sid.objectId,
|
||||
PoolIds::STABLE_COUNT, this);
|
||||
// Shows the autonomous mode used to obtain the star tracker attitude
|
||||
lp_var_t<uint8_t> solutionStrategy = lp_var_t<uint8_t>(sid.objectId,
|
||||
PoolIds::SOLUTION_STRATEGY, this);
|
||||
|
||||
void printSet() {
|
||||
PoolReadGuard rg(this);
|
||||
sif::info << "SolutionSet::printSet: Ticks: "
|
||||
<< this->ticks << std::endl;
|
||||
sif::info << "SolutionSet::printSet: Time: "
|
||||
<< this->time << std::endl;
|
||||
sif::info << "SolutionSet::printSet: Calibrated quaternion Qw: "
|
||||
<< this->caliQw << std::endl;
|
||||
sif::info << "SolutionSet::printSet: Calibrated quaternion Qx: "
|
||||
<< this->caliQx << std::endl;
|
||||
sif::info << "SolutionSet::printSet: Calibrated quaternion Qy: "
|
||||
<< this->caliQy << std::endl;
|
||||
sif::info << "SolutionSet::printSet: Calibrated quaternion Qz: "
|
||||
<< this->caliQz << std::endl;
|
||||
sif::info << "SolutionSet::printSet: Track confidence: "
|
||||
<< this->trackConfidence << std::endl;
|
||||
sif::info << "SolutionSet::printSet: Track Qw: "
|
||||
<< this->trackQw << std::endl;
|
||||
sif::info << "SolutionSet::printSet: Track Qx: "
|
||||
<< this->trackQx << std::endl;
|
||||
sif::info << "SolutionSet::printSet: Track Qy: "
|
||||
<< this->trackQy << std::endl;
|
||||
sif::info << "SolutionSet::printSet: Track Qz: "
|
||||
<< this->trackQz << std::endl;
|
||||
sif::info << "SolutionSet::printSet: Track removed: "
|
||||
<< static_cast<uint8_t>(this->trackRemoved) << std::endl;
|
||||
sif::info << "SolutionSet::printSet: Number of stars centroided: "
|
||||
<< static_cast<uint8_t>(this->starsCentroided) << std::endl;
|
||||
sif::info << "SolutionSet::printSet: Number of stars matched database: "
|
||||
<< static_cast<uint8_t>(this->starsMatchedDatabase) << std::endl;
|
||||
sif::info << "SolutionSet::printSet: LISA Qw: "
|
||||
<< this->lisaQw << std::endl;
|
||||
sif::info << "SolutionSet::printSet: LISA Qx: "
|
||||
<< this->lisaQx << std::endl;
|
||||
sif::info << "SolutionSet::printSet: LISA Qy: "
|
||||
<< this->lisaQy << std::endl;
|
||||
sif::info << "SolutionSet::printSet: LISA Qz: "
|
||||
<< this->lisaQz << std::endl;
|
||||
sif::info << "SolutionSet::printSet: LISA Percentage close: "
|
||||
<< this->lisaPercentageClose << std::endl;
|
||||
sif::info << "SolutionSet::printSet: LISA number of close stars: "
|
||||
<< static_cast<uint8_t>(this->lisaNrClose) << std::endl;
|
||||
sif::info << "SolutionSet::printSet: Is trust worthy: "
|
||||
<< static_cast<uint8_t>(this->isTrustWorthy) << std::endl;
|
||||
sif::info << "SolutionSet::printSet: Stable count: "
|
||||
<< this->stableCount << std::endl;
|
||||
sif::info << "SolutionSet::printSet: Solution strategy: "
|
||||
<< static_cast<uint8_t>(this->solutionStrategy) << std::endl;
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif /* MISSION_STARTRACKER_DEFINITIONS_H_ */
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user