eive-obsw/mission/devices/GyroADIS1650XHandler.cpp

482 lines
19 KiB
C++
Raw Normal View History

#include "GyroADIS1650XHandler.h"
2021-05-26 13:30:12 +02:00
#include <fsfw/action/HasActionsIF.h>
2021-05-24 20:33:59 +02:00
#include <fsfw/datapool/PoolReadGuard.h>
2021-05-25 12:45:51 +02:00
#if OBSW_ADIS1650X_LINUX_COM_IF == 1
2021-08-03 15:58:01 +02:00
#include "fsfw_hal/linux/utility.h"
#include "fsfw_hal/linux/spi/SpiCookie.h"
#include "fsfw_hal/linux/spi/SpiComIF.h"
#include "fsfw_hal/linux/UnixFileGuard.h"
2021-05-25 12:45:51 +02:00
#include <sys/ioctl.h>
2021-05-24 20:57:12 +02:00
#include <unistd.h>
2021-05-25 12:45:51 +02:00
#endif
2021-05-24 20:57:12 +02:00
GyroADIS1650XHandler::GyroADIS1650XHandler(object_id_t objectId,
object_id_t deviceCommunication, CookieIF * comCookie, ADIS1650X::Type type):
DeviceHandlerBase(objectId, deviceCommunication, comCookie), adisType(type),
primaryDataset(this), configDataset(this), breakCountdown() {
#if FSFW_HAL_ADIS1650X_GYRO_DEBUG == 1
2021-05-25 13:53:03 +02:00
debugDivider = new PeriodicOperationDivider(5);
#endif
#if OBSW_ADIS1650X_LINUX_COM_IF == 1
2021-05-24 20:33:59 +02:00
SpiCookie* cookie = dynamic_cast<SpiCookie*>(comCookie);
if(cookie != nullptr) {
cookie->setCallbackMode(&spiSendCallback, this);
}
#endif
2021-05-24 00:30:46 +02:00
}
void GyroADIS1650XHandler::doStartUp() {
2021-05-26 13:41:26 +02:00
// Initial 310 ms start up time after power-up
2021-05-24 01:20:44 +02:00
if(internalState == InternalState::STARTUP) {
2021-05-26 13:30:12 +02:00
if(not commandExecuted) {
breakCountdown.setTimeout(ADIS1650X::START_UP_TIME);
2021-05-26 13:30:12 +02:00
commandExecuted = true;
}
if(breakCountdown.hasTimedOut()) {
internalState = InternalState::CONFIG;
2021-05-26 13:41:26 +02:00
commandExecuted = false;
2021-05-26 13:30:12 +02:00
}
}
2021-05-26 13:41:26 +02:00
// Read all configuration registers first
2021-05-26 13:30:12 +02:00
if(internalState == InternalState::CONFIG) {
2021-05-24 01:20:44 +02:00
if(commandExecuted) {
commandExecuted = false;
internalState = InternalState::IDLE;
}
}
2021-05-26 13:30:12 +02:00
2021-05-25 13:53:03 +02:00
if(internalState == InternalState::IDLE) {
setMode(MODE_NORMAL);
2021-05-26 13:41:26 +02:00
// setMode(MODE_ON);
2021-05-25 13:53:03 +02:00
}
2021-05-24 00:30:46 +02:00
}
void GyroADIS1650XHandler::doShutDown() {
2021-05-26 13:30:12 +02:00
commandExecuted = false;
setMode(_MODE_POWER_DOWN);
2021-05-24 00:30:46 +02:00
}
ReturnValue_t GyroADIS1650XHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
*id = ADIS1650X::READ_SENSOR_DATA;
2021-05-24 20:33:59 +02:00
return buildCommandFromCommand(*id, nullptr, 0);
2021-05-24 00:30:46 +02:00
}
ReturnValue_t GyroADIS1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
2021-05-24 20:33:59 +02:00
switch(internalState) {
2021-05-26 13:41:26 +02:00
case(InternalState::CONFIG): {
*id = ADIS1650X::READ_OUT_CONFIG;
2021-05-24 20:33:59 +02:00
buildCommandFromCommand(*id, nullptr, 0);
break;
}
2021-05-26 13:41:26 +02:00
case(InternalState::STARTUP): {
return NOTHING_TO_SEND;
2021-05-26 13:41:26 +02:00
break;
}
2021-05-24 20:33:59 +02:00
default: {
/* Might be a configuration error. */
sif::debug << "GyroADIS16507Handler::buildTransitionDeviceCommand: "
"Unknown internal state!" << std::endl;
return HasReturnvaluesIF::RETURN_OK;
}
}
2021-05-24 00:30:46 +02:00
return HasReturnvaluesIF::RETURN_OK;
}
ReturnValue_t GyroADIS1650XHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
2021-05-24 00:30:46 +02:00
const uint8_t *commandData, size_t commandDataLen) {
2021-05-24 20:33:59 +02:00
switch(deviceCommand) {
case(ADIS1650X::READ_OUT_CONFIG): {
this->rawPacketLen = ADIS1650X::CONFIG_READOUT_SIZE;
uint8_t regList[5];
regList[0] = ADIS1650X::DIAG_STAT_REG;
regList[1] = ADIS1650X::FILTER_CTRL_REG;
regList[2] = ADIS1650X::MSC_CTRL_REG;
regList[3] = ADIS1650X::DEC_RATE_REG;
regList[4] = ADIS1650X::PROD_ID_REG;
2021-05-24 20:33:59 +02:00
prepareReadCommand(regList, sizeof(regList));
this->rawPacket = commandBuffer.data();
break;
}
case(ADIS1650X::READ_SENSOR_DATA): {
2021-05-26 13:30:12 +02:00
if(breakCountdown.isBusy()) {
// A glob command is pending and sensor data can't be read currently
return NO_REPLY_EXPECTED;
}
std::memcpy(commandBuffer.data(), ADIS1650X::BURST_READ_ENABLE.data(),
ADIS1650X::BURST_READ_ENABLE.size());
2021-05-24 20:33:59 +02:00
std::memset(commandBuffer.data() + 2, 0, 10 * 2);
this->rawPacketLen = ADIS1650X::SENSOR_READOUT_SIZE;
2021-05-24 20:33:59 +02:00
this->rawPacket = commandBuffer.data();
break;
}
case(ADIS1650X::SELF_TEST_SENSORS): {
2021-05-26 13:30:12 +02:00
if(breakCountdown.isBusy()) {
// Another glob command is pending
return HasActionsIF::IS_BUSY;
}
prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::SENSOR_SELF_TEST, 0x00);
breakCountdown.setTimeout(ADIS1650X::SELF_TEST_BREAK);
2021-05-26 13:11:48 +02:00
break;
}
case(ADIS1650X::SELF_TEST_MEMORY): {
2021-05-26 13:30:12 +02:00
if(breakCountdown.isBusy()) {
// Another glob command is pending
return HasActionsIF::IS_BUSY;
}
prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FLASH_MEMORY_TEST, 0x00);
breakCountdown.setTimeout(ADIS1650X::FLASH_MEMORY_TEST_BREAK);
2021-05-26 13:11:48 +02:00
break;
}
case(ADIS1650X::UPDATE_NV_CONFIGURATION): {
2021-05-26 13:30:12 +02:00
if(breakCountdown.isBusy()) {
// Another glob command is pending
return HasActionsIF::IS_BUSY;
}
prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FLASH_MEMORY_UPDATE, 0x00);
breakCountdown.setTimeout(ADIS1650X::FLASH_MEMORY_UPDATE_BREAK);
2021-05-26 13:11:48 +02:00
break;
}
case(ADIS1650X::RESET_SENSOR_CONFIGURATION): {
2021-05-26 13:30:12 +02:00
if(breakCountdown.isBusy()) {
// Another glob command is pending
return HasActionsIF::IS_BUSY;
}
prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FACTORY_CALIBRATION, 0x00);
breakCountdown.setTimeout(ADIS1650X::FACTORY_CALIBRATION_BREAK);
2021-05-26 13:11:48 +02:00
break;
}
case(ADIS1650X::SW_RESET): {
2021-05-26 13:30:12 +02:00
if(breakCountdown.isBusy()) {
// Another glob command is pending
return HasActionsIF::IS_BUSY;
}
prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::SOFTWARE_RESET, 0x00);
breakCountdown.setTimeout(ADIS1650X::SW_RESET_BREAK);
2021-05-26 13:11:48 +02:00
break;
}
case(ADIS1650X::PRINT_CURRENT_CONFIGURATION): {
2021-05-26 13:11:48 +02:00
#if OBSW_VERBOSE_LEVEL >= 1
PoolReadGuard pg(&configDataset);
sif::info << "ADIS16507 Sensor configuration: DIAG_STAT: 0x" << std::hex << std::setw(4) <<
std::setfill('0') << "0x" << configDataset.diagStatReg.value << std::endl;
sif::info << "MSC_CTRL: " << std::hex << std::setw(4) << "0x" <<
configDataset.mscCtrlReg.value << " | FILT_CTRL: 0x" <<
configDataset.filterSetting.value << " | DEC_RATE: 0x" <<
configDataset.decRateReg.value << std::setfill(' ') << std::endl;
#endif /* OBSW_VERBOSE_LEVEL >= 1 */
}
2021-05-24 20:33:59 +02:00
}
2021-05-24 00:30:46 +02:00
return HasReturnvaluesIF::RETURN_OK;
}
void GyroADIS1650XHandler::fillCommandAndReplyMap() {
insertInCommandAndReplyMap(ADIS1650X::READ_SENSOR_DATA, 1, &primaryDataset);
insertInCommandAndReplyMap(ADIS1650X::READ_OUT_CONFIG, 1, &configDataset);
insertInCommandAndReplyMap(ADIS1650X::SELF_TEST_SENSORS, 1);
insertInCommandAndReplyMap(ADIS1650X::SELF_TEST_MEMORY, 1);
insertInCommandAndReplyMap(ADIS1650X::UPDATE_NV_CONFIGURATION, 1);
insertInCommandAndReplyMap(ADIS1650X::RESET_SENSOR_CONFIGURATION, 1);
insertInCommandAndReplyMap(ADIS1650X::SW_RESET, 1);
insertInCommandAndReplyMap(ADIS1650X::PRINT_CURRENT_CONFIGURATION, 1);
2021-05-24 00:30:46 +02:00
}
ReturnValue_t GyroADIS1650XHandler::scanForReply(const uint8_t *start, size_t remainingSize,
2021-05-24 00:30:46 +02:00
DeviceCommandId_t *foundId, size_t *foundLen) {
2021-05-24 20:33:59 +02:00
/* For SPI, the ID will always be the one of the last sent command. */
*foundId = this->getPendingCommand();
*foundLen = this->rawPacketLen;
2021-05-24 00:30:46 +02:00
return HasReturnvaluesIF::RETURN_OK;
}
ReturnValue_t GyroADIS1650XHandler::interpretDeviceReply(DeviceCommandId_t id,
2021-05-24 00:30:46 +02:00
const uint8_t *packet) {
2021-05-24 20:33:59 +02:00
switch(id) {
case(ADIS1650X::READ_OUT_CONFIG): {
2021-05-24 20:33:59 +02:00
PoolReadGuard rg(&configDataset);
2021-05-25 13:53:03 +02:00
uint16_t readProdId = packet[10] << 8 | packet[11];
if(((adisType == ADIS1650X::Type::ADIS16507) and
(readProdId != ADIS1650X::PROD_ID_16507)) or
((adisType == ADIS1650X::Type::ADIS16505) and
(readProdId != ADIS1650X::PROD_ID_16505))) {
2021-05-24 20:33:59 +02:00
#if OBSW_VERBOSE_LEVEL >= 1
sif::warning << "GyroADIS16507Handler::interpretDeviceReply: Invalid product ID "
<< readProdId << std::endl;
2021-05-24 20:33:59 +02:00
#endif
return HasReturnvaluesIF::RETURN_FAILED;
}
2021-05-25 13:53:03 +02:00
configDataset.diagStatReg.value = packet[2] << 8 | packet[3];
configDataset.filterSetting.value = packet[4] << 8 | packet[5];
configDataset.mscCtrlReg.value = packet[6] << 8 | packet[7];
configDataset.decRateReg.value = packet[8] << 8 | packet[9];
configDataset.setValidity(true, true);
2021-05-26 13:41:26 +02:00
if(internalState == InternalState::CONFIG) {
2021-05-24 20:33:59 +02:00
commandExecuted = true;
}
break;
}
case(ADIS1650X::READ_SENSOR_DATA): {
return handleSensorData(packet);
}
2021-05-24 20:57:12 +02:00
}
return HasReturnvaluesIF::RETURN_OK;
}
2021-05-24 20:57:12 +02:00
ReturnValue_t GyroADIS1650XHandler::handleSensorData(const uint8_t *packet) {
BurstModes burstMode = getBurstMode();
switch(burstMode) {
case(BurstModes::BURST_16_BURST_SEL_1):
case(BurstModes::BURST_32_BURST_SEL_1): {
sif::warning << "GyroADIS16507Handler::interpretDeviceReply: Analysis with BURST_SEL1"
" not implemented!" << std::endl;
return HasReturnvaluesIF::RETURN_OK;
}
case(BurstModes::BURST_16_BURST_SEL_0): {
uint16_t checksum = packet[20] << 8 | packet[21];
/* Now verify the read checksum with the expected checksum
according to datasheet p. 20 */
uint16_t calcChecksum = 0;
2021-05-25 15:16:18 +02:00
for(size_t idx = 2; idx < 20; idx ++) {
calcChecksum += packet[idx];
2021-05-24 20:57:12 +02:00
}
if(checksum != calcChecksum) {
#if OBSW_VERBOSE_LEVEL >= 1
sif::warning << "GyroADIS16507Handler::interpretDeviceReply: "
"Invalid checksum detected!" << std::endl;
#endif
return HasReturnvaluesIF::RETURN_FAILED;
2021-05-24 20:57:12 +02:00
}
ReturnValue_t result = configDataset.diagStatReg.read();
if(result == HasReturnvaluesIF::RETURN_OK) {
configDataset.diagStatReg.value = packet[2] << 8 | packet[3];
configDataset.diagStatReg.setValid(true);
}
configDataset.diagStatReg.commit();
2021-05-25 13:53:03 +02:00
{
PoolReadGuard pg(&primaryDataset);
int16_t angVelocXRaw = packet[4] << 8 | packet[5];
primaryDataset.angVelocX.value = static_cast<float>(angVelocXRaw) / INT16_MAX *
ADIS1650X::GYRO_RANGE;
2021-05-25 13:53:03 +02:00
int16_t angVelocYRaw = packet[6] << 8 | packet[7];
primaryDataset.angVelocY.value = static_cast<float>(angVelocYRaw) / INT16_MAX *
ADIS1650X::GYRO_RANGE;
2021-05-25 13:53:03 +02:00
int16_t angVelocZRaw = packet[8] << 8 | packet[9];
primaryDataset.angVelocZ.value = static_cast<float>(angVelocZRaw) / INT16_MAX *
ADIS1650X::GYRO_RANGE;
float accelScaling = 0;
if(adisType == ADIS1650X::Type::ADIS16507) {
accelScaling = ADIS1650X::ACCELEROMETER_RANGE_16507;
} else if(adisType == ADIS1650X::Type::ADIS16505) {
accelScaling = ADIS1650X::ACCELEROMETER_RANGE_16505;
} else {
sif::warning << "GyroADIS16507Handler::handleSensorData: "
"Unknown ADIS type" << std::endl;
}
2021-05-25 13:53:03 +02:00
int16_t accelXRaw = packet[10] << 8 | packet[11];
primaryDataset.accelX.value = static_cast<float>(accelXRaw) / INT16_MAX *
accelScaling;
2021-05-25 13:53:03 +02:00
int16_t accelYRaw = packet[12] << 8 | packet[13];
primaryDataset.accelY.value = static_cast<float>(accelYRaw) / INT16_MAX *
accelScaling;
2021-05-25 13:53:03 +02:00
int16_t accelZRaw = packet[14] << 8 | packet[15];
primaryDataset.accelZ.value = static_cast<float>(accelZRaw) / INT16_MAX *
accelScaling;
2021-05-25 13:53:03 +02:00
int16_t temperatureRaw = packet[16] << 8 | packet[17];
primaryDataset.temperature.value = static_cast<float>(temperatureRaw) * 0.1;
// Ignore data counter for now
primaryDataset.setValidity(true, true);
}
#if FSFW_HAL_ADIS1650X_GYRO_DEBUG == 1
2021-05-25 13:53:03 +02:00
if(debugDivider->checkAndIncrement()) {
2021-05-25 15:16:18 +02:00
sif::info << "GyroADIS16507Handler: Angular velocities in deg / s" << std::endl;
sif::info << "X: " << primaryDataset.angVelocX.value << std::endl;
sif::info << "Y: " << primaryDataset.angVelocY.value << std::endl;
sif::info << "Z: " << primaryDataset.angVelocZ.value << std::endl;
sif::info << "GyroADIS16507Handler: Accelerations in m / s^2: " << std::endl;
sif::info << "X: " << primaryDataset.accelX.value << std::endl;
sif::info << "Y: " << primaryDataset.accelY.value << std::endl;
sif::info << "Z: " << primaryDataset.accelZ.value << std::endl;
2021-05-25 13:53:03 +02:00
}
#endif
break;
}
case(BurstModes::BURST_32_BURST_SEL_0): {
break;
2021-05-24 20:33:59 +02:00
}
}
return HasReturnvaluesIF::RETURN_OK;
2021-05-24 00:30:46 +02:00
}
2021-05-24 01:20:44 +02:00
uint32_t GyroADIS1650XHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
2021-05-24 01:20:44 +02:00
return 5000;
}
2021-05-24 20:33:59 +02:00
void GyroADIS1650XHandler::prepareWriteCommand(uint8_t startReg, uint8_t valueOne,
2021-05-26 13:11:48 +02:00
uint8_t valueTwo) {
uint8_t secondReg = startReg + 1;
startReg |= ADIS1650X::WRITE_MASK;
secondReg |= ADIS1650X::WRITE_MASK;
2021-05-26 13:11:48 +02:00
commandBuffer[0] = startReg;
commandBuffer[1] = valueOne;
commandBuffer[2] = secondReg;
commandBuffer[3] = valueTwo;
2021-05-26 13:30:12 +02:00
this->rawPacketLen = 4;
this->rawPacket = commandBuffer.data();
2021-05-26 13:11:48 +02:00
}
void GyroADIS1650XHandler::prepareReadCommand(uint8_t *regList, size_t len) {
2021-05-24 20:33:59 +02:00
for(size_t idx = 0; idx < len; idx++) {
commandBuffer[idx * 2] = regList[idx];
commandBuffer[idx * 2 + 1] = 0x00;
}
commandBuffer[len * 2] = 0x00;
commandBuffer[len * 2 + 1] = 0x00;
}
ReturnValue_t GyroADIS1650XHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
2021-05-24 20:33:59 +02:00
LocalDataPoolManager &poolManager) {
localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_X, new PoolEntry<double>({0.0}));
localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Y, new PoolEntry<double>({0.0}));
localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Z, new PoolEntry<double>({0.0}));
localDataPoolMap.emplace(ADIS1650X::ACCELERATION_X, new PoolEntry<double>({0.0}));
localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Y, new PoolEntry<double>({0.0}));
localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Z, new PoolEntry<double>({0.0}));
localDataPoolMap.emplace(ADIS1650X::TEMPERATURE, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(ADIS1650X::DIAG_STAT_REGISTER, new PoolEntry<uint16_t>());
localDataPoolMap.emplace(ADIS1650X::FILTER_SETTINGS, new PoolEntry<uint8_t>());
localDataPoolMap.emplace(ADIS1650X::MSC_CTRL_REGISTER, new PoolEntry<uint16_t>());
localDataPoolMap.emplace(ADIS1650X::DEC_RATE_REGISTER, new PoolEntry<uint16_t>());
2021-05-24 20:33:59 +02:00
return HasReturnvaluesIF::RETURN_OK;
}
GyroADIS1650XHandler::BurstModes GyroADIS1650XHandler::getBurstMode() {
2021-05-24 20:57:12 +02:00
configDataset.mscCtrlReg.read();
uint16_t currentCtrlReg = configDataset.mscCtrlReg.value;
configDataset.mscCtrlReg.commit();
if((currentCtrlReg & ADIS1650X::BURST_32_BIT) == ADIS1650X::BURST_32_BIT) {
if((currentCtrlReg & ADIS1650X::BURST_SEL_BIT) == ADIS1650X::BURST_SEL_BIT) {
2021-05-24 20:57:12 +02:00
return BurstModes::BURST_32_BURST_SEL_1;
}
else {
return BurstModes::BURST_32_BURST_SEL_0;
}
}
else {
if((currentCtrlReg & ADIS1650X::BURST_SEL_BIT) == ADIS1650X::BURST_SEL_BIT) {
2021-05-24 20:57:12 +02:00
return BurstModes::BURST_16_BURST_SEL_1;
}
else {
return BurstModes::BURST_16_BURST_SEL_0;
}
}
}
#if OBSW_ADIS1650X_LINUX_COM_IF == 1
2021-05-24 20:33:59 +02:00
ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie *cookie,
2021-05-24 20:33:59 +02:00
const uint8_t *sendData, size_t sendLen, void *args) {
GyroADIS1650XHandler* handler = reinterpret_cast<GyroADIS1650XHandler*>(args);
2021-05-24 20:33:59 +02:00
if(handler == nullptr) {
sif::error << "GyroADIS16507Handler::spiSendCallback: Passed handler pointer is invalid!"
<< std::endl;
return HasReturnvaluesIF::RETURN_FAILED;
}
DeviceCommandId_t currentCommand = handler->getPendingCommand();
switch(currentCommand) {
case(ADIS1650X::READ_SENSOR_DATA): {
2021-05-24 20:33:59 +02:00
return comIf->performRegularSendOperation(cookie, sendData, sendLen);
}
case(ADIS1650X::READ_OUT_CONFIG):
2021-05-25 12:45:51 +02:00
default: {
ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
int retval = 0;
// Prepare transfer
int fileDescriptor = 0;
std::string device = cookie->getSpiDevice();
UnixFileGuard fileHelper(device, &fileDescriptor, O_RDWR, "SpiComIF::sendMessage");
2021-05-25 12:45:51 +02:00
if(fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) {
return SpiComIF::OPENING_FILE_FAILED;
}
spi::SpiModes spiMode = spi::SpiModes::MODE_0;
uint32_t spiSpeed = 0;
cookie->getSpiParameters(spiMode, spiSpeed, nullptr);
comIf->setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
cookie->assignWriteBuffer(sendData);
2021-09-15 17:26:23 +02:00
cookie->setTransferSize(2);
2021-05-25 12:45:51 +02:00
gpioId_t gpioId = cookie->getChipSelectPin();
GpioIF* gpioIF = comIf->getGpioInterface();
MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
uint32_t timeoutMs = 0;
MutexIF* mutex = comIf->getMutex(&timeoutType, &timeoutMs);
if(mutex == nullptr or gpioIF == nullptr) {
#if OBSW_VERBOSE_LEVEL >= 1
sif::warning << "GyroADIS16507Handler::spiSendCallback: "
"Mutex or GPIO interface invalid" << std::endl;
return HasReturnvaluesIF::RETURN_FAILED;
#endif
}
2021-05-25 13:53:03 +02:00
if(gpioId != gpio::NO_GPIO) {
result = mutex->lockMutex(timeoutType, timeoutMs);
if (result != RETURN_OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "SpiComIF::sendMessage: Failed to lock mutex" << std::endl;
#endif
return result;
}
}
2021-05-25 12:45:51 +02:00
size_t idx = 0;
while(idx < sendLen) {
// 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;
}
2021-09-25 16:38:39 +02:00
#if FSFW_HAL_SPI_WIRETAPPING == 1
2021-05-25 12:45:51 +02:00
comIf->performSpiWiretapping(cookie);
#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */
if(gpioId != gpio::NO_GPIO) {
gpioIF->pullHigh(gpioId);
}
idx += 2;
if(idx < sendLen) {
usleep(ADIS1650X::STALL_TIME_MICROSECONDS);
2021-05-25 12:45:51 +02:00
}
2021-05-25 13:53:03 +02:00
spi_ioc_transfer* transferStruct = cookie->getTransferStructHandle();
transferStruct->tx_buf += 2;
transferStruct->rx_buf += 2;
}
if(gpioId != gpio::NO_GPIO) {
mutex->unlockMutex();
2021-05-25 12:45:51 +02:00
}
2021-05-24 20:33:59 +02:00
}
}
return HasReturnvaluesIF::RETURN_OK;
}
#endif /* OBSW_ADIS1650X_LINUX_COM_IF == 1 */