eive-obsw/linux/acs/SusPolling.cpp

233 lines
6.8 KiB
C++
Raw Normal View History

2023-02-28 19:14:15 +01:00
#include "SusPolling.h"
#include <fsfw/tasks/SemaphoreFactory.h>
2023-03-01 16:36:21 +01:00
#include <fsfw/tasks/TaskFactory.h>
2023-03-01 18:24:44 +01:00
#include <fsfw/timemanager/Stopwatch.h>
2023-02-28 19:14:15 +01:00
#include <fsfw_hal/linux/spi/SpiCookie.h>
2023-03-26 16:42:00 +02:00
#include <mission/acs/susMax1227Helpers.h>
2023-03-01 16:36:21 +01:00
#include <mission/controller/acs/AcsParameters.h>
2023-03-26 16:42:00 +02:00
#include <mission/tcs/max1227.h>
2023-03-01 16:36:21 +01:00
#include <unistd.h>
2023-02-28 19:14:15 +01:00
using namespace returnvalue;
SusPolling::SusPolling(object_id_t objectId, SpiComIF& spiComIF, GpioIF& gpioIF)
: SystemObject(objectId), spiComIF(spiComIF), gpioIF(gpioIF) {
semaphore = SemaphoreFactory::instance()->createBinarySemaphore();
semaphore->acquire();
ipcLock = MutexFactory::instance()->createMutex();
}
ReturnValue_t SusPolling::performOperation(uint8_t operationCode) {
while (true) {
ipcLock->lockMutex();
state = InternalState::IDLE;
ipcLock->unlockMutex();
semaphore->acquire();
2023-03-01 16:36:21 +01:00
// Give SUS handlers a chance to submit all requests.
TaskFactory::delayTask(2);
2023-03-01 18:24:44 +01:00
{
// Takes 4-5 ms in debug mode.
// Stopwatch watch;
handleSusPolling();
}
2023-03-01 16:36:21 +01:00
// Protection against tardy tasks unlocking the thread again immediately.
TaskFactory::delayTask(20);
2023-02-28 19:14:15 +01:00
}
return OK;
}
ReturnValue_t SusPolling::initialize() { return OK; }
ReturnValue_t SusPolling::initializeInterface(CookieIF* cookie) {
auto* spiCookie = dynamic_cast<SpiCookie*>(cookie);
if (spiCookie == nullptr) {
return FAILED;
}
int susIdx = addressToIndex(spiCookie->getSpiAddress());
if (susIdx < 0) {
return FAILED;
}
susDevs[susIdx].cookie = spiCookie;
return spiComIF.initializeInterface(cookie);
}
ReturnValue_t SusPolling::sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) {
auto* spiCookie = dynamic_cast<SpiCookie*>(cookie);
if (spiCookie == nullptr) {
return FAILED;
}
int susIdx = addressToIndex(spiCookie->getSpiAddress());
if (susIdx < 0) {
return FAILED;
}
if (sendLen != sizeof(acs::SusRequest)) {
return FAILED;
}
const auto* susReq = reinterpret_cast<const acs::SusRequest*>(sendData);
2023-03-01 10:47:57 +01:00
MutexGuard mg(ipcLock);
2023-02-28 19:14:15 +01:00
if (susDevs[susIdx].mode != susReq->mode) {
2023-03-01 16:36:21 +01:00
if (susReq->mode == acs::SimpleSensorMode::NORMAL) {
susDevs[susIdx].performStartup = true;
2023-04-14 18:45:58 +02:00
susDevs[susIdx].replyResult = returnvalue::FAILED;
2023-03-01 10:47:57 +01:00
} else {
2023-03-01 16:36:21 +01:00
susDevs[susIdx].ownReply.cfgWasSet = false;
2023-03-01 10:47:57 +01:00
susDevs[susIdx].ownReply.dataWasSet = false;
2023-04-14 18:45:58 +02:00
// We are off now, but DHB wants a proper reply.
susDevs[susIdx].replyResult = returnvalue::OK;
2023-03-01 10:47:57 +01:00
}
susDevs[susIdx].mode = susReq->mode;
2023-02-28 19:14:15 +01:00
}
2023-03-01 16:36:21 +01:00
if (state == InternalState::IDLE) {
2023-03-24 00:59:41 +01:00
state = InternalState::IS_BUSY;
2023-03-01 16:36:21 +01:00
semaphore->release();
}
2023-02-28 19:14:15 +01:00
return OK;
}
ReturnValue_t SusPolling::getSendSuccess(CookieIF* cookie) { return OK; }
ReturnValue_t SusPolling::requestReceiveMessage(CookieIF* cookie, size_t requestLen) { return OK; }
ReturnValue_t SusPolling::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) {
2023-03-01 10:47:57 +01:00
auto* spiCookie = dynamic_cast<SpiCookie*>(cookie);
if (spiCookie == nullptr) {
return FAILED;
}
int susIdx = addressToIndex(spiCookie->getSpiAddress());
if (susIdx < 0) {
return FAILED;
}
2023-04-11 17:29:32 +02:00
if (susDevs[susIdx].replyResult != returnvalue::OK) {
2023-04-11 17:12:31 +02:00
return susDevs[susIdx].replyResult;
}
2023-03-01 10:47:57 +01:00
MutexGuard mg(ipcLock);
std::memcpy(&susDevs[susIdx].readerReply, &susDevs[susIdx].ownReply, sizeof(acs::SusReply));
*buffer = reinterpret_cast<uint8_t*>(&susDevs[susIdx].readerReply);
*size = sizeof(acs::SusReply);
2023-04-11 17:12:31 +02:00
return susDevs[susIdx].replyResult;
2023-02-28 19:14:15 +01:00
}
2023-03-01 16:36:21 +01:00
ReturnValue_t SusPolling::handleSusPolling() {
ReturnValue_t result;
acs::SimpleSensorMode modes[12];
bool performStartups[12]{};
bool cfgsWereSet[12]{};
uint8_t idx = 0;
{
MutexGuard mg(ipcLock);
for (idx = 0; idx < 12; idx++) {
modes[idx] = susDevs[idx].mode;
performStartups[idx] = susDevs[idx].performStartup;
}
}
for (idx = 0; idx < 12; idx++) {
if (modes[idx] == acs::SimpleSensorMode::NORMAL) {
if (performStartups[idx]) {
// Startup handling.
cmdBuf[0] = susMax1227::SETUP_INT_CLOKED;
result = spiComIF.sendMessage(susDevs[idx].cookie, cmdBuf.data(), 1);
if (result != OK) {
susDevs[idx].replyResult = result;
continue;
}
MutexGuard mg(ipcLock);
susDevs[idx].ownReply.cfgWasSet = true;
cfgsWereSet[idx] = true;
susDevs[idx].performStartup = true;
}
}
}
for (idx = 0; idx < 12; idx++) {
if (modes[idx] == acs::SimpleSensorMode::NORMAL and cfgsWereSet[idx]) {
// Regular sensor polling.
cmdBuf[0] = max1227::buildResetByte(true);
cmdBuf[1] = susMax1227::CONVERSION;
result = spiComIF.sendMessage(susDevs[idx].cookie, cmdBuf.data(), 2);
if (result != OK) {
susDevs[idx].replyResult = result;
continue;
}
}
}
// Internal conversion time is 3.5 us
usleep(4);
for (idx = 0; idx < 12; idx++) {
if (modes[idx] == acs::SimpleSensorMode::NORMAL and cfgsWereSet[idx]) {
std::memset(cmdBuf.data(), 0, susMax1227::SIZE_READ_INT_CONVERSIONS);
result = spiComIF.sendMessage(susDevs[idx].cookie, cmdBuf.data(),
susMax1227::SIZE_READ_INT_CONVERSIONS);
if (result != OK) {
susDevs[idx].replyResult = result;
continue;
}
result = spiComIF.readReceivedMessage(susDevs[idx].cookie, &rawReply, &dummy);
if (result != OK) {
susDevs[idx].replyResult = result;
continue;
}
MutexGuard mg(ipcLock);
susDevs[idx].ownReply.tempRaw = ((rawReply[0] & 0x0f) << 8) | rawReply[1];
2023-04-11 17:12:31 +02:00
// Reply is all ones. Sensor is probably off or faulty when
// it should not be.
2023-04-11 17:29:32 +02:00
if (susDevs[idx].ownReply.tempRaw == 0x0fff) {
2023-04-11 17:12:31 +02:00
susDevs[idx].replyResult = returnvalue::FAILED;
} else {
susDevs[idx].replyResult = returnvalue::OK;
for (unsigned chIdx = 0; chIdx < 6; chIdx++) {
susDevs[idx].ownReply.channelsRaw[chIdx] =
(rawReply[chIdx * 2 + 2] << 8) | rawReply[chIdx * 2 + 3];
}
susDevs[idx].ownReply.dataWasSet = true;
2023-03-01 16:36:21 +01:00
}
}
}
return OK;
}
2023-02-28 19:14:15 +01:00
int SusPolling::addressToIndex(address_t addr) {
switch (addr) {
case (addresses::SUS_0):
return 0;
break;
case (addresses::SUS_1):
return 1;
break;
case (addresses::SUS_2):
return 2;
break;
case (addresses::SUS_3):
return 3;
break;
case (addresses::SUS_4):
return 4;
break;
case (addresses::SUS_5):
return 5;
break;
case (addresses::SUS_6):
return 6;
break;
case (addresses::SUS_7):
return 7;
break;
case (addresses::SUS_8):
return 8;
break;
case (addresses::SUS_9):
return 9;
break;
case (addresses::SUS_10):
return 10;
break;
case (addresses::SUS_11):
return 11;
break;
default: {
return -1;
}
}
}