well this doesnt work
All checks were successful
EIVE/eive-obsw/pipeline/pr-v3.0.0-dev This commit looks good

This commit is contained in:
Marius Eggert 2023-06-06 16:02:11 +02:00
parent 3be495fc30
commit 6a2d5b8161
7 changed files with 118 additions and 12 deletions

View File

@ -113,6 +113,7 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
if (req->mode != adis.mode) { if (req->mode != adis.mode) {
if (req->mode == acs::SimpleSensorMode::NORMAL) { if (req->mode == acs::SimpleSensorMode::NORMAL) {
adis.type = req->type; adis.type = req->type;
adis.decRate = req->cfg.decRateReg;
// The initial countdown is handled by the device handler now. // The initial countdown is handled by the device handler now.
// adis.countdown.setTimeout(adis1650x::START_UP_TIME); // adis.countdown.setTimeout(adis1650x::START_UP_TIME);
if (adis.type == adis1650x::Type::ADIS16507) { if (adis.type == adis1650x::Type::ADIS16507) {
@ -376,6 +377,81 @@ void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) {
} }
} }
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 < 2; idx++) {
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 < 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 AcsBoardPolling::readAdisCfg(SpiCookie& cookie, size_t transferLen) {
ReturnValue_t result = returnvalue::OK; ReturnValue_t result = returnvalue::OK;
int retval = 0; int retval = 0;
@ -455,15 +531,20 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {
ReturnValue_t result; ReturnValue_t result;
acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF; acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
bool mustPerformStartup = false; bool mustPerformStartup = false;
uint16_t decRate = 0;
{ {
MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
mode = gyro.mode; mode = gyro.mode;
decRate = gyro.decRate;
mustPerformStartup = gyro.performStartup; mustPerformStartup = gyro.performStartup;
} }
if (mode == acs::SimpleSensorMode::OFF) { if (mode == acs::SimpleSensorMode::OFF) {
return; return;
} }
if (mustPerformStartup) { if (mustPerformStartup) {
adis1650x::prepareWriteRegCommand(adis1650x::DEC_RATE_REG, decRate, cmdBuf.data(),
cmdBuf.size());
writeAdisReg(*gyro.cookie);
uint8_t regList[6]; uint8_t regList[6];
// Read configuration // Read configuration
regList[0] = adis1650x::DIAG_STAT_REG; regList[0] = adis1650x::DIAG_STAT_REG;
@ -491,13 +572,19 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {
gyro.replyResult = returnvalue::FAILED; gyro.replyResult = returnvalue::FAILED;
return; 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); MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
gyro.ownReply.cfgWasSet = true; gyro.ownReply.cfgWasSet = true;
gyro.ownReply.cfg.diagStat = (rawReply[2] << 8) | rawReply[3]; gyro.ownReply.cfg.diagStat = (rawReply[2] << 8) | rawReply[3];
gyro.ownReply.cfg.filterSetting = (rawReply[4] << 8) | rawReply[5]; gyro.ownReply.cfg.filterSetting = (rawReply[4] << 8) | rawReply[5];
gyro.ownReply.cfg.rangMdl = (rawReply[6] << 8) | rawReply[7]; gyro.ownReply.cfg.rangMdl = (rawReply[6] << 8) | rawReply[7];
gyro.ownReply.cfg.mscCtrlReg = (rawReply[8] << 8) | rawReply[9]; gyro.ownReply.cfg.mscCtrlReg = (rawReply[8] << 8) | rawReply[9];
gyro.ownReply.cfg.decRateReg = (rawReply[10] << 8) | rawReply[11]; gyro.ownReply.cfg.decRateReg = decRateReadBack;
gyro.ownReply.cfg.prodId = prodId; gyro.ownReply.cfg.prodId = prodId;
gyro.ownReply.data.sensitivity = adis1650x::rangMdlToSensitivity(gyro.ownReply.cfg.rangMdl); gyro.ownReply.data.sensitivity = adis1650x::rangMdlToSensitivity(gyro.ownReply.cfg.rangMdl);
gyro.performStartup = false; gyro.performStartup = false;

View File

@ -37,6 +37,7 @@ class AcsBoardPolling : public SystemObject,
struct GyroAdis : public DevBase { struct GyroAdis : public DevBase {
adis1650x::Type type; adis1650x::Type type;
uint16_t decRate;
Countdown countdown; Countdown countdown;
acs::Adis1650XReply ownReply; acs::Adis1650XReply ownReply;
acs::Adis1650XReply readerReply; acs::Adis1650XReply readerReply;
@ -84,6 +85,8 @@ class AcsBoardPolling : public SystemObject,
void gyroAdisHandler(GyroAdis& gyro); void gyroAdisHandler(GyroAdis& gyro);
void mgmLis3Handler(MgmLis3& mgm); void mgmLis3Handler(MgmLis3& mgm);
void mgmRm3100Handler(MgmRm3100& mgm); void mgmRm3100Handler(MgmRm3100& mgm);
// This fumction configures the register as specified on p.21 of the datasheet.
ReturnValue_t writeAdisReg(SpiCookie& cookie);
// Special readout: 16us stall time between small 2 byte transfers. // Special readout: 16us stall time between small 2 byte transfers.
ReturnValue_t readAdisCfg(SpiCookie& spiCookie, size_t transferLen); ReturnValue_t readAdisCfg(SpiCookie& spiCookie, size_t transferLen);
}; };

View File

@ -26,12 +26,12 @@ void GyrAdis1650XHandler::doStartUp() {
breakCountdown.setTimeout(adis1650x::START_UP_TIME); breakCountdown.setTimeout(adis1650x::START_UP_TIME);
commandExecuted = true; commandExecuted = true;
} }
if (breakCountdown.hasTimedOut()) { }
if (internalState == InternalState::STARTUP_DONE) {
updatePeriodicReply(true, adis1650x::REPLY); updatePeriodicReply(true, adis1650x::REPLY);
setMode(MODE_ON); setMode(MODE_ON);
internalState = InternalState::NONE; internalState = InternalState::NONE;
} }
}
} }
void GyrAdis1650XHandler::doShutDown() { void GyrAdis1650XHandler::doShutDown() {
@ -61,6 +61,8 @@ ReturnValue_t GyrAdis1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_
return NOTHING_TO_SEND; return NOTHING_TO_SEND;
} }
*id = adis1650x::REQUEST; *id = adis1650x::REQUEST;
adisRequest.cfg.decRateReg = adis1650x::DEC_RATE;
internalState = InternalState::STARTUP_DONE;
return preparePeriodicRequest(acs::SimpleSensorMode::NORMAL); return preparePeriodicRequest(acs::SimpleSensorMode::NORMAL);
} }
case (InternalState::SHUTDOWN): { case (InternalState::SHUTDOWN): {

View File

@ -48,7 +48,7 @@ class GyrAdis1650XHandler : public DeviceHandlerBase {
bool warningSwitch = true; bool warningSwitch = true;
enum class InternalState { NONE, STARTUP, SHUTDOWN }; enum class InternalState { NONE, STARTUP, STARTUP_DONE, SHUTDOWN };
InternalState internalState = InternalState::STARTUP; InternalState internalState = InternalState::STARTUP;
bool commandExecuted = false; bool commandExecuted = false;

View File

@ -8,11 +8,6 @@
namespace acs { namespace acs {
struct Adis1650XRequest {
SimpleSensorMode mode;
adis1650x::Type type;
};
struct Adis1650XConfig { struct Adis1650XConfig {
uint16_t diagStat; uint16_t diagStat;
uint16_t filterSetting; uint16_t filterSetting;
@ -22,6 +17,12 @@ struct Adis1650XConfig {
uint16_t prodId; uint16_t prodId;
}; };
struct Adis1650XRequest {
SimpleSensorMode mode;
adis1650x::Type type;
Adis1650XConfig cfg;
};
struct Adis1650XData { struct Adis1650XData {
double sensitivity = 0.0; double sensitivity = 0.0;
// Angular velocities in all axes (X, Y and Z) // Angular velocities in all axes (X, Y and Z)

View File

@ -52,3 +52,14 @@ double adis1650x::rangMdlToSensitivity(uint16_t rangMdl) {
} }
} }
} }
void adis1650x::prepareWriteRegCommand(uint8_t regStart, uint16_t val, uint8_t* outBuf,
size_t maxLen) {
if (maxLen < 4) {
return;
}
outBuf[0] = regStart | adis1650x::WRITE_MASK;
outBuf[1] = val & 0Xff;
outBuf[2] = (regStart + 1) | adis1650x::WRITE_MASK;
outBuf[3] = (val >> 8) & 0xff;
}

View File

@ -16,6 +16,8 @@ enum class BurstModes {
}; };
size_t prepareReadCommand(const uint8_t* regList, size_t len, uint8_t* outBuf, size_t maxLen); size_t prepareReadCommand(const uint8_t* regList, size_t len, uint8_t* outBuf, size_t maxLen);
void prepareWriteRegCommand(uint8_t regStart, uint16_t val, uint8_t* outBuf, size_t maxLen);
BurstModes burstModeFromMscCtrl(uint16_t mscCtrl); BurstModes burstModeFromMscCtrl(uint16_t mscCtrl);
double rangMdlToSensitivity(uint16_t rangMdl); double rangMdlToSensitivity(uint16_t rangMdl);