From 72c0503ac175893b895c3456f61ef74465bfc0d9 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Sun, 19 Dec 2021 11:29:40 +0100 Subject: [PATCH] changed deserialization method for star tracker datasets --- .../startracker/StarTrackerDefinitions.h | 14 +- .../startracker/StarTrackerHandler.cpp | 445 +++--------------- .../devices/startracker/StarTrackerHandler.h | 81 +--- 3 files changed, 86 insertions(+), 454 deletions(-) diff --git a/bsp_q7s/devices/startracker/StarTrackerDefinitions.h b/bsp_q7s/devices/startracker/StarTrackerDefinitions.h index 9fdd4aca..425c69d0 100644 --- a/bsp_q7s/devices/startracker/StarTrackerDefinitions.h +++ b/bsp_q7s/devices/startracker/StarTrackerDefinitions.h @@ -270,6 +270,8 @@ class TemperatureSet: public StaticLocalDataSet { public: + static const size_t SIZE = 20; + TemperatureSet(HasLocalDataPoolIF* owner): StaticLocalDataSet(owner, TEMPERATURE_SET_ID) { } @@ -308,6 +310,8 @@ class VersionSet: public StaticLocalDataSet { public: + static const size_t SIZE = 15; + VersionSet(HasLocalDataPoolIF* owner): StaticLocalDataSet(owner, VERSION_SET_ID) { } @@ -351,6 +355,8 @@ class InterfaceSet: public StaticLocalDataSet { public: + static const size_t SIZE = 20; + InterfaceSet(HasLocalDataPoolIF* owner): StaticLocalDataSet(owner, REQ_INTERFACE) { } @@ -390,6 +396,8 @@ class PowerSet: public StaticLocalDataSet { public: + static const size_t SIZE = 76; + PowerSet(HasLocalDataPoolIF* owner): StaticLocalDataSet(owner, REQ_INTERFACE) { } @@ -485,6 +493,8 @@ class TimeSet: public StaticLocalDataSet { public: + static const size_t SIZE = 24; + TimeSet(HasLocalDataPoolIF* owner): StaticLocalDataSet(owner, TIME_SET_ID) { } @@ -525,6 +535,8 @@ class SolutionSet: public StaticLocalDataSet { 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) { diff --git a/bsp_q7s/devices/startracker/StarTrackerHandler.cpp b/bsp_q7s/devices/startracker/StarTrackerHandler.cpp index cf02a2c7..35d6a58a 100644 --- a/bsp_q7s/devices/startracker/StarTrackerHandler.cpp +++ b/bsp_q7s/devices/startracker/StarTrackerHandler.cpp @@ -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( { 0 })); localDataPoolMap.emplace(StarTracker::HISTOGRAM_BIND8, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::TICKS_CONTRAST_SET, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::TIME_CONTRAST_SET, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BINA0, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BINA1, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BINA2, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BINA3, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BINA4, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BINA5, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BINA6, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BINA7, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BINA8, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BINB0, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BINB1, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BINB2, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BINB3, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BINB4, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BINB5, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BINB6, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BINB7, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BINB8, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BINC8, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BINC0, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BINC1, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BINC2, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BINC3, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BINC4, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BINC5, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BINC6, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BINC7, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BINC8, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BIND0, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BIND1, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BIND2, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BIND3, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BIND4, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BIND5, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BIND6, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BIND7, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CONTRAST_BIND8, new PoolEntry( { 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(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(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(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(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(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(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(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(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(*(buffer + 7)) << 56 - | static_cast(*(buffer + 6)) << 48 - | static_cast(*(buffer + 5)) << 40 - | static_cast(*(buffer + 4)) << 32 - | static_cast(*(buffer + 3)) << 24 - | static_cast(*(buffer + 2)) << 16 - | static_cast(*(buffer + 1)) << 8 - | static_cast(*(buffer)); - return word; -} - void StarTrackerHandler::handleStartup(const uint8_t* parameterId) { switch(*parameterId) { case (StarTracker::ID::LIMITS): { diff --git a/bsp_q7s/devices/startracker/StarTrackerHandler.h b/bsp_q7s/devices/startracker/StarTrackerHandler.h index 0fcf14d0..e534f590 100644 --- a/bsp_q7s/devices/startracker/StarTrackerHandler.h +++ b/bsp_q7s/devices/startracker/StarTrackerHandler.h @@ -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_ */