eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 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
parent 608e3eeda2
commit 7b048baa71
2 changed files with 51 additions and 11 deletions
Show Stats 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): {

6
mission/devices/devicedefinitions/GyroADIS16507Definitions.h
View File

@ -58,6 +58,7 @@ enum PrimaryPoolIds: lp_id_t {
};
enum ConfigPoolIds: lp_id_t {
DIAG_STAT_REGISTER,
FILTER_SETTINGS,
MSC_CTRL_REGISTER,
DEC_RATE_REGISTER,
@ -75,7 +76,7 @@ enum FilterSettings: uint8_t {
}
class AdisGyroPrimaryDataset: public StaticLocalDataSet<7 * sizeof(float)> {
class AdisGyroPrimaryDataset: public StaticLocalDataSet<8> {
public:
/** Constructor for data users like controllers */
@ -107,7 +108,7 @@ private:
StaticLocalDataSet(hkOwner, ADIS16507::ADIS_DATASET_ID) {}
};
class AdisGyroConfigDataset: public StaticLocalDataSet<32> {
class AdisGyroConfigDataset: public StaticLocalDataSet<5> {
public:
/** Constructor for data users like controllers */
@ -116,6 +117,7 @@ public:
setAllVariablesReadOnly();
}
lp_var_t<uint16_t> diagStatReg = lp_var_t<uint16_t>(sid.objectId, ADIS16507::DIAG_STAT_REG);
lp_var_t<uint8_t> filterSetting = lp_var_t<uint8_t>(sid.objectId, ADIS16507::FILTER_SETTINGS);
lp_var_t<uint16_t> mscCtrlReg = lp_var_t<uint16_t>(sid.objectId, ADIS16507::MSC_CTRL_REGISTER);
lp_var_t<uint16_t> decRateReg = lp_var_t<uint16_t>(sid.objectId, ADIS16507::DEC_RATE_REGISTER);