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:
@ -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;
|
||||
|
||||
Reference in New Issue
Block a user