From af32941352b6d9144b8e393e96353fdc86810831 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 10 Dec 2021 17:46:52 +0100 Subject: [PATCH] ADIS device handler can now handle multiple types - Added handling for ADIS16507 and ADIS16505 type --- bsp_q7s/core/ObjectFactory.cpp | 14 +- linux/fsfwconfig/FSFWConfig.h.in | 2 +- .../pollingSequenceFactory.cpp | 2 +- mission/devices/CMakeLists.txt | 2 +- ...07Handler.cpp => GyroADIS1650XHandler.cpp} | 202 ++++++++++-------- ...S16507Handler.h => GyroADIS1650XHandler.h} | 12 +- ...finitions.h => GyroADIS1650XDefinitions.h} | 47 ++-- 7 files changed, 150 insertions(+), 131 deletions(-) rename mission/devices/{GyroADIS16507Handler.cpp => GyroADIS1650XHandler.cpp} (69%) rename mission/devices/{GyroADIS16507Handler.h => GyroADIS1650XHandler.h} (89%) rename mission/devices/devicedefinitions/{GyroADIS16507Definitions.h => GyroADIS1650XDefinitions.h} (80%) diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index bc428f05..e3deba9d 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -36,7 +36,7 @@ #include "mission/devices/P60DockHandler.h" #include "mission/devices/Tmp1075Handler.h" #include "mission/devices/Max31865PT1000Handler.h" -#include "mission/devices/GyroADIS16507Handler.h" +#include "mission/devices/GyroADIS1650XHandler.h" #include "mission/devices/IMTQHandler.h" #include "mission/devices/SyrlinksHkHandler.h" #include "mission/devices/PlocMPSoCHandler.h" @@ -511,10 +511,10 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF *gpioComIF, UartComI // Commented until ACS board V2 in in clean room again // Gyro 0 Side A spiCookie = new SpiCookie(addresses::GYRO_0_ADIS, gpioIds::GYRO_0_ADIS_CS, spiDev, - ADIS16507::MAXIMUM_REPLY_SIZE, spi::DEFAULT_ADIS16507_MODE, + ADIS1650X::MAXIMUM_REPLY_SIZE, spi::DEFAULT_ADIS16507_MODE, spi::DEFAULT_ADIS16507_SPEED); - auto adisHandler = new GyroADIS16507Handler(objects::GYRO_0_ADIS_HANDLER, objects::SPI_COM_IF, - spiCookie); + auto adisHandler = new GyroADIS1650XHandler(objects::GYRO_0_ADIS_HANDLER, objects::SPI_COM_IF, + spiCookie, ADIS1650X::Type::ADIS16505); adisHandler->setStartUpImmediately(); // Gyro 1 Side A spiCookie = new SpiCookie(addresses::GYRO_1_L3G, gpioIds::GYRO_1_L3G_CS, spiDev, @@ -527,10 +527,10 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF *gpioComIF, UartComI #endif // Gyro 2 Side B spiCookie = new SpiCookie(addresses::GYRO_2_ADIS, gpioIds::GYRO_2_ADIS_CS, spiDev, - ADIS16507::MAXIMUM_REPLY_SIZE, spi::DEFAULT_ADIS16507_MODE, + ADIS1650X::MAXIMUM_REPLY_SIZE, spi::DEFAULT_ADIS16507_MODE, spi::DEFAULT_ADIS16507_SPEED); - adisHandler = new GyroADIS16507Handler(objects::GYRO_2_ADIS_HANDLER, objects::SPI_COM_IF, - spiCookie); + adisHandler = new GyroADIS1650XHandler(objects::GYRO_2_ADIS_HANDLER, objects::SPI_COM_IF, + spiCookie, ADIS1650X::Type::ADIS16505); adisHandler->setStartUpImmediately(); // Gyro 3 Side B spiCookie = new SpiCookie(addresses::GYRO_3_L3G, gpioIds::GYRO_3_L3G_CS, spiDev, diff --git a/linux/fsfwconfig/FSFWConfig.h.in b/linux/fsfwconfig/FSFWConfig.h.in index 3b87078c..9e8f2016 100644 --- a/linux/fsfwconfig/FSFWConfig.h.in +++ b/linux/fsfwconfig/FSFWConfig.h.in @@ -78,6 +78,6 @@ static constexpr size_t FSFW_MAX_TM_PACKET_SIZE = 2048; #define FSFW_HAL_L3GD20_GYRO_DEBUG 0 #define FSFW_HAL_RM3100_MGM_DEBUG 0 #define FSFW_HAL_LIS3MDL_MGM_DEBUG 0 -#define FSFW_HAL_ADIS16507_GYRO_DEBUG 0 +#define FSFW_HAL_ADIS1650X_GYRO_DEBUG 0 #endif /* CONFIG_FSFWCONFIG_H_ */ diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp index 02f37438..73a27116 100644 --- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp +++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp @@ -439,7 +439,7 @@ ReturnValue_t pst::pstSpi(FixedTimeslotTaskIF *thisSequence) { #endif #if OBSW_ADD_ACS_BOARD == 1 - bool enableAside = true; + bool enableAside = false; bool enableBside = true; if(enableAside) { // A side diff --git a/mission/devices/CMakeLists.txt b/mission/devices/CMakeLists.txt index ea3fdbea..cbfbe4a5 100644 --- a/mission/devices/CMakeLists.txt +++ b/mission/devices/CMakeLists.txt @@ -12,7 +12,7 @@ target_sources(${TARGET_NAME} PUBLIC IMTQHandler.cpp PlocMPSoCHandler.cpp RadiationSensorHandler.cpp - GyroADIS16507Handler.cpp + GyroADIS1650XHandler.cpp RwHandler.cpp ) diff --git a/mission/devices/GyroADIS16507Handler.cpp b/mission/devices/GyroADIS1650XHandler.cpp similarity index 69% rename from mission/devices/GyroADIS16507Handler.cpp rename to mission/devices/GyroADIS1650XHandler.cpp index 07e0d643..4acf9196 100644 --- a/mission/devices/GyroADIS16507Handler.cpp +++ b/mission/devices/GyroADIS1650XHandler.cpp @@ -1,8 +1,8 @@ -#include "GyroADIS16507Handler.h" +#include "GyroADIS1650XHandler.h" #include #include -#if OBSW_ADIS16507_LINUX_COM_IF == 1 +#if OBSW_ADIS1650X_LINUX_COM_IF == 1 #include "fsfw_hal/linux/utility.h" #include "fsfw_hal/linux/spi/SpiCookie.h" #include "fsfw_hal/linux/spi/SpiComIF.h" @@ -11,15 +11,15 @@ #include #endif -GyroADIS16507Handler::GyroADIS16507Handler(object_id_t objectId, - object_id_t deviceCommunication, CookieIF * comCookie): - DeviceHandlerBase(objectId, deviceCommunication, comCookie), primaryDataset(this), - configDataset(this), breakCountdown() { -#if FSFW_HAL_ADIS16507_GYRO_DEBUG == 1 +GyroADIS1650XHandler::GyroADIS1650XHandler(object_id_t objectId, + object_id_t deviceCommunication, CookieIF * comCookie, ADIS1650X::Type type): + DeviceHandlerBase(objectId, deviceCommunication, comCookie), adisType(type), + primaryDataset(this), configDataset(this), breakCountdown() { +#if FSFW_HAL_ADIS1650X_GYRO_DEBUG == 1 debugDivider = new PeriodicOperationDivider(5); #endif -#if OBSW_ADIS16507_LINUX_COM_IF == 1 +#if OBSW_ADIS1650X_LINUX_COM_IF == 1 SpiCookie* cookie = dynamic_cast(comCookie); if(cookie != nullptr) { cookie->setCallbackMode(&spiSendCallback, this); @@ -27,11 +27,11 @@ GyroADIS16507Handler::GyroADIS16507Handler(object_id_t objectId, #endif } -void GyroADIS16507Handler::doStartUp() { +void GyroADIS1650XHandler::doStartUp() { // Initial 310 ms start up time after power-up if(internalState == InternalState::STARTUP) { if(not commandExecuted) { - breakCountdown.setTimeout(ADIS16507::START_UP_TIME); + breakCountdown.setTimeout(ADIS1650X::START_UP_TIME); commandExecuted = true; } if(breakCountdown.hasTimedOut()) { @@ -54,20 +54,20 @@ void GyroADIS16507Handler::doStartUp() { } } -void GyroADIS16507Handler::doShutDown() { +void GyroADIS1650XHandler::doShutDown() { commandExecuted = false; setMode(_MODE_POWER_DOWN); } -ReturnValue_t GyroADIS16507Handler::buildNormalDeviceCommand(DeviceCommandId_t *id) { - *id = ADIS16507::READ_SENSOR_DATA; +ReturnValue_t GyroADIS1650XHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) { + *id = ADIS1650X::READ_SENSOR_DATA; return buildCommandFromCommand(*id, nullptr, 0); } -ReturnValue_t GyroADIS16507Handler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { +ReturnValue_t GyroADIS1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { switch(internalState) { case(InternalState::CONFIG): { - *id = ADIS16507::READ_OUT_CONFIG; + *id = ADIS1650X::READ_OUT_CONFIG; buildCommandFromCommand(*id, nullptr, 0); break; } @@ -85,79 +85,79 @@ ReturnValue_t GyroADIS16507Handler::buildTransitionDeviceCommand(DeviceCommandId return HasReturnvaluesIF::RETURN_OK; } -ReturnValue_t GyroADIS16507Handler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, +ReturnValue_t GyroADIS1650XHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen) { switch(deviceCommand) { - case(ADIS16507::READ_OUT_CONFIG): { - this->rawPacketLen = ADIS16507::CONFIG_READOUT_SIZE; + case(ADIS1650X::READ_OUT_CONFIG): { + this->rawPacketLen = ADIS1650X::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; + regList[0] = ADIS1650X::DIAG_STAT_REG; + regList[1] = ADIS1650X::FILTER_CTRL_REG; + regList[2] = ADIS1650X::MSC_CTRL_REG; + regList[3] = ADIS1650X::DEC_RATE_REG; + regList[4] = ADIS1650X::PROD_ID_REG; prepareReadCommand(regList, sizeof(regList)); this->rawPacket = commandBuffer.data(); break; } - case(ADIS16507::READ_SENSOR_DATA): { + 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(), ADIS16507::BURST_READ_ENABLE.data(), - ADIS16507::BURST_READ_ENABLE.size()); + std::memcpy(commandBuffer.data(), ADIS1650X::BURST_READ_ENABLE.data(), + ADIS1650X::BURST_READ_ENABLE.size()); std::memset(commandBuffer.data() + 2, 0, 10 * 2); - this->rawPacketLen = ADIS16507::SENSOR_READOUT_SIZE; + this->rawPacketLen = ADIS1650X::SENSOR_READOUT_SIZE; this->rawPacket = commandBuffer.data(); break; } - case(ADIS16507::SELF_TEST_SENSORS): { + case(ADIS1650X::SELF_TEST_SENSORS): { if(breakCountdown.isBusy()) { // Another glob command is pending return HasActionsIF::IS_BUSY; } - prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::SENSOR_SELF_TEST, 0x00); - breakCountdown.setTimeout(ADIS16507::SELF_TEST_BREAK); + prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::SENSOR_SELF_TEST, 0x00); + breakCountdown.setTimeout(ADIS1650X::SELF_TEST_BREAK); break; } - case(ADIS16507::SELF_TEST_MEMORY): { + case(ADIS1650X::SELF_TEST_MEMORY): { if(breakCountdown.isBusy()) { // Another glob command is pending return HasActionsIF::IS_BUSY; } - prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::FLASH_MEMORY_TEST, 0x00); - breakCountdown.setTimeout(ADIS16507::FLASH_MEMORY_TEST_BREAK); + prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FLASH_MEMORY_TEST, 0x00); + breakCountdown.setTimeout(ADIS1650X::FLASH_MEMORY_TEST_BREAK); break; } - case(ADIS16507::UPDATE_NV_CONFIGURATION): { + case(ADIS1650X::UPDATE_NV_CONFIGURATION): { if(breakCountdown.isBusy()) { // Another glob command is pending return HasActionsIF::IS_BUSY; } - prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::FLASH_MEMORY_UPDATE, 0x00); - breakCountdown.setTimeout(ADIS16507::FLASH_MEMORY_UPDATE_BREAK); + prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FLASH_MEMORY_UPDATE, 0x00); + breakCountdown.setTimeout(ADIS1650X::FLASH_MEMORY_UPDATE_BREAK); break; } - case(ADIS16507::RESET_SENSOR_CONFIGURATION): { + case(ADIS1650X::RESET_SENSOR_CONFIGURATION): { if(breakCountdown.isBusy()) { // Another glob command is pending return HasActionsIF::IS_BUSY; } - prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::FACTORY_CALIBRATION, 0x00); - breakCountdown.setTimeout(ADIS16507::FACTORY_CALIBRATION_BREAK); + prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FACTORY_CALIBRATION, 0x00); + breakCountdown.setTimeout(ADIS1650X::FACTORY_CALIBRATION_BREAK); break; } - case(ADIS16507::SW_RESET): { + case(ADIS1650X::SW_RESET): { if(breakCountdown.isBusy()) { // Another glob command is pending return HasActionsIF::IS_BUSY; } - prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::SOFTWARE_RESET, 0x00); - breakCountdown.setTimeout(ADIS16507::SW_RESET_BREAK); + prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::SOFTWARE_RESET, 0x00); + breakCountdown.setTimeout(ADIS1650X::SW_RESET_BREAK); break; } - case(ADIS16507::PRINT_CURRENT_CONFIGURATION): { + 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) << @@ -172,18 +172,18 @@ ReturnValue_t GyroADIS16507Handler::buildCommandFromCommand(DeviceCommandId_t de return HasReturnvaluesIF::RETURN_OK; } -void GyroADIS16507Handler::fillCommandAndReplyMap() { - insertInCommandAndReplyMap(ADIS16507::READ_SENSOR_DATA, 1, &primaryDataset); - insertInCommandAndReplyMap(ADIS16507::READ_OUT_CONFIG, 1, &configDataset); - insertInCommandAndReplyMap(ADIS16507::SELF_TEST_SENSORS, 1); - insertInCommandAndReplyMap(ADIS16507::SELF_TEST_MEMORY, 1); - insertInCommandAndReplyMap(ADIS16507::UPDATE_NV_CONFIGURATION, 1); - insertInCommandAndReplyMap(ADIS16507::RESET_SENSOR_CONFIGURATION, 1); - insertInCommandAndReplyMap(ADIS16507::SW_RESET, 1); - insertInCommandAndReplyMap(ADIS16507::PRINT_CURRENT_CONFIGURATION, 1); +void GyroADIS1650XHandler::fillCommandAndReplyMap() { + 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 GyroADIS16507Handler::scanForReply(const uint8_t *start, size_t remainingSize, +ReturnValue_t GyroADIS1650XHandler::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(); @@ -192,16 +192,19 @@ ReturnValue_t GyroADIS16507Handler::scanForReply(const uint8_t *start, size_t re return HasReturnvaluesIF::RETURN_OK; } -ReturnValue_t GyroADIS16507Handler::interpretDeviceReply(DeviceCommandId_t id, +ReturnValue_t GyroADIS1650XHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { switch(id) { - case(ADIS16507::READ_OUT_CONFIG): { + case(ADIS1650X::READ_OUT_CONFIG): { PoolReadGuard rg(&configDataset); uint16_t readProdId = packet[10] << 8 | packet[11]; - if (readProdId != ADIS16507::PROD_ID) { + if(((adisType == ADIS1650X::Type::ADIS16507) and + (readProdId != ADIS1650X::PROD_ID_16507)) or + ((adisType == ADIS1650X::Type::ADIS16505) and + (readProdId != ADIS1650X::PROD_ID_16505))) { #if OBSW_VERBOSE_LEVEL >= 1 - sif::warning << "GyroADIS16507Handler::interpretDeviceReply: Invalid product ID!" - << std::endl; + sif::warning << "GyroADIS16507Handler::interpretDeviceReply: Invalid product ID " + << readProdId << std::endl; #endif return HasReturnvaluesIF::RETURN_FAILED; } @@ -215,7 +218,7 @@ ReturnValue_t GyroADIS16507Handler::interpretDeviceReply(DeviceCommandId_t id, } break; } - case(ADIS16507::READ_SENSOR_DATA): { + case(ADIS1650X::READ_SENSOR_DATA): { return handleSensorData(packet); } @@ -224,7 +227,7 @@ ReturnValue_t GyroADIS16507Handler::interpretDeviceReply(DeviceCommandId_t id, } -ReturnValue_t GyroADIS16507Handler::handleSensorData(const uint8_t *packet) { +ReturnValue_t GyroADIS1650XHandler::handleSensorData(const uint8_t *packet) { BurstModes burstMode = getBurstMode(); switch(burstMode) { case(BurstModes::BURST_16_BURST_SEL_1): @@ -260,23 +263,32 @@ ReturnValue_t GyroADIS16507Handler::handleSensorData(const uint8_t *packet) { PoolReadGuard pg(&primaryDataset); int16_t angVelocXRaw = packet[4] << 8 | packet[5]; primaryDataset.angVelocX.value = static_cast(angVelocXRaw) / INT16_MAX * - ADIS16507::GYRO_RANGE; + ADIS1650X::GYRO_RANGE; int16_t angVelocYRaw = packet[6] << 8 | packet[7]; primaryDataset.angVelocY.value = static_cast(angVelocYRaw) / INT16_MAX * - ADIS16507::GYRO_RANGE; + ADIS1650X::GYRO_RANGE; int16_t angVelocZRaw = packet[8] << 8 | packet[9]; primaryDataset.angVelocZ.value = static_cast(angVelocZRaw) / INT16_MAX * - ADIS16507::GYRO_RANGE; + ADIS1650X::GYRO_RANGE; + 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 << "GyroADIS16507Handler::handleSensorData: " + "Unknown ADIS type" << std::endl; + } int16_t accelXRaw = packet[10] << 8 | packet[11]; primaryDataset.accelX.value = static_cast(accelXRaw) / INT16_MAX * - ADIS16507::ACCELEROMETER_RANGE; + accelScaling; int16_t accelYRaw = packet[12] << 8 | packet[13]; primaryDataset.accelY.value = static_cast(accelYRaw) / INT16_MAX * - ADIS16507::ACCELEROMETER_RANGE; + accelScaling; int16_t accelZRaw = packet[14] << 8 | packet[15]; primaryDataset.accelZ.value = static_cast(accelZRaw) / INT16_MAX * - ADIS16507::ACCELEROMETER_RANGE; + accelScaling; int16_t temperatureRaw = packet[16] << 8 | packet[17]; primaryDataset.temperature.value = static_cast(temperatureRaw) * 0.1; @@ -284,7 +296,7 @@ ReturnValue_t GyroADIS16507Handler::handleSensorData(const uint8_t *packet) { primaryDataset.setValidity(true, true); } -#if FSFW_HAL_ADIS16507_GYRO_DEBUG == 1 +#if FSFW_HAL_ADIS1650X_GYRO_DEBUG == 1 if(debugDivider->checkAndIncrement()) { sif::info << "GyroADIS16507Handler: Angular velocities in deg / s" << std::endl; sif::info << "X: " << primaryDataset.angVelocX.value << std::endl; @@ -306,15 +318,15 @@ ReturnValue_t GyroADIS16507Handler::handleSensorData(const uint8_t *packet) { return HasReturnvaluesIF::RETURN_OK; } -uint32_t GyroADIS16507Handler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { +uint32_t GyroADIS1650XHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 5000; } -void GyroADIS16507Handler::prepareWriteCommand(uint8_t startReg, uint8_t valueOne, +void GyroADIS1650XHandler::prepareWriteCommand(uint8_t startReg, uint8_t valueOne, uint8_t valueTwo) { uint8_t secondReg = startReg + 1; - startReg |= ADIS16507::WRITE_MASK; - secondReg |= ADIS16507::WRITE_MASK; + startReg |= ADIS1650X::WRITE_MASK; + secondReg |= ADIS1650X::WRITE_MASK; commandBuffer[0] = startReg; commandBuffer[1] = valueOne; commandBuffer[2] = secondReg; @@ -323,7 +335,7 @@ void GyroADIS16507Handler::prepareWriteCommand(uint8_t startReg, uint8_t valueOn this->rawPacket = commandBuffer.data(); } -void GyroADIS16507Handler::prepareReadCommand(uint8_t *regList, size_t len) { +void GyroADIS1650XHandler::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; @@ -332,29 +344,29 @@ void GyroADIS16507Handler::prepareReadCommand(uint8_t *regList, size_t len) { commandBuffer[len * 2 + 1] = 0x00; } -ReturnValue_t GyroADIS16507Handler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, +ReturnValue_t GyroADIS1650XHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { - localDataPoolMap.emplace(ADIS16507::ANG_VELOC_X, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS16507::ANG_VELOC_Y, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS16507::ANG_VELOC_Z, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS16507::ACCELERATION_X, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS16507::ACCELERATION_Y, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS16507::ACCELERATION_Z, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS16507::TEMPERATURE, new PoolEntry({0.0})); + localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_X, new PoolEntry({0.0})); + localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Y, new PoolEntry({0.0})); + localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Z, new PoolEntry({0.0})); + localDataPoolMap.emplace(ADIS1650X::ACCELERATION_X, new PoolEntry({0.0})); + localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Y, new PoolEntry({0.0})); + localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Z, new PoolEntry({0.0})); + localDataPoolMap.emplace(ADIS1650X::TEMPERATURE, new PoolEntry({0.0})); - localDataPoolMap.emplace(ADIS16507::DIAG_STAT_REGISTER, new PoolEntry()); - localDataPoolMap.emplace(ADIS16507::FILTER_SETTINGS, new PoolEntry()); - localDataPoolMap.emplace(ADIS16507::MSC_CTRL_REGISTER, new PoolEntry()); - localDataPoolMap.emplace(ADIS16507::DEC_RATE_REGISTER, new PoolEntry()); + localDataPoolMap.emplace(ADIS1650X::DIAG_STAT_REGISTER, new PoolEntry()); + localDataPoolMap.emplace(ADIS1650X::FILTER_SETTINGS, new PoolEntry()); + localDataPoolMap.emplace(ADIS1650X::MSC_CTRL_REGISTER, new PoolEntry()); + localDataPoolMap.emplace(ADIS1650X::DEC_RATE_REGISTER, new PoolEntry()); return HasReturnvaluesIF::RETURN_OK; } -GyroADIS16507Handler::BurstModes GyroADIS16507Handler::getBurstMode() { +GyroADIS1650XHandler::BurstModes GyroADIS1650XHandler::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) { + if((currentCtrlReg & ADIS1650X::BURST_32_BIT) == ADIS1650X::BURST_32_BIT) { + if((currentCtrlReg & ADIS1650X::BURST_SEL_BIT) == ADIS1650X::BURST_SEL_BIT) { return BurstModes::BURST_32_BURST_SEL_1; } else { @@ -362,7 +374,7 @@ GyroADIS16507Handler::BurstModes GyroADIS16507Handler::getBurstMode() { } } else { - if((currentCtrlReg & ADIS16507::BURST_SEL_BIT) == ADIS16507::BURST_SEL_BIT) { + if((currentCtrlReg & ADIS1650X::BURST_SEL_BIT) == ADIS1650X::BURST_SEL_BIT) { return BurstModes::BURST_16_BURST_SEL_1; } else { @@ -371,11 +383,11 @@ GyroADIS16507Handler::BurstModes GyroADIS16507Handler::getBurstMode() { } } -#if OBSW_ADIS16507_LINUX_COM_IF == 1 +#if OBSW_ADIS1650X_LINUX_COM_IF == 1 -ReturnValue_t GyroADIS16507Handler::spiSendCallback(SpiComIF *comIf, SpiCookie *cookie, +ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie *cookie, const uint8_t *sendData, size_t sendLen, void *args) { - GyroADIS16507Handler* handler = reinterpret_cast(args); + GyroADIS1650XHandler* handler = reinterpret_cast(args); if(handler == nullptr) { sif::error << "GyroADIS16507Handler::spiSendCallback: Passed handler pointer is invalid!" << std::endl; @@ -383,10 +395,10 @@ ReturnValue_t GyroADIS16507Handler::spiSendCallback(SpiComIF *comIf, SpiCookie * } DeviceCommandId_t currentCommand = handler->getPendingCommand(); switch(currentCommand) { - case(ADIS16507::READ_SENSOR_DATA): { + case(ADIS1650X::READ_SENSOR_DATA): { return comIf->performRegularSendOperation(cookie, sendData, sendLen); } - case(ADIS16507::READ_OUT_CONFIG): + case(ADIS1650X::READ_OUT_CONFIG): default: { ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; int retval = 0; @@ -451,7 +463,7 @@ ReturnValue_t GyroADIS16507Handler::spiSendCallback(SpiComIF *comIf, SpiCookie * idx += 2; if(idx < sendLen) { - usleep(ADIS16507::STALL_TIME_MICROSECONDS); + usleep(ADIS1650X::STALL_TIME_MICROSECONDS); } spi_ioc_transfer* transferStruct = cookie->getTransferStructHandle(); transferStruct->tx_buf += 2; @@ -466,4 +478,4 @@ ReturnValue_t GyroADIS16507Handler::spiSendCallback(SpiComIF *comIf, SpiCookie * return HasReturnvaluesIF::RETURN_OK; } -#endif /* OBSW_ADIS16507_LINUX_COM_IF == 1 */ +#endif /* OBSW_ADIS1650X_LINUX_COM_IF == 1 */ diff --git a/mission/devices/GyroADIS16507Handler.h b/mission/devices/GyroADIS1650XHandler.h similarity index 89% rename from mission/devices/GyroADIS16507Handler.h rename to mission/devices/GyroADIS1650XHandler.h index 467e0906..60adeabe 100644 --- a/mission/devices/GyroADIS16507Handler.h +++ b/mission/devices/GyroADIS1650XHandler.h @@ -3,7 +3,7 @@ #include "OBSWConfig.h" #include "FSFWConfig.h" -#include "devicedefinitions/GyroADIS16507Definitions.h" +#include "devicedefinitions/GyroADIS1650XDefinitions.h" #include "fsfw/globalfunctions/PeriodicOperationDivider.h" #include "fsfw/devicehandlers/DeviceHandlerBase.h" @@ -19,10 +19,10 @@ class SpiCookie; * Flight manual: * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/ADIS16507_Gyro */ -class GyroADIS16507Handler: public DeviceHandlerBase { +class GyroADIS1650XHandler: public DeviceHandlerBase { public: - GyroADIS16507Handler(object_id_t objectId, object_id_t deviceCommunication, - CookieIF * comCookie); + GyroADIS1650XHandler(object_id_t objectId, object_id_t deviceCommunication, + CookieIF* comCookie, ADIS1650X::Type type); /* DeviceHandlerBase abstract function implementation */ void doStartUp() override; @@ -42,7 +42,7 @@ public: private: std::array commandBuffer; - + ADIS1650X::Type adisType; AdisGyroPrimaryDataset primaryDataset; AdisGyroConfigDataset configDataset; @@ -71,7 +71,7 @@ private: const uint8_t *sendData, size_t sendLen, void* args); #endif -#if FSFW_HAL_ADIS16507_GYRO_DEBUG == 1 +#if FSFW_HAL_ADIS1650X_GYRO_DEBUG == 1 PeriodicOperationDivider* debugDivider; #endif Countdown breakCountdown; diff --git a/mission/devices/devicedefinitions/GyroADIS16507Definitions.h b/mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h similarity index 80% rename from mission/devices/devicedefinitions/GyroADIS16507Definitions.h rename to mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h index b83c984d..62744e3a 100644 --- a/mission/devices/devicedefinitions/GyroADIS16507Definitions.h +++ b/mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h @@ -6,17 +6,24 @@ #include -namespace ADIS16507 { +namespace ADIS1650X { + +enum class Type { + ADIS16505, + ADIS16507 +}; static constexpr size_t MAXIMUM_REPLY_SIZE = 64; static constexpr uint8_t WRITE_MASK = 0b1000'0000; static constexpr uint32_t GYRO_RANGE = 125; -static constexpr uint32_t ACCELEROMETER_RANGE = 392; +static constexpr uint32_t ACCELEROMETER_RANGE_16507 = 392; +static constexpr float ACCELEROMETER_RANGE_16505 = 78.4; static constexpr uint32_t STALL_TIME_MICROSECONDS = 16; -static constexpr uint16_t PROD_ID = 16507; +static constexpr uint16_t PROD_ID_16507 = 16507; +static constexpr uint16_t PROD_ID_16505 = 16505; static constexpr std::array BURST_READ_ENABLE = {0x68, 0x00}; @@ -95,31 +102,31 @@ public: /** Constructor for data users like controllers */ AdisGyroPrimaryDataset(object_id_t adisId): - StaticLocalDataSet(sid_t(adisId, ADIS16507::ADIS_DATASET_ID)) { + StaticLocalDataSet(sid_t(adisId, ADIS1650X::ADIS_DATASET_ID)) { setAllVariablesReadOnly(); } /* Angular velocities in degrees per second (DPS) */ lp_var_t angVelocX = lp_var_t(sid.objectId, - ADIS16507::ANG_VELOC_X, this); + ADIS1650X::ANG_VELOC_X, this); lp_var_t angVelocY = lp_var_t(sid.objectId, - ADIS16507::ANG_VELOC_Y, this); + ADIS1650X::ANG_VELOC_Y, this); lp_var_t angVelocZ = lp_var_t(sid.objectId, - ADIS16507::ANG_VELOC_Z, this); + ADIS1650X::ANG_VELOC_Z, this); lp_var_t accelX = lp_var_t(sid.objectId, - ADIS16507::ACCELERATION_X, this); + ADIS1650X::ACCELERATION_X, this); lp_var_t accelY = lp_var_t(sid.objectId, - ADIS16507::ACCELERATION_Y, this); + ADIS1650X::ACCELERATION_Y, this); lp_var_t accelZ = lp_var_t(sid.objectId, - ADIS16507::ACCELERATION_Z, this); + ADIS1650X::ACCELERATION_Z, this); lp_var_t temperature = lp_var_t(sid.objectId, - ADIS16507::TEMPERATURE, this); + ADIS1650X::TEMPERATURE, this); private: - friend class GyroADIS16507Handler; + friend class GyroADIS1650XHandler; /** Constructor for the data creator */ AdisGyroPrimaryDataset(HasLocalDataPoolIF* hkOwner): - StaticLocalDataSet(hkOwner, ADIS16507::ADIS_DATASET_ID) {} + StaticLocalDataSet(hkOwner, ADIS1650X::ADIS_DATASET_ID) {} }; class AdisGyroConfigDataset: public StaticLocalDataSet<5> { @@ -127,20 +134,20 @@ public: /** Constructor for data users like controllers */ AdisGyroConfigDataset(object_id_t adisId): - StaticLocalDataSet(sid_t(adisId, ADIS16507::ADIS_CFG_DATASET_ID)) { + StaticLocalDataSet(sid_t(adisId, ADIS1650X::ADIS_CFG_DATASET_ID)) { setAllVariablesReadOnly(); } lp_var_t diagStatReg = lp_var_t(sid.objectId, - ADIS16507::DIAG_STAT_REGISTER); - lp_var_t filterSetting = lp_var_t(sid.objectId, ADIS16507::FILTER_SETTINGS); - lp_var_t mscCtrlReg = lp_var_t(sid.objectId, ADIS16507::MSC_CTRL_REGISTER); - lp_var_t decRateReg = lp_var_t(sid.objectId, ADIS16507::DEC_RATE_REGISTER); + ADIS1650X::DIAG_STAT_REGISTER); + lp_var_t filterSetting = lp_var_t(sid.objectId, ADIS1650X::FILTER_SETTINGS); + lp_var_t mscCtrlReg = lp_var_t(sid.objectId, ADIS1650X::MSC_CTRL_REGISTER); + lp_var_t decRateReg = lp_var_t(sid.objectId, ADIS1650X::DEC_RATE_REGISTER); private: - friend class GyroADIS16507Handler; + friend class GyroADIS1650XHandler; /** Constructor for the data creator */ AdisGyroConfigDataset(HasLocalDataPoolIF* hkOwner): - StaticLocalDataSet(hkOwner, ADIS16507::ADIS_CFG_DATASET_ID) {} + StaticLocalDataSet(hkOwner, ADIS1650X::ADIS_CFG_DATASET_ID) {} }; #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GYROADIS16507DEFINITIONS_H_ */