eive/eive-obsw
eive/eive-obsw
12
3
Fork 0
Code Issues 3 Pull Requests 3 Releases 117 Wiki Activity

changed deserialization method for star tracker datasets

Browse Source
This commit is contained in:
Jakob Meier 2021-12-19 11:29:40 +01:00
parent 654e75fec8
commit 72c0503ac1
3 changed files with 86 additions and 454 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
bsp_q7s/devices/startracker/StarTrackerDefinitions.h
View File

@ -270,6 +270,8 @@ class TemperatureSet:
public StaticLocalDataSet<TEMPERATURE_SET_ENTRIES> {
public:
static const size_t SIZE = 20;
TemperatureSet(HasLocalDataPoolIF* owner):
StaticLocalDataSet(owner, TEMPERATURE_SET_ID) {
}
@ -308,6 +310,8 @@ class VersionSet:
public StaticLocalDataSet<VERSION_SET_ENTRIES> {
public:
static const size_t SIZE = 15;
VersionSet(HasLocalDataPoolIF* owner):
StaticLocalDataSet(owner, VERSION_SET_ID) {
}
@ -351,6 +355,8 @@ class InterfaceSet:
public StaticLocalDataSet<INTERFACE_SET_ENTRIES> {
public:
static const size_t SIZE = 20;
InterfaceSet(HasLocalDataPoolIF* owner):
StaticLocalDataSet(owner, REQ_INTERFACE) {
}
@ -390,6 +396,8 @@ class PowerSet:
public StaticLocalDataSet<POWER_SET_ENTRIES> {
public:
static const size_t SIZE = 76;
PowerSet(HasLocalDataPoolIF* owner):
StaticLocalDataSet(owner, REQ_INTERFACE) {
}
@ -485,6 +493,8 @@ class TimeSet:
public StaticLocalDataSet<TIME_SET_ENTRIES> {
public:
static const size_t SIZE = 24;
TimeSet(HasLocalDataPoolIF* owner):
StaticLocalDataSet(owner, TIME_SET_ID) {
}
@ -525,6 +535,8 @@ class SolutionSet:
public StaticLocalDataSet<SOLUTION_SET_ENTRIES> {
public:
static const size_t SIZE = 78;
SolutionSet(HasLocalDataPoolIF* owner):
StaticLocalDataSet(owner, SOLUTION_SET_ID) {
}
@ -740,7 +752,7 @@ class ContrastSet:
public:
// Size of dataset
static const size_t SIZE = 154;
static const size_t SIZE = 156;
ContrastSet(HasLocalDataPoolIF* owner):
StaticLocalDataSet(owner, CONTRAST_SET_ID) {

445
bsp_q7s/devices/startracker/StarTrackerHandler.cpp
View File

@ -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): {

81
bsp_q7s/devices/startracker/StarTrackerHandler.h
View File

@ -111,8 +111,8 @@ private:
static const ReturnValue_t FILENAME_TOO_LONG = MAKE_RETURN_CODE(0xAC);
//! [EXPORT] : [COMMENT] Received version reply with invalid program ID
static const ReturnValue_t INVALID_PROGRAM = MAKE_RETURN_CODE(0xAD);
//! [EXPORT] : [COMMENT] Status field of histogram reply signals error
static const ReturnValue_t HISTOGRAM_REQ_FAILED = MAKE_RETURN_CODE(0xAE);
//! [EXPORT] : [COMMENT] Status field of tm reply signals error
static const ReturnValue_t TM_REPLY_ERROR = MAKE_RETURN_CODE(0xAE);
//! [EXPORT] : [COMMENT] Status field of contrast reply signals error
static const ReturnValue_t CONTRAST_REQ_FAILED = MAKE_RETURN_CODE(0xAE);
@ -311,82 +311,25 @@ private:
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.
*/
ReturnValue_t handleTemperatureTm();
/**
* @brief Checks histogram reply and deserializes data into histogram dataset
*/
ReturnValue_t handleHistogramTm();
/**
* @brief Checks contrast reply and deserializes data into contrast dataset
*/
ReturnValue_t handleContrastTm();
/**
* @brief This function handles the telemetry reply of a version request.
*/
ReturnValue_t handleVersionTm();
/**
* @brief Checks the loaded program by means of the version set
*/
ReturnValue_t checkProgram();
/**
* @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(const 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(const uint8_t* buffer);
/**
* @brief Handles the startup state machine
*/
void handleStartup(const uint8_t* parameterId);
/**
* @brief Handles telemtry replies and fills the appropriate dataset
*
* @param dataset Dataset where reply data will be written to
* @param size Size of the dataset
*
* @return RETURN_OK if successful, otherwise error return value
*/
ReturnValue_t handleTm(LocalPoolDataSetBase& dataset, size_t size);
};
#endif /* MISSION_DEVICES_STARTRACKERHANDLER_H_ */