eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 121 Wiki Activity

vieles
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Robin Müller
2023-02-26 14:55:33 +01:00
parent 9b4026a1c9
commit 828b6adf77
31 changed files with 1720 additions and 732 deletions
Download Patch File Download Diff File Expand all files Collapse all files

453
linux/devices/AcsBoardPolling.cpp Normal file
View File

@ -0,0 +1,453 @@
#include "AcsBoardPolling.h"
#include <fcntl.h>
#include <fsfw/globalfunctions/arrayprinter.h>
#include <fsfw/tasks/SemaphoreFactory.h>
#include <fsfw/tasks/TaskFactory.h>
#include <fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h>
#include <fsfw_hal/linux/UnixFileGuard.h>
#include <fsfw_hal/linux/spi/SpiCookie.h>
#include <fsfw_hal/linux/utility.h>
#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
#include <sys/ioctl.h>
#include "devices/gpioIds.h"
using namespace returnvalue;
AcsBoardPolling::AcsBoardPolling(object_id_t objectId, SpiComIF& lowLevelComIF, GpioIF& gpioIF)
: SystemObject(objectId), spiComIF(lowLevelComIF), gpioIF(gpioIF) {
semaphore = SemaphoreFactory::instance()->createBinarySemaphore();
semaphore->acquire();
ipcLock = MutexFactory::instance()->createMutex();
}
ReturnValue_t AcsBoardPolling::performOperation(uint8_t operationCode) {
while (true) {
ipcLock->lockMutex();
state = InternalState::IDLE;
ipcLock->unlockMutex();
semaphore->acquire();
// Give all tasks or the PST some time to submit all consecutive requests.
TaskFactory::delayTask(2);
gyroAdisHandler(gyro0Adis);
gyroAdisHandler(gyro2Adis);
gyroL3gHandler(gyro1L3g);
gyroL3gHandler(gyro3L3g);
// To prevent task being not reactivated by tardy tasks
TaskFactory::delayTask(20);
}
return returnvalue::OK;
}
ReturnValue_t AcsBoardPolling::initialize() { return returnvalue::OK; }
ReturnValue_t AcsBoardPolling::initializeInterface(CookieIF* cookie) {
SpiCookie* spiCookie = dynamic_cast<SpiCookie*>(cookie);
if (spiCookie == nullptr) {
return returnvalue::FAILED;
}
switch (spiCookie->getChipSelectPin()) {
case (gpioIds::MGM_0_LIS3_CS): {
mgm0L3Cookie = spiCookie;
break;
}
case (gpioIds::MGM_1_RM3100_CS): {
mgm1Rm3100Cookie = spiCookie;
break;
}
case (gpioIds::MGM_2_LIS3_CS): {
mgm2L3Cookie = spiCookie;
break;
}
case (gpioIds::MGM_3_RM3100_CS): {
mgm3Rm3100Cookie = spiCookie;
break;
}
case (gpioIds::GYRO_0_ADIS_CS): {
gyro0Adis.cookie = spiCookie;
break;
}
case (gpioIds::GYRO_1_L3G_CS): {
gyro1L3g.cookie = spiCookie;
break;
}
case (gpioIds::GYRO_2_ADIS_CS): {
gyro2Adis.cookie = spiCookie;
break;
}
case (gpioIds::GYRO_3_L3G_CS): {
gyro3L3g.cookie = spiCookie;
break;
}
default: {
sif::error << "AcsBoardPollingTask: invalid spi cookie" << std::endl;
}
}
return spiComIF.initializeInterface(cookie);
}
ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* sendData,
size_t sendLen) {
SpiCookie* spiCookie = dynamic_cast<SpiCookie*>(cookie);
if (spiCookie == nullptr) {
return returnvalue::FAILED;
}
auto handleAdisRequest = [&](GyroAdis& adis) {
if (sendLen != sizeof(acs::Adis1650XRequest)) {
sif::error << "AcsBoardPolling: invalid adis request send length";
adis.replyResult = returnvalue::FAILED;
return returnvalue::FAILED;
}
auto* req = reinterpret_cast<const acs::Adis1650XRequest*>(sendData);
MutexGuard mg(ipcLock);
if (req->mode != adis.mode) {
if (req->mode == acs::SimpleSensorMode::NORMAL) {
adis.type = req->type;
adis.countdown.setTimeout(adis1650x::START_UP_TIME);
adis.countdown.resetTimer();
if (adis.type == adis1650x::Type::ADIS16507) {
adis.ownReply.data.accelScaling = adis1650x::ACCELEROMETER_RANGE_16507;
} else if (adis.type == adis1650x::Type::ADIS16505) {
adis.ownReply.data.accelScaling = adis1650x::ACCELEROMETER_RANGE_16505;
} else {
sif::warning << "AcsBoardPolling: Unknown ADIS type" << std::endl;
}
adis.performStartup = true;
} else if (req->mode == acs::SimpleSensorMode::OFF) {
adis.performStartup = false;
adis.ownReply.cfgWasSet = false;
adis.ownReply.dataWasSet = false;
}
adis.mode = req->mode;
}
return returnvalue::OK;
};
auto handleL3gRequest = [&](GyroL3g& gyro) {
if (sendLen != sizeof(acs::GyroL3gRequest)) {
sif::error << "AcsBoardPolling: invalid l3g request send length";
gyro.replyResult = returnvalue::FAILED;
return returnvalue::FAILED;
}
auto* req = reinterpret_cast<const acs::GyroL3gRequest*>(sendData);
MutexGuard mg(ipcLock);
if (req->mode != gyro.mode) {
if (req->mode == acs::SimpleSensorMode::NORMAL) {
gyro.performStartup = true;
} else {
gyro.ownReply.cfgWasSet = false;
}
gyro.mode = req->mode;
}
return returnvalue::OK;
};
switch (spiCookie->getChipSelectPin()) {
case (gpioIds::GYRO_0_ADIS_CS): {
handleAdisRequest(gyro0Adis);
break;
}
case (gpioIds::GYRO_2_ADIS_CS): {
handleAdisRequest(gyro2Adis);
break;
}
case (gpioIds::GYRO_1_L3G_CS): {
handleL3gRequest(gyro1L3g);
break;
}
case (gpioIds::GYRO_3_L3G_CS): {
handleL3gRequest(gyro3L3g);
break;
}
}
MutexGuard mg(ipcLock);
if (state == InternalState::IDLE) {
state = InternalState::BUSY;
semaphore->release();
}
return returnvalue::OK;
}
ReturnValue_t AcsBoardPolling::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; }
ReturnValue_t AcsBoardPolling::requestReceiveMessage(CookieIF* cookie, size_t requestLen) {
return returnvalue::OK;
}
ReturnValue_t AcsBoardPolling::readReceivedMessage(CookieIF* cookie, uint8_t** buffer,
size_t* size) {
SpiCookie* spiCookie = dynamic_cast<SpiCookie*>(cookie);
if (spiCookie == nullptr) {
return returnvalue::FAILED;
}
MutexGuard mg(ipcLock);
auto handleAdisReply = [&](GyroAdis& gyro) {
std::memcpy(&gyro.readerReply, &gyro.ownReply, sizeof(acs::Adis1650XReply));
*buffer = reinterpret_cast<uint8_t*>(&gyro.readerReply);
*size = sizeof(acs::Adis1650XReply);
};
auto handleL3gReply = [&](GyroL3g& gyro) {
std::memcpy(&gyro.readerReply, &gyro.ownReply, sizeof(acs::GyroL3gReply));
*buffer = reinterpret_cast<uint8_t*>(&gyro.readerReply);
*size = sizeof(acs::GyroL3gReply);
};
switch (spiCookie->getChipSelectPin()) {
case (gpioIds::GYRO_0_ADIS_CS): {
handleAdisReply(gyro0Adis);
return gyro0Adis.replyResult;
}
case (gpioIds::GYRO_2_ADIS_CS): {
handleAdisReply(gyro2Adis);
return gyro2Adis.replyResult;
}
case (gpioIds::GYRO_1_L3G_CS): {
handleL3gReply(gyro1L3g);
return gyro1L3g.replyResult;
}
case (gpioIds::GYRO_3_L3G_CS): {
handleL3gReply(gyro3L3g);
return gyro3L3g.replyResult;
}
}
return returnvalue::OK;
}
void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) {
ReturnValue_t result;
acs::SimpleSensorMode mode;
bool gyroPerformStartup;
{
MutexGuard mg(ipcLock);
mode = l3g.mode;
gyroPerformStartup = l3g.performStartup;
}
if (mode == acs::SimpleSensorMode::NORMAL) {
if (gyroPerformStartup) {
cmdBuf[0] = l3gd20h::CTRL_REG_1 | l3gd20h::AUTO_INCREMENT_MASK;
std::memcpy(cmdBuf.data() + 1, l3g.sensorCfg, 5);
result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), 6);
if (result != returnvalue::OK) {
l3g.replyResult = returnvalue::OK;
}
// Ignore useless reply and red config
cmdBuf[0] = l3gd20h::CTRL_REG_1 | l3gd20h::AUTO_INCREMENT_MASK | l3gd20h::READ_MASK;
std::memset(cmdBuf.data() + 1, 0, 5);
result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), 6);
if (result != returnvalue::OK) {
l3g.replyResult = returnvalue::OK;
}
result = spiComIF.readReceivedMessage(l3g.cookie, &rawReply, &dummy);
if (result != returnvalue::OK) {
l3g.replyResult = returnvalue::OK;
}
MutexGuard mg(ipcLock);
// Cross check configuration as verification that communication is working
for (uint8_t idx = 0; idx < 5; idx++) {
if (rawReply[idx + 1] != l3g.sensorCfg[idx]) {
sif::warning << "AcsBoardPolling: l3g config check missmatch" << std::endl;
l3g.replyResult = returnvalue::FAILED;
return;
}
}
l3g.ownReply.cfgWasSet = true;
l3g.ownReply.sensitivity = l3gd20h::ctrlReg4ToSensitivity(l3g.sensorCfg[3]);
}
cmdBuf[0] = l3gd20h::READ_START | l3gd20h::AUTO_INCREMENT_MASK | l3gd20h::READ_MASK;
std::memset(cmdBuf.data() + 1, 0, l3gd20h::READ_LEN);
result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), 6);
if (result != returnvalue::OK) {
l3g.replyResult = returnvalue::FAILED;
return;
}
// The regular read function always returns the full sensor config as well. Use that
// to verify communications.
for (uint8_t idx = 0; idx < 5; idx++) {
if (rawReply[idx + 1] != l3g.sensorCfg[idx]) {
sif::warning << "AcsBoardPolling: l3g config check missmatch" << std::endl;
l3g.replyResult = returnvalue::FAILED;
return;
}
}
l3g.ownReply.statusReg = rawReply[l3gd20h::STATUS_IDX];
l3g.ownReply.angVelocities[0] = (rawReply[l3gd20h::OUT_X_H] << 8) | rawReply[l3gd20h::OUT_X_L];
l3g.ownReply.angVelocities[1] = (rawReply[l3gd20h::OUT_Y_H] << 8) | rawReply[l3gd20h::OUT_Y_L];
l3g.ownReply.angVelocities[2] = (rawReply[l3gd20h::OUT_Z_H] << 8) | rawReply[l3gd20h::OUT_Z_L];
l3g.ownReply.tempOffsetRaw = rawReply[l3gd20h::TEMPERATURE_IDX];
}
}
ReturnValue_t AcsBoardPolling::readAdisCfg(SpiCookie& cookie, size_t transferLen) {
ReturnValue_t result = returnvalue::OK;
int retval = 0;
// Prepare transfer
int fileDescriptor = 0;
std::string device = spiComIF.getSpiDev();
UnixFileGuard fileHelper(device, fileDescriptor, O_RDWR, "SpiComIF::sendMessage");
if (fileHelper.getOpenResult() != returnvalue::OK) {
return SpiComIF::OPENING_FILE_FAILED;
}
spi::SpiModes spiMode = spi::SpiModes::MODE_0;
uint32_t spiSpeed = 0;
cookie.getSpiParameters(spiMode, spiSpeed, nullptr);
spiComIF.setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
cookie.assignWriteBuffer(cmdBuf.data());
cookie.setTransferSize(2);
gpioId_t gpioId = cookie.getChipSelectPin();
MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
uint32_t timeoutMs = 0;
MutexIF* mutex = spiComIF.getCsMutex();
cookie.getMutexParams(timeoutType, timeoutMs);
if (mutex == nullptr) {
sif::warning << "GyroADIS16507Handler::spiSendCallback: "
"Mutex or GPIO interface invalid"
<< std::endl;
return returnvalue::FAILED;
}
size_t idx = 0;
spi_ioc_transfer* transferStruct = cookie.getTransferStructHandle();
uint64_t origTx = transferStruct->tx_buf;
uint64_t origRx = transferStruct->rx_buf;
while (idx < transferLen) {
result = mutex->lockMutex(timeoutType, timeoutMs);
if (result != returnvalue::OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "AcsBoardPolling: Failed to lock mutex" << std::endl;
#endif
return result;
}
// Pull SPI CS low. For now, no support for active high given
if (gpioId != gpio::NO_GPIO) {
gpioIF.pullLow(gpioId);
}
// Execute transfer
// Initiate a full duplex SPI transfer.
retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), cookie.getTransferStructHandle());
if (retval < 0) {
utility::handleIoctlError("SpiComIF::sendMessage: ioctl error.");
result = SpiComIF::FULL_DUPLEX_TRANSFER_FAILED;
}
#if FSFW_HAL_SPI_WIRETAPPING == 1
comIf->performSpiWiretapping(cookie);
#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */
if (gpioId != gpio::NO_GPIO) {
gpioIF.pullHigh(gpioId);
}
mutex->unlockMutex();
idx += 2;
transferStruct->tx_buf += 2;
transferStruct->rx_buf += 2;
if (idx < transferLen) {
usleep(adis1650x::STALL_TIME_MICROSECONDS);
}
}
transferStruct->tx_buf = origTx;
transferStruct->rx_buf = origRx;
cookie.setTransferSize(transferLen);
return returnvalue::OK;
}
void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {
ReturnValue_t result;
acs::SimpleSensorMode mode;
bool cdHasTimedOut = false;
bool mustPerformStartup = false;
{
MutexGuard mg(ipcLock);
mode = gyro.mode;
cdHasTimedOut = gyro.countdown.hasTimedOut();
mustPerformStartup = gyro.performStartup;
}
if (mode == acs::SimpleSensorMode::NORMAL and cdHasTimedOut) {
if (mustPerformStartup) {
uint8_t regList[6];
// Read configuration
regList[0] = adis1650x::DIAG_STAT_REG;
regList[1] = adis1650x::FILTER_CTRL_REG;
regList[2] = adis1650x::RANG_MDL_REG;
regList[3] = adis1650x::MSC_CTRL_REG;
regList[4] = adis1650x::DEC_RATE_REG;
regList[5] = adis1650x::PROD_ID_REG;
size_t transferLen =
adis1650x::prepareReadCommand(regList, sizeof(regList), cmdBuf.data(), cmdBuf.size());
result = readAdisCfg(*gyro.cookie, transferLen);
if (result != returnvalue::OK) {
gyro.replyResult = result;
return;
}
result = spiComIF.readReceivedMessage(gyro.cookie, &rawReply, &dummy);
if (result != returnvalue::OK or rawReply == nullptr) {
gyro.replyResult = result;
return;
}
uint16_t prodId = (rawReply[12] << 8) | rawReply[13];
if (((gyro.type == adis1650x::Type::ADIS16505) and (prodId != adis1650x::PROD_ID_16505)) or
((gyro.type == adis1650x::Type::ADIS16507) and (prodId != adis1650x::PROD_ID_16507))) {
sif::warning << "AcsPollingTask: Invalid ADIS product ID " << prodId << std::endl;
gyro.replyResult = returnvalue::FAILED;
return;
}
MutexGuard mg(ipcLock);
gyro.ownReply.cfgWasSet = true;
gyro.ownReply.cfg.diagStat = (rawReply[2] << 8) | rawReply[3];
gyro.ownReply.cfg.filterSetting = (rawReply[4] << 8) | rawReply[5];
gyro.ownReply.cfg.rangMdl = (rawReply[6] << 8) | rawReply[7];
gyro.ownReply.cfg.mscCtrlReg = (rawReply[8] << 8) | rawReply[9];
gyro.ownReply.cfg.decRateReg = (rawReply[10] << 8) | rawReply[11];
gyro.ownReply.cfg.prodId = prodId;
gyro.ownReply.data.sensitivity = adis1650x::rangMdlToSensitivity(gyro.ownReply.cfg.rangMdl);
gyro.performStartup = false;
}
// Read regular registers
std::memcpy(cmdBuf.data(), adis1650x::BURST_READ_ENABLE.data(),
adis1650x::BURST_READ_ENABLE.size());
std::memset(cmdBuf.data() + 2, 0, 10 * 2);
result = spiComIF.sendMessage(gyro.cookie, cmdBuf.data(), adis1650x::SENSOR_READOUT_SIZE);
if (result != returnvalue::OK) {
gyro.replyResult = returnvalue::FAILED;
return;
}
result = spiComIF.readReceivedMessage(gyro.cookie, &rawReply, &dummy);
if (result != returnvalue::OK or rawReply == nullptr) {
gyro.replyResult = returnvalue::FAILED;
return;
}
uint16_t checksum = (rawReply[20] << 8) | rawReply[21];
// Now verify the read checksum with the expected checksum according to datasheet p. 20
uint16_t calcChecksum = 0;
for (size_t idx = 2; idx < 20; idx++) {
calcChecksum += rawReply[idx];
}
if (checksum != calcChecksum) {
sif::warning << "AcsPollingTask: Invalid ADIS reply checksum" << std::endl;
gyro.replyResult = returnvalue::FAILED;
return;
}
auto burstMode = adis1650x::burstModeFromMscCtrl(gyro.ownReply.cfg.mscCtrlReg);
if (burstMode != adis1650x::BurstModes::BURST_16_BURST_SEL_0) {
sif::error << "GyroADIS1650XHandler::interpretDeviceReply: Analysis for select burst mode"
" not implemented!"
<< std::endl;
gyro.replyResult = returnvalue::FAILED;
return;
}
MutexGuard mg(ipcLock);
gyro.ownReply.dataWasSet = true;
gyro.ownReply.cfg.diagStat = rawReply[2] << 8 | rawReply[3];
gyro.ownReply.data.angVelocities[0] = (rawReply[4] << 8) | rawReply[5];
gyro.ownReply.data.angVelocities[1] = (rawReply[6] << 8) | rawReply[7];
gyro.ownReply.data.angVelocities[2] = (rawReply[8] << 8) | rawReply[9];
gyro.ownReply.data.accelerations[0] = (rawReply[10] << 8) | rawReply[11];
gyro.ownReply.data.accelerations[1] = (rawReply[12] << 8) | rawReply[13];
gyro.ownReply.data.accelerations[2] = (rawReply[14] << 8) | rawReply[15];
gyro.ownReply.data.temperatureRaw = (rawReply[16] << 8) | rawReply[17];
}
}

81
linux/devices/AcsBoardPolling.h Normal file
View File

@ -0,0 +1,81 @@
#ifndef LINUX_DEVICES_ACSBOARDPOLLING_H_
#define LINUX_DEVICES_ACSBOARDPOLLING_H_
#include <fsfw/devicehandlers/DeviceCommunicationIF.h>
#include <fsfw/objectmanager/SystemObject.h>
#include <fsfw/tasks/ExecutableObjectIF.h>
#include <fsfw/tasks/SemaphoreIF.h>
#include <fsfw_hal/linux/spi/SpiComIF.h>
#include <mission/devices/devicedefinitions/acsPolling.h>
#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
class AcsBoardPolling : public SystemObject,
public ExecutableObjectIF,
public DeviceCommunicationIF {
public:
AcsBoardPolling(object_id_t objectId, SpiComIF& lowLevelComIF, GpioIF& gpioIF);
ReturnValue_t performOperation(uint8_t operationCode) override;
ReturnValue_t initialize() override;
private:
enum class InternalState { IDLE, BUSY } state = InternalState::IDLE;
MutexIF* ipcLock;
SemaphoreIF* semaphore;
std::array<uint8_t, 32> cmdBuf;
std::array<uint8_t, 32> replyBuf;
bool mgm0L3IsOn = false;
SpiCookie* mgm0L3Cookie = nullptr;
bool mgm1Rm3100IsOn = false;
SpiCookie* mgm1Rm3100Cookie = nullptr;
bool mgm2L3IsOn = false;
SpiCookie* mgm2L3Cookie = nullptr;
bool mgm3Rm3100IsOn = false;
SpiCookie* mgm3Rm3100Cookie = nullptr;
struct GyroAdis {
adis1650x::Type type;
bool isOn = false;
bool performStartup = false;
SpiCookie* cookie = nullptr;
Countdown countdown;
acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
ReturnValue_t replyResult;
acs::Adis1650XReply ownReply;
acs::Adis1650XReply readerReply;
};
GyroAdis gyro0Adis{};
GyroAdis gyro2Adis{};
struct GyroL3g {
bool performStartup = false;
SpiCookie* cookie = nullptr;
acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
uint8_t sensorCfg[5];
ReturnValue_t replyResult;
acs::GyroL3gReply ownReply;
acs::GyroL3gReply readerReply;
};
GyroL3g gyro1L3g{};
GyroL3g gyro3L3g{};
uint8_t* rawReply = nullptr;
size_t dummy = 0;
SpiComIF& spiComIF;
GpioIF& gpioIF;
ReturnValue_t initializeInterface(CookieIF* cookie) override;
ReturnValue_t sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) override;
ReturnValue_t getSendSuccess(CookieIF* cookie) override;
ReturnValue_t requestReceiveMessage(CookieIF* cookie, size_t requestLen) override;
ReturnValue_t readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) override;
void gyroL3gHandler(GyroL3g& l3g);
void gyroAdisHandler(GyroAdis& gyro);
// Special readout: 16us stall time between small 2 byte transfers.
ReturnValue_t readAdisCfg(SpiCookie& spiCookie, size_t transferLen);
};
#endif /* LINUX_DEVICES_ACSBOARDPOLLING_H_ */

9
linux/devices/CMakeLists.txt
View File

@ -4,8 +4,13 @@ endif()
target_sources(
${OBSW_NAME}
PRIVATE Max31865RtdPolling.cpp ScexUartReader.cpp ImtqPollingTask.cpp
ScexDleParser.cpp ScexHelper.cpp RwPollingTask.cpp)
PRIVATE Max31865RtdPolling.cpp
ScexUartReader.cpp
ImtqPollingTask.cpp
ScexDleParser.cpp
ScexHelper.cpp
RwPollingTask.cpp
AcsBoardPolling.cpp)
add_subdirectory(ploc)

1
linux/fsfwconfig/objects/systemObjectList.h
View File

@ -47,7 +47,6 @@ enum sourceObjects : uint32_t {
GPIO_IF = 0x49010005,
/* Custom device handler */
RW_POLLING_TASK = 0x49020005,
/* 0x54 ('T') for test handlers */
TEST_TASK = 0x54694269,