From fe62e38afc139a159cb7830fd4aa0fe402f94fe1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 24 May 2021 20:33:59 +0200 Subject: [PATCH] adis handler continued --- fsfw_hal | 2 +- linux/fsfwconfig/OBSWConfig.h | 23 +-- mission/devices/GyroADIS16507Handler.cpp | 131 +++++++++++++++++- mission/devices/GyroADIS16507Handler.h | 26 +++- mission/devices/GyroL3GD20Handler.cpp | 4 +- .../GyroADIS16507Definitions.h | 111 +++++++++++++++ .../MGMHandlerLIS3Definitions.h | 2 +- 7 files changed, 277 insertions(+), 22 deletions(-) diff --git a/fsfw_hal b/fsfw_hal index d801319c..73a427d7 160000 --- a/fsfw_hal +++ b/fsfw_hal @@ -1 +1 @@ -Subproject commit d801319c12713d08cbdbc571ee2a922ce2f0c851 +Subproject commit 73a427d7e0605519aecfa53fa289b94e0143394f diff --git a/linux/fsfwconfig/OBSWConfig.h b/linux/fsfwconfig/OBSWConfig.h index 88040233..3894aecb 100644 --- a/linux/fsfwconfig/OBSWConfig.h +++ b/linux/fsfwconfig/OBSWConfig.h @@ -26,17 +26,20 @@ debugging. */ #define TEST_SUS_HANDLER 1 #define TEST_PLOC_HANDLER 0 -#define TE0720 0 -#define TE0720_HEATER_TEST 0 +#define TE0720 0 +#define TE0720_HEATER_TEST 0 -#define P60DOCK_DEBUG 0 -#define PDU1_DEBUG 0 -#define PDU2_DEBUG 0 -#define ACU_DEBUG 0 -#define SYRLINKS_DEBUG 0 -#define IMQT_DEBUG 0 -#define DEBUG_RAD_SENSOR 1 -#define DEBUG_SUS 1 +#define P60DOCK_DEBUG 0 +#define PDU1_DEBUG 0 +#define PDU2_DEBUG 0 +#define ACU_DEBUG 0 +#define SYRLINKS_DEBUG 0 +#define IMQT_DEBUG 0 +#define DEBUG_RAD_SENSOR 1 +#define DEBUG_SUS 1 + +// Leave at one as the BSP is linux. Used by the ADIS16507 device handler +#define OBSW_ADIS16507_LINUX_COM_IF 1 #include "OBSWVersion.h" diff --git a/mission/devices/GyroADIS16507Handler.cpp b/mission/devices/GyroADIS16507Handler.cpp index 8e0ca077..1413bfca 100644 --- a/mission/devices/GyroADIS16507Handler.cpp +++ b/mission/devices/GyroADIS16507Handler.cpp @@ -1,8 +1,21 @@ +#include #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 + GyroADIS16507Handler::GyroADIS16507Handler(object_id_t objectId, object_id_t deviceCommunication, CookieIF * comCookie): - DeviceHandlerBase(objectId, deviceCommunication, comCookie) { + DeviceHandlerBase(objectId, deviceCommunication, comCookie), primaryDataset(this), + configDataset(this) { +#if OBSW_ADIS16507_LINUX_COM_IF == 1 + SpiCookie* cookie = dynamic_cast(comCookie); + if(cookie != nullptr) { + cookie->setCallbackMode(&spiSendCallback, this); + } +#endif } void GyroADIS16507Handler::doStartUp() { @@ -18,15 +31,50 @@ void GyroADIS16507Handler::doShutDown() { } ReturnValue_t GyroADIS16507Handler::buildNormalDeviceCommand(DeviceCommandId_t *id) { - return HasReturnvaluesIF::RETURN_OK; + *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[4]; + regList[0] = ADIS16507::FILTER_CTRL_REG; + regList[1] = ADIS16507::MSC_CTRL_REG; + regList[2] = ADIS16507::DEC_RATE_REG; + regList[3] = 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; } @@ -35,17 +83,90 @@ void GyroADIS16507Handler::fillCommandAndReplyMap() { 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) { - return HasReturnvaluesIF::RETURN_OK; -} + switch(id) { + case(ADIS16507::READ_OUT_CONFIG): { + PoolReadGuard rg(&configDataset); + uint16_t readProdId = packet[6] << 8 | packet[7]; + 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.filterSetting.value = packet[0] << 8 | packet[1]; + configDataset.mscCtrlReg.value = packet[2] << 8 | packet[3]; + configDataset.decRateReg.value = packet[4] << 8 | packet[5]; + if(internalState == InternalState::STARTUP) { + commandExecuted = true; + } + break; + } + case(ADIS16507::READ_SENSOR_DATA): { -void GyroADIS16507Handler::setNormalDatapoolEntriesInvalid() { + } + + } + 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({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::FILTER_SETTINGS, new PoolEntry()); + localDataPoolMap.emplace(ADIS16507::MSC_CTRL_REGISTER, new PoolEntry()); + localDataPoolMap.emplace(ADIS16507::DEC_RATE_REGISTER, new PoolEntry()); + return HasReturnvaluesIF::RETURN_OK; +} + +#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(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): { + + } + } + return HasReturnvaluesIF::RETURN_OK; +} + +#endif /* OBSW_ADIS16507_LINUX_COM_IF == 1 */ diff --git a/mission/devices/GyroADIS16507Handler.h b/mission/devices/GyroADIS16507Handler.h index f03c49fb..a387675f 100644 --- a/mission/devices/GyroADIS16507Handler.h +++ b/mission/devices/GyroADIS16507Handler.h @@ -1,12 +1,19 @@ #ifndef MISSION_DEVICES_GYROADIS16507HANDLER_H_ #define MISSION_DEVICES_GYROADIS16507HANDLER_H_ -#include +#include "OBSWConfig.h" +#include "fsfw/devicehandlers/DeviceHandlerBase.h" #include "devicedefinitions/GyroADIS16507Definitions.h" +#if OBSW_ADIS16507_LINUX_COM_IF == 1 +class SpiComIF; +class SpiCookie; +#endif + class GyroADIS16507Handler: public DeviceHandlerBase { public: - GyroADIS16507Handler(object_id_t objectId, object_id_t deviceCommunication, CookieIF * comCookie); + GyroADIS16507Handler(object_id_t objectId, object_id_t deviceCommunication, + CookieIF * comCookie); /* DeviceHandlerBase abstract function implementation */ void doStartUp() override; @@ -20,10 +27,15 @@ public: DeviceCommandId_t *foundId, size_t *foundLen) override; ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override; - void setNormalDatapoolEntriesInvalid() override; uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override; + ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) override; private: + std::array commandBuffer; + + AdisGyroPrimaryDataset primaryDataset; + AdisGyroConfigDataset configDataset; enum class InternalState { STARTUP, @@ -32,6 +44,14 @@ private: InternalState internalState = InternalState::STARTUP; bool commandExecuted = false; + + void prepareReadCommand(uint8_t* regList, size_t len); + +#if OBSW_ADIS16507_LINUX_COM_IF == 1 + static ReturnValue_t spiSendCallback(SpiComIF* comIf, SpiCookie *cookie, + const uint8_t *sendData, size_t sendLen, void* args); +#endif + }; diff --git a/mission/devices/GyroL3GD20Handler.cpp b/mission/devices/GyroL3GD20Handler.cpp index 850be83c..35899035 100644 --- a/mission/devices/GyroL3GD20Handler.cpp +++ b/mission/devices/GyroL3GD20Handler.cpp @@ -65,8 +65,8 @@ ReturnValue_t GyroHandlerL3GD20H::buildTransitionDeviceCommand(DeviceCommandId_t } default: /* Might be a configuration error. */ - sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown " - << "internal state!" << std::endl; + sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown internal state!" << + std::endl; return HasReturnvaluesIF::RETURN_OK; } return HasReturnvaluesIF::RETURN_OK; diff --git a/mission/devices/devicedefinitions/GyroADIS16507Definitions.h b/mission/devices/devicedefinitions/GyroADIS16507Definitions.h index 311fd8bc..bacc29cf 100644 --- a/mission/devices/devicedefinitions/GyroADIS16507Definitions.h +++ b/mission/devices/devicedefinitions/GyroADIS16507Definitions.h @@ -1,11 +1,122 @@ #ifndef MISSION_DEVICES_DEVICEDEFINITIONS_GYROADIS16507DEFINITIONS_H_ #define MISSION_DEVICES_DEVICEDEFINITIONS_GYROADIS16507DEFINITIONS_H_ +#include "fsfw/datapoollocal/StaticLocalDataSet.h" + #include namespace ADIS16507 { static constexpr size_t MAXIMUM_REPLY_SIZE = 64; + +static constexpr uint32_t GYRO_RANGE = 125; +static constexpr uint32_t ACCELEROMETER_RANGE = 392; + +static constexpr uint16_t PROD_ID = 16507; + +static constexpr std::array BURST_READ_ENABLE = {0x68, 0x00}; + +static constexpr uint8_t DIAG_STAT_REG = 0x02; +static constexpr uint8_t FILTER_CTRL_REG = 0x5c; +static constexpr uint8_t MSC_CTRL_REG = 0x60; +static constexpr uint8_t DEC_RATE_REG = 0x64; +static constexpr uint8_t GLOB_CMD = 0x68; +static constexpr uint8_t PROD_ID_REG = 0x72; + +static constexpr DeviceCommandId_t READ_SENSOR_DATA = 0; +static constexpr DeviceCommandId_t READ_OUT_CONFIG = 1; +static constexpr DeviceCommandId_t SELF_TEST_SENSORS = 2; +static constexpr DeviceCommandId_t SELF_TEST_MEMORY = 3; +static constexpr DeviceCommandId_t UPDATE_NV_CONFIGURATION = 4; +static constexpr DeviceCommandId_t SELECT_BURST_READ_MODE = 5; + +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 size_t CONFIG_READOUT_SIZE = 8; +static constexpr size_t SENSOR_READOUT_SIZE = 22; + +static constexpr uint32_t ADIS_DATASET_ID = READ_SENSOR_DATA; +static constexpr uint32_t ADIS_CFG_DATASET_ID = READ_OUT_CONFIG; + +enum PrimaryPoolIds: lp_id_t { + ANG_VELOC_X, + ANG_VELOC_Y, + ANG_VELOC_Z, + ACCELERATION_X, + ACCELERATION_Y, + ACCELERATION_Z, + TEMPERATURE +}; + +enum ConfigPoolIds: lp_id_t { + FILTER_SETTINGS, + MSC_CTRL_REGISTER, + DEC_RATE_REGISTER, +}; + +enum FilterSettings: uint8_t { + NO_FILTER = 0, + TWO_TAPS = 1, + FOUR_TAPS = 2, + EIGHT_TAPS = 3, + SIXTEEN_TAPS = 4, + THIRTYTWO_TAPS = 5, + SIXTYFOUR_TAPS = 6 +}; + } +class AdisGyroPrimaryDataset: public StaticLocalDataSet<7 * sizeof(float)> { +public: + + /** Constructor for data users like controllers */ + AdisGyroPrimaryDataset(object_id_t adisId): + StaticLocalDataSet(sid_t(adisId, ADIS16507::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); + lp_var_t angVelocY = lp_var_t(sid.objectId, + ADIS16507::ANG_VELOC_Y, this); + lp_var_t angVelocZ = lp_var_t(sid.objectId, + ADIS16507::ANG_VELOC_Z, this); + lp_var_t accelX = lp_var_t(sid.objectId, + ADIS16507::ACCELERATION_X, this); + lp_var_t accelY = lp_var_t(sid.objectId, + ADIS16507::ACCELERATION_Y, this); + lp_var_t accelZ = lp_var_t(sid.objectId, + ADIS16507::ACCELERATION_Z, this); + lp_var_t temperature = lp_var_t(sid.objectId, + ADIS16507::TEMPERATURE, this); +private: + + friend class GyroADIS16507Handler; + /** Constructor for the data creator */ + AdisGyroPrimaryDataset(HasLocalDataPoolIF* hkOwner): + StaticLocalDataSet(hkOwner, ADIS16507::ADIS_DATASET_ID) {} +}; + +class AdisGyroConfigDataset: public StaticLocalDataSet<32> { +public: + + /** Constructor for data users like controllers */ + AdisGyroConfigDataset(object_id_t adisId): + StaticLocalDataSet(sid_t(adisId, ADIS16507::ADIS_DATASET_ID)) { + setAllVariablesReadOnly(); + } + + 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); +private: + friend class GyroADIS16507Handler; + /** Constructor for the data creator */ + AdisGyroConfigDataset(HasLocalDataPoolIF* hkOwner): + StaticLocalDataSet(hkOwner, ADIS16507::ADIS_CFG_DATASET_ID) {} +}; + #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GYROADIS16507DEFINITIONS_H_ */ diff --git a/mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h b/mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h index b8b2463d..88940ad7 100644 --- a/mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h +++ b/mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h @@ -142,7 +142,7 @@ enum MgmPoolIds: lp_id_t { TEMPERATURE_CELCIUS }; -class MgmPrimaryDataset: public StaticLocalDataSet<3 * sizeof(float)> { +class MgmPrimaryDataset: public StaticLocalDataSet<4 * sizeof(float)> { public: MgmPrimaryDataset(HasLocalDataPoolIF* hkOwner): StaticLocalDataSet(hkOwner, MGM_DATA_SET_ID) {}