516 lines
21 KiB
C++
516 lines
21 KiB
C++
|
#include "mission/devices/GyrAdis1650XHandler.h"
|
||
|
|
|
||
|
|
#include "fsfw/action/HasActionsIF.h"
|
||
|
|
#include "fsfw/datapool/PoolReadGuard.h"
|
||
|
|
#include "mission/devices/devicedefinitions/acsPolling.h"
|
||
|
|
|
||
|
|
GyrAdis1650XHandler::GyrAdis1650XHandler(object_id_t objectId, object_id_t deviceCommunication,
|
||
|
|
CookieIF *comCookie, adis1650x::Type type)
|
||
|
|
: DeviceHandlerBase(objectId, deviceCommunication, comCookie),
|
||
|
|
adisType(type),
|
||
|
|
primaryDataset(this),
|
||
|
|
configDataset(this),
|
||
|
|
breakCountdown() {
|
||
|
|
adisRequest.type = adisType;
|
||
|
|
}
|
||
|
|
|
||
|
|
void GyrAdis1650XHandler::doStartUp() {
|
||
|
|
// Initial 310 ms start up time after power-up
|
||
|
|
if (internalState == InternalState::STARTUP) {
|
||
|
|
if (not commandExecuted) {
|
||
|
|
warningSwitch = true;
|
||
|
|
breakCountdown.setTimeout(adis1650x::START_UP_TIME);
|
||
|
|
commandExecuted = true;
|
||
|
|
}
|
||
|
|
if (breakCountdown.hasTimedOut()) {
|
||
|
|
updatePeriodicReply(true, adis1650x::REPLY);
|
||
|
|
if (goToNormalMode) {
|
||
|
|
setMode(MODE_NORMAL);
|
||
|
|
} else {
|
||
|
|
setMode(MODE_ON);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// // Read all configuration registers first
|
||
|
|
// if (internalState == InternalState::CONFIG) {
|
||
|
|
// if (commandExecuted) {
|
||
|
|
// commandExecuted = false;
|
||
|
|
// internalState = InternalState::IDLE;
|
||
|
|
// }
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// if (internalState == InternalState::IDLE) {
|
||
|
|
// }
|
||
|
|
}
|
||
|
|
|
||
|
|
void GyrAdis1650XHandler::doShutDown() {
|
||
|
|
if (internalState != InternalState::SHUTDOWN) {
|
||
|
|
commandExecuted = false;
|
||
|
|
primaryDataset.setValidity(false, true);
|
||
|
|
internalState = InternalState::SHUTDOWN;
|
||
|
|
}
|
||
|
|
if (commandExecuted) {
|
||
|
|
updatePeriodicReply(false, adis1650x::REPLY);
|
||
|
|
setMode(_MODE_POWER_DOWN);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t GyrAdis1650XHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
|
||
|
|
*id = adis1650x::REQUEST;
|
||
|
|
return preparePeriodicRequest(acs::SimpleSensorMode::NORMAL);
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t GyrAdis1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
|
||
|
|
switch (internalState) {
|
||
|
|
case (InternalState::STARTUP): {
|
||
|
|
*id = adis1650x::REQUEST;
|
||
|
|
return preparePeriodicRequest(acs::SimpleSensorMode::NORMAL);
|
||
|
|
}
|
||
|
|
case (InternalState::SHUTDOWN): {
|
||
|
|
*id = adis1650x::REQUEST;
|
||
|
|
acs::Adis1650XRequest *request = reinterpret_cast<acs::Adis1650XRequest *>(cmdBuf.data());
|
||
|
|
request->mode = acs::SimpleSensorMode::OFF;
|
||
|
|
request->type = adisType;
|
||
|
|
return returnvalue::OK;
|
||
|
|
}
|
||
|
|
default: {
|
||
|
|
// Might be a configuration error
|
||
|
|
sif::debug << "GyroADIS16507Handler::buildTransitionDeviceCommand: "
|
||
|
|
"Unknown internal state!"
|
||
|
|
<< std::endl;
|
||
|
|
return returnvalue::OK;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return returnvalue::OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t GyrAdis1650XHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
|
||
|
|
const uint8_t *commandData,
|
||
|
|
size_t commandDataLen) {
|
||
|
|
// switch (deviceCommand) {
|
||
|
|
// case (adis1650x::READ_OUT_CONFIG): {
|
||
|
|
// this->rawPacketLen = adis1650x::CONFIG_READOUT_SIZE;
|
||
|
|
// uint8_t regList[6] = {};
|
||
|
|
// regList[0] = adis1650x::DIAG_STAT_REG;
|
||
|
|
// regList[1] = adis1650x::FILTER_CTRL_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;
|
||
|
|
// adis1650x::prepareReadCommand(regList, sizeof(regList), commandBuffer.data(),
|
||
|
|
// commandBuffer.size());
|
||
|
|
// this->rawPacket = commandBuffer.data();
|
||
|
|
// break;
|
||
|
|
// }
|
||
|
|
// case (adis1650x::READ_SENSOR_DATA): {
|
||
|
|
// if (breakCountdown.isBusy()) {
|
||
|
|
// // A glob command is pending and sensor data can't be read currently
|
||
|
|
// return NO_REPLY_EXPECTED;
|
||
|
|
// }
|
||
|
|
// std::memcpy(commandBuffer.data(), adis1650x::BURST_READ_ENABLE.data(),
|
||
|
|
// adis1650x::BURST_READ_ENABLE.size());
|
||
|
|
// std::memset(commandBuffer.data() + 2, 0, 10 * 2);
|
||
|
|
// this->rawPacketLen = adis1650x::SENSOR_READOUT_SIZE;
|
||
|
|
// this->rawPacket = commandBuffer.data();
|
||
|
|
// break;
|
||
|
|
// }
|
||
|
|
// TODO: Convert to special request
|
||
|
|
|
||
|
|
// case (adis1650x::SELF_TEST_SENSORS): {
|
||
|
|
// if (breakCountdown.isBusy()) {
|
||
|
|
// // Another glob command is pending
|
||
|
|
// return HasActionsIF::IS_BUSY;
|
||
|
|
// }
|
||
|
|
// prepareWriteCommand(adis1650x::GLOB_CMD, adis1650x::GlobCmds::SENSOR_SELF_TEST, 0x00);
|
||
|
|
// breakCountdown.setTimeout(adis1650x::SELF_TEST_BREAK);
|
||
|
|
// break;
|
||
|
|
// }
|
||
|
|
// case (adis1650x::SELF_TEST_MEMORY): {
|
||
|
|
// if (breakCountdown.isBusy()) {
|
||
|
|
// // Another glob command is pending
|
||
|
|
// return HasActionsIF::IS_BUSY;
|
||
|
|
// }
|
||
|
|
// prepareWriteCommand(adis1650x::GLOB_CMD, adis1650x::GlobCmds::FLASH_MEMORY_TEST, 0x00);
|
||
|
|
// breakCountdown.setTimeout(adis1650x::FLASH_MEMORY_TEST_BREAK);
|
||
|
|
// break;
|
||
|
|
// }
|
||
|
|
// case (adis1650x::UPDATE_NV_CONFIGURATION): {
|
||
|
|
// if (breakCountdown.isBusy()) {
|
||
|
|
// // Another glob command is pending
|
||
|
|
// return HasActionsIF::IS_BUSY;
|
||
|
|
// }
|
||
|
|
// prepareWriteCommand(adis1650x::GLOB_CMD, adis1650x::GlobCmds::FLASH_MEMORY_UPDATE,
|
||
|
|
// 0x00); breakCountdown.setTimeout(adis1650x::FLASH_MEMORY_UPDATE_BREAK); break;
|
||
|
|
// }
|
||
|
|
// case (adis1650x::RESET_SENSOR_CONFIGURATION): {
|
||
|
|
// if (breakCountdown.isBusy()) {
|
||
|
|
// // Another glob command is pending
|
||
|
|
// return HasActionsIF::IS_BUSY;
|
||
|
|
// }
|
||
|
|
// prepareWriteCommand(adis1650x::GLOB_CMD, adis1650x::GlobCmds::FACTORY_CALIBRATION,
|
||
|
|
// 0x00); breakCountdown.setTimeout(adis1650x::FACTORY_CALIBRATION_BREAK); break;
|
||
|
|
// }
|
||
|
|
// case (adis1650x::SW_RESET): {
|
||
|
|
// if (breakCountdown.isBusy()) {
|
||
|
|
// // Another glob command is pending
|
||
|
|
// return HasActionsIF::IS_BUSY;
|
||
|
|
// }
|
||
|
|
// prepareWriteCommand(adis1650x::GLOB_CMD, adis1650x::GlobCmds::SOFTWARE_RESET, 0x00);
|
||
|
|
// breakCountdown.setTimeout(adis1650x::SW_RESET_BREAK);
|
||
|
|
// break;
|
||
|
|
// }
|
||
|
|
// case (adis1650x::PRINT_CURRENT_CONFIGURATION): {
|
||
|
|
//#if OBSW_VERBOSE_LEVEL >= 1
|
||
|
|
// PoolReadGuard pg(&configDataset);
|
||
|
|
// sif::info << "ADIS16507 Sensor configuration: DIAG_STAT: 0x" << std::hex << std::setw(4)
|
||
|
|
// << std::setfill('0') << "0x" << configDataset.diagStatReg.value << std::endl;
|
||
|
|
// sif::info << "MSC_CTRL: " << std::hex << std::setw(4) << "0x"
|
||
|
|
// << configDataset.mscCtrlReg.value << " | FILT_CTRL: 0x"
|
||
|
|
// << configDataset.filterSetting.value << " | DEC_RATE: 0x"
|
||
|
|
// << configDataset.decRateReg.value << std::setfill(' ') << std::endl;
|
||
|
|
//#endif /* OBSW_VERBOSE_LEVEL >= 1 */
|
||
|
|
//
|
||
|
|
//}
|
||
|
|
//}
|
||
|
|
// return returnvalue::OK;
|
||
|
|
return NOTHING_TO_SEND;
|
||
|
|
}
|
||
|
|
|
||
|
|
void GyrAdis1650XHandler::fillCommandAndReplyMap() {
|
||
|
|
insertInCommandMap(adis1650x::REQUEST);
|
||
|
|
insertInReplyMap(adis1650x::REPLY, 5, nullptr, 0, true);
|
||
|
|
// insertInCommandAndReplyMap(adis1650x::READ_SENSOR_DATA, 1, &primaryDataset);
|
||
|
|
// insertInCommandAndReplyMap(adis1650x::READ_OUT_CONFIG, 1, &configDataset);
|
||
|
|
// insertInCommandAndReplyMap(adis1650x::SELF_TEST_SENSORS, 1);
|
||
|
|
// insertInCommandAndReplyMap(adis1650x::SELF_TEST_MEMORY, 1);
|
||
|
|
// insertInCommandAndReplyMap(adis1650x::UPDATE_NV_CONFIGURATION, 1);
|
||
|
|
// insertInCommandAndReplyMap(adis1650x::RESET_SENSOR_CONFIGURATION, 1);
|
||
|
|
// insertInCommandAndReplyMap(adis1650x::SW_RESET, 1);
|
||
|
|
// insertInCommandAndReplyMap(adis1650x::PRINT_CURRENT_CONFIGURATION, 1);
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t GyrAdis1650XHandler::scanForReply(const uint8_t *start, size_t remainingSize,
|
||
|
|
DeviceCommandId_t *foundId, size_t *foundLen) {
|
||
|
|
if (breakCountdown.isBusy() or getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON) {
|
||
|
|
return IGNORE_FULL_PACKET;
|
||
|
|
}
|
||
|
|
if (remainingSize != sizeof(acs::Adis1650XReply)) {
|
||
|
|
*foundLen = remainingSize;
|
||
|
|
return returnvalue::FAILED;
|
||
|
|
}
|
||
|
|
*foundId = adis1650x::REPLY;
|
||
|
|
*foundLen = remainingSize;
|
||
|
|
|
||
|
|
return returnvalue::OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t GyrAdis1650XHandler::interpretDeviceReply(DeviceCommandId_t id,
|
||
|
|
const uint8_t *packet) {
|
||
|
|
using namespace adis1650x;
|
||
|
|
const acs::Adis1650XReply *reply = reinterpret_cast<const acs::Adis1650XReply *>(packet);
|
||
|
|
if (internalState == InternalState::STARTUP and reply->cfgWasSet) {
|
||
|
|
switch (adisType) {
|
||
|
|
case (adis1650x::Type::ADIS16507): {
|
||
|
|
if (reply->cfg.prodId != adis1650x::PROD_ID_16507) {
|
||
|
|
sif::error << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID "
|
||
|
|
<< reply->cfg.prodId << "for ADIS type 16507" << std::endl;
|
||
|
|
return returnvalue::FAILED;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case (adis1650x::Type::ADIS16505): {
|
||
|
|
if (reply->cfg.prodId != adis1650x::PROD_ID_16505) {
|
||
|
|
sif::error << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID "
|
||
|
|
<< reply->cfg.prodId << "for ADIS type 16505" << std::endl;
|
||
|
|
return returnvalue::FAILED;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
PoolReadGuard rg(&configDataset);
|
||
|
|
configDataset.setValidity(true, true);
|
||
|
|
configDataset.mscCtrlReg = reply->cfg.mscCtrlReg;
|
||
|
|
configDataset.rangMdl = reply->cfg.rangMdl;
|
||
|
|
configDataset.diagStatReg = reply->cfg.diagStat;
|
||
|
|
configDataset.filterSetting = reply->cfg.filterSetting;
|
||
|
|
configDataset.decRateReg = reply->cfg.decRateReg;
|
||
|
|
commandExecuted = true;
|
||
|
|
}
|
||
|
|
if (reply->dataWasSet) {
|
||
|
|
{
|
||
|
|
PoolReadGuard rg(&configDataset);
|
||
|
|
configDataset.diagStatReg = reply->cfg.diagStat;
|
||
|
|
}
|
||
|
|
auto sensitivity = reply->data.sensitivity;
|
||
|
|
auto accelSensitivity = reply->data.accelScaling;
|
||
|
|
PoolReadGuard pg(&primaryDataset);
|
||
|
|
primaryDataset.setValidity(true, true);
|
||
|
|
primaryDataset.angVelocX = static_cast<double>(reply->data.angVelocities[0]) * sensitivity;
|
||
|
|
primaryDataset.angVelocY = static_cast<double>(reply->data.angVelocities[1]) * sensitivity;
|
||
|
|
primaryDataset.angVelocZ = static_cast<double>(reply->data.angVelocities[2]) * sensitivity;
|
||
|
|
// TODO: Check whether we need to divide by INT16_MAX
|
||
|
|
primaryDataset.accelX = static_cast<double>(reply->data.accelerations[0]) * accelSensitivity;
|
||
|
|
primaryDataset.accelY = static_cast<double>(reply->data.accelerations[1]) * accelSensitivity;
|
||
|
|
primaryDataset.accelZ = static_cast<double>(reply->data.accelerations[2]) * accelSensitivity;
|
||
|
|
primaryDataset.temperature.value = static_cast<float>(reply->data.temperatureRaw) * 0.1;
|
||
|
|
}
|
||
|
|
return returnvalue::OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
LocalPoolDataSetBase *GyrAdis1650XHandler::getDataSetHandle(sid_t sid) {
|
||
|
|
if (sid.ownerSetId == adis1650x::ADIS_DATASET_ID) {
|
||
|
|
return &primaryDataset;
|
||
|
|
} else if (sid.ownerSetId == adis1650x::ADIS_CFG_DATASET_ID) {
|
||
|
|
return &configDataset;
|
||
|
|
}
|
||
|
|
return nullptr;
|
||
|
|
}
|
||
|
|
|
||
|
|
// ReturnValue_t GyroADIS1650XHandler::handleSensorData(const uint8_t *packet) {
|
||
|
|
// using namespace adis1650x;
|
||
|
|
// BurstModes burstMode = getBurstMode();
|
||
|
|
// switch (burstMode) {
|
||
|
|
// case (BurstModes::BURST_16_BURST_SEL_1):
|
||
|
|
// case (BurstModes::BURST_32_BURST_SEL_1): {
|
||
|
|
// sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: Analysis with BURST_SEL1"
|
||
|
|
// " not implemented!"
|
||
|
|
// << std::endl;
|
||
|
|
// return returnvalue::OK;
|
||
|
|
// }
|
||
|
|
// case (adis1650x::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 < 20; idx++) {
|
||
|
|
// calcChecksum += packet[idx];
|
||
|
|
// }
|
||
|
|
// if (checksum != calcChecksum) {
|
||
|
|
//#if OBSW_VERBOSE_LEVEL >= 1
|
||
|
|
// sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: "
|
||
|
|
// "Invalid checksum detected!"
|
||
|
|
// << std::endl;
|
||
|
|
//#endif
|
||
|
|
// return returnvalue::FAILED;
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// ReturnValue_t result = configDataset.diagStatReg.read();
|
||
|
|
// if (result == returnvalue::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) * sensitivity;
|
||
|
|
// int16_t angVelocYRaw = packet[6] << 8 | packet[7];
|
||
|
|
// primaryDataset.angVelocY.value = static_cast<float>(angVelocYRaw) * sensitivity;
|
||
|
|
// int16_t angVelocZRaw = packet[8] << 8 | packet[9];
|
||
|
|
// primaryDataset.angVelocZ.value = static_cast<float>(angVelocZRaw) * sensitivity;
|
||
|
|
//
|
||
|
|
// float accelScaling = 0;
|
||
|
|
// if (adisType == adis1650x::Type::ADIS16507) {
|
||
|
|
// accelScaling = adis1650x::ACCELEROMETER_RANGE_16507;
|
||
|
|
// } else if (adisType == adis1650x::Type::ADIS16505) {
|
||
|
|
// accelScaling = adis1650x::ACCELEROMETER_RANGE_16505;
|
||
|
|
// } else {
|
||
|
|
// sif::warning << "GyroADIS1650XHandler::handleSensorData: "
|
||
|
|
// "Unknown ADIS type"
|
||
|
|
// << std::endl;
|
||
|
|
// }
|
||
|
|
// int16_t accelXRaw = packet[10] << 8 | packet[11];
|
||
|
|
// primaryDataset.accelX.value = static_cast<float>(accelXRaw) / INT16_MAX * accelScaling;
|
||
|
|
// int16_t accelYRaw = packet[12] << 8 | packet[13];
|
||
|
|
// primaryDataset.accelY.value = static_cast<float>(accelYRaw) / INT16_MAX * accelScaling;
|
||
|
|
// int16_t accelZRaw = packet[14] << 8 | packet[15];
|
||
|
|
// primaryDataset.accelZ.value = static_cast<float>(accelZRaw) / INT16_MAX * accelScaling;
|
||
|
|
//
|
||
|
|
// 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);
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// if (periodicPrintout) {
|
||
|
|
// if (debugDivider.checkAndIncrement()) {
|
||
|
|
// sif::info << "GyroADIS1650XHandler: Angular velocities in deg / s" << std::endl;
|
||
|
|
// sif::info << "X: " << primaryDataset.angVelocX.value << std::endl;
|
||
|
|
// sif::info << "Y: " << primaryDataset.angVelocY.value << std::endl;
|
||
|
|
// sif::info << "Z: " << primaryDataset.angVelocZ.value << std::endl;
|
||
|
|
// sif::info << "GyroADIS1650XHandler: Accelerations in m / s^2: " << std::endl;
|
||
|
|
// sif::info << "X: " << primaryDataset.accelX.value << std::endl;
|
||
|
|
// sif::info << "Y: " << primaryDataset.accelY.value << std::endl;
|
||
|
|
// sif::info << "Z: " << primaryDataset.accelZ.value << std::endl;
|
||
|
|
// }
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// break;
|
||
|
|
// }
|
||
|
|
// case (BurstModes::BURST_32_BURST_SEL_0): {
|
||
|
|
// break;
|
||
|
|
// }
|
||
|
|
// }
|
||
|
|
// return returnvalue::OK;
|
||
|
|
// }
|
||
|
|
|
||
|
|
uint32_t GyrAdis1650XHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 6000; }
|
||
|
|
|
||
|
|
void GyrAdis1650XHandler::prepareWriteCommand(uint8_t startReg, uint8_t valueOne,
|
||
|
|
uint8_t valueTwo) {
|
||
|
|
uint8_t secondReg = startReg + 1;
|
||
|
|
startReg |= adis1650x::WRITE_MASK;
|
||
|
|
secondReg |= adis1650x::WRITE_MASK;
|
||
|
|
cmdBuf[0] = startReg;
|
||
|
|
cmdBuf[1] = valueOne;
|
||
|
|
cmdBuf[2] = secondReg;
|
||
|
|
cmdBuf[3] = valueTwo;
|
||
|
|
this->rawPacketLen = 4;
|
||
|
|
this->rawPacket = cmdBuf.data();
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t GyrAdis1650XHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
|
||
|
|
LocalDataPoolManager &poolManager) {
|
||
|
|
localDataPoolMap.emplace(adis1650x::ANG_VELOC_X, new PoolEntry<double>({0.0}));
|
||
|
|
localDataPoolMap.emplace(adis1650x::ANG_VELOC_Y, new PoolEntry<double>({0.0}));
|
||
|
|
localDataPoolMap.emplace(adis1650x::ANG_VELOC_Z, new PoolEntry<double>({0.0}));
|
||
|
|
localDataPoolMap.emplace(adis1650x::ACCELERATION_X, new PoolEntry<double>({0.0}));
|
||
|
|
localDataPoolMap.emplace(adis1650x::ACCELERATION_Y, new PoolEntry<double>({0.0}));
|
||
|
|
localDataPoolMap.emplace(adis1650x::ACCELERATION_Z, new PoolEntry<double>({0.0}));
|
||
|
|
localDataPoolMap.emplace(adis1650x::TEMPERATURE, new PoolEntry<float>({0.0}));
|
||
|
|
|
||
|
|
localDataPoolMap.emplace(adis1650x::DIAG_STAT_REGISTER, new PoolEntry<uint16_t>());
|
||
|
|
localDataPoolMap.emplace(adis1650x::FILTER_SETTINGS, new PoolEntry<uint8_t>());
|
||
|
|
localDataPoolMap.emplace(adis1650x::RANG_MDL, &rangMdl);
|
||
|
|
localDataPoolMap.emplace(adis1650x::MSC_CTRL_REGISTER, new PoolEntry<uint16_t>());
|
||
|
|
localDataPoolMap.emplace(adis1650x::DEC_RATE_REGISTER, new PoolEntry<uint16_t>());
|
||
|
|
poolManager.subscribeForRegularPeriodicPacket(
|
||
|
|
subdp::RegularHkPeriodicParams(primaryDataset.getSid(), false, 5.0));
|
||
|
|
return returnvalue::OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
adis1650x::BurstModes GyrAdis1650XHandler::getBurstMode() {
|
||
|
|
using namespace adis1650x;
|
||
|
|
configDataset.mscCtrlReg.read();
|
||
|
|
uint16_t currentCtrlReg = configDataset.mscCtrlReg.value;
|
||
|
|
configDataset.mscCtrlReg.commit();
|
||
|
|
return adis1650x::burstModeFromMscCtrl(currentCtrlReg);
|
||
|
|
}
|
||
|
|
|
||
|
|
//#ifdef FSFW_OSAL_LINUX
|
||
|
|
|
||
|
|
// ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie *cookie,
|
||
|
|
// const uint8_t *sendData, size_t sendLen,
|
||
|
|
// void *args) {
|
||
|
|
// GyroADIS1650XHandler *handler = reinterpret_cast<GyroADIS1650XHandler *>(args);
|
||
|
|
// if (handler == nullptr) {
|
||
|
|
// sif::error << "GyroADIS16507Handler::spiSendCallback: Passed handler pointer is invalid!"
|
||
|
|
// << std::endl;
|
||
|
|
// return returnvalue::FAILED;
|
||
|
|
// }
|
||
|
|
// DeviceCommandId_t currentCommand = handler->getPendingCommand();
|
||
|
|
// switch (currentCommand) {
|
||
|
|
// case (adis1650x::READ_SENSOR_DATA): {
|
||
|
|
// return comIf->performRegularSendOperation(cookie, sendData, sendLen);
|
||
|
|
// }
|
||
|
|
// case (adis1650x::READ_OUT_CONFIG):
|
||
|
|
// default: {
|
||
|
|
// ReturnValue_t result = returnvalue::OK;
|
||
|
|
// int retval = 0;
|
||
|
|
// // Prepare transfer
|
||
|
|
// int fileDescriptor = 0;
|
||
|
|
// std::string device = comIf->getSpiDev();
|
||
|
|
// UnixFileGuard fileHelper(device, fileDescriptor, O_RDWR, "SpiComIF::sendMessage");
|
||
|
|
// if (fileHelper.getOpenResult() != returnvalue::OK) {
|
||
|
|
// return SpiComIF::OPENING_FILE_FAILED;
|
||
|
|
// }
|
||
|
|
// spi::SpiModes spiMode = spi::SpiModes::MODE_0;
|
||
|
|
// uint32_t spiSpeed = 0;
|
||
|
|
// cookie->getSpiParameters(spiMode, spiSpeed, nullptr);
|
||
|
|
// comIf->setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
|
||
|
|
// cookie->assignWriteBuffer(sendData);
|
||
|
|
// cookie->setTransferSize(2);
|
||
|
|
//
|
||
|
|
// gpioId_t gpioId = cookie->getChipSelectPin();
|
||
|
|
// GpioIF &gpioIF = comIf->getGpioInterface();
|
||
|
|
// MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
|
||
|
|
// uint32_t timeoutMs = 0;
|
||
|
|
// MutexIF *mutex = comIf->getCsMutex();
|
||
|
|
// cookie->getMutexParams(timeoutType, timeoutMs);
|
||
|
|
// if (mutex == nullptr) {
|
||
|
|
//#if OBSW_VERBOSE_LEVEL >= 1
|
||
|
|
// sif::warning << "GyroADIS16507Handler::spiSendCallback: "
|
||
|
|
// "Mutex or GPIO interface invalid"
|
||
|
|
// << std::endl;
|
||
|
|
// return returnvalue::FAILED;
|
||
|
|
//#endif
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// if (gpioId != gpio::NO_GPIO) {
|
||
|
|
// result = mutex->lockMutex(timeoutType, timeoutMs);
|
||
|
|
// if (result != returnvalue::OK) {
|
||
|
|
//#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||
|
|
// sif::error << "SpiComIF::sendMessage: Failed to lock mutex" << std::endl;
|
||
|
|
//#endif
|
||
|
|
// return result;
|
||
|
|
// }
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// size_t idx = 0;
|
||
|
|
// spi_ioc_transfer *transferStruct = cookie->getTransferStructHandle();
|
||
|
|
// uint64_t origTx = transferStruct->tx_buf;
|
||
|
|
// uint64_t origRx = transferStruct->rx_buf;
|
||
|
|
// while (idx < sendLen) {
|
||
|
|
// // Pull SPI CS low. For now, no support for active high given
|
||
|
|
// if (gpioId != gpio::NO_GPIO) {
|
||
|
|
// gpioIF.pullLow(gpioId);
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// // Execute transfer
|
||
|
|
// // Initiate a full duplex SPI transfer.
|
||
|
|
// retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), cookie->getTransferStructHandle());
|
||
|
|
// if (retval < 0) {
|
||
|
|
// utility::handleIoctlError("SpiComIF::sendMessage: ioctl error.");
|
||
|
|
// result = SpiComIF::FULL_DUPLEX_TRANSFER_FAILED;
|
||
|
|
// }
|
||
|
|
//#if FSFW_HAL_SPI_WIRETAPPING == 1
|
||
|
|
// comIf->performSpiWiretapping(cookie);
|
||
|
|
//#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */
|
||
|
|
//
|
||
|
|
// if (gpioId != gpio::NO_GPIO) {
|
||
|
|
// gpioIF.pullHigh(gpioId);
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// idx += 2;
|
||
|
|
// if (idx < sendLen) {
|
||
|
|
// usleep(adis1650x::STALL_TIME_MICROSECONDS);
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// transferStruct->tx_buf += 2;
|
||
|
|
// transferStruct->rx_buf += 2;
|
||
|
|
// }
|
||
|
|
// transferStruct->tx_buf = origTx;
|
||
|
|
// transferStruct->rx_buf = origRx;
|
||
|
|
// if (gpioId != gpio::NO_GPIO) {
|
||
|
|
// mutex->unlockMutex();
|
||
|
|
// }
|
||
|
|
// }
|
||
|
|
// }
|
||
|
|
// return returnvalue::OK;
|
||
|
|
// }
|
||
|
|
|
||
|
|
void GyrAdis1650XHandler::setToGoToNormalModeImmediately() { goToNormalMode = true; }
|
||
|
|
|
||
|
|
void GyrAdis1650XHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) {
|
||
|
|
periodicPrintout = enable;
|
||
|
|
debugDivider.setDivider(divider);
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t GyrAdis1650XHandler::preparePeriodicRequest(acs::SimpleSensorMode mode) {
|
||
|
|
adisRequest.mode = mode;
|
||
|
|
rawPacket = reinterpret_cast<uint8_t *>(&adisRequest);
|
||
|
|
rawPacketLen = sizeof(acs::Adis1650XRequest);
|
||
|
|
return returnvalue::OK;
|
||
|
|
}
|