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

add handling for correct range setting
Some checks are pending
EIVE/eive-obsw/pipeline/head Build started...
Details
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
Details

Browse Source
This commit is contained in:
Robin Müller
2023-01-18 13:43:28 +01:00
parent a70570f79d
commit 3682a22021
4 changed files with 56 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
mission/devices/GyroADIS1650XHandler.cpp
View File

@ -99,12 +99,13 @@ ReturnValue_t GyroADIS1650XHandler::buildCommandFromCommand(DeviceCommandId_t de
switch (deviceCommand) {
case (ADIS1650X::READ_OUT_CONFIG): {
this->rawPacketLen = ADIS1650X::CONFIG_READOUT_SIZE;
uint8_t regList[5] = {};
uint8_t regList[6] = {};
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;
regList[2] = ADIS1650X::RANG_MDL_REG;
regList[3] = ADIS1650X::MSC_CTRL_REG;
regList[4] = ADIS1650X::DEC_RATE_REG;
regList[5] = ADIS1650X::PROD_ID_REG;
prepareReadCommand(regList, sizeof(regList));
this->rawPacket = commandBuffer.data();
break;
@ -203,6 +204,7 @@ ReturnValue_t GyroADIS1650XHandler::scanForReply(const uint8_t *start, size_t re
ReturnValue_t GyroADIS1650XHandler::interpretDeviceReply(DeviceCommandId_t id,
const uint8_t *packet) {
using namespace ADIS1650X;
switch (id) {
case (ADIS1650X::READ_OUT_CONFIG): {
uint16_t readProdId = packet[10] << 8 | packet[11];
@ -220,8 +222,32 @@ ReturnValue_t GyroADIS1650XHandler::interpretDeviceReply(DeviceCommandId_t id,
PoolReadGuard rg(&configDataset);
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];
uint16_t rangMdlRaw = packet[6] << 8 | packet[7];
ADIS1650X::RangMdlBitfield bitfield =
static_cast<ADIS1650X::RangMdlBitfield>((rangMdlRaw >> 2) & 0b11);
switch (bitfield) {
case (ADIS1650X::RangMdlBitfield::RANGE_125_1BMLZ): {
range = GyroRange::MODEL_1BMLZ;
break;
}
case (ADIS1650X::RangMdlBitfield::RANGE_500_2BMLZ): {
range = GyroRange::MODEL_2BMLZ;
break;
}
case (ADIS1650X::RangMdlBitfield::RANGE_2000_3BMLZ): {
range = GyroRange::MODEL_3BMLZ;
break;
}
case (RangMdlBitfield::RESERVED): {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "ADIS1650X: Unexpected value for RANG_MDL register" << std::endl;
#endif
break;
}
}
configDataset.rangMdl.value = rangMdlRaw;
configDataset.mscCtrlReg.value = packet[8] << 8 | packet[9];
configDataset.decRateReg.value = packet[10] << 8 | packet[11];
configDataset.setValidity(true, true);
if (internalState == InternalState::CONFIG) {
commandExecuted = true;
@ -271,14 +297,11 @@ ReturnValue_t GyroADIS1650XHandler::handleSensorData(const uint8_t *packet) {
{
PoolReadGuard pg(&primaryDataset);
int16_t angVelocXRaw = packet[4] << 8 | packet[5];
primaryDataset.angVelocX.value =
static_cast<float>(angVelocXRaw) / INT16_MAX * ADIS1650X::GYRO_RANGE;
primaryDataset.angVelocX.value = static_cast<float>(angVelocXRaw) / INT16_MAX * range;
int16_t angVelocYRaw = packet[6] << 8 | packet[7];
primaryDataset.angVelocY.value =
static_cast<float>(angVelocYRaw) / INT16_MAX * ADIS1650X::GYRO_RANGE;
primaryDataset.angVelocY.value = static_cast<float>(angVelocYRaw) / INT16_MAX * range;
int16_t angVelocZRaw = packet[8] << 8 | packet[9];
primaryDataset.angVelocZ.value =
static_cast<float>(angVelocZRaw) / INT16_MAX * ADIS1650X::GYRO_RANGE;
primaryDataset.angVelocZ.value = static_cast<float>(angVelocZRaw) / INT16_MAX * range;
float accelScaling = 0;
if (adisType == ADIS1650X::Type::ADIS16507) {