STR extensions
This commit is contained in:
parent
7d1d913220
commit
6a6789ae99
@ -607,6 +607,10 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi
|
||||
result = prepareRequestLisaParams();
|
||||
return result;
|
||||
}
|
||||
case (startracker::REQ_MATCHED_CENTROIDS): {
|
||||
result = prepareRequestMatchedCentroids();
|
||||
return result;
|
||||
}
|
||||
case (startracker::REQ_MATCHING): {
|
||||
result = prepareRequestMatchingParams();
|
||||
return result;
|
||||
@ -729,6 +733,8 @@ void StarTrackerHandler::fillCommandAndReplyMap() {
|
||||
startracker::MAX_FRAME_SIZE * 2 + 2);
|
||||
this->insertInCommandAndReplyMap(startracker::REQ_DEBUG_CAMERA, 3, &debugCameraSet,
|
||||
startracker::MAX_FRAME_SIZE * 2 + 2);
|
||||
this->insertInCommandAndReplyMap(startracker::REQ_MATCHED_CENTROIDS, 3, &matchedCentroids,
|
||||
startracker::MAX_FRAME_SIZE * 2 + 2);
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::isModeCombinationValid(Mode_t mode, Submode_t submode) {
|
||||
@ -860,6 +866,16 @@ void StarTrackerHandler::setUpJsonCfgs(JsonConfigs& cfgs, const char* paramJsonF
|
||||
JCFG_DONE = true;
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::statusFieldCheck(const uint8_t* rawFrame) {
|
||||
uint8_t status = startracker::getStatusField(rawFrame);
|
||||
if (status != startracker::STATUS_OK) {
|
||||
sif::warning << "StarTrackerHandler::handleTm: Reply error: "
|
||||
<< static_cast<unsigned int>(status) << std::endl;
|
||||
return REPLY_ERROR;
|
||||
}
|
||||
return returnvalue::OK;
|
||||
}
|
||||
|
||||
void StarTrackerHandler::bootBootloader() {
|
||||
if (internalState == InternalState::IDLE) {
|
||||
internalState = InternalState::BOOT_BOOTLOADER;
|
||||
@ -896,24 +912,24 @@ ReturnValue_t StarTrackerHandler::scanForReply(const uint8_t* start, size_t rema
|
||||
}
|
||||
case TMTC_ACTIONREPLY: {
|
||||
*foundLen = remainingSize;
|
||||
fullPacketLen = remainingSize;
|
||||
return scanForActionReply(startracker::getId(start), foundId);
|
||||
break;
|
||||
}
|
||||
case TMTC_SETPARAMREPLY: {
|
||||
*foundLen = remainingSize;
|
||||
fullPacketLen = remainingSize;
|
||||
return scanForSetParameterReply(startracker::getId(start), foundId);
|
||||
break;
|
||||
}
|
||||
case TMTC_PARAMREPLY: {
|
||||
*foundLen = remainingSize;
|
||||
fullPacketLen = remainingSize;
|
||||
return scanForGetParameterReply(startracker::getId(start), foundId);
|
||||
break;
|
||||
}
|
||||
case TMTC_TELEMETRYREPLYA:
|
||||
case TMTC_TELEMETRYREPLY: {
|
||||
*foundLen = remainingSize;
|
||||
fullPacketLen = remainingSize;
|
||||
return scanForTmReply(startracker::getId(start), foundId);
|
||||
break;
|
||||
}
|
||||
default: {
|
||||
sif::debug << "StarTrackerHandler::scanForReply: Reply has invalid type id" << std::endl;
|
||||
@ -970,6 +986,10 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id,
|
||||
result = handleTm(packet, solutionSet, startracker::SolutionSet::SIZE, "REQ_SOLUTION");
|
||||
break;
|
||||
}
|
||||
case (startracker::REQ_MATCHED_CENTROIDS): {
|
||||
result = handleMatchedCentroidTm(packet);
|
||||
break;
|
||||
}
|
||||
case (startracker::REQ_TEMPERATURE): {
|
||||
result = handleTm(packet, temperatureSet, startracker::TemperatureSet::SIZE, "REQ_TEMP");
|
||||
break;
|
||||
@ -1032,6 +1052,10 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id,
|
||||
handleParamRequest(packet, trackingSet, startracker::TrackingSet::SIZE);
|
||||
break;
|
||||
}
|
||||
case (startracker::REQ_MATCHED_CENTROIDS): {
|
||||
handleParamRequest(packet, matchedCentroids, startracker::MatchedCentroidsSet::SIZE);
|
||||
break;
|
||||
}
|
||||
case (startracker::REQ_VALIDATION): {
|
||||
handleParamRequest(packet, validationSet, startracker::ValidationSet::SIZE);
|
||||
break;
|
||||
@ -1328,6 +1352,20 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l
|
||||
localDataPoolMap.emplace(startracker::DEBUG_CAMERA_TEST, new PoolEntry<uint32_t>({0}));
|
||||
|
||||
localDataPoolMap.emplace(startracker::CHKSUM, new PoolEntry<uint32_t>({0}));
|
||||
localDataPoolMap.emplace(startracker::PoolIds::NUM_MATCHED_CENTROIDS, new PoolEntry<uint8_t>());
|
||||
localDataPoolMap.emplace(startracker::PoolIds::MATCHED_CENTROIDS_STAR_IDS,
|
||||
new PoolEntry<uint32_t>(16));
|
||||
localDataPoolMap.emplace(startracker::PoolIds::MATCHED_CENTROIDS_X_COORDS,
|
||||
new PoolEntry<float>(16));
|
||||
localDataPoolMap.emplace(startracker::PoolIds::MATCHED_CENTROIDS_Y_COORDS,
|
||||
new PoolEntry<float>(16));
|
||||
localDataPoolMap.emplace(startracker::PoolIds::MATCHED_CENTROIDS_X_ERRORS,
|
||||
new PoolEntry<float>(16));
|
||||
localDataPoolMap.emplace(startracker::PoolIds::MATCHED_CENTROIDS_Y_ERRORS,
|
||||
new PoolEntry<float>(16));
|
||||
localDataPoolMap.emplace(startracker::PoolIds::TICKS_MATCHED_CENTROIDS,
|
||||
new PoolEntry<uint32_t>());
|
||||
localDataPoolMap.emplace(startracker::PoolIds::TIME_MATCHED_CENTROIDS, new PoolEntry<uint64_t>());
|
||||
|
||||
poolManager.subscribeForDiagPeriodicPacket(
|
||||
subdp::DiagnosticsHkPeriodicParams(temperatureSet.getSid(), false, 10.0));
|
||||
@ -1760,6 +1798,14 @@ void StarTrackerHandler::prepareHistogramRequest() {
|
||||
rawPacketLen = length;
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::prepareRequestMatchedCentroids() {
|
||||
uint32_t length = 0;
|
||||
arc_tm_pack_matchedcentroids_req(commandBuffer, &length);
|
||||
rawPacket = commandBuffer;
|
||||
rawPacketLen = length;
|
||||
return returnvalue::OK;
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::prepareParamCommand(const uint8_t* commandData,
|
||||
size_t commandDataLen,
|
||||
ArcsecJsonParamBase& paramSet,
|
||||
@ -2039,12 +2085,9 @@ ReturnValue_t StarTrackerHandler::checkProgram() {
|
||||
|
||||
ReturnValue_t StarTrackerHandler::handleTm(const uint8_t* rawFrame, LocalPoolDataSetBase& dataset,
|
||||
size_t size, const char* context) {
|
||||
ReturnValue_t result = returnvalue::OK;
|
||||
uint8_t status = startracker::getStatusField(rawFrame);
|
||||
if (status != startracker::STATUS_OK) {
|
||||
sif::warning << "StarTrackerHandler::handleTm: Reply error: "
|
||||
<< static_cast<unsigned int>(status) << std::endl;
|
||||
return REPLY_ERROR;
|
||||
ReturnValue_t result = statusFieldCheck(rawFrame);
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
result = dataset.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
|
||||
if (result != returnvalue::OK) {
|
||||
@ -2069,6 +2112,66 @@ ReturnValue_t StarTrackerHandler::handleTm(const uint8_t* rawFrame, LocalPoolDat
|
||||
return result;
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::handleMatchedCentroidTm(const uint8_t* rawFrame) {
|
||||
ReturnValue_t result = statusFieldCheck(rawFrame);
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
const uint8_t* reply = rawFrame + TICKS_OFFSET;
|
||||
size_t remainingLen = fullPacketLen;
|
||||
PoolReadGuard pg(&matchedCentroids);
|
||||
result = pg.getReadResult();
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
result = SerializeAdapter::deSerialize(&matchedCentroids.ticks, &rawFrame, &remainingLen,
|
||||
SerializeIF::Endianness::LITTLE);
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
result = SerializeAdapter::deSerialize(&matchedCentroids.timeUs, &rawFrame, &remainingLen,
|
||||
SerializeIF::Endianness::LITTLE);
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
result = SerializeAdapter::deSerialize(&matchedCentroids.numberOfMatchedCentroids, &rawFrame,
|
||||
&remainingLen, SerializeIF::Endianness::LITTLE);
|
||||
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
// Yeah, we serialize it like that because I can't model anything with that local datapool crap.
|
||||
for (unsigned idx = 0; idx < 16; idx++) {
|
||||
result = SerializeAdapter::deSerialize(&matchedCentroids.starIds[idx], &rawFrame, &remainingLen,
|
||||
SerializeIF::Endianness::LITTLE);
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
result = SerializeAdapter::deSerialize(&matchedCentroids.xCoords[idx], &rawFrame, &remainingLen,
|
||||
SerializeIF::Endianness::LITTLE);
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
result = SerializeAdapter::deSerialize(&matchedCentroids.yCoords[idx], &rawFrame, &remainingLen,
|
||||
SerializeIF::Endianness::LITTLE);
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
result = SerializeAdapter::deSerialize(&matchedCentroids.xErrors[idx], &rawFrame, &remainingLen,
|
||||
SerializeIF::Endianness::LITTLE);
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
result = SerializeAdapter::deSerialize(&matchedCentroids.yErrors[idx], &rawFrame, &remainingLen,
|
||||
SerializeIF::Endianness::LITTLE);
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
}
|
||||
matchedCentroids.setValidity(true, true);
|
||||
return returnvalue::OK;
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::handleActionReplySet(const uint8_t* rawFrame,
|
||||
LocalPoolDataSetBase& dataset, size_t size) {
|
||||
ReturnValue_t result = returnvalue::OK;
|
||||
@ -2204,6 +2307,7 @@ ReturnValue_t StarTrackerHandler::checkCommand(ActionId_t actionId) {
|
||||
case startracker::REQ_SUBSCRIPTION:
|
||||
case startracker::REQ_LOG_SUBSCRIPTION:
|
||||
case startracker::REQ_DEBUG_CAMERA:
|
||||
case startracker::REQ_MATCHED_CENTROIDS:
|
||||
if (getMode() == MODE_ON and getSubmode() != startracker::Program::FIRMWARE) {
|
||||
return STARTRACKER_NOT_RUNNING_FIRMWARE;
|
||||
}
|
||||
|
@ -204,6 +204,7 @@ class StarTrackerHandler : public DeviceHandlerBase {
|
||||
startracker::SubscriptionSet subscriptionSet;
|
||||
startracker::LogSubscriptionSet logSubscriptionSet;
|
||||
startracker::DebugCameraSet debugCameraSet;
|
||||
startracker::MatchedCentroidsSet matchedCentroids;
|
||||
|
||||
// Pointer to object responsible for uploading and downloading images to/from the star tracker
|
||||
StrComHandler* strHelper = nullptr;
|
||||
@ -297,6 +298,8 @@ class StarTrackerHandler : public DeviceHandlerBase {
|
||||
|
||||
const power::Switch_t powerSwitch = power::NO_SWITCH;
|
||||
|
||||
size_t fullPacketLen = 0;
|
||||
|
||||
/**
|
||||
* @brief Handles internal state
|
||||
*/
|
||||
@ -429,6 +432,7 @@ class StarTrackerHandler : public DeviceHandlerBase {
|
||||
ReturnValue_t prepareRequestCentroidingParams();
|
||||
ReturnValue_t prepareRequestLisaParams();
|
||||
ReturnValue_t prepareRequestMatchingParams();
|
||||
ReturnValue_t prepareRequestMatchedCentroids();
|
||||
ReturnValue_t prepareRequestTrackingParams();
|
||||
ReturnValue_t prepareRequestValidationParams();
|
||||
ReturnValue_t prepareRequestAlgoParams();
|
||||
@ -477,6 +481,7 @@ class StarTrackerHandler : public DeviceHandlerBase {
|
||||
*/
|
||||
void handleStartup(uint8_t tmType, uint8_t parameterId);
|
||||
|
||||
ReturnValue_t statusFieldCheck(const uint8_t* rawFrame);
|
||||
/**
|
||||
* @brief Handles telemtry replies and fills the appropriate dataset
|
||||
*
|
||||
@ -488,6 +493,8 @@ class StarTrackerHandler : public DeviceHandlerBase {
|
||||
ReturnValue_t handleTm(const uint8_t* rawFrame, LocalPoolDataSetBase& dataset, size_t size,
|
||||
const char* context);
|
||||
|
||||
ReturnValue_t handleMatchedCentroidTm(const uint8_t* rawFrame);
|
||||
|
||||
/**
|
||||
* @brief Checks if star tracker is in valid mode for executing the received command.
|
||||
*
|
||||
|
@ -272,7 +272,16 @@ enum PoolIds : lp_id_t {
|
||||
LOG_SUBSCRIPTION_LEVEL2,
|
||||
LOG_SUBSCRIPTION_MODULE2,
|
||||
DEBUG_CAMERA_TIMING,
|
||||
DEBUG_CAMERA_TEST
|
||||
DEBUG_CAMERA_TEST,
|
||||
|
||||
TICKS_MATCHED_CENTROIDS,
|
||||
TIME_MATCHED_CENTROIDS,
|
||||
NUM_MATCHED_CENTROIDS,
|
||||
MATCHED_CENTROIDS_STAR_IDS,
|
||||
MATCHED_CENTROIDS_X_COORDS,
|
||||
MATCHED_CENTROIDS_Y_COORDS,
|
||||
MATCHED_CENTROIDS_X_ERRORS,
|
||||
MATCHED_CENTROIDS_Y_ERRORS,
|
||||
};
|
||||
|
||||
static const DeviceCommandId_t PING_REQUEST = 0;
|
||||
@ -328,6 +337,7 @@ static const DeviceCommandId_t DISABLE_TIMESTAMP_GENERATION = 85;
|
||||
static const DeviceCommandId_t ENABLE_TIMESTAMP_GENERATION = 86;
|
||||
static constexpr DeviceCommandId_t SET_TIME_FROM_SYS_TIME = 87;
|
||||
static constexpr DeviceCommandId_t AUTO_THRESHOLD = 88;
|
||||
static constexpr DeviceCommandId_t REQ_MATCHED_CENTROIDS = 89;
|
||||
static const DeviceCommandId_t NONE = 0xFFFFFFFF;
|
||||
|
||||
static const uint32_t VERSION_SET_ID = REQ_VERSION;
|
||||
@ -352,6 +362,7 @@ static const uint32_t ALGO_SET_ID = REQ_ALGO;
|
||||
static const uint32_t SUBSCRIPTION_SET_ID = REQ_SUBSCRIPTION;
|
||||
static const uint32_t LOG_SUBSCRIPTION_SET_ID = REQ_LOG_SUBSCRIPTION;
|
||||
static const uint32_t DEBUG_CAMERA_SET_ID = REQ_DEBUG_CAMERA;
|
||||
static const uint32_t MATCHED_CENTROIDS_SET_ID = REQ_MATCHED_CENTROIDS;
|
||||
|
||||
/** Max size of unencoded frame */
|
||||
static const size_t MAX_FRAME_SIZE = 1200;
|
||||
@ -666,7 +677,7 @@ class SolutionSet : public StaticLocalDataSet<SOLUTION_SET_ENTRIES> {
|
||||
|
||||
// Ticks timestamp
|
||||
lp_var_t<uint32_t> ticks = lp_var_t<uint32_t>(sid.objectId, PoolIds::TICKS_SOLUTION_SET, this);
|
||||
/// Unix time stamp
|
||||
// Unix time stamp
|
||||
lp_var_t<uint64_t> timeUs = lp_var_t<uint64_t>(sid.objectId, PoolIds::TIME_SOLUTION_SET, this);
|
||||
// Calibrated quaternion (takes into account the mounting quaternion), typically same as
|
||||
// track q values
|
||||
@ -1381,6 +1392,35 @@ class ValidationSet : public StaticLocalDataSet<VALIDATION_SET_ENTRIES> {
|
||||
}
|
||||
};
|
||||
|
||||
class MatchedCentroidsSet : public StaticLocalDataSet<20> {
|
||||
public:
|
||||
static constexpr size_t SIZE = 321;
|
||||
MatchedCentroidsSet(HasLocalDataPoolIF* owner)
|
||||
: StaticLocalDataSet(owner, MATCHED_CENTROIDS_SET_ID) {}
|
||||
MatchedCentroidsSet(object_id_t objectId)
|
||||
: StaticLocalDataSet(sid_t(objectId, MATCHED_CENTROIDS_SET_ID)) {}
|
||||
// Ticks timestamp
|
||||
lp_var_t<uint32_t> ticks =
|
||||
lp_var_t<uint32_t>(sid.objectId, PoolIds::TICKS_MATCHED_CENTROIDS, this);
|
||||
// Unix time stamp
|
||||
lp_var_t<uint64_t> timeUs =
|
||||
lp_var_t<uint64_t>(sid.objectId, PoolIds::TIME_MATCHED_CENTROIDS, this);
|
||||
lp_var_t<uint8_t> numberOfMatchedCentroids =
|
||||
lp_var_t<uint8_t>(sid.objectId, PoolIds::NUM_MATCHED_CENTROIDS, this);
|
||||
lp_vec_t<uint32_t, 16> starIds =
|
||||
lp_vec_t<uint32_t, 16>(sid.objectId, PoolIds::MATCHED_CENTROIDS_STAR_IDS);
|
||||
lp_vec_t<float, 16> xCoords =
|
||||
lp_vec_t<float, 16>(sid.objectId, PoolIds::MATCHED_CENTROIDS_X_COORDS);
|
||||
lp_vec_t<float, 16> yCoords =
|
||||
lp_vec_t<float, 16>(sid.objectId, PoolIds::MATCHED_CENTROIDS_Y_COORDS);
|
||||
lp_vec_t<float, 16> xErrors =
|
||||
lp_vec_t<float, 16>(sid.objectId, PoolIds::MATCHED_CENTROIDS_X_ERRORS);
|
||||
lp_vec_t<float, 16> yErrors =
|
||||
lp_vec_t<float, 16>(sid.objectId, PoolIds::MATCHED_CENTROIDS_Y_ERRORS);
|
||||
|
||||
private:
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Will store the requested algo parameters
|
||||
*/
|
||||
|
2
tmtc
2
tmtc
@ -1 +1 @@
|
||||
Subproject commit 22a42108b431bafa707de2b3a24cad9de95b70d0
|
||||
Subproject commit 783bdd297a931a96c8b077ec2c2456a203b23ffc
|
Loading…
Reference in New Issue
Block a user