From 8284ebec9f41b3748983dcabe12ea8f67e101379 Mon Sep 17 00:00:00 2001 From: Martin Zietz Date: Thu, 24 Jun 2021 12:04:36 +0200 Subject: [PATCH] rw wip --- bsp_q7s/ObjectFactory.cpp | 34 ++++++++++----- bsp_q7s/gpio/gpioCallbacks.cpp | 8 ++-- bsp_q7s/spiCallbacks/rwSpiCallback.cpp | 42 ++++++++++++++----- common/config/spiConf.h | 2 +- linux/fsfwconfig/devices/gpioIds.h | 6 +++ mission/devices/RwHandler.cpp | 10 ++++- mission/devices/RwHandler.h | 6 +++ .../devices/devicedefinitions/RwDefinitions.h | 28 +++++++------ 8 files changed, 95 insertions(+), 41 deletions(-) diff --git a/bsp_q7s/ObjectFactory.cpp b/bsp_q7s/ObjectFactory.cpp index 821d94fd..7507b5e6 100644 --- a/bsp_q7s/ObjectFactory.cpp +++ b/bsp_q7s/ObjectFactory.cpp @@ -552,25 +552,39 @@ void ObjectFactory::produce(void* args){ new PlocHandler(objects::PLOC_HANDLER, objects::UART_COM_IF, plocUartCookie); GpioCookie* gpioCookieRw = new GpioCookie; - GpioCallback* enRw1 = new GpioCallback(std::string("Chip select reaction wheel 1"), gpio::OUT, + GpioCallback* csRw1 = new GpioCallback(std::string("Chip select reaction wheel 1"), gpio::OUT, 1, &gpioCallbacks::spiCsDecoderCallback, gpioComIF); - gpioCookieRw->addGpio(gpioIds::EN_RW1, enRw1); - GpioCallback* enRw2 = new GpioCallback(std::string("Chip select reaction wheel 2"), gpio::OUT, + gpioCookieRw->addGpio(gpioIds::CS_RW1, csRw1); + GpioCallback* csRw2 = new GpioCallback(std::string("Chip select reaction wheel 2"), gpio::OUT, 1, &gpioCallbacks::spiCsDecoderCallback, gpioComIF); - gpioCookieRw->addGpio(gpioIds::EN_RW2, enRw2); - GpioCallback* enRw3 = new GpioCallback(std::string("Chip select reaction wheel 3"), gpio::OUT, + gpioCookieRw->addGpio(gpioIds::CS_RW2, csRw2); + GpioCallback* csRw3 = new GpioCallback(std::string("Chip select reaction wheel 3"), gpio::OUT, 1, &gpioCallbacks::spiCsDecoderCallback, gpioComIF); - gpioCookieRw->addGpio(gpioIds::EN_RW3, enRw3); - GpioCallback* enRw4 = new GpioCallback(std::string("Chip select reaction wheel 4"), gpio::OUT, + gpioCookieRw->addGpio(gpioIds::CS_RW3, csRw3); + GpioCallback* csRw4 = new GpioCallback(std::string("Chip select reaction wheel 4"), gpio::OUT, 1, &gpioCallbacks::spiCsDecoderCallback, gpioComIF); - gpioCookieRw->addGpio(gpioIds::EN_RW4, enRw4); + gpioCookieRw->addGpio(gpioIds::CS_RW4, csRw4); + + GpiodRegular* enRw1 = new GpiodRegular(std::string("gpiochip5"), 7, + std::string("Enable reaction wheel 1"), gpio::OUT, 0); + gpioCookieRw->addGpio(gpioIds::EN_RW1, enRw1); + GpiodRegular* enRw2 = new GpiodRegular(std::string("gpiochip5"), 3, + std::string("Enable reaction wheel 2"), gpio::OUT, 0); + gpioCookieRw->addGpio(gpioIds::EN_RW2, enRw2); + GpiodRegular* enRw3 = new GpiodRegular(std::string("gpiochip5"), 11, + std::string("Enable reaction wheel 3"), gpio::OUT, 0); + gpioCookieRw->addGpio(gpioIds::EN_RW3, enRw3); + GpiodRegular* enRw4 = new GpiodRegular(std::string("gpiochip5"), 6, + std::string("Enable reaction wheel 4"), gpio::OUT, 0); + gpioCookieRw->addGpio(gpioIds::EN_RW4, enRw4); gpioComIF->addGpios(gpioCookieRw); - auto rw1SpiCookie = new SpiCookie(addresses::RW1, gpioIds::EN_RW1, "/dev/spidev2.0", + auto rw1SpiCookie = new SpiCookie(addresses::RW1, gpioIds::CS_RW1, "/dev/spidev2.0", RwDefinitions::MAX_REPLY_SIZE, spi::RW_MODE, spi::RW_SPEED, &rwSpiCallback, nullptr); - auto rwHandler1 = new RwHandler(objects::RW1, objects::SPI_COM_IF, rw1SpiCookie); + auto rwHandler1 = new RwHandler(objects::RW1, objects::SPI_COM_IF, rw1SpiCookie, gpioComIF, + gpioIds::EN_RW1); rwHandler1->setStartUpImmediately(); rw1SpiCookie->setCallbackArgs(rwHandler1); diff --git a/bsp_q7s/gpio/gpioCallbacks.cpp b/bsp_q7s/gpio/gpioCallbacks.cpp index ede2311a..49e22249 100644 --- a/bsp_q7s/gpio/gpioCallbacks.cpp +++ b/bsp_q7s/gpio/gpioCallbacks.cpp @@ -220,22 +220,22 @@ void spiCsDecoderCallback(gpioId_t gpioId, gpio::GpioOperation gpioOp, int value selectY6(); break; } - case(gpioIds::EN_RW1): { + case(gpioIds::CS_RW1): { enableRwDecoder(); selectY0(); break; } - case(gpioIds::EN_RW2): { + case(gpioIds::CS_RW2): { enableRwDecoder(); selectY1(); break; } - case(gpioIds::EN_RW3): { + case(gpioIds::CS_RW3): { enableRwDecoder(); selectY3(); break; } - case(gpioIds::EN_RW4): { + case(gpioIds::CS_RW4): { enableRwDecoder(); selectY4(); break; diff --git a/bsp_q7s/spiCallbacks/rwSpiCallback.cpp b/bsp_q7s/spiCallbacks/rwSpiCallback.cpp index d1abf55e..2ad2d9fd 100644 --- a/bsp_q7s/spiCallbacks/rwSpiCallback.cpp +++ b/bsp_q7s/spiCallbacks/rwSpiCallback.cpp @@ -90,6 +90,7 @@ ReturnValue_t rwSpiCallback(SpiComIF* comIf, SpiCookie *cookie, const uint8_t *s default: writeBuffer[0] = *(sendData + idx); writeSize = 1; + break; } if (write(fileDescriptor, writeBuffer, writeSize) != static_cast(writeSize)) { sif::error << "rwSpiCallback: Write failed!" << std::endl; @@ -139,19 +140,28 @@ ReturnValue_t rwSpiCallback(SpiComIF* comIf, SpiCookie *cookie, const uint8_t *s size_t replyBufferSize = cookie->getMaxBufferSize(); + /** There must be a delay of 20 ms after sending the command */ + usleep(RwDefinitions::SPI_REPLY_DELAY); + + /** Receiving reply data */ uint8_t byteRead = 0; /** Reading the reply frame */ - if(read(fileDescriptor, &byteRead, 1) != 1) { - if(gpioId != gpio::NO_GPIO) { - if (gpioIF->pullHigh(gpioId) != HasReturnvaluesIF::RETURN_OK) { - sif::error << "rwSpiCallback: Failed to pull chip select high" << std::endl; - } - } - if(mutex->unlockMutex() != HasReturnvaluesIF::RETURN_OK) { - sif::error << "rwSpiCallback: Failed to unlock mutex"; - } - sif::error << "rwSpiCallback: Failed to read first byte of reply frame" << std::endl; - return RwHandler::SPI_READ_FAILURE; +// if(read(fileDescriptor, &byteRead, 1) != 1) { +// if(gpioId != gpio::NO_GPIO) { +// if (gpioIF->pullHigh(gpioId) != HasReturnvaluesIF::RETURN_OK) { +// sif::error << "rwSpiCallback: Failed to pull chip select high" << std::endl; +// } +// } +// if(mutex->unlockMutex() != HasReturnvaluesIF::RETURN_OK) { +// sif::error << "rwSpiCallback: Failed to unlock mutex"; +// } +// sif::error << "rwSpiCallback: Failed to read first byte of reply frame" << std::endl; +// return RwHandler::SPI_READ_FAILURE; +// } + + uint8_t readbuffer[10]; + if(read(fileDescriptor, readbuffer, 10) != 10) { + sif::error << "Failed to read all bytes" << std::endl;; } /** First byte must be the start sign 0x7E */ @@ -168,8 +178,18 @@ ReturnValue_t rwSpiCallback(SpiComIF* comIf, SpiCookie *cookie, const uint8_t *s return RwHandler::MISSING_START_SIGN; } + /** + * The reaction wheel responds with empty frames while preparing the reply data. + * However, receiving more than 5 empty frames will be interpreted as an error. + */ + for (int idx = 0; idx < 10; idx++) { + sif::error << "rwSpiCallback: Empty frame timeout"; + return RwHandler::NO_REPLY; + } + size_t decodedFrameLen = 0; for (; decodedFrameLen < replyBufferSize; decodedFrameLen++) { + byteRead = 0; if(read(fileDescriptor, &byteRead, 1) != 1) { sif::error << "rwSpiCallback: Read failed" << std::endl; return RwHandler::SPI_READ_FAILURE; diff --git a/common/config/spiConf.h b/common/config/spiConf.h index d76ac9b8..47600eb3 100644 --- a/common/config/spiConf.h +++ b/common/config/spiConf.h @@ -26,7 +26,7 @@ static constexpr spi::SpiModes DEFAULT_MAX_1227_MODE = spi::SpiModes::MODE_3; static constexpr uint32_t DEFAULT_ADIS16507_SPEED = 976'000; static constexpr spi::SpiModes DEFAULT_ADIS16507_MODE = spi::SpiModes::MODE_3; -static constexpr uint32_t RW_SPEED = 300'000; +static constexpr uint32_t RW_SPEED = 300000; static constexpr spi::SpiModes RW_MODE = spi::SpiModes::MODE_0; } diff --git a/linux/fsfwconfig/devices/gpioIds.h b/linux/fsfwconfig/devices/gpioIds.h index 0a541477..718bc9bf 100644 --- a/linux/fsfwconfig/devices/gpioIds.h +++ b/linux/fsfwconfig/devices/gpioIds.h @@ -74,6 +74,12 @@ namespace gpioIds { EN_RW2, EN_RW3, EN_RW4, + + CS_RW1, + CS_RW2, + CS_RW3, + CS_RW4, + EN_RW_CS }; } diff --git a/mission/devices/RwHandler.cpp b/mission/devices/RwHandler.cpp index c029372b..a80c97cc 100644 --- a/mission/devices/RwHandler.cpp +++ b/mission/devices/RwHandler.cpp @@ -6,7 +6,8 @@ RwHandler::RwHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie, LinuxLibgpioIF* gpioComIF, gpioId_t enableGpio) : - DeviceHandlerBase(objectId, comIF, comCookie), temperatureSet(this), statusSet(this) { + DeviceHandlerBase(objectId, comIF, comCookie), gpioComIF(gpioComIF), enableGpio(enableGpio), + temperatureSet(this), statusSet(this) { if (comCookie == NULL) { sif::error << "RwHandler: Invalid com cookie" << std::endl; } @@ -19,6 +20,9 @@ RwHandler::~RwHandler() { } void RwHandler::doStartUp() { + if(gpioComIF->pullHigh(enableGpio) != RETURN_OK) { + sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to high"; + } #if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1 setMode(MODE_NORMAL); #else @@ -27,7 +31,9 @@ void RwHandler::doStartUp() { } void RwHandler::doShutDown() { - + if(gpioComIF->pullLow(enableGpio) != RETURN_OK) { + sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to low"; + } } ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t * id) { diff --git a/mission/devices/RwHandler.h b/mission/devices/RwHandler.h index f68b57b7..0a9d58d0 100644 --- a/mission/devices/RwHandler.h +++ b/mission/devices/RwHandler.h @@ -3,6 +3,7 @@ #include #include +#include #include /** @@ -43,6 +44,8 @@ public: static const ReturnValue_t INVALID_SUBSTITUTE = MAKE_RETURN_CODE(0xB3); //! [EXPORT] : [COMMENT] HDLC decoding mechanism never receives the end sign 0x7E static const ReturnValue_t MISSING_END_SIGN = MAKE_RETURN_CODE(0xB4); + //! [EXPORT] : [COMMENT] Reaction wheel only responds with empty frames. + static const ReturnValue_t NO_REPLY = MAKE_RETURN_CODE(0xB5); protected: void doStartUp() override; @@ -76,6 +79,9 @@ private: //! [EXPORT] : [COMMENT] Reaction wheel reply has invalid crc static const ReturnValue_t CRC_ERROR = MAKE_RETURN_CODE(0xA4); + LinuxLibgpioIF* gpioComIF = nullptr; + gpioId_t enableGpio = gpio::NO_GPIO; + RwDefinitions::TemperatureSet temperatureSet; RwDefinitions::StatusSet statusSet; diff --git a/mission/devices/devicedefinitions/RwDefinitions.h b/mission/devices/devicedefinitions/RwDefinitions.h index 247b7bc6..23907eb3 100644 --- a/mission/devices/devicedefinitions/RwDefinitions.h +++ b/mission/devices/devicedefinitions/RwDefinitions.h @@ -8,6 +8,8 @@ namespace RwDefinitions { +static const uint32_t SPI_REPLY_DELAY = 70000; //us + enum PoolIds: lp_id_t { TEMPERATURE_C, CURR_SPEED, @@ -16,25 +18,25 @@ enum PoolIds: lp_id_t { CLC_MODE }; -static constexpr DeviceCommandId_t GET_RW_STATUS = 4; -static constexpr DeviceCommandId_t SET_SPEED = 6; -static constexpr DeviceCommandId_t GET_TEMPERATURE = 8; +static const DeviceCommandId_t GET_RW_STATUS = 4; +static const DeviceCommandId_t SET_SPEED = 6; +static const DeviceCommandId_t GET_TEMPERATURE = 8; -static constexpr uint32_t TEMPERATURE_SET_ID = GET_TEMPERATURE; -static constexpr uint32_t STATUS_SET_ID = GET_RW_STATUS; +static const uint32_t TEMPERATURE_SET_ID = GET_TEMPERATURE; +static const uint32_t STATUS_SET_ID = GET_RW_STATUS; -static constexpr size_t SIZE_GET_RW_STATUS = 14; -static constexpr size_t SIZE_SET_SPEED_REPLY = 4; -static constexpr size_t SIZE_GET_TEMPERATURE_REPLY = 8; +static const size_t SIZE_GET_RW_STATUS = 14; +static const size_t SIZE_SET_SPEED_REPLY = 4; +static const size_t SIZE_GET_TEMPERATURE_REPLY = 8; /** Max size when requesting telemetry */ -static constexpr size_t SIZE_GET_TELEMETRY_REPLY = 83; +static const size_t SIZE_GET_TELEMETRY_REPLY = 83; /** Set speed command has maximum size */ -static constexpr size_t MAX_CMD_SIZE = 9; -static constexpr size_t MAX_REPLY_SIZE = SIZE_GET_TELEMETRY_REPLY; +static const size_t MAX_CMD_SIZE = 9; +static const size_t MAX_REPLY_SIZE = SIZE_GET_TELEMETRY_REPLY; -static constexpr uint8_t TEMPERATURE_SET_ENTRIES = 1; -static constexpr uint8_t STATUS_SET_ENTRIES = 4; +static const uint8_t TEMPERATURE_SET_ENTRIES = 1; +static const uint8_t STATUS_SET_ENTRIES = 4; /** * @brief This dataset can be used to store the temperature of a reaction wheel.