From 04166f87d82f3729b4462437677bae9105c1c88e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 9 Jun 2021 23:20:42 +0200 Subject: [PATCH] several important changes for stm32 --- CMakeLists.txt | 24 +++ devicehandlers/CMakeLists.txt | 3 + devicehandlers/GyroL3GD20Handler.cpp | 260 +++++++++++++++++++++++++++ devicehandlers/GyroL3GD20Handler.h | 80 +++++++++ linux/i2c/CMakeLists.txt | 2 +- linux/spi/CMakeLists.txt | 2 +- stm32h7/CMakeLists.txt | 2 +- stm32h7/devicetest/CMakeLists.txt | 2 +- stm32h7/i2c/CMakeLists.txt | 2 +- stm32h7/spi/CMakeLists.txt | 2 +- stm32h7/uart/CMakeLists.txt | 2 +- 11 files changed, 374 insertions(+), 7 deletions(-) create mode 100644 devicehandlers/CMakeLists.txt create mode 100644 devicehandlers/GyroL3GD20Handler.cpp create mode 100644 devicehandlers/GyroL3GD20Handler.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 72dc17b..e3b75fc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,8 +34,32 @@ target_link_libraries(${LIB_FSFW_HAL_NAME} PRIVATE ${LIB_FSFW_NAME} ) +foreach(INCLUDE_PATH ${FSFW_HAL_ADDITIONAL_INC_PATHS}) + if(IS_ABSOLUTE ${INCLUDE_PATH}) + set(CURR_ABS_INC_PATH "${INCLUDE_PATH}") + else() + get_filename_component(CURR_ABS_INC_PATH + ${INCLUDE_PATH} REALPATH BASE_DIR ${CMAKE_SOURCE_DIR}) + endif() + + if(CMAKE_VERBOSE) + message(STATUS "FSFW include path: ${CURR_ABS_INC_PATH}") + endif() + + list(APPEND FSFW_HAL_ADD_INC_PATHS_ABS ${CURR_ABS_INC_PATH}) +endforeach() + target_include_directories(${LIB_FSFW_HAL_NAME} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} + ${FSFW_HAL_ADD_INC_PATHS_ABS} +) + +target_compile_definitions(${LIB_FSFW_HAL_NAME} PRIVATE + ${FSFW_HAL_DEFINES} +) + +target_link_libraries(${LIB_FSFW_HAL_NAME} PRIVATE + ${FSFW_HAL_LINK_LIBS} ) if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") diff --git a/devicehandlers/CMakeLists.txt b/devicehandlers/CMakeLists.txt new file mode 100644 index 0000000..1cde7e4 --- /dev/null +++ b/devicehandlers/CMakeLists.txt @@ -0,0 +1,3 @@ +target_sources(${LIB_FSFW_HAL_NAME} PRIVATE + GyroL3GD20Handler.cpp +) diff --git a/devicehandlers/GyroL3GD20Handler.cpp b/devicehandlers/GyroL3GD20Handler.cpp new file mode 100644 index 0000000..5820d93 --- /dev/null +++ b/devicehandlers/GyroL3GD20Handler.cpp @@ -0,0 +1,260 @@ +#include "GyroL3GD20Handler.h" +#include + +#include + +GyroHandlerL3GD20H::GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceCommunication, + CookieIF *comCookie): + DeviceHandlerBase(objectId, deviceCommunication, comCookie), + dataset(this) { +#if L3GD20_GYRO_DEBUG == 1 + debugDivider = new PeriodicOperationDivider(5); +#endif +} + +GyroHandlerL3GD20H::~GyroHandlerL3GD20H() {} + +void GyroHandlerL3GD20H::doStartUp() { + if(internalState == InternalState::NONE) { + internalState = InternalState::CONFIGURE; + } + + if(internalState == InternalState::CONFIGURE) { + if(commandExecuted) { + 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() { + setMode(_MODE_POWER_DOWN); +} + +ReturnValue_t GyroHandlerL3GD20H::buildTransitionDeviceCommand(DeviceCommandId_t *id) { + switch(internalState) { + case(InternalState::NONE): + case(InternalState::NORMAL): { + return HasReturnvaluesIF::RETURN_OK; + } + case(InternalState::CONFIGURE): { + *id = L3GD20H::CONFIGURE_CTRL_REGS; + uint8_t command [5]; + command[0] = L3GD20H::CTRL_REG_1_VAL; + command[1] = L3GD20H::CTRL_REG_2_VAL; + command[2] = L3GD20H::CTRL_REG_3_VAL; + command[3] = L3GD20H::CTRL_REG_4_VAL; + command[4] = L3GD20H::CTRL_REG_5_VAL; + return buildCommandFromCommand(*id, command, 5); + } + case(InternalState::CHECK_REGS): { + *id = L3GD20H::READ_REGS; + return buildCommandFromCommand(*id, nullptr, 0); + } + default: + /* Might be a configuration error. */ + sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown internal state!" << + std::endl; + return HasReturnvaluesIF::RETURN_OK; + } + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t GyroHandlerL3GD20H::buildNormalDeviceCommand(DeviceCommandId_t *id) { + *id = L3GD20H::READ_REGS; + return buildCommandFromCommand(*id, nullptr, 0); +} + +ReturnValue_t GyroHandlerL3GD20H::buildCommandFromCommand( + DeviceCommandId_t deviceCommand, const uint8_t *commandData, + size_t commandDataLen) { + switch(deviceCommand) { + case(L3GD20H::READ_REGS): { + commandBuffer[0] = L3GD20H::READ_START | L3GD20H::AUTO_INCREMENT_MASK | + L3GD20H::READ_MASK; + + std::memset(commandBuffer + 1, 0, L3GD20H::READ_LEN); + rawPacket = commandBuffer; + rawPacketLen = L3GD20H::READ_LEN + 1; + break; + } + case(L3GD20H::CONFIGURE_CTRL_REGS): { + commandBuffer[0] = L3GD20H::CTRL_REG_1 | L3GD20H::AUTO_INCREMENT_MASK; + if(commandData == nullptr or commandDataLen != 5) { + return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; + } + + ctrlReg1Value = commandData[0]; + ctrlReg2Value = commandData[1]; + ctrlReg3Value = commandData[2]; + ctrlReg4Value = commandData[3]; + ctrlReg5Value = commandData[4]; + + bool fsH = ctrlReg4Value & L3GD20H::SET_FS_1; + bool fsL = ctrlReg4Value & L3GD20H::SET_FS_0; + + if(not fsH and not fsL) { + scaleFactor = static_cast(L3GD20H::RANGE_DPS_00) / INT16_MAX; + } + else if(not fsH and fsL) { + scaleFactor = static_cast(L3GD20H::RANGE_DPS_01) / INT16_MAX; + } + else { + scaleFactor = static_cast(L3GD20H::RANGE_DPS_11) / INT16_MAX; + } + + commandBuffer[1] = ctrlReg1Value; + commandBuffer[2] = ctrlReg2Value; + commandBuffer[3] = ctrlReg3Value; + commandBuffer[4] = ctrlReg4Value; + commandBuffer[5] = ctrlReg5Value; + + rawPacket = commandBuffer; + rawPacketLen = 6; + break; + } + case(L3GD20H::READ_CTRL_REGS): { + commandBuffer[0] = L3GD20H::READ_START | L3GD20H::AUTO_INCREMENT_MASK | + L3GD20H::READ_MASK; + + std::memset(commandBuffer + 1, 0, 5); + rawPacket = commandBuffer; + rawPacketLen = 6; + break; + } + default: + return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; + } + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t GyroHandlerL3GD20H::scanForReply(const uint8_t *start, size_t len, + 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; + + /* Data with SPI Interface has always this answer */ + if (start[0] == 0b11111111) { + return HasReturnvaluesIF::RETURN_OK; + } + return DeviceHandlerIF::INVALID_DATA; +} + +ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id, + const uint8_t *packet) { + ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + switch(id) { + case(L3GD20H::CONFIGURE_CTRL_REGS): { + commandExecuted = true; + break; + } + case(L3GD20H::READ_CTRL_REGS): { + if(packet[1] == ctrlReg1Value and packet[2] == ctrlReg2Value and + packet[3] == ctrlReg3Value and packet[4] == ctrlReg4Value and + packet[5] == ctrlReg5Value) { + commandExecuted = true; + } + else { + /* Attempt reconfiguration. */ + internalState = InternalState::CONFIGURE; + return DeviceHandlerIF::DEVICE_REPLY_INVALID; + } + break; + } + 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]; + + float angVelocX = (packet[L3GD20H::OUT_X_H] << 8 | + packet[L3GD20H::OUT_X_L]) * scaleFactor; + float angVelocY = (packet[L3GD20H::OUT_Y_H] << 8 | + packet[L3GD20H::OUT_Y_L]) * scaleFactor; + float angVelocZ = (packet[L3GD20H::OUT_Z_H] << 8 | + packet[L3GD20H::OUT_Z_L]) * scaleFactor; + + int8_t temperaturOffset = (-1) * packet[L3GD20H::TEMPERATURE_IDX]; + float temperature = 25.0 + temperaturOffset; +#if L3GD20_GYRO_DEBUG == 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) { + dataset.angVelocX = angVelocX; + dataset.angVelocY = angVelocY; + dataset.angVelocZ = angVelocZ; + dataset.temperature = temperature; + dataset.setValidity(true, true); + } + break; + } + default: + return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; + } + return result; +} + + +uint32_t GyroHandlerL3GD20H::getTransitionDelayMs(Mode_t from, Mode_t to) { + return 10000; +} + +ReturnValue_t GyroHandlerL3GD20H::initializeLocalDataPool( + localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { + localDataPoolMap.emplace(L3GD20H::ANG_VELOC_X, + new PoolEntry({0.0})); + localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Y, + new PoolEntry({0.0})); + localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Z, + new PoolEntry({0.0})); + localDataPoolMap.emplace(L3GD20H::TEMPERATURE, + new PoolEntry({0.0})); + return HasReturnvaluesIF::RETURN_OK; +} + +void GyroHandlerL3GD20H::fillCommandAndReplyMap() { + insertInCommandAndReplyMap(L3GD20H::READ_REGS, 1, &dataset); + insertInCommandAndReplyMap(L3GD20H::CONFIGURE_CTRL_REGS, 1); + insertInCommandAndReplyMap(L3GD20H::READ_CTRL_REGS, 1); +} + +void GyroHandlerL3GD20H::modeChanged() { + internalState = InternalState::NONE; +} diff --git a/devicehandlers/GyroL3GD20Handler.h b/devicehandlers/GyroL3GD20Handler.h new file mode 100644 index 0000000..7f5ee1c --- /dev/null +++ b/devicehandlers/GyroL3GD20Handler.h @@ -0,0 +1,80 @@ +#ifndef MISSION_DEVICES_GYROL3GD20HANDLER_H_ +#define MISSION_DEVICES_GYROL3GD20HANDLER_H_ + +#include "devicedefinitions/GyroL3GD20Definitions.h" +#include + +#include +#include + + +/** + * @brief Device Handler for the L3GD20H gyroscope sensor + * (https://www.st.com/en/mems-and-sensors/l3gd20h.html) + * @details + * Advanced documentation: + * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/L3GD20H_Gyro + * + * Data is read big endian with the smallest possible range of 245 degrees per second. + */ +class GyroHandlerL3GD20H: public DeviceHandlerBase { +public: + GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceCommunication, + CookieIF* comCookie); + virtual ~GyroHandlerL3GD20H(); + +protected: + + /* DeviceHandlerBase overrides */ + ReturnValue_t buildTransitionDeviceCommand( + DeviceCommandId_t *id) override; + void doStartUp() override; + void doShutDown() override; + ReturnValue_t buildNormalDeviceCommand( + DeviceCommandId_t *id) override; + ReturnValue_t buildCommandFromCommand( + DeviceCommandId_t deviceCommand, const uint8_t *commandData, + size_t commandDataLen) override; + ReturnValue_t scanForReply(const uint8_t *start, size_t len, + DeviceCommandId_t *foundId, size_t *foundLen) override; + ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, + const uint8_t *packet) override; + + void fillCommandAndReplyMap() override; + void modeChanged() override; + uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; + ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) override; + +private: + GyroPrimaryDataset dataset; + + enum class InternalState { + NONE, + CONFIGURE, + CHECK_REGS, + NORMAL + }; + InternalState internalState = InternalState::NONE; + bool commandExecuted = false; + + uint8_t statusReg = 0; + + uint8_t ctrlReg1Value = L3GD20H::CTRL_REG_1_VAL; + uint8_t ctrlReg2Value = L3GD20H::CTRL_REG_2_VAL; + uint8_t ctrlReg3Value = L3GD20H::CTRL_REG_3_VAL; + uint8_t ctrlReg4Value = L3GD20H::CTRL_REG_4_VAL; + uint8_t ctrlReg5Value = L3GD20H::CTRL_REG_5_VAL; + + uint8_t commandBuffer[L3GD20H::READ_LEN + 1]; + + float scaleFactor = static_cast(L3GD20H::RANGE_DPS_00) / INT16_MAX; + +#if L3GD20_GYRO_DEBUG == 1 + PeriodicOperationDivider* debugDivider = nullptr; +#endif +}; + + + +#endif /* MISSION_DEVICES_GYROL3GD20HANDLER_H_ */ diff --git a/linux/i2c/CMakeLists.txt b/linux/i2c/CMakeLists.txt index 3f4bad3..0e50313 100644 --- a/linux/i2c/CMakeLists.txt +++ b/linux/i2c/CMakeLists.txt @@ -1,4 +1,4 @@ -target_sources(${TARGET_NAME} PUBLIC +target_sources(${LIB_FSFW_HAL_NAME} PUBLIC I2cComIF.cpp I2cCookie.cpp ) diff --git a/linux/spi/CMakeLists.txt b/linux/spi/CMakeLists.txt index cb1c927..5794547 100644 --- a/linux/spi/CMakeLists.txt +++ b/linux/spi/CMakeLists.txt @@ -1,4 +1,4 @@ -target_sources(${TARGET_NAME} PUBLIC +target_sources(${LIB_FSFW_HAL_NAME} PUBLIC SpiComIF.cpp SpiCookie.cpp ) diff --git a/stm32h7/CMakeLists.txt b/stm32h7/CMakeLists.txt index 3837285..eed01fd 100644 --- a/stm32h7/CMakeLists.txt +++ b/stm32h7/CMakeLists.txt @@ -1,6 +1,6 @@ add_subdirectory(spi) add_subdirectory(devicetest) -target_sources(${TARGET_NAME} PRIVATE +target_sources(${LIB_FSFW_HAL_NAME} PRIVATE dmaInterrupts.cpp ) diff --git a/stm32h7/devicetest/CMakeLists.txt b/stm32h7/devicetest/CMakeLists.txt index dab7eaf..1ee4313 100644 --- a/stm32h7/devicetest/CMakeLists.txt +++ b/stm32h7/devicetest/CMakeLists.txt @@ -1,3 +1,3 @@ -target_sources(${TARGET_NAME} PRIVATE +target_sources(${LIB_FSFW_HAL_NAME} PRIVATE GyroL3GD20H.cpp ) \ No newline at end of file diff --git a/stm32h7/i2c/CMakeLists.txt b/stm32h7/i2c/CMakeLists.txt index 8e57e01..aa3194a 100644 --- a/stm32h7/i2c/CMakeLists.txt +++ b/stm32h7/i2c/CMakeLists.txt @@ -1,2 +1,2 @@ -target_sources(${TARGET_NAME} PRIVATE +target_sources(${LIB_FSFW_HAL_NAME} PRIVATE ) diff --git a/stm32h7/spi/CMakeLists.txt b/stm32h7/spi/CMakeLists.txt index c566f53..ba814e2 100644 --- a/stm32h7/spi/CMakeLists.txt +++ b/stm32h7/spi/CMakeLists.txt @@ -1,4 +1,4 @@ -target_sources(${TARGET_NAME} PRIVATE +target_sources(${LIB_FSFW_HAL_NAME} PRIVATE spiCore.cpp spiDefinitions.cpp spiInterrupts.cpp diff --git a/stm32h7/uart/CMakeLists.txt b/stm32h7/uart/CMakeLists.txt index 8e57e01..aa3194a 100644 --- a/stm32h7/uart/CMakeLists.txt +++ b/stm32h7/uart/CMakeLists.txt @@ -1,2 +1,2 @@ -target_sources(${TARGET_NAME} PRIVATE +target_sources(${LIB_FSFW_HAL_NAME} PRIVATE )