eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

IMTQ Commanding Fix #430

Merged
muellerr merged 9 commits from imtq-cmd-fix into develop 2023-03-04 17:07:07 +01:00
Conversation 0 Commits 9 Files Changed 11 +205 -177
6 changed files with 205 additions and 177 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit c8a2395d61 - Show all commits

2
bsp_q7s/fs/SdCardManager.h
View File

@ -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();

19
linux/devices/ImtqPollingTask.cpp
View File

@ -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;

6
mission/controller/AcsController.cpp
View File

@ -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() {

140
mission/devices/ImtqHandler.cpp
View File

@ -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;

19
mission/devices/ImtqHandler.h
View File

@ -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;

196
mission/devices/devicedefinitions/imtqHelpers.h
View File

@ -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 {