a lot of bugfixes for IMTQ
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
This commit is contained in:
parent
f6f3f17505
commit
c8a2395d61
@ -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();
|
||||
|
||||
@ -35,6 +35,9 @@ ReturnValue_t ImtqPollingTask::performOperation(uint8_t operationCode) {
|
||||
handleActuateStep();
|
||||
break;
|
||||
}
|
||||
default: {
|
||||
break;
|
||||
}
|
||||
};
|
||||
}
|
||||
return returnvalue::OK;
|
||||
@ -192,15 +195,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;
|
||||
}
|
||||
@ -325,9 +328,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;
|
||||
|
||||
@ -198,9 +198,9 @@ void AcsController::performSafe() {
|
||||
|
||||
updateCtrlValData(errAng);
|
||||
updateActuatorCmdData(cmdDipolMtqs);
|
||||
// commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
|
||||
// acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
|
||||
// acsParameters.rwHandlingParameters.rampTime);
|
||||
//commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2] /*500, 500, 500 */,
|
||||
// acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
|
||||
// acsParameters.rwHandlingParameters.rampTime);
|
||||
}
|
||||
|
||||
void AcsController::performDetumble() {
|
||||
|
||||
@ -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): {
|
||||
requestStep = imtq::RequestType::ACTUATE;
|
||||
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;
|
||||
}
|
||||
@ -133,6 +156,9 @@ ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
|
||||
*id = imtq::cmdIds::START_ACTUATION_DIPOLE;
|
||||
return buildCommandFromCommand(*id, nullptr, 0);
|
||||
}
|
||||
default: {
|
||||
return NOTHING_TO_SEND;
|
||||
}
|
||||
}
|
||||
return NOTHING_TO_SEND;
|
||||
}
|
||||
@ -145,11 +171,13 @@ 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);
|
||||
// ImtqRequest request(commandBuffer, sizeof(commandBuffer));
|
||||
request.request = imtq::RequestType::MEASURE_NO_ACTUATION;
|
||||
request.specialRequest = specialRequest;
|
||||
// request.setMeasureRequest(specialRequest);
|
||||
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,10 +209,10 @@ 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;
|
||||
rawPacket = reinterpret_cast<uint8_t*>(&request);
|
||||
rawPacketLen = sizeof(ImtqRequest);
|
||||
return returnvalue::OK;
|
||||
}
|
||||
case (imtq::cmdIds::START_ACTUATION_DIPOLE): {
|
||||
@ -193,7 +221,6 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
|
||||
if (commandData != nullptr && commandDataLen < 8) {
|
||||
return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
|
||||
}
|
||||
ImtqRequest request(commandBuffer, sizeof(commandBuffer));
|
||||
{
|
||||
PoolReadGuard pg(&dipoleSet);
|
||||
// Commands override anything which was set in the software
|
||||
@ -205,22 +232,26 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
|
||||
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);
|
||||
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:
|
||||
@ -264,7 +295,6 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
|
||||
}
|
||||
// arrayprinter::print(packet, ImtqReplies::BASE_LEN);
|
||||
if (requestStep == imtq::RequestType::MEASURE_NO_ACTUATION) {
|
||||
requestStep = imtq::RequestType::ACTUATE;
|
||||
// sif::debug << "handle measure" << std::endl;
|
||||
ImtqRepliesDefault replies(packet);
|
||||
if (specialRequestActive) {
|
||||
@ -329,9 +359,8 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
|
||||
} else {
|
||||
status = result;
|
||||
}
|
||||
} else {
|
||||
} else if (requestStep == imtq::RequestType::ACTUATE) {
|
||||
// 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());
|
||||
@ -411,21 +440,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 */
|
||||
@ -433,8 +467,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}));
|
||||
@ -733,7 +770,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;
|
||||
}
|
||||
@ -762,10 +799,12 @@ ReturnValue_t ImtqHandler::parseStatusByte(imtq::CC::CC command, const uint8_t*
|
||||
sif::error << "IMTQ::parseStatusByte: IMTQ replied internal processing error" << 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) {
|
||||
@ -779,20 +818,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,
|
||||
@ -806,12 +845,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
|
||||
}
|
||||
@ -852,7 +894,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;
|
||||
|
||||
@ -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<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];
|
||||
// uint8_t commandBuffer[imtq::MAX_COMMAND_SIZE];
|
||||
ImtqRequest request{};
|
||||
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,
|
||||
@ -195,20 +195,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 +484,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 +559,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 +645,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 +1124,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 {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user