From 0db53f44c33a63a7596365f4bdbb19bd5800e656 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 7 Mar 2021 14:06:29 +0100 Subject: [PATCH] L3DG20H device handler tested --- bsp_q7s/InitMission.cpp | 3 +- bsp_rpi/InitMission.cpp | 3 +- bsp_rpi/ObjectFactory.cpp | 35 +++++---- bsp_rpi/boardconfig/rpi_config.h | 4 +- fsfwconfig/CMakeLists.txt | 2 +- fsfwconfig/OBSWConfig.h | 22 +++--- fsfwconfig/devices/spi.h | 27 +++++++ ...Factory.cpp => pollingSequenceFactory.cpp} | 15 +++- ...enceFactory.h => pollingSequenceFactory.h} | 1 + linux/boardtest/SpiTestClass.cpp | 8 +- linux/boardtest/SpiTestClass.h | 2 +- linux/spi/SpiComIF.cpp | 6 +- linux/spi/SpiComIF.h | 2 +- linux/spi/SpiCookie.cpp | 14 +++- linux/spi/SpiCookie.h | 14 +++- linux/spi/spiDefinitions.h | 2 +- mission/devices/GyroL3GD20Handler.cpp | 77 +++++++++++++++---- mission/devices/GyroL3GD20Handler.h | 21 +++-- mission/devices/MGMHandlerLIS3MDL.cpp | 7 +- mission/devices/MGMHandlerRM3100.cpp | 47 ++++++----- mission/devices/MGMHandlerRM3100.h | 10 +-- .../devicedefinitions/GyroL3GD20Definitions.h | 48 +++++++----- .../MGMHandlerLIS3Definitions.h | 3 + .../MGMHandlerRM3100Definitions.h | 3 + 24 files changed, 260 insertions(+), 116 deletions(-) create mode 100644 fsfwconfig/devices/spi.h rename fsfwconfig/pollingsequence/{PollingSequenceFactory.cpp => pollingSequenceFactory.cpp} (91%) rename fsfwconfig/pollingsequence/{PollingSequenceFactory.h => pollingSequenceFactory.h} (99%) diff --git a/bsp_q7s/InitMission.cpp b/bsp_q7s/InitMission.cpp index 72240c5b..4fda5f85 100644 --- a/bsp_q7s/InitMission.cpp +++ b/bsp_q7s/InitMission.cpp @@ -11,8 +11,7 @@ #include #include #include -#include - +#include #include /* This is configured for linux without CR */ diff --git a/bsp_rpi/InitMission.cpp b/bsp_rpi/InitMission.cpp index ce2a5940..5788f0f1 100644 --- a/bsp_rpi/InitMission.cpp +++ b/bsp_rpi/InitMission.cpp @@ -3,8 +3,6 @@ #include #include -#include - #include #include @@ -14,6 +12,7 @@ #include #include #include +#include #include diff --git a/bsp_rpi/ObjectFactory.cpp b/bsp_rpi/ObjectFactory.cpp index 8979e701..13cadfb3 100644 --- a/bsp_rpi/ObjectFactory.cpp +++ b/bsp_rpi/ObjectFactory.cpp @@ -1,23 +1,25 @@ #include "ObjectFactory.h" #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include #include #include #include - #include +#include #include #include #include +#include #include #include @@ -26,8 +28,8 @@ #include #include #include -#include -#include +#include + void Factory::setStaticFrameworkObjectIds() { PusServiceBase::packetSource = objects::PUS_PACKET_DISTRIBUTOR; @@ -99,16 +101,23 @@ void ObjectFactory::produce(){ gpioIF->addGpios(gpioCookieAcsBoard); std::string spiDev = "/dev/spidev0.0"; - SpiCookie* spiCookie = new SpiCookie(addresses::MGM_0_LIS3, gpioIds::MGM_0_LIS3_CS, spiDev, 24, - spi::SpiMode::MODE_3, 3'900'000); + SpiCookie* spiCookie = new SpiCookie(addresses::MGM_0_LIS3, gpioIds::MGM_0_LIS3_CS, spiDev, + MGMLIS3MDL::MAX_BUFFER_SIZE, spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED); auto mgmLis3Handler = new MGMHandlerLIS3MDL(objects::MGM_0_LIS3_HANDLER, objects::SPI_COM_IF, spiCookie); mgmLis3Handler->setStartUpImmediately(); - spiCookie = new SpiCookie(addresses::MGM_1_RM3100, gpioIds::MGM_1_RM3100_CS, spiDev, 16, - spi::SpiMode::MODE_3, 976'000); + + spiCookie = new SpiCookie(addresses::MGM_1_RM3100, gpioIds::MGM_1_RM3100_CS, spiDev, + RM3100::MAX_BUFFER_SIZE, spi::DEFAULT_RM3100_MODE, spi::DEFAULT_RM3100_SPEED); auto mgmRm3100Handler = new MGMHandlerRM3100(objects::MGM_1_RM3100_HANDLER, objects::SPI_COM_IF, spiCookie); mgmRm3100Handler->setStartUpImmediately(); + spiCookie = new SpiCookie(addresses::GYRO_1_L3G, gpioIds::GYRO_1_L3G_CS, spiDev, + L3GD20H::MAX_BUFFER_SIZE, spi::DEFAULT_L3G_MODE, spi::DEFAULT_L3G_SPEED); + auto gyroL3gHandler = new GyroHandlerL3GD20H(objects::GYRO_1_L3G_HANDLER, objects::SPI_COM_IF, + spiCookie); + gyroL3gHandler->setStartUpImmediately(); + #endif /* RPI_TEST_ACS_BOARD == 1 */ } diff --git a/bsp_rpi/boardconfig/rpi_config.h b/bsp_rpi/boardconfig/rpi_config.h index be8dac4e..d1acacb6 100644 --- a/bsp_rpi/boardconfig/rpi_config.h +++ b/bsp_rpi/boardconfig/rpi_config.h @@ -7,8 +7,8 @@ #define RPI_LOOPBACK_TEST_GPIO 0 /* Only one of those 2 should be enabled! */ -#define RPI_ADD_SPI_TEST 1 -#define RPI_TEST_ACS_BOARD 0 +#define RPI_ADD_SPI_TEST 0 +#define RPI_TEST_ACS_BOARD 1 /* Adapt these values accordingly */ namespace gpio { diff --git a/fsfwconfig/CMakeLists.txt b/fsfwconfig/CMakeLists.txt index 897789ef..87ddea03 100644 --- a/fsfwconfig/CMakeLists.txt +++ b/fsfwconfig/CMakeLists.txt @@ -1,6 +1,6 @@ target_sources(${TARGET_NAME} PRIVATE ipc/MissionMessageTypes.cpp - pollingsequence/PollingSequenceFactory.cpp + pollingsequence/pollingSequenceFactory.cpp ) target_include_directories(${TARGET_NAME} PUBLIC diff --git a/fsfwconfig/OBSWConfig.h b/fsfwconfig/OBSWConfig.h index ab1526bd..b8fecaac 100644 --- a/fsfwconfig/OBSWConfig.h +++ b/fsfwconfig/OBSWConfig.h @@ -14,20 +14,22 @@ /* These defines should be disabled for mission code but are useful for debugging. */ -#define OBSW_VERBOSE_LEVEL 1 -#define OBSW_PRINT_MISSED_DEADLINES 1 -#define OBSW_ADD_TEST_CODE 1 -#define TEST_LIBGPIOD 0 +#define OBSW_VERBOSE_LEVEL 1 +#define OBSW_PRINT_MISSED_DEADLINES 1 +#define OBSW_ADD_TEST_CODE 1 +#define TEST_LIBGPIOD 0 -#define TE0720 0 +#define TE0720 0 -#define P60DOCK_DEBUG 0 -#define PDU1_DEBUG 0 -#define PDU2_DEBUG 0 -#define ACU_DEBUG 1 +#define P60DOCK_DEBUG 0 +#define PDU1_DEBUG 0 +#define PDU2_DEBUG 0 +#define ACU_DEBUG 1 +/* Can be used to switch device to NORMAL mode immediately */ +#define OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP 1 /* Can be used for low-level debugging of the SPI bus */ -#define FSFW_LINUX_SPI_WIRETAPPING 0 +#define FSFW_LINUX_SPI_WIRETAPPING 0 #ifdef __cplusplus diff --git a/fsfwconfig/devices/spi.h b/fsfwconfig/devices/spi.h new file mode 100644 index 00000000..7085aa4d --- /dev/null +++ b/fsfwconfig/devices/spi.h @@ -0,0 +1,27 @@ +#ifndef FSFWCONFIG_DEVICES_SPI_H_ +#define FSFWCONFIG_DEVICES_SPI_H_ + +#include +#include + +/** + * SPI configuration will be contained here to let the device handlers remain independent + * of SPI specific properties. + */ +namespace spi { + +/* Default values, changing them is not supported for now */ +static constexpr uint32_t DEFAULT_LIS3_SPEED = 3'900'000; +static constexpr spi::SpiModes DEFAULT_LIS3_MODE = spi::SpiModes::MODE_3; + +static constexpr uint32_t DEFAULT_RM3100_SPEED = 976'000; +static constexpr spi::SpiModes DEFAULT_RM3100_MODE = spi::SpiModes::MODE_3; + +static constexpr uint32_t DEFAULT_L3G_SPEED = 3'900'000; +static constexpr spi::SpiModes DEFAULT_L3G_MODE = spi::SpiModes::MODE_3; + +} + + + +#endif /* FSFWCONFIG_DEVICES_SPI_H_ */ diff --git a/fsfwconfig/pollingsequence/PollingSequenceFactory.cpp b/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp similarity index 91% rename from fsfwconfig/pollingsequence/PollingSequenceFactory.cpp rename to fsfwconfig/pollingsequence/pollingSequenceFactory.cpp index 6fd92ec6..04b1367b 100644 --- a/fsfwconfig/pollingsequence/PollingSequenceFactory.cpp +++ b/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp @@ -1,9 +1,11 @@ +#include "pollingSequenceFactory.h" + #include #include #include #include #include -#include + ReturnValue_t pst::pollingSequenceInitDefault(FixedTimeslotTaskIF *thisSequence) { @@ -120,6 +122,17 @@ ReturnValue_t pst::pollingSequenceAcsTest(FixedTimeslotTaskIF *thisSequence) { DeviceHandlerIF::SEND_READ); thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.8, DeviceHandlerIF::GET_READ); + + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0, + DeviceHandlerIF::PERFORM_OPERATION); + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.2, + DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.4, + DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.6, + DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.8, + DeviceHandlerIF::GET_READ); if (thisSequence->checkSequence() != HasReturnvaluesIF::RETURN_OK) { sif::error << "Initialization of ACS Board PST failed" << std::endl; return HasReturnvaluesIF::RETURN_FAILED; diff --git a/fsfwconfig/pollingsequence/PollingSequenceFactory.h b/fsfwconfig/pollingsequence/pollingSequenceFactory.h similarity index 99% rename from fsfwconfig/pollingsequence/PollingSequenceFactory.h rename to fsfwconfig/pollingsequence/pollingSequenceFactory.h index 4003058b..09dd7242 100644 --- a/fsfwconfig/pollingsequence/PollingSequenceFactory.h +++ b/fsfwconfig/pollingsequence/pollingSequenceFactory.h @@ -1,5 +1,6 @@ #ifndef POLLINGSEQUENCEFACTORY_H_ #define POLLINGSEQUENCEFACTORY_H_ + #include class FixedTimeslotTaskIF; diff --git a/linux/boardtest/SpiTestClass.cpp b/linux/boardtest/SpiTestClass.cpp index f3077ad4..3090c115 100644 --- a/linux/boardtest/SpiTestClass.cpp +++ b/linux/boardtest/SpiTestClass.cpp @@ -71,7 +71,7 @@ void SpiTestClass::performRm3100Test(uint8_t mgmId) { } uint32_t rm3100speed = 976'000; uint8_t rm3100revidReg = 0x36; - spi::SpiMode rm3100mode = spi::SpiMode::MODE_3; + spi::SpiModes rm3100mode = spi::SpiModes::MODE_3; #ifdef RASPBERRY_PI std::string deviceName = "/dev/spidev0.0"; @@ -171,7 +171,7 @@ void SpiTestClass::performLis3MdlTest(uint8_t lis3Id) { currentGpioId = gpioIds::MGM_2_LIS3_CS; } uint32_t spiSpeed = 3'900'000; - spi::SpiMode spiMode = spi::SpiMode::MODE_3; + spi::SpiModes spiMode = spi::SpiModes::MODE_3; #ifdef RASPBERRY_PI std::string deviceName = "/dev/spidev0.0"; #else @@ -217,7 +217,7 @@ void SpiTestClass::performL3gTest(uint8_t l3gId) { currentGpioId = gpioIds::GYRO_2_L3G_CS; } uint32_t spiSpeed = 3'900'000; - spi::SpiMode spiMode = spi::SpiMode::MODE_3; + spi::SpiModes spiMode = spi::SpiModes::MODE_3; #ifdef RASPBERRY_PI std::string deviceName = "/dev/spidev0.0"; #else @@ -334,7 +334,7 @@ void SpiTestClass::acsInit() { } } -void SpiTestClass::setSpiSpeedAndMode(int spiFd, spi::SpiMode mode, uint32_t speed) { +void SpiTestClass::setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed) { int mode_test = SPI_MODE_3; int retval = ioctl(spiFd, SPI_IOC_WR_MODE, &mode_test);//reinterpret_cast(&mode)); if(retval != 0) { diff --git a/linux/boardtest/SpiTestClass.h b/linux/boardtest/SpiTestClass.h index 5998022b..c567bc45 100644 --- a/linux/boardtest/SpiTestClass.h +++ b/linux/boardtest/SpiTestClass.h @@ -50,7 +50,7 @@ private: static constexpr uint8_t RM3100_READ_MASK = STM_READ_MASK; static constexpr uint8_t STM_AUTO_INCR_MASK = 0b0100'0000; - void setSpiSpeedAndMode(int spiFd, spi::SpiMode mode, uint32_t speed); + void setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed); void writeStmRegister(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t value, bool autoIncrement); diff --git a/linux/spi/SpiComIF.cpp b/linux/spi/SpiComIF.cpp index f678ce91..cc3b3cce 100644 --- a/linux/spi/SpiComIF.cpp +++ b/linux/spi/SpiComIF.cpp @@ -79,7 +79,7 @@ ReturnValue_t SpiComIF::initializeInterface(CookieIF *cookie) { } size_t spiSpeed = 0; - spi::SpiMode spiMode = spi::SpiMode::MODE_0; + spi::SpiModes spiMode = spi::SpiModes::MODE_0; SpiCookie::UncommonParameters params; spiCookie->getSpiParameters(spiMode, spiSpeed, ¶ms); @@ -163,7 +163,7 @@ ReturnValue_t SpiComIF::sendMessage(CookieIF *cookie, const uint8_t *sendData, s if(fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) { return OPENING_FILE_FAILED; } - spi::SpiMode spiMode = spi::SpiMode::MODE_0; + spi::SpiModes spiMode = spi::SpiModes::MODE_0; uint32_t spiSpeed = 0; spiCookie->getSpiParameters(spiMode, spiSpeed, nullptr); setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed); @@ -308,7 +308,7 @@ ReturnValue_t SpiComIF::getReadBuffer(address_t spiAddress, uint8_t** buffer) { return HasReturnvaluesIF::RETURN_OK; } -void SpiComIF::setSpiSpeedAndMode(int spiFd, spi::SpiMode mode, uint32_t speed) { +void SpiComIF::setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed) { int retval = ioctl(spiFd, SPI_IOC_WR_MODE, reinterpret_cast(&mode)); if(retval != 0) { utility::handleIoctlError("SpiTestClass::performRm3100Test: Setting SPI mode failed!"); diff --git a/linux/spi/SpiComIF.h b/linux/spi/SpiComIF.h index cbcaa4e0..0e27a595 100644 --- a/linux/spi/SpiComIF.h +++ b/linux/spi/SpiComIF.h @@ -57,7 +57,7 @@ private: ReturnValue_t getReadBuffer(address_t spiAddress, uint8_t** buffer); - void setSpiSpeedAndMode(int spiFd, spi::SpiMode mode, uint32_t speed); + void setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed); }; #endif /* LINUX_SPI_SPICOMIF_H_ */ diff --git a/linux/spi/SpiCookie.cpp b/linux/spi/SpiCookie.cpp index f0b81f67..91117682 100644 --- a/linux/spi/SpiCookie.cpp +++ b/linux/spi/SpiCookie.cpp @@ -1,17 +1,17 @@ #include "SpiCookie.h" SpiCookie::SpiCookie(address_t spiAddress, gpioId_t chipSelect, std::string spiDev, - const size_t maxSize, spi::SpiMode spiMode, uint32_t spiSpeed): spiAddress(spiAddress), + const size_t maxSize, spi::SpiModes spiMode, uint32_t spiSpeed): spiAddress(spiAddress), chipSelectPin(chipSelect), spiDevice(spiDev), maxSize(maxSize), spiMode(spiMode), spiSpeed(spiSpeed) { } SpiCookie::SpiCookie(address_t spiAddress, std::string spiDev, const size_t maxSize, - spi::SpiMode spiMode, uint32_t spiSpeed): + spi::SpiModes spiMode, uint32_t spiSpeed): SpiCookie(spiAddress, gpio::NO_GPIO, spiDev, maxSize, spiMode, spiSpeed) { } -void SpiCookie::getSpiParameters(spi::SpiMode& spiMode, uint32_t& spiSpeed, +void SpiCookie::getSpiParameters(spi::SpiModes& spiMode, uint32_t& spiSpeed, UncommonParameters* parameters) const { spiMode = this->spiMode; spiSpeed = this->spiSpeed; @@ -97,3 +97,11 @@ void SpiCookie::assignTransferSize(size_t transferSize) { size_t SpiCookie::getCurrentTransferSize() const { return spiTransferStruct.len; } + +void SpiCookie::setSpiSpeed(uint32_t newSpeed) { + this->spiSpeed = newSpeed; +} + +void SpiCookie::setSpiMode(spi::SpiModes newMode) { + this->spiMode = newMode; +} diff --git a/linux/spi/SpiCookie.h b/linux/spi/SpiCookie.h index 42004993..59d0e206 100644 --- a/linux/spi/SpiCookie.h +++ b/linux/spi/SpiCookie.h @@ -19,19 +19,25 @@ public: * @param maxSize */ SpiCookie(address_t spiAddress, gpioId_t chipSelect, std::string spiDev, - const size_t maxReplySize, spi::SpiMode spiMode, uint32_t spiSpeed); + const size_t maxReplySize, spi::SpiModes spiMode, uint32_t spiSpeed); /** * Like constructor above, but without a dedicated GPIO CS. Can be used for hardware * slave select or if CS logic is performed with decoders. */ SpiCookie(address_t spiAddress, std::string spiDev, const size_t maxReplySize, - spi::SpiMode spiMode, uint32_t spiSpeed); + spi::SpiModes spiMode, uint32_t spiSpeed); address_t getSpiAddress() const; std::string getSpiDevice() const; gpioId_t getChipSelectPin() const; size_t getMaxBufferSize() const; + + /** Enables changing SPI speed at run-time */ + void setSpiSpeed(uint32_t newSpeed); + /** Enables changing the SPI mode at run-time */ + void setSpiMode(spi::SpiModes newMode); + /** * True if SPI transfers should be performed in full duplex mode * @return @@ -81,7 +87,7 @@ public: void setCsHigh(bool enable); void setBitsPerWord(uint8_t bitsPerWord); - void getSpiParameters(spi::SpiMode& spiMode, uint32_t& spiSpeed, + void getSpiParameters(spi::SpiModes& spiMode, uint32_t& spiSpeed, UncommonParameters* parameters = nullptr) const; /** @@ -100,7 +106,7 @@ private: std::string spiDevice; const size_t maxSize; - spi::SpiMode spiMode; + spi::SpiModes spiMode; uint32_t spiSpeed; bool halfDuplex = false; diff --git a/linux/spi/spiDefinitions.h b/linux/spi/spiDefinitions.h index b4310697..e8c48147 100644 --- a/linux/spi/spiDefinitions.h +++ b/linux/spi/spiDefinitions.h @@ -5,7 +5,7 @@ namespace spi { -enum SpiMode: uint8_t { +enum SpiModes: uint8_t { MODE_0, MODE_1, MODE_2, diff --git a/mission/devices/GyroL3GD20Handler.cpp b/mission/devices/GyroL3GD20Handler.cpp index 279edf73..abf9731c 100644 --- a/mission/devices/GyroL3GD20Handler.cpp +++ b/mission/devices/GyroL3GD20Handler.cpp @@ -1,25 +1,42 @@ -#include +#include "GyroL3GD20Handler.h" +#include + #include GyroHandlerL3GD20H::GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie): DeviceHandlerBase(objectId, deviceCommunication, comCookie), dataset(this) { +#if OBSW_VERBOSE_LEVEL >= 1 + debugDivider = new PeriodicOperationDivider(5); +#endif } GyroHandlerL3GD20H::~GyroHandlerL3GD20H() {} void GyroHandlerL3GD20H::doStartUp() { - if(internalState == InternalState::STATE_NONE) { - internalState = InternalState::STATE_CONFIGURE; + if(internalState == InternalState::NONE) { + internalState = InternalState::CONFIGURE; } - if(internalState == InternalState::STATE_CONFIGURE) { + if(internalState == InternalState::CONFIGURE) { if(commandExecuted) { - internalState = InternalState::STATE_NORMAL; + internalState = InternalState::CHECK_REGS; commandExecuted = false; } } + + if(internalState == InternalState::CHECK_REGS) { + if(commandExecuted) { + internalState = InternalState::NORMAL; +#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1 + setMode(MODE_NORMAL); +#else + setMode(_MODE_TO_ON); +#endif + commandExecuted = false; + } + } } void GyroHandlerL3GD20H::doShutDown() { @@ -28,11 +45,11 @@ void GyroHandlerL3GD20H::doShutDown() { ReturnValue_t GyroHandlerL3GD20H::buildTransitionDeviceCommand(DeviceCommandId_t *id) { switch(internalState) { - case(InternalState::STATE_NONE): - case(InternalState::STATE_NORMAL): { + case(InternalState::NONE): + case(InternalState::NORMAL): { return HasReturnvaluesIF::RETURN_OK; } - case(InternalState::STATE_CONFIGURE): { + case(InternalState::CONFIGURE): { *id = L3GD20H::CONFIGURE_CTRL_REGS; uint8_t command [5]; command[0] = L3GD20H::CTRL_REG_1_VAL; @@ -41,10 +58,13 @@ ReturnValue_t GyroHandlerL3GD20H::buildTransitionDeviceCommand(DeviceCommandId_t command[3] = L3GD20H::CTRL_REG_4_VAL; command[4] = L3GD20H::CTRL_REG_5_VAL; return buildCommandFromCommand(*id, command, 5); - break; + } + case(InternalState::CHECK_REGS): { + *id = L3GD20H::READ_REGS; + return buildCommandFromCommand(*id, nullptr, 0); } default: - // might be a configuration error. + /* Might be a configuration error. */ sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown " << "internal state!" << std::endl; return HasReturnvaluesIF::RETURN_OK; @@ -122,11 +142,11 @@ ReturnValue_t GyroHandlerL3GD20H::buildCommandFromCommand( ReturnValue_t GyroHandlerL3GD20H::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) { - // SPI, ID will always be the one of the last sent command. + /* For SPI, the ID will always be the one of the last sent command. */ *foundId = this->getPendingCommand(); *foundLen = this->rawPacketLen; - // Data with SPI Interface has always this answer + /* Data with SPI Interface has always this answer */ if (start[0] == 0b11111111) { return HasReturnvaluesIF::RETURN_OK; } @@ -138,6 +158,7 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id, ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; switch(id) { case(L3GD20H::CONFIGURE_CTRL_REGS): { + commandExecuted = true; break; } case(L3GD20H::READ_CTRL_REGS): { @@ -147,18 +168,23 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id, commandExecuted = true; } else { - // Attempt reconfiguration. - internalState = InternalState::STATE_CONFIGURE; + /* Attempt reconfiguration. */ + internalState = InternalState::CONFIGURE; return DeviceHandlerIF::DEVICE_REPLY_INVALID; } break; } - case(L3GD20H::READ_START): { + case(L3GD20H::READ_REGS): { if(packet[1] != ctrlReg1Value and packet[2] != ctrlReg2Value and packet[3] != ctrlReg3Value and packet[4] != ctrlReg4Value and packet[5] != ctrlReg5Value) { return DeviceHandlerIF::DEVICE_REPLY_INVALID; } + else { + if(internalState == InternalState::CHECK_REGS) { + commandExecuted = true; + } + } statusReg = packet[L3GD20H::STATUS_IDX]; @@ -171,6 +197,23 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id, int8_t temperaturOffset = (-1) * packet[L3GD20H::TEMPERATURE_IDX]; float temperature = 25.0 + temperaturOffset; +#if OBSW_VERBOSE_LEVEL >= 1 + if(debugDivider->checkAndIncrement()) { + /* Set terminal to utf-8 if there is an issue with micro printout. */ +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::info << "GyroHandlerL3GD20H: Angular velocities in degrees per second:" << + std::endl; + sif::info << "X: " << angVelocX << " \xC2\xB0" << std::endl; + sif::info << "Y: " << angVelocY << " \xC2\xB0" << std::endl; + sif::info << "Z: " << angVelocZ << " \xC2\xB0" << std::endl; +#else + sif::printInfo("GyroHandlerL3GD20H: Angular velocities in degrees per second:\n"); + sif::printInfo("X: %f " "\xC2\xB0" "T\n", angVelocX); + sif::printInfo("Y: %f " "\xC2\xB0" "T\n", angVelocY); + sif::printInfo("Z: %f " "\xC2\xB0" "T\n", angVelocZ); +#endif + } +#endif PoolReadGuard readSet(&dataset); if(readSet.getReadResult() == HasReturnvaluesIF::RETURN_OK) { @@ -190,7 +233,7 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id, uint32_t GyroHandlerL3GD20H::getTransitionDelayMs(Mode_t from, Mode_t to) { - return 20000; + return 25000; } ReturnValue_t GyroHandlerL3GD20H::initializeLocalDataPool( @@ -213,5 +256,5 @@ void GyroHandlerL3GD20H::fillCommandAndReplyMap() { } void GyroHandlerL3GD20H::modeChanged() { - internalState = InternalState::STATE_NONE; + internalState = InternalState::NONE; } diff --git a/mission/devices/GyroL3GD20Handler.h b/mission/devices/GyroL3GD20Handler.h index 952ff194..442e28f8 100644 --- a/mission/devices/GyroL3GD20Handler.h +++ b/mission/devices/GyroL3GD20Handler.h @@ -1,8 +1,12 @@ #ifndef MISSION_DEVICES_GYROL3GD20HANDLER_H_ #define MISSION_DEVICES_GYROL3GD20HANDLER_H_ -#include #include "devicedefinitions/GyroL3GD20Definitions.h" +#include + +#include +#include + /** * @brief Device Handler for the L3GD20H gyroscope sensor @@ -43,14 +47,15 @@ protected: LocalDataPoolManager &poolManager) override; private: - L3GD20H::GyroPrimaryDataset dataset; + GyroPrimaryDataset dataset; enum class InternalState { - STATE_NONE, - STATE_CONFIGURE, - STATE_NORMAL + NONE, + CONFIGURE, + CHECK_REGS, + NORMAL }; - InternalState internalState = InternalState::STATE_NONE; + InternalState internalState = InternalState::NONE; bool commandExecuted = false; uint8_t statusReg = 0; @@ -64,6 +69,10 @@ private: uint8_t commandBuffer[L3GD20H::READ_LEN + 1]; float scaleFactor = static_cast(L3GD20H::RANGE_DPS_00) / INT16_MAX; + +#if OBSW_VERBOSE_LEVEL >= 1 + PeriodicOperationDivider* debugDivider = nullptr; +#endif }; diff --git a/mission/devices/MGMHandlerLIS3MDL.cpp b/mission/devices/MGMHandlerLIS3MDL.cpp index d2573f73..8fca515a 100644 --- a/mission/devices/MGMHandlerLIS3MDL.cpp +++ b/mission/devices/MGMHandlerLIS3MDL.cpp @@ -7,7 +7,7 @@ MGMHandlerLIS3MDL::MGMHandlerLIS3MDL(object_id_t objectId, DeviceHandlerBase(objectId, deviceCommunication, comCookie), dataset(this) { #if OBSW_VERBOSE_LEVEL >= 1 - debugDivider = new PeriodicOperationDivider(10); + debugDivider = new PeriodicOperationDivider(5); #endif /* Set to default values right away. */ registers[0] = MGMLIS3MDL::CTRL_REG1_DEFAULT; @@ -44,8 +44,11 @@ void MGMHandlerLIS3MDL::doStartUp() { /* Set up cached registers which will be used to configure the MGM. */ if(commandExecuted) { commandExecuted = false; - /* Replace _MODE_TO_ON with MODE_NORMAL to jump to normal mode quickly */ +#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1 + setMode(MODE_NORMAL); +#else setMode(_MODE_TO_ON); +#endif } break; } diff --git a/mission/devices/MGMHandlerRM3100.cpp b/mission/devices/MGMHandlerRM3100.cpp index 2f6cef0e..ae97f6ff 100644 --- a/mission/devices/MGMHandlerRM3100.cpp +++ b/mission/devices/MGMHandlerRM3100.cpp @@ -12,7 +12,7 @@ MGMHandlerRM3100::MGMHandlerRM3100(object_id_t objectId, DeviceHandlerBase(objectId, deviceCommunication, comCookie), primaryDataset(this) { #if OBSW_VERBOSE_LEVEL >= 1 - debugDivider = new PeriodicOperationDivider(10); + debugDivider = new PeriodicOperationDivider(5); #endif } @@ -20,28 +20,34 @@ MGMHandlerRM3100::~MGMHandlerRM3100() {} void MGMHandlerRM3100::doStartUp() { switch(internalState) { - case(InternalState::STATE_NONE): { - internalState = InternalState::STATE_CONFIGURE_CMM; + case(InternalState::NONE): { + internalState = InternalState::CONFIGURE_CMM; break; } - case(InternalState::STATE_CONFIGURE_CMM): { - internalState = InternalState::STATE_READ_CMM; + case(InternalState::CONFIGURE_CMM): { + internalState = InternalState::READ_CMM; break; } - case(InternalState::STATE_READ_CMM): { + case(InternalState::READ_CMM): { if(commandExecuted) { internalState = InternalState::STATE_CONFIGURE_TMRC; } break; } case(InternalState::STATE_CONFIGURE_TMRC): { - internalState = InternalState::STATE_READ_TMRC; + if(commandExecuted) { + internalState = InternalState::STATE_READ_TMRC; + } break; } case(InternalState::STATE_READ_TMRC): { if(commandExecuted) { - internalState = InternalState::STATE_NORMAL; + internalState = InternalState::NORMAL; +#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1 + setMode(MODE_NORMAL); +#else setMode(_MODE_TO_ON); +#endif } break; } @@ -58,15 +64,15 @@ void MGMHandlerRM3100::doShutDown() { ReturnValue_t MGMHandlerRM3100::buildTransitionDeviceCommand( DeviceCommandId_t *id) { switch(internalState) { - case(InternalState::STATE_NONE): - case(InternalState::STATE_NORMAL): { + case(InternalState::NONE): + case(InternalState::NORMAL): { return HasReturnvaluesIF::RETURN_OK; } - case(InternalState::STATE_CONFIGURE_CMM): { + case(InternalState::CONFIGURE_CMM): { *id = RM3100::CONFIGURE_CMM; break; } - case(InternalState::STATE_READ_CMM): { + case(InternalState::READ_CMM): { *id = RM3100::READ_CMM; break; } @@ -79,9 +85,9 @@ ReturnValue_t MGMHandlerRM3100::buildTransitionDeviceCommand( break; } default: - // might be a configuration error. - sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown " - << "internal state!" << std::endl; + /* Might be a configuration error. */ + sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown internal state!" << + std::endl; return HasReturnvaluesIF::RETURN_OK; } @@ -164,6 +170,9 @@ ReturnValue_t MGMHandlerRM3100::interpretDeviceReply( case(RM3100::CONFIGURE_CYCLE_COUNT): case(RM3100::CONFIGURE_TMRC): { /* We can only check whether write was successful with read operation. */ + if(mode == _MODE_START_UP) { + commandExecuted = true; + } break; } case(RM3100::READ_CMM): { @@ -171,12 +180,12 @@ ReturnValue_t MGMHandlerRM3100::interpretDeviceReply( /* We clear the seventh bit in any case * because this one is zero sometimes for some reason */ bitutil::bitClear(&cmmValue, 6); - if(cmmValue == cmmRegValue) { + if(cmmValue == cmmRegValue and internalState == InternalState::READ_CMM) { commandExecuted = true; } else { /* Attempt reconfiguration. */ - internalState = InternalState::STATE_CONFIGURE_CMM; + internalState = InternalState::CONFIGURE_CMM; return DeviceHandlerIF::DEVICE_REPLY_INVALID; } break; @@ -304,7 +313,7 @@ void MGMHandlerRM3100::fillCommandAndReplyMap() { } void MGMHandlerRM3100::modeChanged(void) { - internalState = InternalState::STATE_NONE; + internalState = InternalState::NONE; } ReturnValue_t MGMHandlerRM3100::initializeLocalDataPool( @@ -316,7 +325,7 @@ ReturnValue_t MGMHandlerRM3100::initializeLocalDataPool( } uint32_t MGMHandlerRM3100::getTransitionDelayMs(Mode_t from, Mode_t to) { - return 10000; + return 15000; } ReturnValue_t MGMHandlerRM3100::handleDataReadout(const uint8_t *packet) { diff --git a/mission/devices/MGMHandlerRM3100.h b/mission/devices/MGMHandlerRM3100.h index 968953bf..ce107637 100644 --- a/mission/devices/MGMHandlerRM3100.h +++ b/mission/devices/MGMHandlerRM3100.h @@ -61,18 +61,18 @@ protected: private: enum class InternalState { - STATE_NONE, - STATE_CONFIGURE_CMM, - STATE_READ_CMM, + NONE, + CONFIGURE_CMM, + READ_CMM, // The cycle count states are propably not going to be used because // the default cycle count will be used. STATE_CONFIGURE_CYCLE_COUNT, STATE_READ_CYCLE_COUNT, STATE_CONFIGURE_TMRC, STATE_READ_TMRC, - STATE_NORMAL + NORMAL }; - InternalState internalState = InternalState::STATE_NONE; + InternalState internalState = InternalState::NONE; bool commandExecuted = false; RM3100::Rm3100PrimaryDataset primaryDataset; diff --git a/mission/devices/devicedefinitions/GyroL3GD20Definitions.h b/mission/devices/devicedefinitions/GyroL3GD20Definitions.h index ee1cef30..30669b1d 100644 --- a/mission/devices/devicedefinitions/GyroL3GD20Definitions.h +++ b/mission/devices/devicedefinitions/GyroL3GD20Definitions.h @@ -1,11 +1,15 @@ #ifndef MISSION_DEVICES_DEVICEDEFINITIONS_GYROL3GD20DEFINITIONS_H_ #define MISSION_DEVICES_DEVICEDEFINITIONS_GYROL3GD20DEFINITIONS_H_ +#include #include #include namespace L3GD20H { +/* Actual size is 15 but we round up a bit */ +static constexpr size_t MAX_BUFFER_SIZE = 16; + static constexpr uint8_t READ_MASK = 0b1000'0000; static constexpr uint8_t AUTO_INCREMENT_MASK = 0b0100'0000; @@ -104,27 +108,33 @@ enum GyroPoolIds: lp_id_t { TEMPERATURE }; -class GyroPrimaryDataset: public StaticLocalDataSet<3 * sizeof(float)> { -public: - GyroPrimaryDataset(HasLocalDataPoolIF* hkOwner): - StaticLocalDataSet(hkOwner, GYRO_DATASET_ID) {} - - GyroPrimaryDataset(object_id_t mgmId): - StaticLocalDataSet(sid_t(mgmId, GYRO_DATASET_ID)) {} - - // Angular velocities in degrees per second (DPS) - lp_var_t angVelocX = lp_var_t(sid.objectId, - ANG_VELOC_X, this); - lp_var_t angVelocY = lp_var_t(sid.objectId, - ANG_VELOC_Y, this); - lp_var_t angVelocZ = lp_var_t(sid.objectId, - ANG_VELOC_Z, this); - lp_var_t temperature = lp_var_t(sid.objectId, - TEMPERATURE, this); -}; - } +class GyroPrimaryDataset: public StaticLocalDataSet<3 * sizeof(float)> { +public: + + /** Constructor for data users like controllers */ + GyroPrimaryDataset(object_id_t mgmId): + StaticLocalDataSet(sid_t(mgmId, L3GD20H::GYRO_DATASET_ID)) { + setAllVariablesReadOnly(); + } + + /* Angular velocities in degrees per second (DPS) */ + lp_var_t angVelocX = lp_var_t(sid.objectId, + L3GD20H::ANG_VELOC_X, this); + lp_var_t angVelocY = lp_var_t(sid.objectId, + L3GD20H::ANG_VELOC_Y, this); + lp_var_t angVelocZ = lp_var_t(sid.objectId, + L3GD20H::ANG_VELOC_Z, this); + lp_var_t temperature = lp_var_t(sid.objectId, + L3GD20H::TEMPERATURE, this); +private: + + friend class GyroHandlerL3GD20H; + /** Constructor for the data creator */ + GyroPrimaryDataset(HasLocalDataPoolIF* hkOwner): + StaticLocalDataSet(hkOwner, L3GD20H::GYRO_DATASET_ID) {} +}; #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GYROL3GD20DEFINITIONS_H_ */ diff --git a/mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h b/mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h index 0d51cda1..b8b2463d 100644 --- a/mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h +++ b/mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h @@ -15,6 +15,9 @@ enum opMode { LOW, MEDIUM, HIGH, ULTRA }; +/* Actually 15, we just round up a bit */ +static constexpr size_t MAX_BUFFER_SIZE = 16; + static constexpr uint8_t GAUSS_TO_MICROTESLA_FACTOR = 100; static const DeviceCommandId_t SETUP_MGM = 0x00; diff --git a/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h b/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h index 921c9e5f..2856c170 100644 --- a/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h +++ b/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h @@ -9,6 +9,9 @@ namespace RM3100 { +/* Actually 10, we round up a little bit */ +static constexpr size_t MAX_BUFFER_SIZE = 12; + static constexpr uint8_t READ_MASK = 0b1000'0000; /*----------------------------------------------------------------------------*/