533 lines
16 KiB
C++
533 lines
16 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/timemanager/Stopwatch.h>
|
|
#include <fsfw_hal/common/spi/spiCommon.h>
|
|
#include <fsfw_hal/linux/utility.h>
|
|
#include <sys/ioctl.h>
|
|
#include <unistd.h>
|
|
|
|
#include "devConf.h"
|
|
#include "mission/devices/devicedefinitions/rwHelpers.h"
|
|
|
|
RwPollingTask::RwPollingTask(object_id_t objectId, const char* spiDev, GpioIF& gpioIF)
|
|
: SystemObject(objectId), spiDev(spiDev), gpioIF(gpioIF) {
|
|
semaphore = SemaphoreFactory::instance()->createBinarySemaphore();
|
|
semaphore->acquire();
|
|
ipcLock = MutexFactory::instance()->createMutex();
|
|
spiLock = MutexFactory::instance()->createMutex();
|
|
}
|
|
|
|
ReturnValue_t RwPollingTask::performOperation(uint8_t operationCode) {
|
|
for (unsigned i = 0; i < 4; i++) {
|
|
if (rwCookies[i] == nullptr) {
|
|
sif::error << "Invalid RW cookie at index" << i << std::endl;
|
|
return returnvalue::FAILED;
|
|
}
|
|
}
|
|
while (true) {
|
|
ipcLock->lockMutex();
|
|
state = InternalState::IDLE;
|
|
ipcLock->unlockMutex();
|
|
semaphore->acquire();
|
|
// This loop takes 50 ms on a debug build.
|
|
// Stopwatch watch;
|
|
TaskFactory::delayTask(5);
|
|
int fd = 0;
|
|
for (auto& skip : skipCommandingForRw) {
|
|
skip = false;
|
|
}
|
|
setAllReadFlagsFalse();
|
|
ReturnValue_t result = openSpi(O_RDWR, fd);
|
|
if (result != returnvalue::OK) {
|
|
continue;
|
|
}
|
|
for (unsigned idx = 0; idx < rwCookies.size(); idx++) {
|
|
if (rwCookies[idx]->specialRequest == rws::SpecialRwRequest::RESET_MCU) {
|
|
prepareSimpleCommand(rws::RESET_MCU);
|
|
// No point in commanding that specific RW for the cycle.
|
|
skipCommandingForRw[idx] = true;
|
|
writeOneRwCmd(idx, fd);
|
|
} else if (rwCookies[idx]->setSpeed) {
|
|
prepareSetSpeedCmd(idx);
|
|
if (writeOneRwCmd(idx, fd) != returnvalue::OK) {
|
|
continue;
|
|
}
|
|
}
|
|
}
|
|
closeSpi(fd);
|
|
if (readAllRws(rws::SET_SPEED) != returnvalue::OK) {
|
|
continue;
|
|
}
|
|
prepareSimpleCommand(rws::GET_LAST_RESET_STATUS);
|
|
if (writeAndReadAllRws(rws::GET_LAST_RESET_STATUS) != returnvalue::OK) {
|
|
continue;
|
|
}
|
|
prepareSimpleCommand(rws::GET_RW_STATUS);
|
|
if (writeAndReadAllRws(rws::GET_RW_STATUS) != returnvalue::OK) {
|
|
continue;
|
|
}
|
|
prepareSimpleCommand(rws::GET_TEMPERATURE);
|
|
if (writeAndReadAllRws(rws::GET_TEMPERATURE) != returnvalue::OK) {
|
|
continue;
|
|
}
|
|
prepareSimpleCommand(rws::CLEAR_LAST_RESET_STATUS);
|
|
if (writeAndReadAllRws(rws::CLEAR_LAST_RESET_STATUS) != returnvalue::OK) {
|
|
continue;
|
|
}
|
|
handleSpecialRequests();
|
|
}
|
|
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t RwPollingTask::initialize() { return returnvalue::OK; }
|
|
|
|
ReturnValue_t RwPollingTask::initializeInterface(CookieIF* cookie) {
|
|
// 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) {
|
|
int fd = open(spiDev, O_RDWR);
|
|
if (fd < 0) {
|
|
sif::error << "could not open RW SPI bus" << std::endl;
|
|
return returnvalue::FAILED;
|
|
}
|
|
spi::SpiModes mode = spi::RW_MODE;
|
|
int retval = ioctl(fd, SPI_IOC_WR_MODE, reinterpret_cast<uint8_t*>(&mode));
|
|
if (retval != 0) {
|
|
utility::handleIoctlError("SpiComIF::setSpiSpeedAndMode: Setting SPI mode failed");
|
|
}
|
|
|
|
retval = ioctl(fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi::RW_SPEED);
|
|
if (retval != 0) {
|
|
utility::handleIoctlError("SpiComIF::setSpiSpeedAndMode: Setting SPI speed failed");
|
|
}
|
|
close(fd);
|
|
modeAndSpeedWasSet = true;
|
|
}
|
|
|
|
auto* rwCookie = dynamic_cast<RwCookie*>(cookie);
|
|
if (rwCookie == nullptr) {
|
|
sif::error << "RwPollingTask::initializeInterface: Wrong cookie" << std::endl;
|
|
return returnvalue::FAILED;
|
|
}
|
|
rwCookies[rwCookie->rwIdx] = rwCookie;
|
|
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t RwPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendData,
|
|
size_t sendLen) {
|
|
if (sendData == nullptr or sendLen < 8) {
|
|
return DeviceHandlerIF::INVALID_DATA;
|
|
}
|
|
int32_t speed = 0;
|
|
uint16_t rampTime = 0;
|
|
const uint8_t* currentBuf = sendData;
|
|
bool setSpeed = currentBuf[0];
|
|
currentBuf += 1;
|
|
sendLen -= 1;
|
|
SerializeAdapter::deSerialize(&speed, ¤tBuf, &sendLen, SerializeIF::Endianness::MACHINE);
|
|
SerializeAdapter::deSerialize(&rampTime, ¤tBuf, &sendLen, SerializeIF::Endianness::MACHINE);
|
|
rws::SpecialRwRequest specialRequest = rws::SpecialRwRequest::REQUEST_NONE;
|
|
if (sendLen == 8 and sendData[7] < static_cast<uint8_t>(rws::SpecialRwRequest::NUM_REQUESTS)) {
|
|
specialRequest = static_cast<rws::SpecialRwRequest>(sendData[7]);
|
|
}
|
|
RwCookie* rwCookie = dynamic_cast<RwCookie*>(cookie);
|
|
if (rwCookie == nullptr) {
|
|
return returnvalue::FAILED;
|
|
}
|
|
{
|
|
MutexGuard mg(ipcLock);
|
|
rwCookie->setSpeed = setSpeed;
|
|
rwCookie->currentRwSpeed = speed;
|
|
rwCookie->currentRampTime = rampTime;
|
|
rwCookie->specialRequest = specialRequest;
|
|
if (state == InternalState::IDLE) {
|
|
state = InternalState::BUSY;
|
|
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(DeviceCommandId_t id) {
|
|
// Stopwatch watch;
|
|
ReturnValue_t result = returnvalue::OK;
|
|
|
|
int fd = 0;
|
|
result = openSpi(O_RDWR, fd);
|
|
if (result != returnvalue::OK) {
|
|
return result;
|
|
}
|
|
for (unsigned idx = 0; idx < rwCookies.size(); idx++) {
|
|
if (skipCommandingForRw[idx]) {
|
|
continue;
|
|
}
|
|
ReturnValue_t result = sendOneMessage(fd, *rwCookies[idx]);
|
|
if (result != returnvalue::OK) {
|
|
closeSpi(fd);
|
|
return returnvalue::FAILED;
|
|
}
|
|
}
|
|
|
|
closeSpi(fd);
|
|
return readAllRws(id);
|
|
}
|
|
|
|
ReturnValue_t RwPollingTask::openSpi(int flags, int& fd) {
|
|
fd = open(spiDev, flags);
|
|
if (fd < 0) {
|
|
sif::error << "RwPollingTask::openSpi: Failed to open device file" << std::endl;
|
|
return SpiComIF::OPENING_FILE_FAILED;
|
|
}
|
|
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf,
|
|
size_t maxReplyLen) {
|
|
ReturnValue_t result = returnvalue::OK;
|
|
int fd = 0;
|
|
gpioId_t gpioId = rwCookie.getChipSelectPin();
|
|
uint8_t byteRead = 0;
|
|
result = openSpi(O_RDWR, fd);
|
|
if (result != returnvalue::OK) {
|
|
return result;
|
|
}
|
|
pullCsLow(gpioId, gpioIF);
|
|
bool lastByteWasFrameMarker = false;
|
|
Countdown cd(3000);
|
|
size_t readIdx = 0;
|
|
|
|
while (true) {
|
|
lastByteWasFrameMarker = false;
|
|
if (read(fd, &byteRead, 1) != 1) {
|
|
sif::error << "RwPollingTask: Read failed. " << strerror(errno) << std::endl;
|
|
pullCsHigh(gpioId, gpioIF);
|
|
closeSpi(fd);
|
|
return rws::SPI_READ_FAILURE;
|
|
}
|
|
if (byteRead == rws::FRAME_DELIMITER) {
|
|
lastByteWasFrameMarker = true;
|
|
}
|
|
// Start of frame detected.
|
|
if (byteRead != rws::FRAME_DELIMITER and not lastByteWasFrameMarker) {
|
|
break;
|
|
}
|
|
|
|
if (readIdx % 100 == 0 && cd.hasTimedOut()) {
|
|
pullCsHigh(gpioId, gpioIF);
|
|
closeSpi(fd);
|
|
return rws::SPI_READ_FAILURE;
|
|
}
|
|
readIdx++;
|
|
}
|
|
|
|
#if FSFW_HAL_SPI_WIRETAPPING == 1
|
|
sif::info << "RW start marker detected" << std::endl;
|
|
#endif
|
|
|
|
size_t decodedFrameLen = 0;
|
|
|
|
while (decodedFrameLen < maxReplyLen) {
|
|
// First byte already read in
|
|
if (decodedFrameLen != 0) {
|
|
byteRead = 0;
|
|
if (read(fd, &byteRead, 1) != 1) {
|
|
sif::error << "RwPollingTask: Read failed" << std::endl;
|
|
result = rws::SPI_READ_FAILURE;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (byteRead == rws::FRAME_DELIMITER) {
|
|
// Reached end of frame
|
|
break;
|
|
} else if (byteRead == 0x7D) {
|
|
if (read(fd, &byteRead, 1) != 1) {
|
|
sif::error << "RwPollingTask: Read failed" << std::endl;
|
|
result = rws::SPI_READ_FAILURE;
|
|
break;
|
|
}
|
|
if (byteRead == 0x5E) {
|
|
*(replyBuf + decodedFrameLen) = 0x7E;
|
|
decodedFrameLen++;
|
|
continue;
|
|
} else if (byteRead == 0x5D) {
|
|
*(replyBuf + decodedFrameLen) = 0x7D;
|
|
decodedFrameLen++;
|
|
continue;
|
|
} else {
|
|
sif::error << "RwPollingTask: Invalid substitute" << std::endl;
|
|
result = rws::INVALID_SUBSTITUTE;
|
|
break;
|
|
}
|
|
} else {
|
|
*(replyBuf + decodedFrameLen) = byteRead;
|
|
decodedFrameLen++;
|
|
continue;
|
|
}
|
|
|
|
// Check end marker.
|
|
/**
|
|
* There might be the unlikely case that each byte in a get-telemetry reply has been
|
|
* replaced by its substitute. Then the next byte must correspond to the end sign 0x7E.
|
|
* Otherwise there might be something wrong.
|
|
*/
|
|
if (decodedFrameLen == maxReplyLen) {
|
|
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 != rws::FRAME_DELIMITER) {
|
|
sif::error << "rwSpiCallback::spiCallback: Missing end sign "
|
|
<< static_cast<int>(rws::FRAME_DELIMITER) << std::endl;
|
|
decodedFrameLen--;
|
|
result = rws::MISSING_END_SIGN;
|
|
break;
|
|
}
|
|
}
|
|
result = returnvalue::OK;
|
|
}
|
|
|
|
pullCsHigh(gpioId, gpioIF);
|
|
closeSpi(fd);
|
|
return result;
|
|
}
|
|
|
|
ReturnValue_t RwPollingTask::writeOneRwCmd(uint8_t rwIdx, int fd) {
|
|
ReturnValue_t result = sendOneMessage(fd, *rwCookies[rwIdx]);
|
|
if (result != returnvalue::OK) {
|
|
return returnvalue::FAILED;
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t RwPollingTask::readAllRws(DeviceCommandId_t id) {
|
|
// SPI dev will be opened in readNextReply on demand.
|
|
for (unsigned idx = 0; idx < rwCookies.size(); idx++) {
|
|
if (((id == rws::SET_SPEED) and !rwCookies[idx]->setSpeed) or skipCommandingForRw[idx]) {
|
|
continue;
|
|
}
|
|
uint8_t* replyBuf;
|
|
size_t maxReadLen = idAndIdxToReadBuffer(id, idx, &replyBuf);
|
|
ReturnValue_t result = readNextReply(*rwCookies[idx], replyBuf + 1, maxReadLen);
|
|
if (result == returnvalue::OK) {
|
|
// The first byte is always a flag which shows whether the value was read
|
|
// properly at least once.
|
|
replyBuf[0] = true;
|
|
}
|
|
}
|
|
// SPI is closed in readNextReply as well.
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
size_t RwPollingTask::idAndIdxToReadBuffer(DeviceCommandId_t id, uint8_t rwIdx, uint8_t** ptr) {
|
|
uint8_t* rawStart = rwCookies[rwIdx]->replyBuf.data();
|
|
RwReplies replies(rawStart);
|
|
switch (id) {
|
|
case (rws::GET_RW_STATUS): {
|
|
*ptr = replies.rwStatusReply;
|
|
break;
|
|
}
|
|
case (rws::SET_SPEED): {
|
|
*ptr = replies.setSpeedReply;
|
|
break;
|
|
}
|
|
case (rws::CLEAR_LAST_RESET_STATUS): {
|
|
*ptr = replies.clearLastResetStatusReply;
|
|
break;
|
|
}
|
|
case (rws::GET_LAST_RESET_STATUS): {
|
|
*ptr = replies.getLastResetStatusReply;
|
|
break;
|
|
}
|
|
case (rws::GET_TEMPERATURE): {
|
|
*ptr = replies.readTemperatureReply;
|
|
break;
|
|
}
|
|
case (rws::GET_TM): {
|
|
*ptr = replies.hkDataReply;
|
|
break;
|
|
}
|
|
case (rws::INIT_RW_CONTROLLER): {
|
|
*ptr = replies.initRwControllerReply;
|
|
break;
|
|
}
|
|
default: {
|
|
sif::error << "no reply buffer for rw command " << id << std::endl;
|
|
*ptr = replies.dummyPointer;
|
|
return 0;
|
|
}
|
|
}
|
|
return rws::idToPacketLen(id);
|
|
}
|
|
|
|
void RwPollingTask::fillSpecialRequestArray() {
|
|
for (unsigned idx = 0; idx < rwCookies.size(); idx++) {
|
|
if (skipCommandingForRw[idx]) {
|
|
specialRequestIds[idx] = DeviceHandlerIF::NO_COMMAND_ID;
|
|
continue;
|
|
}
|
|
switch (rwCookies[idx]->specialRequest) {
|
|
case (rws::SpecialRwRequest::GET_TM): {
|
|
specialRequestIds[idx] = rws::GET_TM;
|
|
break;
|
|
}
|
|
case (rws::SpecialRwRequest::INIT_RW_CONTROLLER): {
|
|
specialRequestIds[idx] = rws::INIT_RW_CONTROLLER;
|
|
break;
|
|
}
|
|
default: {
|
|
specialRequestIds[idx] = DeviceHandlerIF::NO_COMMAND_ID;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void RwPollingTask::handleSpecialRequests() {
|
|
int fd = 0;
|
|
fillSpecialRequestArray();
|
|
ReturnValue_t result = openSpi(O_RDWR, fd);
|
|
if (result != returnvalue::OK) {
|
|
return;
|
|
}
|
|
for (unsigned idx = 0; idx < rwCookies.size(); idx++) {
|
|
if (specialRequestIds[idx] == DeviceHandlerIF::NO_COMMAND_ID) {
|
|
continue;
|
|
}
|
|
prepareSimpleCommand(specialRequestIds[idx]);
|
|
writeOneRwCmd(idx, fd);
|
|
}
|
|
closeSpi(fd);
|
|
usleep(rws::SPI_REPLY_DELAY);
|
|
for (unsigned idx = 0; idx < rwCookies.size(); idx++) {
|
|
if (specialRequestIds[idx] == DeviceHandlerIF::NO_COMMAND_ID) {
|
|
continue;
|
|
}
|
|
uint8_t* replyBuf;
|
|
size_t maxReadLen = idAndIdxToReadBuffer(specialRequestIds[idx], idx, &replyBuf);
|
|
readNextReply(*rwCookies[idx], replyBuf, maxReadLen);
|
|
}
|
|
}
|
|
|
|
void RwPollingTask::setAllReadFlagsFalse() {
|
|
for (auto& rwCookie : rwCookies) {
|
|
RwReplies replies(rwCookie->replyBuf.data());
|
|
replies.getLastResetStatusReply[0] = false;
|
|
replies.clearLastResetStatusReply[0] = false;
|
|
replies.hkDataReply[0] = false;
|
|
replies.readTemperatureReply[0] = false;
|
|
replies.rwStatusReply[0] = false;
|
|
replies.setSpeedReply[0] = false;
|
|
replies.initRwControllerReply[0] = false;
|
|
}
|
|
}
|
|
|
|
// 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) {
|
|
gpioId_t gpioId = rwCookie.getChipSelectPin();
|
|
if (spiLock == nullptr) {
|
|
sif::debug << "rwSpiCallback::spiCallback: Mutex or GPIO interface invalid" << std::endl;
|
|
return returnvalue::FAILED;
|
|
}
|
|
// Add datalinklayer like specified in the datasheet.
|
|
size_t lenToSend = 0;
|
|
rws::encodeHdlc(writeBuffer.data(), writeLen, encodedBuffer.data(), lenToSend);
|
|
pullCsLow(gpioId, gpioIF);
|
|
if (write(fd, encodedBuffer.data(), lenToSend) != static_cast<ssize_t>(lenToSend)) {
|
|
sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl;
|
|
pullCsHigh(gpioId, gpioIF);
|
|
return rws::SPI_WRITE_FAILURE;
|
|
}
|
|
pullCsHigh(gpioId, gpioIF);
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t RwPollingTask::pullCsLow(gpioId_t gpioId, GpioIF& gpioIF) {
|
|
ReturnValue_t result = spiLock->lockMutex(TIMEOUT_TYPE, TIMEOUT_MS);
|
|
if (result != returnvalue::OK) {
|
|
sif::debug << "RwPollingTask::pullCsLow: 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 << "RwPollingTask::pullCsLow: Failed to pull chip select low" << std::endl;
|
|
return result;
|
|
}
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
void RwPollingTask::pullCsHigh(gpioId_t gpioId, 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 << "RwPollingTask::pullCsHigh: 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);
|
|
writeLen = 3;
|
|
}
|
|
|
|
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 = 1;
|
|
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);
|
|
writeLen = 9;
|
|
return returnvalue::OK;
|
|
}
|