adis handler continued
This commit is contained in:
parent
fe62e38afc
commit
0fa32ef2ea
@ -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;
|
||||
|
@ -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);
|
||||
|
@ -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;
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user