838 lines
30 KiB
C++
838 lines
30 KiB
C++
#include "AcsBoardPolling.h"
|
|
|
|
#include <fcntl.h>
|
|
#include <fsfw/globalfunctions/arrayprinter.h>
|
|
#include <fsfw/tasks/SemaphoreFactory.h>
|
|
#include <fsfw/tasks/TaskFactory.h>
|
|
#include <fsfw/timemanager/Stopwatch.h>
|
|
#include <fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h>
|
|
#include <fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.h>
|
|
#include <fsfw_hal/linux/UnixFileGuard.h>
|
|
#include <fsfw_hal/linux/spi/SpiCookie.h>
|
|
#include <fsfw_hal/linux/utility.h>
|
|
#include <mission/acs/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(LOCK_TYPE, LOCK_TIMEOUT);
|
|
state = InternalState::IDLE;
|
|
ipcLock->unlockMutex();
|
|
semaphore->acquire();
|
|
// Give all tasks or the PST some time to submit all consecutive requests.
|
|
TaskFactory::delayTask(2);
|
|
{
|
|
// Measured to take 0-1 ms in debug build.
|
|
// Stopwatch watch;
|
|
gyroAdisHandler(gyro0Adis);
|
|
gyroAdisHandler(gyro2Adis);
|
|
gyroL3gHandler(gyro1L3g);
|
|
gyroL3gHandler(gyro3L3g);
|
|
mgmRm3100Handler(mgm1Rm3100);
|
|
mgmRm3100Handler(mgm3Rm3100);
|
|
mgmLis3Handler(mgm0Lis3);
|
|
mgmLis3Handler(mgm2Lis3);
|
|
}
|
|
// 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): {
|
|
mgm0Lis3.cookie = spiCookie;
|
|
break;
|
|
}
|
|
case (gpioIds::MGM_1_RM3100_CS): {
|
|
mgm1Rm3100.cookie = spiCookie;
|
|
break;
|
|
}
|
|
case (gpioIds::MGM_2_LIS3_CS): {
|
|
mgm2Lis3.cookie = spiCookie;
|
|
break;
|
|
}
|
|
case (gpioIds::MGM_3_RM3100_CS): {
|
|
mgm3Rm3100.cookie = 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);
|
|
if (req->mode != adis.mode) {
|
|
if (req->mode == acs::SimpleSensorMode::NORMAL) {
|
|
adis.type = req->type;
|
|
adis.decRate = req->cfg.decRateReg;
|
|
// The initial countdown is handled by the device handler now.
|
|
// adis.countdown.setTimeout(adis1650x::START_UP_TIME);
|
|
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.replyResult = returnvalue::FAILED;
|
|
adis.performStartup = true;
|
|
} else if (req->mode == acs::SimpleSensorMode::OFF) {
|
|
adis.performStartup = false;
|
|
adis.ownReply.cfgWasSet = false;
|
|
adis.ownReply.dataWasSet = false;
|
|
adis.replyResult = returnvalue::OK;
|
|
}
|
|
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);
|
|
if (req->mode != gyro.mode) {
|
|
if (req->mode == acs::SimpleSensorMode::NORMAL) {
|
|
std::memcpy(gyro.sensorCfg, req->ctrlRegs, 5);
|
|
gyro.performStartup = true;
|
|
gyro.replyResult = returnvalue::FAILED;
|
|
} else {
|
|
gyro.ownReply.cfgWasSet = false;
|
|
gyro.replyResult = returnvalue::OK;
|
|
}
|
|
gyro.mode = req->mode;
|
|
}
|
|
return returnvalue::OK;
|
|
};
|
|
auto handleLis3Request = [&](MgmLis3& mgm) {
|
|
if (sendLen != sizeof(acs::MgmLis3Request)) {
|
|
sif::error << "AcsBoardPolling: invalid lis3 request send length";
|
|
mgm.replyResult = returnvalue::FAILED;
|
|
return returnvalue::FAILED;
|
|
}
|
|
auto* req = reinterpret_cast<const acs::MgmLis3Request*>(sendData);
|
|
if (req->mode != mgm.mode) {
|
|
if (req->mode == acs::SimpleSensorMode::NORMAL) {
|
|
mgm.performStartup = true;
|
|
mgm.replyResult = returnvalue::FAILED;
|
|
} else {
|
|
mgm.ownReply.dataWasSet = false;
|
|
mgm.replyResult = returnvalue::OK;
|
|
mgm.ownReply.temperatureWasSet = false;
|
|
}
|
|
mgm.mode = req->mode;
|
|
}
|
|
return returnvalue::OK;
|
|
};
|
|
auto handleRm3100Request = [&](MgmRm3100& mgm) {
|
|
if (sendLen != sizeof(acs::MgmRm3100Request)) {
|
|
sif::error << "AcsBoardPolling: invalid rm3100 request send length";
|
|
mgm.replyResult = returnvalue::FAILED;
|
|
return returnvalue::FAILED;
|
|
}
|
|
auto* req = reinterpret_cast<const acs::MgmRm3100Request*>(sendData);
|
|
if (req->mode != mgm.mode) {
|
|
if (req->mode == acs::SimpleSensorMode::NORMAL) {
|
|
mgm.performStartup = true;
|
|
mgm.replyResult = returnvalue::FAILED;
|
|
} else {
|
|
mgm.ownReply.dataWasRead = false;
|
|
mgm.replyResult = returnvalue::OK;
|
|
}
|
|
mgm.mode = req->mode;
|
|
}
|
|
return returnvalue::OK;
|
|
};
|
|
{
|
|
MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
|
|
switch (spiCookie->getChipSelectPin()) {
|
|
case (gpioIds::MGM_0_LIS3_CS): {
|
|
handleLis3Request(mgm0Lis3);
|
|
break;
|
|
}
|
|
case (gpioIds::MGM_1_RM3100_CS): {
|
|
handleRm3100Request(mgm1Rm3100);
|
|
break;
|
|
}
|
|
case (gpioIds::MGM_2_LIS3_CS): {
|
|
handleLis3Request(mgm2Lis3);
|
|
break;
|
|
}
|
|
case (gpioIds::MGM_3_RM3100_CS): {
|
|
handleRm3100Request(mgm3Rm3100);
|
|
break;
|
|
}
|
|
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;
|
|
}
|
|
}
|
|
if (state == InternalState::IDLE) {
|
|
state = InternalState::IS_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, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
|
|
auto handleAdisReply = [&](GyroAdis& gyro) {
|
|
std::memcpy(&gyro.readerReply, &gyro.ownReply, sizeof(acs::Adis1650XReply));
|
|
*buffer = reinterpret_cast<uint8_t*>(&gyro.readerReply);
|
|
*size = sizeof(acs::Adis1650XReply);
|
|
return gyro.replyResult;
|
|
};
|
|
auto handleL3gReply = [&](GyroL3g& gyro) {
|
|
std::memcpy(&gyro.readerReply, &gyro.ownReply, sizeof(acs::GyroL3gReply));
|
|
*buffer = reinterpret_cast<uint8_t*>(&gyro.readerReply);
|
|
*size = sizeof(acs::GyroL3gReply);
|
|
return gyro.replyResult;
|
|
};
|
|
auto handleRm3100Reply = [&](MgmRm3100& mgm) {
|
|
std::memcpy(&mgm.readerReply, &mgm.ownReply, sizeof(acs::MgmRm3100Reply));
|
|
*buffer = reinterpret_cast<uint8_t*>(&mgm.readerReply);
|
|
*size = sizeof(acs::MgmRm3100Reply);
|
|
return mgm.replyResult;
|
|
};
|
|
auto handleLis3Reply = [&](MgmLis3& mgm) {
|
|
std::memcpy(&mgm.readerReply, &mgm.ownReply, sizeof(acs::MgmLis3Reply));
|
|
*buffer = reinterpret_cast<uint8_t*>(&mgm.readerReply);
|
|
*size = sizeof(acs::MgmLis3Reply);
|
|
return mgm.replyResult;
|
|
};
|
|
switch (spiCookie->getChipSelectPin()) {
|
|
case (gpioIds::MGM_0_LIS3_CS): {
|
|
return handleLis3Reply(mgm0Lis3);
|
|
}
|
|
case (gpioIds::MGM_1_RM3100_CS): {
|
|
return handleRm3100Reply(mgm1Rm3100);
|
|
}
|
|
case (gpioIds::MGM_2_LIS3_CS): {
|
|
return handleLis3Reply(mgm2Lis3);
|
|
}
|
|
case (gpioIds::MGM_3_RM3100_CS): {
|
|
return handleRm3100Reply(mgm3Rm3100);
|
|
}
|
|
case (gpioIds::GYRO_0_ADIS_CS): {
|
|
return handleAdisReply(gyro0Adis);
|
|
}
|
|
case (gpioIds::GYRO_2_ADIS_CS): {
|
|
return handleAdisReply(gyro2Adis);
|
|
}
|
|
case (gpioIds::GYRO_1_L3G_CS): {
|
|
return handleL3gReply(gyro1L3g);
|
|
}
|
|
case (gpioIds::GYRO_3_L3G_CS): {
|
|
return handleL3gReply(gyro3L3g);
|
|
}
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) {
|
|
ReturnValue_t result;
|
|
acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
|
|
bool gyroPerformStartup = false;
|
|
{
|
|
MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
|
|
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 = result;
|
|
}
|
|
// 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 = result;
|
|
}
|
|
result = spiComIF.readReceivedMessage(l3g.cookie, &rawReply, &dummy);
|
|
if (result != returnvalue::OK) {
|
|
l3g.replyResult = result;
|
|
}
|
|
MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
|
|
// 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.replyResult = returnvalue::OK;
|
|
l3g.performStartup = false;
|
|
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(), l3gd20h::READ_LEN + 1);
|
|
if (result != returnvalue::OK) {
|
|
l3g.replyResult = returnvalue::FAILED;
|
|
return;
|
|
}
|
|
result = spiComIF.readReceivedMessage(l3g.cookie, &rawReply, &dummy);
|
|
if (result != returnvalue::OK) {
|
|
l3g.replyResult = returnvalue::FAILED;
|
|
return;
|
|
}
|
|
MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
|
|
// 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.replyResult = returnvalue::OK;
|
|
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::writeAdisReg(SpiCookie& cookie) {
|
|
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 spi::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;
|
|
for (idx = 0; idx < 4; idx += 2) {
|
|
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 = spi::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();
|
|
|
|
transferStruct->tx_buf += 2;
|
|
transferStruct->rx_buf += 2;
|
|
if (idx < 4) {
|
|
usleep(adis1650x::STALL_TIME_MICROSECONDS);
|
|
}
|
|
}
|
|
transferStruct->tx_buf = origTx;
|
|
transferStruct->rx_buf = origRx;
|
|
cookie.setTransferSize(0);
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
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 spi::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 = spi::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 = acs::SimpleSensorMode::OFF;
|
|
bool mustPerformStartup = false;
|
|
uint16_t decRate = 0;
|
|
{
|
|
MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
|
|
mode = gyro.mode;
|
|
decRate = gyro.decRate;
|
|
mustPerformStartup = gyro.performStartup;
|
|
}
|
|
if (mode == acs::SimpleSensorMode::OFF) {
|
|
return;
|
|
}
|
|
if (mustPerformStartup) {
|
|
adis1650x::prepareWriteRegCommand(adis1650x::DEC_RATE_REG, decRate, cmdBuf.data(),
|
|
cmdBuf.size());
|
|
writeAdisReg(*gyro.cookie);
|
|
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;
|
|
}
|
|
uint16_t decRateReadBack = (rawReply[10] << 8) | rawReply[11];
|
|
if (decRateReadBack != decRate) {
|
|
sif::warning << "AcsPollingTask: DEC rate configuration failed. Read " << decRateReadBack
|
|
<< ", expected " << decRate << std::endl;
|
|
gyro.replyResult = returnvalue::FAILED;
|
|
}
|
|
MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
|
|
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 = decRateReadBack;
|
|
gyro.ownReply.cfg.prodId = prodId;
|
|
gyro.ownReply.data.sensitivity = adis1650x::rangMdlToSensitivity(gyro.ownReply.cfg.rangMdl);
|
|
gyro.performStartup = false;
|
|
gyro.replyResult = returnvalue::OK;
|
|
}
|
|
// 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, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
|
|
gyro.replyResult = returnvalue::OK;
|
|
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];
|
|
}
|
|
|
|
void AcsBoardPolling::mgmLis3Handler(MgmLis3& mgm) {
|
|
ReturnValue_t result;
|
|
acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
|
|
bool mustPerformStartup = false;
|
|
{
|
|
MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
|
|
mode = mgm.mode;
|
|
mustPerformStartup = mgm.performStartup;
|
|
}
|
|
if (mode == acs::SimpleSensorMode::NORMAL) {
|
|
if (mustPerformStartup) {
|
|
// To check valid communication, read back identification
|
|
// register which should always be the same value.
|
|
cmdBuf[0] = mgmLis3::readCommand(mgmLis3::IDENTIFY_DEVICE_REG_ADDR);
|
|
cmdBuf[1] = 0x00;
|
|
result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 2);
|
|
if (result != OK) {
|
|
mgm.replyResult = result;
|
|
return;
|
|
}
|
|
result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy);
|
|
if (result != OK) {
|
|
mgm.replyResult = result;
|
|
return;
|
|
}
|
|
if (rawReply[1] != mgmLis3::DEVICE_ID) {
|
|
sif::error << "AcsPollingTask: invalid MGM lis3 device ID" << std::endl;
|
|
mgm.replyResult = result;
|
|
return;
|
|
}
|
|
mgm.cfg[0] = mgmLis3::CTRL_REG1_DEFAULT;
|
|
mgm.cfg[1] = mgmLis3::CTRL_REG2_DEFAULT;
|
|
mgm.cfg[2] = mgmLis3::CTRL_REG3_DEFAULT;
|
|
mgm.cfg[3] = mgmLis3::CTRL_REG4_DEFAULT;
|
|
mgm.cfg[4] = mgmLis3::CTRL_REG5_DEFAULT;
|
|
cmdBuf[0] = mgmLis3::writeCommand(mgmLis3::CTRL_REG1, true);
|
|
std::memcpy(cmdBuf.data() + 1, mgm.cfg, 5);
|
|
result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 6);
|
|
if (result != OK) {
|
|
mgm.replyResult = result;
|
|
return;
|
|
}
|
|
// Done here. We can always read back config and data during periodic handling
|
|
mgm.performStartup = false;
|
|
mgm.replyResult = returnvalue::OK;
|
|
}
|
|
cmdBuf[0] = mgmLis3::readCommand(mgmLis3::CTRL_REG1, true);
|
|
std::memset(cmdBuf.data() + 1, 0, mgmLis3::NR_OF_DATA_AND_CFG_REGISTERS);
|
|
result =
|
|
spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), mgmLis3::NR_OF_DATA_AND_CFG_REGISTERS + 1);
|
|
if (result != returnvalue::OK) {
|
|
mgm.replyResult = result;
|
|
return;
|
|
}
|
|
result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy);
|
|
if (result != returnvalue::OK) {
|
|
mgm.replyResult = result;
|
|
return;
|
|
}
|
|
// Verify communication by re-checking config
|
|
if (rawReply[1] != mgm.cfg[0] or rawReply[2] != mgm.cfg[1] or rawReply[3] != mgm.cfg[2] or
|
|
rawReply[4] != mgm.cfg[3] or rawReply[5] != mgm.cfg[4]) {
|
|
mgm.replyResult = returnvalue::FAILED;
|
|
return;
|
|
}
|
|
{
|
|
MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
|
|
mgm.ownReply.dataWasSet = true;
|
|
mgm.ownReply.sensitivity = mgmLis3::getSensitivityFactor(mgmLis3::getSensitivity(mgm.cfg[1]));
|
|
mgm.ownReply.mgmValuesRaw[0] =
|
|
(rawReply[mgmLis3::X_HIGHBYTE_IDX] << 8) | rawReply[mgmLis3::X_LOWBYTE_IDX];
|
|
mgm.ownReply.mgmValuesRaw[1] =
|
|
(rawReply[mgmLis3::Y_HIGHBYTE_IDX] << 8) | rawReply[mgmLis3::Y_LOWBYTE_IDX];
|
|
mgm.ownReply.mgmValuesRaw[2] =
|
|
(rawReply[mgmLis3::Z_HIGHBYTE_IDX] << 8) | rawReply[mgmLis3::Z_LOWBYTE_IDX];
|
|
}
|
|
// Read tempetature
|
|
cmdBuf[0] = mgmLis3::readCommand(mgmLis3::TEMP_LOWBYTE, true);
|
|
result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 3);
|
|
if (result != returnvalue::OK) {
|
|
mgm.replyResult = result;
|
|
return;
|
|
}
|
|
result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy);
|
|
if (result != returnvalue::OK) {
|
|
mgm.replyResult = result;
|
|
return;
|
|
}
|
|
MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
|
|
mgm.replyResult = returnvalue::OK;
|
|
mgm.ownReply.temperatureWasSet = true;
|
|
mgm.ownReply.temperatureRaw = (rawReply[2] << 8) | rawReply[1];
|
|
}
|
|
}
|
|
|
|
void AcsBoardPolling::mgmRm3100Handler(MgmRm3100& mgm) {
|
|
ReturnValue_t result;
|
|
acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
|
|
bool mustPerformStartup = false;
|
|
{
|
|
MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
|
|
mode = mgm.mode;
|
|
mustPerformStartup = mgm.performStartup;
|
|
}
|
|
if (mode == acs::SimpleSensorMode::NORMAL) {
|
|
if (mustPerformStartup) {
|
|
// Configure CMM first
|
|
cmdBuf[0] = mgmRm3100::CMM_REGISTER;
|
|
cmdBuf[1] = mgmRm3100::CMM_VALUE;
|
|
result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 2);
|
|
if (result != OK) {
|
|
mgm.replyResult = result;
|
|
return;
|
|
}
|
|
// Read back register
|
|
cmdBuf[0] = mgmRm3100::CMM_REGISTER | mgmRm3100::READ_MASK;
|
|
cmdBuf[1] = 0;
|
|
result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 2);
|
|
if (result != OK) {
|
|
mgm.replyResult = result;
|
|
return;
|
|
}
|
|
result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy);
|
|
if (result != OK) {
|
|
mgm.replyResult = result;
|
|
return;
|
|
}
|
|
// For some reason, bit 1 is sometimes set on the reply, even if it is not set for the
|
|
// command.. Ignore it for now by clearing it.
|
|
rawReply[1] &= ~(1 << 1);
|
|
if (rawReply[1] != mgmRm3100::CMM_VALUE) {
|
|
sif::error << "AcsBoardPolling: MGM RM3100 read back CMM invalid" << std::endl;
|
|
mgm.replyResult = result;
|
|
return;
|
|
}
|
|
// Configure TMRC register
|
|
cmdBuf[0] = mgmRm3100::TMRC_REGISTER;
|
|
// hardcoded for now
|
|
cmdBuf[1] = mgm.tmrcValue;
|
|
result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 2);
|
|
if (result != OK) {
|
|
mgm.replyResult = result;
|
|
return;
|
|
}
|
|
// Read back and verify value
|
|
cmdBuf[0] = mgmRm3100::TMRC_REGISTER | mgmRm3100::READ_MASK;
|
|
cmdBuf[1] = 0;
|
|
result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 2);
|
|
if (result != OK) {
|
|
mgm.replyResult = result;
|
|
return;
|
|
}
|
|
result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy);
|
|
if (result != OK) {
|
|
mgm.replyResult = result;
|
|
return;
|
|
}
|
|
if (rawReply[1] != mgm.tmrcValue) {
|
|
sif::error << "AcsBoardPolling: MGM RM3100 read back TMRC invalid" << std::endl;
|
|
mgm.replyResult = result;
|
|
return;
|
|
}
|
|
mgm.performStartup = false;
|
|
mgm.replyResult = returnvalue::OK;
|
|
}
|
|
// Regular read operation
|
|
cmdBuf[0] = mgmRm3100::MEASUREMENT_REG_START | mgmRm3100::READ_MASK;
|
|
std::memset(cmdBuf.data() + 1, 0, 9);
|
|
result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 10);
|
|
if (result != OK) {
|
|
mgm.replyResult = result;
|
|
return;
|
|
}
|
|
result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy);
|
|
if (result != OK) {
|
|
mgm.replyResult = result;
|
|
return;
|
|
}
|
|
MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
|
|
for (uint8_t idx = 0; idx < 3; idx++) {
|
|
// Hardcoded, but note that the gain depends on the cycle count
|
|
// value which is configurable!
|
|
mgm.ownReply.scaleFactors[idx] = 1.0 / mgmRm3100::DEFAULT_GAIN;
|
|
}
|
|
mgm.ownReply.dataWasRead = true;
|
|
mgm.replyResult = returnvalue::OK;
|
|
// Bitshift trickery to account for 24 bit signed value.
|
|
mgm.ownReply.mgmValuesRaw[0] =
|
|
((rawReply[1] << 24) | (rawReply[2] << 16) | (rawReply[3] << 8)) >> 8;
|
|
mgm.ownReply.mgmValuesRaw[1] =
|
|
((rawReply[4] << 24) | (rawReply[5] << 16) | (rawReply[6] << 8)) >> 8;
|
|
mgm.ownReply.mgmValuesRaw[2] =
|
|
((rawReply[7] << 24) | (rawReply[8] << 16) | (rawReply[9] << 8)) >> 8;
|
|
}
|
|
}
|