From b5fdeb93bb70f26a1ee19c567d937eda66e9b0c8 Mon Sep 17 00:00:00 2001 From: "Jakob.Meier" Date: Thu, 24 Jun 2021 17:32:42 +0200 Subject: [PATCH] get temperature and status of reaction wheel --- bsp_q7s/spiCallbacks/rwSpiCallback.cpp | 127 ++++++------------ bsp_q7s/spiCallbacks/rwSpiCallback.h | 9 ++ fsfw_hal | 2 +- mission/devices/RwHandler.cpp | 39 ++++-- .../devices/devicedefinitions/RwDefinitions.h | 9 ++ 5 files changed, 89 insertions(+), 97 deletions(-) diff --git a/bsp_q7s/spiCallbacks/rwSpiCallback.cpp b/bsp_q7s/spiCallbacks/rwSpiCallback.cpp index 2ad2d9fd..41ba70d9 100644 --- a/bsp_q7s/spiCallbacks/rwSpiCallback.cpp +++ b/bsp_q7s/spiCallbacks/rwSpiCallback.cpp @@ -62,14 +62,7 @@ ReturnValue_t rwSpiCallback(SpiComIF* comIf, SpiCookie *cookie, const uint8_t *s if (write(fileDescriptor, writeBuffer, writeSize) != static_cast(writeSize)) { sif::error << "rwSpiCallback: Write failed!" << std::endl; - 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" << std::endl;; - } + closeSpi(gpioId, gpioIF, mutex); return RwHandler::SPI_WRITE_FAILURE; } @@ -94,14 +87,7 @@ ReturnValue_t rwSpiCallback(SpiComIF* comIf, SpiCookie *cookie, const uint8_t *s } if (write(fileDescriptor, writeBuffer, writeSize) != static_cast(writeSize)) { sif::error << "rwSpiCallback: Write failed!" << std::endl; - 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"; - } + closeSpi(gpioId, gpioIF, mutex); return RwHandler::SPI_WRITE_FAILURE; } idx++; @@ -113,28 +99,14 @@ ReturnValue_t rwSpiCallback(SpiComIF* comIf, SpiCookie *cookie, const uint8_t *s if (write(fileDescriptor, writeBuffer, writeSize) != static_cast(writeSize)) { sif::error << "rwSpiCallback: Write failed!" << std::endl; - 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"; - } + closeSpi(gpioId, gpioIF, mutex); return RwHandler::SPI_WRITE_FAILURE; } uint8_t* rxBuf = nullptr; result = comIf->getReadBuffer(cookie->getSpiAddress(), &rxBuf); if(result != HasReturnvaluesIF::RETURN_OK) { - 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"; - } + closeSpi(gpioId, gpioIF, mutex); return result; } @@ -143,56 +115,40 @@ ReturnValue_t rwSpiCallback(SpiComIF* comIf, SpiCookie *cookie, const uint8_t *s /** 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; -// } - - 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 */ - if (byteRead != 0x7E) { - sif::error << "rwSpiCallback: First byte of reply is not 0x7E" << std::endl; - 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"; - } - 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. */ + uint8_t byteRead = 0; for (int idx = 0; idx < 10; idx++) { - sif::error << "rwSpiCallback: Empty frame timeout"; - return RwHandler::NO_REPLY; + if(read(fileDescriptor, &byteRead, 1) != 1) { + sif::error << "rwSpiCallback: Read failed" << std::endl; + closeSpi(gpioId, gpioIF, mutex); + return RwHandler::SPI_READ_FAILURE; + } + + if (byteRead != 0x7E) { + break; + } + + if (idx == 9) { + sif::error << "rwSpiCallback: Empty frame timeout" << std::endl; + closeSpi(gpioId, gpioIF, mutex); + 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; + while(decodedFrameLen < replyBufferSize) { + + /** First byte already read in */ + if (decodedFrameLen != 0) { + byteRead = 0; + if(read(fileDescriptor, &byteRead, 1) != 1) { + sif::error << "rwSpiCallback: Read failed" << std::endl; + result = RwHandler::SPI_READ_FAILURE; + break; + } } if (byteRead == 0x7E) { @@ -217,6 +173,7 @@ ReturnValue_t rwSpiCallback(SpiComIF* comIf, SpiCookie *cookie, const uint8_t *s } else { sif::error << "rwSpiCallback: Invalid substitute" << std::endl; + closeSpi(gpioId, gpioIF, mutex); result = RwHandler::INVALID_SUBSTITUTE; break; } @@ -250,16 +207,18 @@ ReturnValue_t rwSpiCallback(SpiComIF* comIf, SpiCookie *cookie, const uint8_t *s cookie->assignTransferSize(decodedFrameLen); - if(gpioId != gpio::NO_GPIO) { - if (gpioIF->pullHigh(gpioId) != HasReturnvaluesIF::RETURN_OK) { - sif::error << "rwSpiCallback: Failed to pull chip select high" << std::endl; - } - } + closeSpi(gpioId, gpioIF, mutex); - if(gpioId != gpio::NO_GPIO) { - if(mutex->unlockMutex() != HasReturnvaluesIF::RETURN_OK) { - sif::error << "rwSpiCallback: Failed to unlock mutex" << std::endl; - } - } return result; } + +void closeSpi (gpioId_t gpioId, GpioIF* gpioIF, MutexIF* mutex) { + if(gpioId != gpio::NO_GPIO) { + if (gpioIF->pullHigh(gpioId) != HasReturnvaluesIF::RETURN_OK) { + sif::error << "closeSpi: Failed to pull chip select high" << std::endl; + } + } + if(mutex->unlockMutex() != HasReturnvaluesIF::RETURN_OK) { + sif::error << "closeSpi: Failed to unlock mutex" << std::endl;; + } +} diff --git a/bsp_q7s/spiCallbacks/rwSpiCallback.h b/bsp_q7s/spiCallbacks/rwSpiCallback.h index d882bee4..e684c1c1 100644 --- a/bsp_q7s/spiCallbacks/rwSpiCallback.h +++ b/bsp_q7s/spiCallbacks/rwSpiCallback.h @@ -18,4 +18,13 @@ ReturnValue_t rwSpiCallback(SpiComIF* comIf, SpiCookie *cookie, const uint8_t *sendData, size_t sendLen, void* args); +/** + * @brief This function closes a spi session. Pulls the chip select to high an releases the + * mutex. + * @param gpioId Gpio ID of chip select + * @param gpioIF Pointer to gpio interface to drive the chip select + * @param mutex The spi mutex + */ +void closeSpi(gpioId_t gpioId, GpioIF* gpioIF, MutexIF* mutex); + #endif /* BSP_Q7S_RW_SPI_CALLBACK_H_ */ diff --git a/fsfw_hal b/fsfw_hal index 8fe5d0af..2e243e3b 160000 --- a/fsfw_hal +++ b/fsfw_hal @@ -1 +1 @@ -Subproject commit 8fe5d0afa0857025d9dee3c6f266c2503a22c517 +Subproject commit 2e243e3b294870b0c93f85c6b8ff0c74f4f9cce7 diff --git a/mission/devices/RwHandler.cpp b/mission/devices/RwHandler.cpp index a80c97cc..23c49eba 100644 --- a/mission/devices/RwHandler.cpp +++ b/mission/devices/RwHandler.cpp @@ -40,9 +40,11 @@ ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t * id) { switch (communicationStep) { case CommunicationStep::READ_TEMPERATURE: *id = RwDefinitions::GET_TEMPERATURE; + communicationStep = CommunicationStep::GET_RW_SATUS; break; case CommunicationStep::GET_RW_SATUS: *id = RwDefinitions::GET_RW_STATUS; + communicationStep = CommunicationStep::READ_TEMPERATURE; break; default: sif::debug << "RwHandler::buildNormalDeviceCommand: Invalid communication step" @@ -61,6 +63,12 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand ReturnValue_t result = RETURN_OK; switch (deviceCommand) { + case (RwDefinitions::RESET_MCU): { + commandBuffer[0] = static_cast(RwDefinitions::RESET_MCU); + rawPacket = commandBuffer; + rawPacketLen = 1; + return RETURN_OK; + } case (RwDefinitions::GET_RW_STATUS): { prepareGetStatusCmd(commandData, commandDataLen); return RETURN_OK; @@ -89,6 +97,9 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand } void RwHandler::fillCommandAndReplyMap() { + this->insertInCommandMap(RwDefinitions::RESET_MCU); + this->insertInCommandAndReplyMap(RwDefinitions::GET_RW_STATUS, 1, &statusSet, + RwDefinitions::SIZE_GET_RW_STATUS); this->insertInCommandAndReplyMap(RwDefinitions::GET_TEMPERATURE, 1, &temperatureSet, RwDefinitions::SIZE_GET_TEMPERATURE_REPLY); this->insertInCommandAndReplyMap(RwDefinitions::SET_SPEED, 1, nullptr, @@ -98,7 +109,7 @@ void RwHandler::fillCommandAndReplyMap() { ReturnValue_t RwHandler::scanForReply(const uint8_t *start, size_t remainingSize, DeviceCommandId_t *foundId, size_t *foundLen) { - switch (*(start + 1)) { + switch (*(start)) { case (static_cast(RwDefinitions::GET_RW_STATUS)): { *foundLen = RwDefinitions::SIZE_GET_RW_STATUS; *foundId = RwDefinitions::GET_RW_STATUS; @@ -129,14 +140,15 @@ ReturnValue_t RwHandler::scanForReply(const uint8_t *start, size_t remainingSize ReturnValue_t RwHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { /** Check result code */ - if (*(packet + 1) != 0) { + if (*(packet + 1) == RwDefinitions::ERROR) { return EXECUTION_FAILED; } /** Received in little endian byte order */ uint16_t replyCrc = *(packet + sizeOfReply - 1) << 8 | *(packet + sizeOfReply - 2) ; - if (CRC::crc16ccitt(packet, sizeOfReply, 0xFFFF) != replyCrc) { + if (CRC::crc16ccitt(packet, sizeOfReply - 2, 0xFFFF) != replyCrc) { + sif::error << "RwHandler::interpretDeviceReply: cRC error" << std::endl; return CRC_ERROR; } @@ -237,10 +249,12 @@ void RwHandler::prepareSetSpeedCmd(const uint8_t * commandData, size_t commandDa } void RwHandler::handleGetRwStatusReply(const uint8_t* packet) { - uint8_t offset = 0; - statusSet.currSpeed = *(packet + 3) << 24 | *(packet + 2) << 16 | *(packet + 1) << 1 | *packet; + uint8_t offset = 2; + statusSet.currSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 1 | *(packet + offset); offset += 4; - statusSet.referenceSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 1 | *(packet + offset); + statusSet.referenceSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 1 | *(packet + offset); offset += 4; statusSet.state = *(packet + offset); offset += 1; @@ -251,16 +265,17 @@ void RwHandler::handleGetRwStatusReply(const uint8_t* packet) { << " * 0.1 RPM" << std::endl; sif::info << "RwHandler::handleGetRwStatusReply: Reference speed is: " << statusSet.referenceSpeed << " * 0.1 RPM" << std::endl; - sif::info << "RwHandler::handleGetRwStatusReply: State is: " << statusSet.state - << " * 0.1 RPM" << std::endl; - sif::info << "RwHandler::handleGetRwStatusReply: clc mode is: " << statusSet.clcMode - << " * 0.1 RPM" << std::endl; + sif::info << "RwHandler::handleGetRwStatusReply: State is: " + << (unsigned int) statusSet.state.value << std::endl; + sif::info << "RwHandler::handleGetRwStatusReply: clc mode is: " + << (unsigned int) statusSet.clcMode.value << std::endl; #endif } void RwHandler::handleTemperatureReply(const uint8_t* packet) { - temperatureSet.temperatureCelcius = *(packet + 3) << 24 | *(packet + 2) << 16 - | *(packet + 1) << 1 | *packet; + uint8_t offset = 2; + temperatureSet.temperatureCelcius = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 + | *(packet + offset + 1) << 1 | *(packet + offset); #if OBSW_VERBOSE_LEVEL >= 1 && RW_DEBUG == 1 sif::info << "RwHandler::handleTemperatureReply: Temperature: " << temperatureSet.temperatureCelcius << " °C" << std::endl; diff --git a/mission/devices/devicedefinitions/RwDefinitions.h b/mission/devices/devicedefinitions/RwDefinitions.h index 23907eb3..4fe2d368 100644 --- a/mission/devices/devicedefinitions/RwDefinitions.h +++ b/mission/devices/devicedefinitions/RwDefinitions.h @@ -18,6 +18,15 @@ enum PoolIds: lp_id_t { CLC_MODE }; +enum States: uint8_t { + ERROR, + IDLE, + COASTING, + RUNNING_SPEED_STABLE, + RUNNING_SPEED_CHANGING +}; + +static const DeviceCommandId_t RESET_MCU = 1; static const DeviceCommandId_t GET_RW_STATUS = 4; static const DeviceCommandId_t SET_SPEED = 6; static const DeviceCommandId_t GET_TEMPERATURE = 8;