271 lines
10 KiB
C++
271 lines
10 KiB
C++
#include <fsfw/datapool/PoolReadGuard.h>
|
|
#include "GyroADIS16507Handler.h"
|
|
|
|
#if OBSW_ADIS16507_LINUX_COM_IF == 1
|
|
#include "fsfw_hal/linux/spi/SpiCookie.h"
|
|
#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),
|
|
configDataset(this) {
|
|
#if OBSW_ADIS16507_LINUX_COM_IF == 1
|
|
SpiCookie* cookie = dynamic_cast<SpiCookie*>(comCookie);
|
|
if(cookie != nullptr) {
|
|
cookie->setCallbackMode(&spiSendCallback, this);
|
|
}
|
|
#endif
|
|
}
|
|
|
|
void GyroADIS16507Handler::doStartUp() {
|
|
if(internalState == InternalState::STARTUP) {
|
|
if(commandExecuted) {
|
|
commandExecuted = false;
|
|
internalState = InternalState::IDLE;
|
|
}
|
|
}
|
|
}
|
|
|
|
void GyroADIS16507Handler::doShutDown() {
|
|
}
|
|
|
|
ReturnValue_t GyroADIS16507Handler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
|
|
*id = ADIS16507::READ_SENSOR_DATA;
|
|
return buildCommandFromCommand(*id, nullptr, 0);
|
|
}
|
|
|
|
ReturnValue_t GyroADIS16507Handler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
|
|
switch(internalState) {
|
|
case(InternalState::STARTUP): {
|
|
*id = ADIS16507::READ_OUT_CONFIG;
|
|
buildCommandFromCommand(*id, nullptr, 0);
|
|
break;
|
|
}
|
|
default: {
|
|
/* Might be a configuration error. */
|
|
sif::debug << "GyroADIS16507Handler::buildTransitionDeviceCommand: "
|
|
"Unknown internal state!" << std::endl;
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
}
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t GyroADIS16507Handler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
|
|
const uint8_t *commandData, size_t commandDataLen) {
|
|
switch(deviceCommand) {
|
|
case(ADIS16507::READ_OUT_CONFIG): {
|
|
this->rawPacketLen = ADIS16507::CONFIG_READOUT_SIZE;
|
|
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;
|
|
}
|
|
case(ADIS16507::READ_SENSOR_DATA): {
|
|
std::memcpy(commandBuffer.data(), ADIS16507::BURST_READ_ENABLE.data(),
|
|
ADIS16507::BURST_READ_ENABLE.size());
|
|
std::memset(commandBuffer.data() + 2, 0, 10 * 2);
|
|
this->rawPacketLen = ADIS16507::SENSOR_READOUT_SIZE;
|
|
this->rawPacket = commandBuffer.data();
|
|
break;
|
|
}
|
|
}
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
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,
|
|
DeviceCommandId_t *foundId, size_t *foundLen) {
|
|
/* For SPI, the ID will always be the one of the last sent command. */
|
|
*foundId = this->getPendingCommand();
|
|
*foundLen = this->rawPacketLen;
|
|
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t GyroADIS16507Handler::interpretDeviceReply(DeviceCommandId_t id,
|
|
const uint8_t *packet) {
|
|
switch(id) {
|
|
case(ADIS16507::READ_OUT_CONFIG): {
|
|
PoolReadGuard rg(&configDataset);
|
|
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!"
|
|
<< std::endl;
|
|
#endif
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
}
|
|
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;
|
|
}
|
|
break;
|
|
}
|
|
case(ADIS16507::READ_SENSOR_DATA): {
|
|
return handleSensorData(packet);
|
|
}
|
|
|
|
}
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
|
|
ReturnValue_t GyroADIS16507Handler::handleSensorData(const uint8_t *packet) {
|
|
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;
|
|
}
|
|
|
|
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): {
|
|
break;
|
|
}
|
|
}
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
uint32_t GyroADIS16507Handler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
|
|
return 5000;
|
|
}
|
|
|
|
void GyroADIS16507Handler::prepareReadCommand(uint8_t *regList, size_t len) {
|
|
for(size_t idx = 0; idx < len; idx++) {
|
|
commandBuffer[idx * 2] = regList[idx];
|
|
commandBuffer[idx * 2 + 1] = 0x00;
|
|
}
|
|
commandBuffer[len * 2] = 0x00;
|
|
commandBuffer[len * 2 + 1] = 0x00;
|
|
}
|
|
|
|
ReturnValue_t GyroADIS16507Handler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
|
|
LocalDataPoolManager &poolManager) {
|
|
localDataPoolMap.emplace(ADIS16507::ANG_VELOC_X, new PoolEntry<double>({0.0}));
|
|
localDataPoolMap.emplace(ADIS16507::ANG_VELOC_Y, new PoolEntry<double>({0.0}));
|
|
localDataPoolMap.emplace(ADIS16507::ANG_VELOC_Z, new PoolEntry<double>({0.0}));
|
|
localDataPoolMap.emplace(ADIS16507::ACCELERATION_X, new PoolEntry<double>({0.0}));
|
|
localDataPoolMap.emplace(ADIS16507::ACCELERATION_Y, new PoolEntry<double>({0.0}));
|
|
localDataPoolMap.emplace(ADIS16507::ACCELERATION_Z, new PoolEntry<double>({0.0}));
|
|
|
|
localDataPoolMap.emplace(ADIS16507::FILTER_SETTINGS, new PoolEntry<uint8_t>());
|
|
localDataPoolMap.emplace(ADIS16507::MSC_CTRL_REGISTER, new PoolEntry<uint16_t>());
|
|
localDataPoolMap.emplace(ADIS16507::DEC_RATE_REGISTER, new PoolEntry<uint16_t>());
|
|
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,
|
|
const uint8_t *sendData, size_t sendLen, void *args) {
|
|
GyroADIS16507Handler* handler = reinterpret_cast<GyroADIS16507Handler*>(args);
|
|
if(handler == nullptr) {
|
|
sif::error << "GyroADIS16507Handler::spiSendCallback: Passed handler pointer is invalid!"
|
|
<< std::endl;
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
}
|
|
DeviceCommandId_t currentCommand = handler->getPendingCommand();
|
|
switch(currentCommand) {
|
|
case(ADIS16507::READ_SENSOR_DATA): {
|
|
return comIf->performRegularSendOperation(cookie, sendData, sendLen);
|
|
}
|
|
case(ADIS16507::READ_OUT_CONFIG): {
|
|
usleep(ADIS16507::STALL_TIME_MICROSECONDS);
|
|
}
|
|
}
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
#endif /* OBSW_ADIS16507_LINUX_COM_IF == 1 */
|