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