eive-obsw/linux/devices/ImtqPollingTask.cpp

468 lines
15 KiB
C++
Raw Normal View History

2023-02-19 12:25:26 +01:00
#include "ImtqPollingTask.h"
#include <fcntl.h>
#include <fsfw/tasks/SemaphoreFactory.h>
#include <fsfw/tasks/TaskFactory.h>
#include <fsfw/timemanager/Stopwatch.h>
#include <fsfw_hal/linux/UnixFileGuard.h>
#include <linux/i2c-dev.h>
#include <sys/ioctl.h>
#include "fsfw/FSFW.h"
ImtqPollingTask::ImtqPollingTask(object_id_t imtqPollingTask) : SystemObject(imtqPollingTask) {
semaphore = SemaphoreFactory::instance()->createBinarySemaphore();
semaphore->acquire();
ipcLock = MutexFactory::instance()->createMutex();
bufLock = MutexFactory::instance()->createMutex();
}
ReturnValue_t ImtqPollingTask::performOperation(uint8_t operationCode) {
while (true) {
ipcLock->lockMutex();
state = InternalState::IDLE;
ipcLock->unlockMutex();
semaphore->acquire();
comStatus = returnvalue::OK;
// Stopwatch watch;
2023-02-20 02:32:48 +01:00
switch (currentRequest) {
2023-02-20 19:35:36 +01:00
case imtq::RequestType::MEASURE_NO_ACTUATION: {
2023-03-06 11:28:25 +01:00
// Measured to take 24 ms for debug and release builds.
// Stopwatch watch;
2023-02-20 02:32:48 +01:00
handleMeasureStep();
break;
2023-02-19 12:25:26 +01:00
}
2023-02-20 02:32:48 +01:00
case imtq::RequestType::ACTUATE: {
handleActuateStep();
break;
2023-02-19 12:25:26 +01:00
}
2023-03-04 14:32:18 +01:00
default: {
break;
}
2023-02-19 12:25:26 +01:00
};
2023-02-20 02:32:48 +01:00
}
return returnvalue::OK;
}
2023-02-19 12:25:26 +01:00
2023-02-20 02:32:48 +01:00
void ImtqPollingTask::handleMeasureStep() {
size_t replyLen = 0;
uint8_t* replyPtr;
ImtqRepliesDefault replies(replyBuf.data());
2023-03-06 09:17:03 +01:00
// If some startup handling is added later, set configured after it was done once.
replies.setConfigured();
2023-02-20 02:32:48 +01:00
// Can be used later to verify correct timing (e.g. all data has been read)
2023-02-20 19:55:03 +01:00
clearReadFlagsDefault(replies);
2023-02-20 02:32:48 +01:00
auto i2cCmdExecMeasure = [&](imtq::CC::CC cc) {
ccToReplyPtrMeasure(replies, cc, &replyPtr, replyLen);
return i2cCmdExecDefault(cc, replyPtr, replyLen, imtq::MGM_MEASUREMENT_LOW_LEVEL_ERROR);
};
2023-02-19 12:25:26 +01:00
2023-02-20 02:32:48 +01:00
cmdLen = 1;
cmdBuf[0] = imtq::CC::GET_SYSTEM_STATE;
if (i2cCmdExecMeasure(imtq::CC::GET_SYSTEM_STATE) != returnvalue::OK) {
return;
}
2023-02-19 12:25:26 +01:00
2023-02-20 02:32:48 +01:00
ignoreNextActuateRequest =
(replies.getSystemState()[2] == static_cast<uint8_t>(imtq::mode::SELF_TEST));
if (ignoreNextActuateRequest) {
// Do not command anything until self-test is done.
return;
}
if (specialRequest != imtq::SpecialRequest::NONE) {
auto executeSelfTest = [&](imtq::selfTest::Axis axis) {
cmdBuf[0] = imtq::CC::SELF_TEST_CMD;
cmdBuf[1] = axis;
return i2cCmdExecMeasure(imtq::CC::SELF_TEST_CMD);
};
// If a self-test is already ongoing, ignore the request.
if (replies.getSystemState()[2] != static_cast<uint8_t>(imtq::mode::SELF_TEST)) {
switch (specialRequest) {
case (imtq::SpecialRequest::DO_SELF_TEST_POS_X): {
executeSelfTest(imtq::selfTest::Axis::X_POSITIVE);
2023-02-19 12:25:26 +01:00
break;
}
2023-02-20 02:32:48 +01:00
case (imtq::SpecialRequest::DO_SELF_TEST_NEG_X): {
executeSelfTest(imtq::selfTest::Axis::X_NEGATIVE);
2023-02-19 12:25:26 +01:00
break;
}
2023-02-20 02:32:48 +01:00
case (imtq::SpecialRequest::DO_SELF_TEST_POS_Y): {
executeSelfTest(imtq::selfTest::Axis::Y_POSITIVE);
2023-02-19 12:25:26 +01:00
break;
}
2023-02-20 02:32:48 +01:00
case (imtq::SpecialRequest::DO_SELF_TEST_NEG_Y): {
executeSelfTest(imtq::selfTest::Axis::Y_NEGATIVE);
2023-02-19 12:25:26 +01:00
break;
}
2023-02-20 02:32:48 +01:00
case (imtq::SpecialRequest::DO_SELF_TEST_POS_Z): {
executeSelfTest(imtq::selfTest::Axis::Z_POSITIVE);
2023-02-19 12:25:26 +01:00
break;
}
2023-02-20 02:32:48 +01:00
case (imtq::SpecialRequest::DO_SELF_TEST_NEG_Z): {
executeSelfTest(imtq::selfTest::Axis::Z_NEGATIVE);
2023-02-19 12:25:26 +01:00
break;
}
2023-02-20 02:32:48 +01:00
case (imtq::SpecialRequest::GET_SELF_TEST_RESULT): {
cmdBuf[0] = imtq::CC::GET_SELF_TEST_RESULT;
i2cCmdExecMeasure(imtq::CC::GET_SELF_TEST_RESULT);
break;
}
default: {
// Should never happen
2023-02-19 12:25:26 +01:00
break;
}
}
2023-02-20 02:32:48 +01:00
// We are done. Only request self test or results here.
return;
}
2023-02-19 12:25:26 +01:00
}
2023-02-20 02:32:48 +01:00
cmdBuf[0] = imtq::CC::START_MTM_MEASUREMENT;
if (i2cCmdExecMeasure(imtq::CC::START_MTM_MEASUREMENT) != returnvalue::OK) {
return;
}
// Takes a bit of time to take measurements. Subtract a bit because of the delay of previous
// commands.
2023-03-04 17:06:23 +01:00
TaskFactory::delayTask(currentIntegrationTimeMs + MGM_READ_BUFFER_TIME_MS);
2023-02-20 02:32:48 +01:00
cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT;
if (i2cCmdExecMeasure(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
return;
}
2023-03-06 11:28:25 +01:00
bool mgmMeasurementTooOld = false;
2023-03-04 17:06:23 +01:00
// 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) {
2023-03-06 11:28:25 +01:00
replyPtr[0] = false;
mgmMeasurementTooOld = true;
2023-03-04 17:06:23 +01:00
}
2023-02-20 02:32:48 +01:00
cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA;
if (i2cCmdExecMeasure(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) {
return;
}
cmdBuf[0] = imtq::CC::GET_CAL_MTM_MEASUREMENT;
if (i2cCmdExecMeasure(imtq::CC::GET_CAL_MTM_MEASUREMENT) != returnvalue::OK) {
return;
}
2023-03-06 11:46:37 +01:00
if (mgmMeasurementTooOld) {
2023-03-06 11:28:25 +01:00
sif::error << "IMTQ: MGM measurement too old" << std::endl;
}
2023-02-20 02:32:48 +01:00
return;
}
void ImtqPollingTask::handleActuateStep() {
uint8_t* replyPtr = nullptr;
size_t replyLen = 0;
// No point when self-test mode is active.
if (ignoreNextActuateRequest) {
return;
}
ImtqRepliesWithTorque replies(replyBufActuation.data());
// Can be used later to verify correct timing (e.g. all data has been read)
clearReadFlagsWithTorque(replies);
auto i2cCmdExecActuate = [&](imtq::CC::CC cc) {
ccToReplyPtrActuate(replies, cc, &replyPtr, replyLen);
return i2cCmdExecDefault(cc, replyPtr, replyLen, imtq::ACTUATE_CMD_LOW_LEVEL_ERROR);
};
buildDipoleCommand();
if (i2cCmdExecActuate(imtq::CC::START_ACTUATION_DIPOLE) != returnvalue::OK) {
return;
}
2023-03-04 17:06:23 +01:00
TaskFactory::delayTask(10);
2023-02-20 02:32:48 +01:00
cmdLen = 1;
cmdBuf[0] = imtq::CC::START_MTM_MEASUREMENT;
if (i2cCmdExecActuate(imtq::CC::START_MTM_MEASUREMENT) != returnvalue::OK) {
return;
}
2023-03-04 17:06:23 +01:00
TaskFactory::delayTask(currentIntegrationTimeMs + MGM_READ_BUFFER_TIME_MS);
2023-02-20 02:32:48 +01:00
cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT;
if (i2cCmdExecActuate(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
return;
}
2023-03-06 11:28:25 +01:00
bool measurementWasTooOld = false;
2023-03-04 17:06:23 +01:00
// 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) {
2023-03-06 11:28:25 +01:00
measurementWasTooOld = true;
replyPtr[0] = false;
2023-03-04 17:06:23 +01:00
}
2023-02-20 02:32:48 +01:00
cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA;
if (i2cCmdExecActuate(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) {
return;
}
2023-03-06 11:28:25 +01:00
2023-03-06 11:46:37 +01:00
if (measurementWasTooOld) {
2023-03-06 11:28:25 +01:00
sif::error << "IMTQ: MGM measurement too old" << std::endl;
}
2023-02-20 02:32:48 +01:00
return;
2023-02-19 12:25:26 +01:00
}
ReturnValue_t ImtqPollingTask::initialize() { return returnvalue::OK; }
ReturnValue_t ImtqPollingTask::initializeInterface(CookieIF* cookie) {
i2cCookie = dynamic_cast<I2cCookie*>(cookie);
if (i2cCookie == nullptr) {
sif::error << "ImtqPollingTask::initializeInterface: Invalid I2C cookie" << std::endl;
return returnvalue::FAILED;
}
i2cDev = i2cCookie->getDeviceFile().c_str();
i2cAddr = i2cCookie->getAddress();
return returnvalue::OK;
}
ReturnValue_t ImtqPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendData,
size_t sendLen) {
2023-03-06 09:17:03 +01:00
const auto* imtqReq = reinterpret_cast<const imtq::Request*>(sendData);
2023-02-19 12:25:26 +01:00
{
MutexGuard mg(ipcLock);
2023-03-04 14:32:18 +01:00
if (imtqReq->request == imtq::RequestType::ACTUATE) {
std::memcpy(dipoles, imtqReq->dipoles, sizeof(dipoles));
torqueDuration = imtqReq->torqueDuration;
2023-02-19 12:25:26 +01:00
}
2023-03-04 14:32:18 +01:00
currentRequest = imtqReq->request;
specialRequest = imtqReq->specialRequest;
2023-02-19 12:25:26 +01:00
if (state != InternalState::IDLE) {
return returnvalue::FAILED;
}
state = InternalState::BUSY;
}
semaphore->release();
return returnvalue::OK;
}
ReturnValue_t ImtqPollingTask::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; }
ReturnValue_t ImtqPollingTask::requestReceiveMessage(CookieIF* cookie, size_t requestLen) {
return returnvalue::OK;
}
void ImtqPollingTask::ccToReplyPtrMeasure(ImtqRepliesDefault& replies, imtq::CC::CC cc,
uint8_t** replyBuf, size_t& replyLen) {
replyLen = imtq::getReplySize(cc);
switch (cc) {
case (imtq::CC::CC::GET_ENG_HK_DATA): {
*replyBuf = replies.engHk;
break;
}
case (imtq::CC::CC::SOFTWARE_RESET): {
*replyBuf = replies.swReset;
break;
}
case (imtq::CC::CC::GET_SYSTEM_STATE): {
*replyBuf = replies.systemState;
break;
}
case (imtq::CC::CC::START_MTM_MEASUREMENT): {
*replyBuf = replies.startMtmMeasurement;
break;
}
case (imtq::CC::CC::GET_RAW_MTM_MEASUREMENT): {
*replyBuf = replies.rawMgmMeasurement;
break;
}
case (imtq::CC::CC::GET_CAL_MTM_MEASUREMENT): {
*replyBuf = replies.calibMgmMeasurement;
break;
}
default: {
*replyBuf = replies.specialRequestReply;
break;
}
}
}
void ImtqPollingTask::ccToReplyPtrActuate(ImtqRepliesWithTorque& replies, imtq::CC::CC cc,
uint8_t** replyBuf, size_t& replyLen) {
replyLen = imtq::getReplySize(cc);
switch (cc) {
case (imtq::CC::CC::START_ACTUATION_DIPOLE): {
*replyBuf = replies.dipoleActuation;
break;
}
case (imtq::CC::CC::GET_ENG_HK_DATA): {
*replyBuf = replies.engHk;
break;
}
case (imtq::CC::CC::START_MTM_MEASUREMENT): {
*replyBuf = replies.startMtmMeasurement;
break;
}
case (imtq::CC::CC::GET_RAW_MTM_MEASUREMENT): {
*replyBuf = replies.rawMgmMeasurement;
break;
}
default: {
*replyBuf = nullptr;
replyLen = 0;
break;
}
}
}
size_t ImtqPollingTask::getExchangeBufLen(imtq::SpecialRequest specialRequest) {
size_t baseLen = ImtqRepliesDefault::BASE_LEN;
switch (specialRequest) {
case (imtq::SpecialRequest::NONE):
2023-02-20 02:32:48 +01:00
case (imtq::SpecialRequest::DO_SELF_TEST_POS_X):
case (imtq::SpecialRequest::DO_SELF_TEST_NEG_X):
case (imtq::SpecialRequest::DO_SELF_TEST_POS_Y):
case (imtq::SpecialRequest::DO_SELF_TEST_NEG_Y):
case (imtq::SpecialRequest::DO_SELF_TEST_POS_Z):
case (imtq::SpecialRequest::DO_SELF_TEST_NEG_Z): {
2023-02-19 12:25:26 +01:00
break;
}
case (imtq::SpecialRequest::GET_SELF_TEST_RESULT): {
baseLen += imtq::replySize::SELF_TEST_RESULTS;
break;
}
}
return baseLen;
}
void ImtqPollingTask::buildDipoleCommand() {
cmdBuf[0] = imtq::CC::CC::START_ACTUATION_DIPOLE;
uint8_t* serPtr = cmdBuf.data() + 1;
size_t serLen = 0;
for (uint8_t idx = 0; idx < 3; idx++) {
SerializeAdapter::serialize(&dipoles[idx], &serPtr, &serLen, cmdBuf.size(),
SerializeIF::Endianness::LITTLE);
}
SerializeAdapter::serialize(&torqueDuration, &serPtr, &serLen, cmdBuf.size(),
SerializeIF::Endianness::LITTLE);
2023-03-04 17:06:23 +01:00
// sif::debug << "Dipole X: " << dipoles[0] << std::endl;
// sif::debug << "Torqeu Dur: " << torqueDuration << std::endl;
2023-02-19 12:25:26 +01:00
cmdLen = 1 + serLen;
}
ReturnValue_t ImtqPollingTask::readReceivedMessage(CookieIF* cookie, uint8_t** buffer,
size_t* size) {
imtq::RequestType currentRequest;
{
MutexGuard mg(ipcLock);
currentRequest = this->currentRequest;
}
size_t replyLen = 0;
MutexGuard mg(bufLock);
2023-02-20 19:35:36 +01:00
if (currentRequest == imtq::RequestType::MEASURE_NO_ACTUATION) {
2023-02-19 12:25:26 +01:00
replyLen = getExchangeBufLen(specialRequest);
memcpy(exchangeBuf.data(), replyBuf.data(), replyLen);
2023-03-04 14:32:18 +01:00
} else if (currentRequest == imtq::RequestType::ACTUATE) {
2023-02-19 12:25:26 +01:00
replyLen = ImtqRepliesWithTorque::BASE_LEN;
memcpy(exchangeBuf.data(), replyBufActuation.data(), replyLen);
2023-03-04 14:32:18 +01:00
} else {
*size = 0;
2023-02-19 12:25:26 +01:00
}
*buffer = exchangeBuf.data();
*size = replyLen;
return comStatus;
}
2023-02-20 02:32:48 +01:00
void ImtqPollingTask::clearReadFlagsDefault(ImtqRepliesDefault& replies) {
2023-02-19 12:25:26 +01:00
replies.calibMgmMeasurement[0] = false;
replies.rawMgmMeasurement[0] = false;
replies.systemState[0] = false;
replies.specialRequestReply[0] = false;
replies.engHk[0] = false;
}
2023-02-20 02:32:48 +01:00
ReturnValue_t ImtqPollingTask::i2cCmdExecDefault(imtq::CC::CC cc, uint8_t* replyPtr,
size_t replyLen, ReturnValue_t comErrIfFails) {
ReturnValue_t res = performI2cFullRequest(replyPtr + 1, replyLen);
if (res != returnvalue::OK) {
sif::error << "IMTQ: I2C transaction for command 0x" << std::hex << std::setw(2) << cc
<< " failed" << std::dec << std::endl;
comStatus = comErrIfFails;
return returnvalue::FAILED;
}
if (replyPtr[1] != cc) {
sif::warning << "IMTQ: Unexpected CC 0x" << std::hex << std::setw(2)
<< static_cast<int>(replyPtr[1]) << " for command 0x" << cc << std::dec
<< std::endl;
comStatus = comErrIfFails;
return returnvalue::FAILED;
}
replyPtr[0] = true;
return returnvalue::OK;
}
void ImtqPollingTask::clearReadFlagsWithTorque(ImtqRepliesWithTorque& replies) {
replies.dipoleActuation[0] = false;
replies.engHk[0] = false;
replies.rawMgmMeasurement[0] = false;
replies.startMtmMeasurement[0] = false;
}
2023-02-19 12:25:26 +01:00
ReturnValue_t ImtqPollingTask::performI2cFullRequest(uint8_t* reply, size_t replyLen) {
int fd = 0;
if (cmdLen == 0 or reply == nullptr) {
return returnvalue::FAILED;
}
{
UnixFileGuard fileHelper(i2cDev, fd, O_RDWR, "ImtqPollingTask::performI2cFullRequest");
if (fileHelper.getOpenResult() != returnvalue::OK) {
return fileHelper.getOpenResult();
}
if (ioctl(fd, I2C_SLAVE, i2cAddr) < 0) {
sif::warning << "Opening IMTQ slave device failed with code " << errno << ": "
<< strerror(errno) << std::endl;
}
int written = write(fd, cmdBuf.data(), cmdLen);
if (written < 0) {
sif::error << "IMTQ: Failed to send with error code " << errno
<< ". Error description: " << strerror(errno) << std::endl;
return returnvalue::FAILED;
} else if (static_cast<size_t>(written) != cmdLen) {
sif::error << "IMTQ: Could not write all bytes" << std::endl;
return returnvalue::FAILED;
}
}
#if FSFW_HAL_I2C_WIRETAPPING == 1
sif::info << "Sent I2C data to bus " << deviceFile << ":" << std::endl;
arrayprinter::print(sendData, sendLen);
#endif
// wait 1 ms like specified in the datasheet. This is the time the IMTQ needs
// to prepare a reply.
usleep(1000);
{
UnixFileGuard fileHelper(i2cDev, fd, O_RDWR, "ImtqPollingTask::performI2cFullRequest");
if (fileHelper.getOpenResult() != returnvalue::OK) {
return fileHelper.getOpenResult();
}
if (ioctl(fd, I2C_SLAVE, i2cAddr) < 0) {
sif::warning << "Opening IMTQ slave device failed with code " << errno << ": "
<< strerror(errno) << std::endl;
}
MutexGuard mg(bufLock);
int readLen = read(fd, reply, replyLen);
if (readLen != static_cast<int>(replyLen)) {
if (readLen < 0) {
sif::warning << "IMTQ: Reading failed with error code " << errno << " | " << strerror(errno)
<< std::endl;
} else {
sif::warning << "IMTQ: Read only" << readLen << " from " << replyLen << " bytes"
<< std::endl;
}
}
}
if (reply[0] == 0xff or reply[1] == 0xff) {
sif::warning << "IMTQ: No reply available after 1 millisecond";
return NO_REPLY_AVAILABLE;
}
return returnvalue::OK;
}