adis handler continued

This commit is contained in:
Robin Müller 2021-05-24 20:57:12 +02:00 committed by Robin Mueller
parent fe62e38afc
commit 0fa32ef2ea
3 changed files with 72 additions and 1 deletions

View File

@ -6,6 +6,8 @@
#include "fsfw_hal/linux/spi/SpiComIF.h"
#endif
#include <unistd.h>
GyroADIS16507Handler::GyroADIS16507Handler(object_id_t objectId,
object_id_t deviceCommunication, CookieIF * comCookie):
DeviceHandlerBase(objectId, deviceCommunication, comCookie), primaryDataset(this),
@ -79,6 +81,8 @@ ReturnValue_t GyroADIS16507Handler::buildCommandFromCommand(DeviceCommandId_t de
}
void GyroADIS16507Handler::fillCommandAndReplyMap() {
insertInCommandAndReplyMap(ADIS16507::READ_SENSOR_DATA, 1, &primaryDataset);
insertInCommandAndReplyMap(ADIS16507::READ_OUT_CONFIG, 1, &configDataset);
}
ReturnValue_t GyroADIS16507Handler::scanForReply(const uint8_t *start, size_t remainingSize,
@ -112,6 +116,35 @@ ReturnValue_t GyroADIS16507Handler::interpretDeviceReply(DeviceCommandId_t id,
break;
}
case(ADIS16507::READ_SENSOR_DATA): {
BurstModes burstMode = getBurstMode();
switch(burstMode) {
case(BurstModes::BURST_16_BURST_SEL_1):
case(BurstModes::BURST_32_BURST_SEL_1): {
sif::warning << "GyroADIS16507Handler::interpretDeviceReply: Analysis with BURST_SEL1"
" not implemented!" << std::endl;
return HasReturnvaluesIF::RETURN_OK;
}
case(BurstModes::BURST_16_BURST_SEL_0): {
uint16_t checksum = packet[20] << 8 | packet[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 < 22; idx ++) {
calcChecksum += packet[idx];
}
if(checksum != calcChecksum) {
#if OBSW_VERBOSE_LEVEL >= 1
sif::warning << "GyroADIS16507Handler::interpretDeviceReply: "
"Invalid checksum detected!" << std::endl;
#endif
return HasReturnvaluesIF::RETURN_FAILED;
}
}
case(BurstModes::BURST_32_BURST_SEL_0): {
}
}
}
@ -147,6 +180,28 @@ ReturnValue_t GyroADIS16507Handler::initializeLocalDataPool(localpool::DataPool
return HasReturnvaluesIF::RETURN_OK;
}
GyroADIS16507Handler::BurstModes GyroADIS16507Handler::getBurstMode() {
configDataset.mscCtrlReg.read();
uint16_t currentCtrlReg = configDataset.mscCtrlReg.value;
configDataset.mscCtrlReg.commit();
if((currentCtrlReg & ADIS16507::BURST_32_BIT) == ADIS16507::BURST_32_BIT) {
if((currentCtrlReg & ADIS16507::BURST_SEL_BIT) == ADIS16507::BURST_SEL_BIT) {
return BurstModes::BURST_32_BURST_SEL_1;
}
else {
return BurstModes::BURST_32_BURST_SEL_0;
}
}
else {
if((currentCtrlReg & ADIS16507::BURST_SEL_BIT) == ADIS16507::BURST_SEL_BIT) {
return BurstModes::BURST_16_BURST_SEL_1;
}
else {
return BurstModes::BURST_16_BURST_SEL_0;
}
}
}
#if OBSW_ADIS16507_LINUX_COM_IF == 1
ReturnValue_t GyroADIS16507Handler::spiSendCallback(SpiComIF *comIf, SpiCookie *cookie,
@ -163,7 +218,7 @@ ReturnValue_t GyroADIS16507Handler::spiSendCallback(SpiComIF *comIf, SpiCookie *
return comIf->performRegularSendOperation(cookie, sendData, sendLen);
}
case(ADIS16507::READ_OUT_CONFIG): {
usleep(ADIS16507::STALL_TIME_MICROSECONDS);
}
}
return HasReturnvaluesIF::RETURN_OK;

View File

@ -42,11 +42,20 @@ private:
IDLE
};
enum class BurstModes {
BURST_16_BURST_SEL_0,
BURST_16_BURST_SEL_1,
BURST_32_BURST_SEL_0,
BURST_32_BURST_SEL_1
};
InternalState internalState = InternalState::STARTUP;
bool commandExecuted = false;
void prepareReadCommand(uint8_t* regList, size_t len);
BurstModes getBurstMode();
#if OBSW_ADIS16507_LINUX_COM_IF == 1
static ReturnValue_t spiSendCallback(SpiComIF* comIf, SpiCookie *cookie,
const uint8_t *sendData, size_t sendLen, void* args);

View File

@ -12,6 +12,8 @@ static constexpr size_t MAXIMUM_REPLY_SIZE = 64;
static constexpr uint32_t GYRO_RANGE = 125;
static constexpr uint32_t ACCELEROMETER_RANGE = 392;
static constexpr uint32_t STALL_TIME_MICROSECONDS = 16;
static constexpr uint16_t PROD_ID = 16507;
static constexpr std::array<uint8_t, 2> BURST_READ_ENABLE = {0x68, 0x00};
@ -34,6 +36,11 @@ static constexpr DeviceCommandId_t RESET_SENSOR_CONFIGURATION = 30;
static constexpr DeviceCommandId_t SW_RESET = 31;
static constexpr DeviceCommandId_t PRINT_CURRENT_CONFIGURATION = 32;
static constexpr uint16_t BURST_32_BIT = 1 << 9;
static constexpr uint16_t BURST_SEL_BIT = 1 << 8;
static constexpr uint16_t LIN_ACCEL_COMPENSATION_BIT = 1 << 7;
static constexpr uint16_t POINT_PERCUSSION_COMPENSATION_BIT = 1 << 6;
static constexpr size_t CONFIG_READOUT_SIZE = 8;
static constexpr size_t SENSOR_READOUT_SIZE = 22;