Robin Mueller
d0613727ba
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
434 lines
14 KiB
C++
434 lines
14 KiB
C++
#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;
|
|
switch (currentRequest) {
|
|
case imtq::RequestType::MEASURE_NO_ACTUATION: {
|
|
handleMeasureStep();
|
|
break;
|
|
}
|
|
case imtq::RequestType::ACTUATE: {
|
|
handleActuateStep();
|
|
break;
|
|
}
|
|
};
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
void ImtqPollingTask::handleMeasureStep() {
|
|
size_t replyLen = 0;
|
|
uint8_t* replyPtr;
|
|
ImtqRepliesDefault replies(replyBuf.data());
|
|
// Can be used later to verify correct timing (e.g. all data has been read)
|
|
clearReadFlagsDefault(replies);
|
|
auto i2cCmdExecMeasure = [&](imtq::CC::CC cc) {
|
|
ccToReplyPtrMeasure(replies, cc, &replyPtr, replyLen);
|
|
return i2cCmdExecDefault(cc, replyPtr, replyLen, imtq::MGM_MEASUREMENT_LOW_LEVEL_ERROR);
|
|
};
|
|
|
|
cmdLen = 1;
|
|
cmdBuf[0] = imtq::CC::GET_SYSTEM_STATE;
|
|
if (i2cCmdExecMeasure(imtq::CC::GET_SYSTEM_STATE) != returnvalue::OK) {
|
|
return;
|
|
}
|
|
|
|
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);
|
|
break;
|
|
}
|
|
case (imtq::SpecialRequest::DO_SELF_TEST_NEG_X): {
|
|
executeSelfTest(imtq::selfTest::Axis::X_NEGATIVE);
|
|
break;
|
|
}
|
|
case (imtq::SpecialRequest::DO_SELF_TEST_POS_Y): {
|
|
executeSelfTest(imtq::selfTest::Axis::Y_POSITIVE);
|
|
break;
|
|
}
|
|
case (imtq::SpecialRequest::DO_SELF_TEST_NEG_Y): {
|
|
executeSelfTest(imtq::selfTest::Axis::Y_NEGATIVE);
|
|
break;
|
|
}
|
|
case (imtq::SpecialRequest::DO_SELF_TEST_POS_Z): {
|
|
executeSelfTest(imtq::selfTest::Axis::Z_POSITIVE);
|
|
break;
|
|
}
|
|
case (imtq::SpecialRequest::DO_SELF_TEST_NEG_Z): {
|
|
executeSelfTest(imtq::selfTest::Axis::Z_NEGATIVE);
|
|
break;
|
|
}
|
|
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
|
|
break;
|
|
}
|
|
}
|
|
// We are done. Only request self test or results here.
|
|
return;
|
|
}
|
|
}
|
|
|
|
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.
|
|
TaskFactory::delayTask(currentIntegrationTimeMs);
|
|
|
|
cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT;
|
|
if (i2cCmdExecMeasure(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
|
|
return;
|
|
}
|
|
|
|
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;
|
|
}
|
|
// sif::debug << "measure done" << std::endl;
|
|
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;
|
|
}
|
|
|
|
cmdLen = 1;
|
|
cmdBuf[0] = imtq::CC::START_MTM_MEASUREMENT;
|
|
if (i2cCmdExecActuate(imtq::CC::START_MTM_MEASUREMENT) != returnvalue::OK) {
|
|
return;
|
|
}
|
|
|
|
TaskFactory::delayTask(currentIntegrationTimeMs);
|
|
|
|
cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT;
|
|
if (i2cCmdExecActuate(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
|
|
return;
|
|
}
|
|
cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA;
|
|
if (i2cCmdExecActuate(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) {
|
|
return;
|
|
}
|
|
// sif::debug << "measure with torque done" << std::endl;
|
|
return;
|
|
}
|
|
|
|
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) {
|
|
ImtqRequest request(sendData, sendLen);
|
|
{
|
|
MutexGuard mg(ipcLock);
|
|
currentRequest = request.getRequestType();
|
|
if (currentRequest == imtq::RequestType::ACTUATE) {
|
|
std::memcpy(dipoles, request.getDipoles(), 6);
|
|
torqueDuration = request.getTorqueDuration();
|
|
}
|
|
specialRequest = request.getSpecialRequest();
|
|
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):
|
|
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): {
|
|
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);
|
|
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);
|
|
if (currentRequest == imtq::RequestType::MEASURE_NO_ACTUATION) {
|
|
replyLen = getExchangeBufLen(specialRequest);
|
|
memcpy(exchangeBuf.data(), replyBuf.data(), replyLen);
|
|
} else {
|
|
replyLen = ImtqRepliesWithTorque::BASE_LEN;
|
|
memcpy(exchangeBuf.data(), replyBufActuation.data(), replyLen);
|
|
}
|
|
*buffer = exchangeBuf.data();
|
|
*size = replyLen;
|
|
return comStatus;
|
|
}
|
|
|
|
void ImtqPollingTask::clearReadFlagsDefault(ImtqRepliesDefault& replies) {
|
|
replies.calibMgmMeasurement[0] = false;
|
|
replies.rawMgmMeasurement[0] = false;
|
|
replies.systemState[0] = false;
|
|
replies.specialRequestReply[0] = false;
|
|
replies.engHk[0] = false;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
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;
|
|
}
|