Merge pull request 'IMTQ Commanding Fix' (#430) from imtq-cmd-fix into develop
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Reviewed-on: #430 Reviewed-by: Robin Müller <muellerr@irs.uni-stuttgart.de>
This commit is contained in:
commit
3003a4c4d1
@ -16,6 +16,13 @@ will consitute of a breaking change warranting a new major release:
|
||||
|
||||
# [unreleased]
|
||||
|
||||
## Fixed
|
||||
|
||||
- Bumped FSFW: `Countdown` and `Stopwatch` use new monotonic clock API now.
|
||||
- IMTQ: Various fixes, most notably missing buffer time after starting MGM measurement
|
||||
and corrections for actuator commanding.
|
||||
PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/430
|
||||
|
||||
# [v1.34.0] 2023-03-03
|
||||
|
||||
eive-tmtc: v2.16.3
|
||||
|
||||
@ -225,7 +225,7 @@ class SdCardManager : public SystemObject, public SdCardMountedIF {
|
||||
bool markedUnusable = false;
|
||||
MutexIF* sdLock = nullptr;
|
||||
static constexpr MutexIF::TimeoutType LOCK_TYPE = MutexIF::TimeoutType::WAITING;
|
||||
static constexpr uint32_t LOCK_TIMEOUT = 40;
|
||||
static constexpr uint32_t LOCK_TIMEOUT = 150;
|
||||
static constexpr char LOCK_CTX[] = "SdCardManager";
|
||||
|
||||
SdCardManager();
|
||||
|
||||
@ -62,7 +62,7 @@ static constexpr uint32_t SCHED_BLOCK_2_SENSOR_READ_MS = 30;
|
||||
static constexpr uint32_t SCHED_BLOCK_3_READ_IMTQ_MGM_MS = 42;
|
||||
static constexpr uint32_t SCHED_BLOCK_4_ACS_CTRL_MS = 45;
|
||||
static constexpr uint32_t SCHED_BLOCK_5_ACTUATOR_MS = 55;
|
||||
static constexpr uint32_t SCHED_BLOCK_6_IMTQ_BLOCK_2_MS = 95;
|
||||
static constexpr uint32_t SCHED_BLOCK_6_IMTQ_BLOCK_2_MS = 105;
|
||||
static constexpr uint32_t SCHED_BLOCK_RTD = 150;
|
||||
static constexpr uint32_t SCHED_BLOCK_7_RW_READ_MS = 300;
|
||||
static constexpr uint32_t SCHED_BLOCK_8_PLPCDU_MS = 320;
|
||||
|
||||
2
fsfw
2
fsfw
@ -1 +1 @@
|
||||
Subproject commit 33de15205b4560c54a108e35609536374b026c22
|
||||
Subproject commit 2c5af91db170ffd19f5cc6726642593fa9e3a059
|
||||
@ -35,6 +35,9 @@ ReturnValue_t ImtqPollingTask::performOperation(uint8_t operationCode) {
|
||||
handleActuateStep();
|
||||
break;
|
||||
}
|
||||
default: {
|
||||
break;
|
||||
}
|
||||
};
|
||||
}
|
||||
return returnvalue::OK;
|
||||
@ -118,12 +121,24 @@ void ImtqPollingTask::handleMeasureStep() {
|
||||
}
|
||||
// Takes a bit of time to take measurements. Subtract a bit because of the delay of previous
|
||||
// commands.
|
||||
TaskFactory::delayTask(currentIntegrationTimeMs);
|
||||
TaskFactory::delayTask(currentIntegrationTimeMs + MGM_READ_BUFFER_TIME_MS);
|
||||
|
||||
cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT;
|
||||
if (i2cCmdExecMeasure(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
|
||||
return;
|
||||
}
|
||||
// See p.39 of the iMTQ user manual. If the NEW bit of the STAT bitfield is not set, we probably
|
||||
// have old data. Which can be really bad for ACS. And everything.
|
||||
if ((replyPtr[2] >> 7) == 0) {
|
||||
sif::error << "IMTQ: MGM measurement too old" << std::endl;
|
||||
TaskFactory::delayTask(2);
|
||||
if (i2cCmdExecMeasure(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
|
||||
return;
|
||||
}
|
||||
if ((replyPtr[2] >> 7) == 0b1) {
|
||||
replyPtr[0] = false;
|
||||
}
|
||||
}
|
||||
|
||||
cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA;
|
||||
if (i2cCmdExecMeasure(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) {
|
||||
@ -157,18 +172,34 @@ void ImtqPollingTask::handleActuateStep() {
|
||||
return;
|
||||
}
|
||||
|
||||
TaskFactory::delayTask(10);
|
||||
|
||||
cmdLen = 1;
|
||||
cmdBuf[0] = imtq::CC::START_MTM_MEASUREMENT;
|
||||
if (i2cCmdExecActuate(imtq::CC::START_MTM_MEASUREMENT) != returnvalue::OK) {
|
||||
return;
|
||||
}
|
||||
|
||||
TaskFactory::delayTask(currentIntegrationTimeMs);
|
||||
TaskFactory::delayTask(currentIntegrationTimeMs + MGM_READ_BUFFER_TIME_MS);
|
||||
|
||||
cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT;
|
||||
if (i2cCmdExecActuate(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
|
||||
return;
|
||||
}
|
||||
// See p.39 of the iMTQ user manual. If the NEW bit of the STAT bitfield is not set, we probably
|
||||
// have old data. Which can be really bad for ACS. And everything.
|
||||
if ((replyPtr[2] >> 7) == 0) {
|
||||
sif::error << "IMTQ: MGM measurement too old" << std::endl;
|
||||
TaskFactory::delayTask(2);
|
||||
cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT;
|
||||
if (i2cCmdExecActuate(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
|
||||
return;
|
||||
}
|
||||
if ((replyPtr[2] >> 7) == 0b1) {
|
||||
replyPtr[0] = false;
|
||||
}
|
||||
}
|
||||
|
||||
cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA;
|
||||
if (i2cCmdExecActuate(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) {
|
||||
return;
|
||||
@ -192,15 +223,15 @@ ReturnValue_t ImtqPollingTask::initializeInterface(CookieIF* cookie) {
|
||||
|
||||
ReturnValue_t ImtqPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendData,
|
||||
size_t sendLen) {
|
||||
ImtqRequest request(sendData, sendLen);
|
||||
const auto* imtqReq = reinterpret_cast<const ImtqRequest*>(sendData);
|
||||
{
|
||||
MutexGuard mg(ipcLock);
|
||||
currentRequest = request.getRequestType();
|
||||
if (currentRequest == imtq::RequestType::ACTUATE) {
|
||||
std::memcpy(dipoles, request.getDipoles(), 6);
|
||||
torqueDuration = request.getTorqueDuration();
|
||||
if (imtqReq->request == imtq::RequestType::ACTUATE) {
|
||||
std::memcpy(dipoles, imtqReq->dipoles, sizeof(dipoles));
|
||||
torqueDuration = imtqReq->torqueDuration;
|
||||
}
|
||||
specialRequest = request.getSpecialRequest();
|
||||
currentRequest = imtqReq->request;
|
||||
specialRequest = imtqReq->specialRequest;
|
||||
if (state != InternalState::IDLE) {
|
||||
return returnvalue::FAILED;
|
||||
}
|
||||
@ -309,6 +340,8 @@ void ImtqPollingTask::buildDipoleCommand() {
|
||||
}
|
||||
SerializeAdapter::serialize(&torqueDuration, &serPtr, &serLen, cmdBuf.size(),
|
||||
SerializeIF::Endianness::LITTLE);
|
||||
// sif::debug << "Dipole X: " << dipoles[0] << std::endl;
|
||||
// sif::debug << "Torqeu Dur: " << torqueDuration << std::endl;
|
||||
cmdLen = 1 + serLen;
|
||||
}
|
||||
|
||||
@ -325,9 +358,11 @@ ReturnValue_t ImtqPollingTask::readReceivedMessage(CookieIF* cookie, uint8_t** b
|
||||
if (currentRequest == imtq::RequestType::MEASURE_NO_ACTUATION) {
|
||||
replyLen = getExchangeBufLen(specialRequest);
|
||||
memcpy(exchangeBuf.data(), replyBuf.data(), replyLen);
|
||||
} else {
|
||||
} else if (currentRequest == imtq::RequestType::ACTUATE) {
|
||||
replyLen = ImtqRepliesWithTorque::BASE_LEN;
|
||||
memcpy(exchangeBuf.data(), replyBufActuation.data(), replyLen);
|
||||
} else {
|
||||
*size = 0;
|
||||
}
|
||||
*buffer = exchangeBuf.data();
|
||||
*size = replyLen;
|
||||
|
||||
@ -32,6 +32,8 @@ class ImtqPollingTask : public SystemObject,
|
||||
const char* i2cDev = nullptr;
|
||||
address_t i2cAddr = 0;
|
||||
uint32_t currentIntegrationTimeMs = 10;
|
||||
// Required in addition to integration time, otherwise old data might be read.
|
||||
static constexpr uint32_t MGM_READ_BUFFER_TIME_MS = 3;
|
||||
bool ignoreNextActuateRequest = false;
|
||||
|
||||
imtq::SpecialRequest specialRequest = imtq::SpecialRequest::NONE;
|
||||
|
||||
@ -198,7 +198,7 @@ void AcsController::performSafe() {
|
||||
|
||||
updateCtrlValData(errAng);
|
||||
updateActuatorCmdData(cmdDipolMtqs);
|
||||
// commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
|
||||
//commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2]/*500, 500, 500*/,
|
||||
// acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
|
||||
// acsParameters.rwHandlingParameters.rampTime);
|
||||
}
|
||||
|
||||
@ -53,6 +53,13 @@ ImtqHandler::ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comC
|
||||
|
||||
ReturnValue_t ImtqHandler::performOperation(uint8_t opCode) {
|
||||
uint8_t dhbOpCode = DeviceHandlerIF::PERFORM_OPERATION;
|
||||
auto actuateStep = [&]() {
|
||||
if (ignoreActForRestOfComSteps) {
|
||||
requestStep = imtq::RequestType::DO_NOTHING;
|
||||
} else {
|
||||
requestStep = imtq::RequestType::ACTUATE;
|
||||
}
|
||||
};
|
||||
switch (static_cast<imtq::ComStep>(opCode)) {
|
||||
case (imtq::ComStep::DHB_OP): {
|
||||
break;
|
||||
@ -78,22 +85,38 @@ ReturnValue_t ImtqHandler::performOperation(uint8_t opCode) {
|
||||
break;
|
||||
}
|
||||
case (imtq::ComStep::START_ACTUATE_SEND): {
|
||||
if (manualTorqueCmdActive) {
|
||||
if (manuallyCommandedTorqueDuration.isBusy()) {
|
||||
ignoreActForRestOfComSteps = true;
|
||||
requestStep = imtq::RequestType::DO_NOTHING;
|
||||
} else {
|
||||
manualTorqueCmdActive = false;
|
||||
PoolReadGuard pg(&dipoleSet);
|
||||
dipoleSet.dipoles[0] = 0;
|
||||
dipoleSet.dipoles[1] = 0;
|
||||
dipoleSet.dipoles[2] = 0;
|
||||
dipoleSet.currentTorqueDurationMs = 0;
|
||||
requestStep = imtq::RequestType::ACTUATE;
|
||||
ignoreActForRestOfComSteps = false;
|
||||
}
|
||||
} else {
|
||||
requestStep = imtq::RequestType::ACTUATE;
|
||||
}
|
||||
dhbOpCode = DeviceHandlerIF::SEND_WRITE;
|
||||
break;
|
||||
}
|
||||
case (imtq::ComStep::START_ACTUATE_GET): {
|
||||
requestStep = imtq::RequestType::ACTUATE;
|
||||
actuateStep();
|
||||
dhbOpCode = DeviceHandlerIF::GET_WRITE;
|
||||
break;
|
||||
}
|
||||
case (imtq::ComStep::READ_ACTUATE_SEND): {
|
||||
requestStep = imtq::RequestType::ACTUATE;
|
||||
actuateStep();
|
||||
dhbOpCode = DeviceHandlerIF::SEND_READ;
|
||||
break;
|
||||
}
|
||||
case (imtq::ComStep::READ_ACTUATE_GET): {
|
||||
requestStep = imtq::RequestType::ACTUATE;
|
||||
actuateStep();
|
||||
dhbOpCode = DeviceHandlerIF::GET_READ;
|
||||
break;
|
||||
}
|
||||
@ -108,7 +131,8 @@ ReturnValue_t ImtqHandler::performOperation(uint8_t opCode) {
|
||||
ImtqHandler::~ImtqHandler() = default;
|
||||
|
||||
void ImtqHandler::doStartUp() {
|
||||
updatePeriodicReply(true, imtq::cmdIds::REPLY);
|
||||
updatePeriodicReply(true, imtq::cmdIds::REPLY_NO_TORQUE);
|
||||
updatePeriodicReply(true, imtq::cmdIds::REPLY_WITH_TORQUE);
|
||||
if (goToNormalMode) {
|
||||
setMode(MODE_NORMAL);
|
||||
} else {
|
||||
@ -117,7 +141,8 @@ void ImtqHandler::doStartUp() {
|
||||
}
|
||||
|
||||
void ImtqHandler::doShutDown() {
|
||||
updatePeriodicReply(false, imtq::cmdIds::REPLY);
|
||||
updatePeriodicReply(false, imtq::cmdIds::REPLY_NO_TORQUE);
|
||||
updatePeriodicReply(false, imtq::cmdIds::REPLY_WITH_TORQUE);
|
||||
specialRequestActive = false;
|
||||
firstReplyCycle = true;
|
||||
setMode(_MODE_POWER_DOWN);
|
||||
@ -133,6 +158,14 @@ ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
|
||||
*id = imtq::cmdIds::START_ACTUATION_DIPOLE;
|
||||
return buildCommandFromCommand(*id, nullptr, 0);
|
||||
}
|
||||
default: {
|
||||
*id = imtq::cmdIds::REQUEST;
|
||||
request.request = imtq::RequestType::DO_NOTHING;
|
||||
request.specialRequest = imtq::SpecialRequest::NONE;
|
||||
rawPacket = reinterpret_cast<uint8_t*>(&request);
|
||||
rawPacketLen = sizeof(ImtqRequest);
|
||||
return returnvalue::OK;
|
||||
}
|
||||
}
|
||||
return NOTHING_TO_SEND;
|
||||
}
|
||||
@ -145,11 +178,12 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
|
||||
const uint8_t* commandData,
|
||||
size_t commandDataLen) {
|
||||
auto genericSpecialRequest = [&](imtq::SpecialRequest specialRequest) {
|
||||
ImtqRequest request(commandBuffer, sizeof(commandBuffer));
|
||||
request.setMeasureRequest(specialRequest);
|
||||
request.request = imtq::RequestType::MEASURE_NO_ACTUATION;
|
||||
request.specialRequest = specialRequest;
|
||||
expectedReply = imtq::cmdIds::REPLY_NO_TORQUE;
|
||||
specialRequestActive = true;
|
||||
rawPacket = commandBuffer;
|
||||
rawPacketLen = ImtqRequest::REQUEST_LEN;
|
||||
rawPacket = reinterpret_cast<uint8_t*>(&request);
|
||||
rawPacketLen = sizeof(ImtqRequest);
|
||||
};
|
||||
switch (deviceCommand) {
|
||||
case (imtq::cmdIds::POS_X_SELF_TEST): {
|
||||
@ -181,45 +215,50 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
|
||||
return returnvalue::OK;
|
||||
}
|
||||
case (imtq::cmdIds::REQUEST): {
|
||||
ImtqRequest request(commandBuffer, sizeof(commandBuffer));
|
||||
request.setMeasureRequest(imtq::SpecialRequest::NONE);
|
||||
rawPacket = commandBuffer;
|
||||
rawPacketLen = ImtqRequest::REQUEST_LEN;
|
||||
request.request = imtq::RequestType::MEASURE_NO_ACTUATION;
|
||||
request.specialRequest = imtq::SpecialRequest::NONE;
|
||||
expectedReply = imtq::cmdIds::REPLY_NO_TORQUE;
|
||||
rawPacket = reinterpret_cast<uint8_t*>(&request);
|
||||
rawPacketLen = sizeof(ImtqRequest);
|
||||
return returnvalue::OK;
|
||||
}
|
||||
case (imtq::cmdIds::START_ACTUATION_DIPOLE): {
|
||||
/* IMTQ expects low byte first */
|
||||
// commandBuffer[0] = imtq::CC::START_ACTUATION_DIPOLE;
|
||||
if (commandData != nullptr && commandDataLen < 8) {
|
||||
return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
|
||||
}
|
||||
ImtqRequest request(commandBuffer, sizeof(commandBuffer));
|
||||
{
|
||||
// Do this in any case to read values which might be commanded by the ACS controller.
|
||||
PoolReadGuard pg(&dipoleSet);
|
||||
// Commands override anything which was set in the software
|
||||
if (commandData != nullptr) {
|
||||
// Read set dipole values from local pool
|
||||
PoolReadGuard pg(&dipoleSet);
|
||||
|
||||
int16_t xDipole = 0, yDipole = 0, zDipole = 0;
|
||||
uint16_t torqueDuration = 0;
|
||||
dipoleSet.xDipole = xDipole;
|
||||
dipoleSet.yDipole = yDipole;
|
||||
dipoleSet.zDipole = zDipole;
|
||||
dipoleSet.currentTorqueDurationMs = torqueDuration;
|
||||
dipoleSet.setValidityBufferGeneration(false);
|
||||
ReturnValue_t result = dipoleSet.deSerialize(&commandData, &commandDataLen,
|
||||
SerializeIF::Endianness::NETWORK);
|
||||
dipoleSet.setValidityBufferGeneration(true);
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
manualTorqueCmdActive = true;
|
||||
manuallyCommandedTorqueDuration.setTimeout(dipoleSet.currentTorqueDurationMs.value);
|
||||
}
|
||||
}
|
||||
|
||||
request.setActuateRequest(dipoleSet.xDipole.value, dipoleSet.yDipole.value,
|
||||
dipoleSet.zDipole.value, dipoleSet.currentTorqueDurationMs.value);
|
||||
expectedReply = imtq::cmdIds::REPLY_WITH_TORQUE;
|
||||
request.request = imtq::RequestType::ACTUATE;
|
||||
request.specialRequest = imtq::SpecialRequest::NONE;
|
||||
std::memcpy(request.dipoles, dipoleSet.dipoles.value, sizeof(request.dipoles));
|
||||
request.torqueDuration = dipoleSet.currentTorqueDurationMs.value;
|
||||
if (ACTUATION_WIRETAPPING) {
|
||||
sif::debug << "Actuating IMTQ with parameters x = " << dipoleSet.xDipole.value
|
||||
<< ", y = " << dipoleSet.yDipole.value << ", z = " << dipoleSet.zDipole.value
|
||||
sif::debug << "Actuating IMTQ with parameters x = " << dipoleSet.dipoles[0]
|
||||
<< ", y = " << dipoleSet.dipoles[1] << ", z = " << dipoleSet.dipoles[2]
|
||||
<< ", duration = " << dipoleSet.currentTorqueDurationMs.value << std::endl;
|
||||
}
|
||||
MutexGuard mg(torquer::lazyLock(), torquer::LOCK_TYPE, torquer::LOCK_TIMEOUT,
|
||||
torquer::LOCK_CTX);
|
||||
torquer::TORQUEING = true;
|
||||
torquer::TORQUE_COUNTDOWN.setTimeout(dipoleSet.currentTorqueDurationMs.value);
|
||||
rawPacket = commandBuffer;
|
||||
rawPacketLen = ImtqRequest::REQUEST_LEN;
|
||||
rawPacket = reinterpret_cast<uint8_t*>(&request);
|
||||
rawPacketLen = sizeof(ImtqRequest);
|
||||
return returnvalue::OK;
|
||||
}
|
||||
default:
|
||||
@ -231,7 +270,8 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
|
||||
void ImtqHandler::fillCommandAndReplyMap() {
|
||||
insertInCommandMap(imtq::cmdIds::REQUEST);
|
||||
insertInCommandMap(imtq::cmdIds::START_ACTUATION_DIPOLE);
|
||||
insertInReplyMap(imtq::cmdIds::REPLY, 5, nullptr, 0, true);
|
||||
insertInReplyMap(imtq::cmdIds::REPLY_NO_TORQUE, 5, nullptr, 0, true);
|
||||
insertInReplyMap(imtq::cmdIds::REPLY_WITH_TORQUE, 20, nullptr, 0, true);
|
||||
insertInCommandMap(imtq::cmdIds::POS_X_SELF_TEST);
|
||||
insertInCommandMap(imtq::cmdIds::NEG_X_SELF_TEST);
|
||||
insertInCommandMap(imtq::cmdIds::POS_Y_SELF_TEST);
|
||||
@ -248,7 +288,7 @@ ReturnValue_t ImtqHandler::scanForReply(const uint8_t* start, size_t remainingSi
|
||||
}
|
||||
if (remainingSize > 0) {
|
||||
*foundLen = remainingSize;
|
||||
*foundId = imtq::cmdIds::REPLY;
|
||||
*foundId = expectedReply;
|
||||
return returnvalue::OK;
|
||||
}
|
||||
return returnvalue::FAILED;
|
||||
@ -262,8 +302,7 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
|
||||
return returnvalue::OK;
|
||||
}
|
||||
// arrayprinter::print(packet, ImtqReplies::BASE_LEN);
|
||||
if (requestStep == imtq::RequestType::MEASURE_NO_ACTUATION) {
|
||||
requestStep = imtq::RequestType::ACTUATE;
|
||||
if (expectedReply == imtq::cmdIds::REPLY_NO_TORQUE) {
|
||||
// sif::debug << "handle measure" << std::endl;
|
||||
ImtqRepliesDefault replies(packet);
|
||||
if (specialRequestActive) {
|
||||
@ -328,9 +367,8 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
|
||||
} else {
|
||||
status = result;
|
||||
}
|
||||
} else {
|
||||
} else if (expectedReply == imtq::cmdIds::REPLY_WITH_TORQUE) {
|
||||
// sif::debug << "handle measure with torque" << std::endl;
|
||||
requestStep = imtq::RequestType::MEASURE_NO_ACTUATION;
|
||||
ImtqRepliesWithTorque replies(packet);
|
||||
if (replies.wasDipoleActuationRead()) {
|
||||
parseStatusByte(imtq::CC::START_ACTUATION_DIPOLE, replies.getDipoleActuation());
|
||||
@ -375,6 +413,8 @@ LocalPoolDataSetBase* ImtqHandler::getDataSetHandle(sid_t sid) {
|
||||
return &hkDatasetNoTorque;
|
||||
} else if (sid == dipoleSet.getSid()) {
|
||||
return &dipoleSet;
|
||||
} else if (sid == statusSet.getSid()) {
|
||||
return &statusSet;
|
||||
} else if (sid == hkDatasetWithTorque.getSid()) {
|
||||
return &hkDatasetWithTorque;
|
||||
} else if (sid == rawMtmWithTorque.getSid()) {
|
||||
@ -410,21 +450,26 @@ ReturnValue_t ImtqHandler::initializeLocalDataPool(localpool::DataPool& localDat
|
||||
localDataPoolMap.emplace(imtq::STATUS_BYTE_CONF, &statusConfig);
|
||||
localDataPoolMap.emplace(imtq::STATUS_BYTE_ERROR, &statusError);
|
||||
localDataPoolMap.emplace(imtq::STATUS_BYTE_UPTIME, &statusUptime);
|
||||
|
||||
// ENG HK No Torque
|
||||
localDataPoolMap.emplace(imtq::DIGITAL_VOLTAGE_MV, new PoolEntry<uint16_t>({0}));
|
||||
localDataPoolMap.emplace(imtq::ANALOG_VOLTAGE_MV, new PoolEntry<uint16_t>({0}));
|
||||
localDataPoolMap.emplace(imtq::DIGITAL_CURRENT, new PoolEntry<float>({0}));
|
||||
localDataPoolMap.emplace(imtq::ANALOG_CURRENT, new PoolEntry<float>({0}));
|
||||
localDataPoolMap.emplace(imtq::COIL_X_CURRENT, new PoolEntry<float>({0}));
|
||||
localDataPoolMap.emplace(imtq::COIL_Y_CURRENT, new PoolEntry<float>({0}));
|
||||
localDataPoolMap.emplace(imtq::COIL_Z_CURRENT, new PoolEntry<float>({0}));
|
||||
localDataPoolMap.emplace(imtq::COIL_X_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
||||
localDataPoolMap.emplace(imtq::COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
||||
localDataPoolMap.emplace(imtq::COIL_Z_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
||||
localDataPoolMap.emplace(imtq::COIL_CURRENTS, &coilCurrentsMilliampsNoTorque);
|
||||
localDataPoolMap.emplace(imtq::COIL_TEMPERATURES, &coilTempsNoTorque);
|
||||
localDataPoolMap.emplace(imtq::MCU_TEMPERATURE, new PoolEntry<int16_t>({0}));
|
||||
|
||||
localDataPoolMap.emplace(imtq::DIPOLES_X, &dipoleXEntry);
|
||||
localDataPoolMap.emplace(imtq::DIPOLES_Y, &dipoleYEntry);
|
||||
localDataPoolMap.emplace(imtq::DIPOLES_Z, &dipoleZEntry);
|
||||
// ENG HK With Torque
|
||||
localDataPoolMap.emplace(imtq::DIGITAL_VOLTAGE_MV_WT, new PoolEntry<uint16_t>({0}));
|
||||
localDataPoolMap.emplace(imtq::ANALOG_VOLTAGE_MV_WT, new PoolEntry<uint16_t>({0}));
|
||||
localDataPoolMap.emplace(imtq::DIGITAL_CURRENT_WT, new PoolEntry<float>({0}));
|
||||
localDataPoolMap.emplace(imtq::ANALOG_CURRENT_WT, new PoolEntry<float>({0}));
|
||||
localDataPoolMap.emplace(imtq::COIL_CURRENTS_WT, &coilCurrentsMilliampsWithTorque);
|
||||
localDataPoolMap.emplace(imtq::COIL_TEMPERATURES_WT, &coilTempsWithTorque);
|
||||
localDataPoolMap.emplace(imtq::MCU_TEMPERATURE_WT, new PoolEntry<int16_t>({0}));
|
||||
|
||||
localDataPoolMap.emplace(imtq::DIPOLES_ID, &dipolesPoolEntry);
|
||||
localDataPoolMap.emplace(imtq::CURRENT_TORQUE_DURATION, &torqueDurationEntry);
|
||||
|
||||
/** Entries of calibrated MTM measurement dataset */
|
||||
@ -432,8 +477,11 @@ ReturnValue_t ImtqHandler::initializeLocalDataPool(localpool::DataPool& localDat
|
||||
localDataPoolMap.emplace(imtq::ACTUATION_CAL_STATUS, new PoolEntry<uint8_t>({0}));
|
||||
|
||||
/** Entries of raw MTM measurement dataset */
|
||||
localDataPoolMap.emplace(imtq::MTM_RAW, new PoolEntry<float>(3));
|
||||
localDataPoolMap.emplace(imtq::ACTUATION_RAW_STATUS, new PoolEntry<uint8_t>({0}));
|
||||
localDataPoolMap.emplace(imtq::MTM_RAW, &mtmRawNoTorque);
|
||||
localDataPoolMap.emplace(imtq::ACTUATION_RAW_STATUS, &actStatusNoTorque);
|
||||
|
||||
localDataPoolMap.emplace(imtq::MTM_RAW_WT, &mtmRawWithTorque);
|
||||
localDataPoolMap.emplace(imtq::ACTUATION_RAW_STATUS_WT, &actStatusWithTorque);
|
||||
|
||||
/** INIT measurements for positive X axis test */
|
||||
localDataPoolMap.emplace(imtq::INIT_POS_X_ERR, new PoolEntry<uint8_t>({0}));
|
||||
@ -709,6 +757,10 @@ ReturnValue_t ImtqHandler::initializeLocalDataPool(localpool::DataPool& localDat
|
||||
subdp::DiagnosticsHkPeriodicParams(rawMtmWithTorque.getSid(), false, 10.0));
|
||||
poolManager.subscribeForDiagPeriodicPacket(
|
||||
subdp::DiagnosticsHkPeriodicParams(calMtmMeasurementSet.getSid(), false, 10.0));
|
||||
poolManager.subscribeForRegularPeriodicPacket(
|
||||
subdp::RegularHkPeriodicParams(statusSet.getSid(), false, 10.0));
|
||||
poolManager.subscribeForDiagPeriodicPacket(
|
||||
subdp::DiagnosticsHkPeriodicParams(dipoleSet.getSid(), false, 10.0));
|
||||
return DeviceHandlerBase::initializeLocalDataPool(localDataPoolMap, poolManager);
|
||||
}
|
||||
|
||||
@ -732,7 +784,7 @@ ReturnValue_t ImtqHandler::getSelfTestCommandId(DeviceCommandId_t* id) {
|
||||
}
|
||||
|
||||
ReturnValue_t ImtqHandler::parseStatusByte(imtq::CC::CC command, const uint8_t* packet) {
|
||||
uint8_t cmdErrorField = *(packet + 1) & 0xF;
|
||||
uint8_t cmdErrorField = packet[1] & 0xF;
|
||||
if (cmdErrorField == 0) {
|
||||
return returnvalue::OK;
|
||||
}
|
||||
@ -755,16 +807,20 @@ ReturnValue_t ImtqHandler::parseStatusByte(imtq::CC::CC command, const uint8_t*
|
||||
<< " has invalid parameter" << std::endl;
|
||||
return imtq::PARAMETER_INVALID;
|
||||
case 5:
|
||||
sif::error << "IMTQ::parseStatusByte: CC 0x" << std::setw(2) << " unavailable" << std::endl;
|
||||
sif::error << "IMTQ::parseStatusByte: CC 0x" << std::setw(2) << command << " unavailable"
|
||||
<< std::endl;
|
||||
return imtq::CC_UNAVAILABLE;
|
||||
case 7:
|
||||
sif::error << "IMTQ::parseStatusByte: IMTQ replied internal processing error" << std::endl;
|
||||
sif::error << "IMTQ::parseStatusByte: Internal processing error for command 0x"
|
||||
<< std::setw(2) << command << std::endl;
|
||||
return imtq::INTERNAL_PROCESSING_ERROR;
|
||||
default:
|
||||
sif::error << "IMTQ::parseStatusByte: CMD Error field contains unknown error code 0x"
|
||||
<< static_cast<int>(cmdErrorField) << std::endl;
|
||||
sif::error << "IMTQ::parseStatusByte: CMD error field for command 0x" << std::setw(2)
|
||||
<< command << " contains unknown error code 0x" << static_cast<int>(cmdErrorField)
|
||||
<< std::endl;
|
||||
return imtq::CMD_ERR_UNKNOWN;
|
||||
}
|
||||
sif::error << std::dec;
|
||||
}
|
||||
|
||||
void ImtqHandler::fillEngHkDataset(imtq::HkDataset& hkDataset, const uint8_t* packet) {
|
||||
@ -778,20 +834,20 @@ void ImtqHandler::fillEngHkDataset(imtq::HkDataset& hkDataset, const uint8_t* pa
|
||||
offset += 2;
|
||||
hkDataset.analogCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
||||
offset += 2;
|
||||
hkDataset.coilXCurrentmA =
|
||||
hkDataset.coilCurrentsMilliamps[0] =
|
||||
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
||||
offset += 2;
|
||||
hkDataset.coilYCurrentmA =
|
||||
hkDataset.coilCurrentsMilliamps[1] =
|
||||
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
||||
offset += 2;
|
||||
hkDataset.coilZCurrentmA =
|
||||
hkDataset.coilCurrentsMilliamps[2] =
|
||||
static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
||||
offset += 2;
|
||||
hkDataset.coilXTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
|
||||
hkDataset.coilTemperatures[0] = (*(packet + offset + 1) << 8 | *(packet + offset));
|
||||
offset += 2;
|
||||
hkDataset.coilYTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
|
||||
hkDataset.coilTemperatures[1] = (*(packet + offset + 1) << 8 | *(packet + offset));
|
||||
offset += 2;
|
||||
hkDataset.coilZTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
|
||||
hkDataset.coilTemperatures[2] = (*(packet + offset + 1) << 8 | *(packet + offset));
|
||||
offset += 2;
|
||||
size_t dummy = 2;
|
||||
SerializeAdapter::deSerialize(&hkDataset.mcuTemperature.value, packet + offset, &dummy,
|
||||
@ -805,12 +861,15 @@ void ImtqHandler::fillEngHkDataset(imtq::HkDataset& hkDataset, const uint8_t* pa
|
||||
sif::info << "IMTQ analog voltage: " << hkDataset.analogVoltageMv << " mV" << std::endl;
|
||||
sif::info << "IMTQ digital current: " << hkDataset.digitalCurrentmA << " mA" << std::endl;
|
||||
sif::info << "IMTQ analog current: " << hkDataset.analogCurrentmA << " mA" << std::endl;
|
||||
sif::info << "IMTQ coil X current: " << hkDataset.coilXCurrentmA << " mA" << std::endl;
|
||||
sif::info << "IMTQ coil Y current: " << hkDataset.coilYCurrentmA << " mA" << std::endl;
|
||||
sif::info << "IMTQ coil Z current: " << hkDataset.coilZCurrentmA << " mA" << std::endl;
|
||||
sif::info << "IMTQ coil X temperature: " << hkDataset.coilXTemperature << " °C" << std::endl;
|
||||
sif::info << "IMTQ coil Y temperature: " << hkDataset.coilYTemperature << " °C" << std::endl;
|
||||
sif::info << "IMTQ coil Z temperature: " << hkDataset.coilZTemperature << " °C" << std::endl;
|
||||
sif::info << "IMTQ coil X current: " << hkDataset.coilCurrentsMilliamps[0] << " mA"
|
||||
<< std::endl;
|
||||
sif::info << "IMTQ coil Y current: " << hkDataset.coilCurrentsMilliamps[1] << " mA"
|
||||
<< std::endl;
|
||||
sif::info << "IMTQ coil Z current: " << hkDataset.coilCurrentsMilliamps[2] << " mA"
|
||||
<< std::endl;
|
||||
sif::info << "IMTQ coil X temperature: " << hkDataset.coilTemperatures[0] << " °C" << std::endl;
|
||||
sif::info << "IMTQ coil Y temperature: " << hkDataset.coilTemperatures[1] << " °C" << std::endl;
|
||||
sif::info << "IMTQ coil Z temperature: " << hkDataset.coilTemperatures[2] << " °C" << std::endl;
|
||||
sif::info << "IMTQ coil MCU temperature: " << hkDataset.mcuTemperature << " °C" << std::endl;
|
||||
#endif
|
||||
}
|
||||
@ -851,7 +910,7 @@ void ImtqHandler::fillCalibratedMtmDataset(const uint8_t* packet) {
|
||||
void ImtqHandler::fillRawMtmDataset(imtq::RawMtmMeasurementSet& set, const uint8_t* packet) {
|
||||
PoolReadGuard rg(&set);
|
||||
if (rg.getReadResult() != returnvalue::OK) {
|
||||
sif::error << "ImtqHandler::fillRawMtmDataset: Lock failure" << std::endl;
|
||||
sif::error << "ImtqHandler::fillRawMtmDataset: Read failure" << std::endl;
|
||||
}
|
||||
unsigned int offset = 2;
|
||||
size_t deSerLen = 16;
|
||||
@ -887,6 +946,7 @@ void ImtqHandler::fillRawMtmDataset(imtq::RawMtmMeasurementSet& set, const uint8
|
||||
set.setValidity(true, true);
|
||||
if (debugMode) {
|
||||
#if OBSW_VERBOSE_LEVEL >= 1
|
||||
sif::info << "Set ID: " << set.getSid().ownerSetId << std::endl;
|
||||
sif::info << "IMTQ raw MTM measurement X: " << set.mtmRawNt[0] << " nT" << std::endl;
|
||||
sif::info << "IMTQ raw MTM measurement Y: " << set.mtmRawNt[1] << " nT" << std::endl;
|
||||
sif::info << "IMTQ raw MTM measurement Z: " << set.mtmRawNt[2] << " nT" << std::endl;
|
||||
|
||||
@ -98,6 +98,9 @@ class ImtqHandler : public DeviceHandlerBase {
|
||||
imtq::NegYSelfTestSet negYselfTestDataset;
|
||||
imtq::PosZSelfTestSet posZselfTestDataset;
|
||||
imtq::NegZSelfTestSet negZselfTestDataset;
|
||||
bool manualTorqueCmdActive = false;
|
||||
bool ignoreActForRestOfComSteps = false;
|
||||
Countdown manuallyCommandedTorqueDuration = Countdown();
|
||||
|
||||
NormalPollingMode pollingMode = NormalPollingMode::UNCALIBRATED;
|
||||
|
||||
@ -107,13 +110,21 @@ class ImtqHandler : public DeviceHandlerBase {
|
||||
PoolEntry<uint32_t> statusUptime = PoolEntry<uint32_t>({0});
|
||||
|
||||
PoolEntry<int32_t> mgmCalEntry = PoolEntry<int32_t>(3);
|
||||
PoolEntry<int16_t> dipoleXEntry = PoolEntry<int16_t>(0, false);
|
||||
PoolEntry<int16_t> dipoleYEntry = PoolEntry<int16_t>(0, false);
|
||||
PoolEntry<int16_t> dipoleZEntry = PoolEntry<int16_t>(0, false);
|
||||
PoolEntry<uint16_t> torqueDurationEntry = PoolEntry<uint16_t>(0, false);
|
||||
PoolEntry<int16_t> dipolesPoolEntry = PoolEntry<int16_t>({0, 0, 0}, false);
|
||||
PoolEntry<uint16_t> torqueDurationEntry = PoolEntry<uint16_t>({0}, false);
|
||||
PoolEntry<float> coilCurrentsMilliampsNoTorque = PoolEntry<float>(3);
|
||||
PoolEntry<float> coilCurrentsMilliampsWithTorque = PoolEntry<float>(3);
|
||||
PoolEntry<int16_t> coilTempsNoTorque = PoolEntry<int16_t>(3);
|
||||
PoolEntry<int16_t> coilTempsWithTorque = PoolEntry<int16_t>(3);
|
||||
PoolEntry<float> mtmRawNoTorque = PoolEntry<float>(3);
|
||||
PoolEntry<uint8_t> actStatusNoTorque = PoolEntry<uint8_t>(1);
|
||||
PoolEntry<float> mtmRawWithTorque = PoolEntry<float>(3);
|
||||
PoolEntry<uint8_t> actStatusWithTorque = PoolEntry<uint8_t>(1);
|
||||
|
||||
power::Switch_t switcher = power::NO_SWITCH;
|
||||
|
||||
uint8_t commandBuffer[imtq::MAX_COMMAND_SIZE];
|
||||
ImtqRequest request{};
|
||||
DeviceCommandId_t expectedReply = imtq::cmdIds::REPLY_WITH_TORQUE;
|
||||
bool goToNormalMode = false;
|
||||
bool debugMode = false;
|
||||
bool specialRequestActive = false;
|
||||
|
||||
@ -22,7 +22,7 @@ enum ComStep : uint8_t {
|
||||
READ_ACTUATE_GET = 8,
|
||||
};
|
||||
|
||||
enum class RequestType : uint8_t { MEASURE_NO_ACTUATION, ACTUATE };
|
||||
enum class RequestType : uint8_t { MEASURE_NO_ACTUATION, ACTUATE, DO_NOTHING };
|
||||
|
||||
enum class SpecialRequest : uint8_t {
|
||||
NONE = 0,
|
||||
@ -53,7 +53,8 @@ static const ReturnValue_t UNEXPECTED_SELF_TEST_REPLY = MAKE_RETURN_CODE(0xA7);
|
||||
namespace cmdIds {
|
||||
|
||||
static constexpr DeviceCommandId_t REQUEST = 0x70;
|
||||
static constexpr DeviceCommandId_t REPLY = 0x71;
|
||||
static constexpr DeviceCommandId_t REPLY_NO_TORQUE = 0x71;
|
||||
static constexpr DeviceCommandId_t REPLY_WITH_TORQUE = 0x72;
|
||||
static const DeviceCommandId_t START_ACTUATION_DIPOLE = 0x2;
|
||||
static const DeviceCommandId_t POS_X_SELF_TEST = 0x7;
|
||||
static const DeviceCommandId_t NEG_X_SELF_TEST = 0x8;
|
||||
@ -195,20 +196,28 @@ enum PoolIds : lp_id_t {
|
||||
ANALOG_VOLTAGE_MV,
|
||||
DIGITAL_CURRENT,
|
||||
ANALOG_CURRENT,
|
||||
COIL_X_CURRENT,
|
||||
COIL_Y_CURRENT,
|
||||
COIL_Z_CURRENT,
|
||||
COIL_X_TEMPERATURE,
|
||||
COIL_Y_TEMPERATURE,
|
||||
COIL_Z_TEMPERATURE,
|
||||
COIL_CURRENTS,
|
||||
COIL_TEMPERATURES,
|
||||
MCU_TEMPERATURE,
|
||||
|
||||
DIGITAL_VOLTAGE_MV_WT,
|
||||
ANALOG_VOLTAGE_MV_WT,
|
||||
DIGITAL_CURRENT_WT,
|
||||
ANALOG_CURRENT_WT,
|
||||
COIL_CURRENTS_WT,
|
||||
COIL_TEMPERATURES_WT,
|
||||
MCU_TEMPERATURE_WT,
|
||||
|
||||
MGM_CAL_NT,
|
||||
ACTUATION_CAL_STATUS,
|
||||
|
||||
MTM_RAW,
|
||||
ACTUATION_RAW_STATUS,
|
||||
DIPOLES_X,
|
||||
DIPOLES_Y,
|
||||
DIPOLES_Z,
|
||||
|
||||
MTM_RAW_WT,
|
||||
ACTUATION_RAW_STATUS_WT,
|
||||
|
||||
DIPOLES_ID,
|
||||
CURRENT_TORQUE_DURATION,
|
||||
|
||||
INIT_POS_X_ERR,
|
||||
@ -476,34 +485,56 @@ class StatusDataset : public StaticLocalDataSet<4> {
|
||||
|
||||
class HkDataset : public StaticLocalDataSet<HK_SET_POOL_ENTRIES> {
|
||||
public:
|
||||
HkDataset(HasLocalDataPoolIF* owner, uint32_t setId) : StaticLocalDataSet(owner, setId) {}
|
||||
HkDataset(HasLocalDataPoolIF* owner, uint32_t setId, std::array<lp_id_t, 7> pids)
|
||||
: StaticLocalDataSet(owner, setId),
|
||||
digitalVoltageMv(sid.objectId, pids[0], this),
|
||||
analogVoltageMv(sid.objectId, pids[1], this),
|
||||
digitalCurrentmA(sid.objectId, pids[2], this),
|
||||
analogCurrentmA(sid.objectId, pids[3], this),
|
||||
coilCurrentsMilliamps(sid.objectId, pids[4], this),
|
||||
/** All temperatures in [C] for X, Y, Z */
|
||||
coilTemperatures(sid.objectId, pids[5], this),
|
||||
mcuTemperature(sid.objectId, pids[6], this) {}
|
||||
|
||||
HkDataset(object_id_t objectId, uint32_t setId) : StaticLocalDataSet(sid_t(objectId, setId)) {}
|
||||
HkDataset(object_id_t objectId, uint32_t setId, std::array<lp_id_t, 7> pids)
|
||||
: StaticLocalDataSet(sid_t(objectId, setId)),
|
||||
digitalVoltageMv(sid.objectId, pids[0], this),
|
||||
analogVoltageMv(sid.objectId, pids[1], this),
|
||||
digitalCurrentmA(sid.objectId, pids[2], this),
|
||||
analogCurrentmA(sid.objectId, pids[3], this),
|
||||
coilCurrentsMilliamps(sid.objectId, pids[4], this),
|
||||
/** All temperatures in [C] for X, Y, Z */
|
||||
coilTemperatures(sid.objectId, pids[5], this),
|
||||
mcuTemperature(sid.objectId, pids[6], this) {}
|
||||
|
||||
// Engineering HK variables
|
||||
lp_var_t<uint16_t> digitalVoltageMv = lp_var_t<uint16_t>(sid.objectId, DIGITAL_VOLTAGE_MV, this);
|
||||
lp_var_t<uint16_t> analogVoltageMv = lp_var_t<uint16_t>(sid.objectId, ANALOG_VOLTAGE_MV, this);
|
||||
lp_var_t<float> digitalCurrentmA = lp_var_t<float>(sid.objectId, DIGITAL_CURRENT, this);
|
||||
lp_var_t<float> analogCurrentmA = lp_var_t<float>(sid.objectId, ANALOG_CURRENT, this);
|
||||
lp_var_t<float> coilXCurrentmA = lp_var_t<float>(sid.objectId, COIL_X_CURRENT, this);
|
||||
lp_var_t<float> coilYCurrentmA = lp_var_t<float>(sid.objectId, COIL_Y_CURRENT, this);
|
||||
lp_var_t<float> coilZCurrentmA = lp_var_t<float>(sid.objectId, COIL_Z_CURRENT, this);
|
||||
/** All temperatures in [<5B>C] */
|
||||
lp_var_t<int16_t> coilXTemperature = lp_var_t<int16_t>(sid.objectId, COIL_X_TEMPERATURE, this);
|
||||
lp_var_t<int16_t> coilYTemperature = lp_var_t<int16_t>(sid.objectId, COIL_Y_TEMPERATURE, this);
|
||||
lp_var_t<int16_t> coilZTemperature = lp_var_t<int16_t>(sid.objectId, COIL_Z_TEMPERATURE, this);
|
||||
lp_var_t<int16_t> mcuTemperature = lp_var_t<int16_t>(sid.objectId, MCU_TEMPERATURE, this);
|
||||
lp_var_t<uint16_t> digitalVoltageMv;
|
||||
lp_var_t<uint16_t> analogVoltageMv;
|
||||
lp_var_t<float> digitalCurrentmA;
|
||||
lp_var_t<float> analogCurrentmA;
|
||||
lp_vec_t<float, 3> coilCurrentsMilliamps;
|
||||
/** All temperatures in [C] for X, Y, Z */
|
||||
lp_vec_t<int16_t, 3> coilTemperatures;
|
||||
lp_var_t<int16_t> mcuTemperature;
|
||||
|
||||
private:
|
||||
};
|
||||
|
||||
class HkDatasetNoTorque : public HkDataset {
|
||||
public:
|
||||
HkDatasetNoTorque(HasLocalDataPoolIF* owner) : HkDataset(owner, imtq::SetIds::ENG_HK_NO_TORQUE) {}
|
||||
HkDatasetNoTorque(HasLocalDataPoolIF* owner)
|
||||
: HkDataset(owner, imtq::SetIds::ENG_HK_NO_TORQUE,
|
||||
{DIGITAL_VOLTAGE_MV, ANALOG_VOLTAGE_MV, DIGITAL_CURRENT, ANALOG_CURRENT,
|
||||
COIL_CURRENTS, COIL_TEMPERATURES, MCU_TEMPERATURE}) {}
|
||||
};
|
||||
|
||||
class HkDatasetWithTorque : public HkDataset {
|
||||
public:
|
||||
HkDatasetWithTorque(HasLocalDataPoolIF* owner)
|
||||
: HkDataset(owner, imtq::SetIds::ENG_HK_SET_WITH_TORQUE) {}
|
||||
: HkDataset(owner, imtq::SetIds::ENG_HK_SET_WITH_TORQUE,
|
||||
{DIGITAL_VOLTAGE_MV_WT, ANALOG_VOLTAGE_MV_WT, DIGITAL_CURRENT_WT,
|
||||
ANALOG_CURRENT_WT, COIL_CURRENTS_WT, COIL_TEMPERATURES_WT, MCU_TEMPERATURE_WT}) {
|
||||
}
|
||||
};
|
||||
/**
|
||||
*
|
||||
@ -529,32 +560,39 @@ class CalibratedMtmMeasurementSet : public StaticLocalDataSet<CAL_MTM_POOL_ENTRI
|
||||
*/
|
||||
class RawMtmMeasurementSet : public StaticLocalDataSet<CAL_MTM_POOL_ENTRIES> {
|
||||
public:
|
||||
RawMtmMeasurementSet(HasLocalDataPoolIF* owner, uint32_t setId)
|
||||
: StaticLocalDataSet(owner, setId) {}
|
||||
RawMtmMeasurementSet(object_id_t objectId, uint32_t setId, std::array<lp_id_t, 2> pids)
|
||||
: StaticLocalDataSet(sid_t(objectId, setId)),
|
||||
mtmRawNt(sid.objectId, pids.at(0), this),
|
||||
coilActuationStatus(sid.objectId, pids.at(1), this) {}
|
||||
|
||||
RawMtmMeasurementSet(object_id_t objectId, uint32_t setId)
|
||||
: StaticLocalDataSet(sid_t(objectId, setId)) {}
|
||||
RawMtmMeasurementSet(HasLocalDataPoolIF* owner, uint32_t setId, std::array<lp_id_t, 2> pids)
|
||||
: StaticLocalDataSet(owner, setId),
|
||||
mtmRawNt(sid.objectId, pids.at(0), this),
|
||||
coilActuationStatus(sid.objectId, pids.at(1), this) {}
|
||||
|
||||
/** The unit of all measurements is nT */
|
||||
lp_vec_t<float, 3> mtmRawNt = lp_vec_t<float, 3>(sid.objectId, MTM_RAW, this);
|
||||
lp_vec_t<float, 3> mtmRawNt;
|
||||
/** 1 if coils were actuating during measurement otherwise 0 */
|
||||
lp_var_t<uint8_t> coilActuationStatus =
|
||||
lp_var_t<uint8_t>(sid.objectId, ACTUATION_RAW_STATUS, this);
|
||||
lp_var_t<uint8_t> coilActuationStatus;
|
||||
};
|
||||
|
||||
class RawMtmMeasurementNoTorque : public RawMtmMeasurementSet {
|
||||
public:
|
||||
RawMtmMeasurementNoTorque(HasLocalDataPoolIF* owner)
|
||||
: RawMtmMeasurementSet(owner, imtq::SetIds::RAW_MTM_NO_TORQUE) {}
|
||||
: RawMtmMeasurementSet(owner, imtq::SetIds::RAW_MTM_NO_TORQUE,
|
||||
{PoolIds::MTM_RAW, PoolIds::ACTUATION_RAW_STATUS}) {}
|
||||
RawMtmMeasurementNoTorque(object_id_t objectId)
|
||||
: RawMtmMeasurementSet(objectId, imtq::SetIds::RAW_MTM_NO_TORQUE) {}
|
||||
: RawMtmMeasurementSet(objectId, imtq::SetIds::RAW_MTM_NO_TORQUE,
|
||||
{PoolIds::MTM_RAW, PoolIds::ACTUATION_RAW_STATUS}) {}
|
||||
};
|
||||
class RawMtmMeasurementWithTorque : public RawMtmMeasurementSet {
|
||||
public:
|
||||
RawMtmMeasurementWithTorque(HasLocalDataPoolIF* owner)
|
||||
: RawMtmMeasurementSet(owner, imtq::SetIds::RAW_MTM_WITH_TORQUE) {}
|
||||
: RawMtmMeasurementSet(owner, imtq::SetIds::RAW_MTM_WITH_TORQUE,
|
||||
{PoolIds::MTM_RAW_WT, PoolIds::ACTUATION_RAW_STATUS_WT}) {}
|
||||
RawMtmMeasurementWithTorque(object_id_t objectId)
|
||||
: RawMtmMeasurementSet(objectId, imtq::SetIds::RAW_MTM_WITH_TORQUE) {}
|
||||
: RawMtmMeasurementSet(objectId, imtq::SetIds::RAW_MTM_WITH_TORQUE,
|
||||
{PoolIds::MTM_RAW_WT, PoolIds::ACTUATION_RAW_STATUS_WT}) {}
|
||||
};
|
||||
|
||||
/**
|
||||
@ -608,28 +646,16 @@ class DipoleActuationSet : public StaticLocalDataSet<4> {
|
||||
|
||||
void setDipoles(int16_t xDipole_, int16_t yDipole_, int16_t zDipole_,
|
||||
uint16_t currentTorqueDurationMs_) {
|
||||
if (xDipole.value != xDipole_) {
|
||||
xDipole = xDipole_;
|
||||
}
|
||||
if (yDipole.value != yDipole_) {
|
||||
yDipole = yDipole_;
|
||||
}
|
||||
if (zDipole.value != zDipole_) {
|
||||
zDipole = zDipole_;
|
||||
}
|
||||
dipoles[0] = xDipole_;
|
||||
dipoles[1] = yDipole_;
|
||||
dipoles[2] = zDipole_;
|
||||
currentTorqueDurationMs = currentTorqueDurationMs_;
|
||||
}
|
||||
|
||||
void getDipoles(int16_t& xDipole_, int16_t& yDipole_, int16_t& zDipole_) {
|
||||
xDipole_ = xDipole.value;
|
||||
yDipole_ = yDipole.value;
|
||||
zDipole_ = zDipole.value;
|
||||
}
|
||||
const int16_t* getDipoles() const { return dipoles.value; }
|
||||
|
||||
private:
|
||||
lp_var_t<int16_t> xDipole = lp_var_t<int16_t>(sid.objectId, DIPOLES_X, this);
|
||||
lp_var_t<int16_t> yDipole = lp_var_t<int16_t>(sid.objectId, DIPOLES_Y, this);
|
||||
lp_var_t<int16_t> zDipole = lp_var_t<int16_t>(sid.objectId, DIPOLES_Z, this);
|
||||
lp_vec_t<int16_t, 3> dipoles = lp_vec_t<int16_t, 3>(sid.objectId, DIPOLES_ID, this);
|
||||
lp_var_t<uint16_t> currentTorqueDurationMs =
|
||||
lp_var_t<uint16_t>(sid.objectId, CURRENT_TORQUE_DURATION, this);
|
||||
};
|
||||
@ -1099,65 +1125,10 @@ class NegZSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
|
||||
} // namespace imtq
|
||||
|
||||
struct ImtqRequest {
|
||||
friend class ImtqHandler;
|
||||
|
||||
public:
|
||||
static constexpr size_t REQUEST_LEN = 10;
|
||||
|
||||
ImtqRequest(const uint8_t* data, size_t maxSize)
|
||||
: rawData(const_cast<uint8_t*>(data)), maxSize(maxSize) {}
|
||||
|
||||
imtq::RequestType getRequestType() const { return static_cast<imtq::RequestType>(rawData[0]); }
|
||||
|
||||
void setMeasureRequest(imtq::SpecialRequest specialRequest) {
|
||||
rawData[0] = static_cast<uint8_t>(imtq::RequestType::MEASURE_NO_ACTUATION);
|
||||
rawData[1] = static_cast<uint8_t>(specialRequest);
|
||||
}
|
||||
|
||||
void setActuateRequest(int16_t dipoleX, int16_t dipoleY, int16_t dipoleZ,
|
||||
uint16_t torqueDuration) {
|
||||
size_t dummy = 0;
|
||||
rawData[0] = static_cast<uint8_t>(imtq::RequestType::ACTUATE);
|
||||
rawData[1] = static_cast<uint8_t>(imtq::SpecialRequest::NONE);
|
||||
uint8_t* serPtr = rawData + 2;
|
||||
SerializeAdapter::serialize(&dipoleX, &serPtr, &dummy, maxSize,
|
||||
SerializeIF::Endianness::MACHINE);
|
||||
SerializeAdapter::serialize(&dipoleY, &serPtr, &dummy, maxSize,
|
||||
SerializeIF::Endianness::MACHINE);
|
||||
SerializeAdapter::serialize(&dipoleZ, &serPtr, &dummy, maxSize,
|
||||
SerializeIF::Endianness::MACHINE);
|
||||
SerializeAdapter::serialize(&torqueDuration, &serPtr, &dummy, maxSize,
|
||||
SerializeIF::Endianness::MACHINE);
|
||||
}
|
||||
|
||||
uint8_t* startOfActuateDataPtr() { return rawData + 2; }
|
||||
|
||||
int16_t* getDipoles() { return reinterpret_cast<int16_t*>(rawData + 2); }
|
||||
|
||||
uint16_t getTorqueDuration() {
|
||||
uint8_t* data = rawData + 2 + 6;
|
||||
uint16_t value = 0;
|
||||
size_t dummy = 0;
|
||||
SerializeAdapter::deSerialize(&value, data, &dummy, SerializeIF::Endianness::MACHINE);
|
||||
return value;
|
||||
}
|
||||
|
||||
void setSpecialRequest(imtq::SpecialRequest specialRequest) {
|
||||
rawData[1] = static_cast<uint8_t>(specialRequest);
|
||||
}
|
||||
|
||||
imtq::SpecialRequest getSpecialRequest() const {
|
||||
return static_cast<imtq::SpecialRequest>(rawData[1]);
|
||||
}
|
||||
|
||||
private:
|
||||
ImtqRequest(uint8_t* rawData, size_t maxLen) : rawData(rawData) {
|
||||
if (rawData != nullptr) {
|
||||
rawData[0] = static_cast<uint8_t>(imtq::RequestType::MEASURE_NO_ACTUATION);
|
||||
}
|
||||
}
|
||||
uint8_t* rawData;
|
||||
size_t maxSize = 0;
|
||||
imtq::RequestType request = imtq::RequestType::MEASURE_NO_ACTUATION;
|
||||
imtq::SpecialRequest specialRequest = imtq::SpecialRequest::NONE;
|
||||
int16_t dipoles[3]{};
|
||||
uint16_t torqueDuration = 0;
|
||||
};
|
||||
|
||||
struct ImtqRepliesDefault {
|
||||
|
||||
2
tmtc
2
tmtc
@ -1 +1 @@
|
||||
Subproject commit 94ae2d16e21ade8f89723b2e62356967a67b171d
|
||||
Subproject commit 09c694cf9c72762114de760f728a76051b3745d9
|
||||
Loading…
Reference in New Issue
Block a user