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

Dual Side to Single Side Bugfix #497

Merged
meggert merged 4 commits from bugfix_dual_to_single_side into develop 2023-03-20 14:52:53 +01:00
Conversation 0 Commits 4 Files Changed 5 +84 -83
2 changed files with 84 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 7cf3247cbd - Show all commits

162
linux/devices/AcsBoardPolling.cpp
View File

@ -451,95 +451,99 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {
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, 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 = (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 (mode == acs::SimpleSensorMode::OFF) {
return;
}
if (not cdHasTimedOut) {
return;
}
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 = returnvalue::FAILED;
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 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.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];
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, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
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) {

5
mission/devices/GyrAdis1650XHandler.cpp
View File

@ -65,10 +65,7 @@ ReturnValue_t GyrAdis1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_
}
case (InternalState::SHUTDOWN): {
*id = adis1650x::REQUEST;
acs::Adis1650XRequest *request = reinterpret_cast<acs::Adis1650XRequest *>(cmdBuf.data());
request->mode = acs::SimpleSensorMode::OFF;
request->type = adisType;
return returnvalue::OK;
return preparePeriodicRequest(acs::SimpleSensorMode::OFF);
}
default: {
return NOTHING_TO_SEND;