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 +70 -20
8 changed files with 70 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 4c1b79fd66 - Show all commits

2
common/config/eive/definitions.h
View File

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

34
linux/devices/ImtqPollingTask.cpp
View File

@ -121,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) {
@ -160,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;
@ -312,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;
}

2
linux/devices/ImtqPollingTask.h
View File

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

2
mission/controller/AcsController.cpp
View File

@ -198,7 +198,7 @@ void AcsController::performSafe() {
updateCtrlValData(errAng);
updateActuatorCmdData(cmdDipolMtqs);
//commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2] /*500, 500, 500 */,
//commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2]/*500, 500, 500*/,
// acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
// acsParameters.rwHandlingParameters.rampTime);
}

43
mission/devices/ImtqHandler.cpp
View File

@ -131,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 {
@ -140,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);
@ -157,7 +159,12 @@ ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
return buildCommandFromCommand(*id, nullptr, 0);
}
default: {
return NOTHING_TO_SEND;
*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;
@ -171,10 +178,9 @@ 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.request = imtq::RequestType::MEASURE_NO_ACTUATION;
request.specialRequest = specialRequest;
// request.setMeasureRequest(specialRequest);
expectedReply = imtq::cmdIds::REPLY_NO_TORQUE;
specialRequestActive = true;
rawPacket = reinterpret_cast<uint8_t*>(&request);
rawPacketLen = sizeof(ImtqRequest);
@ -211,17 +217,17 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
case (imtq::cmdIds::REQUEST): {
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;
}
{
// 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) {
@ -237,6 +243,7 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
}
}
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));
@ -263,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);
@ -280,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;
@ -294,7 +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) {
if (expectedReply == imtq::cmdIds::REPLY_NO_TORQUE) {
// sif::debug << "handle measure" << std::endl;
ImtqRepliesDefault replies(packet);
if (specialRequestActive) {
@ -359,7 +367,7 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
} else {
status = result;
}
} else if (requestStep == imtq::RequestType::ACTUATE) {
} else if (expectedReply == imtq::cmdIds::REPLY_WITH_TORQUE) {
// sif::debug << "handle measure with torque" << std::endl;
ImtqRepliesWithTorque replies(packet);
if (replies.wasDipoleActuationRead()) {
@ -405,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()) {
@ -747,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);
}
@ -793,10 +807,12 @@ 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 for command 0x" << std::setw(2)
@ -930,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;

2
mission/devices/ImtqHandler.h
View File

@ -123,8 +123,8 @@ class ImtqHandler : public DeviceHandlerBase {
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;

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

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

2
tmtc

@ -1 +1 @@
Subproject commit 94ae2d16e21ade8f89723b2e62356967a67b171d
Subproject commit 09c694cf9c72762114de760f728a76051b3745d9