changed deserialization method for star tracker datasets
This commit is contained in:
@ -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): {
|
||||
|
Reference in New Issue
Block a user