From 4dee8658fd0f6131cb58e712ebcf71c18bbb0cda Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Wed, 15 Dec 2021 19:32:43 +0100 Subject: [PATCH 1/6] histogram and contrast telemetry set --- .../devices/startracker/ArcsecDatalinkLayer.h | 2 +- .../startracker/StarTrackerDefinitions.h | 354 ++++++++++++++++-- .../startracker/StarTrackerHandler.cpp | 147 +++++++- .../devices/startracker/StarTrackerHandler.h | 23 ++ fsfw | 2 +- 5 files changed, 482 insertions(+), 46 deletions(-) diff --git a/bsp_q7s/devices/startracker/ArcsecDatalinkLayer.h b/bsp_q7s/devices/startracker/ArcsecDatalinkLayer.h index eb5beecc..84b04426 100644 --- a/bsp_q7s/devices/startracker/ArcsecDatalinkLayer.h +++ b/bsp_q7s/devices/startracker/ArcsecDatalinkLayer.h @@ -51,7 +51,7 @@ public: uint8_t getReplyFrameType(); /** - * @brief Returns pointer to reply packet. + * @brief Returns pointer to reply packet (first entry normally action ID, telemetry ID etc.) */ const uint8_t* getReply(); diff --git a/bsp_q7s/devices/startracker/StarTrackerDefinitions.h b/bsp_q7s/devices/startracker/StarTrackerDefinitions.h index 8c87dcfc..4ff30ec0 100644 --- a/bsp_q7s/devices/startracker/StarTrackerDefinitions.h +++ b/bsp_q7s/devices/startracker/StarTrackerDefinitions.h @@ -81,7 +81,85 @@ enum PoolIds: lp_id_t { LISA_NR_CLOSE, TRUST_WORTHY, STABLE_COUNT, - SOLUTION_STRATEGY + SOLUTION_STRATEGY, + + TICKS_HISTOGRAM_SET, + TIME_HISTOGRAM_SET, + HISTOGRAM_BINA0, + HISTOGRAM_BINA1, + HISTOGRAM_BINA2, + HISTOGRAM_BINA3, + HISTOGRAM_BINA4, + HISTOGRAM_BINA5, + HISTOGRAM_BINA6, + HISTOGRAM_BINA7, + HISTOGRAM_BINA8, + HISTOGRAM_BINB0, + HISTOGRAM_BINB1, + HISTOGRAM_BINB2, + HISTOGRAM_BINB3, + HISTOGRAM_BINB4, + HISTOGRAM_BINB5, + HISTOGRAM_BINB6, + HISTOGRAM_BINB7, + HISTOGRAM_BINB8, + HISTOGRAM_BINC0, + HISTOGRAM_BINC1, + HISTOGRAM_BINC2, + HISTOGRAM_BINC3, + HISTOGRAM_BINC4, + HISTOGRAM_BINC5, + HISTOGRAM_BINC6, + HISTOGRAM_BINC7, + HISTOGRAM_BINC8, + HISTOGRAM_BIND0, + HISTOGRAM_BIND1, + HISTOGRAM_BIND2, + HISTOGRAM_BIND3, + HISTOGRAM_BIND4, + HISTOGRAM_BIND5, + HISTOGRAM_BIND6, + HISTOGRAM_BIND7, + HISTOGRAM_BIND8, + + TICKS_CONTRAST_SET, + TIME_CONTRAST_SET, + CONTRAST_BINA0, + CONTRAST_BINA1, + CONTRAST_BINA2, + CONTRAST_BINA3, + CONTRAST_BINA4, + CONTRAST_BINA5, + CONTRAST_BINA6, + CONTRAST_BINA7, + CONTRAST_BINA8, + CONTRAST_BINB0, + CONTRAST_BINB1, + CONTRAST_BINB2, + CONTRAST_BINB3, + CONTRAST_BINB4, + CONTRAST_BINB5, + CONTRAST_BINB6, + CONTRAST_BINB7, + CONTRAST_BINB8, + CONTRAST_BINC0, + CONTRAST_BINC1, + CONTRAST_BINC2, + CONTRAST_BINC3, + CONTRAST_BINC4, + CONTRAST_BINC5, + CONTRAST_BINC6, + CONTRAST_BINC7, + CONTRAST_BINC8, + CONTRAST_BIND0, + CONTRAST_BIND1, + CONTRAST_BIND2, + CONTRAST_BIND3, + CONTRAST_BIND4, + CONTRAST_BIND5, + CONTRAST_BIND6, + CONTRAST_BIND7, + CONTRAST_BIND8 }; static const DeviceCommandId_t PING_REQUEST = 0; @@ -102,6 +180,8 @@ static const DeviceCommandId_t UPLOAD_CENTROID = 17; static const DeviceCommandId_t SUBSCRIBE_TO_TM = 18; static const DeviceCommandId_t REQ_SOLUTION = 24; static const DeviceCommandId_t REQ_TEMPERATURE = 25; +static const DeviceCommandId_t REQ_HISTOGRAM = 28; +static const DeviceCommandId_t REQ_CONTRAST = 29; static const DeviceCommandId_t LIMITS = 40; static const DeviceCommandId_t MOUNTING = 41; static const DeviceCommandId_t CAMERA = 42; @@ -129,6 +209,8 @@ static const uint32_t POWER_SET_ID = REQ_POWER; static const uint32_t TEMPERATURE_SET_ID = REQ_TEMPERATURE; static const uint32_t TIME_SET_ID = REQ_TIME; static const uint32_t SOLUTION_SET_ID = REQ_SOLUTION; +static const uint32_t HISTOGRAM_SET_ID = REQ_HISTOGRAM; +static const uint32_t CONTRAST_SET_ID = REQ_CONTRAST; /** Max size of unencoded frame */ static const size_t MAX_FRAME_SIZE = 1200; @@ -139,6 +221,8 @@ static const uint8_t INTERFACE_SET_ENTRIES = 4; static const uint8_t POWER_SET_ENTRIES = 18; static const uint8_t TIME_SET_ENTRIES = 4; static const uint8_t SOLUTION_SET_ENTRIES = 23; +static const uint8_t HISTOGRAM_SET_ENTRIES = 38; +static const uint8_t CONTRAST_SET_ENTRIES = 38; // Action, parameter and telemetry IDs namespace ID { @@ -162,6 +246,8 @@ namespace ID { static const uint8_t SUBSCRIBE = 18; static const uint8_t SOLUTION = 24; static const uint8_t TEMPERATURE = 25; + static const uint8_t HISTOGRAM = 28; + static const uint8_t CONTRAST = 29; static const uint8_t TIME = 1; static const uint8_t WRITE = 2; static const uint8_t READ = 3; @@ -455,60 +541,44 @@ public: PoolIds::TIME_SOLUTION_SET, this); // Calibrated quaternion (takes into account the mounting quaternion), typically same as // track q values - lp_var_t caliQw = lp_var_t(sid.objectId, - PoolIds::CALI_QW, this); - lp_var_t caliQx = lp_var_t(sid.objectId, - PoolIds::CALI_QX, this); - lp_var_t caliQy = lp_var_t(sid.objectId, - PoolIds::CALI_QY, this); - lp_var_t caliQz = lp_var_t(sid.objectId, - PoolIds::CALI_QZ, this); + lp_var_t caliQw = lp_var_t(sid.objectId, PoolIds::CALI_QW, this); + lp_var_t caliQx = lp_var_t(sid.objectId, PoolIds::CALI_QX, this); + lp_var_t caliQy = lp_var_t(sid.objectId, PoolIds::CALI_QY, this); + lp_var_t caliQz = lp_var_t(sid.objectId, PoolIds::CALI_QZ, this); // The lower this value the more confidence that the star tracker solution is correct - lp_var_t trackConfidence = lp_var_t(sid.objectId, - PoolIds::TRACK_CONFIDENCE, this); + lp_var_t trackConfidence = lp_var_t(sid.objectId, PoolIds::TRACK_CONFIDENCE, + this); // Estimated attitude of spacecraft - lp_var_t trackQw = lp_var_t(sid.objectId, - PoolIds::TRACK_QW, this); - lp_var_t trackQx = lp_var_t(sid.objectId, - PoolIds::TRACK_QX, this); - lp_var_t trackQy = lp_var_t(sid.objectId, - PoolIds::TRACK_QY, this); - lp_var_t trackQz = lp_var_t(sid.objectId, - PoolIds::TRACK_QZ, this); + lp_var_t trackQw = lp_var_t(sid.objectId, PoolIds::TRACK_QW, this); + lp_var_t trackQx = lp_var_t(sid.objectId, PoolIds::TRACK_QX, this); + lp_var_t trackQy = lp_var_t(sid.objectId, PoolIds::TRACK_QY, this); + lp_var_t trackQz = lp_var_t(sid.objectId, PoolIds::TRACK_QZ, this); // Number of stars removed from tracking solution - lp_var_t trackRemoved = lp_var_t(sid.objectId, - PoolIds::TRACK_REMOVED, this); + lp_var_t trackRemoved = lp_var_t(sid.objectId, PoolIds::TRACK_REMOVED, this); // Number of stars for which a valid centroid was found - lp_var_t starsCentroided = lp_var_t(sid.objectId, - PoolIds::STARS_CENTROIDED, this); + lp_var_t starsCentroided = lp_var_t(sid.objectId, PoolIds::STARS_CENTROIDED, + this); // Number of stars that matched to a database star lp_var_t starsMatchedDatabase = lp_var_t(sid.objectId, PoolIds::STARS_MATCHED_DATABASE, this); // Result of LISA (lost in space algorithm), searches for stars without prior knowledge of // attitude - lp_var_t lisaQw = lp_var_t(sid.objectId, - PoolIds::LISA_QW, this); - lp_var_t lisaQx = lp_var_t(sid.objectId, - PoolIds::LISA_QX, this); - lp_var_t lisaQy = lp_var_t(sid.objectId, - PoolIds::LISA_QY, this); - lp_var_t lisaQz = lp_var_t(sid.objectId, - PoolIds::LISA_QZ, this); + lp_var_t lisaQw = lp_var_t(sid.objectId, PoolIds::LISA_QW, this); + lp_var_t lisaQx = lp_var_t(sid.objectId, PoolIds::LISA_QX, this); + lp_var_t lisaQy = lp_var_t(sid.objectId, PoolIds::LISA_QY, this); + lp_var_t lisaQz = lp_var_t(sid.objectId, PoolIds::LISA_QZ, this); // Percentage of close stars in LISA solution - lp_var_t lisaPercentageClose = lp_var_t(sid.objectId, - PoolIds::LISA_PERC_CLOSE, this); + lp_var_t lisaPercentageClose = lp_var_t(sid.objectId, PoolIds::LISA_PERC_CLOSE, + this); // Number of close stars in LISA solution - lp_var_t lisaNrClose = lp_var_t(sid.objectId, - PoolIds::LISA_NR_CLOSE, this); + lp_var_t lisaNrClose = lp_var_t(sid.objectId, PoolIds::LISA_NR_CLOSE, this); // Gives a combined overview of the validation parameters (1 for valid solution, otherwise 0) - lp_var_t isTrustWorthy = lp_var_t(sid.objectId, - PoolIds::TRUST_WORTHY, this); + lp_var_t isTrustWorthy = lp_var_t(sid.objectId, PoolIds::TRUST_WORTHY, this); // Number of times the validation criteria was met - lp_var_t stableCount = lp_var_t(sid.objectId, - PoolIds::STABLE_COUNT, this); + lp_var_t stableCount = lp_var_t(sid.objectId, PoolIds::STABLE_COUNT, this); // Shows the autonomous mode used to obtain the star tracker attitude - lp_var_t solutionStrategy = lp_var_t(sid.objectId, - PoolIds::SOLUTION_STRATEGY, this); + lp_var_t solutionStrategy = lp_var_t(sid.objectId, PoolIds::SOLUTION_STRATEGY, + this); void printSet() { PoolReadGuard rg(this); @@ -561,6 +631,208 @@ public: } }; +/** + * @brief Dataset to store the histogram + */ +class HistogramSet: + public StaticLocalDataSet { +public: + + // Size of dataset + static const size_t SIZE = 154; + + HistogramSet(HasLocalDataPoolIF* owner): + StaticLocalDataSet(owner, HISTOGRAM_SET_ID) { + } + + HistogramSet(object_id_t objectId): + StaticLocalDataSet(sid_t(objectId, HISTOGRAM_SET_ID)) { + } + + lp_var_t ticks = lp_var_t(sid.objectId, PoolIds::TICKS_HISTOGRAM_SET, this); + lp_var_t time = lp_var_t(sid.objectId, PoolIds::TIME_HISTOGRAM_SET, this); + lp_var_t binA0 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINA0, this); + lp_var_t binA1 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINA1, this); + lp_var_t binA2 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINA2, this); + lp_var_t binA3 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINA3, this); + lp_var_t binA4 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINA4, this); + lp_var_t binA5 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINA5, this); + lp_var_t binA6 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINA6, this); + lp_var_t binA7 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINA7, this); + lp_var_t binA8 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINA8, this); + lp_var_t binb0 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINB0, this); + lp_var_t binB1 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINB1, this); + lp_var_t binB2 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINB2, this); + lp_var_t binB3 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINB3, this); + lp_var_t binB4 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINB4, this); + lp_var_t binB5 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINB5, this); + lp_var_t binB6 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINB6, this); + lp_var_t binB7 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINB7, this); + lp_var_t binB8 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINB8, this); + lp_var_t binC0 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINC0, this); + lp_var_t binC1 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINC1, this); + lp_var_t binC2 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINC2, this); + lp_var_t binC3 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINC3, this); + lp_var_t binC4 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINC4, this); + lp_var_t binC5 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINC5, this); + lp_var_t binC6 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINC6, this); + lp_var_t binC7 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINC7, this); + lp_var_t binC8 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINC8, this); + lp_var_t binD0 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BIND0, this); + lp_var_t binD1 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BIND1, this); + lp_var_t binD2 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BIND2, this); + lp_var_t binD3 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BIND3, this); + lp_var_t binD4 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BIND4, this); + lp_var_t binD5 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BIND5, this); + lp_var_t binD6 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BIND6, this); + lp_var_t binD7 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BIND7, this); + lp_var_t binD8 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BIND8, this); + + void printSet() { + PoolReadGuard rg(this); + sif::info << "HistogramSet::printSet: Ticks: " << this->ticks << std::endl; + sif::info << "HistogramSet::printSet: Time (time stamp): " << this->time << " us" + << std::endl; + sif::info << "HistogramSet::printSet: BinA0: " << this->binA0 << std::endl; + sif::info << "HistogramSet::printSet: BinA1: " << this->binA1 << std::endl; + sif::info << "HistogramSet::printSet: BinA2: " << this->binA2 << std::endl; + sif::info << "HistogramSet::printSet: BinA3: " << this->binA3 << std::endl; + sif::info << "HistogramSet::printSet: BinA4: " << this->binA4 << std::endl; + sif::info << "HistogramSet::printSet: BinA5: " << this->binA5 << std::endl; + sif::info << "HistogramSet::printSet: BinA6: " << this->binA6 << std::endl; + sif::info << "HistogramSet::printSet: BinA7: " << this->binA7 << std::endl; + sif::info << "HistogramSet::printSet: BinA8: " << this->binA8 << std::endl; + sif::info << "HistogramSet::printSet: BinB0: " << this->binA0 << std::endl; + sif::info << "HistogramSet::printSet: BinB1: " << this->binB1 << std::endl; + sif::info << "HistogramSet::printSet: BinB2: " << this->binB2 << std::endl; + sif::info << "HistogramSet::printSet: BinB3: " << this->binB3 << std::endl; + sif::info << "HistogramSet::printSet: BinB4: " << this->binB4 << std::endl; + sif::info << "HistogramSet::printSet: BinB5: " << this->binB5 << std::endl; + sif::info << "HistogramSet::printSet: BinB6: " << this->binB6 << std::endl; + sif::info << "HistogramSet::printSet: BinB7: " << this->binB7 << std::endl; + sif::info << "HistogramSet::printSet: BinB8: " << this->binB8 << std::endl; + sif::info << "HistogramSet::printSet: BinC0: " << this->binC0 << std::endl; + sif::info << "HistogramSet::printSet: BinC1: " << this->binC1 << std::endl; + sif::info << "HistogramSet::printSet: BinC2: " << this->binC2 << std::endl; + sif::info << "HistogramSet::printSet: BinC3: " << this->binC3 << std::endl; + sif::info << "HistogramSet::printSet: BinC4: " << this->binC4 << std::endl; + sif::info << "HistogramSet::printSet: BinC5: " << this->binC5 << std::endl; + sif::info << "HistogramSet::printSet: BinC6: " << this->binC6 << std::endl; + sif::info << "HistogramSet::printSet: BinC7: " << this->binC7 << std::endl; + sif::info << "HistogramSet::printSet: BinC8: " << this->binC8 << std::endl; + sif::info << "HistogramSet::printSet: BinD0: " << this->binD0 << std::endl; + sif::info << "HistogramSet::printSet: BinD1: " << this->binD1 << std::endl; + sif::info << "HistogramSet::printSet: BinD2: " << this->binD2 << std::endl; + sif::info << "HistogramSet::printSet: BinD3: " << this->binD3 << std::endl; + sif::info << "HistogramSet::printSet: BinD4: " << this->binD4 << std::endl; + sif::info << "HistogramSet::printSet: BinD5: " << this->binD5 << std::endl; + sif::info << "HistogramSet::printSet: BinD6: " << this->binD6 << std::endl; + sif::info << "HistogramSet::printSet: BinD7: " << this->binD7 << std::endl; + sif::info << "HistogramSet::printSet: BinD8: " << this->binD8 << std::endl; + } +}; + +/** + * @brief Dataset to store the contrast telemetry data + */ +class ContrastSet: + public StaticLocalDataSet { +public: + + // Size of dataset + static const size_t SIZE = 154; + + ContrastSet(HasLocalDataPoolIF* owner): + StaticLocalDataSet(owner, CONTRAST_SET_ID) { + } + + ContrastSet(object_id_t objectId): + StaticLocalDataSet(sid_t(objectId, CONTRAST_SET_ID)) { + } + + lp_var_t ticks = lp_var_t(sid.objectId, PoolIds::TICKS_CONTRAST_SET, this); + lp_var_t time = lp_var_t(sid.objectId, PoolIds::TIME_CONTRAST_SET, this); + lp_var_t binA0 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BINA0, this); + lp_var_t binA1 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BINA1, this); + lp_var_t binA2 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BINA2, this); + lp_var_t binA3 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BINA3, this); + lp_var_t binA4 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BINA4, this); + lp_var_t binA5 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BINA5, this); + lp_var_t binA6 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BINA6, this); + lp_var_t binA7 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BINA7, this); + lp_var_t binA8 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BINA8, this); + lp_var_t binb0 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BINB0, this); + lp_var_t binB1 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BINB1, this); + lp_var_t binB2 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BINB2, this); + lp_var_t binB3 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BINB3, this); + lp_var_t binB4 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BINB4, this); + lp_var_t binB5 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BINB5, this); + lp_var_t binB6 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BINB6, this); + lp_var_t binB7 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BINB7, this); + lp_var_t binB8 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BINB8, this); + lp_var_t binC0 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BINC0, this); + lp_var_t binC1 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BINC1, this); + lp_var_t binC2 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BINC2, this); + lp_var_t binC3 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BINC3, this); + lp_var_t binC4 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BINC4, this); + lp_var_t binC5 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BINC5, this); + lp_var_t binC6 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BINC6, this); + lp_var_t binC7 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BINC7, this); + lp_var_t binC8 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BINC8, this); + lp_var_t binD0 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BIND0, this); + lp_var_t binD1 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BIND1, this); + lp_var_t binD2 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BIND2, this); + lp_var_t binD3 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BIND3, this); + lp_var_t binD4 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BIND4, this); + lp_var_t binD5 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BIND5, this); + lp_var_t binD6 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BIND6, this); + lp_var_t binD7 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BIND7, this); + lp_var_t binD8 = lp_var_t(sid.objectId, PoolIds::CONTRAST_BIND8, this); + + void printSet() { + PoolReadGuard rg(this); + sif::info << "ContrastSet::printSet: Ticks: " << this->ticks << std::endl; + sif::info << "ContrastSet::printSet: Time (time stamp): " << this->time << " us" + << std::endl; + sif::info << "ContrastSet::printSet: BinA0: " << this->binA0 << std::endl; + sif::info << "ContrastSet::printSet: BinA1: " << this->binA1 << std::endl; + sif::info << "ContrastSet::printSet: BinA2: " << this->binA2 << std::endl; + sif::info << "ContrastSet::printSet: BinA3: " << this->binA3 << std::endl; + sif::info << "ContrastSet::printSet: BinA4: " << this->binA4 << std::endl; + sif::info << "ContrastSet::printSet: BinA5: " << this->binA5 << std::endl; + sif::info << "ContrastSet::printSet: BinA6: " << this->binA6 << std::endl; + sif::info << "ContrastSet::printSet: BinA7: " << this->binA7 << std::endl; + sif::info << "ContrastSet::printSet: BinA8: " << this->binA8 << std::endl; + sif::info << "ContrastSet::printSet: BinB0: " << this->binA0 << std::endl; + sif::info << "ContrastSet::printSet: BinB1: " << this->binB1 << std::endl; + sif::info << "ContrastSet::printSet: BinB2: " << this->binB2 << std::endl; + sif::info << "ContrastSet::printSet: BinB3: " << this->binB3 << std::endl; + sif::info << "ContrastSet::printSet: BinB4: " << this->binB4 << std::endl; + sif::info << "ContrastSet::printSet: BinB5: " << this->binB5 << std::endl; + sif::info << "ContrastSet::printSet: BinB6: " << this->binB6 << std::endl; + sif::info << "ContrastSet::printSet: BinB7: " << this->binB7 << std::endl; + sif::info << "ContrastSet::printSet: BinB8: " << this->binB8 << std::endl; + sif::info << "ContrastSet::printSet: BinC0: " << this->binC0 << std::endl; + sif::info << "ContrastSet::printSet: BinC1: " << this->binC1 << std::endl; + sif::info << "ContrastSet::printSet: BinC2: " << this->binC2 << std::endl; + sif::info << "ContrastSet::printSet: BinC3: " << this->binC3 << std::endl; + sif::info << "ContrastSet::printSet: BinC4: " << this->binC4 << std::endl; + sif::info << "ContrastSet::printSet: BinC5: " << this->binC5 << std::endl; + sif::info << "ContrastSet::printSet: BinC6: " << this->binC6 << std::endl; + sif::info << "ContrastSet::printSet: BinC7: " << this->binC7 << std::endl; + sif::info << "ContrastSet::printSet: BinC8: " << this->binC8 << std::endl; + sif::info << "ContrastSet::printSet: BinD0: " << this->binD0 << std::endl; + sif::info << "ContrastSet::printSet: BinD1: " << this->binD1 << std::endl; + sif::info << "ContrastSet::printSet: BinD2: " << this->binD2 << std::endl; + sif::info << "ContrastSet::printSet: BinD3: " << this->binD3 << std::endl; + sif::info << "ContrastSet::printSet: BinD4: " << this->binD4 << std::endl; + sif::info << "ContrastSet::printSet: BinD5: " << this->binD5 << std::endl; + sif::info << "ContrastSet::printSet: BinD6: " << this->binD6 << std::endl; + sif::info << "ContrastSet::printSet: BinD7: " << this->binD7 << std::endl; + sif::info << "ContrastSet::printSet: BinD8: " << this->binD8 << std::endl; + } +}; + } #endif /* MISSION_STARTRACKER_DEFINITIONS_H_ */ diff --git a/bsp_q7s/devices/startracker/StarTrackerHandler.cpp b/bsp_q7s/devices/startracker/StarTrackerHandler.cpp index f957f9fc..f1e029dc 100644 --- a/bsp_q7s/devices/startracker/StarTrackerHandler.cpp +++ b/bsp_q7s/devices/startracker/StarTrackerHandler.cpp @@ -16,8 +16,8 @@ extern "C" { StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie, StrImageLoader* strImageLoader) : DeviceHandlerBase(objectId, comIF, comCookie), temperatureSet(this), versionSet(this), powerSet( - this), interfaceSet(this), timeSet(this), solutionSet(this), strImageLoader( - strImageLoader) { + this), interfaceSet(this), timeSet(this), solutionSet(this), histogramSet(this), + contrastSet(this), strImageLoader(strImageLoader) { if (comCookie == nullptr) { sif::error << "StarTrackerHandler: Invalid com cookie" << std::endl; } @@ -310,6 +310,14 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi prepareTemperatureRequest(); return RETURN_OK; } + case (StarTracker::REQ_HISTOGRAM): { + prepareHistogramRequest(); + return RETURN_OK; + } + case (StarTracker::REQ_CONTRAST): { + prepareContrastRequest(); + return RETURN_OK; + } case (StarTracker::RESET_ERROR): { prepareErrorResetRequest(); return RETURN_OK; @@ -394,6 +402,10 @@ void StarTrackerHandler::fillCommandAndReplyMap() { StarTracker::MAX_FRAME_SIZE * 2 + 2); this->insertInCommandAndReplyMap(StarTracker::REQ_TEMPERATURE, 3, &temperatureSet, StarTracker::MAX_FRAME_SIZE * 2 + 2); + this->insertInCommandAndReplyMap(StarTracker::REQ_HISTOGRAM, 3, &histogramSet, + StarTracker::MAX_FRAME_SIZE * 2 + 2); + this->insertInCommandAndReplyMap(StarTracker::REQ_CONTRAST, 3, &contrastSet, + StarTracker::MAX_FRAME_SIZE * 2 + 2); this->insertInCommandAndReplyMap(StarTracker::LIMITS, 3, nullptr, StarTracker::MAX_FRAME_SIZE * 2 + 2); this->insertInCommandAndReplyMap(StarTracker::MOUNTING, 3, nullptr, @@ -525,6 +537,14 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, con handleTemperatureTm(); break; } + case (StarTracker::REQ_HISTOGRAM): { + handleHistogramTm(); + break; + } + case (StarTracker::REQ_CONTRAST): { + handleContrastTm(); + break; + } case (StarTracker::LIMITS): case (StarTracker::MOUNTING): case (StarTracker::CAMERA): @@ -554,7 +574,7 @@ void StarTrackerHandler::setNormalDatapoolEntriesInvalid() { } uint32_t StarTrackerHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { - return 5000; + return 20000; } ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, @@ -632,6 +652,45 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l localDataPoolMap.emplace(StarTracker::STABLE_COUNT, new PoolEntry( { 0 })); localDataPoolMap.emplace(StarTracker::SOLUTION_STRATEGY, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::TICKS_HISTOGRAM_SET, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::TIME_HISTOGRAM_SET, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINA0, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINA1, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINA2, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINA3, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINA4, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINA5, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINA6, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINA7, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINA8, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINB0, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINB1, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINB2, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINB3, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINB4, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINB5, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINB6, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINB7, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINB8, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINC0, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINC1, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINC2, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINC3, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINC4, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINC5, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINC6, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINC7, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINC8, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BIND0, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BIND1, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BIND2, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BIND3, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BIND4, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BIND5, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BIND6, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BIND7, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::HISTOGRAM_BIND8, new PoolEntry( { 0 })); + return RETURN_OK; } @@ -764,6 +823,14 @@ ReturnValue_t StarTrackerHandler::scanForTmReply(DeviceCommandId_t *foundId) { *foundId = StarTracker::REQ_TEMPERATURE; break; } + case (StarTracker::ID::HISTOGRAM): { + *foundId = StarTracker::REQ_HISTOGRAM; + break; + } + case (StarTracker::ID::CONTRAST): { + *foundId = StarTracker::REQ_CONTRAST; + break; + } case (StarTracker::ID::TIME): { *foundId = StarTracker::REQ_TIME; break; @@ -909,6 +976,22 @@ void StarTrackerHandler::prepareTemperatureRequest() { rawPacketLen = dataLinkLayer.getEncodedLength(); } +void StarTrackerHandler::prepareHistogramRequest() { + uint32_t length = 0; + arc_tm_pack_histogram_req(commandBuffer, &length); + dataLinkLayer.encodeFrame(commandBuffer, length); + rawPacket = dataLinkLayer.getEncodedFrame(); + rawPacketLen = dataLinkLayer.getEncodedLength(); +} + +void StarTrackerHandler::prepareContrastRequest() { + uint32_t length = 0; + arc_tm_pack_contrast_req(commandBuffer, &length); + dataLinkLayer.encodeFrame(commandBuffer, length); + rawPacket = dataLinkLayer.getEncodedFrame(); + rawPacketLen = dataLinkLayer.getEncodedLength(); +} + void StarTrackerHandler::prepareErrorResetRequest() { uint32_t length = 0; struct ResetErrorSignalActionRequest req; @@ -1319,6 +1402,64 @@ ReturnValue_t StarTrackerHandler::handleTemperatureTm() { 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; + result = HISTOGRAM_REQ_FAILED; + return result; + } + 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; + result = histogramSet.deSerialize(&reply, &size, SerializeIF::Endianness::LITTLE); + if (result != RETURN_OK) { + sif::warning << "StarTrackerHandler::handleHistogramTm: Deserialization failed" + << std::endl; + } +#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 + histogramSet.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); diff --git a/bsp_q7s/devices/startracker/StarTrackerHandler.h b/bsp_q7s/devices/startracker/StarTrackerHandler.h index e22ff739..0fcf14d0 100644 --- a/bsp_q7s/devices/startracker/StarTrackerHandler.h +++ b/bsp_q7s/devices/startracker/StarTrackerHandler.h @@ -111,6 +111,10 @@ 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 contrast reply signals error + static const ReturnValue_t CONTRAST_REQ_FAILED = MAKE_RETURN_CODE(0xAE); static const size_t MAX_PATH_SIZE = 50; static const size_t MAX_FILE_NAME = 30; @@ -143,6 +147,8 @@ private: StarTracker::InterfaceSet interfaceSet; StarTracker::TimeSet timeSet; StarTracker::SolutionSet solutionSet; + StarTracker::HistogramSet histogramSet; + StarTracker::ContrastSet contrastSet; // Pointer to object responsible for uploading and downloading images to/from the star tracker StrImageLoader* strImageLoader = nullptr; @@ -268,6 +274,13 @@ private: */ void prepareTemperatureRequest(); + /** + * @brief Fills command buffer with data to request histogram + */ + void prepareHistogramRequest(); + + void prepareContrastRequest(); + /** * @brief Fills command buffer with command to reset the error signal of the star tracker */ @@ -313,6 +326,16 @@ private: */ 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. */ diff --git a/fsfw b/fsfw index ceb87b5a..9a858eb5 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit ceb87b5abb2992a18266328e0ea34d9af15db7af +Subproject commit 9a858eb54c5603ccd3cbd2423cdaf87b20f0c0c4 From 2ed0bdf1323dae2d4320f50fcb84b6392b5f3867 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Fri, 17 Dec 2021 10:45:55 +0100 Subject: [PATCH 2/6] acu print stats --- .../startracker/StarTrackerDefinitions.h | 2 +- .../startracker/StarTrackerHandler.cpp | 7 +++- mission/devices/ACUHandler.cpp | 41 +++++++++++++++++++ mission/devices/ACUHandler.h | 10 +++++ mission/devices/GomspaceDeviceHandler.cpp | 6 ++- mission/devices/GomspaceDeviceHandler.h | 5 +++ .../devicedefinitions/GomspaceDefinitions.h | 5 ++- 7 files changed, 71 insertions(+), 5 deletions(-) diff --git a/bsp_q7s/devices/startracker/StarTrackerDefinitions.h b/bsp_q7s/devices/startracker/StarTrackerDefinitions.h index 4ff30ec0..18fe38d7 100644 --- a/bsp_q7s/devices/startracker/StarTrackerDefinitions.h +++ b/bsp_q7s/devices/startracker/StarTrackerDefinitions.h @@ -639,7 +639,7 @@ class HistogramSet: public: // Size of dataset - static const size_t SIZE = 154; + static const size_t SIZE = 156; HistogramSet(HasLocalDataPoolIF* owner): StaticLocalDataSet(owner, HISTOGRAM_SET_ID) { diff --git a/bsp_q7s/devices/startracker/StarTrackerHandler.cpp b/bsp_q7s/devices/startracker/StarTrackerHandler.cpp index f1e029dc..16960f76 100644 --- a/bsp_q7s/devices/startracker/StarTrackerHandler.cpp +++ b/bsp_q7s/devices/startracker/StarTrackerHandler.cpp @@ -1411,8 +1411,7 @@ ReturnValue_t StarTrackerHandler::handleHistogramTm() { if(status != StarTracker::STATUS_OK) { sif::warning << "StarTrackerHandler::handleHistogramTm: Reply error: " << static_cast(status) << std::endl; - result = HISTOGRAM_REQ_FAILED; - return result; + return HISTOGRAM_REQ_FAILED; } result = histogramSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT); if (result != RETURN_OK) { @@ -1425,6 +1424,10 @@ ReturnValue_t StarTrackerHandler::handleHistogramTm() { sif::warning << "StarTrackerHandler::handleHistogramTm: Deserialization failed" << std::endl; } + result = histogramSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT); + if (result != RETURN_OK) { + return result; + } #if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 histogramSet.printSet(); #endif diff --git a/mission/devices/ACUHandler.cpp b/mission/devices/ACUHandler.cpp index 24bd52fe..a52e48ff 100644 --- a/mission/devices/ACUHandler.cpp +++ b/mission/devices/ACUHandler.cpp @@ -266,3 +266,44 @@ ReturnValue_t ACUHandler::initializeLocalDataPool( return HasReturnvaluesIF::RETURN_OK; } + +ReturnValue_t ACUHandler::deviceSpecificCommand(DeviceCommandId_t cmd) { + switch(cmd) { + case PRINT_CHANNEL_STATS: { + printChannelStats(); + return RETURN_OK; + } + default: { + return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; + } + } +} + +void ACUHandler::printChannelStats() { + PoolReadGuard pg(&acuHkTableDataset); + sif::info << "ACU Info: Stats, Current [mA], Voltage [mV]" << std::endl; + sif::info << std::setw(30) << std::left << "Ch0" << std::dec << "| " << + static_cast(acuHkTableDataset.currentInChannel0.value) << ", " << + std::setw(4) << std::right << acuHkTableDataset.voltageInChannel0.value << ", " + << std::setw(4) << std::endl; + sif::info << std::setw(30) << std::left << "Ch1" << std::dec << "| " << + static_cast(acuHkTableDataset.currentInChannel1.value) << ", " << + std::setw(4) << std::right << acuHkTableDataset.voltageInChannel1.value << ", " + << std::setw(4) << std::endl; + sif::info << std::setw(30) << std::left << "Ch2" << std::dec << "| " << + static_cast(acuHkTableDataset.currentInChannel2.value) << ", " << + std::setw(4) << std::right << acuHkTableDataset.voltageInChannel2.value << ", " + << std::setw(4) << std::endl; + sif::info << std::setw(30) << std::left << "Ch3" << std::dec << "| " << + static_cast(acuHkTableDataset.currentInChannel3.value) << ", " << + std::setw(4) << std::right << acuHkTableDataset.voltageInChannel3.value << ", " + << std::setw(4) << std::endl; + sif::info << std::setw(30) << std::left << "Ch4" << std::dec << "| " << + static_cast(acuHkTableDataset.currentInChannel4.value) << ", " << + std::setw(4) << std::right << acuHkTableDataset.voltageInChannel4.value << ", " + << std::setw(4) << std::endl; + sif::info << std::setw(30) << std::left << "Ch5" << std::dec << "| " << + static_cast(acuHkTableDataset.currentInChannel5.value) << ", " << + std::setw(4) << std::right << acuHkTableDataset.voltageInChannel5.value << ", " + << std::setw(4) << std::endl; +} diff --git a/mission/devices/ACUHandler.h b/mission/devices/ACUHandler.h index 8ab32a24..886faba7 100644 --- a/mission/devices/ACUHandler.h +++ b/mission/devices/ACUHandler.h @@ -3,6 +3,7 @@ #include "GomspaceDeviceHandler.h" #include +#include "fsfw/datapool/PoolReadGuard.h" /** * @brief Handler for the ACU from Gomspace. Monitors and controls the battery charging via @@ -25,8 +26,12 @@ protected: */ virtual ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t * id) override; + virtual ReturnValue_t deviceSpecificCommand(DeviceCommandId_t cmd) override; + private: + static const DeviceCommandId_t PRINT_CHANNEL_STATS = 51; + ACU::HkTableDataset acuHkTableDataset; /** @@ -34,6 +39,11 @@ private: * the values in the acuHkTableDataset. */ void parseHkTableReply(const uint8_t *packet); + + /** + * @brief Prints channel statistics (current and voltage) to console + */ + void printChannelStats(); }; #endif /* MISSION_DEVICES_ACUHANDLER_H_ */ diff --git a/mission/devices/GomspaceDeviceHandler.cpp b/mission/devices/GomspaceDeviceHandler.cpp index e5912969..95e35a5e 100644 --- a/mission/devices/GomspaceDeviceHandler.cpp +++ b/mission/devices/GomspaceDeviceHandler.cpp @@ -87,7 +87,7 @@ ReturnValue_t GomspaceDeviceHandler::buildCommandFromCommand( break; } default: - return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; + return deviceSpecificCommand(deviceCommand); } return HasReturnvaluesIF::RETURN_OK; } @@ -398,6 +398,10 @@ LocalPoolDataSetBase* GomspaceDeviceHandler::getDataSetHandle(sid_t sid) { } } +ReturnValue_t GomspaceDeviceHandler::deviceSpecificCommand(DeviceCommandId_t cmd) { + return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; +} + void GomspaceDeviceHandler::setModeNormal() { mode = MODE_NORMAL; } diff --git a/mission/devices/GomspaceDeviceHandler.h b/mission/devices/GomspaceDeviceHandler.h index 78257b83..bb11ba0e 100644 --- a/mission/devices/GomspaceDeviceHandler.h +++ b/mission/devices/GomspaceDeviceHandler.h @@ -105,6 +105,11 @@ protected: virtual LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override; + /** + * @brief Can be used by gomspace devices to implement device specific commands. + */ + virtual ReturnValue_t deviceSpecificCommand(DeviceCommandId_t cmd); + private: /** diff --git a/mission/devices/devicedefinitions/GomspaceDefinitions.h b/mission/devices/devicedefinitions/GomspaceDefinitions.h index bd509239..0fbbc5b0 100644 --- a/mission/devices/devicedefinitions/GomspaceDefinitions.h +++ b/mission/devices/devicedefinitions/GomspaceDefinitions.h @@ -25,7 +25,10 @@ static const uint8_t REBOOT_PORT = 4; static const uint8_t PARAM_PORT = 7; static const uint8_t P60_PORT_GNDWDT_RESET = 9; -/* Device commands are derived from the rparam.h of the gomspace lib */ +/** + * Device commands are derived from the rparam.h of the gomspace lib.. + * IDs above 50 are reserved for device specific commands. + */ static const DeviceCommandId_t PING = 1; //!< [EXPORT] : [COMMAND] static const DeviceCommandId_t NONE = 2; // Set when no command is pending static const DeviceCommandId_t REBOOT = 4; //!< [EXPORT] : [COMMAND] From 2dd8fb4f4531270da10e61b9a2703c7c0ac9ace1 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Fri, 17 Dec 2021 15:08:33 +0100 Subject: [PATCH 3/6] acu command to print current and voltage of ACU channels --- mission/devices/ACUHandler.cpp | 37 ++++++++++++------------- mission/devices/ACUHandler.h | 2 ++ mission/devices/GomspaceDeviceHandler.h | 2 +- 3 files changed, 21 insertions(+), 20 deletions(-) diff --git a/mission/devices/ACUHandler.cpp b/mission/devices/ACUHandler.cpp index a52e48ff..043942fe 100644 --- a/mission/devices/ACUHandler.cpp +++ b/mission/devices/ACUHandler.cpp @@ -16,6 +16,11 @@ ReturnValue_t ACUHandler::buildNormalDeviceCommand( return buildCommandFromCommand(*id, NULL, 0); } +void ACUHandler::fillCommandAndReplyMap() { + GomspaceDeviceHandler::fillCommandAndReplyMap(); + this->insertInCommandMap(PRINT_CHANNEL_STATS); +} + void ACUHandler::letChildHandleHkReply(DeviceCommandId_t id, const uint8_t *packet) { parseHkTableReply(packet); @@ -281,29 +286,23 @@ ReturnValue_t ACUHandler::deviceSpecificCommand(DeviceCommandId_t cmd) { void ACUHandler::printChannelStats() { PoolReadGuard pg(&acuHkTableDataset); - sif::info << "ACU Info: Stats, Current [mA], Voltage [mV]" << std::endl; - sif::info << std::setw(30) << std::left << "Ch0" << std::dec << "| " << + sif::info << "ACU Info: Current [mA], Voltage [mV]" << std::endl; + sif::info << std::setw(8) << std::left << "Ch0" << std::dec << "| " << static_cast(acuHkTableDataset.currentInChannel0.value) << ", " << - std::setw(4) << std::right << acuHkTableDataset.voltageInChannel0.value << ", " - << std::setw(4) << std::endl; - sif::info << std::setw(30) << std::left << "Ch1" << std::dec << "| " << + std::setw(13) << std::right << acuHkTableDataset.voltageInChannel0.value << std::endl; + sif::info << std::setw(8) << std::left << "Ch1" << std::dec << "| " << static_cast(acuHkTableDataset.currentInChannel1.value) << ", " << - std::setw(4) << std::right << acuHkTableDataset.voltageInChannel1.value << ", " - << std::setw(4) << std::endl; - sif::info << std::setw(30) << std::left << "Ch2" << std::dec << "| " << + std::setw(13) << std::right << acuHkTableDataset.voltageInChannel1.value << std::endl; + sif::info << std::setw(8) << std::left << "Ch2" << std::dec << "| " << static_cast(acuHkTableDataset.currentInChannel2.value) << ", " << - std::setw(4) << std::right << acuHkTableDataset.voltageInChannel2.value << ", " - << std::setw(4) << std::endl; - sif::info << std::setw(30) << std::left << "Ch3" << std::dec << "| " << + std::setw(13) << std::right << acuHkTableDataset.voltageInChannel2.value << std::endl; + sif::info << std::setw(8) << std::left << "Ch3" << std::dec << "| " << static_cast(acuHkTableDataset.currentInChannel3.value) << ", " << - std::setw(4) << std::right << acuHkTableDataset.voltageInChannel3.value << ", " - << std::setw(4) << std::endl; - sif::info << std::setw(30) << std::left << "Ch4" << std::dec << "| " << + std::setw(13) << std::right << acuHkTableDataset.voltageInChannel3.value << std::endl; + sif::info << std::setw(8) << std::left << "Ch4" << std::dec << "| " << static_cast(acuHkTableDataset.currentInChannel4.value) << ", " << - std::setw(4) << std::right << acuHkTableDataset.voltageInChannel4.value << ", " - << std::setw(4) << std::endl; - sif::info << std::setw(30) << std::left << "Ch5" << std::dec << "| " << + std::setw(13) << std::right << acuHkTableDataset.voltageInChannel4.value << std::endl; + sif::info << std::setw(8) << std::left << "Ch5" << std::dec << "| " << static_cast(acuHkTableDataset.currentInChannel5.value) << ", " << - std::setw(4) << std::right << acuHkTableDataset.voltageInChannel5.value << ", " - << std::setw(4) << std::endl; + std::setw(13) << std::right << acuHkTableDataset.voltageInChannel5.value << std::endl; } diff --git a/mission/devices/ACUHandler.h b/mission/devices/ACUHandler.h index 886faba7..2bb688e7 100644 --- a/mission/devices/ACUHandler.h +++ b/mission/devices/ACUHandler.h @@ -26,6 +26,8 @@ protected: */ virtual ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t * id) override; + virtual void fillCommandAndReplyMap() override; + virtual ReturnValue_t deviceSpecificCommand(DeviceCommandId_t cmd) override; private: diff --git a/mission/devices/GomspaceDeviceHandler.h b/mission/devices/GomspaceDeviceHandler.h index bb11ba0e..29156fce 100644 --- a/mission/devices/GomspaceDeviceHandler.h +++ b/mission/devices/GomspaceDeviceHandler.h @@ -73,7 +73,7 @@ protected: virtual ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t * id) override; ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t * id) override; - void fillCommandAndReplyMap() override; + virtual void fillCommandAndReplyMap() override; ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t * commandData,size_t commandDataLen) override; ReturnValue_t scanForReply(const uint8_t *start, size_t remainingSize, From 3163995d06093aeb4bd386ef97f5f09be14c2920 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Fri, 17 Dec 2021 15:24:23 +0100 Subject: [PATCH 4/6] improvements of printout --- mission/devices/ACUHandler.cpp | 24 ++++++++++++------------ tmtc | 2 +- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/mission/devices/ACUHandler.cpp b/mission/devices/ACUHandler.cpp index 043942fe..955b1703 100644 --- a/mission/devices/ACUHandler.cpp +++ b/mission/devices/ACUHandler.cpp @@ -288,21 +288,21 @@ void ACUHandler::printChannelStats() { PoolReadGuard pg(&acuHkTableDataset); sif::info << "ACU Info: Current [mA], Voltage [mV]" << std::endl; sif::info << std::setw(8) << std::left << "Ch0" << std::dec << "| " << - static_cast(acuHkTableDataset.currentInChannel0.value) << ", " << - std::setw(13) << std::right << acuHkTableDataset.voltageInChannel0.value << std::endl; + static_cast(acuHkTableDataset.currentInChannel0.value) << + std::setw(15) << std::right << acuHkTableDataset.voltageInChannel0.value << std::endl; sif::info << std::setw(8) << std::left << "Ch1" << std::dec << "| " << - static_cast(acuHkTableDataset.currentInChannel1.value) << ", " << - std::setw(13) << std::right << acuHkTableDataset.voltageInChannel1.value << std::endl; + static_cast(acuHkTableDataset.currentInChannel1.value) << + std::setw(15) << std::right << acuHkTableDataset.voltageInChannel1.value << std::endl; sif::info << std::setw(8) << std::left << "Ch2" << std::dec << "| " << - static_cast(acuHkTableDataset.currentInChannel2.value) << ", " << - std::setw(13) << std::right << acuHkTableDataset.voltageInChannel2.value << std::endl; + static_cast(acuHkTableDataset.currentInChannel2.value) << + std::setw(15) << std::right << acuHkTableDataset.voltageInChannel2.value << std::endl; sif::info << std::setw(8) << std::left << "Ch3" << std::dec << "| " << - static_cast(acuHkTableDataset.currentInChannel3.value) << ", " << - std::setw(13) << std::right << acuHkTableDataset.voltageInChannel3.value << std::endl; + static_cast(acuHkTableDataset.currentInChannel3.value) << + std::setw(15) << std::right << acuHkTableDataset.voltageInChannel3.value << std::endl; sif::info << std::setw(8) << std::left << "Ch4" << std::dec << "| " << - static_cast(acuHkTableDataset.currentInChannel4.value) << ", " << - std::setw(13) << std::right << acuHkTableDataset.voltageInChannel4.value << std::endl; + static_cast(acuHkTableDataset.currentInChannel4.value) << + std::setw(15) << std::right << acuHkTableDataset.voltageInChannel4.value << std::endl; sif::info << std::setw(8) << std::left << "Ch5" << std::dec << "| " << - static_cast(acuHkTableDataset.currentInChannel5.value) << ", " << - std::setw(13) << std::right << acuHkTableDataset.voltageInChannel5.value << std::endl; + static_cast(acuHkTableDataset.currentInChannel5.value) << + std::setw(15) << std::right << acuHkTableDataset.voltageInChannel5.value << std::endl; } diff --git a/tmtc b/tmtc index 3377243c..f5613e5e 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 3377243c07ceb4c5d58d35468dedefb71e1c4aa5 +Subproject commit f5613e5e80013d2d0c33938a56a334ce2c7c9f5a From 654e75fec850a864b7777ef01020b5b5ffb9eac7 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Sat, 18 Dec 2021 11:33:29 +0100 Subject: [PATCH 5/6] histogram tm disabled validity buffer for deserialization --- bsp_q7s/devices/startracker/StarTrackerDefinitions.h | 4 ++-- bsp_q7s/devices/startracker/StarTrackerHandler.cpp | 2 ++ 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/bsp_q7s/devices/startracker/StarTrackerDefinitions.h b/bsp_q7s/devices/startracker/StarTrackerDefinitions.h index 18fe38d7..9fdd4aca 100644 --- a/bsp_q7s/devices/startracker/StarTrackerDefinitions.h +++ b/bsp_q7s/devices/startracker/StarTrackerDefinitions.h @@ -660,7 +660,7 @@ public: lp_var_t binA6 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINA6, this); lp_var_t binA7 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINA7, this); lp_var_t binA8 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINA8, this); - lp_var_t binb0 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINB0, this); + lp_var_t binB0 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINB0, this); lp_var_t binB1 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINB1, this); lp_var_t binB2 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINB2, this); lp_var_t binB3 = lp_var_t(sid.objectId, PoolIds::HISTOGRAM_BINB3, this); @@ -702,7 +702,7 @@ public: sif::info << "HistogramSet::printSet: BinA6: " << this->binA6 << std::endl; sif::info << "HistogramSet::printSet: BinA7: " << this->binA7 << std::endl; sif::info << "HistogramSet::printSet: BinA8: " << this->binA8 << std::endl; - sif::info << "HistogramSet::printSet: BinB0: " << this->binA0 << std::endl; + sif::info << "HistogramSet::printSet: BinB0: " << this->binB0 << std::endl; sif::info << "HistogramSet::printSet: BinB1: " << this->binB1 << std::endl; sif::info << "HistogramSet::printSet: BinB2: " << this->binB2 << std::endl; sif::info << "HistogramSet::printSet: BinB3: " << this->binB3 << std::endl; diff --git a/bsp_q7s/devices/startracker/StarTrackerHandler.cpp b/bsp_q7s/devices/startracker/StarTrackerHandler.cpp index 16960f76..cf02a2c7 100644 --- a/bsp_q7s/devices/startracker/StarTrackerHandler.cpp +++ b/bsp_q7s/devices/startracker/StarTrackerHandler.cpp @@ -1419,7 +1419,9 @@ ReturnValue_t StarTrackerHandler::handleHistogramTm() { } 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" << std::endl; From 72c0503ac175893b895c3456f61ef74465bfc0d9 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Sun, 19 Dec 2021 11:29:40 +0100 Subject: [PATCH 6/6] 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_ */