Merge branch 'meier/startrackerActions' into meier/startracker
This commit is contained in:
Jakob Meier
commit
428d2562ac
@ -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();
|
||||
|
||||
|
||||
@ -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;
|
||||
@ -184,6 +270,8 @@ class TemperatureSet:
|
||||
public StaticLocalDataSet<TEMPERATURE_SET_ENTRIES> {
|
||||
public:
|
||||
|
||||
static const size_t SIZE = 20;
|
||||
|
||||
TemperatureSet(HasLocalDataPoolIF* owner):
|
||||
StaticLocalDataSet(owner, TEMPERATURE_SET_ID) {
|
||||
}
|
||||
@ -222,6 +310,8 @@ class VersionSet:
|
||||
public StaticLocalDataSet<VERSION_SET_ENTRIES> {
|
||||
public:
|
||||
|
||||
static const size_t SIZE = 15;
|
||||
|
||||
VersionSet(HasLocalDataPoolIF* owner):
|
||||
StaticLocalDataSet(owner, VERSION_SET_ID) {
|
||||
}
|
||||
@ -265,6 +355,8 @@ class InterfaceSet:
|
||||
public StaticLocalDataSet<INTERFACE_SET_ENTRIES> {
|
||||
public:
|
||||
|
||||
static const size_t SIZE = 20;
|
||||
|
||||
InterfaceSet(HasLocalDataPoolIF* owner):
|
||||
StaticLocalDataSet(owner, REQ_INTERFACE) {
|
||||
}
|
||||
@ -304,6 +396,8 @@ class PowerSet:
|
||||
public StaticLocalDataSet<POWER_SET_ENTRIES> {
|
||||
public:
|
||||
|
||||
static const size_t SIZE = 76;
|
||||
|
||||
PowerSet(HasLocalDataPoolIF* owner):
|
||||
StaticLocalDataSet(owner, REQ_INTERFACE) {
|
||||
}
|
||||
@ -399,6 +493,8 @@ class TimeSet:
|
||||
public StaticLocalDataSet<TIME_SET_ENTRIES> {
|
||||
public:
|
||||
|
||||
static const size_t SIZE = 24;
|
||||
|
||||
TimeSet(HasLocalDataPoolIF* owner):
|
||||
StaticLocalDataSet(owner, TIME_SET_ID) {
|
||||
}
|
||||
@ -439,6 +535,8 @@ class SolutionSet:
|
||||
public StaticLocalDataSet<SOLUTION_SET_ENTRIES> {
|
||||
public:
|
||||
|
||||
static const size_t SIZE = 78;
|
||||
|
||||
SolutionSet(HasLocalDataPoolIF* owner):
|
||||
StaticLocalDataSet(owner, SOLUTION_SET_ID) {
|
||||
}
|
||||
@ -455,60 +553,44 @@ public:
|
||||
PoolIds::TIME_SOLUTION_SET, this);
|
||||
// Calibrated quaternion (takes into account the mounting quaternion), typically same as
|
||||
// track q values
|
||||
lp_var_t<float> caliQw = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::CALI_QW, this);
|
||||
lp_var_t<float> caliQx = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::CALI_QX, this);
|
||||
lp_var_t<float> caliQy = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::CALI_QY, this);
|
||||
lp_var_t<float> caliQz = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::CALI_QZ, this);
|
||||
lp_var_t<float> caliQw = lp_var_t<float>(sid.objectId, PoolIds::CALI_QW, this);
|
||||
lp_var_t<float> caliQx = lp_var_t<float>(sid.objectId, PoolIds::CALI_QX, this);
|
||||
lp_var_t<float> caliQy = lp_var_t<float>(sid.objectId, PoolIds::CALI_QY, this);
|
||||
lp_var_t<float> caliQz = lp_var_t<float>(sid.objectId, PoolIds::CALI_QZ, this);
|
||||
// The lower this value the more confidence that the star tracker solution is correct
|
||||
lp_var_t<float> trackConfidence = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::TRACK_CONFIDENCE, this);
|
||||
lp_var_t<float> trackConfidence = lp_var_t<float>(sid.objectId, PoolIds::TRACK_CONFIDENCE,
|
||||
this);
|
||||
// Estimated attitude of spacecraft
|
||||
lp_var_t<float> trackQw = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::TRACK_QW, this);
|
||||
lp_var_t<float> trackQx = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::TRACK_QX, this);
|
||||
lp_var_t<float> trackQy = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::TRACK_QY, this);
|
||||
lp_var_t<float> trackQz = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::TRACK_QZ, this);
|
||||
lp_var_t<float> trackQw = lp_var_t<float>(sid.objectId, PoolIds::TRACK_QW, this);
|
||||
lp_var_t<float> trackQx = lp_var_t<float>(sid.objectId, PoolIds::TRACK_QX, this);
|
||||
lp_var_t<float> trackQy = lp_var_t<float>(sid.objectId, PoolIds::TRACK_QY, this);
|
||||
lp_var_t<float> trackQz = lp_var_t<float>(sid.objectId, PoolIds::TRACK_QZ, this);
|
||||
// Number of stars removed from tracking solution
|
||||
lp_var_t<uint8_t> trackRemoved = lp_var_t<uint8_t>(sid.objectId,
|
||||
PoolIds::TRACK_REMOVED, this);
|
||||
lp_var_t<uint8_t> trackRemoved = lp_var_t<uint8_t>(sid.objectId, PoolIds::TRACK_REMOVED, this);
|
||||
// Number of stars for which a valid centroid was found
|
||||
lp_var_t<uint8_t> starsCentroided = lp_var_t<uint8_t>(sid.objectId,
|
||||
PoolIds::STARS_CENTROIDED, this);
|
||||
lp_var_t<uint8_t> starsCentroided = lp_var_t<uint8_t>(sid.objectId, PoolIds::STARS_CENTROIDED,
|
||||
this);
|
||||
// Number of stars that matched to a database star
|
||||
lp_var_t<uint8_t> starsMatchedDatabase = lp_var_t<uint8_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<float> lisaQw = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::LISA_QW, this);
|
||||
lp_var_t<float> lisaQx = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::LISA_QX, this);
|
||||
lp_var_t<float> lisaQy = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::LISA_QY, this);
|
||||
lp_var_t<float> lisaQz = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::LISA_QZ, this);
|
||||
lp_var_t<float> lisaQw = lp_var_t<float>(sid.objectId, PoolIds::LISA_QW, this);
|
||||
lp_var_t<float> lisaQx = lp_var_t<float>(sid.objectId, PoolIds::LISA_QX, this);
|
||||
lp_var_t<float> lisaQy = lp_var_t<float>(sid.objectId, PoolIds::LISA_QY, this);
|
||||
lp_var_t<float> lisaQz = lp_var_t<float>(sid.objectId, PoolIds::LISA_QZ, this);
|
||||
// Percentage of close stars in LISA solution
|
||||
lp_var_t<float> lisaPercentageClose = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::LISA_PERC_CLOSE, this);
|
||||
lp_var_t<float> lisaPercentageClose = lp_var_t<float>(sid.objectId, PoolIds::LISA_PERC_CLOSE,
|
||||
this);
|
||||
// Number of close stars in LISA solution
|
||||
lp_var_t<uint8_t> lisaNrClose = lp_var_t<uint8_t>(sid.objectId,
|
||||
PoolIds::LISA_NR_CLOSE, this);
|
||||
lp_var_t<uint8_t> lisaNrClose = lp_var_t<uint8_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<uint8_t> isTrustWorthy = lp_var_t<uint8_t>(sid.objectId,
|
||||
PoolIds::TRUST_WORTHY, this);
|
||||
lp_var_t<uint8_t> isTrustWorthy = lp_var_t<uint8_t>(sid.objectId, PoolIds::TRUST_WORTHY, this);
|
||||
// Number of times the validation criteria was met
|
||||
lp_var_t<uint32_t> stableCount = lp_var_t<uint32_t>(sid.objectId,
|
||||
PoolIds::STABLE_COUNT, this);
|
||||
lp_var_t<uint32_t> stableCount = lp_var_t<uint32_t>(sid.objectId, PoolIds::STABLE_COUNT, this);
|
||||
// Shows the autonomous mode used to obtain the star tracker attitude
|
||||
lp_var_t<uint8_t> solutionStrategy = lp_var_t<uint8_t>(sid.objectId,
|
||||
PoolIds::SOLUTION_STRATEGY, this);
|
||||
lp_var_t<uint8_t> solutionStrategy = lp_var_t<uint8_t>(sid.objectId, PoolIds::SOLUTION_STRATEGY,
|
||||
this);
|
||||
|
||||
void printSet() {
|
||||
PoolReadGuard rg(this);
|
||||
@ -561,6 +643,208 @@ public:
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Dataset to store the histogram
|
||||
*/
|
||||
class HistogramSet:
|
||||
public StaticLocalDataSet<HISTOGRAM_SET_ENTRIES> {
|
||||
public:
|
||||
|
||||
// Size of dataset
|
||||
static const size_t SIZE = 156;
|
||||
|
||||
HistogramSet(HasLocalDataPoolIF* owner):
|
||||
StaticLocalDataSet(owner, HISTOGRAM_SET_ID) {
|
||||
}
|
||||
|
||||
HistogramSet(object_id_t objectId):
|
||||
StaticLocalDataSet(sid_t(objectId, HISTOGRAM_SET_ID)) {
|
||||
}
|
||||
|
||||
lp_var_t<uint32_t> ticks = lp_var_t<uint32_t>(sid.objectId, PoolIds::TICKS_HISTOGRAM_SET, this);
|
||||
lp_var_t<uint64_t> time = lp_var_t<uint64_t>(sid.objectId, PoolIds::TIME_HISTOGRAM_SET, this);
|
||||
lp_var_t<uint32_t> binA0 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BINA0, this);
|
||||
lp_var_t<uint32_t> binA1 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BINA1, this);
|
||||
lp_var_t<uint32_t> binA2 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BINA2, this);
|
||||
lp_var_t<uint32_t> binA3 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BINA3, this);
|
||||
lp_var_t<uint32_t> binA4 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BINA4, this);
|
||||
lp_var_t<uint32_t> binA5 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BINA5, this);
|
||||
lp_var_t<uint32_t> binA6 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BINA6, this);
|
||||
lp_var_t<uint32_t> binA7 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BINA7, this);
|
||||
lp_var_t<uint32_t> binA8 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BINA8, this);
|
||||
lp_var_t<uint32_t> binB0 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BINB0, this);
|
||||
lp_var_t<uint32_t> binB1 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BINB1, this);
|
||||
lp_var_t<uint32_t> binB2 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BINB2, this);
|
||||
lp_var_t<uint32_t> binB3 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BINB3, this);
|
||||
lp_var_t<uint32_t> binB4 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BINB4, this);
|
||||
lp_var_t<uint32_t> binB5 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BINB5, this);
|
||||
lp_var_t<uint32_t> binB6 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BINB6, this);
|
||||
lp_var_t<uint32_t> binB7 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BINB7, this);
|
||||
lp_var_t<uint32_t> binB8 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BINB8, this);
|
||||
lp_var_t<uint32_t> binC0 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BINC0, this);
|
||||
lp_var_t<uint32_t> binC1 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BINC1, this);
|
||||
lp_var_t<uint32_t> binC2 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BINC2, this);
|
||||
lp_var_t<uint32_t> binC3 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BINC3, this);
|
||||
lp_var_t<uint32_t> binC4 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BINC4, this);
|
||||
lp_var_t<uint32_t> binC5 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BINC5, this);
|
||||
lp_var_t<uint32_t> binC6 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BINC6, this);
|
||||
lp_var_t<uint32_t> binC7 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BINC7, this);
|
||||
lp_var_t<uint32_t> binC8 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BINC8, this);
|
||||
lp_var_t<uint32_t> binD0 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BIND0, this);
|
||||
lp_var_t<uint32_t> binD1 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BIND1, this);
|
||||
lp_var_t<uint32_t> binD2 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BIND2, this);
|
||||
lp_var_t<uint32_t> binD3 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BIND3, this);
|
||||
lp_var_t<uint32_t> binD4 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BIND4, this);
|
||||
lp_var_t<uint32_t> binD5 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BIND5, this);
|
||||
lp_var_t<uint32_t> binD6 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BIND6, this);
|
||||
lp_var_t<uint32_t> binD7 = lp_var_t<uint32_t>(sid.objectId, PoolIds::HISTOGRAM_BIND7, this);
|
||||
lp_var_t<uint32_t> binD8 = lp_var_t<uint32_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->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;
|
||||
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<CONTRAST_SET_ENTRIES> {
|
||||
public:
|
||||
|
||||
// Size of dataset
|
||||
static const size_t SIZE = 156;
|
||||
|
||||
ContrastSet(HasLocalDataPoolIF* owner):
|
||||
StaticLocalDataSet(owner, CONTRAST_SET_ID) {
|
||||
}
|
||||
|
||||
ContrastSet(object_id_t objectId):
|
||||
StaticLocalDataSet(sid_t(objectId, CONTRAST_SET_ID)) {
|
||||
}
|
||||
|
||||
lp_var_t<uint32_t> ticks = lp_var_t<uint32_t>(sid.objectId, PoolIds::TICKS_CONTRAST_SET, this);
|
||||
lp_var_t<uint64_t> time = lp_var_t<uint64_t>(sid.objectId, PoolIds::TIME_CONTRAST_SET, this);
|
||||
lp_var_t<uint32_t> binA0 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BINA0, this);
|
||||
lp_var_t<uint32_t> binA1 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BINA1, this);
|
||||
lp_var_t<uint32_t> binA2 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BINA2, this);
|
||||
lp_var_t<uint32_t> binA3 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BINA3, this);
|
||||
lp_var_t<uint32_t> binA4 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BINA4, this);
|
||||
lp_var_t<uint32_t> binA5 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BINA5, this);
|
||||
lp_var_t<uint32_t> binA6 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BINA6, this);
|
||||
lp_var_t<uint32_t> binA7 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BINA7, this);
|
||||
lp_var_t<uint32_t> binA8 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BINA8, this);
|
||||
lp_var_t<uint32_t> binb0 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BINB0, this);
|
||||
lp_var_t<uint32_t> binB1 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BINB1, this);
|
||||
lp_var_t<uint32_t> binB2 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BINB2, this);
|
||||
lp_var_t<uint32_t> binB3 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BINB3, this);
|
||||
lp_var_t<uint32_t> binB4 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BINB4, this);
|
||||
lp_var_t<uint32_t> binB5 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BINB5, this);
|
||||
lp_var_t<uint32_t> binB6 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BINB6, this);
|
||||
lp_var_t<uint32_t> binB7 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BINB7, this);
|
||||
lp_var_t<uint32_t> binB8 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BINB8, this);
|
||||
lp_var_t<uint32_t> binC0 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BINC0, this);
|
||||
lp_var_t<uint32_t> binC1 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BINC1, this);
|
||||
lp_var_t<uint32_t> binC2 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BINC2, this);
|
||||
lp_var_t<uint32_t> binC3 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BINC3, this);
|
||||
lp_var_t<uint32_t> binC4 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BINC4, this);
|
||||
lp_var_t<uint32_t> binC5 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BINC5, this);
|
||||
lp_var_t<uint32_t> binC6 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BINC6, this);
|
||||
lp_var_t<uint32_t> binC7 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BINC7, this);
|
||||
lp_var_t<uint32_t> binC8 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BINC8, this);
|
||||
lp_var_t<uint32_t> binD0 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BIND0, this);
|
||||
lp_var_t<uint32_t> binD1 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BIND1, this);
|
||||
lp_var_t<uint32_t> binD2 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BIND2, this);
|
||||
lp_var_t<uint32_t> binD3 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BIND3, this);
|
||||
lp_var_t<uint32_t> binD4 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BIND4, this);
|
||||
lp_var_t<uint32_t> binD5 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BIND5, this);
|
||||
lp_var_t<uint32_t> binD6 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BIND6, this);
|
||||
lp_var_t<uint32_t> binD7 = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_BIND7, this);
|
||||
lp_var_t<uint32_t> binD8 = lp_var_t<uint32_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_ */
|
||||
|
||||
@ -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,
|
||||
@ -479,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): {
|
||||
@ -499,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;
|
||||
}
|
||||
@ -510,19 +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): {
|
||||
result = handleTm(histogramSet, StarTracker::HistogramSet::SIZE);
|
||||
break;
|
||||
}
|
||||
case (StarTracker::REQ_CONTRAST): {
|
||||
result = handleTm(contrastSet, StarTracker::ContrastSet::SIZE);
|
||||
break;
|
||||
}
|
||||
case (StarTracker::LIMITS):
|
||||
@ -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,85 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l
|
||||
localDataPoolMap.emplace(StarTracker::STABLE_COUNT, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::SOLUTION_STRATEGY, new PoolEntry<uint8_t>( { 0 }));
|
||||
|
||||
localDataPoolMap.emplace(StarTracker::TICKS_HISTOGRAM_SET, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::TIME_HISTOGRAM_SET, new PoolEntry<uint64_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINA0, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINA1, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINA2, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINA3, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINA4, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINA5, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINA6, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINA7, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINA8, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINB0, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINB1, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINB2, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINB3, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINB4, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINB5, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINB6, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINB7, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINB8, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINC0, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINC1, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINC2, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINC3, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINC4, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINC5, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINC6, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINC7, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BINC8, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BIND0, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BIND1, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BIND2, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BIND3, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BIND4, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BIND5, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BIND6, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BIND7, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::HISTOGRAM_BIND8, new PoolEntry<uint32_t>( { 0 }));
|
||||
|
||||
localDataPoolMap.emplace(StarTracker::TICKS_CONTRAST_SET, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::TIME_CONTRAST_SET, new PoolEntry<uint64_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BINA0, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BINA1, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BINA2, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BINA3, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BINA4, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BINA5, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BINA6, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BINA7, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BINA8, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BINB0, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BINB1, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BINB2, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BINB3, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BINB4, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BINB5, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BINB6, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BINB7, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BINB8, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BINC8, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BINC0, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BINC1, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BINC2, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BINC3, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BINC4, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BINC5, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BINC6, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BINC7, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BINC8, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BIND0, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BIND1, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BIND2, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BIND3, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BIND4, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BIND5, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BIND6, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BIND7, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CONTRAST_BIND8, new PoolEntry<uint32_t>( { 0 }));
|
||||
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
@ -764,6 +863,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 +1016,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;
|
||||
@ -993,74 +1116,6 @@ ReturnValue_t StarTrackerHandler::handlePingReply() {
|
||||
return result;
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::handleTimeTm() {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
result = timeSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
uint32_t offset = TM_DATA_FIELD_OFFSET;
|
||||
uint8_t status = 0;
|
||||
uint32_t ticks = 0;
|
||||
uint64_t time = 0;
|
||||
getTmHeaderData(&status, &ticks, &time);
|
||||
if(status != StarTracker::STATUS_OK) {
|
||||
sif::warning << "StarTrackerHandler::handleTimeTm: Reply error: "
|
||||
<< static_cast<unsigned int>(status) << std::endl;
|
||||
result = VERSION_REQ_FAILED;
|
||||
return result;
|
||||
}
|
||||
const uint8_t* reply = dataLinkLayer.getReply();
|
||||
timeSet.ticks = ticks;
|
||||
timeSet.time = time;
|
||||
timeSet.runTime = deserializeUint32(reply + offset);
|
||||
offset += sizeof(uint32_t);
|
||||
timeSet.unixTime = deserializeUint64(reply + offset);
|
||||
result = timeSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
|
||||
timeSet.printSet();
|
||||
#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
|
||||
return result;
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::handleVersionTm() {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
uint32_t offset = TM_DATA_FIELD_OFFSET;
|
||||
uint8_t status = 0;
|
||||
uint32_t ticks = 0;
|
||||
uint64_t time = 0;
|
||||
getTmHeaderData(&status, &ticks, &time);
|
||||
if(status != StarTracker::STATUS_OK) {
|
||||
sif::warning << "StarTrackerHandler::handleVersionTm: Reply error: "
|
||||
<< static_cast<unsigned int>(status) << std::endl;
|
||||
result = VERSION_REQ_FAILED;
|
||||
return result;
|
||||
}
|
||||
result = versionSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
const uint8_t* reply = dataLinkLayer.getReply();
|
||||
versionSet.ticks = ticks;
|
||||
versionSet.time = time;
|
||||
versionSet.program = *(reply + offset);
|
||||
offset += 1;
|
||||
versionSet.major = *(reply + offset);
|
||||
offset += 1;
|
||||
versionSet.minor = *(reply + offset);
|
||||
result = versionSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
|
||||
versionSet.printSet();
|
||||
#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
|
||||
return result;
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::checkProgram() {
|
||||
PoolReadGuard pg(&versionSet);
|
||||
switch(versionSet.program.value) {
|
||||
@ -1085,269 +1140,37 @@ ReturnValue_t StarTrackerHandler::checkProgram() {
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::handleInterfaceTm() {
|
||||
ReturnValue_t StarTrackerHandler::handleTm(LocalPoolDataSetBase& dataset, size_t size) {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
uint32_t offset = TM_DATA_FIELD_OFFSET;
|
||||
uint8_t status = 0;
|
||||
uint32_t ticks = 0;
|
||||
uint64_t time = 0;
|
||||
getTmHeaderData(&status, &ticks, &time);
|
||||
uint8_t status = *(dataLinkLayer.getReply() + STATUS_OFFSET);
|
||||
if(status != StarTracker::STATUS_OK) {
|
||||
sif::warning << "StarTrackerHandler::handleInterfaceTm: Reply error: "
|
||||
sif::warning << "StarTrackerHandler::handleTm: Reply error: "
|
||||
<< static_cast<unsigned int>(status) << std::endl;
|
||||
result = INTERFACE_REQ_FAILED;
|
||||
return result;
|
||||
return TM_REPLY_ERROR;
|
||||
}
|
||||
result = interfaceSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
||||
result = dataset.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
const uint8_t* reply = dataLinkLayer.getReply();
|
||||
interfaceSet.ticks = ticks;
|
||||
interfaceSet.time = time;
|
||||
size_t size = sizeof(uint32_t);
|
||||
interfaceSet.frameCount = deserializeUint32(reply + offset);
|
||||
offset += size;
|
||||
interfaceSet.checksumerrorCount = deserializeUint32(reply + offset);
|
||||
result = interfaceSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
||||
const uint8_t* reply = dataLinkLayer.getReply() + TICKS_OFFSET;
|
||||
dataset.setValidityBufferGeneration(false);
|
||||
result = dataset.deSerialize(&reply, &size, SerializeIF::Endianness::LITTLE);
|
||||
if (result != RETURN_OK) {
|
||||
sif::warning << "StarTrackerHandler::handleTm: Deserialization failed"
|
||||
<< std::endl;
|
||||
}
|
||||
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
|
||||
interfaceSet.printSet();
|
||||
#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
|
||||
return result;
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::handlePowerTm() {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
uint32_t offset = TM_DATA_FIELD_OFFSET;
|
||||
uint8_t status = 0;
|
||||
uint32_t ticks = 0;
|
||||
uint64_t time = 0;
|
||||
getTmHeaderData(&status, &ticks, &time);
|
||||
if(status != StarTracker::STATUS_OK) {
|
||||
sif::warning << "StarTrackerHandler::handlePowerTm: Reply error: "
|
||||
<< static_cast<unsigned int>(status) << std::endl;
|
||||
result = POWER_REQ_FAILED;
|
||||
return result;
|
||||
}
|
||||
result = powerSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
const uint8_t* reply = dataLinkLayer.getReply();
|
||||
powerSet.ticks= ticks;
|
||||
powerSet.time= time;
|
||||
float value = 0;
|
||||
std::memcpy(&value, reply + offset, sizeof(value));
|
||||
powerSet.mcuCurrent = value;
|
||||
offset += 4;
|
||||
std::memcpy(&value, reply + offset, sizeof(value));
|
||||
powerSet.mcuVoltage = value;
|
||||
offset += 4;
|
||||
std::memcpy(&value, reply + offset, sizeof(value));
|
||||
powerSet.fpgaCoreCurrent = value;
|
||||
offset += 4;
|
||||
std::memcpy(&value, reply + offset, sizeof(value));
|
||||
powerSet.fpgaCoreVoltage = value;
|
||||
offset += 4;
|
||||
std::memcpy(&value, reply + offset, sizeof(value));
|
||||
powerSet.fpga18Current = value;
|
||||
offset += 4;
|
||||
std::memcpy(&value, reply + offset, sizeof(value));
|
||||
powerSet.fpga18Voltage = value;
|
||||
offset += 4;
|
||||
std::memcpy(&value, reply + offset, sizeof(value));
|
||||
powerSet.fpga25Current = value;
|
||||
offset += 4;
|
||||
std::memcpy(&value, reply + offset, sizeof(value));
|
||||
powerSet.fpga25Voltage = value;
|
||||
offset += 4;
|
||||
std::memcpy(&value, reply + offset, sizeof(value));
|
||||
powerSet.cmv21Current = value;
|
||||
offset += 4;
|
||||
std::memcpy(&value, reply + offset, sizeof(value));
|
||||
powerSet.cmv21Voltage = value;
|
||||
offset += 4;
|
||||
std::memcpy(&value, reply + offset, sizeof(value));
|
||||
powerSet.cmvPixCurrent= value;
|
||||
offset += 4;
|
||||
std::memcpy(&value, reply + offset, sizeof(value));
|
||||
powerSet.cmvPixVoltage = value;
|
||||
offset += 4;
|
||||
std::memcpy(&value, reply + offset, sizeof(value));
|
||||
powerSet.cmv33Current= value;
|
||||
offset += 4;
|
||||
std::memcpy(&value, reply + offset, sizeof(value));
|
||||
powerSet.cmv33Voltage = value;
|
||||
offset += 4;
|
||||
std::memcpy(&value, reply + offset, sizeof(value));
|
||||
powerSet.cmvResCurrent= value;
|
||||
offset += 4;
|
||||
std::memcpy(&value, reply + offset, sizeof(value));
|
||||
powerSet.cmvResVoltage = value;
|
||||
result = powerSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
|
||||
powerSet.printSet();
|
||||
#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
|
||||
return result;
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::handleSolutionTm() {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
result = solutionSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
uint32_t offset = TM_DATA_FIELD_OFFSET;
|
||||
uint8_t status = 0;
|
||||
uint32_t ticks = 0;
|
||||
uint64_t time = 0;
|
||||
getTmHeaderData(&status, &ticks, &time);
|
||||
if(status != StarTracker::STATUS_OK) {
|
||||
sif::warning << "StarTrackerHandler::handleSolutioneTm: Reply error: "
|
||||
<< static_cast<unsigned int>(status) << std::endl;
|
||||
result = TEMPERATURE_REQ_FAILED;
|
||||
return result;
|
||||
}
|
||||
const uint8_t* reply = dataLinkLayer.getReply();
|
||||
solutionSet.ticks= ticks;
|
||||
solutionSet.time= time;
|
||||
float word = 0;
|
||||
std::memcpy(&word, reply + offset, sizeof(float));
|
||||
solutionSet.caliQw = word;
|
||||
offset += sizeof(float);
|
||||
std::memcpy(&word, reply + offset, sizeof(float));
|
||||
solutionSet.caliQx = word;
|
||||
offset += sizeof(float);
|
||||
std::memcpy(&word, reply + offset, sizeof(float));
|
||||
solutionSet.caliQy = word;
|
||||
offset += sizeof(float);
|
||||
std::memcpy(&word, reply + offset, sizeof(float));
|
||||
solutionSet.caliQz = word;
|
||||
offset += sizeof(float);
|
||||
std::memcpy(&word, reply + offset, sizeof(float));
|
||||
solutionSet.trackConfidence = word;
|
||||
offset += sizeof(float);
|
||||
std::memcpy(&word, reply + offset, sizeof(float));
|
||||
solutionSet.trackQw = word;
|
||||
offset += sizeof(float);
|
||||
std::memcpy(&word, reply + offset, sizeof(float));
|
||||
solutionSet.trackQx = word;
|
||||
offset += sizeof(float);
|
||||
std::memcpy(&word, reply + offset, sizeof(float));
|
||||
solutionSet.trackQy = word;
|
||||
offset += sizeof(float);
|
||||
std::memcpy(&word, reply + offset, sizeof(float));
|
||||
solutionSet.trackQz = word;
|
||||
offset += sizeof(float);
|
||||
solutionSet.trackRemoved = *(reply + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
solutionSet.starsCentroided = *(reply + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
solutionSet.starsMatchedDatabase = *(reply + offset);
|
||||
offset += sizeof(float);
|
||||
std::memcpy(&word, reply + offset, sizeof(float));
|
||||
solutionSet.lisaQw = word;
|
||||
offset += sizeof(float);
|
||||
std::memcpy(&word, reply + offset, sizeof(float));
|
||||
solutionSet.lisaQx = word;
|
||||
offset += sizeof(float);
|
||||
std::memcpy(&word, reply + offset, sizeof(float));
|
||||
solutionSet.lisaQy = word;
|
||||
offset += sizeof(float);
|
||||
std::memcpy(&word, reply + offset, sizeof(float));
|
||||
solutionSet.lisaQz = word;
|
||||
offset += sizeof(float);
|
||||
std::memcpy(&word, reply + offset, sizeof(float));
|
||||
solutionSet.lisaPercentageClose = word;
|
||||
offset += sizeof(float);
|
||||
solutionSet.lisaNrClose = *(reply + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
solutionSet.isTrustWorthy = *(reply + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
solutionSet.stableCount = *(reply + offset);
|
||||
offset += sizeof(uint32_t);
|
||||
solutionSet.stableCount = *(reply + offset);
|
||||
result = solutionSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
|
||||
solutionSet.printSet();
|
||||
dataset.printSet();
|
||||
#endif
|
||||
return result;
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::handleTemperatureTm() {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
uint32_t offset = TM_DATA_FIELD_OFFSET;
|
||||
uint8_t status = 0;
|
||||
uint32_t ticks = 0;
|
||||
uint64_t time = 0;
|
||||
getTmHeaderData(&status, &ticks, &time);
|
||||
if(status != StarTracker::STATUS_OK) {
|
||||
sif::warning << "StarTrackerHandler::handleTemperatureTm: Reply error: "
|
||||
<< static_cast<unsigned int>(status) << std::endl;
|
||||
result = TEMPERATURE_REQ_FAILED;
|
||||
return result;
|
||||
}
|
||||
result = temperatureSet.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
const uint8_t* reply = dataLinkLayer.getReply();
|
||||
temperatureSet.ticks= ticks;
|
||||
temperatureSet.time= time;
|
||||
float temperature = 0;
|
||||
std::memcpy(&temperature, reply + offset, sizeof(temperature));
|
||||
temperatureSet.mcuTemperature = temperature;
|
||||
offset += sizeof(float);
|
||||
std::memcpy(&temperature, reply + offset, sizeof(temperature));
|
||||
temperatureSet.cmosTemperature = temperature;
|
||||
result = temperatureSet.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
|
||||
temperatureSet.printSet();
|
||||
#endif
|
||||
return result;
|
||||
}
|
||||
|
||||
void StarTrackerHandler::getTmHeaderData(uint8_t* status, uint32_t* ticks, uint64_t* time) {
|
||||
const uint8_t* reply = dataLinkLayer.getReply();
|
||||
*status = *(reply + STATUS_OFFSET);
|
||||
*ticks = deserializeUint32(reply + TICKS_OFFSET);
|
||||
*time = deserializeUint64(reply + TIME_OFFSET);
|
||||
}
|
||||
|
||||
uint32_t StarTrackerHandler::deserializeUint32(const uint8_t* buffer) {
|
||||
uint32_t word = 0;
|
||||
word = *(buffer + 3) << 24
|
||||
| *(buffer + 2) << 16
|
||||
| *(buffer + 1) << 8
|
||||
| *(buffer);
|
||||
return word;
|
||||
}
|
||||
|
||||
uint64_t StarTrackerHandler::deserializeUint64(const uint8_t* buffer) {
|
||||
uint64_t word = 0;
|
||||
word = static_cast<uint64_t>(*(buffer + 7)) << 56
|
||||
| static_cast<uint64_t>(*(buffer + 6)) << 48
|
||||
| static_cast<uint64_t>(*(buffer + 5)) << 40
|
||||
| static_cast<uint64_t>(*(buffer + 4)) << 32
|
||||
| static_cast<uint64_t>(*(buffer + 3)) << 24
|
||||
| static_cast<uint64_t>(*(buffer + 2)) << 16
|
||||
| static_cast<uint64_t>(*(buffer + 1)) << 8
|
||||
| static_cast<uint64_t>(*(buffer));
|
||||
return word;
|
||||
}
|
||||
|
||||
void StarTrackerHandler::handleStartup(const uint8_t* parameterId) {
|
||||
switch(*parameterId) {
|
||||
case (StarTracker::ID::LIMITS): {
|
||||
|
||||
@ -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 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);
|
||||
|
||||
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
|
||||
*/
|
||||
@ -298,72 +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 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_ */
|
||||
|
||||
@ -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);
|
||||
@ -266,3 +271,38 @@ 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: Current [mA], Voltage [mV]" << std::endl;
|
||||
sif::info << std::setw(8) << std::left << "Ch0" << std::dec << "| " <<
|
||||
static_cast<unsigned int>(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<unsigned int>(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<unsigned int>(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<unsigned int>(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<unsigned int>(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<unsigned int>(acuHkTableDataset.currentInChannel5.value) <<
|
||||
std::setw(15) << std::right << acuHkTableDataset.voltageInChannel5.value << std::endl;
|
||||
}
|
||||
|
||||
@ -3,6 +3,7 @@
|
||||
|
||||
#include "GomspaceDeviceHandler.h"
|
||||
#include <mission/devices/devicedefinitions/GomspaceDefinitions.h>
|
||||
#include "fsfw/datapool/PoolReadGuard.h"
|
||||
|
||||
/**
|
||||
* @brief Handler for the ACU from Gomspace. Monitors and controls the battery charging via
|
||||
@ -25,8 +26,14 @@ protected:
|
||||
*/
|
||||
virtual ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t * id) override;
|
||||
|
||||
virtual void fillCommandAndReplyMap() override;
|
||||
|
||||
virtual ReturnValue_t deviceSpecificCommand(DeviceCommandId_t cmd) override;
|
||||
|
||||
private:
|
||||
|
||||
static const DeviceCommandId_t PRINT_CHANNEL_STATS = 51;
|
||||
|
||||
ACU::HkTableDataset acuHkTableDataset;
|
||||
|
||||
/**
|
||||
@ -34,6 +41,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_ */
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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,
|
||||
@ -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:
|
||||
|
||||
/**
|
||||
|
||||
@ -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]
|
||||
|
||||
2
tmtc
2
tmtc
@ -1 +1 @@
|
||||
Subproject commit 3377243c07ceb4c5d58d35468dedefb71e1c4aa5
|
||||
Subproject commit f5613e5e80013d2d0c33938a56a334ce2c7c9f5a
|
||||
Loading…
Reference in New Issue
Block a user