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

reply handling for sensor data implemented

Browse Source
This commit is contained in:
Robin Müller
2021-05-24 21:15:23 +02:00
committed by Robin Mueller
parent 853a22c55a
commit 9ca10f5a8e
2 changed files with 51 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

56
mission/devices/GyroADIS16507Handler.cpp
View File

@ -59,11 +59,12 @@ ReturnValue_t GyroADIS16507Handler::buildCommandFromCommand(DeviceCommandId_t de
switch(deviceCommand) {
case(ADIS16507::READ_OUT_CONFIG): {
this->rawPacketLen = ADIS16507::CONFIG_READOUT_SIZE;
uint8_t regList[4];
regList[0] = ADIS16507::FILTER_CTRL_REG;
regList[1] = ADIS16507::MSC_CTRL_REG;
regList[2] = ADIS16507::DEC_RATE_REG;
regList[3] = ADIS16507::PROD_ID_REG;
uint8_t regList[5];
regList[0] = ADIS16507::DIAG_STAT_REG;
regList[1] = ADIS16507::FILTER_CTRL_REG;
regList[2] = ADIS16507::MSC_CTRL_REG;
regList[3] = ADIS16507::DEC_RATE_REG;
regList[4] = ADIS16507::PROD_ID_REG;
prepareReadCommand(regList, sizeof(regList));
this->rawPacket = commandBuffer.data();
break;
@ -99,7 +100,7 @@ ReturnValue_t GyroADIS16507Handler::interpretDeviceReply(DeviceCommandId_t id,
switch(id) {
case(ADIS16507::READ_OUT_CONFIG): {
PoolReadGuard rg(&configDataset);
uint16_t readProdId = packet[6] << 8 | packet[7];
uint16_t readProdId = packet[8] << 8 | packet[9];
if (readProdId != ADIS16507::PROD_ID) {
#if OBSW_VERBOSE_LEVEL >= 1
sif::debug << "GyroADIS16507Handler::interpretDeviceReply: Invalid product ID!"
@ -107,9 +108,11 @@ ReturnValue_t GyroADIS16507Handler::interpretDeviceReply(DeviceCommandId_t id,
#endif
return HasReturnvaluesIF::RETURN_FAILED;
}
configDataset.filterSetting.value = packet[0] << 8 | packet[1];
configDataset.mscCtrlReg.value = packet[2] << 8 | packet[3];
configDataset.decRateReg.value = packet[4] << 8 | packet[5];
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.setValidity(true, true);
if(internalState == InternalState::STARTUP) {
commandExecuted = true;
}
@ -148,6 +151,41 @@ ReturnValue_t GyroADIS16507Handler::handleSensorData(const uint8_t *packet) {
#endif
return HasReturnvaluesIF::RETURN_FAILED;
}
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();
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 temperatureRaw = packet[16] << 8 | packet[17];
primaryDataset.temperature.value = static_cast<float>(temperatureRaw) * 0.1;
// Ignore data counter for now
primaryDataset.setValidity(true, true);
break;
}
case(BurstModes::BURST_32_BURST_SEL_0): {