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

some bugfixes for adis datasets

Browse Source
This commit is contained in:
Robin Müller
2021-05-25 13:53:03 +02:00
committed by Robin Mueller
parent 1262ccd65d
commit b9bf245d68
9 changed files with 89 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files

109
mission/devices/GyroADIS16507Handler.cpp
View File

@ -15,6 +15,10 @@ GyroADIS16507Handler::GyroADIS16507Handler(object_id_t objectId,
object_id_t deviceCommunication, CookieIF * comCookie):
DeviceHandlerBase(objectId, deviceCommunication, comCookie), primaryDataset(this),
configDataset(this) {
#if ADIS16507_DEBUG == 1
debugDivider = new PeriodicOperationDivider(5);
#endif
#if OBSW_ADIS16507_LINUX_COM_IF == 1
SpiCookie* cookie = dynamic_cast<SpiCookie*>(comCookie);
if(cookie != nullptr) {
@ -30,6 +34,9 @@ void GyroADIS16507Handler::doStartUp() {
internalState = InternalState::IDLE;
}
}
if(internalState == InternalState::IDLE) {
setMode(MODE_NORMAL);
}
}
void GyroADIS16507Handler::doShutDown() {
@ -103,7 +110,7 @@ ReturnValue_t GyroADIS16507Handler::interpretDeviceReply(DeviceCommandId_t id,
switch(id) {
case(ADIS16507::READ_OUT_CONFIG): {
PoolReadGuard rg(&configDataset);
uint16_t readProdId = packet[8] << 8 | packet[9];
uint16_t readProdId = packet[10] << 8 | packet[11];
if (readProdId != ADIS16507::PROD_ID) {
#if OBSW_VERBOSE_LEVEL >= 1
sif::debug << "GyroADIS16507Handler::interpretDeviceReply: Invalid product ID!"
@ -111,10 +118,10 @@ ReturnValue_t GyroADIS16507Handler::interpretDeviceReply(DeviceCommandId_t id,
#endif
return HasReturnvaluesIF::RETURN_FAILED;
}
configDataset.diagStatReg.value = packet[0] << 8 | packet[1];
configDataset.filterSetting.value = packet[2] << 8 | packet[3];
configDataset.mscCtrlReg.value = packet[4] << 8 | packet[5];
configDataset.decRateReg.value = packet[6] << 8 | packet[7];
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);
if(internalState == InternalState::STARTUP) {
commandExecuted = true;
@ -162,33 +169,44 @@ ReturnValue_t GyroADIS16507Handler::handleSensorData(const uint8_t *packet) {
}
configDataset.diagStatReg.commit();
PoolReadGuard pg(&primaryDataset);
int16_t angVelocXRaw = packet[4] << 8 | packet[5];
primaryDataset.angVelocX.value = static_cast<float>(angVelocXRaw) / INT16_MAX *
ADIS16507::GYRO_RANGE;
int16_t angVelocYRaw = packet[6] << 8 | packet[7];
primaryDataset.angVelocY.value = static_cast<float>(angVelocYRaw) / INT16_MAX *
ADIS16507::GYRO_RANGE;
int16_t angVelocZRaw = packet[8] << 8 | packet[9];
primaryDataset.angVelocZ.value = static_cast<float>(angVelocZRaw) / INT16_MAX *
ADIS16507::GYRO_RANGE;
{
PoolReadGuard pg(&primaryDataset);
int16_t angVelocXRaw = packet[4] << 8 | packet[5];
primaryDataset.angVelocX.value = static_cast<float>(angVelocXRaw) / INT16_MAX *
ADIS16507::GYRO_RANGE;
int16_t angVelocYRaw = packet[6] << 8 | packet[7];
primaryDataset.angVelocY.value = static_cast<float>(angVelocYRaw) / INT16_MAX *
ADIS16507::GYRO_RANGE;
int16_t angVelocZRaw = packet[8] << 8 | packet[9];
primaryDataset.angVelocZ.value = static_cast<float>(angVelocZRaw) / INT16_MAX *
ADIS16507::GYRO_RANGE;
int16_t accelXRaw = packet[10] << 8 | packet[11];
primaryDataset.accelX.value = static_cast<float>(accelXRaw) / INT16_MAX *
ADIS16507::GYRO_RANGE;
int16_t accelYRaw = packet[12] << 8 | packet[13];
primaryDataset.accelY.value = static_cast<float>(accelYRaw) / INT16_MAX *
ADIS16507::GYRO_RANGE;
int16_t accelZRaw = packet[14] << 8 | packet[15];
primaryDataset.accelZ.value = static_cast<float>(accelZRaw) / INT16_MAX *
ADIS16507::GYRO_RANGE;
int16_t accelXRaw = packet[10] << 8 | packet[11];
primaryDataset.accelX.value = static_cast<float>(accelXRaw) / INT16_MAX *
ADIS16507::GYRO_RANGE;
int16_t accelYRaw = packet[12] << 8 | packet[13];
primaryDataset.accelY.value = static_cast<float>(accelYRaw) / INT16_MAX *
ADIS16507::GYRO_RANGE;
int16_t accelZRaw = packet[14] << 8 | packet[15];
primaryDataset.accelZ.value = static_cast<float>(accelZRaw) / INT16_MAX *
ADIS16507::GYRO_RANGE;
int16_t temperatureRaw = packet[16] << 8 | packet[17];
primaryDataset.temperature.value = static_cast<float>(temperatureRaw) * 0.1;
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);
}
// Ignore data counter for now
#if ADIS16507_DEBUG == 1
if(debugDivider->checkAndIncrement()) {
sif::info << "GyroADIS16507Handler: Angular velocities in degrees per second:" <<
std::endl;
sif::info << "X: " << primaryDataset.angVelocX.value << " \xC2\xB0" << std::endl;
sif::info << "Y: " << primaryDataset.angVelocY.value << " \xC2\xB0" << std::endl;
sif::info << "Z: " << primaryDataset.angVelocZ.value << " \xC2\xB0" << std::endl;
}
#endif
primaryDataset.setValidity(true, true);
break;
}
case(BurstModes::BURST_32_BURST_SEL_0): {
@ -219,7 +237,9 @@ ReturnValue_t GyroADIS16507Handler::initializeLocalDataPool(localpool::DataPool
localDataPoolMap.emplace(ADIS16507::ACCELERATION_X, new PoolEntry<double>({0.0}));
localDataPoolMap.emplace(ADIS16507::ACCELERATION_Y, new PoolEntry<double>({0.0}));
localDataPoolMap.emplace(ADIS16507::ACCELERATION_Z, new PoolEntry<double>({0.0}));
localDataPoolMap.emplace(ADIS16507::TEMPERATURE, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(ADIS16507::DIAG_STAT_REGISTER, new PoolEntry<uint16_t>());
localDataPoolMap.emplace(ADIS16507::FILTER_SETTINGS, new PoolEntry<uint8_t>());
localDataPoolMap.emplace(ADIS16507::MSC_CTRL_REGISTER, new PoolEntry<uint16_t>());
localDataPoolMap.emplace(ADIS16507::DEC_RATE_REGISTER, new PoolEntry<uint16_t>());
@ -294,17 +314,20 @@ ReturnValue_t GyroADIS16507Handler::spiSendCallback(SpiComIF *comIf, SpiCookie *
#endif
}
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;
}
}
size_t idx = 0;
while(idx < sendLen) {
// Pull SPI CS low. For now, no support for active high given
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;
}
gpioIF->pullLow(gpioId);
}
@ -321,21 +344,19 @@ ReturnValue_t GyroADIS16507Handler::spiSendCallback(SpiComIF *comIf, SpiCookie *
if(gpioId != gpio::NO_GPIO) {
gpioIF->pullHigh(gpioId);
result = mutex->unlockMutex();
if (result != RETURN_OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "SpiComIF::getSendSuccess: Failed to unlock mutex" << std::endl;
#endif
return result;
}
}
idx += 2;
if(idx < sendLen) {
usleep(ADIS16507::STALL_TIME_MICROSECONDS);
}
sendData += 2;
cookie->getTransferStructHandle()->rx_buf += 2;
spi_ioc_transfer* transferStruct = cookie->getTransferStructHandle();
transferStruct->tx_buf += 2;
transferStruct->rx_buf += 2;
}
if(gpioId != gpio::NO_GPIO) {
mutex->unlockMutex();
}
}
}