finish acs and com
This commit is contained in:
220
linux/acs/SusPolling.cpp
Normal file
220
linux/acs/SusPolling.cpp
Normal file
@ -0,0 +1,220 @@
|
||||
#include "SusPolling.h"
|
||||
|
||||
#include <fsfw/tasks/SemaphoreFactory.h>
|
||||
#include <fsfw/tasks/TaskFactory.h>
|
||||
#include <fsfw/timemanager/Stopwatch.h>
|
||||
#include <fsfw_hal/linux/spi/SpiCookie.h>
|
||||
#include <mission/controller/acs/AcsParameters.h>
|
||||
#include <mission/devices/max1227.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include "mission/devices/devicedefinitions/susMax1227Helpers.h"
|
||||
|
||||
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();
|
||||
// Give SUS handlers a chance to submit all requests.
|
||||
TaskFactory::delayTask(2);
|
||||
{
|
||||
// Takes 4-5 ms in debug mode.
|
||||
// Stopwatch watch;
|
||||
handleSusPolling();
|
||||
}
|
||||
// Protection against tardy tasks unlocking the thread again immediately.
|
||||
TaskFactory::delayTask(20);
|
||||
}
|
||||
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);
|
||||
MutexGuard mg(ipcLock);
|
||||
if (susDevs[susIdx].mode != susReq->mode) {
|
||||
if (susReq->mode == acs::SimpleSensorMode::NORMAL) {
|
||||
susDevs[susIdx].performStartup = true;
|
||||
} else {
|
||||
susDevs[susIdx].ownReply.cfgWasSet = false;
|
||||
susDevs[susIdx].ownReply.dataWasSet = false;
|
||||
}
|
||||
susDevs[susIdx].mode = susReq->mode;
|
||||
}
|
||||
if (state == InternalState::IDLE) {
|
||||
state = InternalState::IS_BUSY;
|
||||
semaphore->release();
|
||||
}
|
||||
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) {
|
||||
auto* spiCookie = dynamic_cast<SpiCookie*>(cookie);
|
||||
if (spiCookie == nullptr) {
|
||||
return FAILED;
|
||||
}
|
||||
int susIdx = addressToIndex(spiCookie->getSpiAddress());
|
||||
if (susIdx < 0) {
|
||||
return FAILED;
|
||||
}
|
||||
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);
|
||||
return OK;
|
||||
}
|
||||
|
||||
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];
|
||||
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;
|
||||
}
|
||||
}
|
||||
return OK;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user