402 lines
13 KiB
C++
402 lines
13 KiB
C++
|
#include "RwPollingTask.h"
|
||
|
|
|
||
|
|
#include <fcntl.h>
|
||
|
|
#include <fsfw/globalfunctions/CRC.h>
|
||
|
|
#include <fsfw/tasks/SemaphoreFactory.h>
|
||
|
|
#include <fsfw/tasks/TaskFactory.h>
|
||
|
|
#include <fsfw_hal/common/spi/spiCommon.h>
|
||
|
|
#include <unistd.h>
|
||
|
|
|
||
|
|
#include "devConf.h"
|
||
|
|
#include "mission/devices/devicedefinitions/rwHelpers.h"
|
||
|
|
|
||
|
|
RwPollingTask::RwPollingTask(object_id_t objectId, SpiComIF* spiIF)
|
||
|
|
: SystemObject(objectId), spiIF(spiIF) {
|
||
|
|
semaphore = SemaphoreFactory::instance()->createBinarySemaphore();
|
||
|
|
semaphore->acquire();
|
||
|
|
ipcLock = MutexFactory::instance()->createMutex();
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t RwPollingTask::performOperation(uint8_t operationCode) {
|
||
|
|
ipcLock->lockMutex();
|
||
|
|
state = InternalState::IDLE;
|
||
|
|
ipcLock->unlockMutex();
|
||
|
|
while (true) {
|
||
|
|
semaphore->acquire();
|
||
|
|
for (unsigned idx = 0; idx < rwCookies.size(); idx++) {
|
||
|
|
prepareSetSpeedCmd(idx);
|
||
|
|
writeOneRw(idx);
|
||
|
|
}
|
||
|
|
readAllRws(fd, spiLock, dev)
|
||
|
|
// writeAndReadAllRws(sendData, sendDataLen)
|
||
|
|
int bytesRead = 0;
|
||
|
|
}
|
||
|
|
return returnvalue::OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t RwPollingTask::initialize() { return returnvalue::OK; }
|
||
|
|
|
||
|
|
ReturnValue_t RwPollingTask::initializeInterface(CookieIF* cookie) {
|
||
|
|
spiIF->getSpiDev();
|
||
|
|
// We are in protected section, so we can use the static variable here without issues.
|
||
|
|
// We don't need to set the speed because a SPI core is used, but the mode has to be set once
|
||
|
|
// correctly for all RWs
|
||
|
|
if (not modeAndSpeedWasSet) {
|
||
|
|
auto& dev = spiIF->getSpiDev();
|
||
|
|
int fd = open(dev.c_str(), O_RDWR);
|
||
|
|
if (fd < 0) {
|
||
|
|
sif::error << "could not open RW SPI bus" << std::endl;
|
||
|
|
return returnvalue::FAILED;
|
||
|
|
}
|
||
|
|
spiIF->setSpiSpeedAndMode(fd, spi::RW_MODE, spi::RW_SPEED);
|
||
|
|
close(fd);
|
||
|
|
modeAndSpeedWasSet = true;
|
||
|
|
}
|
||
|
|
|
||
|
|
return returnvalue::OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t RwPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendData,
|
||
|
|
size_t sendLen) {
|
||
|
|
if (sendLen < 6) {
|
||
|
|
return DeviceHandlerIF::INVALID_DATA;
|
||
|
|
}
|
||
|
|
int32_t speed = 0;
|
||
|
|
uint16_t rampTime = 0;
|
||
|
|
SerializeAdapter::deSerialize(&speed, &sendData, &sendLen, SerializeIF::Endianness::MACHINE);
|
||
|
|
SerializeAdapter::deSerialize(&rampTime, &sendData, &sendLen, SerializeIF::Endianness::MACHINE);
|
||
|
|
rws::SpecialRwRequest specialRequest = rws::SpecialRwRequest::REQUEST_NONE;
|
||
|
|
if (sendLen == 7 and sendData[6] < rws::SpecialRwRequest::NUM_REQUESTS) {
|
||
|
|
specialRequest = static_cast<rws::SpecialRwRequest>(sendData[6]);
|
||
|
|
}
|
||
|
|
RwCookie* rwCookie = dynamic_cast<RwCookie*>(cookie);
|
||
|
|
{
|
||
|
|
MutexGuard mg(ipcLock);
|
||
|
|
rwCookie->currentRwSpeed = speed;
|
||
|
|
rwCookie->currentRampTime = rampTime;
|
||
|
|
rwCookie->specialRequest = specialRequest;
|
||
|
|
if (state == InternalState::IDLE and rwCookie->rwIdx == 3) {
|
||
|
|
semaphore->release();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return returnvalue::OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t RwPollingTask::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; }
|
||
|
|
|
||
|
|
ReturnValue_t RwPollingTask::requestReceiveMessage(CookieIF* cookie, size_t requestLen) {
|
||
|
|
return returnvalue::OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t RwPollingTask::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) {
|
||
|
|
RwCookie* rwCookie = dynamic_cast<RwCookie*>(cookie);
|
||
|
|
{
|
||
|
|
MutexGuard mg(ipcLock);
|
||
|
|
*buffer = rwCookie->replyBuf.data();
|
||
|
|
*size = rwCookie->replyBuf.size();
|
||
|
|
}
|
||
|
|
return returnvalue::OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t RwPollingTask::writeAndReadAllRws(const uint8_t* sendData, size_t sendDataLen) {
|
||
|
|
// Stopwatch watch;
|
||
|
|
ReturnValue_t result = returnvalue::OK;
|
||
|
|
|
||
|
|
int fd = 0;
|
||
|
|
const std::string& dev = spiIF->getSpiDev();
|
||
|
|
MutexIF* spiLock = spiIF->getCsMutex();
|
||
|
|
result = openSpi(dev, O_RDWR, fd);
|
||
|
|
if (result != returnvalue::OK) {
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
for (unsigned idx = 0; idx < rwCookies.size(); idx++) {
|
||
|
|
ReturnValue_t result = sendOneMessage(fd, *rwCookies[idx], spiLock, sendData, sendDataLen);
|
||
|
|
if (result != returnvalue::OK) {
|
||
|
|
closeSpi(fd);
|
||
|
|
return returnvalue::FAILED;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
closeSpi(fd);
|
||
|
|
usleep(rws::SPI_REPLY_DELAY);
|
||
|
|
return readAllRws(fd, spiLock, dev.c_str());
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t RwPollingTask::openSpi(const std::string& devname, int flags, int& fd) {
|
||
|
|
fd = open(devname.c_str(), flags);
|
||
|
|
if (fd < 0) {
|
||
|
|
sif::error << "rwSpiCallback::spiCallback: Failed to open device file" << std::endl;
|
||
|
|
return SpiComIF::OPENING_FILE_FAILED;
|
||
|
|
}
|
||
|
|
|
||
|
|
return returnvalue::OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t RwPollingTask::readNextReply(const char* spiDev, RwCookie& rwCookie, MutexIF* spiLock,
|
||
|
|
uint8_t* replyBuf) {
|
||
|
|
ReturnValue_t result = returnvalue::OK;
|
||
|
|
int fd = 0;
|
||
|
|
gpioId_t gpioId = rwCookie.getChipSelectPin();
|
||
|
|
GpioIF& gpioIF = spiIF->getGpioInterface();
|
||
|
|
pullCsLow(gpioId, spiLock, gpioIF);
|
||
|
|
for (unsigned idx = 0; idx < MAX_RETRIES_REPLY; idx++) {
|
||
|
|
result = openSpi(spiDev, O_RDWR, fd);
|
||
|
|
if (result != returnvalue::OK) {
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
/**
|
||
|
|
* The reaction wheel responds with empty frames while preparing the reply data.
|
||
|
|
* However, receiving more than 5 empty frames will be interpreted as an error.
|
||
|
|
*/
|
||
|
|
uint8_t byteRead = 0;
|
||
|
|
for (int idx = 0; idx < 5; idx++) {
|
||
|
|
if (read(fd, &byteRead, 1) != 1) {
|
||
|
|
sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl;
|
||
|
|
pullCsHigh(gpioId, spiLock, gpioIF);
|
||
|
|
closeSpi(fd);
|
||
|
|
return rws::SPI_READ_FAILURE;
|
||
|
|
}
|
||
|
|
if (idx == 0) {
|
||
|
|
if (byteRead != FLAG_BYTE) {
|
||
|
|
sif::error << "Invalid data, expected start marker" << std::endl;
|
||
|
|
pullCsHigh(gpioId, spiLock, gpioIF);
|
||
|
|
closeSpi(fd);
|
||
|
|
return rws::NO_START_MARKER;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
if (byteRead != FLAG_BYTE) {
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
pullCsHigh(gpioId, spiLock, gpioIF);
|
||
|
|
closeSpi(fd);
|
||
|
|
if (idx == MAX_RETRIES_REPLY - 1) {
|
||
|
|
sif::error << "rwSpiCallback::spiCallback: Empty frame timeout" << std::endl;
|
||
|
|
return rws::NO_REPLY;
|
||
|
|
}
|
||
|
|
TaskFactory::delayTask(5);
|
||
|
|
}
|
||
|
|
|
||
|
|
#if FSFW_HAL_SPI_WIRETAPPING == 1
|
||
|
|
sif::info << "RW start marker detected" << std::endl;
|
||
|
|
#endif
|
||
|
|
|
||
|
|
size_t decodedFrameLen = 0;
|
||
|
|
|
||
|
|
while (decodedFrameLen < processingBuf.size()) {
|
||
|
|
/** First byte already read in */
|
||
|
|
if (decodedFrameLen != 0) {
|
||
|
|
byteRead = 0;
|
||
|
|
if (read(fd, &byteRead, 1) != 1) {
|
||
|
|
sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl;
|
||
|
|
result = rws::SPI_READ_FAILURE;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
if (byteRead == FLAG_BYTE) {
|
||
|
|
/** Reached end of frame */
|
||
|
|
break;
|
||
|
|
} else if (byteRead == 0x7D) {
|
||
|
|
if (read(fd, &byteRead, 1) != 1) {
|
||
|
|
sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl;
|
||
|
|
result = rws::SPI_READ_FAILURE;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
if (byteRead == 0x5E) {
|
||
|
|
*(processingBuf.data() + decodedFrameLen) = 0x7E;
|
||
|
|
decodedFrameLen++;
|
||
|
|
continue;
|
||
|
|
} else if (byteRead == 0x5D) {
|
||
|
|
*(processingBuf.data() + decodedFrameLen) = 0x7D;
|
||
|
|
decodedFrameLen++;
|
||
|
|
continue;
|
||
|
|
} else {
|
||
|
|
sif::error << "rwSpiCallback::spiCallback: Invalid substitute" << std::endl;
|
||
|
|
closeSpi(fd);
|
||
|
|
result = rws::INVALID_SUBSTITUTE;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
} else {
|
||
|
|
*(processingBuf.data() + decodedFrameLen) = byteRead;
|
||
|
|
decodedFrameLen++;
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* There might be the unlikely case that each byte in a get-telemetry reply has been
|
||
|
|
* replaced by its substitute. Than the next byte must correspond to the end sign 0x7E.
|
||
|
|
* Otherwise there might be something wrong.
|
||
|
|
*/
|
||
|
|
if (decodedFrameLen == processingBuf.size()) {
|
||
|
|
if (read(fd, &byteRead, 1) != 1) {
|
||
|
|
sif::error << "rwSpiCallback::spiCallback: Failed to read last byte" << std::endl;
|
||
|
|
result = rws::SPI_READ_FAILURE;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
if (byteRead != FLAG_BYTE) {
|
||
|
|
sif::error << "rwSpiCallback::spiCallback: Missing end sign "
|
||
|
|
<< static_cast<int>(FLAG_BYTE) << std::endl;
|
||
|
|
decodedFrameLen--;
|
||
|
|
result = rws::MISSING_END_SIGN;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
result = returnvalue::OK;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return returnvalue::OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t RwPollingTask::writeOneRw(uint8_t rwIdx) {
|
||
|
|
int fd = 0;
|
||
|
|
const std::string& dev = spiIF->getSpiDev();
|
||
|
|
MutexIF* spiLock = spiIF->getCsMutex();
|
||
|
|
ReturnValue_t result = openSpi(dev, O_RDWR, fd);
|
||
|
|
if (result != returnvalue::OK) {
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
ReturnValue_t result =
|
||
|
|
sendOneMessage(fd, *rwCookies[rwIdx], spiLock, writeBuffer.data(), writeLen);
|
||
|
|
if (result != returnvalue::OK) {
|
||
|
|
closeSpi(fd);
|
||
|
|
return returnvalue::FAILED;
|
||
|
|
}
|
||
|
|
return returnvalue::OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t RwPollingTask::readAllRws(int fd, MutexIF* spiLock, const char* dev) {
|
||
|
|
for (unsigned idx = 0; idx < rwCookies.size(); idx++) {
|
||
|
|
if (spiLock == nullptr) {
|
||
|
|
sif::debug << "rwSpiCallback::spiCallback: Mutex or GPIO interface invalid" << std::endl;
|
||
|
|
return returnvalue::FAILED;
|
||
|
|
}
|
||
|
|
uint8_t* replyBuf;
|
||
|
|
readNextReply(dev, *rwCookies[idx], spiLock, replyBuf);
|
||
|
|
}
|
||
|
|
|
||
|
|
closeSpi(fd);
|
||
|
|
return returnvalue::OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
// This closes the SPI
|
||
|
|
void RwPollingTask::closeSpi(int fd) {
|
||
|
|
// This will perform the function to close the SPI
|
||
|
|
close(fd);
|
||
|
|
// The SPI is now closed.
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie, MutexIF* spiLock,
|
||
|
|
const uint8_t* data, size_t dataLen) {
|
||
|
|
gpioId_t gpioId = rwCookie.getChipSelectPin();
|
||
|
|
GpioIF& gpioIF = spiIF->getGpioInterface();
|
||
|
|
if (spiLock == nullptr) {
|
||
|
|
sif::debug << "rwSpiCallback::spiCallback: Mutex or GPIO interface invalid" << std::endl;
|
||
|
|
return returnvalue::FAILED;
|
||
|
|
}
|
||
|
|
pullCsLow(gpioId, spiLock, gpioIF);
|
||
|
|
/** Sending frame start sign */
|
||
|
|
writeBuffer[0] = FLAG_BYTE;
|
||
|
|
size_t writeSize = 1;
|
||
|
|
if (write(fd, writeBuffer.data(), writeSize) != static_cast<ssize_t>(writeSize)) {
|
||
|
|
sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl;
|
||
|
|
pullCsHigh(gpioId, spiLock, gpioIF);
|
||
|
|
return rws::SPI_WRITE_FAILURE;
|
||
|
|
}
|
||
|
|
/** Encoding and sending command */
|
||
|
|
size_t idx = 0;
|
||
|
|
while (idx < dataLen) {
|
||
|
|
switch (*(data + idx)) {
|
||
|
|
case 0x7E:
|
||
|
|
writeBuffer[0] = 0x7D;
|
||
|
|
writeBuffer[1] = 0x5E;
|
||
|
|
writeSize = 2;
|
||
|
|
break;
|
||
|
|
case 0x7D:
|
||
|
|
writeBuffer[0] = 0x7D;
|
||
|
|
writeBuffer[1] = 0x5D;
|
||
|
|
writeSize = 2;
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
writeBuffer[0] = *(data + idx);
|
||
|
|
writeSize = 1;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
if (write(fd, writeBuffer.data(), writeSize) != static_cast<ssize_t>(writeSize)) {
|
||
|
|
sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl;
|
||
|
|
pullCsHigh(gpioId, spiLock, gpioIF);
|
||
|
|
return rws::SPI_WRITE_FAILURE;
|
||
|
|
}
|
||
|
|
idx++;
|
||
|
|
/** Sending frame end sign */
|
||
|
|
writeBuffer[0] = FLAG_BYTE;
|
||
|
|
writeSize = 1;
|
||
|
|
|
||
|
|
if (write(fd, writeBuffer.data(), writeSize) != static_cast<ssize_t>(writeSize)) {
|
||
|
|
sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl;
|
||
|
|
pullCsHigh(gpioId, spiLock, gpioIF);
|
||
|
|
return rws::SPI_WRITE_FAILURE;
|
||
|
|
}
|
||
|
|
return returnvalue::OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t RwPollingTask::pullCsLow(gpioId_t gpioId, MutexIF* spiLock, GpioIF& gpioIF) {
|
||
|
|
ReturnValue_t result = spiLock->lockMutex(TIMEOUT_TYPE, TIMEOUT_MS);
|
||
|
|
if (result != returnvalue::OK) {
|
||
|
|
sif::debug << "rwSpiCallback::spiCallback: Failed to lock mutex" << std::endl;
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
// Pull SPI CS low. For now, no support for active high given
|
||
|
|
if (gpioId != gpio::NO_GPIO) {
|
||
|
|
ReturnValue_t result = gpioIF.pullLow(gpioId);
|
||
|
|
if (result != returnvalue::OK) {
|
||
|
|
sif::error << "rwSpiCallback::spiCallback: Failed to pull chip select low" << std::endl;
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return returnvalue::OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
void RwPollingTask::pullCsHigh(gpioId_t gpioId, MutexIF* spiLock, GpioIF& gpioIF) {
|
||
|
|
if (gpioId != gpio::NO_GPIO) {
|
||
|
|
if (gpioIF.pullHigh(gpioId) != returnvalue::OK) {
|
||
|
|
sif::error << "closeSpi: Failed to pull chip select high" << std::endl;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
if (spiLock->unlockMutex() != returnvalue::OK) {
|
||
|
|
sif::error << "rwSpiCallback::closeSpi: Failed to unlock mutex" << std::endl;
|
||
|
|
;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void RwPollingTask::prepareSimpleCommand(DeviceCommandId_t id) {
|
||
|
|
writeBuffer[0] = static_cast<uint8_t>(id);
|
||
|
|
uint16_t crc = CRC::crc16ccitt(writeBuffer.data(), 1, 0xFFFF);
|
||
|
|
writeBuffer[1] = static_cast<uint8_t>(crc & 0xFF);
|
||
|
|
writeBuffer[2] = static_cast<uint8_t>(crc >> 8 & 0xFF);
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t RwPollingTask::prepareSetSpeedCmd(uint8_t rwIdx) {
|
||
|
|
writeBuffer[0] = static_cast<uint8_t>(rws::SET_SPEED);
|
||
|
|
uint8_t* serPtr = writeBuffer.data() + 1;
|
||
|
|
int32_t speedToSet = 0;
|
||
|
|
uint16_t rampTimeToSet = 10;
|
||
|
|
{
|
||
|
|
MutexGuard mg(ipcLock);
|
||
|
|
speedToSet = rwCookies[rwIdx]->currentRwSpeed;
|
||
|
|
rampTimeToSet = rwCookies[rwIdx]->currentRampTime;
|
||
|
|
}
|
||
|
|
size_t serLen = 0;
|
||
|
|
SerializeAdapter::serialize(&speedToSet, &serPtr, &serLen, writeBuffer.size(),
|
||
|
|
SerializeIF::Endianness::LITTLE);
|
||
|
|
SerializeAdapter::serialize(&rampTimeToSet, &serPtr, &serLen, writeBuffer.size(),
|
||
|
|
SerializeIF::Endianness::LITTLE);
|
||
|
|
|
||
|
|
uint16_t crc = CRC::crc16ccitt(writeBuffer.data(), 7, 0xFFFF);
|
||
|
|
writeBuffer[7] = static_cast<uint8_t>(crc & 0xFF);
|
||
|
|
writeBuffer[8] = static_cast<uint8_t>((crc >> 8) & 0xFF);
|
||
|
|
return returnvalue::OK;
|
||
|
|
}
|