|
|
|
@ -491,7 +491,7 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, con
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
case (StarTracker::REQ_TIME): {
|
|
|
|
|
result = handleTimeTm();
|
|
|
|
|
result = handleTm(timeSet, StarTracker::TimeSet::SIZE);
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
case (StarTracker::PING_REQUEST): {
|
|
|
|
@ -511,7 +511,7 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, con
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
case (StarTracker::REQ_VERSION): {
|
|
|
|
|
result = handleVersionTm();
|
|
|
|
|
result = handleTm(versionSet, StarTracker::VersionSet::SIZE);
|
|
|
|
|
if (result != RETURN_OK) {
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
@ -522,27 +522,27 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, con
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
case (StarTracker::REQ_INTERFACE): {
|
|
|
|
|
result = handleInterfaceTm();
|
|
|
|
|
result = handleTm(interfaceSet, StarTracker::InterfaceSet::SIZE);
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
case (StarTracker::REQ_POWER): {
|
|
|
|
|
result = handlePowerTm();
|
|
|
|
|
result = handleTm(powerSet, StarTracker::PowerSet::SIZE);
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
case (StarTracker::REQ_SOLUTION): {
|
|
|
|
|
handleSolutionTm();
|
|
|
|
|
result = handleTm(solutionSet, StarTracker::SolutionSet::SIZE);
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
case (StarTracker::REQ_TEMPERATURE): {
|
|
|
|
|
handleTemperatureTm();
|
|
|
|
|
result = handleTm(temperatureSet, StarTracker::TemperatureSet::SIZE);
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
case (StarTracker::REQ_HISTOGRAM): {
|
|
|
|
|
handleHistogramTm();
|
|
|
|
|
result = handleTm(histogramSet, StarTracker::HistogramSet::SIZE);
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
case (StarTracker::REQ_CONTRAST): {
|
|
|
|
|
handleContrastTm();
|
|
|
|
|
result = handleTm(contrastSet, StarTracker::ContrastSet::SIZE);
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
case (StarTracker::LIMITS):
|
|
|
|
@ -691,6 +691,46 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BIND7, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BIND8, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::TICKS_CONTRAST_SET, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::TIME_CONTRAST_SET, new PoolEntry<uint64_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BINA0, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BINA1, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BINA2, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BINA3, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BINA4, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BINA5, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BINA6, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BINA7, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BINA8, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BINB0, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BINB1, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BINB2, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BINB3, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BINB4, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BINB5, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BINB6, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BINB7, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BINB8, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BINC8, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BINC0, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BINC1, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BINC2, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BINC3, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BINC4, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BINC5, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BINC6, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BINC7, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BINC8, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BIND0, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BIND1, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BIND2, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BIND3, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BIND4, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BIND5, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BIND6, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BIND7, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
localDataPoolMap.emplace(StarTracker::CONTRAST_BIND8, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
|
|
|
|
|
|
return RETURN_OK;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
@ -1076,74 +1116,6 @@ ReturnValue_t StarTrackerHandler::handlePingReply() {
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
ReturnValue_t StarTrackerHandler::handleTimeTm() {
|
|
|
|
|
ReturnValue_t result = RETURN_OK;
|
|
|
|
|
result = timeSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
|
|
|
|
if (result != RETURN_OK) {
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
uint32_t offset = TM_DATA_FIELD_OFFSET;
|
|
|
|
|
uint8_t status = 0;
|
|
|
|
|
uint32_t ticks = 0;
|
|
|
|
|
uint64_t time = 0;
|
|
|
|
|
getTmHeaderData(&status, &ticks, &time);
|
|
|
|
|
if(status != StarTracker::STATUS_OK) {
|
|
|
|
|
sif::warning << "StarTrackerHandler::handleTimeTm: Reply error: "
|
|
|
|
|
<< static_cast<unsigned int>(status) << std::endl;
|
|
|
|
|
result = VERSION_REQ_FAILED;
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
const uint8_t* reply = dataLinkLayer.getReply();
|
|
|
|
|
timeSet.ticks = ticks;
|
|
|
|
|
timeSet.time = time;
|
|
|
|
|
timeSet.runTime = deserializeUint32(reply + offset);
|
|
|
|
|
offset += sizeof(uint32_t);
|
|
|
|
|
timeSet.unixTime = deserializeUint64(reply + offset);
|
|
|
|
|
result = timeSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
|
|
|
|
if (result != RETURN_OK) {
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
|
|
|
|
|
timeSet.printSet();
|
|
|
|
|
#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
ReturnValue_t StarTrackerHandler::handleVersionTm() {
|
|
|
|
|
ReturnValue_t result = RETURN_OK;
|
|
|
|
|
uint32_t offset = TM_DATA_FIELD_OFFSET;
|
|
|
|
|
uint8_t status = 0;
|
|
|
|
|
uint32_t ticks = 0;
|
|
|
|
|
uint64_t time = 0;
|
|
|
|
|
getTmHeaderData(&status, &ticks, &time);
|
|
|
|
|
if(status != StarTracker::STATUS_OK) {
|
|
|
|
|
sif::warning << "StarTrackerHandler::handleVersionTm: Reply error: "
|
|
|
|
|
<< static_cast<unsigned int>(status) << std::endl;
|
|
|
|
|
result = VERSION_REQ_FAILED;
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
result = versionSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
|
|
|
|
if (result != RETURN_OK) {
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
const uint8_t* reply = dataLinkLayer.getReply();
|
|
|
|
|
versionSet.ticks = ticks;
|
|
|
|
|
versionSet.time = time;
|
|
|
|
|
versionSet.program = *(reply + offset);
|
|
|
|
|
offset += 1;
|
|
|
|
|
versionSet.major = *(reply + offset);
|
|
|
|
|
offset += 1;
|
|
|
|
|
versionSet.minor = *(reply + offset);
|
|
|
|
|
result = versionSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
|
|
|
|
if (result != RETURN_OK) {
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
|
|
|
|
|
versionSet.printSet();
|
|
|
|
|
#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
ReturnValue_t StarTrackerHandler::checkProgram() {
|
|
|
|
|
PoolReadGuard pg(&versionSet);
|
|
|
|
|
switch(versionSet.program.value) {
|
|
|
|
@ -1168,332 +1140,37 @@ ReturnValue_t StarTrackerHandler::checkProgram() {
|
|
|
|
|
return RETURN_OK;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
ReturnValue_t StarTrackerHandler::handleInterfaceTm() {
|
|
|
|
|
ReturnValue_t StarTrackerHandler::handleTm(LocalPoolDataSetBase& dataset, size_t size) {
|
|
|
|
|
ReturnValue_t result = RETURN_OK;
|
|
|
|
|
uint32_t offset = TM_DATA_FIELD_OFFSET;
|
|
|
|
|
uint8_t status = 0;
|
|
|
|
|
uint32_t ticks = 0;
|
|
|
|
|
uint64_t time = 0;
|
|
|
|
|
getTmHeaderData(&status, &ticks, &time);
|
|
|
|
|
uint8_t status = *(dataLinkLayer.getReply() + STATUS_OFFSET);
|
|
|
|
|
if(status != StarTracker::STATUS_OK) {
|
|
|
|
|
sif::warning << "StarTrackerHandler::handleInterfaceTm: Reply error: "
|
|
|
|
|
sif::warning << "StarTrackerHandler::handleTm: Reply error: "
|
|
|
|
|
<< static_cast<unsigned int>(status) << std::endl;
|
|
|
|
|
result = INTERFACE_REQ_FAILED;
|
|
|
|
|
return result;
|
|
|
|
|
return TM_REPLY_ERROR;
|
|
|
|
|
}
|
|
|
|
|
result = interfaceSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
|
|
|
|
result = dataset.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
|
|
|
|
if (result != RETURN_OK) {
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
const uint8_t* reply = dataLinkLayer.getReply();
|
|
|
|
|
interfaceSet.ticks = ticks;
|
|
|
|
|
interfaceSet.time = time;
|
|
|
|
|
size_t size = sizeof(uint32_t);
|
|
|
|
|
interfaceSet.frameCount = deserializeUint32(reply + offset);
|
|
|
|
|
offset += size;
|
|
|
|
|
interfaceSet.checksumerrorCount = deserializeUint32(reply + offset);
|
|
|
|
|
result = interfaceSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
|
|
|
|
const uint8_t* reply = dataLinkLayer.getReply() + TICKS_OFFSET;
|
|
|
|
|
dataset.setValidityBufferGeneration(false);
|
|
|
|
|
result = dataset.deSerialize(&reply, &size, SerializeIF::Endianness::LITTLE);
|
|
|
|
|
if (result != RETURN_OK) {
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
|
|
|
|
|
interfaceSet.printSet();
|
|
|
|
|
#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
ReturnValue_t StarTrackerHandler::handlePowerTm() {
|
|
|
|
|
ReturnValue_t result = RETURN_OK;
|
|
|
|
|
uint32_t offset = TM_DATA_FIELD_OFFSET;
|
|
|
|
|
uint8_t status = 0;
|
|
|
|
|
uint32_t ticks = 0;
|
|
|
|
|
uint64_t time = 0;
|
|
|
|
|
getTmHeaderData(&status, &ticks, &time);
|
|
|
|
|
if(status != StarTracker::STATUS_OK) {
|
|
|
|
|
sif::warning << "StarTrackerHandler::handlePowerTm: Reply error: "
|
|
|
|
|
<< static_cast<unsigned int>(status) << std::endl;
|
|
|
|
|
result = POWER_REQ_FAILED;
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
result = powerSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
|
|
|
|
if (result != RETURN_OK) {
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
const uint8_t* reply = dataLinkLayer.getReply();
|
|
|
|
|
powerSet.ticks= ticks;
|
|
|
|
|
powerSet.time= time;
|
|
|
|
|
float value = 0;
|
|
|
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
|
|
|
powerSet.mcuCurrent = value;
|
|
|
|
|
offset += 4;
|
|
|
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
|
|
|
powerSet.mcuVoltage = value;
|
|
|
|
|
offset += 4;
|
|
|
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
|
|
|
powerSet.fpgaCoreCurrent = value;
|
|
|
|
|
offset += 4;
|
|
|
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
|
|
|
powerSet.fpgaCoreVoltage = value;
|
|
|
|
|
offset += 4;
|
|
|
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
|
|
|
powerSet.fpga18Current = value;
|
|
|
|
|
offset += 4;
|
|
|
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
|
|
|
powerSet.fpga18Voltage = value;
|
|
|
|
|
offset += 4;
|
|
|
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
|
|
|
powerSet.fpga25Current = value;
|
|
|
|
|
offset += 4;
|
|
|
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
|
|
|
powerSet.fpga25Voltage = value;
|
|
|
|
|
offset += 4;
|
|
|
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
|
|
|
powerSet.cmv21Current = value;
|
|
|
|
|
offset += 4;
|
|
|
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
|
|
|
powerSet.cmv21Voltage = value;
|
|
|
|
|
offset += 4;
|
|
|
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
|
|
|
powerSet.cmvPixCurrent= value;
|
|
|
|
|
offset += 4;
|
|
|
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
|
|
|
powerSet.cmvPixVoltage = value;
|
|
|
|
|
offset += 4;
|
|
|
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
|
|
|
powerSet.cmv33Current= value;
|
|
|
|
|
offset += 4;
|
|
|
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
|
|
|
powerSet.cmv33Voltage = value;
|
|
|
|
|
offset += 4;
|
|
|
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
|
|
|
powerSet.cmvResCurrent= value;
|
|
|
|
|
offset += 4;
|
|
|
|
|
std::memcpy(&value, reply + offset, sizeof(value));
|
|
|
|
|
powerSet.cmvResVoltage = value;
|
|
|
|
|
result = powerSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
|
|
|
|
if (result != RETURN_OK) {
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
|
|
|
|
|
powerSet.printSet();
|
|
|
|
|
#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
ReturnValue_t StarTrackerHandler::handleSolutionTm() {
|
|
|
|
|
ReturnValue_t result = RETURN_OK;
|
|
|
|
|
result = solutionSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
|
|
|
|
if (result != RETURN_OK) {
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
uint32_t offset = TM_DATA_FIELD_OFFSET;
|
|
|
|
|
uint8_t status = 0;
|
|
|
|
|
uint32_t ticks = 0;
|
|
|
|
|
uint64_t time = 0;
|
|
|
|
|
getTmHeaderData(&status, &ticks, &time);
|
|
|
|
|
if(status != StarTracker::STATUS_OK) {
|
|
|
|
|
sif::warning << "StarTrackerHandler::handleSolutioneTm: Reply error: "
|
|
|
|
|
<< static_cast<unsigned int>(status) << std::endl;
|
|
|
|
|
result = TEMPERATURE_REQ_FAILED;
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
const uint8_t* reply = dataLinkLayer.getReply();
|
|
|
|
|
solutionSet.ticks= ticks;
|
|
|
|
|
solutionSet.time= time;
|
|
|
|
|
float word = 0;
|
|
|
|
|
std::memcpy(&word, reply + offset, sizeof(float));
|
|
|
|
|
solutionSet.caliQw = word;
|
|
|
|
|
offset += sizeof(float);
|
|
|
|
|
std::memcpy(&word, reply + offset, sizeof(float));
|
|
|
|
|
solutionSet.caliQx = word;
|
|
|
|
|
offset += sizeof(float);
|
|
|
|
|
std::memcpy(&word, reply + offset, sizeof(float));
|
|
|
|
|
solutionSet.caliQy = word;
|
|
|
|
|
offset += sizeof(float);
|
|
|
|
|
std::memcpy(&word, reply + offset, sizeof(float));
|
|
|
|
|
solutionSet.caliQz = word;
|
|
|
|
|
offset += sizeof(float);
|
|
|
|
|
std::memcpy(&word, reply + offset, sizeof(float));
|
|
|
|
|
solutionSet.trackConfidence = word;
|
|
|
|
|
offset += sizeof(float);
|
|
|
|
|
std::memcpy(&word, reply + offset, sizeof(float));
|
|
|
|
|
solutionSet.trackQw = word;
|
|
|
|
|
offset += sizeof(float);
|
|
|
|
|
std::memcpy(&word, reply + offset, sizeof(float));
|
|
|
|
|
solutionSet.trackQx = word;
|
|
|
|
|
offset += sizeof(float);
|
|
|
|
|
std::memcpy(&word, reply + offset, sizeof(float));
|
|
|
|
|
solutionSet.trackQy = word;
|
|
|
|
|
offset += sizeof(float);
|
|
|
|
|
std::memcpy(&word, reply + offset, sizeof(float));
|
|
|
|
|
solutionSet.trackQz = word;
|
|
|
|
|
offset += sizeof(float);
|
|
|
|
|
solutionSet.trackRemoved = *(reply + offset);
|
|
|
|
|
offset += sizeof(uint8_t);
|
|
|
|
|
solutionSet.starsCentroided = *(reply + offset);
|
|
|
|
|
offset += sizeof(uint8_t);
|
|
|
|
|
solutionSet.starsMatchedDatabase = *(reply + offset);
|
|
|
|
|
offset += sizeof(float);
|
|
|
|
|
std::memcpy(&word, reply + offset, sizeof(float));
|
|
|
|
|
solutionSet.lisaQw = word;
|
|
|
|
|
offset += sizeof(float);
|
|
|
|
|
std::memcpy(&word, reply + offset, sizeof(float));
|
|
|
|
|
solutionSet.lisaQx = word;
|
|
|
|
|
offset += sizeof(float);
|
|
|
|
|
std::memcpy(&word, reply + offset, sizeof(float));
|
|
|
|
|
solutionSet.lisaQy = word;
|
|
|
|
|
offset += sizeof(float);
|
|
|
|
|
std::memcpy(&word, reply + offset, sizeof(float));
|
|
|
|
|
solutionSet.lisaQz = word;
|
|
|
|
|
offset += sizeof(float);
|
|
|
|
|
std::memcpy(&word, reply + offset, sizeof(float));
|
|
|
|
|
solutionSet.lisaPercentageClose = word;
|
|
|
|
|
offset += sizeof(float);
|
|
|
|
|
solutionSet.lisaNrClose = *(reply + offset);
|
|
|
|
|
offset += sizeof(uint8_t);
|
|
|
|
|
solutionSet.isTrustWorthy = *(reply + offset);
|
|
|
|
|
offset += sizeof(uint8_t);
|
|
|
|
|
solutionSet.stableCount = *(reply + offset);
|
|
|
|
|
offset += sizeof(uint32_t);
|
|
|
|
|
solutionSet.stableCount = *(reply + offset);
|
|
|
|
|
result = solutionSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
|
|
|
|
if (result != RETURN_OK) {
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
|
|
|
|
|
solutionSet.printSet();
|
|
|
|
|
#endif
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
ReturnValue_t StarTrackerHandler::handleTemperatureTm() {
|
|
|
|
|
ReturnValue_t result = RETURN_OK;
|
|
|
|
|
uint32_t offset = TM_DATA_FIELD_OFFSET;
|
|
|
|
|
uint8_t status = 0;
|
|
|
|
|
uint32_t ticks = 0;
|
|
|
|
|
uint64_t time = 0;
|
|
|
|
|
getTmHeaderData(&status, &ticks, &time);
|
|
|
|
|
if(status != StarTracker::STATUS_OK) {
|
|
|
|
|
sif::warning << "StarTrackerHandler::handleTemperatureTm: Reply error: "
|
|
|
|
|
<< static_cast<unsigned int>(status) << std::endl;
|
|
|
|
|
result = TEMPERATURE_REQ_FAILED;
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
result = temperatureSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
|
|
|
|
if (result != RETURN_OK) {
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
const uint8_t* reply = dataLinkLayer.getReply();
|
|
|
|
|
temperatureSet.ticks= ticks;
|
|
|
|
|
temperatureSet.time= time;
|
|
|
|
|
float temperature = 0;
|
|
|
|
|
std::memcpy(&temperature, reply + offset, sizeof(temperature));
|
|
|
|
|
temperatureSet.mcuTemperature = temperature;
|
|
|
|
|
offset += sizeof(float);
|
|
|
|
|
std::memcpy(&temperature, reply + offset, sizeof(temperature));
|
|
|
|
|
temperatureSet.cmosTemperature = temperature;
|
|
|
|
|
result = temperatureSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
|
|
|
|
if (result != RETURN_OK) {
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
|
|
|
|
|
temperatureSet.printSet();
|
|
|
|
|
#endif
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
ReturnValue_t StarTrackerHandler::handleHistogramTm() {
|
|
|
|
|
ReturnValue_t result = RETURN_OK;
|
|
|
|
|
uint8_t status = 0;
|
|
|
|
|
uint32_t ticks = 0;
|
|
|
|
|
uint64_t time = 0;
|
|
|
|
|
getTmHeaderData(&status, &ticks, &time);
|
|
|
|
|
if(status != StarTracker::STATUS_OK) {
|
|
|
|
|
sif::warning << "StarTrackerHandler::handleHistogramTm: Reply error: "
|
|
|
|
|
<< static_cast<unsigned int>(status) << std::endl;
|
|
|
|
|
return HISTOGRAM_REQ_FAILED;
|
|
|
|
|
}
|
|
|
|
|
result = histogramSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
|
|
|
|
if (result != RETURN_OK) {
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
const uint8_t* reply = dataLinkLayer.getReply() + TM_DATA_FIELD_OFFSET;
|
|
|
|
|
size_t size = StarTracker::HistogramSet::SIZE;
|
|
|
|
|
histogramSet.setValidityBufferGeneration(false);
|
|
|
|
|
result = histogramSet.deSerialize(&reply, &size, SerializeIF::Endianness::LITTLE);
|
|
|
|
|
histogramSet.setValidityBufferGeneration(true);
|
|
|
|
|
if (result != RETURN_OK) {
|
|
|
|
|
sif::warning << "StarTrackerHandler::handleHistogramTm: Deserialization failed"
|
|
|
|
|
sif::warning << "StarTrackerHandler::handleTm: Deserialization failed"
|
|
|
|
|
<< std::endl;
|
|
|
|
|
}
|
|
|
|
|
result = histogramSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
|
|
|
|
dataset.setValidityBufferGeneration(true);
|
|
|
|
|
dataset.setValidity(true, true);
|
|
|
|
|
result = dataset.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
|
|
|
|
if (result != RETURN_OK) {
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
|
|
|
|
|
histogramSet.printSet();
|
|
|
|
|
dataset.printSet();
|
|
|
|
|
#endif
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
ReturnValue_t StarTrackerHandler::handleContrastTm() {
|
|
|
|
|
ReturnValue_t result = RETURN_OK;
|
|
|
|
|
uint8_t status = 0;
|
|
|
|
|
uint32_t ticks = 0;
|
|
|
|
|
uint64_t time = 0;
|
|
|
|
|
getTmHeaderData(&status, &ticks, &time);
|
|
|
|
|
if(status != StarTracker::STATUS_OK) {
|
|
|
|
|
sif::warning << "StarTrackerHandler::handleContrastTm: Reply error: "
|
|
|
|
|
<< static_cast<unsigned int>(status) << std::endl;
|
|
|
|
|
result = CONTRAST_REQ_FAILED;
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
result = contrastSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
|
|
|
|
if (result != RETURN_OK) {
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
const uint8_t* reply = dataLinkLayer.getReply() + TM_DATA_FIELD_OFFSET;
|
|
|
|
|
size_t size = StarTracker::ContrastSet::SIZE;
|
|
|
|
|
result = contrastSet.deSerialize(&reply, &size, SerializeIF::Endianness::LITTLE);
|
|
|
|
|
if (result != RETURN_OK) {
|
|
|
|
|
sif::warning << "StarTrackerHandler::handleContrastTm: Deserialization failed"
|
|
|
|
|
<< std::endl;
|
|
|
|
|
}
|
|
|
|
|
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
|
|
|
|
|
contrastSet.printSet();
|
|
|
|
|
#endif
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void StarTrackerHandler::getTmHeaderData(uint8_t* status, uint32_t* ticks, uint64_t* time) {
|
|
|
|
|
const uint8_t* reply = dataLinkLayer.getReply();
|
|
|
|
|
*status = *(reply + STATUS_OFFSET);
|
|
|
|
|
*ticks = deserializeUint32(reply + TICKS_OFFSET);
|
|
|
|
|
*time = deserializeUint64(reply + TIME_OFFSET);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
uint32_t StarTrackerHandler::deserializeUint32(const uint8_t* buffer) {
|
|
|
|
|
uint32_t word = 0;
|
|
|
|
|
word = *(buffer + 3) << 24
|
|
|
|
|
| *(buffer + 2) << 16
|
|
|
|
|
| *(buffer + 1) << 8
|
|
|
|
|
| *(buffer);
|
|
|
|
|
return word;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
uint64_t StarTrackerHandler::deserializeUint64(const uint8_t* buffer) {
|
|
|
|
|
uint64_t word = 0;
|
|
|
|
|
word = static_cast<uint64_t>(*(buffer + 7)) << 56
|
|
|
|
|
| static_cast<uint64_t>(*(buffer + 6)) << 48
|
|
|
|
|
| static_cast<uint64_t>(*(buffer + 5)) << 40
|
|
|
|
|
| static_cast<uint64_t>(*(buffer + 4)) << 32
|
|
|
|
|
| static_cast<uint64_t>(*(buffer + 3)) << 24
|
|
|
|
|
| static_cast<uint64_t>(*(buffer + 2)) << 16
|
|
|
|
|
| static_cast<uint64_t>(*(buffer + 1)) << 8
|
|
|
|
|
| static_cast<uint64_t>(*(buffer));
|
|
|
|
|
return word;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void StarTrackerHandler::handleStartup(const uint8_t* parameterId) {
|
|
|
|
|
switch(*parameterId) {
|
|
|
|
|
case (StarTracker::ID::LIMITS): {
|
|
|
|
|