diff --git a/.gitignore b/.gitignore index 9b89790f971bdc8817c9e536f0eccafcf4009da4..a9f1775b4bc49623280a7697bbb213db7e2b4b91 100644 --- a/.gitignore +++ b/.gitignore @@ -32,3 +32,5 @@ core __pycache__ /scripts/generators/generated /scripts/generators/scxmls + +scripts/logdecoder/logdecoder diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index 36d54fe1acd93824940194e7bfda20c119aeba8f..2cbb65d50d357cbf8bf1386ac3b39b0021f77012 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -25,6 +25,7 @@ variables: stages: - build-release - build-debug + - build-logdecoder - test - lint - documentation @@ -36,7 +37,7 @@ build-release: tags: - miosix script: - - ./sbs --jobs 2 + - ./sbs # Stage build-debug @@ -45,34 +46,54 @@ build-debug: tags: - miosix script: - - ./sbs --jobs 2 --debug + - ./sbs --debug + +# Stage build-logdecoder + +build-logdecoder: + stage: build-logdecoder + tags: + - miosix + script: + - cd scripts/logdecoder + - make # Stage test test: stage: test + tags: + - miosix script: - - ./sbs --jobs 2 --test catch-tests-boardcore + - ./sbs --test catch-tests-boardcore # Stage lint cppcheck: stage: lint + tags: + - miosix script: - ./scripts/linter.py --cppcheck src format: stage: lint + tags: + - miosix script: - ./scripts/linter.py --format src copyright: stage: lint + tags: + - miosix script: - ./scripts/linter.py --copyright src find: stage: lint + tags: + - miosix script: - ./scripts/linter.py --find src diff --git a/.gitmodules b/.gitmodules index 14ebbcc7e259961aa9036a5ccfe3a5afecb0108f..8d94e67246c0107398855117b3e192edaed08c37 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,24 +1,24 @@ -[submodule "libs/miosix-kernel"] - path = libs/miosix-kernel - url = ../miosix-kernel.git -[submodule "libs/tscpp"] - path = libs/tscpp - url = ../third-party/tscpp.git -[submodule "libs/mavlink_skyward_lib"] - path = libs/mavlink_skyward_lib - url = ../mavlink_gs/mavlink_skyward_lib.git -[submodule "libs/eigen"] +[submodule "Catch2"] + path = libs/Catch2 + url = ../third-party/Catch2.git +[submodule "Eigen"] path = libs/eigen url = ../third-party/eigen.git -[submodule "libs/mxgui"] - path = libs/mxgui - url = ../third-party/mxgui.git -[submodule "libs/fmt"] +[submodule "Modern formatting library"] path = libs/fmt url = ../third-party/fmt.git -[submodule "libs/Catch2"] - path = libs/Catch2 - url = ../third-party/Catch2.git -[submodule "libs/miosix-host"] - path = libs/miosix-host - url = ../miosix-host.git +[submodule "MAVLink Skyward library"] + path = libs/mavlink-skyward-lib + url = ../mavlink/mavlink-skyward-lib +[submodule "Miosix Host"] + path = libs/miosix-host + url = ../miosix-host.git +[submodule "Miosix Kernel"] + path = libs/miosix-kernel + url = ../miosix-kernel.git +[submodule "Miosix GUI library"] + path = libs/mxgui + url = ../third-party/mxgui.git +[submodule "Trivial serialization for C++"] + path = libs/tscpp + url = ../third-party/tscpp.git diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index f0d67b4cd47e0a2892448b6e802ecbf24b12b721..67b3b9ff613f4759e09a39a6f2d99f273dc2df33 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -21,9 +21,10 @@ "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common", "${workspaceFolder}/libs/miosix-kernel/miosix/arch/common", "${workspaceFolder}/libs/miosix-kernel/miosix", - "${workspaceFolder}/libs/mavlink_skyward_lib", + "${workspaceFolder}/libs/mavlink-skyward-lib", "${workspaceFolder}/libs/fmt/include", "${workspaceFolder}/libs/eigen", + "${workspaceFolder}/libs/tscpp", "${workspaceFolder}/libs", "${workspaceFolder}/src/shared", "${workspaceFolder}/src/tests" @@ -41,7 +42,7 @@ "${workspaceFolder}/libs/miosix-kernel/miosix/util", "${workspaceFolder}/libs/miosix-kernel/miosix/e20", "${workspaceFolder}/libs/miosix-kernel/miosix/*", - "${workspaceFolder}/libs/mavlink_skyward_lib", + "${workspaceFolder}/libs/mavlink-skyward-lib", "${workspaceFolder}/libs/eigen", "${workspaceFolder}/libs/tscpp", "${workspaceFolder}/libs/mxgui", @@ -73,9 +74,10 @@ "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common", "${workspaceFolder}/libs/miosix-kernel/miosix/arch/common", "${workspaceFolder}/libs/miosix-kernel/miosix", - "${workspaceFolder}/libs/mavlink_skyward_lib", + "${workspaceFolder}/libs/mavlink-skyward-lib", "${workspaceFolder}/libs/fmt/include", "${workspaceFolder}/libs/eigen", + "${workspaceFolder}/libs/tscpp", "${workspaceFolder}/libs", "${workspaceFolder}/src/shared", "${workspaceFolder}/src/tests" @@ -93,7 +95,7 @@ "${workspaceFolder}/libs/miosix-kernel/miosix/util", "${workspaceFolder}/libs/miosix-kernel/miosix/e20", "${workspaceFolder}/libs/miosix-kernel/miosix/*", - "${workspaceFolder}/libs/mavlink_skyward_lib", + "${workspaceFolder}/libs/mavlink-skyward-lib", "${workspaceFolder}/libs/eigen", "${workspaceFolder}/libs/tscpp", "${workspaceFolder}/libs/mxgui", @@ -125,9 +127,10 @@ "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common", "${workspaceFolder}/libs/miosix-kernel/miosix/arch/common", "${workspaceFolder}/libs/miosix-kernel/miosix", - "${workspaceFolder}/libs/mavlink_skyward_lib", + "${workspaceFolder}/libs/mavlink-skyward-lib", "${workspaceFolder}/libs/fmt/include", "${workspaceFolder}/libs/eigen", + "${workspaceFolder}/libs/tscpp", "${workspaceFolder}/libs", "${workspaceFolder}/src/shared", "${workspaceFolder}/src/tests" @@ -145,7 +148,60 @@ "${workspaceFolder}/libs/miosix-kernel/miosix/util", "${workspaceFolder}/libs/miosix-kernel/miosix/e20", "${workspaceFolder}/libs/miosix-kernel/miosix/*", - "${workspaceFolder}/libs/mavlink_skyward_lib", + "${workspaceFolder}/libs/mavlink-skyward-lib", + "${workspaceFolder}/libs/eigen", + "${workspaceFolder}/libs/tscpp", + "${workspaceFolder}/libs/mxgui", + "${workspaceFolder}/libs/fmt", + "${workspaceFolder}/src/shared", + "${workspaceFolder}/src/tests" + ], + "limitSymbolsToIncludedHeaders": true + } + }, + { + "name": "stm32f429zi_parafoil", + "cStandard": "c11", + "cppStandard": "c++11", + "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++", + "defines": [ + "DEBUG", + "_ARCH_CORTEXM4_STM32F4", + "_BOARD_STM32F429ZI_PARAFOIL", + "_MIOSIX_BOARDNAME=stm32f429zi_parafoil", + "HSE_VALUE=8000000", + "SYSCLK_FREQ_168MHz=168000000", + "_MIOSIX", + "__cplusplus=201103L" + ], + "includePath": [ + "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_parafoil", + "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_parafoil", + "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common", + "${workspaceFolder}/libs/miosix-kernel/miosix/arch/common", + "${workspaceFolder}/libs/miosix-kernel/miosix", + "${workspaceFolder}/libs/mavlink-skyward-lib", + "${workspaceFolder}/libs/fmt/include", + "${workspaceFolder}/libs/eigen", + "${workspaceFolder}/libs/tscpp", + "${workspaceFolder}/libs", + "${workspaceFolder}/src/shared", + "${workspaceFolder}/src/tests" + ], + "browse": { + "path": [ + "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_parafoil", + "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_parafoil", + "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common", + "${workspaceFolder}/libs/miosix-kernel/miosix/stdlib_integration", + "${workspaceFolder}/libs/miosix-kernel/miosix/arch/common", + "${workspaceFolder}/libs/miosix-kernel/miosix/interfaces", + "${workspaceFolder}/libs/miosix-kernel/miosix/filesystem", + "${workspaceFolder}/libs/miosix-kernel/miosix/kernel", + "${workspaceFolder}/libs/miosix-kernel/miosix/util", + "${workspaceFolder}/libs/miosix-kernel/miosix/e20", + "${workspaceFolder}/libs/miosix-kernel/miosix/*", + "${workspaceFolder}/libs/mavlink-skyward-lib", "${workspaceFolder}/libs/eigen", "${workspaceFolder}/libs/tscpp", "${workspaceFolder}/libs/mxgui", diff --git a/.vscode/settings.json b/.vscode/settings.json index 0995365f671776c5b1138c4f3c36ef31f97df10e..43e514f2603ebb9bb2b90fdab6c1c80067e2f17d 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -99,32 +99,130 @@ "ranges": "cpp", "compare": "cpp", "concepts": "cpp", - "qtalignedmalloc": "cpp" + "core": "cpp", + "iterativelinearsolvers": "cpp", + "pastixsupport": "cpp", + "superlusupport": "cpp", + "adolcforward": "cpp", + "alignedvector3": "cpp", + "bvh": "cpp", + "fft": "cpp", + "iterativesolvers": "cpp", + "mprealsupport": "cpp", + "matrixfunctions": "cpp", + "nonlinearoptimization": "cpp", + "openglsupport": "cpp", + "polynomials": "cpp", + "autodiff": "cpp", + "sparseextra": "cpp", + "specialfunctions": "cpp" }, "cSpell.words": [ - "Amatruda", + "aelf", + "Alessandro", + "AMSL", + "atthr", "AVDD", - "CPHA", - "Damiano", - "Davide", - "DGNSS", - "GNSS", + "Baro", + "boardcore", + "Boardcorev", + "Corigliano", + "CORTEXM", + "cpitch", + "cppcheck", + "croll", + "cwise", + "cyaw", + "deleteme", + "DMEIE", + "Doxyfile", + "doxygen", + "DRDY", + "Duca", + "Ecompass", + "Eigen", + "elfs", + "Erbetta", + "Fatt", + "Fatttr", + "fedetft's", + "fiprintf", + "Gatttr", + "getdetahstate", "Gpio", + "GPIOA", "GPIOB", + "GPIOC", + "GPIOD", + "GPIOE", + "GPIOF", "GPIOG", + "GPIOS", + "gpstr", + "Hatt", + "HSCMAND", + "HSCMRNN", + "IDLEIE", + "irqn", + "irqv", + "Kalman", + "Katt", + "kbps", + "ldrex", + "LIFCR", + "logdecoder", + "Luca", + "MEKF", + "MINC", "miosix", - "Mosi", - "MSTR", - "NMEA", - "nsat", - "PCLK", - "RXCRCR", - "RXDMAEN", + "mkdir", + "MPXHZ", + "Musso", + "NATT", + "NBAR", + "NDTR", + "NGPS", + "NMAG", + "NMEKF", + "nord", + "NVIC", + "peripehral", + "PINC", + "Pitot", + "Plin", + "Qgbw", + "Qget", + "Qhandle", + "Qput", + "Qwait", + "Qwakeup", + "Riccardo", "RXNE", - "trasfer", - "TXCRCR", - "TXDMAEN", - "UBXGPS" + "RXNEIE", + "sats", + "Satt", + "SDRAM", + "spitch", + "sqdip", + "sqtrp", + "sroll", + "SSCDANN", + "SSCDRRN", + "stof", + "syaw", + "TCIE", + "TEIE", + "testsuite", + "TSCPP", + "Ublox", + "Unsync", + "upcounter", + "USART", + "velnord", + "Xbee", + "xnord", + "yned" ], - "cortex-debug.variableUseNaturalFormat": false + "cSpell.language": "en", + "cSpell.enabled": true } diff --git a/CMakeLists.txt b/CMakeLists.txt index e318826b8b8cd36d6107aca526eb9ef18c10199b..d7ac92ec3282024cfc6891855abbc3b381bcff1c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -39,6 +39,9 @@ sbs_target(bmx160-calibration-entry stm32f429zi_skyward_death_stack_x) add_executable(config-dsgamma src/entrypoints/config-dsgamma.cpp) sbs_target(config-dsgamma stm32f429zi_stm32f4discovery) +add_executable(imu-calibration src/entrypoints/imu-calibration.cpp) +sbs_target(imu-calibration stm32f429zi_parafoil) + add_executable(mxgui-helloworld src/entrypoints/examples/mxgui-helloworld.cpp) sbs_target(mxgui-helloworld stm32f429zi_stm32f4discovery) @@ -55,9 +58,6 @@ sbs_target(sdcard-benchmark stm32f429zi_stm32f4discovery) # Tests # #-----------------------------------------------------------------------------# -add_executable(test-kalman-eigen-benchmark src/tests/kalman/test-kalman-eigen-benchmark.cpp) -sbs_target(test-kalman-eigen-benchmark stm32f429zi_stm32f4discovery) - add_executable(test-logger src/tests/logger/test-logger.cpp) sbs_target(test-logger stm32f429zi_stm32f4discovery) @@ -80,7 +80,7 @@ add_executable(test-sensormanager src/tests/test-sensormanager.cpp) sbs_target(test-sensormanager stm32f429zi_skyward_death_stack_x) add_executable(test-serial src/tests/test-serial.cpp) -sbs_target(test-serial stm32f429zi_skyward_death_stack_x) +sbs_target(test-serial stm32f407vg_stm32f4discovery) add_executable(test-taskscheduler src/tests/scheduler/test-taskscheduler.cpp) sbs_target(test-taskscheduler stm32f407vg_stm32f4discovery) @@ -96,7 +96,7 @@ add_executable(catch-tests-boardcore src/tests/catch/catch-tests-entry.cpp src/tests/catch/examples/example-test-factorial.cpp src/tests/catch/test-aero.cpp - src/tests/catch/test-buttonhandler.cpp + # src/tests/catch/test-buttonhandler.cpp src/tests/catch/test-circularbuffer.cpp src/tests/catch/test-eventbroker.cpp src/tests/catch/test-timestamptimer.cpp @@ -109,6 +109,53 @@ target_compile_definitions(catch-tests-boardcore PRIVATE USE_MOCK_PERIPHERALS) sbs_target(catch-tests-boardcore stm32f429zi_stm32f4discovery) sbs_catch_test(catch-tests-boardcore) +#-----------------------------------------------------------------------------# +# Tests - Actuators # +#-----------------------------------------------------------------------------# + +add_executable(test-hbridge src/tests/actuators/test-hbridge.cpp) +sbs_target(test-hbridge stm32f429zi_stm32f4discovery) + +add_executable(test-servo src/tests/actuators/test-servo.cpp) +sbs_target(test-servo stm32f429zi_stm32f4discovery) + +add_executable(test-buzzer src/tests/actuators/test-buzzer.cpp) +sbs_target(test-buzzer stm32f429zi_hre_test_stand) + +add_executable(test-stepper src/tests/actuators/test-stepper.cpp) +sbs_target(test-stepper stm32f429zi_stm32f4discovery) + +#-----------------------------------------------------------------------------# +# Tests - Algorithms # +#-----------------------------------------------------------------------------# + +add_executable(test-kalman-eigen-benchmark src/tests/algorithms/Kalman/test-kalman-eigen-benchmark.cpp) +sbs_target(test-kalman-eigen-benchmark stm32f429zi_stm32f4discovery) + +add_executable(test-tmp src/tests/algorithms/NAS/test-tmp.cpp) +sbs_target(test-tmp stm32f429zi_skyward_death_stack_x) + +add_executable(test-attitude-parafoil src/tests/algorithms/NAS/test-attitude-parafoil.cpp) +sbs_target(test-attitude-parafoil stm32f429zi_parafoil) + +add_executable(test-attitude-stack src/tests/algorithms/NAS/test-attitude-stack.cpp) +sbs_target(test-attitude-stack stm32f429zi_skyward_death_stack_x) + +add_executable(test-nas-parafoil src/tests/algorithms/NAS/test-nas-parafoil.cpp) +sbs_target(test-nas-parafoil stm32f429zi_parafoil) + +add_executable(test-nas-stack src/tests/algorithms/NAS/test-nas-stack.cpp) +sbs_target(test-nas-stack stm32f429zi_skyward_death_stack_x) + +add_executable(test-nas-with-triad src/tests/algorithms/NAS/test-nas-with-triad.cpp) +sbs_target(test-nas-with-triad stm32f429zi_skyward_death_stack_x) + +add_executable(test-triad src/tests/algorithms/NAS/test-triad.cpp) +sbs_target(test-triad stm32f429zi_skyward_death_stack_x) + +add_executable(test-triad-parafoil src/tests/algorithms/NAS/test-triad-parafoil.cpp) +sbs_target(test-triad-parafoil stm32f429zi_parafoil) + #-----------------------------------------------------------------------------# # Tests - Drivers # #-----------------------------------------------------------------------------# @@ -128,9 +175,6 @@ sbs_target(test-dsgamma stm32f429zi_stm32f4discovery) add_executable(test-general-purpose-timer src/tests/drivers/timer/test-general-purpose-timer.cpp) sbs_target(test-general-purpose-timer stm32f429zi_stm32f4discovery) -add_executable(test-hbridge src/tests/drivers/test-hbridge.cpp) -sbs_target(test-hbridge stm32f429zi_stm32f4discovery) - add_executable(test-internal-adc src/tests/drivers/test-internal-adc.cpp) sbs_target(test-internal-adc stm32f429zi_skyward_death_stack_x) @@ -146,9 +190,6 @@ sbs_target(test-MBLoadCell stm32f407vg_stm32f4discovery) add_executable(test-pwm src/tests/drivers/timer/test-pwm.cpp) sbs_target(test-pwm stm32f429zi_stm32f4discovery) -add_executable(test-servo src/tests/drivers/test-servo.cpp) -sbs_target(test-servo stm32f429zi_stm32f4discovery) - add_executable(test-spi src/tests/drivers/spi/test-spi.cpp) sbs_target(test-spi stm32f429zi_stm32f4discovery) @@ -185,8 +226,8 @@ sbs_target(test-xbee-rcv stm32f429zi_stm32f4discovery) add_executable(test-xbee-snd src/tests/drivers/xbee/test-xbee-snd.cpp) sbs_target(test-xbee-snd stm32f429zi_stm32f4discovery) -add_executable(test-stepper src/tests/drivers/stepper/test-stepper.cpp) -sbs_target(test-stepper stm32f429zi_stm32f4discovery) +add_executable(test-usart src/tests/drivers/usart/test-usart.cpp) +sbs_target(test-usart stm32f407vg_stm32f4discovery) #-----------------------------------------------------------------------------# # Tests - Events # @@ -233,7 +274,7 @@ add_executable(test-sx1278-serial src/tests/drivers/sx1278/test-sx1278-serial.cpp src/tests/drivers/sx1278/test-sx1278-core.cpp ) -sbs_target(test-sx1278-serial stm32f407vg_stm32f4discovery) +sbs_target(test-sx1278-serial stm32f429zi_stm32f4discovery) #-----------------------------------------------------------------------------# # Tests - Sensors # @@ -303,7 +344,7 @@ add_executable(test-max31855 src/tests/sensors/test-max31855.cpp) sbs_target(test-max31855 stm32f429zi_stm32f4discovery) add_executable(test-mpu9250 src/tests/sensors/test-mpu9250.cpp) -sbs_target(test-mpu9250 stm32f407vg_stm32f4discovery) +sbs_target(test-mpu9250 stm32f429zi_parafoil) add_executable(test-ms5803 src/tests/sensors/test-ms5803.cpp) sbs_target(test-ms5803 stm32f429zi_skyward_death_stack_x) @@ -311,6 +352,13 @@ sbs_target(test-ms5803 stm32f429zi_skyward_death_stack_x) add_executable(test-ubxgps src/tests/sensors/test-ubxgps.cpp) sbs_target(test-ubxgps stm32f429zi_skyward_death_stack_x) +add_executable(test-ubloxgps-serial src/tests/sensors/test-ubloxgps.cpp) +sbs_target(test-ubloxgps-serial stm32f429zi_skyward_death_stack_x) + +add_executable(test-ubloxgps-spi src/tests/sensors/test-ubloxgps.cpp) +target_compile_definitions(test-ubloxgps-spi PRIVATE USE_SPI) +sbs_target(test-ubloxgps-spi stm32f407vg_stm32f4discovery) + add_executable(test-vn100 src/tests/sensors/test-vn100.cpp) sbs_target(test-vn100 stm32f407vg_stm32f4discovery) @@ -320,3 +368,6 @@ sbs_target(test-vn100 stm32f407vg_stm32f4discovery) add_executable(test-csvreader src/tests/utils/test-csvreader.cpp) sbs_target(test-csvreader stm32f429zi_stm32f4discovery) + +add_executable(test-buttonhandler src/tests/utils/test-buttonhandler.cpp) +sbs_target(test-buttonhandler stm32f429zi_stm32f4discovery) diff --git a/cmake/boardcore-host.cmake b/cmake/boardcore-host.cmake index 712ef96983d156696b4b7e91ed999a69e0e0dacb..85d11ef2498c72e2528591aabf6a092ebb82b0af 100644 --- a/cmake/boardcore-host.cmake +++ b/cmake/boardcore-host.cmake @@ -25,9 +25,11 @@ add_library(boardcore-host STATIC EXCLUDE_FROM_ALL ${SBS_BASE}/src/shared/diagnostic/CpuMeter.cpp ${SBS_BASE}/src/shared/diagnostic/PrintLogger.cpp + # Actuators + ${SBS_BASE}/src/shared/actuators/Servo/Servo.cpp + # Drivers ${SBS_BASE}/src/shared/drivers/timer/TimestampTimer.cpp - ${SBS_BASE}/src/shared/drivers/servo/Servo.cpp # Events ${SBS_BASE}/src/shared/events/EventBroker.cpp @@ -35,11 +37,6 @@ add_library(boardcore-host STATIC EXCLUDE_FROM_ALL # Logger ${SBS_BASE}/src/shared/logger/Logger.cpp - # Math - ${SBS_BASE}/src/shared/math/Matrix.cpp - ${SBS_BASE}/src/shared/math/SkyQuaternion.cpp - ${SBS_BASE}/src/shared/math/Stats.cpp - # Radio ${SBS_BASE}/src/shared/radio/Xbee/APIFrameParser.cpp @@ -50,11 +47,11 @@ add_library(boardcore-host STATIC EXCLUDE_FROM_ALL ${SBS_BASE}/src/shared/sensors/SensorManager.cpp ${SBS_BASE}/src/shared/sensors/SensorSampler.cpp - # AeroUtils - ${SBS_BASE}/src/shared/utils/aero/AeroUtils.cpp - - # TestUtils - ${SBS_BASE}/src/shared/utils/testutils/TestHelper.cpp + # Utils + ${SBS_BASE}/src/shared/utils/AeroUtils/AeroUtils.cpp + ${SBS_BASE}/src/shared/utils/SkyQuaternion/SkyQuaternion.cpp + ${SBS_BASE}/src/shared/utils/Stats/Stats.cpp + ${SBS_BASE}/src/shared/utils/TestUtils/TestHelper.cpp ) add_library(SkywardBoardcore::Boardcore::host ALIAS boardcore-host) target_include_directories(boardcore-host PUBLIC ${SBS_BASE}/src/shared) diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake index 51a3d2ed92b9bb101bc70c39df4907ef7572653d..5c4264138caf1c3acb46abbdd7cece140b7f27ab 100644 --- a/cmake/boardcore.cmake +++ b/cmake/boardcore.cmake @@ -26,6 +26,14 @@ include(boardcore-host) foreach(OPT_BOARD ${BOARDS}) set(BOARDCORE_LIBRARY boardcore-${OPT_BOARD}) add_library(${BOARDCORE_LIBRARY} STATIC EXCLUDE_FROM_ALL + # Actuators + ${SBS_BASE}/src/shared/actuators/HBridge/HBridge.cpp + ${SBS_BASE}/src/shared/actuators/Servo/Servo.cpp + + + # Algorithms + ${SBS_BASE}/src/shared/algorithms/NAS/NAS.cpp + # Debug ${SBS_BASE}/src/shared/utils/Debug.cpp ${SBS_BASE}/src/shared/diagnostic/CpuMeter.cpp @@ -35,14 +43,13 @@ foreach(OPT_BOARD ${BOARDS}) ${SBS_BASE}/src/shared/drivers/adc/InternalADC.cpp ${SBS_BASE}/src/shared/drivers/canbus/Canbus.cpp ${SBS_BASE}/src/shared/drivers/canbus/CanInterrupt.cpp - ${SBS_BASE}/src/shared/drivers/hbridge/HBridge.cpp ${SBS_BASE}/src/shared/drivers/i2c/stm32f2_f4_i2c.cpp ${SBS_BASE}/src/shared/drivers/interrupt/external_interrupts.cpp ${SBS_BASE}/src/shared/drivers/timer/PWM.cpp ${SBS_BASE}/src/shared/drivers/timer/TimestampTimer.cpp ${SBS_BASE}/src/shared/drivers/runcam/Runcam.cpp - ${SBS_BASE}/src/shared/drivers/servo/Servo.cpp ${SBS_BASE}/src/shared/drivers/spi/SPITransaction.cpp + ${SBS_BASE}/src/shared/drivers/usart/USART.cpp # Events ${SBS_BASE}/src/shared/events/EventBroker.cpp @@ -50,11 +57,6 @@ foreach(OPT_BOARD ${BOARDS}) # Logger ${SBS_BASE}/src/shared/logger/Logger.cpp - # Math - ${SBS_BASE}/src/shared/math/Matrix.cpp - ${SBS_BASE}/src/shared/math/SkyQuaternion.cpp - ${SBS_BASE}/src/shared/math/Stats.cpp - # Radio ${SBS_BASE}/src/shared/radio/gamma868/Gamma868.cpp ${SBS_BASE}/src/shared/radio/Xbee/APIFrameParser.cpp @@ -81,14 +83,19 @@ foreach(OPT_BOARD ${BOARDS}) ${SBS_BASE}/src/shared/sensors/MS5803/MS5803.cpp ${SBS_BASE}/src/shared/sensors/SensorManager.cpp ${SBS_BASE}/src/shared/sensors/SensorSampler.cpp + ${SBS_BASE}/src/shared/sensors/UbloxGPS/UbloxGPS.cpp ${SBS_BASE}/src/shared/sensors/UBXGPS/UBXGPS.cpp ${SBS_BASE}/src/shared/sensors/VN100/VN100.cpp - # AeroUtils - ${SBS_BASE}/src/shared/utils/aero/AeroUtils.cpp + # Calibration + ${SBS_BASE}/src/shared/sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.cpp - # TestUtils - ${SBS_BASE}/src/shared/utils/testutils/TestHelper.cpp + # Utils + ${SBS_BASE}/src/shared/utils/AeroUtils/AeroUtils.cpp + ${SBS_BASE}/src/shared/utils/ButtonHandler/ButtonHandler.cpp + ${SBS_BASE}/src/shared/utils/SkyQuaternion/SkyQuaternion.cpp + ${SBS_BASE}/src/shared/utils/Stats/Stats.cpp + ${SBS_BASE}/src/shared/utils/TestUtils/TestHelper.cpp ) add_library(SkywardBoardcore::Boardcore::${OPT_BOARD} ALIAS ${BOARDCORE_LIBRARY}) target_include_directories(${BOARDCORE_LIBRARY} PUBLIC ${SBS_BASE}/src/shared) diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake index 5e77e8cd88ceb12f063df6c60b3f9abc5d7c9e4f..d77b06c6888e6b171328204c83a08c347729fc59 100644 --- a/cmake/dependencies.cmake +++ b/cmake/dependencies.cmake @@ -47,4 +47,4 @@ add_subdirectory(${SBS_BASE}/libs/Catch2 EXCLUDE_FROM_ALL) list(APPEND CMAKE_MODULE_PATH ${SBS_BASE}/libs/Catch2/contrib) include(Catch) -add_subdirectory(${SBS_BASE}/libs/mavlink_skyward_lib EXCLUDE_FROM_ALL) +add_subdirectory(${SBS_BASE}/libs/mavlink-skyward-lib EXCLUDE_FROM_ALL) diff --git a/doc/Doxyfile b/doc/Doxyfile index 22fa2cf1651e5958dd0cc1bea9af113e7bfaf484..607537c5692282602a46f660851838436d564a88 100644 --- a/doc/Doxyfile +++ b/doc/Doxyfile @@ -873,7 +873,7 @@ RECURSIVE = YES # Note that relative paths are relative to the directory from which doxygen is # run. -EXCLUDE = ./src/shared/utils/testutils +EXCLUDE = ./src/shared/utils/TestUtils # The EXCLUDE_SYMLINKS tag can be used to select whether or not files or # directories that are symbolic links (a Unix file system feature) are excluded diff --git a/libs/mavlink-skyward-lib b/libs/mavlink-skyward-lib new file mode 160000 index 0000000000000000000000000000000000000000..94b57a3b5d5c200e69817066d69bc2e37cf963af --- /dev/null +++ b/libs/mavlink-skyward-lib @@ -0,0 +1 @@ +Subproject commit 94b57a3b5d5c200e69817066d69bc2e37cf963af diff --git a/libs/mavlink_skyward_lib b/libs/mavlink_skyward_lib deleted file mode 160000 index e81ebd93ca06167e672486b944334a7b5b34226b..0000000000000000000000000000000000000000 --- a/libs/mavlink_skyward_lib +++ /dev/null @@ -1 +0,0 @@ -Subproject commit e81ebd93ca06167e672486b944334a7b5b34226b diff --git a/libs/miosix-kernel b/libs/miosix-kernel index e213d17f3ddfb6d5ae223038f5162b387518a501..441aca88b44e5438e04abbef1639cd7f9df7bb37 160000 --- a/libs/miosix-kernel +++ b/libs/miosix-kernel @@ -1 +1 @@ -Subproject commit e213d17f3ddfb6d5ae223038f5162b387518a501 +Subproject commit 441aca88b44e5438e04abbef1639cd7f9df7bb37 diff --git a/libs/mxgui b/libs/mxgui index a3f44e0cc0c7965f947b18621470d22fcabddc7b..0052374bc85c2bf90753faba61ecc08905658c83 160000 --- a/libs/mxgui +++ b/libs/mxgui @@ -1 +1 @@ -Subproject commit a3f44e0cc0c7965f947b18621470d22fcabddc7b +Subproject commit 0052374bc85c2bf90753faba61ecc08905658c83 diff --git a/libs/tscpp b/libs/tscpp index c8674a48e794c246f79c56b163ebab72d3654846..935bf1944e1ddeaf51fd22cdce04a2a97762890f 160000 --- a/libs/tscpp +++ b/libs/tscpp @@ -1 +1 @@ -Subproject commit c8674a48e794c246f79c56b163ebab72d3654846 +Subproject commit 935bf1944e1ddeaf51fd22cdce04a2a97762890f diff --git a/src/shared/math/Matrix.cpp b/old_examples/shared/math/Matrix.cpp similarity index 100% rename from src/shared/math/Matrix.cpp rename to old_examples/shared/math/Matrix.cpp diff --git a/src/shared/math/Matrix.h b/old_examples/shared/math/Matrix.h similarity index 100% rename from src/shared/math/Matrix.h rename to old_examples/shared/math/Matrix.h diff --git a/src/shared/math/Vec3.h b/old_examples/shared/math/Vec3.h similarity index 100% rename from src/shared/math/Vec3.h rename to old_examples/shared/math/Vec3.h diff --git a/old_examples/tests/catch/example-test-fsm.cpp b/old_examples/tests/catch/example-test-fsm.cpp index 5c79a22446c0506fea3c8c91c6de62132d612a25..9c69e2b3000ca19754a076f3c52b379bb12e66b1 100644 --- a/old_examples/tests/catch/example-test-fsm.cpp +++ b/old_examples/tests/catch/example-test-fsm.cpp @@ -34,7 +34,7 @@ #define protected public #include <miosix.h> -#include <utils/testutils/TestHelper.h> +#include <utils/TestUtils/TestHelper.h> #include <catch2/catch.hpp> @@ -131,7 +131,7 @@ public: // Start the thread in the constructor FSMTestFixture() { - sEventBroker.start(); + EventBroker::getInstance().start(); fsm.start(); } // Stop the thread in the destructor @@ -168,7 +168,7 @@ TEST_CASE_METHOD(FSMTestFixture, "Testing async transitions") SECTION("S3 --> S4 if EV_B. Delayed event should be removed") { - EventCounter counter{*sEventBroker}; + EventCounter counter{*EventBroker::getInstance()}; // Post EV_B before EV_D fires. should move to S4 and remove EV_D // from the delayed events diff --git a/old_examples/tests/catch/example-test-fsm.h b/old_examples/tests/catch/example-test-fsm.h index 3aa648ec901654b0a6dbfaf836981f1d66e72e81..03f46222db911c7304992b33a1ee525771567b5e 100644 --- a/old_examples/tests/catch/example-test-fsm.h +++ b/old_examples/tests/catch/example-test-fsm.h @@ -39,8 +39,8 @@ */ enum ExampleEvents : uint8_t { - EV_A = EV_FIRST_SIGNAL, // The first event must always have value - // EV_FIRST_SIGNAL + EV_A = EV_FIRST_CUSTOM, // The first event must always have value + // EV_FIRST_CUSTOM EV_B, // Values for the following event can be manually specified or // assigned automatically EV_C, @@ -110,10 +110,10 @@ public: FSMExample() : FSM(&FSMExample::state_S1), v(0) { // Subscribe for events posted on TOPIC_T1 - sEventBroker.subscribe(this, TOPIC_T1); + EventBroker::getInstance().subscribe(this, TOPIC_T1); } - ~FSMExample() { sEventBroker.unsubscribe(this); } + ~FSMExample() { EventBroker::getInstance().unsubscribe(this); } /* * State function definitions. @@ -207,8 +207,8 @@ public: v = 1; // Post EV_D to itself in 1 seconds - delayed_ev_id = - sEventBroker.postDelayed(Event{EV_D}, TOPIC_T1, 1000); + delayed_ev_id = EventBroker::getInstance().postDelayed( + Event{EV_D}, TOPIC_T1, 1000); break; } @@ -219,7 +219,7 @@ public: // don't remove a delayed event. This is an error! Uncomment to // fix, see the wiki for further information !!! - // sEventBroker.removeDelayed(delayed_ev_id); + // EventBroker::getInstance().removeDelayed(delayed_ev_id); break; } case EV_B: diff --git a/old_examples/tests/catch/misc/xbee-bitrate.cpp b/old_examples/tests/catch/misc/xbee-bitrate.cpp index 6bf877b9ee87a832215351ffbd0d65b2b7bbf44d..47f40a10fce5ea70e4060026333041251e2c4db4 100644 --- a/old_examples/tests/catch/misc/xbee-bitrate.cpp +++ b/old_examples/tests/catch/misc/xbee-bitrate.cpp @@ -22,9 +22,9 @@ #include <drivers/spi/SPIDriver.h> #include <interfaces-impl/hwmapping.h> -#include <math/Stats.h> #include <miosix.h> #include <radio/Xbee/Xbee.h> +#include <utils/Stats/Stats.h> #include <cstdio> #include <iostream> diff --git a/old_examples/tests/catch/misc/xbee-send-rcv.cpp b/old_examples/tests/catch/misc/xbee-send-rcv.cpp index cba700960f8e653a164f6dc9394851a5c00b5044..87c8c8335961362424518bfb8cdf842bca50d559 100644 --- a/old_examples/tests/catch/misc/xbee-send-rcv.cpp +++ b/old_examples/tests/catch/misc/xbee-send-rcv.cpp @@ -22,9 +22,9 @@ #include <drivers/spi/SPIDriver.h> #include <interfaces-impl/hwmapping.h> -#include <math/Stats.h> #include <miosix.h> #include <radio/Xbee/Xbee.h> +#include <utils/Stats/Stats.h> #include <cstdio> #include <iostream> diff --git a/old_examples/tests/catch/misc/xbee-time-to-send.cpp b/old_examples/tests/catch/misc/xbee-time-to-send.cpp index c915459d6b41fb1446239b49d76dfbf70485cd0a..eb08047f3087a7fe5b9aa00aaaecaaf006836e79 100644 --- a/old_examples/tests/catch/misc/xbee-time-to-send.cpp +++ b/old_examples/tests/catch/misc/xbee-time-to-send.cpp @@ -22,9 +22,9 @@ #include <drivers/BusTemplate.h> #include <interfaces-impl/hwmapping.h> -#include <math/Stats.h> #include <miosix.h> #include <radio/Xbee/Xbee.h> +#include <utils/Stats/Stats.h> #include <cstdio> #include <iostream> diff --git a/old_examples/tests/catch/test-xbee.cpp b/old_examples/tests/catch/test-xbee.cpp index 9973a4db02411e48f2acc8bc9f962d1c22493544..3ec81dfba6464ce558d983edec75abb2cbde0d2c 100644 --- a/old_examples/tests/catch/test-xbee.cpp +++ b/old_examples/tests/catch/test-xbee.cpp @@ -30,7 +30,7 @@ #define private public #include <radio/Xbee/Xbee.h> -#include <utils/testutils/BusTemplateMock.h> +#include <utils/TestUtils/BusTemplateMock.h> using std::vector; diff --git a/old_examples/tests/drivers/test-imu-adis.cpp b/old_examples/tests/drivers/test-imu-adis.cpp index e6f5e31ac86b92e29bd1a45348605d402464447d..777c33d8f60da0d7f638fdaaf3ec229f54af6a4e 100644 --- a/old_examples/tests/drivers/test-imu-adis.cpp +++ b/old_examples/tests/drivers/test-imu-adis.cpp @@ -25,9 +25,9 @@ #include <drivers/BusTemplate.h> #include <drivers/spi/SensorSpi.h> #include <interfaces-impl/hwmapping.h> -#include <math/Stats.h> #include <sensors/ADIS16405/ADIS16405.h> #include <sensors/SensorSampler.h> +#include <utils/Stats/Stats.h> using namespace miosix; using namespace miosix::interfaces; diff --git a/old_examples/tests/drivers/test-lsm.cpp b/old_examples/tests/drivers/test-lsm.cpp index 5aec0b6344c8db38d24387512b62cc3d7d5a7fb2..6f0dbc430be8d67b25d945007e6c9519df8647b5 100644 --- a/old_examples/tests/drivers/test-lsm.cpp +++ b/old_examples/tests/drivers/test-lsm.cpp @@ -25,9 +25,9 @@ #include <drivers/BusTemplate.h> #include <drivers/spi/SensorSpi.h> #include <interfaces-impl/hwmapping.h> -#include <math/Stats.h> #include <sensors/LSM6DS3H/LSM6DS3H.h> #include <sensors/SensorSampler.h> +#include <utils/Stats/Stats.h> using namespace miosix; using namespace miosix::interfaces; diff --git a/scripts/generators/eventgen.py b/scripts/generators/eventgen.py index dedf167d77171b073d56623f605f9139855b4010..f5014d9517d50e1ee6b3db965868af308db73ffc 100755 --- a/scripts/generators/eventgen.py +++ b/scripts/generators/eventgen.py @@ -93,7 +93,7 @@ def generate_events_h(events, date): # Prepare the event list enum_event_list = list(events) - enum_event_list[0] += ' = EV_FIRST_SIGNAL' + enum_event_list[0] += ' = EV_FIRST_CUSTOM' enum_event_list = '\n'.join( [' '*4 + event + ',' for event in enum_event_list]) vector_event_list = '\n'.join([' '*4 + event + ',' for event in events]) diff --git a/scripts/generators/fsmgen.py b/scripts/generators/fsmgen.py index b3c6bab216cb027497e6c5c3fd4129f91b583ba7..5288d3618a19e29653f97d4a7ce26e8bc5bc649e 100755 --- a/scripts/generators/fsmgen.py +++ b/scripts/generators/fsmgen.py @@ -59,7 +59,7 @@ STATE_FUNCTION_CASE_TEMPLATE = ' '*8 + 'case {event}:\n' + \ ' '*12 + 'break;\n' + \ ' '*8 + '}}' UTILITY_FUNCTION_DECLARATION_TEMPLATE = 'void {function_name}();' -TOPICS_SUBSCIPTION_TEMPLATE = ' '*4 + 'sEventBroker.subscribe(this, {topic});' +TOPICS_SUBSCIPTION_TEMPLATE = ' '*4 + 'EventBroker::getInstance().subscribe(this, {topic});' UTILITY_FUNCTION_DEFINITION_TEMPLATE = 'void {state_machine_name}Controller::{function_name}()\n{{\n // ...\n}}' STATE_FUNCTION_CASE_TRANSITION_TO_TARGET_TEMPLATE = 'transition(&{state_machine_name}Controller::state_{state_name});' TEST_CASE_METHOD_TEMPLATE = 'TEST_CASE_METHOD({state_machine_name}ControllerFixture, "Testing transitions from {state}")\n' + \ diff --git a/scripts/generators/templates/FSMController.cpp.template b/scripts/generators/templates/FSMController.cpp.template index 583fb435c4bdb89414a829df229a87349a374155..2ea5f355f54898bee5e1eb88647bcb87b97ac4e9 100644 --- a/scripts/generators/templates/FSMController.cpp.template +++ b/scripts/generators/templates/FSMController.cpp.template @@ -41,7 +41,7 @@ using namespace {state_machine_name}Config; {state_machine_name}Controller::~{state_machine_name}Controller() {{ - sEventBroker.unsubscribe(this); + EventBroker::getInstance().unsubscribe(this); }} {states_functions_definitions} diff --git a/scripts/generators/templates/test.cpp.template b/scripts/generators/templates/test.cpp.template index 73a93088404afceda5b285fa48429ce3e88172d0..2eb55502e2ed85421c8bed9310b86efd1e715f86 100644 --- a/scripts/generators/templates/test.cpp.template +++ b/scripts/generators/templates/test.cpp.template @@ -30,11 +30,11 @@ #include <miosix.h> -#include <utils/testutils/catch.hpp> +#include <utils/TestUtils/catch.hpp> #include "{state_machine_name}Controller/{state_machine_name}Controller.h" #include <events/Events.h> -#include <utils/testutils/TestHelper.h> +#include <utils/TestUtils/TestHelper.h> using miosix::Thread; using namespace DeathStackBoard; @@ -46,7 +46,7 @@ public: {state_machine_name}ControllerFixture() {{ controller = new {state_machine_name}Controller(); - sEventBroker.start(); + EventBroker::getInstance().start(); controller->start(); }} @@ -54,8 +54,8 @@ public: ~{state_machine_name}ControllerFixture() {{ controller->stop(); - sEventBroker.unsubscribe(controller); - sEventBroker.clearDelayedEvents(); + EventBroker::getInstance().unsubscribe(controller); + EventBroker::getInstance().clearDelayedEvents(); delete controller; }} diff --git a/scripts/logdecoder/Makefile b/scripts/logdecoder/Makefile index 0a30ee85eaf9786bc68f52dcd335486e4766392e..4f9857ad3fe80b1640b317a05abcedc6a3791dd7 100644 --- a/scripts/logdecoder/Makefile +++ b/scripts/logdecoder/Makefile @@ -1,6 +1,12 @@ +BOARDCORE := ../../ all: - g++ -std=c++11 -O2 -o logdecoder logdecoder.cpp ../libs/tscpp/stream.cpp -I ../libs - + g++ -std=c++17 -O2 -o logdecoder logdecoder.cpp \ + -DCOMPILE_FOR_X86 \ + $(BOARDCORE)libs/tscpp/tscpp/stream.cpp \ + -I$(BOARDCORE)libs/mavlink-skyward-lib \ + -I$(BOARDCORE)libs/eigen \ + -I$(BOARDCORE)libs/tscpp \ + -I$(BOARDCORE)src/shared clean: rm logdecoder diff --git a/scripts/logdecoder/logdecoder.cpp b/scripts/logdecoder/logdecoder.cpp index 96b81e338727137ff3d25d077d1c56891226fd15..8227f53788003dcd68bbef6a6860a4bccc1977fd 100644 --- a/scripts/logdecoder/logdecoder.cpp +++ b/scripts/logdecoder/logdecoder.cpp @@ -1,59 +1,109 @@ -/*************************************************************************** - * Copyright (C) 2018 by Terraneo Federico * - * * - * This program is free software; you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation; either version 2 of the License, or * - * (at your option) any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * As a special exception, if other files instantiate templates or use * - * macros or inline functions from this file, or you compile this file * - * and link it with other works to produce a work based on this file, * - * this file does not by itself cause the resulting work to be covered * - * by the GNU General Public License. However the source code for this * - * file must still be made available in accordance with the GNU General * - * Public License. This exception does not invalidate any other reasons * - * why a work based on this file might be covered by the GNU General * - * Public License. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program; if not, see <http://www.gnu.org/licenses/> * - ***************************************************************************/ - -/* - * This is a stub program for the program that will decode the logged data. - * Fill in the TODO to make it work. +/* Copyright (c) 2018-2022 Skyward Experimental Rocketry + * Authors: Terrane Federico + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. */ -#include <iostream> +#include <logger/Deserializer.h> +#include <logger/LogTypes.h> +#include <tscpp/stream.h> + #include <fstream> +#include <iostream> #include <stdexcept> -#include <tscpp/stream.h> +#include <string> -//TODO: add here include files of serialized classes -#include "../src/shared/logger/LogStats.h" -#include "../src/entrypoints/test_logger.h" +/** + * @brief Binary log files decoder. + * + * This program is to compile for you computer and decodes binary log files + * through the tscpp library. + * + * In LogTypes.h there should be included all the classes you want to + * deserialize. + */ -using namespace std; using namespace tscpp; +using namespace Boardcore; + +void showUsage(const string& cmdName) +{ + std::cerr << "Usage: " << cmdName << " {-a | <log_file_name> | -h}" + << "Options:\n" + << "\t-h,--help\t\tShow help message\n" + << "\t-a,--all Deserialize all logs in the current directory\n" + << std::endl; +} + +bool deserialize(string logName) +{ + std::cout << "Deserializing " << logName << "...\n"; + Deserializer d(logName); + LogTypes::registerTypes(d); + + return d.deserialize(); +} + +bool deserializeAll() +{ + for (int i = 0; i < 100; i++) + { + char nextName[11]; + sprintf(nextName, "log%02d.dat", i); + struct stat st; + if (stat(nextName, &st) != 0) + continue; // File not found + // File found + if (!deserialize(string(nextName))) + return false; + } + return true; +} + +int main(int argc, char* argv[]) +{ + if (argc < 2) + { + showUsage(string(argv[0])); + return 1; // Error + } + + bool success = false; + string arg1 = string(argv[1]); + + // Help message + if (arg1 == "-h" || arg1 == "--help") + { + showUsage(string(argv[0])); + return 0; + } + + // File deserialization + if (arg1 == "-a" || arg1 == "--all") + success = deserializeAll(); + else + success = deserialize(arg1); -int main(int argc, char *argv[]) -try { - TypePoolStream tp; - //TODO: Register the serialized classes - tp.registerType<LogStats>([](LogStats& t){ t.print(cout); cout<<'\n'; }); - tp.registerType<Dummy>([](Dummy& t){ t.print(cout); cout<<'\n'; }); - - if(argc!=2) return 1; - ifstream in(argv[1]); - in.exceptions(ios::eofbit); - UnknownInputArchive ia(in,tp); - for(;;) ia.unserialize(); -} catch(exception&) { + // End + if (success) + std::cout << "Deserialization completed successfully\n"; + else + std::cout << "Deserialization ended with errors\n"; return 0; } diff --git a/scripts/logdecoder/sample_no_cereal.dat.dat b/scripts/logdecoder/sample_no_cereal.dat.dat deleted file mode 100644 index 23dff819fee59674f0cfff237b94fffa191d4a06..0000000000000000000000000000000000000000 Binary files a/scripts/logdecoder/sample_no_cereal.dat.dat and /dev/null differ diff --git a/scripts/logdecoder/xbee/.gitignore b/scripts/logdecoder/xbee/.gitignore deleted file mode 100644 index 3e6fd163051d66dceb9c808d07e314ae4d022ee5..0000000000000000000000000000000000000000 --- a/scripts/logdecoder/xbee/.gitignore +++ /dev/null @@ -1 +0,0 @@ -logdecoder diff --git a/scripts/logdecoder/xbee/LogTypes.h b/scripts/logdecoder/xbee/LogTypes.h deleted file mode 100644 index cb82189b489a302f28b3f62dfd61fec88c1326b7..0000000000000000000000000000000000000000 --- a/scripts/logdecoder/xbee/LogTypes.h +++ /dev/null @@ -1,73 +0,0 @@ -/* Copyright (c) 2015-2018 Skyward Experimental Rocketry - * Author: Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#pragma once - -#include <logger/Deserializer.h> -#include <logger/LogStats.h> -#include <radio/Xbee/APIFramesLog.h> -#include <radio/Xbee/Mark.h> -#include <radio/Xbee/XbeeStatus.h> -#include <radio/Xbee/XbeeTestData.h> - -#include <fstream> -#include <iostream> - -// Serialized classes -using std::ofstream; - -namespace Boardcore -{ - -template <typename T> -void print(T& t, ostream& os) -{ - t.print(os); -} - -template <typename T> -void registerType(Deserializer& ds) -{ - ds.registerType<T>(print<T>, T::header()); -} - -void registerTypes(Deserializer& ds) -{ - registerType<LogStats>(ds); - - registerType<Xbee::APIFrameLog>(ds); - registerType<Xbee::ATCommandFrameLog>(ds); - registerType<Xbee::ATCommandResponseFrameLog>(ds); - registerType<Xbee::ModemStatusFrameLog>(ds); - registerType<Xbee::TXRequestFrameLog>(ds); - registerType<Xbee::TXStatusFrameLog>(ds); - registerType<Xbee::RXPacketFrameLog>(ds); - registerType<Xbee::XbeeStatus>(ds); - - registerType<TxData>(ds); - registerType<RxData>(ds); - registerType<Mark>(ds); - registerType<XbeeConfig>(ds); - registerType<EnergyScanData>(ds); -} - -} // namespace Boardcore diff --git a/scripts/logdecoder/xbee/Makefile b/scripts/logdecoder/xbee/Makefile deleted file mode 100644 index 47258b629723d159885432ac1e485023f5d40002..0000000000000000000000000000000000000000 --- a/scripts/logdecoder/xbee/Makefile +++ /dev/null @@ -1,13 +0,0 @@ -BASE := ../../../../../ - -all: - g++ -std=c++17 -O2 -o logdecoder logdecoder.cpp \ - -DCOMPILE_FOR_X86 \ - $(BASE)libs/tscpp/stream.cpp \ - -I$(BASE)src/shared \ - -I$(BASE)src/tests \ - -I$(BASE)libs \ - -I$(BASE)libs/miosix-kernel/miosix - -clean: - rm logdecoder diff --git a/scripts/logdecoder/xbee/logdecoder.cpp b/scripts/logdecoder/xbee/logdecoder.cpp deleted file mode 100644 index 40577eef2088983028f356bb13b341f97641ed3d..0000000000000000000000000000000000000000 --- a/scripts/logdecoder/xbee/logdecoder.cpp +++ /dev/null @@ -1,168 +0,0 @@ -/* Copyright (c) 2018 Skyward Experimental Rocketry - * Author: Federico Terraneo - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -/* - * This is a stub program for the program that will decode the logged data. - * You must first define a function - * "void registerTypes(Deserializer& ds);" - * - * that registers all the required log data types in the deserializer and then - * INCLUDE this cpp file in your source. finally compile and run - */ - -#include <sys/stat.h> -#include <sys/types.h> -#include <tscpp/stream.h> - -#include <filesystem> -#include <fstream> -#include <iostream> -#include <stdexcept> -#include <string> -#include <vector> - -#include "LogTypes.h" - -using namespace std; -using namespace tscpp; - -namespace fs = filesystem; - -void showHelp(string cmdName) -{ - std::cerr << "Usage: " << cmdName - << " {-a [logs_diretory] | <log_file_path> | -h}" - << "Options:\n" - << "\t-h,--help\t\tShow help message\n" - << "\t-a,--all [dir=\".\"] Deserialize all logs in the provided " - "directory\n" - << std::endl; -} - -vector<fs::path> listLogFiles(fs::path dir) -{ - vector<fs::path> out; - for (const auto& entry : fs::directory_iterator(dir)) - { - if (entry.exists() && entry.is_regular_file()) - { - if (entry.path().extension() == ".dat") - { - out.push_back(entry.path()); - } - } - } - return out; -} - -bool deserialize(fs::path log_path) -{ - cout << "Deserializing " << log_path << ".dat...\n"; - - // remove extension - log_path.replace_extension(""); - Deserializer d(log_path); - registerTypes(d); - - return d.deserialize(); -} - -bool deserializeAll(fs::path dir = ".") -{ - vector<fs::path> logs = listLogFiles(dir); - for (auto log : logs) - { - if (!deserialize(log.string())) - { - return false; - } - } - - return true; -} - -int main(int argc, char* argv[]) -{ - if (argc < 2) - { - showHelp(string(argv[0])); - return 1; - } - - bool success = false; - string arg1 = string(argv[1]); - if (arg1 == "-h" || arg1 == "--help") - { - showHelp(string(argv[0])); - return 0; - } - - if (arg1 == "-a" || arg1 == "--all") - { - fs::path dir = "."; - if (argc == 3) - { - string arg2 = string(argv[2]); - fs::directory_entry entry(arg2); - if (entry.exists() && entry.is_directory()) - { - dir = arg2; - } - else - { - cout << "Second argument after '-a' or '--all' must be a " - "directory\n"; - showHelp(string(argv[0])); - return 1; - } - } - cout << "Deserializing all logs...\n"; - success = deserializeAll(dir); - } - else if (arg1[0] == '-') - { - cerr << "Unknown option\n"; - return 1; - } - else - { - fs::directory_entry entry(arg1); - if (entry.exists() && entry.is_regular_file()) - { - success = deserialize(arg1); - } - else - { - showHelp(string(argv[0])); - return 1; - } - } - - if (success) - { - cout << "Deserialization completed successfully.\n"; - } - else - { - cout << "Deserialization ended with errors.\n"; - } -} diff --git a/src/entrypoints/bmx160-calibration-entry.cpp b/src/entrypoints/bmx160-calibration-entry.cpp index 0124a2310951b7803879e9fce2c79466ba04880f..d2aa4746b97f41ee1b47c5306c6ea884f1f859ef 100644 --- a/src/entrypoints/bmx160-calibration-entry.cpp +++ b/src/entrypoints/bmx160-calibration-entry.cpp @@ -24,7 +24,8 @@ #include <drivers/timer/TimestampTimer.h> #include <sensors/BMX160/BMX160.h> #include <sensors/BMX160/BMX160WithCorrection.h> -#include <sensors/calibration/SoftIronCalibration.h> +#include <sensors/calibration/AxisOrientation.h> +#include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h> #include <utils/Debug.h> #include <fstream> @@ -41,9 +42,9 @@ constexpr int ACC_CALIBRATION_SLEEP_TIME = 25; // [us] constexpr int ACC_CALIBRATION_N_ORIENTATIONS = 6; constexpr int MAG_CALIBRATION_SAMPLES = 500; -constexpr int MAG_CALIBRATION_DURAITON = 60; // [s] +constexpr int MAG_CALIBRATION_DURATION = 60; // [s] constexpr int MAG_CALIBRATION_SLEEP_TIME = - MAG_CALIBRATION_DURAITON * 1000 / MAG_CALIBRATION_SAMPLES; // [us] + MAG_CALIBRATION_DURATION * 1000 / MAG_CALIBRATION_SAMPLES; // [us] BMX160* bmx160; @@ -330,7 +331,7 @@ BMX160CorrectionParameters calibrateMagnetometer( BMX160CorrectionParameters correctionParameters) { Eigen::Matrix<float, 3, 2> m; - auto* calibrationModel = new SoftIronCalibration<MAG_CALIBRATION_SAMPLES>; + auto* calibrationModel = new SoftAndHardIronCalibration; Eigen::Vector3f avgMag{0, 0, 0}, vec; SPIBus bus(SPI1); @@ -389,7 +390,7 @@ BMX160CorrectionParameters calibrateMagnetometer( "the " "most different directions.\n"); printf("The calibration will run for %d seconds\n", - MAG_CALIBRATION_DURAITON); + MAG_CALIBRATION_DURATION); if (!askToContinue()) { @@ -412,26 +413,6 @@ BMX160CorrectionParameters calibrateMagnetometer( printf("Computing the result...."); auto corrector = calibrationModel->computeResult(); - // Save the calibration data in the sd card - // Save the correction parameters in the file - { - std::ofstream magnetometerCalibrationDataFile( - MAG_CALIBRATION_DATA_FILE); - - auto samples = calibrationModel->getSamples(); - int numberOfSamples = samples.rows(); - - for (int i = 0; i < numberOfSamples; i++) - { - auto row = samples.row(i); - for (int j = 0; j < row.cols(); j++) - { - magnetometerCalibrationDataFile << samples.row(i)(j) << ","; - } - magnetometerCalibrationDataFile << "\n"; - } - } - corrector >> m; corrector >> correctionParameters.magnetoParams; diff --git a/src/entrypoints/imu-calibration.cpp b/src/entrypoints/imu-calibration.cpp new file mode 100644 index 0000000000000000000000000000000000000000..6c681ea0eaf784188bceea4c47284471bf1e220d --- /dev/null +++ b/src/entrypoints/imu-calibration.cpp @@ -0,0 +1,131 @@ +/* Copyright (c) 2022 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <sensors/MPU9250/MPU9250.h> +#include <sensors/SensorManager.h> +#include <sensors/calibration/AxisOrientation.h> +#include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h> +#include <utils/Debug.h> + +#include <iostream> + +using namespace Boardcore; +using namespace Eigen; +using namespace miosix; + +int menu(); +bool askToContinue(); + +void calibrateMagnetometer(); + +constexpr int MAG_CALIBRATION_DURATION = 30; // [s] +constexpr int IMU_SAMPLE_PERIOD = 10; // [ms] 100Hz + +int main() +{ + // Greet the user + printf("\nWelcome to the calibration procedure!\n"); + + // Make the user choose what to do + switch (menu()) + { + case 2: + calibrateMagnetometer(); + break; + + default: + break; + } + + printf("end\n"); + reboot(); +} + +int menu() +{ + int choice; + + printf("\nWhat do you want to do?\n"); + printf("1. Calibrate accelerometer\n"); + printf("2. Calibrate magnetometer\n"); + printf("3. Set minimum gyroscope samples for calibration\n"); + printf("\n>> "); + scanf("%d", &choice); + + return choice; +} + +bool askToContinue() +{ + char c; + + std::cout << "Write 'c' to continue, otherwise stop:\n"; + scanf("%c", &c); + + return c != 'c'; +} + +void calibrateMagnetometer() +{ + SPIBus spiBus(SPI1); + MPU9250 mpu(spiBus, sensors::mpu9250::cs::getPin()); + mpu.init(); + + printf("Now the magnetometer calibration will begin\n"); + printf("Please, rotate the gyroscope A LOT!\n"); + printf("The calibration will run for %d seconds\n", + MAG_CALIBRATION_DURATION); + + // if (!askToContinue()) + // return; + printf("Starting...\n"); + + // Prepare the calibration model + SoftAndHardIronCalibration calibration; + + // Prepare and start the sensor manager + TaskScheduler scheduler; + scheduler.addTask([]() { printf("...\n"); }, 500); + scheduler.start(); + + // Wait and then stop the sampling + auto startTick = getTick(); + auto lastTick = startTick; + while (getTick() - startTick < MAG_CALIBRATION_DURATION * 1e3) + { + mpu.sample(); + calibration.feed(mpu.getLastSample()); + Thread::sleepUntil(lastTick + IMU_SAMPLE_PERIOD); + lastTick = getTick(); + } + + scheduler.stop(); + + printf("Computing the result...\n"); + + auto correction = calibration.computeResult(); + + printf("b: the bias vector\n"); + std::cout << correction.getOffset().transpose() << std::endl; + printf("g: the gain to be multiplied to the input vector\n"); + std::cout << correction.getGain().transpose() << std::endl; +} diff --git a/src/entrypoints/kernel-testsuite.cpp b/src/entrypoints/kernel-testsuite.cpp index 97cf173c2cf7e6434d56b635335c4bc94d18326c..89c3260272c61d4c0e2e4247573fc97362176ce3 100644 --- a/src/entrypoints/kernel-testsuite.cpp +++ b/src/entrypoints/kernel-testsuite.cpp @@ -426,8 +426,8 @@ void process_test_file_concurrency() fail("Wrong data from file 2"); } - fclose(f1); // cppcheck-suppress nullPointerRedundantCheck - fclose(f2); // cppcheck-suppress nullPointerRedundantCheck + fclose(f1); + fclose(f2); pass(); } @@ -584,7 +584,7 @@ void syscall_test_files() fail("Open with O_RDWR should have failed, the file doesn't exist"); break; case 2: - fail("Cannot craete new file"); + fail("Cannot create new file"); break; case 3: fail("file descriptor not valid"); @@ -596,7 +596,7 @@ void syscall_test_files() fail("read has read the wrong amount of data"); break; case 6: - fail("readed data doesn't match the written one"); + fail("read data doesn't match the written one"); break; case 7: fail("close hasn't returned 0"); @@ -605,10 +605,10 @@ void syscall_test_files() fail("open with O_RDWR failed, but the file exists"); break; case 9: - fail("read has return the wrogn amount of data"); + fail("read has return the wrong amount of data"); break; case 10: - fail("readed data doesn't match the written one"); + fail("read data doesn't match the written one"); break; case 11: fail("close hasn't returned 0"); @@ -2133,7 +2133,7 @@ static void test_9() // Test 10 // /* -tests exception propagation throug entry point of a thread +tests exception propagation through entry point of a thread */ #ifndef __NO_EXCEPTIONS @@ -4171,7 +4171,7 @@ static void test_24() checkAtomicOps<Derived1, Derived1>(); checkAtomicOps<Derived2, Derived2>(); - // atomic_load(), atomic_store(), with polimorphism + // atomic_load(), atomic_store(), with polymorphism checkAtomicOps<Base0, Derived0>(); checkAtomicOps<Middle1, Derived1>(); checkAtomicOps<Middle1, Derived2>(); @@ -4530,6 +4530,7 @@ static void fs_test_1() i = 0; for (;;) { + // cppcheck-suppress nullPointerRedundantCheck j = fread(buf, 1, 1024, f); if (j == 0) break; @@ -4549,6 +4550,7 @@ static void fs_test_1() i = 0; for (;;) { + // cppcheck-suppress nullPointerRedundantCheck j = fread(buf, 1, 1024, f); if (j == 0) break; @@ -4568,6 +4570,7 @@ static void fs_test_1() i = 0; for (;;) { + // cppcheck-suppress nullPointerRedundantCheck j = fread(buf, 1, 1024, f); if (j == 0) break; @@ -4592,7 +4595,10 @@ static void fs_test_1() if ((f = fopen("/sd/testdir/file_4.txt", "a")) == NULL) fail("can't open a file_4.txt"); for (i = 2; i <= 128; i++) + { + // cppcheck-suppress nullPointerRedundantCheck fprintf(f, "Hello world line %03d\n", i); + } if (fclose(f) != 0) // cppcheck-suppress nullPointerRedundantCheck fail("Can't close a file_4.txt"); // Reading to check (only first 2 lines) @@ -4603,11 +4609,10 @@ static void fs_test_1() fgets(line, sizeof(line), f); if (strcmp(line, "Hello world line 001\n")) fail("file_4.txt line 1 error"); - // cppcheck-suppress nullPointerRedundantCheck fgets(line, sizeof(line), f); if (strcmp(line, "Hello world line 002\n")) fail("file_4.txt line 2 error"); - if (fclose(f) != 0) // cppcheck-suppress nullPointerRedundantCheck + if (fclose(f) != 0) fail("Can't close r file_4.txt"); // Test fseek and ftell. When reaching this point file_4.txt contains: // Hello world line 001\n @@ -4617,10 +4622,8 @@ static void fs_test_1() if ((f = fopen("/sd/testdir/file_4.txt", "r")) == NULL) fail("can't open r2 file_4.txt"); if (ftell(f) != 0) // cppcheck-suppress nullPointerRedundantCheck - fail("File opend but cursor not @ address 0"); - // cppcheck-suppress nullPointerRedundantCheck + fail("File opened but cursor not @ address 0"); fseek(f, -4, SEEK_END); // Seek to 128\n - // cppcheck-suppress nullPointerRedundantCheck if ((fgetc(f) != '1') | (fgetc(f) != '2') | (fgetc(f) != '8')) fail("fgetc SEEK_END"); if (ftell(f) != (21 * 128 - 1)) @@ -4628,31 +4631,25 @@ static void fs_test_1() iprintf("ftell=%ld\n", ftell(f)); fail("ftell() 1"); } - // cppcheck-suppress nullPointerRedundantCheck fseek(f, 21 + 17, SEEK_SET); // Seek to 002\n - if ((fgetc(f) != '0') | // cppcheck-suppress nullPointerRedundantCheck - (fgetc(f) != '0') | // cppcheck-suppress nullPointerRedundantCheck - (fgetc(f) != '2') | // cppcheck-suppress nullPointerRedundantCheck - (fgetc(f) != '\n')) // cppcheck-suppress nullPointerRedundantCheck + if ((fgetc(f) != '0') | (fgetc(f) != '0') | (fgetc(f) != '2') | + (fgetc(f) != '\n')) fail("fgetc SEEK_SET"); - if (ftell(f) != (21 * 2)) // cppcheck-suppress nullPointerRedundantCheck + if (ftell(f) != (21 * 2)) { iprintf("ftell=%ld\n", ftell(f)); fail("ftell() 2"); } - // cppcheck-suppress nullPointerRedundantCheck fseek(f, 21 * 50 + 17, SEEK_CUR); // Seek to 053\n - if ((fgetc(f) != '0') | // cppcheck-suppress nullPointerRedundantCheck - (fgetc(f) != '5') | // cppcheck-suppress nullPointerRedundantCheck - (fgetc(f) != '3') | // cppcheck-suppress nullPointerRedundantCheck - (fgetc(f) != '\n')) // cppcheck-suppress nullPointerRedundantCheck + if ((fgetc(f) != '0') | (fgetc(f) != '5') | (fgetc(f) != '3') | + (fgetc(f) != '\n')) fail("fgetc SEEK_CUR"); - if (ftell(f) != (21 * 53)) // cppcheck-suppress nullPointerRedundantCheck + if (ftell(f) != (21 * 53)) { iprintf("ftell=%ld\n", ftell(f)); fail("ftell() 2"); } - if (fclose(f) != 0) // cppcheck-suppress nullPointerRedundantCheck + if (fclose(f) != 0) fail("Can't close r2 file_4.txt"); // Testing remove() if ((f = fopen("/sd/testdir/deleteme.txt", "w")) == NULL) @@ -4745,7 +4742,7 @@ static void checkInDir(const std::string &d, bool createFile) fgets(s, sizeof(s), f); // cppcheck-suppress nullPointerRedundantCheck if (strcmp(s, teststr)) fail("file content after rename"); - fclose(f); // cppcheck-suppress nullPointerRedundantCheck + fclose(f); if (unlink((d + filename2).c_str())) fail("unlink 3"); if (checkDirContent(d, 0, filename2) == false) @@ -4794,10 +4791,11 @@ static void fs_test_3() for (unsigned int j = 0; j < size; j++) buf[j] = rand() & 0xff; checksum ^= crc16(buf, size); + // cppcheck-suppress nullPointerRedundantCheck if (fwrite(buf, 1, size, f) != size) fail("write"); } - if (fclose(f) != 0) // cppcheck-suppress nullPointerRedundantCheck + if (fclose(f) != 0) fail("close 1"); if ((f = fopen(name, "r")) == NULL) @@ -4807,11 +4805,12 @@ static void fs_test_3() for (unsigned int i = 0; i < numBlocks; i++) { memset(buf, 0, size); + // cppcheck-suppress nullPointerRedundantCheck if (fread(buf, 1, size, f) != size) fail("read"); outChecksum ^= crc16(buf, size); } - if (fclose(f) != 0) // cppcheck-suppress nullPointerRedundantCheck + if (fclose(f) != 0) fail("close 2"); delete[] buf; @@ -5013,6 +5012,7 @@ const int nThreads = 8; bool flags[nThreads]; static int throwable(std::vector<int> &v) __attribute__((noinline)); +// cppcheck-suppress containerOutOfBounds static int throwable(std::vector<int> &v) { return v.at(10); } static void test(void *argv) @@ -5168,7 +5168,7 @@ static void benchmark_2() // /* tests: -Fisesystem write speed and latency +File system write speed and latency makes a 1MB file and measures time required to read/write it. */ @@ -5259,7 +5259,7 @@ quit: // /* tests: -Mutex lonk/unlock time +Mutex lock/unlock time */ volatile bool b4_end = false; diff --git a/src/entrypoints/sdcard-benchmark.cpp b/src/entrypoints/sdcard-benchmark.cpp index f41ede96ff37ce124d3c97b984e867df26c839fb..89aa596440629e7fca70113320d987ad15e63467 100644 --- a/src/entrypoints/sdcard-benchmark.cpp +++ b/src/entrypoints/sdcard-benchmark.cpp @@ -26,8 +26,8 @@ #include <ctime> #include <iostream> // #include <thread> -#include <math/Stats.h> #include <miosix.h> +#include <utils/Stats/Stats.h> #include <vector> diff --git a/src/shared/Singleton.h b/src/shared/Singleton.h index 5639c0b15810496bd6a8e669ff19e0ab409f1ee3..5c3975f35899c20e78d8ffd2252d9fc626754ea3 100644 --- a/src/shared/Singleton.h +++ b/src/shared/Singleton.h @@ -57,7 +57,7 @@ protected: Singleton() {} public: - Singleton(const Singleton&) = delete; + Singleton(const Singleton&) = delete; Singleton& operator=(const Singleton&) = delete; }; diff --git a/src/shared/actuators/Buzzer.h b/src/shared/actuators/Buzzer.h new file mode 100644 index 0000000000000000000000000000000000000000..50c6b33409f87870da9f18dbeb04d4050f4216dc --- /dev/null +++ b/src/shared/actuators/Buzzer.h @@ -0,0 +1,174 @@ +/* Copyright (c) 2022 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <drivers/timer/GeneralPurposeTimer.h> +#include <miosix.h> + +namespace Boardcore +{ + +/** + * @brief This driver does not provide a square wave signal but insted is a + * simple utility that provides long PWM signals to make the buzzer beep + * on and off. + */ +class Buzzer +{ +public: + /** + * Note: The timer must be a General Purpose Timer. + * + * @param timer Timer used to provide the alternating on and off signal. + * @param channel Timer channel to output the signal. + */ + Buzzer(TIM_TypeDef *timer, TimerUtils::Channel channel); + + /** + * @brief Turns on the buzzer. + * + * Note: Keeps the timer off. + */ + void on(); + + /** + * @brief Turns off the buzzer. + * + * Note: disables and resets the timer. + */ + void off(); + + /** + * @brief Turns on the buzzer for the specified amount of time. + * + * Note: This function wait for the given duration with Thread::sleep. + * + * @param ms Beep duration in milliseconds. + */ + void oneTimeToggle(uint16_t ms); + + /** + * @brief Turns on and off the buzzer indefinitely with the given timings. + * + * @param onTime Buzzer on time [ms]. + * @param offTime Buzzer off time [ms]. + */ + void continuoslyToggle(uint16_t onTime, uint16_t offTime); + +private: + GP16bitTimer timer; + TimerUtils::Channel channel; +}; + +inline Buzzer::Buzzer(TIM_TypeDef *timer, TimerUtils::Channel channel) + : timer(timer), channel(channel) +{ + // this->timer.setPrescaler( + // TimerUtils::computePrescalerValue(timer, frequency * 2)); + // this->timer.setAutoReloadRegister(1); + // this->timer.setOutputCompareMode(channel, + // TimerUtils::OutputCompareMode::TOGGLE); + // this->timer.setCaptureCompareRegister(channel, 1); + // this->timer.generateUpdate(); + // this->timer.enableCaptureCompareOutput(channel); + // this->timer.enableCaptureCompareComplementaryOutput(channel); +} + +inline void Buzzer::on() +{ + // First turn off and reset the timer + off(); + + // Set the polarity of the channel to low to make the output high + timer.setOutputCompareMode(channel, + TimerUtils::OutputCompareMode::FORCE_ACTIVE); + timer.setCaptureComparePolarity( + channel, TimerUtils::OutputComparePolarity::ACTIVE_LOW); + timer.enableCaptureCompareOutput(channel); +} + +inline void Buzzer::off() +{ + timer.disable(); + timer.reset(); +} + +inline void Buzzer::oneTimeToggle(uint16_t ms) +{ + if (ms == 0) + return; + + // First turn off and reset the timer + off(); + + // Set the prescaler with a 10KHz frequency + timer.setPrescaler( + TimerUtils::computePrescalerValue(timer.getTimer(), 10000)); + + // Make the timer reload after the specified amount of millisseconds + timer.setAutoReloadRegister(ms * 10); + timer.setCaptureCompareRegister(channel, 1); + + timer.setOutputCompareMode(channel, + TimerUtils::OutputCompareMode::PWM_MODE_2); + timer.setCaptureComparePolarity( + channel, TimerUtils::OutputComparePolarity::ACTIVE_LOW); + timer.enableCaptureCompareOutput(channel); + + // Update the configuration + timer.generateUpdate(); + + // Use One Pulse Mode to stop the timer after the first update event + timer.enableOnePulseMode(); + timer.enable(); +} + +inline void Buzzer::continuoslyToggle(uint16_t onTime, uint16_t offTime) +{ + if (onTime == 0 || offTime == 0) + return; + + // First turn off and reset the timer + off(); + + // Set the prescaler with a 10KHz frequency + timer.setPrescaler( + TimerUtils::computePrescalerValue(timer.getTimer(), 10000)); + + // Make the timer reload after the specified amount of millisseconds + timer.setAutoReloadRegister((onTime + offTime) * 10); + timer.setCaptureCompareRegister(channel, onTime * 10); + + timer.setOutputCompareMode(channel, + TimerUtils::OutputCompareMode::PWM_MODE_1); + timer.setCaptureComparePolarity( + channel, TimerUtils::OutputComparePolarity::ACTIVE_LOW); + timer.enableCaptureCompareOutput(channel); + + // Update the configuration + timer.generateUpdate(); + + timer.enable(); +} + +} // namespace Boardcore diff --git a/src/shared/drivers/hbridge/HBridge.cpp b/src/shared/actuators/HBridge/HBridge.cpp similarity index 100% rename from src/shared/drivers/hbridge/HBridge.cpp rename to src/shared/actuators/HBridge/HBridge.cpp diff --git a/src/shared/drivers/hbridge/HBridge.h b/src/shared/actuators/HBridge/HBridge.h similarity index 100% rename from src/shared/drivers/hbridge/HBridge.h rename to src/shared/actuators/HBridge/HBridge.h diff --git a/src/shared/drivers/servo/Servo.cpp b/src/shared/actuators/Servo/Servo.cpp similarity index 91% rename from src/shared/drivers/servo/Servo.cpp rename to src/shared/actuators/Servo/Servo.cpp index 0c4460cd2aa5ee009750a42ded9052488abce8bd..ceffefd2963bda1cb9984ea73974b692d62df2ba 100644 --- a/src/shared/drivers/servo/Servo.cpp +++ b/src/shared/actuators/Servo/Servo.cpp @@ -22,6 +22,8 @@ #include "Servo.h" +#include <drivers/timer/TimestampTimer.h> + #include "miosix.h" namespace Boardcore @@ -97,4 +99,16 @@ float Servo::getPosition180Deg() { return getPosition() * 1800; } float Servo::getPosition360Deg() { return getPosition() * 3600; } +ServoData Servo::getState() +{ + return {TimestampTimer::getInstance().getTimestamp(), + +#ifndef COMPILE_FOR_HOST + pwm.getTimer().getTimerNumber(), static_cast<uint8_t>(pwmChannel), +#else + 0, 0, +#endif + getPosition()}; +} + } // namespace Boardcore diff --git a/src/shared/drivers/servo/Servo.h b/src/shared/actuators/Servo/Servo.h similarity index 97% rename from src/shared/drivers/servo/Servo.h rename to src/shared/actuators/Servo/Servo.h index b76baba37e0cae8f9479da48886272cb637a7710..2e0a511eb412e5a52f71a4399357d927ded90d62 100644 --- a/src/shared/drivers/servo/Servo.h +++ b/src/shared/actuators/Servo/Servo.h @@ -24,6 +24,8 @@ #include <drivers/timer/PWM.h> #endif +#include "ServoData.h" + #pragma once namespace Boardcore @@ -122,6 +124,11 @@ public: float getPosition360Deg(); + /** + * @brief Returns the current position and the current timestamp. + */ + ServoData getState(); + private: // This class is not copyable! Servo& operator=(const Servo&) = delete; diff --git a/src/shared/algorithms/kalman/ExtendedKalmanEigen/ExtendedKalmanConfig.h b/src/shared/actuators/Servo/ServoData.h similarity index 64% rename from src/shared/algorithms/kalman/ExtendedKalmanEigen/ExtendedKalmanConfig.h rename to src/shared/actuators/Servo/ServoData.h index 65242fc3082bb372fe4e8cc3b966ae901a5204b4..b4ff14d357a09257f6d9882ed6921c640fad430e 100644 --- a/src/shared/algorithms/kalman/ExtendedKalmanEigen/ExtendedKalmanConfig.h +++ b/src/shared/actuators/Servo/ServoData.h @@ -1,5 +1,5 @@ -/* Copyright (c) 2020 Skyward Experimental Rocketry - * Authors: Alessandro Del Duca, Luca Conterio +/* Copyright (c) 2022 Skyward Experimental Rocketry + * Author: Alberto Nidasio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -22,30 +22,25 @@ #pragma once -#include <Eigen/Dense> +#include <ostream> -namespace DeathStackBoard +namespace Boardcore { -// function with 2 vectors as parameters -typedef std::function<MatrixXf(VectorXf, VectorXf)> function_2v; -// function with 1 vector as parameter -typedef std::function<MatrixXf(VectorXf)> function_v; - -struct ExtendedKalmanConfig +struct ServoData { - uint8_t n; - uint8_t m; - uint8_t p; - - Eigen::Matrix<float, 12, 12> P; - Eigen::Matrix<float, 6, 6> Q; - Vector<float, 13> x; - - function_v h; - - function_2v dfdx; - function_v dhdx; + uint64_t timestamp; + uint8_t timer; + uint8_t channel; + float position; + + static std::string header() { return "timestamp,timer,channel,position\n"; } + + void print(std::ostream& os) const + { + os << timestamp << "," << timer << "," << channel << "," << position + << "\n"; + } }; -} // namespace DeathStackBoard +} // namespace Boardcore diff --git a/src/shared/drivers/stepper/Stepper.h b/src/shared/actuators/Stepper.h similarity index 99% rename from src/shared/drivers/stepper/Stepper.h rename to src/shared/actuators/Stepper.h index 57942567c189acce7c4824fa6cbbf059055f18ca..b08639af1075f1d3534df3fd5543f3fe0e5e6b25 100644 --- a/src/shared/drivers/stepper/Stepper.h +++ b/src/shared/actuators/Stepper.h @@ -24,7 +24,7 @@ #include <interfaces-impl/gpio_impl.h> #include <interfaces/delays.h> -#include <utils/testutils/MockGpioPin.h> +#include <utils/TestUtils/MockGpioPin.h> namespace Boardcore { @@ -261,4 +261,4 @@ inline int Stepper::getMicrosteppingValue() return 1; } -} // namespace Boardcore \ No newline at end of file +} // namespace Boardcore diff --git a/src/shared/algorithms/Algorithm.h b/src/shared/algorithms/Algorithm.h index 7f8ace5e3da652e594dbeee7101a2ee39e8a9b83..56d31830cdc7218ecd795ab34e003e57261a13d8 100644 --- a/src/shared/algorithms/Algorithm.h +++ b/src/shared/algorithms/Algorithm.h @@ -22,6 +22,9 @@ #pragma once +namespace Boardcore +{ + class Algorithm { public: @@ -63,3 +66,5 @@ protected: bool running = false; }; + +} // namespace Boardcore diff --git a/src/shared/algorithms/kalman/KalmanEigen.h b/src/shared/algorithms/Kalman/Kalman.h similarity index 97% rename from src/shared/algorithms/kalman/KalmanEigen.h rename to src/shared/algorithms/Kalman/Kalman.h index 214e1184e9f75f2d7835fde22f99826ce533d2c3..204311d7532230955459247fcc4ece54e2777785 100644 --- a/src/shared/algorithms/kalman/KalmanEigen.h +++ b/src/shared/algorithms/Kalman/Kalman.h @@ -36,7 +36,7 @@ namespace Boardcore * dynamically. */ template <typename t, uint8_t n, uint8_t p> -class KalmanEigen +class Kalman { using MatrixNN = Eigen::Matrix<t, n, n>; using MatrixPN = Eigen::Matrix<t, p, n>; @@ -47,7 +47,7 @@ class KalmanEigen public: /** - * @brief Configuration struct for the KalmanEigen class. + * @brief Configuration struct for the Kalman class. */ struct KalmanConfig { @@ -63,7 +63,7 @@ public: * @param config configuration object containing all the initialized * matrices */ - KalmanEigen(const KalmanConfig& config) + Kalman(const KalmanConfig& config) : F(config.F), H(config.H), Q(config.Q), R(config.R), P(config.P), S(MatrixPP::Zero(p, p)), K(MatrixNP::Zero(n, p)), x(config.x) { diff --git a/src/shared/algorithms/NAS/NAS.cpp b/src/shared/algorithms/NAS/NAS.cpp new file mode 100644 index 0000000000000000000000000000000000000000..1296b639b48483d3a0309d5136e77c5a97c5fc7e --- /dev/null +++ b/src/shared/algorithms/NAS/NAS.cpp @@ -0,0 +1,338 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Authors: Alessandro Del Duca, Luca Conterio, Marco Cella + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include "NAS.h" + +#include <drivers/timer/TimestampTimer.h> +#include <utils/SkyQuaternion/SkyQuaternion.h> + +#include <iostream> + +using namespace Boardcore; +using namespace Eigen; + +namespace Boardcore +{ + +NAS::NAS(NASConfig config) : config(config) +{ + // Covariance setup + { + // clang-format off + Eigen::Matrix3f P_pos{ + {config.P_POS, 0, 0}, + {0, config.P_POS, 0}, + {0, 0, config.P_POS_VERTICAL} + }; + Eigen::Matrix3f P_vel{ + {config.P_VEL, 0, 0}, + {0, config.P_VEL, 0}, + {0, 0, config.P_VEL_VERTICAL} + }; + Eigen::Matrix3f P_att = Matrix3f::Identity() * config.P_ATT; + Eigen::Matrix3f P_bias = Matrix3f::Identity() * config.P_BIAS; + P << P_pos, MatrixXf::Zero(3, 9), + MatrixXf::Zero(3, 3), P_vel, MatrixXf::Zero(3, 6), + MatrixXf::Zero(3, 6), P_att, MatrixXf::Zero(3, 3), + MatrixXf::Zero(3, 9), P_bias; // cppcheck-suppress constStatement + // clang-format on + } + + // GPS matrixes + { + H_gps = Matrix<float, 4, 6>::Zero(); + H_gps.coeffRef(0, 0) = 1; + H_gps.coeffRef(1, 1) = 1; + H_gps.coeffRef(2, 3) = 1; + H_gps.coeffRef(3, 4) = 1; + H_gps_tr = H_gps.transpose(); + R_gps << config.SIGMA_GPS * Matrix<float, 4, 4>::Identity(); + } + + // Magnetometer utility matrix + R_mag << config.SIGMA_MAG * Matrix3f::Identity(); + + // Other utility matrixes + { + // clang-format off + Q_quat << (config.SIGMA_W * config.SIGMA_W * config.T + (1.0f / 3.0f) * config.SIGMA_BETA * config.SIGMA_BETA * config.T * config.T * config.T) * Matrix3f::Identity(), + (0.5f * config.SIGMA_BETA * config.SIGMA_BETA * config.T * config.T) * Matrix3f::Identity(), + (0.5f * config.SIGMA_BETA * config.SIGMA_BETA * config.T * config.T) * Matrix3f::Identity(), // cppcheck-suppress constStatement + (config.SIGMA_BETA * config.SIGMA_BETA * config.T) * Matrix3f::Identity(); + // clang-format on + + Q_lin << config.SIGMA_POS * Matrix3f::Identity(), MatrixXf::Zero(3, 3), + // cppcheck-suppress constStatement + MatrixXf::Zero(3, 3), config.SIGMA_VEL * Matrix3f::Identity(); + + F_lin = Matrix<float, 6, 6>::Identity(); + F_lin.block<3, 3>(0, 3) = config.T * Matrix3f::Identity(); + F_lin_tr = F_lin.transpose(); + + F_quat << -Matrix3f::Identity(), -Matrix3f::Identity() * config.T, + Matrix3f::Zero(3, 3), Matrix3f::Identity(); + F_quat_tr = F_quat.transpose(); + + G_quat << -Matrix3f::Identity(), Matrix3f::Zero(3, 3), + Matrix3f::Zero(3, 3), Matrix3f::Identity(); + G_quat_tr = G_quat.transpose(); + } +} + +void NAS::predictAcc(const Vector3f& acceleration) +{ + Matrix3f A = body2ned(x.block<4, 1>(IDX_QUAT, 0)); + Vector3f pos = x.block<3, 1>(IDX_POS, 0); + Vector3f vel = x.block<3, 1>(IDX_VEL, 0); + + // Update position by integrating the velocity + pos = pos + vel * config.T; + + // Measured acceleration in NED frame without gravity + Vector3f a = A * acceleration - gravityNed; + + // Update velocity by integrating the acceleration + vel = vel + a * config.T; + + // Save the updated state + x.block<3, 1>(IDX_POS, 0) = pos; + x.block<3, 1>(IDX_VEL, 0) = vel; + + // Variance propagation + Eigen::Matrix<float, 6, 6> Pl = P.block<6, 6>(0, 0); + P.block<6, 6>(0, 0) = F_lin * Pl * F_lin_tr + Q_lin; +} + +void NAS::predictGyro(const Vector3f& angularVelocity) +{ + Vector3f bias = x.block<3, 1>(IDX_BIAS, 0); + Vector4f q = x.block<4, 1>(IDX_QUAT, 0); + + Vector3f omega = angularVelocity - bias; + // clang-format off + Matrix4f omega_mat{ + { 0.0f, omega(2), -omega(1), omega(0)}, + {-omega(2), 0.0f, omega(0), omega(1)}, + { omega(1), -omega(0), 0.0f, omega(2)}, + {-omega(0), -omega(1), -omega(2), 0.0f} + }; + // clang-format on + + // Update orientation by integrating the angular velocity + q = (Matrix4f::Identity() + 0.5F * omega_mat * config.T) * q; + q.normalize(); + + // Save the updated state + x.block<4, 1>(IDX_QUAT, 0) = q; + + // Variance propagation + // TODO: Optimize last G_quat * Q_quat * G_att_tr + Matrix<float, 6, 6> Pq = P.block<6, 6>(IDX_QUAT, IDX_QUAT); + Pq = F_quat * Pq * F_quat_tr + G_quat * Q_quat * G_quat_tr; + P.block<6, 6>(IDX_QUAT, IDX_QUAT) = Pq; +} + +void NAS::correctBaro(const float pressure, const float mslPress, + const float mslTemp) +{ + Matrix<float, 1, 6> H = Matrix<float, 1, 6>::Zero(); + + // Temperature at current altitude. Since in NED the altitude is negative, + // mslTemperature returns temperature at current altitude and not at msl + float temp = Aeroutils::mslTemperature(mslTemp, x(2)); + + // Compute gradient of the altitude-pressure function + H[2] = Constants::a * Constants::n * mslPress * + powf(1 - Constants::a * x(2) / temp, -Constants::n - 1) / temp; + + Eigen::Matrix<float, 6, 6> Pl = P.block<6, 6>(0, 0); + Matrix<float, 1, 1> S = + H * Pl * H.transpose() + Matrix<float, 1, 1>(config.SIGMA_BAR); + Matrix<float, 6, 1> K = Pl * H.transpose() * S.inverse(); + P.block<6, 6>(0, 0) = (Matrix<float, 6, 6>::Identity() - K * H) * Pl; + + float y_hat = Aeroutils::mslPressure(mslPress, mslTemp, x(2)); + + // Update the state + x.head<6>() = x.head<6>() + K * (pressure - y_hat); +} + +void NAS::correctGPS(const Vector4f& gps) +{ + Eigen::Matrix<float, 6, 6> Pl = P.block<6, 6>(0, 0); + + Matrix<float, 4, 4> S = H_gps * Pl * H_gps_tr + R_gps; + Matrix<float, 6, 4> K = Pl * H_gps_tr * S.inverse(); + + P.block<6, 6>(0, 0) = + (Eigen::Matrix<float, 6, 6>::Identity() - K * H_gps) * Pl; + + // Current state [n e vn ve] + Matrix<float, 4, 1> H{x(0), x(1), x(3), x(4)}; + + // Update the state + x.head<6>() = x.head<6>() + K * (gps - H); +} + +void NAS::correctMag(const Vector3f& mag) +{ + Vector4f q = x.block<4, 1>(IDX_QUAT, 0); + Matrix3f A = body2ned(q).transpose(); + + // Rotate the NED magnetic field in the relative reference frame + Vector3f mEst = A * config.NED_MAG; + + // clang-format off + Matrix3f M{ + { 0, -mEst(2), mEst(1)}, + { mEst(2), 0, -mEst(0)}, + {-mEst(1), mEst(0), 0} + }; + // clang-format on + + Matrix<float, 3, 6> H; + H << M, Matrix3f::Zero(3, 3); + Matrix<float, 6, 6> Pq = P.block<6, 6>(IDX_QUAT, IDX_QUAT); + Matrix<float, 3, 3> S = H * Pq * H.transpose() + R_mag; + + Matrix<float, 6, 3> K = Pq * H.transpose() * S.inverse(); + Matrix<float, 6, 1> dx = K * (mag - mEst); + Vector4f r{0.5f * dx(0), 0.5f * dx(1), 0.5f * dx(2), + sqrtf(1.0f - 0.25f * dx.head<3>().transpose() * dx.head<3>())}; + + x.block<4, 1>(IDX_QUAT, 0) = SkyQuaternion::quatProd(r, q); + x.block<3, 1>(IDX_BIAS, 0) += dx.tail<3>(); + Matrix<float, 6, 6> tmp = Matrix<float, 6, 6>::Identity() - K * H; + Pq = tmp * Pq * tmp.transpose() + K * R_mag * K.transpose(); + P.block<6, 6>(IDX_QUAT, IDX_QUAT) = Pq; +} + +void NAS::correctAcc(const Eigen::Vector3f& acc) +{ + Vector4f q = x.block<4, 1>(IDX_QUAT, 0); + Matrix3f A = body2ned(q).transpose(); + + // Rotate the NED magnetic field in the relative reference frame + Vector3f aEst = A * gravityNed; + + // Gradient matrix + // clang-format off + Matrix3f M{ + { 0, -aEst(2), aEst(1)}, + { aEst(2), 0, -aEst(0)}, + {-aEst(1), aEst(0), 0} + }; + // clang-format on + + Matrix<float, 3, 6> H; + H << M, Matrix3f::Zero(3, 3); + Matrix<float, 6, 6> Pq = P.block<6, 6>(IDX_QUAT, IDX_QUAT); + Matrix<float, 3, 3> S = + H * Pq * H.transpose() + R_mag; // TODO: Change R_mag with R_acc + + Matrix<float, 6, 3> K = Pq * H.transpose() * S.inverse(); + + aEst.normalize(); + Vector3f e = acc - aEst; + Matrix<float, 6, 1> dx = K * e; + Vector4f r{0.5f * dx(0), 0.5f * dx(1), 0.5f * dx(2), + sqrtf(1.0f - 0.25f * dx.head<3>().squaredNorm())}; + + x.block<4, 1>(IDX_QUAT, 0) = SkyQuaternion::quatProd(r, q); + x.block<3, 1>(IDX_BIAS, 0) += dx.tail<3>(); + Matrix<float, 6, 6> tmp = Matrix<float, 6, 6>::Identity() - K * H; + Pq = tmp * Pq * tmp.transpose() + K * R_mag * K.transpose(); + P.block<6, 6>(IDX_QUAT, IDX_QUAT) = Pq; +} + +void NAS::correctPitot(const float deltaP, const float staticP) +{ + if (deltaP >= 0) + { + float c = 343; // Speed of sound + Vector3f vel = x.block<3, 1>(IDX_VEL, 0); + float vPitot; + + if (vel.norm() / c >= 0.9419) + { + float rho = 1.225; // Air density + vPitot = -sqrtf( + fabs(-2.0f * c * c + + sqrtf((4.0f * powf(c, 4) + 8.0f * c * c * deltaP / rho)))); + } + else + { + float gamma = 1.4; // Heat capacity ratio of dry air + float p0 = staticP + deltaP; + vPitot = + -sqrtf(c * c * 2 / (gamma - 1) * + fabs(powf((p0 / staticP), (gamma - 1) / gamma) - 1)); + } + + Matrix<float, 1, 6> H = Matrix<float, 1, 6>::Zero(); + H.coeffRef(0, 5) = 1; + + Eigen::Matrix<float, 6, 6> Pl = P.block<6, 6>(0, 0); + Matrix<float, 1, 1> S = + H * Pl * H.transpose() + Matrix<float, 1, 1>(config.SIGMA_PITOT); + + Matrix<float, 6, 1> K = Pl * H.transpose() * S.inverse(); + x.head<6>() = x.head<6>() + K * (vPitot - x[5]); + P.block<6, 6>(0, 0) = (Matrix<float, 6, 6>::Identity() - K * H) * Pl; + } +} + +NASState NAS::getState() const +{ + return NASState(TimestampTimer::getInstance().getTimestamp(), x); +} + +Eigen::Matrix<float, 13, 1> NAS::getX() const { return x; } + +void NAS::setState(const NASState& state) { this->x = state.getX(); } + +void NAS::setX(const Eigen::Matrix<float, 13, 1>& x) { this->x = x; } + +Matrix3f NAS::body2ned(const Vector4f& q) +{ + // clang-format off + return Matrix3f{ + { + q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3], + 2.0f * (q[0] * q[1] - q[2] * q[3]), + 2.0f * (q[0] * q[2] + q[1] * q[3]), + }, + { + 2.0f * (q[0] * q[1] + q[2] * q[3]), + - q[0] * q[0] + q[1] * q[1] - q[2] * q[2] + q[3] * q[3], + 2.0f * (q[1] * q[2] - q[0] * q[3]), + }, + { + 2.0f * (q[0] * q[2] - q[1] * q[3]), + 2.0f * (q[1] * q[2] + q[0] * q[3]), + - q[0] * q[0] - q[1] * q[1] + q[2] * q[2] + q[3] * q[3] + } + }; + // clang-format on +} + +} // namespace Boardcore diff --git a/src/shared/algorithms/NAS/NAS.h b/src/shared/algorithms/NAS/NAS.h new file mode 100644 index 0000000000000000000000000000000000000000..d232f4beb6f6951c1d8ecded27b3aea6c2c9a77f --- /dev/null +++ b/src/shared/algorithms/NAS/NAS.h @@ -0,0 +1,168 @@ +/* Copyright (c) 2020-2022 Skyward Experimental Rocketry + * Authors: Alessandro Del Duca, Luca Conterio, Marco Cella + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <utils/AeroUtils/AeroUtils.h> +#include <utils/Constants.h> + +#include <Eigen/Dense> + +#include "NASConfig.h" +#include "NASState.h" + +namespace Boardcore +{ + +class NAS +{ +public: + ///< Index of position elements in the state. + static constexpr uint16_t IDX_POS = 0; + + ///< Index of velocity elements in the state. + static constexpr uint16_t IDX_VEL = 3; + + ///< Index of quaternions elements in the state. + static constexpr uint16_t IDX_QUAT = 6; + + ///< Index of bias elements in the state. + static constexpr uint16_t IDX_BIAS = 10; + + explicit NAS(NASConfig config); + + /** + * @brief Prediction with accelerometer data. + * + * @param u Vector with acceleration data [x y z][m/s^2] + */ + void predictAcc(const Eigen::Vector3f& acceleration); + + /** + * @brief Prediction with gyroscope data. + * + * @param u Vector with angular velocity data [x y z] + */ + void predictGyro(const Eigen::Vector3f& angularVelocity); + + /** + * @brief Correction with barometer data. + * + * @param pressure Pressure read from the barometer [Pa] + * @param mslPress Pressure at sea level [Pa] + * @param mslTemp Temperature at sea level [Kelvin] + */ + void correctBaro(const float pressure, const float mslPress, + const float mslTemp); + + /** + * @brief Correction with gps data. + * + * @param gps Vector of the gps readings [n e vn ve][m m m/s m/s] + * @param sats_num Number of satellites available + */ + void correctGPS(const Eigen::Vector4f& gps); + + /** + * @brief Correction with magnetometer data. + * + * @param mag Normalized vector of the magnetometer readings [x y z] + */ + void correctMag(const Eigen::Vector3f& mag); + + /** + * @brief Correction with accelerometer data. + * + * @param u Normaliezed vector with acceleration data [x y z] + */ + void correctAcc(const Eigen::Vector3f& acceleration); + + /** + * @brief Correction with pitot pressure. + * + * Do not call this function after apogee! + * + * @param deltaP Delta pressure measured by the differential sensor [Pa] + * @param staticP Static pressure [Pa] + */ + void correctPitot(const float deltaP, const float staticP); + + /** + * @return EKF state. + */ + NASState getState() const; + + /** + * @return State vector [n e d vn ve vd qx qy qz qw bx by bz]. + */ + Eigen::Matrix<float, 13, 1> getX() const; + + /** + * @param state EKF state. + */ + void setState(const NASState& state); + + /** + * @param state State vector [n e d vn ve vd qx qy qz qw bx by bz]. + */ + void setX(const Eigen::Matrix<float, 13, 1>& x); + +private: + /** + * @brief Return a rotation matrix from body from to NED frame; + * + * TODO: Move to SkyQuaternion utilities + */ + Eigen::Matrix3f body2ned(const Eigen::Vector4f& q); + + ///< Extended Kalman filter configuration parameters + NASConfig config; + + ///< State vector [n e d vn ve vd qx qy qz qw bx by bz] + Eigen::Matrix<float, 13, 1> x; + + ///< Covariance matrix + Eigen::Matrix<float, 12, 12> P; + + ///< NED gravity vector [m/s^2] + Eigen::Vector3f gravityNed{0.0f, 0.0f, -Constants::g}; + + // Utility matrixes used for the gps + Eigen::Matrix<float, 4, 6> H_gps; + Eigen::Matrix<float, 6, 4> H_gps_tr; + Eigen::Matrix<float, 4, 4> R_gps; + + // Utility matrixes used for the magnetometer + Eigen::Matrix3f R_mag; + + // Other utility matrixes + Eigen::Matrix<float, 6, 6> Q_quat; + Eigen::Matrix<float, 6, 6> Q_lin; + Eigen::Matrix<float, 6, 6> F_lin; + Eigen::Matrix<float, 6, 6> F_lin_tr; + Eigen::Matrix<float, 6, 6> F_quat; + Eigen::Matrix<float, 6, 6> F_quat_tr; + Eigen::Matrix<float, 6, 6> G_quat; + Eigen::Matrix<float, 6, 6> G_quat_tr; +}; + +} // namespace Boardcore diff --git a/src/shared/algorithms/NAS/NASConfig.h b/src/shared/algorithms/NAS/NASConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..b73b2f8f8b3bdf6a5eb1d48194d81f9a8ad0ad75 --- /dev/null +++ b/src/shared/algorithms/NAS/NASConfig.h @@ -0,0 +1,56 @@ +/* Copyright (c) 2022 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <Eigen/Dense> + +namespace Boardcore +{ + +struct NASConfig +{ + float T; ///< [s] Sample period + float SIGMA_BETA; ///< [rad/s^2] Estimated gyroscope bias variance + float SIGMA_W; ///< [rad^2] Estimated gyroscope variance + float SIGMA_MAG; ///< [uT^2] Estimated magnetometer variance + float SIGMA_GPS; ///< [m^2] Estimated GPS variance + float SIGMA_BAR; ///< [m^2] Estimated altitude variance + float SIGMA_POS; ///< [m^2] Estimated variance of the position noise + float SIGMA_VEL; ///< [(m/s)^2] Estimated variance of the velocity noise + float SIGMA_PITOT; ///< [Pa^2] Estimated variance of the pitot velocity + + float P_POS; ///< Position prediction covariance + float P_POS_VERTICAL; + + float P_VEL; ///< Velocity prediction covariance + float P_VEL_VERTICAL; + + float P_ATT; ///< Attitude prediction covariance + float P_BIAS; ///< Bias prediction covariance + + float SATS_NUM = 6.0f; ///< Number of satellites used at setup time + + Eigen::Vector3f NED_MAG; ///< Normalized magnetic field vector in NED frame +}; + +} // namespace Boardcore diff --git a/src/shared/algorithms/NAS/NASState.h b/src/shared/algorithms/NAS/NASState.h new file mode 100644 index 0000000000000000000000000000000000000000..6c5a39db9c12ba68a643b5755a1bd26085ebd205 --- /dev/null +++ b/src/shared/algorithms/NAS/NASState.h @@ -0,0 +1,85 @@ +/* Copyright (c) 2022 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <Eigen/Core> + +namespace Boardcore +{ + +struct NASState +{ + uint64_t timestamp; + + // 13 extended kalman states + + // Position [m] + float n = 0; + float e = 0; + float d = 0; + + // Velocity [m/s] + float vn = 0; + float ve = 0; + float vd = 0; + + // Attitude as quaternion + float qx = 0; + float qy = 0; + float qz = 0; + float qw = 1; + + // Gyroscope bias + float bx = 0; + float by = 0; + float bz = 0; + + NASState() {} + + NASState(uint64_t timestamp, Eigen::Matrix<float, 13, 1> x) + : timestamp(timestamp), n(x(0)), e(x(1)), d(x(2)), vn(x(3)), ve(x(4)), + vd(x(5)), qx(x(6)), qy(x(7)), qz(x(8)), qw(x(9)), bx(x(10)), + by(x(11)), bz(x(12)) + { + } + + Eigen::Matrix<float, 13, 1> getX() const + { + return Eigen::Matrix<float, 13, 1>(n, e, d, vn, ve, vd, qx, qy, qz, qw, + bx, by, bz); + } + + static std::string header() + { + return "timestamp,n,e,d,vn,ve,vd,qx,qy,qz,qw,bx,by,bz\n"; + } + + void print(std::ostream& os) const + { + os << timestamp << "," << n << "," << e << "," << d << "," << vn << "," + << ve << "," << vd << "," << qx << "," << qy << "," << qz << "," + << qw << "," << bx << "," << by << "," << bz << "\n"; + } +}; + +} // namespace Boardcore diff --git a/src/shared/algorithms/NAS/StateInitializer.h b/src/shared/algorithms/NAS/StateInitializer.h new file mode 100644 index 0000000000000000000000000000000000000000..959597beb70db8e4b2133eacf12df22d90f10add --- /dev/null +++ b/src/shared/algorithms/NAS/StateInitializer.h @@ -0,0 +1,169 @@ +/* Copyright (c) 2022 Skyward Experimental Rocketry + * Authors: Marco Cella, Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <diagnostic/PrintLogger.h> +#include <utils/AeroUtils/AeroUtils.h> +#include <utils/SkyQuaternion/SkyQuaternion.h> + +#include <Eigen/Dense> + +#include "NAS.h" + +namespace Boardcore +{ + +/** + * @brief Utility used to initialize the extended kalman filter's state. + * + * The intended use is the following: + * - Instantiate the object, the state is initialize to zero; + * - Call positionInit to set the initial position; + * - Call either eCompass or triad to set the initial orientation; + * - Get the initial state for the kalman with getInitX + */ +class StateInitializer +{ +public: + /** + * @brief Initialize the state of the Extended Kalman Filter. + * + * The state is initialized to zero. Velocity should be null and gyro biases + * can be left to zero since the sensor performs self-calibration and the + * measurements are already compensated. + */ + StateInitializer(); + + /** + * @brief Ecompass algorithm to estimate the attitude before the liftoff. + * + * ecompass reference: + * https://de.mathworks.com/help/fusion/ref/ecompass.html + * + * @param acc 3x1 accelerometer readings [x y z][m/s^2]. + * @param mag 3x1 magnetometer readings [x y z][uT]. + * + */ + void eCompass(const Eigen::Vector3f acc, const Eigen::Vector3f mag); + + /** + * @brief Triad algorithm to estimate the attitude before the liftoff. + * + * triad reference: + * https://en.wikipedia.org/wiki/Triad_method + * https://www.aero.iitb.ac.in/satelliteWiki/index.php/Triad_Algorithm + * + * @param acc Normalized accelerometer readings [x y z]. + * @param mag Normalized magnetometer readings [x y z]. + * @param nedMag Normalized magnetic field vector in NED frame [x y z]. + */ + void triad(Eigen::Vector3f& acc, Eigen::Vector3f& mag, + Eigen::Vector3f& nedMag); + + /** + * @brief Initialization of the position at a specific altitude + * + * @param pressure Current pressure [Pas] + * @param pressureRef Pressure at reference altitude (must be > 0) [Pa] + * @param temperatureRef Temperature at reference altitude [K] + */ + void positionInit(const float pressure, const float pressureRef, + const float temperatureRef); + + Eigen::Matrix<float, 13, 1> getInitX(); + +private: + Eigen::Matrix<float, 13, 1> x_init; + + PrintLogger log = Logging::getLogger("initstates"); +}; + +StateInitializer::StateInitializer() { x_init << Eigen::MatrixXf::Zero(13, 1); } + +void StateInitializer::eCompass(const Eigen::Vector3f acc, + const Eigen::Vector3f mag) +{ + // ndr: since this method runs only when the rocket is stationary, there's + // no need to add the gravity vector because the accelerometers already + // measure it. This is not true if we consider the flying rocket. + + Eigen::Vector3f am(acc.cross(mag)); + + Eigen::Matrix3f R; + R << am.cross(acc), am, acc; + R.col(0).normalize(); + R.col(1).normalize(); + R.col(2).normalize(); + + Eigen::Vector4f x_quat = SkyQuaternion::rotationMatrix2quat(R); + + x_init(NAS::IDX_QUAT) = x_quat(0); + x_init(NAS::IDX_QUAT + 1) = x_quat(1); + x_init(NAS::IDX_QUAT + 2) = x_quat(2); + x_init(NAS::IDX_QUAT + 3) = x_quat(3); +} + +void StateInitializer::triad(Eigen::Vector3f& acc, Eigen::Vector3f& mag, + Eigen::Vector3f& nedMag) +{ + // The gravity vector is expected to be read inversely because + // accelerometers read the binding reaction + Eigen::Vector3f nedAcc(0.0f, 0.0f, -1.0f); + + // Compute the reference triad + Eigen::Vector3f R1 = nedAcc; + Eigen::Vector3f R2 = nedAcc.cross(nedMag).normalized(); + Eigen::Vector3f R3 = R1.cross(R2); + + // Compute the measured triad + Eigen::Vector3f r1 = acc; + Eigen::Vector3f r2 = acc.cross(mag).normalized(); + Eigen::Vector3f r3 = r1.cross(r2); + + // Compose the reference and measured matrixes + Eigen::Matrix3f M; + M << R1, R2, R3; + Eigen::Matrix3f m; + m << r1, r2, r3; + + // Compute the rotation matrix and the corresponding quaternion + Eigen::Matrix3f A = M * m.transpose(); + Eigen::Vector4f q = SkyQuaternion::rotationMatrix2quat(A); + + // Save the orientation in the state + x_init.block<4, 1>(NAS::IDX_QUAT, 0) = q; +} + +void StateInitializer::positionInit(const float pressure, + const float pressureRef, + const float temperatureRef) +{ + x_init(0) = 0.0; + x_init(1) = 0.0; + // msl altitude (in NED, so negative) + x_init(2) = -Aeroutils::relAltitude(pressure, pressureRef, temperatureRef); +} + +Eigen::Matrix<float, 13, 1> StateInitializer::getInitX() { return x_init; } + +} // namespace Boardcore diff --git a/src/shared/algorithms/kalman/ExtendedKalmanEigen/ExtendedKalmanEigen.cpp b/src/shared/algorithms/kalman/ExtendedKalmanEigen/ExtendedKalmanEigen.cpp deleted file mode 100644 index c3fe8804bd3368b1171befeaf24d688df91f7c39..0000000000000000000000000000000000000000 --- a/src/shared/algorithms/kalman/ExtendedKalmanEigen/ExtendedKalmanEigen.cpp +++ /dev/null @@ -1,304 +0,0 @@ -/* Copyright (c) 2020 Skyward Experimental Rocketry - * Authors: Alessandro Del Duca, Luca Conterio, Marco Cella - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#include <NavigationAttitudeSystem/ExtendedKalmanEigen.h> - -using namespace Boardcore; -using namespace Eigen; - -namespace DeathStackBoard -{ - -using namespace NASConfigs; - -ExtendedKalmanEigen::ExtendedKalmanEigen() -{ - eye6.setIdentity(); - eye4.setIdentity(); - eye3.setIdentity(); - eye2.setIdentity(); - - // clang-format off - - R_bar << SIGMA_BAR; - R_mag << SIGMA_MAG * eye3; - R_gps << eye4 * SIGMA_GPS / SATS_NUM; - Q_mag << (SIGMA_W * SIGMA_W * T + - (1.0F / 3.0F) * SIGMA_BETA * SIGMA_BETA * T * T * T) * - eye3, - (0.5F * SIGMA_BETA * SIGMA_BETA * T * T) * eye3, - // cppcheck-suppress constStatement - (0.5F * SIGMA_BETA * SIGMA_BETA * T * T) * eye3, - (SIGMA_BETA * SIGMA_BETA * T) * eye3; - - P_pos << P_POS, 0, 0, - 0, P_POS, 0, - 0, 0, P_POS_VERTICAL; - P_vel << P_VEL, 0, 0, - 0, P_VEL, 0, - 0, 0, P_VEL_VERTICAL; - P_att = eye3 * P_ATT; - P_bias = eye3 * P_BIAS; - P << P_pos, Eigen::MatrixXf::Zero(3, N - 4), Eigen::MatrixXf::Zero(3, 3), P_vel, - Eigen::MatrixXf::Zero(3, N - 7), Eigen::MatrixXf::Zero(3, 6), P_att, - Eigen::MatrixXf::Zero(3, N - 10), Eigen::MatrixXf::Zero(3, 9), P_bias; - - Q_pos = eye3 * SIGMA_POS; - Q_vel = eye3 * SIGMA_VEL; - Q_lin << Q_pos, Eigen::MatrixXf::Zero(3, NL - 3), Eigen::MatrixXf::Zero(3, 3), Q_vel; - - F << 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, - Eigen::MatrixXf::Zero(3, NL); - F = eye6 + T * F; - Ftr = F.transpose(); - - H_gps << 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, - // cppcheck-suppress constStatement - 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F; - H_gpstr = H_gps.transpose(); - - Fatt << -eye3, -eye3 * T, Eigen::Matrix3f::Zero(3, 3), eye3; - Fatttr = Fatt.transpose(); - - Gatt << -eye3, Eigen::Matrix3f::Zero(3, 3), - Eigen::Matrix3f::Zero(3, 3), eye3; - Gatttr = Gatt.transpose(); - - g << 0.0F, 0.0F, aeroutils::constants::g; // [m/s^2] - - // clang-format on -} - -void ExtendedKalmanEigen::predict(const Eigen::Vector3f& u) -{ - Eigen::Matrix3f A; - Eigen::Vector3f a; - VectorNf x_dot; - - Plin = P.block<NL, NL>(0, 0); - P.block<NL, NL>(0, 0) = F * Plin * Ftr + Q_lin; - - float q1 = - x(NL); // Since the index begins from 0, the first quaternion - // component will be at index = # linear states before itself - float q2 = x(NL + 1); - float q3 = x(NL + 2); - float q4 = x(NL + 3); - - A << powf(q1, 2) - powf(q2, 2) - powf(q3, 2) + powf(q4, 2), - 2.0F * q1 * q2 + 2.0F * q3 * q4, 2.0F * q1 * q3 - 2.0F * q2 * q4, - 2.0F * q1 * q2 - 2.0F * q3 * q4, - -powf(q1, 2) + powf(q2, 2) - powf(q3, 2) + powf(q4, 2), - 2.0F * q2 * q3 + 2.0F * q1 * q4, 2.0F * q1 * q3 + 2.0F * q2 * q4, - // cppcheck-suppress constStatement - 2.0F * q2 * q3 - 2.0F * q1 * q4, - -powf(q1, 2) - powf(q2, 2) + powf(q3, 2) + powf(q4, 2); - - a = A.transpose() * u + - g; // Real accelerometers don't measure g during the flight - - x_dot << x(3), x(4), x(5), a, - Eigen::MatrixXf::Zero( - NATT, - 1); // The quaternions and the biases don't need to be updated - - x = x + T * x_dot; -} - -void ExtendedKalmanEigen::correctBaro(const float y, const float msl_press, - const float msl_temp) -{ - Matrix<float, NL, NBAR> K_bar; - Matrix<float, NBAR, NL> H_bar; - Matrix<float, NBAR, NBAR> S_bar; - - Plin = P.block<NL, NL>(0, 0); - - // temperature at current altitude : - // since x(2) (altitude) is negative, mslTemperature returns temperature at - // current altitude and not at mean sea level - float temp = aeroutils::mslTemperature(msl_temp, x(2)); - - // compute gradient of the altitude-pressure function - float dp_dx = aeroutils::constants::a * aeroutils::constants::n * - msl_press * - powf(1 - aeroutils::constants::a * x(2) / temp, - -aeroutils::constants::n - 1) / - temp; - - H_bar << 0.0F, 0.0F, dp_dx, - Eigen::MatrixXf::Zero(1, N - 3 - NATT); // Gradient of h_bar - - S_bar = H_bar * Plin * H_bar.transpose() + R_bar; - - K_bar = Plin * H_bar.transpose() * S_bar.inverse(); - - P.block<NL, NL>(0, 0) = (eye6 - K_bar * H_bar) * Plin; - - float y_hat = aeroutils::mslPressure(msl_press, msl_temp, x(2)); - - x.head(NL) = x.head(NL) + K_bar * (y - y_hat); - - // float res_bar = y - h_bar; -} - -void ExtendedKalmanEigen::correctGPS(const Vector4f& y, const uint8_t sats_num) -{ - Matrix<float, NGPS, 1> h_gps; - Matrix<float, NL, NGPS> K_gps; - Matrix<float, NGPS, 1> res_gps; - Matrix<float, NGPS, NGPS> S_gps; - - float xnord = y(0); - float yest = y(1); - float velnord = y(2); - float velest = y(3); - - Matrix<float, NGPS, 1> yned(xnord, yest, velnord, velest); - - R_gps = eye4 * SIGMA_GPS / sqrtf(sats_num); - - Plin = P.block<NL, NL>(0, 0); - - S_gps = H_gps * Plin * H_gpstr + R_gps; - - K_gps = Plin * H_gpstr * S_gps.inverse(); - - P.block<NL, NL>(0, 0) = (eye6 - K_gps * H_gps) * Plin; - - h_gps << x(0), x(1), x(3), x(4); - - x.head(NL) = x.head(NL) + K_gps * (yned - h_gps); - - // cppcheck-suppress unreadVariable - res_gps = y - h_gps; -} - -const VectorNf& ExtendedKalmanEigen::getState() { return x; } - -void ExtendedKalmanEigen::setX(const VectorNf& x) { this->x = x; } - -/* - MULTIPLICATIVE EXTENDED KALMAN FILTER -*/ -void ExtendedKalmanEigen::predictMEKF(const Eigen::Vector3f& u) -{ - Eigen::Vector3f omega; - Eigen::Vector3f prev_bias; - Matrix4f omega_mat; - Eigen::Matrix3f omega_submat; - - q << x(NL), x(NL + 1), x(NL + 2), x(NL + 3); - prev_bias << x(NL + 4), x(NL + 5), x(NL + 6); - - omega = u - prev_bias; - - omega_submat << 0.0F, -omega(2), omega(1), omega(2), 0.0F, -omega(0), - // cppcheck-suppress constStatement - -omega(1), omega(0), 0.0F; - - // cppcheck-suppress constStatement - omega_mat << -omega_submat, omega, -omega.transpose(), 0.0F; - - q = (eye4 + 0.5F * omega_mat * T) * q; - q.normalize(); - - // cppcheck-suppress constStatement - x.tail(NATT) << q, prev_bias; - - Patt = P.block<NMEKF, NMEKF>( - NL, NL); // The block of P related to the attitude starts at (NL, NL) - // because the index starts from 0. The block is of size - // NMEKF x NMEKF. - P.block<NMEKF, NMEKF>(NL, NL) = - Fatt * Patt * Fatttr + - Gatt * Q_mag * Gatttr; // Update only the attitude related part of P -} - -void ExtendedKalmanEigen::correctMEKF(const Eigen::Vector3f& y) -{ - Eigen::Matrix3f A; - Eigen::Vector3f z; - Vector4f r; - Eigen::Matrix3f z_mat; - Vector4f u_att; - Eigen::Vector3f r_sub; - Matrix<float, NMAG, NMAG> Satt; - Matrix<float, NMAG, NMEKF> Hatt; - Matrix<float, NMEKF, NMAG> Katt; - Matrix<float, NMEKF, 1> delta_x; - - float q1 = x(NL); - float q2 = x(NL + 1); - float q3 = x(NL + 2); - float q4 = x(NL + 3); - - A << powf(q1, 2) - powf(q2, 2) - powf(q3, 2) + powf(q4, 2), - 2.0F * q1 * q2 + 2.0F * q3 * q4, 2.0F * q1 * q3 - 2.0F * q2 * q4, - 2.0F * q1 * q2 - 2.0F * q3 * q4, - -powf(q1, 2) + powf(q2, 2) - powf(q3, 2) + powf(q4, 2), - 2.0F * q2 * q3 + 2.0F * q1 * q4, 2.0F * q1 * q3 + 2.0F * q2 * q4, - // cppcheck-suppress constStatement - 2.0F * q2 * q3 - 2.0F * q1 * q4, - -powf(q1, 2) - powf(q2, 2) + powf(q3, 2) + powf(q4, 2); - - z = A * NED_MAG; - - // cppcheck-suppress constStatement - z_mat << 0.0F, -z(2), z(1), z(2), 0.0F, -z(0), -z(1), z(0), 0.0F; - - Hatt << z_mat, Eigen::Matrix3f::Zero(3, 3); - - Patt = P.block<NMEKF, NMEKF>(NL, NL); - Satt = Hatt * Patt * Hatt.transpose() + R_mag; - - Katt = Patt * Hatt.transpose() * Satt.inverse(); - - delta_x = Katt * (y - z); - - r_sub << delta_x(0), delta_x(1), delta_x(2); - - r << 0.5F * r_sub, sqrtf(1.0F - 0.25F * r_sub.transpose() * r_sub); - - // cppcheck-suppress constStatement - q << q1, q2, q3, q4; - u_att = quater.quatProd(r, q); - float u_norm = u_att.norm(); - - x(NL) = u_att(0) / u_norm; - x(NL + 1) = u_att(1) / u_norm; - x(NL + 2) = u_att(2) / u_norm; - x(NL + 3) = u_att(3) / u_norm; - x(NL + 4) = x(NL + 4) + delta_x(3); - x(NL + 5) = x(NL + 5) + delta_x(4); - x(NL + 6) = x(NL + 6) + delta_x(5); - - P.block<NMEKF, NMEKF>(NL, NL) = - (eye6 - Katt * Hatt) * Patt * (eye6 - Katt * Hatt).transpose() + - Katt * R_mag * Katt.transpose(); -} - -} // namespace DeathStackBoard diff --git a/src/shared/algorithms/kalman/ExtendedKalmanEigen/ExtendedKalmanEigen.h b/src/shared/algorithms/kalman/ExtendedKalmanEigen/ExtendedKalmanEigen.h deleted file mode 100644 index 4b1f5ca73bbd29ddbae0a5f310eda04da63c8aba..0000000000000000000000000000000000000000 --- a/src/shared/algorithms/kalman/ExtendedKalmanEigen/ExtendedKalmanEigen.h +++ /dev/null @@ -1,132 +0,0 @@ -/* Copyright (c) 2020 Skyward Experimental Rocketry - * Authors: Alessandro Del Duca, Luca Conterio, Marco Cella - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#pragma once - -#include <configs/NASConfig.h> -#include <math/SkyQuaternion.h> -#include <utils/aero/AeroUtils.h> - -#include <Eigen/Dense> - -namespace DeathStackBoard -{ - -using VectorNf = Eigen::Matrix<float, NASConfigs::N, 1>; - -class ExtendedKalmanEigen -{ - -public: - ExtendedKalmanEigen(); - - /** - * @brief Prediction step of the EKF. - * - * @param u 3x1 Vector of the accelerometer readings [ax ay az]. - */ - void predict(const Eigen::Vector3f& u); - - /** - * @brief EKF correction of the barometer data. - * - * @param y Pressure read from the barometer [Pa] - * @param msl_press Pressure at sea level [Pa] - * @param msl_temp Temperature at sea level [Kelvin] - */ - void correctBaro(const float y, const float msl_press, - const float msl_temp); - - /** - * @brief EKF correction of the gps readings. - * - * @param y 4x1 Vector of the gps readings [longitude, latitude, - * gps_nord_vel, gps_east_vel]. - * @param sats_num Number of satellites available - */ - void correctGPS(const Eigen::Vector4f& y, const uint8_t sats_num); - - /** - * @brief Prediction step of the Multiplicative EKF. - * - * @param u 3x1 Vector of the gyroscope readings [wx wy wz]. - */ - void predictMEKF(const Eigen::Vector3f& u); - - /** - * @brief MEKF correction of the magnetometer readings. - * - * @param y 3x1 Vector of the magnetometer readings [mx my mz]. - */ - void correctMEKF(const Eigen::Vector3f& y); - - /** - * @return 13x1 State vector [px py pz vx vy vz qx qy qz qw bx by bz]. - */ - const VectorNf& getState(); - - /** - * @param x 13x1 State vector [px py pz vx vy vz qx qy qz qw bx by bz]. - */ - void setX(const VectorNf& x); - -private: - VectorNf x; - Eigen::Matrix<float, NASConfigs::NP, NASConfigs::NP> P; - Eigen::Matrix<float, NASConfigs::NL, NASConfigs::NL> F; - Eigen::Matrix<float, NASConfigs::NL, NASConfigs::NL> Ftr; - - Eigen::Matrix3f P_pos; - Eigen::Matrix3f P_vel; - Eigen::Matrix3f P_att; - Eigen::Matrix3f P_bias; - Eigen::Matrix<float, NASConfigs::NL, NASConfigs::NL> Plin; - - Eigen::Matrix3f Q_pos; - Eigen::Matrix3f Q_vel; - Eigen::Matrix<float, NASConfigs::NL, NASConfigs::NL> Q_lin; - - Eigen::Vector3f g; - Eigen::Matrix2f eye2; - Eigen::Matrix3f eye3; - Eigen::Matrix4f eye4; - Eigen::Matrix<float, 6, 6> eye6; - - Eigen::Matrix<float, NASConfigs::NBAR, NASConfigs::NBAR> R_bar; - - Eigen::Matrix<float, NASConfigs::NGPS, NASConfigs::NGPS> R_gps; - Eigen::Matrix<float, NASConfigs::NGPS, NASConfigs::NL> H_gps; - Eigen::Matrix<float, NASConfigs::NL, NASConfigs::NGPS> H_gpstr; - - Eigen::Vector4f q; - Eigen::Matrix<float, NASConfigs::NMAG, NASConfigs::NMAG> R_mag; - Eigen::Matrix<float, NASConfigs::NMEKF, NASConfigs::NMEKF> Q_mag; - Eigen::Matrix<float, NASConfigs::NMEKF, NASConfigs::NMEKF> Fatt; - Eigen::Matrix<float, NASConfigs::NMEKF, NASConfigs::NMEKF> Fatttr; - Eigen::Matrix<float, NASConfigs::NMEKF, NASConfigs::NMEKF> Gatt; - Eigen::Matrix<float, NASConfigs::NMEKF, NASConfigs::NMEKF> Gatttr; - Eigen::Matrix<float, NASConfigs::NMEKF, NASConfigs::NMEKF> Patt; - - Boardcore::SkyQuaternion quater; -}; - -} // namespace DeathStackBoard diff --git a/src/shared/diagnostic/LogSink.h b/src/shared/diagnostic/LogSink.h index 1efbaf52e445a8e8616f0c213688a85e048c3db2..c06917f36c5edf7869f92b7240815ff1a89322a9 100644 --- a/src/shared/diagnostic/LogSink.h +++ b/src/shared/diagnostic/LogSink.h @@ -33,7 +33,7 @@ class LogSink { public: LogSink() {} - LogSink(const LogSink&) = delete; + LogSink(const LogSink&) = delete; LogSink& operator=(const LogSink&) = delete; virtual ~LogSink() {} diff --git a/src/shared/diagnostic/PrintLogger.h b/src/shared/diagnostic/PrintLogger.h index 02758ee784a9e63a93d285283741efa2d5b8dd23..d8b253b77e4aa9264b0227fd32926af43ef1ee36 100644 --- a/src/shared/diagnostic/PrintLogger.h +++ b/src/shared/diagnostic/PrintLogger.h @@ -100,7 +100,7 @@ class Logging : public Singleton<Logging> friend class PrintLogger; public: - static PrintLogger getLogger(string name) + static PrintLogger getLogger(const string& name) { return PrintLogger(getInstance(), name); } diff --git a/src/shared/diagnostic/StackData.h b/src/shared/diagnostic/StackData.h index 5e9f58b702cc7d3490832fc71880131875748707..4441aafd0c3891986e11d5242f58207f2b961698 100644 --- a/src/shared/diagnostic/StackData.h +++ b/src/shared/diagnostic/StackData.h @@ -35,6 +35,7 @@ enum ThreadId : uint8_t THID_MAV_RECEIVER, THID_MAV_SENDER, THID_XBEE, + THID_GPS, THID_EVT_BROKER, THID_LOGGER_PACK, THID_LOGGER_WRITE, diff --git a/src/shared/drivers/runcam/Runcam.h b/src/shared/drivers/runcam/Runcam.h index fe089afee56c02845d0f084be8ec3ceb7bbaea00..eabb564aee0b43d768e2b0c15fa0f490f4d2310f 100644 --- a/src/shared/drivers/runcam/Runcam.h +++ b/src/shared/drivers/runcam/Runcam.h @@ -130,7 +130,7 @@ public: private: /** - * @brief Confiugre Serial Communication + * @brief Configure Serial Communication */ bool configureSerialCommunication(); diff --git a/src/shared/drivers/spi/SPI.h b/src/shared/drivers/spi/SPI.h index 855cd260a39ab06d43984776306e97127f953f51..950e276edc563aae76dc714ee95aa078457fe1a2 100644 --- a/src/shared/drivers/spi/SPI.h +++ b/src/shared/drivers/spi/SPI.h @@ -29,7 +29,7 @@ #ifndef USE_MOCK_PERIPHERALS using SPIType = SPI_TypeDef; #else -#include <utils/testutils/FakeSpiTypedef.h> +#include <utils/TestUtils/FakeSpiTypedef.h> using SPIType = FakeSpiTypedef; #endif @@ -205,7 +205,7 @@ public: * @param data Buffer containing data to write. * @param size Size of the buffer in bytes. */ - void write(uint8_t *data, size_t nBytes); + void write(const uint8_t *data, size_t nBytes); /** * @brief Writes multiple half words to the bus. @@ -213,7 +213,7 @@ public: * @param data Buffer containing data to write. * @param size Size of the buffer in bytes. */ - void write(uint16_t *data, size_t nBytes); + void write(const uint16_t *data, size_t nBytes); /** * @brief Full duplex transmission of one byte on the bus. @@ -393,7 +393,7 @@ inline void SPI::write(uint16_t data) (void)spi->DR; } -inline void SPI::write(uint8_t *data, size_t nBytes) +inline void SPI::write(const uint8_t *data, size_t nBytes) { // Write the first data item in the Tx buffer spi->DR = data[0]; @@ -419,7 +419,7 @@ inline void SPI::write(uint8_t *data, size_t nBytes) (void)spi->DR; } -inline void SPI::write(uint16_t *data, size_t nBytes) +inline void SPI::write(const uint16_t *data, size_t nBytes) { // Set 16 bit frame format set16BitFrameFormat(); diff --git a/src/shared/drivers/spi/SPIBus.h b/src/shared/drivers/spi/SPIBus.h index 4706f158dcf519ee68ba74ae1830de7441b78e03..0d5b0c0b7faeb8c166fd55b8e454a1409ba22aac 100644 --- a/src/shared/drivers/spi/SPIBus.h +++ b/src/shared/drivers/spi/SPIBus.h @@ -46,10 +46,10 @@ public: SPIBus(SPIType* spi); ///< Delete copy/move contructors/operators. - SPIBus(const SPIBus&) = delete; + SPIBus(const SPIBus&) = delete; SPIBus& operator=(const SPIBus&) = delete; SPIBus(SPIBus&&) = delete; - SPIBus& operator=(SPIBus&&) = delete; + SPIBus& operator=(SPIBus&&) = delete; /** * @brief Configures and enables the bus with the provided configuration. @@ -168,37 +168,43 @@ public: private: SPI spi; SPIBusConfig config{}; + bool firstConfigApplied = false; }; inline SPIBus::SPIBus(SPIType* spi) : spi(spi) {} inline void SPIBus::configure(SPIBusConfig newConfig) { - // Save the new configuration - config = newConfig; + // Do not reconfigure if already in the correct configuration. + if (!firstConfigApplied || newConfig != config) + { + // Save the new configuration + config = newConfig; + firstConfigApplied = true; - // Wait until the peripheral is done before changing configuration - spi.waitPeripheral(); + // Wait until the peripheral is done before changing configuration + spi.waitPeripheral(); - // Disable the peripheral - spi.disable(); + // Disable the peripheral + spi.disable(); - // Configure clock polarity and phase - spi.setMode(config.mode); + // Configure clock polarity and phase + spi.setMode(config.mode); - // Configure clock frequency - spi.setClockDiver(config.clockDivider); + // Configure clock frequency + spi.setClockDiver(config.clockDivider); - // Configure bit order - spi.setBitOrder(config.bitOrder); + // Configure bit order + spi.setBitOrder(config.bitOrder); - // Configure chip select and master mode - spi.enableSoftwareSlaveManagement(); - spi.enableInternalSlaveSelection(); - spi.setMasterConfiguration(); + // Configure chip select and master mode + spi.enableSoftwareSlaveManagement(); + spi.enableInternalSlaveSelection(); + spi.setMasterConfiguration(); - // Enable the peripheral - spi.enable(); + // Enable the peripheral + spi.enable(); + } } inline void SPIBus::select(GpioType& cs) diff --git a/src/shared/drivers/spi/SPIBusInterface.h b/src/shared/drivers/spi/SPIBusInterface.h index e0eb8bd38f5366451aff51f82aec89102fbb328e..f10a7e33228d5b43bf701e5e8178cead2abf8411 100644 --- a/src/shared/drivers/spi/SPIBusInterface.h +++ b/src/shared/drivers/spi/SPIBusInterface.h @@ -29,7 +29,7 @@ #ifndef USE_MOCK_PERIPHERALS using GpioType = miosix::GpioPin; #else -#include <utils/testutils/MockGpioPin.h> +#include <utils/TestUtils/MockGpioPin.h> using GpioType = MockGpioPin; #endif @@ -93,10 +93,10 @@ public: SPIBusInterface() {} ///< Delete copy/move contructors/operators. - SPIBusInterface(const SPIBusInterface&) = delete; + SPIBusInterface(const SPIBusInterface&) = delete; SPIBusInterface& operator=(const SPIBusInterface&) = delete; SPIBusInterface(SPIBusInterface&&) = delete; - SPIBusInterface& operator=(SPIBusInterface&&) = delete; + SPIBusInterface& operator=(SPIBusInterface&&) = delete; /** * @brief Configures the bus with the provided configuration parameters. diff --git a/src/shared/drivers/spi/SPISlaveBus.h b/src/shared/drivers/spi/SPISlaveBus.h index a530c7bd0cfc44e81dad78b60abd7d86764eb3c4..fcd274c25937ff768f112e0312176d0d3344024f 100644 --- a/src/shared/drivers/spi/SPISlaveBus.h +++ b/src/shared/drivers/spi/SPISlaveBus.h @@ -38,10 +38,10 @@ public: SPISlaveBus(SPIType* spi, SPISignalGenerator signalGenerator); ///< Delete copy/move contructors/operators. - SPISlaveBus(const SPISlaveBus&) = delete; + SPISlaveBus(const SPISlaveBus&) = delete; SPISlaveBus& operator=(const SPISlaveBus&) = delete; SPISlaveBus(SPISlaveBus&&) = delete; - SPISlaveBus& operator=(SPISlaveBus&&) = delete; + SPISlaveBus& operator=(SPISlaveBus&&) = delete; /** * @brief Configures and enables the bus with the provided configuration. diff --git a/src/shared/drivers/spi/SPITransaction.h b/src/shared/drivers/spi/SPITransaction.h index 3cc057519782f9a0abdc4ce96c9e0e440ad2ce33..6c98cafbe1b4ba524f8ade19e399acb4c2b61b47 100644 --- a/src/shared/drivers/spi/SPITransaction.h +++ b/src/shared/drivers/spi/SPITransaction.h @@ -84,10 +84,10 @@ public: WriteBit writeBit = WriteBit::NORMAL); ///< Delete copy/move contructors/operators. - SPITransaction(const SPITransaction &) = delete; + SPITransaction(const SPITransaction &) = delete; SPITransaction &operator=(const SPITransaction &) = delete; SPITransaction(SPITransaction &&) = delete; - SPITransaction &operator=(SPITransaction &&) = delete; + SPITransaction &operator=(SPITransaction &&) = delete; /** * @brief Returns the underlying bus for low level access. diff --git a/src/shared/drivers/timer/BasicTimer.h b/src/shared/drivers/timer/BasicTimer.h index 1260739b7c1f3bc8f8357c102d623630ec8b43cf..57c8476a4170ec10823c1743162fa0e619821bc5 100644 --- a/src/shared/drivers/timer/BasicTimer.h +++ b/src/shared/drivers/timer/BasicTimer.h @@ -85,12 +85,14 @@ public: explicit BasicTimer(TIM_TypeDef *timer); /** - * @brief Resets the registers and disables the peripheral clock. + * @brief Disables the peripheral clock. */ ~BasicTimer(); TIM_TypeDef *getTimer(); + uint8_t getTimerNumber(); + /** * @brief Resets the timer configuration to the default state. * @@ -207,14 +209,44 @@ inline BasicTimer::BasicTimer(TIM_TypeDef *timer) : timer(timer) ClockUtils::enablePeripheralClock(timer); } -inline BasicTimer::~BasicTimer() -{ - reset(); - ClockUtils::disablePeripheralClock(timer); -} +inline BasicTimer::~BasicTimer() { ClockUtils::disablePeripheralClock(timer); } inline TIM_TypeDef *BasicTimer::getTimer() { return timer; } +inline uint8_t BasicTimer::getTimerNumber() +{ + if (timer == TIM1) + return 1; + else if (timer == TIM2) + return 2; + else if (timer == TIM3) + return 3; + else if (timer == TIM4) + return 4; + else if (timer == TIM5) + return 5; + else if (timer == TIM6) + return 6; + else if (timer == TIM7) + return 7; + else if (timer == TIM8) + return 8; + else if (timer == TIM9) + return 9; + else if (timer == TIM10) + return 10; + else if (timer == TIM11) + return 11; + else if (timer == TIM12) + return 12; + else if (timer == TIM13) + return 13; + else if (timer == TIM14) + return 14; + else + return 0; +} + inline void BasicTimer::reset() { timer->CR1 = 0; diff --git a/src/shared/drivers/timer/GeneralPurposeTimer.h b/src/shared/drivers/timer/GeneralPurposeTimer.h index ca968af17cdf858d7224842373987a2be33d9690..de22562375a598275a56c8231ed9e8e5752cb5c7 100644 --- a/src/shared/drivers/timer/GeneralPurposeTimer.h +++ b/src/shared/drivers/timer/GeneralPurposeTimer.h @@ -110,7 +110,7 @@ public: explicit GeneralPurposeTimer(TIM_TypeDef *timer); /** - * @brief Resets the registers and disables the peripheral clock. + * @brief Disables the peripheral clock. */ ~GeneralPurposeTimer(); @@ -230,7 +230,6 @@ inline GeneralPurposeTimer<T>::GeneralPurposeTimer(TIM_TypeDef *timer) template <typename T> inline GeneralPurposeTimer<T>::~GeneralPurposeTimer() { - reset(); ClockUtils::disablePeripheralClock(timer); } diff --git a/src/shared/drivers/timer/PWM.cpp b/src/shared/drivers/timer/PWM.cpp index 471466913c65ab4e768d3b2d5cbea301d6e371fc..f8e2faa107ec05978202ac4d8725ef2ae8d2313c 100644 --- a/src/shared/drivers/timer/PWM.cpp +++ b/src/shared/drivers/timer/PWM.cpp @@ -30,8 +30,6 @@ PWM::PWM(TIM_TypeDef* const timer, unsigned int pwmFrequency, : timer(timer), pwmFrequency(pwmFrequency), dutyCycleResolution(dutyCycleResolution) { - // TODO: Enable the peripheral clock - // Erase the previous timer configuration this->timer.reset(); @@ -39,11 +37,7 @@ PWM::PWM(TIM_TypeDef* const timer, unsigned int pwmFrequency, setTimerConfiguration(); } -PWM::~PWM() -{ - // TODO: Disable the peripheral clock - timer.reset(); -} +PWM::~PWM() { timer.reset(); } void PWM::setFrequency(unsigned int pwmFrequency) { @@ -90,10 +84,12 @@ void PWM::setDutyCycle(TimerUtils::Channel channel, float dutyCycle) float PWM::getDutyCycle(TimerUtils::Channel channel) { - return static_cast<float>(timer.readCaptureCompareRegister(channel) / - timer.readAutoReloadRegister()); + return static_cast<float>(timer.readCaptureCompareRegister(channel)) / + static_cast<float>(timer.readAutoReloadRegister()); } +GP16bitTimer PWM::getTimer() { return timer; } + void PWM::setTimerConfiguration() { timer.disable(); diff --git a/src/shared/drivers/timer/PWM.h b/src/shared/drivers/timer/PWM.h index 988613fa78738e2bbbd93d15c244c937cc3b2645..d30e5d45ca031a536acf963a4b50739d99d79b49 100644 --- a/src/shared/drivers/timer/PWM.h +++ b/src/shared/drivers/timer/PWM.h @@ -35,13 +35,13 @@ namespace Boardcore * when deleted) but no channels are enabled. This means that you will only need * to use the channels without worrying about the underlying timer. * - * The PWM driver accepts a the pointer to the peripheral registers of a timer - * and uses it as a 16bit general purpose timer. No checks are in place to this + * The PWM driver accepts a pointer to the peripheral registers of a timer and + * uses it as a 16bit general purpose timer. No checks are in place to this * pointer, thus make sure to pass a proper value! * * Moreover, even 32bit general purpose timers are used as if they where 16bit. * At the moment there is no need for further accuracy but if it ever will be, - * exceeding this class is simble, just add a template parameter and pass it to + * exceeding this class is simple, just add a template parameter and pass it to * the GeneralPurposeTimer parameter. * * Check out the following spread sheet to visually see how the timers registers @@ -95,6 +95,11 @@ public: */ float getDutyCycle(TimerUtils::Channel channel); + /** + * @brief Returns the timer used to generate the pwm signal. + */ + GP16bitTimer getTimer(); + private: // This class is not copyable! PWM& operator=(const PWM&) = delete; diff --git a/src/shared/drivers/timer/TimerUtils.h b/src/shared/drivers/timer/TimerUtils.h index b575dcb16a35b3a11e0f42e2f60aa2cf039ba1a6..7d619419f2c13d1b73f4ab649038b6e294c953ea 100644 --- a/src/shared/drivers/timer/TimerUtils.h +++ b/src/shared/drivers/timer/TimerUtils.h @@ -241,7 +241,7 @@ enum class OutputComparePolarity : uint16_t ACTIVE_LOW = 0x1 }; -enum class Channel : int +enum class Channel : uint8_t { CHANNEL_1 = 0, CHANNEL_2 = 1, @@ -254,7 +254,7 @@ enum class Channel : int * * @return Timer input clock, APB1 or ABP2. */ -ClockUtils::APB getTimerInputClock(TIM_TypeDef *timer); +ClockUtils::APB getTimerInputClock(const TIM_TypeDef *timer); /** * @brief Returns the timer clock frequency before the prescaler. @@ -264,7 +264,7 @@ ClockUtils::APB getTimerInputClock(TIM_TypeDef *timer); * @param timer Timer to use. * @return Prescaler input frequency. */ -uint32_t getPrescalerInputFrequency(TIM_TypeDef *timer); +uint32_t getPrescalerInputFrequency(const TIM_TypeDef *timer); /** * @brief Return the timer clock frequency. @@ -360,7 +360,7 @@ uint16_t computePrescalerValue(TIM_TypeDef *timer, int targetFrequency); } // namespace TimerUtils -inline ClockUtils::APB TimerUtils::getTimerInputClock(TIM_TypeDef *timer) +inline ClockUtils::APB TimerUtils::getTimerInputClock(const TIM_TypeDef *timer) { // Timers can be connected to APB1 or APB2 clocks. // APB1: TIM2-7,12-15 @@ -377,7 +377,7 @@ inline ClockUtils::APB TimerUtils::getTimerInputClock(TIM_TypeDef *timer) } } -inline uint32_t TimerUtils::getPrescalerInputFrequency(TIM_TypeDef *timer) +inline uint32_t TimerUtils::getPrescalerInputFrequency(const TIM_TypeDef *timer) { return ClockUtils::getAPBFrequency(getTimerInputClock(timer)); } diff --git a/src/shared/drivers/timer/TimestampTimer.h b/src/shared/drivers/timer/TimestampTimer.h index 3f54fb1ad6959892353230fce24cc75a5e813f5c..8b2bc6f2f445bc0a21f3fd32bd754be102759e25 100644 --- a/src/shared/drivers/timer/TimestampTimer.h +++ b/src/shared/drivers/timer/TimestampTimer.h @@ -101,7 +101,7 @@ private: inline uint64_t TimestampTimer::getTimestamp() { #ifdef COMPILE_FOR_HOST - return 0; + return time(NULL); #else // With a timer frequency of 250KHz, the conversion from timer ticks to // microseconds only take a 2 byte shift (x4) diff --git a/src/shared/drivers/usart/USART.cpp b/src/shared/drivers/usart/USART.cpp new file mode 100644 index 0000000000000000000000000000000000000000..bc5612a0855997b158c2a1cc5f35c2b01248ce94 --- /dev/null +++ b/src/shared/drivers/usart/USART.cpp @@ -0,0 +1,780 @@ +/* Copyright (c) 2022 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include "drivers/usart/USART.h" + +#include <fcntl.h> +#include <stdio.h> +#include <utils/Debug.h> + +#include <string> + +#include "arch/common/drivers/serial.h" +#include "filesystem/file_access.h" +#include "miosix.h" + +Boardcore::USART *Boardcore::USART::ports[N_USART_PORTS]; + +/** + * \internal Interrupt routine for usart1 actual implementation. + */ +void __attribute__((used)) usart1irqImplBoardcore() +{ + Boardcore::USART *port_boardcore = Boardcore::USART::ports[0]; + if (port_boardcore) + port_boardcore->IRQhandleInterrupt(); + else + { + miosix::STM32Serial *port = miosix::STM32Serial::ports[0]; + if (port) + port->IRQhandleInterrupt(); + } +} + +/** + * \internal Interrupt routine for usart1. + */ +void __attribute__((naked, used)) USART1_IRQHandler() +{ + saveContext(); + asm volatile("bl _Z22usart1irqImplBoardcorev"); + restoreContext(); +} + +/** + * \internal Interrupt routine for usart2 actual implementation. + */ +void __attribute__((used)) usart2irqImplBoardcore() +{ + Boardcore::USART *port_boardcore = Boardcore::USART::ports[1]; + if (port_boardcore) + port_boardcore->IRQhandleInterrupt(); + else + { + miosix::STM32Serial *port = miosix::STM32Serial::ports[1]; + if (port) + port->IRQhandleInterrupt(); + } +} + +/** + * \internal Interrupt routine for usart2. + */ +void __attribute__((naked, used)) USART2_IRQHandler() +{ + saveContext(); + asm volatile("bl _Z22usart2irqImplBoardcorev"); + restoreContext(); +} + +/** + * \internal Interrupt routine for usart3 actual implementation. + */ +void __attribute__((used)) usart3irqImplBoardcore() +{ + Boardcore::USART *port_boardcore = Boardcore::USART::ports[2]; + if (port_boardcore) + port_boardcore->IRQhandleInterrupt(); + else + { + miosix::STM32Serial *port = miosix::STM32Serial::ports[2]; + if (port) + port->IRQhandleInterrupt(); + } +} + +/** + * \internal Interrupt routine for usart3. + */ +void __attribute__((naked, used)) USART3_IRQHandler() +{ + saveContext(); + asm volatile("bl _Z22usart3irqImplBoardcorev"); + restoreContext(); +} + +/** + * \internal Interrupt routine for uart4 actual implementation. + */ +void __attribute__((used)) uart4irqImplBoardcore() +{ + Boardcore::USART *port_boardcore = Boardcore::USART::ports[3]; + if (port_boardcore) + port_boardcore->IRQhandleInterrupt(); + else + { + miosix::STM32Serial *port = miosix::STM32Serial::ports[3]; + if (port) + port->IRQhandleInterrupt(); + } +} + +/** + * \internal Interrupt routine for uart4. + */ +void __attribute__((naked, used)) UART4_IRQHandler() +{ + saveContext(); + asm volatile("bl _Z21uart4irqImplBoardcorev"); + restoreContext(); +} + +/** + * \internal Interrupt routine for uart5 actual implementation. + */ +void __attribute__((used)) uart5irqImplBoardcore() +{ + Boardcore::USART *port_boardcore = Boardcore::USART::ports[4]; + if (port_boardcore) + port_boardcore->IRQhandleInterrupt(); + else + { + miosix::STM32Serial *port = miosix::STM32Serial::ports[4]; + if (port) + port->IRQhandleInterrupt(); + } +} + +/** + * \internal Interrupt routine for uart5. + */ +void __attribute__((naked, used)) UART5_IRQHandler() +{ + saveContext(); + asm volatile("bl _Z21uart5irqImplBoardcorev"); + restoreContext(); +} + +/** + * \internal Interrupt routine for usart6 actual implementation. + */ +void __attribute__((used)) usart6irqImplBoardcore() +{ + Boardcore::USART *port_boardcore = Boardcore::USART::ports[5]; + if (port_boardcore) + port_boardcore->IRQhandleInterrupt(); + else + { + miosix::STM32Serial *port = miosix::STM32Serial::ports[5]; + if (port) + port->IRQhandleInterrupt(); + } +} + +/** + * \internal Interrupt routine for usart6. + */ +void __attribute__((naked, used)) USART6_IRQHandler() +{ + saveContext(); + asm volatile("bl _Z22usart6irqImplBoardcorev"); + restoreContext(); +} + +#ifdef STM32F429xx + +/** + * \internal Interrupt routine for uart7 actual implementation. + */ +void __attribute__((used)) uart7irqImplBoardcore() +{ + Boardcore::USART *port_boardcore = Boardcore::USART::ports[6]; + if (port_boardcore) + port_boardcore->IRQhandleInterrupt(); + else + { + miosix::STM32Serial *port = miosix::STM32Serial::ports[6]; + if (port) + port->IRQhandleInterrupt(); + } +} + +/** + * \internal Interrupt routine for uart7. + */ +void __attribute__((naked, used)) UART7_IRQHandler() +{ + saveContext(); + asm volatile("bl _Z21uart7irqImplBoardcorev"); + restoreContext(); +} + +/** + * \internal Interrupt routine for uart8 actual implementation. + */ +void __attribute__((used)) uart8irqImplBoardcore() +{ + Boardcore::USART *port_boardcore = Boardcore::USART::ports[7]; + if (port_boardcore) + port_boardcore->IRQhandleInterrupt(); + else + { + miosix::STM32Serial *port = miosix::STM32Serial::ports[7]; + if (port) + port->IRQhandleInterrupt(); + } +} + +/** + * \internal Interrupt routine for uart8. + */ +void __attribute__((naked, used)) UART8_IRQHandler() +{ + saveContext(); + asm volatile("bl _Z21uart8irqImplBoardcorev"); + restoreContext(); +} + +#endif // STM32F429xx + +namespace Boardcore +{ + +USARTInterface::~USARTInterface() {} + +bool USARTInterface::initPins(miosix::GpioPin tx, int nAFtx, miosix::GpioPin rx, + int nAFrx) +{ + if (pinInitialized) + { + return false; + } + + miosix::FastInterruptDisableLock dLock; + + this->tx = tx; + this->rx = rx; + + tx.mode(miosix::Mode::ALTERNATE); + tx.alternateFunction(nAFtx); + + rx.mode(miosix::Mode::ALTERNATE); + rx.alternateFunction(nAFrx); + + pinInitialized = true; + return true; +} + +void USART::IRQhandleInterrupt() +{ + char c; + + // If read data register is empty then read data + if (usart->SR & USART_SR_RXNE) + { + // Always read data, since this clears interrupt flags + c = usart->DR; + // If no error put data in buffer + if (!(usart->SR & USART_SR_FE)) + if (rxQueue.tryPut(c) == false) // FIFO overflow + ; + + idle = false; + } + + if (usart->SR & USART_SR_IDLE) + idle = true; + + if (usart->SR & USART_SR_IDLE || rxQueue.size() >= rxQueue.capacity() / 2) + { + c = usart->DR; // Clears interrupt flags + + // Enough data in buffer or idle line, awake thread + if (rxWaiting) + { + rxWaiting->IRQwakeup(); + rxWaiting = 0; + } + } +} + +USART::USART(USARTType *usart, Baudrate baudrate, unsigned int queueLen) + : rxQueue(queueLen) +{ + // Setting the id of the serial port + switch (reinterpret_cast<uint32_t>(usart)) + { + case USART1_BASE: + this->id = 1; + initPins(u1tx1::getPin(), 7, u1rx1::getPin(), 7); + irqn = USART1_IRQn; + break; + case USART2_BASE: + this->id = 2; + initPins(u2tx1::getPin(), 7, u2rx1::getPin(), 7); + irqn = USART2_IRQn; + break; + case USART3_BASE: + this->id = 3; + initPins(u3tx1::getPin(), 7, u3rx1::getPin(), 7); + irqn = USART3_IRQn; + break; + case UART4_BASE: + this->id = 4; + initPins(u4tx1::getPin(), 8, u4rx1::getPin(), 8); + irqn = UART4_IRQn; + break; + case UART5_BASE: + this->id = 5; + initPins(u5tx::getPin(), 8, u5rx::getPin(), 8); + irqn = UART5_IRQn; + break; + case USART6_BASE: + this->id = 6; + initPins(u6tx1::getPin(), 8, u6rx1::getPin(), 8); + irqn = USART6_IRQn; + break; +#ifdef STM32F429xx + case UART7_BASE: + this->id = 7; + initPins(u7tx1::getPin(), 8, u7rx1::getPin(), 8); + irqn = UART7_IRQn; + break; + case UART8_BASE: + this->id = 8; + initPins(u8tx::getPin(), 8, u8rx::getPin(), 8); + irqn = UART8_IRQn; + break; +#endif // STM32F429xx + } + + commonConstructor(usart, baudrate); +} + +USART::USART(USARTType *usart, Baudrate baudrate, miosix::GpioPin tx, + miosix::GpioPin rx, unsigned int queueLen) + : rxQueue(queueLen) +{ + // Setting the id of the serial port + switch (reinterpret_cast<uint32_t>(usart)) + { + case USART1_BASE: + this->id = 1; + initPins(tx, 7, rx, 7); + irqn = USART1_IRQn; + break; + case USART2_BASE: + this->id = 2; + initPins(tx, 7, rx, 7); + irqn = USART2_IRQn; + break; + case USART3_BASE: + this->id = 3; + initPins(tx, 7, rx, 7); + irqn = USART3_IRQn; + break; + case UART4_BASE: + this->id = 4; + initPins(tx, 8, rx, 8); + irqn = UART4_IRQn; + break; + case UART5_BASE: + this->id = 5; + initPins(tx, 8, rx, 8); + irqn = UART5_IRQn; + break; + case USART6_BASE: + this->id = 6; + initPins(tx, 8, rx, 8); + irqn = USART6_IRQn; + break; +#ifdef STM32F429xx + case UART7_BASE: + this->id = 7; + initPins(tx, 8, rx, 8); + irqn = UART7_IRQn; + break; + case UART8_BASE: + this->id = 8; + initPins(tx, 8, rx, 8); + irqn = UART8_IRQn; + break; +#endif // STM32F429xx + } + + commonConstructor(usart, baudrate); +} + +void USART::commonConstructor(USARTType *usart, Baudrate baudrate) +{ + this->usart = usart; + + // Enabling the peripehral on the right APB + ClockUtils::enablePeripheralClock(usart); + RCC_SYNC(); + + // Enabling the usart peripheral + { + miosix::FastInterruptDisableLock dLock; + usart->CR1 |= USART_CR1_UE; + } + + // Setting the baudrate chosen + setBaudrate(baudrate); + + // Default settings + setStopBits(1); + setWordLength(USART::WordLength::BIT8); + setParity(USART::ParityBit::NO_PARITY); + setOversampling(false); +} + +USART::~USART() +{ + { + miosix::FastInterruptDisableLock dLock; + + // Take out the usart object we are going to destruct + USART::ports[this->id - 1] = nullptr; + + // Disabling the usart + usart->CR1 &= ~(USART_CR1_UE | USART_CR1_TE | USART_CR1_RE); + + // Disabling the interrupt of the serial port + NVIC_DisableIRQ(irqn); + } +} + +bool USART::init() +{ + if (id < 1 || id > MAX_SERIAL_PORTS || !pinInitialized) + { + TRACE("Not supported USART id or pins not initialized\n"); + return false; + } + + { + miosix::FastInterruptDisableLock dLock; + + // Enable usart, receiver, receiver interrupt and idle interrupt + usart->CR1 |= USART_CR1_RXNEIE // Interrupt on data received + | USART_CR1_IDLEIE // interrupt on idle line + | USART_CR1_TE // Transmission enabled + | USART_CR1_RE; // Reception enabled + + // Sample only one bit + usart->CR3 |= USART_CR3_ONEBIT; + + // Enabling the interrupt for the relative serial port + NVIC_SetPriority(irqn, 15); + NVIC_EnableIRQ(irqn); + + // Add to the array of usarts so that the interrupts can see it + USART::ports[id - 1] = this; + } + + // Clearing the queue for random data read at the beginning + miosix::Thread::sleep(1); + this->clearQueue(); + + return true; +} + +void USART::setWordLength(WordLength wordLength) +{ + + miosix::FastInterruptDisableLock dLock; + (wordLength == WordLength::BIT8 ? usart->CR1 &= ~USART_CR1_M + : usart->CR1 |= USART_CR1_M); + this->wordLength = wordLength; +} + +void USART::setParity(ParityBit parity) +{ + miosix::FastInterruptDisableLock dLock; + (parity == ParityBit::NO_PARITY ? usart->CR1 &= ~USART_CR1_PCE + : usart->CR1 |= USART_CR1_PCE); + this->parity = parity; +} + +void USART::setStopBits(int stopBits) +{ + miosix::FastInterruptDisableLock dLock; + this->stopBits = stopBits; + usart->CR2 &= ~USART_CR2_STOP; + if (stopBits == 2) + { + usart->CR2 |= USART_CR2_STOP_1; + } +} + +void USART::setOversampling(bool oversampling) +{ + miosix::FastInterruptDisableLock dLock; + this->over8 = oversampling; + (oversampling ? usart->CR1 |= USART_CR1_OVER8 + : usart->CR1 &= ~USART_CR1_OVER8); +} + +void USART::setBaudrate(Baudrate baudrate) +{ + /* + * Baudrate setting: + * fixed point: mantissa first 12 bits, other 4 bits are the fixed point + * value of the fraction. + * - if over8==0 => DIV_Fraction[3:0] (all fraction used) + * USART_DIV = f/(16*baud) + * - if over8==1 => 0+DIV_Fraction[2:0] (first bit of fraction is 0) + * USART_DIV = f/(8*baud) + */ + miosix::InterruptDisableLock dLock; + + // USART1 and USART6 is always connected to the APB2, while all the others + // UART/USART peripherals are always connected to APB1 + uint32_t f = ClockUtils::getAPBFrequency( + (id == 1 || id == 6 ? ClockUtils::APB::APB2 // High speed APB2 + : ClockUtils::APB::APB1)); // Low speed APB1, + + // <<4 in order to shift to left of 4 positions, to create a fixed point + // number of 4 decimal digits /8 == >>3 in order to divide per 8 (from the + // formula in the datasheet) + uint32_t USART_DIV = ((f << 1) / (((int)baudrate * (over8 ? 1 : 2)))); + + // rounding to the nearest + uint32_t brr = (USART_DIV / 2) + (USART_DIV & 1); + + if (over8) + { + brr += brr & 1; + } + + usart->BRR = brr; + + this->baudrate = baudrate; +} + +int USART::read(void *buffer, size_t nBytes) +{ + miosix::Lock<miosix::FastMutex> l(rxMutex); + + char *buf = reinterpret_cast<char *>(buffer); + size_t result = 0; + miosix::FastInterruptDisableLock dLock; + for (;;) + { + // Try to get data from the queue + for (; result < nBytes; result++) + { + if (rxQueue.tryGet(buf[result]) == false) + break; + // This is here just not to keep IRQ disabled for the whole loop + miosix::FastInterruptEnableLock eLock(dLock); + } + + // Not checking if the nBytes read are more than 0 + if (result == nBytes || (idle && result > 0)) + break; + + // Wait for data in the queue + do + { + rxWaiting = miosix::Thread::IRQgetCurrentThread(); + miosix::Thread::IRQwait(); + { + miosix::FastInterruptEnableLock eLock(dLock); + miosix::Thread::yield(); + } + } while (rxWaiting); + } + + return result; +} + +int USART::write(void *buffer, size_t nBytes) +{ + miosix::Lock<miosix::FastMutex> l(txMutex); + + // TODO: Use the send complete interrupt in order not to have a busy while + // loop waiting + const char *buf = reinterpret_cast<const char *>(buffer); + size_t i = 0; + for (i = 0; i < nBytes; i++) + { + while ((usart->SR & USART_SR_TXE) == 0) + ; + + usart->DR = *buf; + buf++; + } + + return i; +} + +int USART::writeString(const char *buffer) +{ + int i = 0; + miosix::Lock<miosix::FastMutex> l(txMutex); + + // Send everything, also the ending '\0' character + usart->DR = *buffer; + i++; + while (*buffer != '\0') + { + buffer++; + while (!(usart->SR & USART_SR_TXE)) + ; + + usart->DR = *buffer; + i++; + }; + + return i; +} + +void USART::clearQueue() { rxQueue.reset(); } + +STM32SerialWrapper::STM32SerialWrapper(USARTType *usart, Baudrate baudrate) +{ + this->usart = usart; + this->baudrate = baudrate; + switch (reinterpret_cast<uint32_t>(usart)) + { + case USART1_BASE: + this->id = 1; + initPins(u1tx1::getPin(), 7, u1rx1::getPin(), 7); + this->serialPortName = std::string("usart1"); + break; + case USART2_BASE: + this->id = 2; + initPins(u2tx1::getPin(), 7, u2rx1::getPin(), 7); + this->serialPortName = std::string("usart2"); + break; + case USART3_BASE: + this->id = 3; + initPins(u3tx1::getPin(), 7, u3rx1::getPin(), 7); + this->serialPortName = std::string("usart3"); + break; + } + initialized = false; + fd = -1; +} + +STM32SerialWrapper::STM32SerialWrapper(USARTType *usart, Baudrate baudrate, + miosix::GpioPin tx, miosix::GpioPin rx) +{ + this->usart = usart; + this->baudrate = baudrate; + switch (reinterpret_cast<uint32_t>(usart)) + { + case USART1_BASE: + this->id = 1; + this->serialPortName = std::string("usart1"); + break; + case USART2_BASE: + this->id = 2; + this->serialPortName = std::string("usart2"); + break; + case USART3_BASE: + this->id = 3; + this->serialPortName = std::string("usart3"); + break; + } + initPins(tx, 7, rx, 7); + initialized = false; + fd = -1; +} + +STM32SerialWrapper::~STM32SerialWrapper() +{ + miosix::intrusive_ref_ptr<miosix::DevFs> devFs = + miosix::FilesystemManager::instance().getDevFs(); + close(fd); + devFs->remove(serialPortName.c_str()); +} + +bool STM32SerialWrapper::init() +{ + if (id > 3) + { + TRACE( + "[STM32SerialWrapper] USART id greater than 3 is not supported\n"); + return false; + } + + if (initialized) + { + TRACE( + "[STM32SerialWrapper] Error : serial communication already " + "initialized!\n"); + return false; + } + else if (!serialCommSetup()) + { + TRACE( + "[STM32SerialWrapper] Error : can't initialize serial " + "communication!\n"); + return false; + } + + initialized = true; + return true; +} + +bool STM32SerialWrapper::serialCommSetup() +{ + // Creates and adds the serial port to the devices + if (!pinInitialized) + serial = new miosix::STM32Serial(id, static_cast<int>(baudrate)); + else + { + serial = + new miosix::STM32Serial(id, static_cast<int>(baudrate), tx, rx); + } + + // Adds a device to the file system + if (!miosix::FilesystemManager::instance().getDevFs()->addDevice( + serialPortName.c_str(), + miosix::intrusive_ref_ptr<miosix::Device>(serial))) + return false; + + // Path string "/dev/<name_of_port>" for the port we want to open + std::string serialPortPath = "/dev/" + serialPortName; + + // Open serial port + fd = open(serialPortPath.c_str(), O_RDWR); + + if (fd <= -1) + { + TRACE("Cannot open %s\n", serialPortPath.c_str()); + return false; + } + + return true; +} + +int STM32SerialWrapper::writeString(const char *data) +{ + // strlen + 1 in order to send the '/0' terminated string + return ::write(fd, data, strlen(data) + 1); +} + +int STM32SerialWrapper::write(void *buf, size_t nChars) +{ + return ::write(fd, buf, nChars); +} + +int STM32SerialWrapper::read(void *buf, size_t nBytes) +{ + return ::read(fd, buf, nBytes); +} + +} // namespace Boardcore diff --git a/src/shared/drivers/usart/USART.h b/src/shared/drivers/usart/USART.h new file mode 100644 index 0000000000000000000000000000000000000000..05cee3a1662984393fc8790c31dab329eb1f201c --- /dev/null +++ b/src/shared/drivers/usart/USART.h @@ -0,0 +1,426 @@ +/* Copyright (c) 2022 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <interfaces/arch_registers.h> +#include <miosix.h> +#include <utils/ClockUtils.h> + +#include <string> + +#include "arch/common/drivers/serial.h" + +#ifndef USE_MOCK_PERIPHERALS +using USARTType = USART_TypeDef; +#else +// TODO: Create test utils +#endif + +#ifdef STM32F429xx +#define N_USART_PORTS 8 +#else +#define N_USART_PORTS 6 +#endif + +// A nice feature of the stm32 is that the USART are connected to the same +// GPIOS in all families, stm32f1, f2, f4 and l1. Additionally, USART1 and +// USART6 are always connected to the APB2, while the other USART/UARTs are +// connected to the APB1. + +// USART1: AF7 +typedef miosix::Gpio<GPIOB_BASE, 6> u1tx1; +typedef miosix::Gpio<GPIOB_BASE, 7> u1rx1; +typedef miosix::Gpio<GPIOA_BASE, 9> u1tx2; +typedef miosix::Gpio<GPIOA_BASE, 10> u1rx2; +// typedef miosix::Gpio<GPIOA_BASE, 11> u1cts; +// typedef miosix::Gpio<GPIOA_BASE, 12> u1rts; + +// USART2: AF7 +typedef miosix::Gpio<GPIOA_BASE, 2> u2tx1; +typedef miosix::Gpio<GPIOA_BASE, 3> u2rx1; +typedef miosix::Gpio<GPIOD_BASE, 5> u2tx2; +typedef miosix::Gpio<GPIOD_BASE, 6> u2rx2; +// typedef miosix::Gpio<GPIOA_BASE, 0> u2cts; +// typedef miosix::Gpio<GPIOA_BASE, 1> u2rts; + +// USART3: AF7 +typedef miosix::Gpio<GPIOB_BASE, 10> u3tx1; +typedef miosix::Gpio<GPIOB_BASE, 11> u3rx1; +typedef miosix::Gpio<GPIOD_BASE, 8> u3tx2; +typedef miosix::Gpio<GPIOD_BASE, 9> u3rx2; +// typedef miosix::Gpio<GPIOB_BASE, 13> u3cts; +// typedef miosix::Gpio<GPIOB_BASE, 14> u3rts; + +// UART4: AF8 +typedef miosix::Gpio<GPIOA_BASE, 0> u4tx1; +typedef miosix::Gpio<GPIOA_BASE, 1> u4rx1; +typedef miosix::Gpio<GPIOC_BASE, 10> u4tx2; +typedef miosix::Gpio<GPIOC_BASE, 11> u4rx2; + +// UART5: AF8 +typedef miosix::Gpio<GPIOC_BASE, 12> u5tx; +typedef miosix::Gpio<GPIOD_BASE, 2> u5rx; + +// USART6: AF8 +typedef miosix::Gpio<GPIOC_BASE, 6> u6tx1; +typedef miosix::Gpio<GPIOC_BASE, 7> u6rx1; +#ifdef STM32F429xx +typedef miosix::Gpio<GPIOG_BASE, 14> u6tx2; +typedef miosix::Gpio<GPIOG_BASE, 9> u6rx2; + +// USART7: AF8 +typedef miosix::Gpio<GPIOE_BASE, 8> u7tx1; +typedef miosix::Gpio<GPIOE_BASE, 7> u7rx1; +typedef miosix::Gpio<GPIOF_BASE, 7> u7tx2; +typedef miosix::Gpio<GPIOF_BASE, 6> u7rx2; + +// USART8: AF8 +typedef miosix::Gpio<GPIOE_BASE, 1> u8tx; +typedef miosix::Gpio<GPIOE_BASE, 0> u8rx; +#endif // STM32F429xx + +namespace Boardcore +{ +/** + * @brief Abstract class that implements the interface for the USART/UART serial + * communication. + */ +class USARTInterface +{ +public: + enum class Baudrate : int + { + // B1200 = 1200, // NOT WORKING WITH 1200 baud + B2400 = 2400, + B9600 = 9600, + B19200 = 19200, + B38400 = 38400, + B57600 = 57600, + B115200 = 115200, + B230400 = 230400, + B460800 = 460800, + B921600 = 921600 + }; + + virtual ~USARTInterface() = 0; + + /** + * @brief Initializes the peripheral enabling his interrupts, the interrupts + * in the NVIC. + * + * All the setup phase (with the setting of the pins and their alternate + * functions) must be done before the initialization of the peripheral. + */ + virtual bool init() = 0; + + /** + * @brief Blocking read operation to read nBytes or till the data transfer + * is complete. + */ + virtual int read(void *buffer, size_t nBytes) = 0; + + /** + * @brief Blocking write operation. + */ + virtual int write(void *buf, size_t nChars) = 0; + + /** + * @brief Write a string to the serial, comprising the '\0' character. + */ + virtual int writeString(const char *buffer) = 0; + + /** + * @brief Returns the id of the serial. + */ + int getId() { return id; }; + +protected: + /** + * @brief Initializes the pins with the appropriate alternate functions. + * + * @param tx Tranmission pin. + * @param nAFtx Tranmission pin alternate function. + * @param rx Reception pin. + * @param nAFrx Reception pin alternate function. + */ + bool initPins(miosix::GpioPin tx, int nAFtx, miosix::GpioPin rx, int nAFrx); + + ///< True if initPins() already called successfully, false otherwise + bool pinInitialized = false; + + miosix::GpioPin tx{GPIOA_BASE, 0}; + miosix::GpioPin rx{GPIOA_BASE, 0}; + + USARTType *usart; + int id = 1; ///< Can be 1, 2, 3, 4, 5, 6, 7, 8 + bool initialized = false; + Baudrate baudrate; ///< Baudrate of the serial communication +}; + +/** + * @brief Driver for STM32F4 low level USART/UART peripheral. + * + * It allows to configure some low level parameters such as word length, parity, + * stop bits and oversampling. + */ +class USART : public USARTInterface +{ +public: + enum class WordLength : bool + { + BIT8 = 0, + BIT9 = 1 + }; + + enum class ParityBit : bool + { + NO_PARITY = 0, + PARITY = 1 + }; + + ///< Pointer to serial port classes to let interrupts access the classes + static USART *ports[]; + + /** + * @brief Interrupt handler that deals with receive and idle interrupts. + * + * Used to implement the reads, fills up a buffer when the interrupt is + * fired. + */ + void IRQhandleInterrupt(); + + /** + * @brief Automatically enables the peripheral and timer peripheral clock. + * + * Sets the default values for all the parameters (1 stop bit, 8 bit data, + * no control flow and no oversampling). + * Initializes the serial port using the default pins, which are: + * - USART1: tx=PA9 rx=PA10 + * - USART2: tx=PA2 rx=PA3 + * - USART3: tx=PB10 rx=PB11 + * - UART4: tx=PA0 rx=PA1 + * - UART5: tx=PC12 rx=PD2 + * - USART6: tx=PC6 rx=PC7 + * - UART7: tx=PE8 rx=PE7 + * - UART8: tx=PE1 rx=PE0 + * + * @param usart structure that represents the usart peripheral [accepted + * are: USART1, USART2, USART3, UART4, UART5, USART6, UART7, UART8]. + * @param baudrate member of the enum Baudrate that represents the baudrate + * with which the communication will take place. + */ + USART(USARTType *usart, Baudrate baudrate, + unsigned int queueLen = usart_queue_default_capacity); + + /** + * @brief Automatically enables the peripheral and timer peripheral clock. + * + * Sets the default values for all the parameters (1 stop bit, 8 bit data, + * no control flow and no oversampling). + * Initializes the serial port using custom pins. + * + * @param usart Structure that represents the usart peripheral [accepted + * are: USART1, USART2, USART3, UART4, UART5, USART6, UART7, UART8]. + * @param baudrate Member of the enum Baudrate that represents the baudrate + * with which the communication will take place. + * @param tx Tranmission pin. + * @param rx Reception pin. + */ + USART(USARTType *usart, Baudrate baudrate, miosix::GpioPin tx, + miosix::GpioPin rx, + unsigned int queueLen = usart_queue_default_capacity); + + /** + * @brief Disables the flags for the generation of the interrupts, the IRQ + * from the NVIC, the peripheral and removes his pointer from the ports + * list. + */ + ~USART(); + + /** + * @brief Initializes the peripheral enabling his interrupts, the interrupts + * in the NVIC and setting the pins with the appropriate alternate + * functions. + * + * All the setup phase must be done before the initialization of the + * peripheral. The pins must be initialized before calling this function. + */ + bool init(); + + /** + * @brief Blocking read operation to read nBytes or till the data transfer + * is complete. + */ + int read(void *buffer, size_t nBytes); + + /** + * @brief Blocking write operation. + */ + int write(void *buf, size_t nChars); + + /** + * @brief Write a string to the serial, comprising the '\0' character. + */ + int writeString(const char *buffer); + + /** + * @brief Set the length of the word to 8 or to 9. + * + * @param wl WordLength element that represents the length of the word. + */ + void setWordLength(WordLength wl); + + /** + * @brief Set the presence of the parity in the data sent. + * + * @param pb ParityBit element that represents the presence of the parity + * bit. + */ + void setParity(ParityBit pb); + + /** + * @brief Set the number of stop bits. + * + * @param stopBits number of stop bits [1,2]. + */ + void setStopBits(int stopBits); + + /** + * @brief Set the baudrate in the BRR register. + * + * @param pb Baudrate element that represents the baudrate. + */ + void setBaudrate(Baudrate br); + + /** + * @brief Sets the Over8 bit. + * + * If it is set, the speed is increased; If it is reset the tolerance is + * increased. + */ + void setOversampling(bool oversampling); + + /** + * @brief Clears the rxQueue. + */ + void clearQueue(); + +private: + void commonConstructor(USARTType *usart, Baudrate baudrate); + + IRQn_Type irqn; + miosix::FastMutex rxMutex; ///< mutex for receiving on serial + miosix::FastMutex txMutex; ///< mutex for transmitting on serial + + ///< Pointer to the waiting on receive thread + miosix::Thread *rxWaiting = 0; + + miosix::DynUnsyncQueue<char> rxQueue; ///< Receiving queue + bool idle = true; ///< Receiver idle + ParityBit parity = ParityBit::NO_PARITY; + WordLength wordLength = WordLength::BIT8; + int stopBits = 1; ///< Number of stop bits [1,2] + bool over8 = false; ///< Oversalmpling 8 bit + + ///< Default queue length + const static unsigned int usart_queue_default_capacity = 256; +}; + +/** + * @brief Wrapper for the STM32Serial driver in miosix. + */ +class STM32SerialWrapper : public USARTInterface +{ +public: + /** + * @brief Initializes the serialPortName and initializes the default pins, + * which are: + * - USART1: tx=PA9 rx=PA10 + * - USART2: tx=PA2 rx=PA3 + * - USART3: tx=PB10 rx=PB11 + * @param usart structure that represents the usart peripheral [accepted + * are: USART1, USART2, USART3]. + * @param baudrate member of the enum Baudrate that represents the baudrate + * with which the communication will take place. + */ + STM32SerialWrapper(USARTType *usart, Baudrate baudrate); + + /** + * @brief Initializes the serialPortName and initializes the serial port + * using custom pins. + * @param usart structure that represents the usart peripheral [accepted + * are: USART1, USART2, USART3]. + * @param baudrate member of the enum Baudrate that represents the baudrate + * with which the communication will take place. + * @param tx Tranmission pin + * @param rx Reception pin + */ + STM32SerialWrapper(USARTType *usart, Baudrate baudrate, miosix::GpioPin tx, + miosix::GpioPin rx); + + /** + * @brief Removes the device from the list of the devices and closes the + * file of the device. + */ + ~STM32SerialWrapper(); + + /** + * @brief Initializes the peripheral. + * + * @see{STM32SerialWrapper::serialCommSetup} + */ + bool init(); + + /** + * @brief Blocking read operation to read nBytes or till the data transfer + * is complete. + */ + int read(void *buffer, size_t nBytes); + + /** + * @brief Blocking write operation. + */ + int write(void *buf, size_t nChars); + + /** + * @brief Write a string to the serial, comprising the '\0' character. + */ + int writeString(const char *buffer); + +private: + /** + * @brief Creates a device that represents the serial port, adds it to the + * file system and opens the file that represents the device. + */ + bool serialCommSetup(); + + miosix::STM32Serial *serial; ///< Pointer to the serial object + + //< Port name of the port that has to be created for the communication + std::string serialPortName; + + ///< File descriptor of the serial port file opened for transmission + int fd; +}; + +} // namespace Boardcore diff --git a/src/shared/events/Event.h b/src/shared/events/Event.h index d35c6ab219a2d5c2c7e0060e75eece7ea2eeb075..c38ec3ef83a50bbbe86428bbb8f7d9e89a363427 100644 --- a/src/shared/events/Event.h +++ b/src/shared/events/Event.h @@ -27,38 +27,27 @@ namespace Boardcore { -enum EventSignal : uint8_t +typedef uint8_t Event; + +enum BasicEvent : Event { EV_ENTRY = 0, EV_EXIT = 1, EV_EMPTY = 2, EV_INIT = 3, - EV_FIRST_SIGNAL = 4 + EV_FIRST_CUSTOM = 4 }; /** - * Example definiton of custom signals: - - enum CustomSignal : uint8_t - { - SIG_ONE = SG_FIRST_SIGNAL, - SIG_TWO, - SIG_THREE, - SIG_FOUR - }; - + * Example definition of custom events: + * + * enum CustomEvent : Event + * { + * EV_ONE = EV_FIRST_CUSTOM, + * EV_TWO, + * EV_THREE. + * EV_FOUR + * } */ -struct Event -{ - uint8_t code; -}; - -/* Example of extended Event structure - - struct ExtendedEvent : public Event{ - uint32_t customMember; - }; -*/ - } // namespace Boardcore diff --git a/src/shared/events/EventBroker.cpp b/src/shared/events/EventBroker.cpp index 019aefc8e0464bd9ab3b88e4e3cb27cf329e1d0c..b748fc1e91393133da2709cab2a761fe341ed442 100644 --- a/src/shared/events/EventBroker.cpp +++ b/src/shared/events/EventBroker.cpp @@ -29,11 +29,10 @@ namespace Boardcore { EventBroker::EventBroker() {} - void EventBroker::post(const Event& ev, uint8_t topic) { #ifdef TRACE_EVENTS - LOG_DEBUG(logger, "Event: {}, Topic: {}", ev.code, topic); + LOG_DEBUG(logger, "Event: {}, Topic: {}", ev, topic); #endif Lock<FastMutex> lock(mtxSubscribers); @@ -66,6 +65,34 @@ void EventBroker::removeDelayed(uint16_t id) } } +void EventBroker::subscribe(EventHandlerBase* subscriber, uint8_t topic) +{ + Lock<FastMutex> lock(mtxSubscribers); + subscribers[topic].push_back(subscriber); +} + +void EventBroker::unsubscribe(EventHandlerBase* subscriber, uint8_t topic) +{ + Lock<FastMutex> lock(mtxSubscribers); + + deleteSubscriber(subscribers.at(topic), subscriber); +} + +void EventBroker::unsubscribe(EventHandlerBase* subscriber) +{ + Lock<FastMutex> lock(mtxSubscribers); + for (auto it = subscribers.begin(); it != subscribers.end(); it++) + { + deleteSubscriber(it->second, subscriber); + } +} + +void EventBroker::clearDelayedEvents() +{ + Lock<FastMutex> lock(mtxDelayedEvents); + delayedEvents.clear(); +} + // Posts delayed events with expired deadline void EventBroker::run() { @@ -110,28 +137,6 @@ void EventBroker::run() } } -void EventBroker::subscribe(EventHandlerBase* subscriber, uint8_t topic) -{ - Lock<FastMutex> lock(mtxSubscribers); - subscribers[topic].push_back(subscriber); -} - -void EventBroker::unsubscribe(EventHandlerBase* subscriber, uint8_t topic) -{ - Lock<FastMutex> lock(mtxSubscribers); - - deleteSubscriber(subscribers.at(topic), subscriber); -} - -void EventBroker::unsubscribe(EventHandlerBase* subscriber) -{ - Lock<FastMutex> lock(mtxSubscribers); - for (auto it = subscribers.begin(); it != subscribers.end(); it++) - { - deleteSubscriber(it->second, subscriber); - } -} - void EventBroker::deleteSubscriber(vector<EventHandlerBase*>& subs, EventHandlerBase* subscriber) { @@ -150,10 +155,4 @@ void EventBroker::deleteSubscriber(vector<EventHandlerBase*>& subs, } } -void EventBroker::clearDelayedEvents() -{ - Lock<FastMutex> lock(mtxDelayedEvents); - delayedEvents.clear(); -} - } // namespace Boardcore diff --git a/src/shared/events/EventBroker.h b/src/shared/events/EventBroker.h index 27222a5a486524adefff64905e649689a422cda4..581ff8011164a47766542460c3959b9fcad1ec4f 100644 --- a/src/shared/events/EventBroker.h +++ b/src/shared/events/EventBroker.h @@ -62,8 +62,6 @@ class EventBroker : public Singleton<EventBroker>, public ActiveObject public: /** * Posts an event to the specified topic. - * @param ev - * @param topic */ void post(const Event& ev, uint8_t topic); @@ -150,14 +148,12 @@ public: /** * @brief Construct a new Event Broker object. + * * Public access required for testing purposes. Use the singleton interface * to access this class in production code. - * */ EventBroker(); - virtual ~EventBroker(){}; - private: /** * Private structure for holding a delayed event data in the list. @@ -190,5 +186,3 @@ private: }; } // namespace Boardcore - -#define sEventBroker Boardcore::Singleton<Boardcore::EventBroker>::getInstance() diff --git a/src/shared/events/FSM.h b/src/shared/events/FSM.h index 0f32e4c973f5334264ee67cae30937dbff064fe4..f4ed570643450d16616eafcd84c575b02b6de859 100644 --- a/src/shared/events/FSM.h +++ b/src/shared/events/FSM.h @@ -63,8 +63,8 @@ FSM<T>::FSM(void (T::*initialState)(const Event&), unsigned int stacksize, miosix::Priority priority) : EventHandler(stacksize, priority) { - state = initialState; - specialEvent.code = EV_ENTRY; + state = initialState; + specialEvent = EV_ENTRY; postEvent(specialEvent); } @@ -74,10 +74,10 @@ FSM<T>::~FSM(){}; template <class T> void FSM<T>::transition(void (T::*nextState)(const Event&)) { - specialEvent.code = EV_EXIT; + specialEvent = EV_EXIT; (static_cast<T*>(this)->*state)(specialEvent); - state = nextState; - specialEvent.code = EV_ENTRY; + state = nextState; + specialEvent = EV_ENTRY; (static_cast<T*>(this)->*state)(specialEvent); } diff --git a/src/shared/events/utils/EventCounter.h b/src/shared/events/utils/EventCounter.h index 27708720c00ab3a8987dfd98c0c70e6c9c568460..9f0914d26e17c0fca31960c706320deb5c5eebed 100644 --- a/src/shared/events/utils/EventCounter.h +++ b/src/shared/events/utils/EventCounter.h @@ -60,27 +60,22 @@ public: { Lock<FastMutex> l(mutex); - ++mapCounter[ev.code]; + ++mapCounter[ev]; ++totalCount; - lastEvent = ev.code; + lastEvent = ev; } /** - * @brief Returns the number of times a specific event has been received + * @brief Returns the number of times a specific event has been received. */ - unsigned int getCount(const Event& ev) { return getCount(ev.code); } - - /** - * @brief Returns the number of times a specific event has been received - */ - unsigned int getCount(uint8_t evSig) + unsigned int getCount(const Event& ev) { Lock<FastMutex> l(mutex); - if (mapCounter.count(evSig) == 1) + if (mapCounter.count(ev) == 1) { - return mapCounter.at(evSig); + return mapCounter.at(ev); } return 0; @@ -92,7 +87,7 @@ public: unsigned int getTotalCount() { return totalCount; } /** - * @brief Returns the signature of the last event received (ev.code) + * @brief Returns the signature of the last event received (ev) */ uint8_t getLastEvent() { return lastEvent; } diff --git a/src/shared/events/utils/EventInjector.h b/src/shared/events/utils/EventInjector.h index d57c820f137c6385fb6ce75bbb7ede6a1a89692d..c9df410e213085153a1e6146e5b123d2fd89dcbd 100644 --- a/src/shared/events/utils/EventInjector.h +++ b/src/shared/events/utils/EventInjector.h @@ -57,7 +57,7 @@ protected: getline(cin, temp); stringstream(temp) >> ev >> topic; - sEventBroker.post({(uint8_t)ev}, topic); + EventBroker::getInstance().post({(uint8_t)ev}, topic); } } }; diff --git a/src/shared/events/utils/EventSniffer.h b/src/shared/events/utils/EventSniffer.h index 19dff058ca32d00d0d6edf2c0b1755f62e317b69..67785774247ec3f5e6c17f5b2194365766d4e685 100644 --- a/src/shared/events/utils/EventSniffer.h +++ b/src/shared/events/utils/EventSniffer.h @@ -94,10 +94,7 @@ private: parent.broker.subscribe(this, topic); } - void postEvent(const Event& ev) - { - parent.onEventReceived(ev.code, topic); - } + void postEvent(const Event& ev) { parent.onEventReceived(ev, topic); } ~Sniffer() { parent.broker.unsubscribe(this); } diff --git a/src/shared/logger/Deserializer.h b/src/shared/logger/Deserializer.h index 63bdfbe53bec423e99ff24544062da452b0adba7..189582f0a17fcd6731236ca004dd3909e9b1d362 100644 --- a/src/shared/logger/Deserializer.h +++ b/src/shared/logger/Deserializer.h @@ -1,5 +1,5 @@ -/* Copyright (c) 2015-2018 Skyward Experimental Rocketry - * Author: Luca Erbetta +/* Copyright (c) 2015-2022 Skyward Experimental Rocketry + * Authors: Luca Erbetta, Alberto Nidasio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -27,6 +27,7 @@ #include <cstdio> #include <fstream> +#include <iostream> #include <limits> #include <ostream> #include <string> @@ -46,156 +47,189 @@ typedef std::numeric_limits<float> flt; // linter off /** - * Class used to deserialize log files created using fedetft's logger. + * @brief Class used to deserialize the binary logs created using fedetft's + * logger into csv files. */ class Deserializer { public: - Deserializer(std::string logfile, std::string prefix = "") - : prefix(prefix), logFile(logfile), - logFileWithExt(prefix + logFile + ".dat") - { - } + /** + * @brief Initializes the deserializer with the filename provided. + */ + Deserializer(std::string fileName); - ~Deserializer() - { - if (!closed) - { - for (auto it = fileStreams.begin(); it != fileStreams.end(); it++) - { - (*it)->close(); - delete *it; - } - } - } + ~Deserializer(); /** - * Register a type to be deserialized, and the associated print function + * @brief Register a type to be deserialized. + * + * Node: The object type must provide a static header function and a print + * function with the following prototypes: + * static std::string header() + * void print(std::ostream& os) const * - * @param t the object to be deserialized - * @param fncPrint function that prints the deserialized data on the - * provided output stream. + * @tparam T The object type to be deserialized. */ template <typename T> - bool registerType(std::function<void(T& t, std::ostream& os)> fncPrint, - std::string header = "") - { - if (closed) - { - printf("Error: Deserializer is closed.\n"); - return false; - } + void registerType(); + + /** + * @brief Deserializes the provided file. + * + * @return Whether the deserialization was successful. + */ + bool deserialize(); + + /** + * @brief Closes all the openend files. + */ + void close(); + +private: + /** + * @brief Function to print a data element into its csv file. + * + * @tparam T Type of the object. + * @param t Object to be printed in the file. + * @param path Path where to save the file. + * @param prefix Prefix added to the file. + */ + template <typename T> + void printType(T& t, std::string path = "", std::string prefix = ""); - char cFilename[128]; - sprintf(cFilename, "%s%s_%s.csv", prefix.c_str(), logFile.c_str(), - typeid(T).name()); + bool closed = false; - std::string filename(cFilename); + std::map<std::string, std::ofstream*> fileStreams; + tscpp::TypePoolStream tps; + + std::string logFilename; + std::string logFilenameWithoutExtension; + std::string logFolderPath; +}; - std::ofstream* stream = new std::ofstream(); +Deserializer::Deserializer(std::string logFilename) : logFilename(logFilename) +{ + // Prepare the folder path + logFilenameWithoutExtension = + logFilename.substr(0, logFilename.find_last_of(".")); + logFolderPath = logFilenameWithoutExtension + "/"; +} + +Deserializer::~Deserializer() { close(); } + +template <typename T> +void Deserializer::registerType() +{ + std::function<void(T & t)> callback = + std::bind(&Deserializer::printType<T>, this, std::placeholders::_1, + logFolderPath, logFilenameWithoutExtension); + + tps.registerType<T>(callback); +} + +template <typename T> +void Deserializer::printType(T& t, std::string path, std::string prefix) +{ + std::string demangledTypeName = tscpp::demangle(typeid(T).name()); + static std::ofstream* stream; + + try + { + stream = fileStreams.at(demangledTypeName); + } + // If not already initialize, open the file and write the header + catch (std::out_of_range e) + { + stream = new std::ofstream(); + std::string filename = path + prefix + "_" + demangledTypeName + ".csv"; + std::cout << "Creating file " + filename << std::endl; stream->open(filename); if (!stream->is_open()) { printf("Error opening file %s.\n", filename.c_str()); perror("Error is:"); - delete stream; - return false; + return; } - fileStreams.push_back(stream); - stream->precision(flt::max_digits10); // Set stream precision to - // maximum float precision - // Print the header - if (header.length() > 0) - { - *stream << header; - } + // Print the header in the file + *stream << T::header(); - using namespace std::placeholders; // for _1 + // Add the file to the vector such that it will be closed + fileStreams.emplace(demangledTypeName, stream); + } - std::function<void(T & t)> callback = - std::bind(fncPrint, _1, std::ref(*stream)); + // Print data into the file if it is open + if (stream->is_open()) + t.print(std::ref(*stream)); +} - tps.registerType<T>(callback); +bool Deserializer::deserialize() +{ + if (closed) + return false; - return true; - } + // Create the folder + mkdir(logFolderPath.c_str(), 0777); - /** - * @brief Deserializes the provided file. - * - * @return Wheter the deserialization was successful. - */ - bool deserialize() + // Move the log file into the folder + if (rename(logFilename.c_str(), (logFolderPath + logFilename).c_str())) { - if (closed) - { - return false; - } + std::cout << logFilename + " does not exists." << std::endl; + return false; + } - bool success = true; - std::string unknownTypeName; + // Open the log file + std::ifstream file(logFolderPath + logFilename); - struct stat st; - if (stat(logFileWithExt.c_str(), &st) != 0) + // Check if the file exists + if (!file) + { + std::cout << logFolderPath + logFilename + " does not exists." + << std::endl; + return false; + } + + tscpp::UnknownInputArchive inputArchive(file, tps); + while (true) + { + try { - printf("File %s does not exists.\n", logFileWithExt.c_str()); - return false; + inputArchive.unserialize(); } - - std::ifstream file(logFileWithExt); - // file.open; - tscpp::UnknownInputArchive ia(file, tps); - int i = 0; - while (success) + catch (tscpp::TscppException& ex) { - try + // Reached end of file + if (strcmp(ex.what(), "eof") == 0) { - ia.unserialize(); + return true; } - catch (tscpp::TscppException& ex) + // Unknown type found + else if (strcmp(ex.what(), "unknown type") == 0) { - // Reached end of file - if (strcmp(ex.what(), "eof") == 0) - { - break; - } - else if (strcmp(ex.what(), "unknown type") == 0) - { - unknownTypeName = ex.name(); - success = false; - printf("Unknown type found: %s\n", unknownTypeName.c_str()); - break; - } + std::string unknownTypeName = ex.name(); + std::cout << "Unknown type found: " << unknownTypeName + << std::endl; + return false; } } - file.close(); - return success; } - void close() + file.close(); + return true; +} + +void Deserializer::close() +{ + if (!closed) { - if (!closed) + closed = true; + for (auto it = fileStreams.begin(); it != fileStreams.end(); it++) { - closed = true; - for (auto it = fileStreams.begin(); it != fileStreams.end(); it++) - { - (*it)->close(); - delete *it; - } + it->second->close(); + delete it->second; } } - -private: - bool closed = false; - - std::vector<std::ofstream*> fileStreams; - tscpp::TypePoolStream tps; - - std::string prefix; - std::string logFile; - std::string logFileWithExt; -}; +} } // namespace Boardcore diff --git a/src/shared/logger/LogTypes.h b/src/shared/logger/LogTypes.h new file mode 100644 index 0000000000000000000000000000000000000000..6623fbf11bee706926ebb76ddb2c3d2c66aca122 --- /dev/null +++ b/src/shared/logger/LogTypes.h @@ -0,0 +1,116 @@ +/* Copyright (c) 2015-2022 Skyward Experimental Rocketry + * Authors: Luca Erbetta, Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <actuators/Servo/ServoData.h> +#include <algorithms/NAS/NASState.h> +#include <diagnostic/PrintLoggerData.h> +#include <drivers/adc/InternalADCData.h> +#include <events/EventData.h> +#include <logger/Deserializer.h> +#include <logger/LoggerStats.h> +#include <radio/MavlinkDriver/MavlinkStatus.h> +#include <radio/Xbee/XbeeStatus.h> +#include <scheduler/TaskSchedulerData.h> +#include <sensors/ADS1118/ADS1118Data.h> +#include <sensors/ADS131M04/ADS131M04Data.h> +#include <sensors/BME280/BME280Data.h> +#include <sensors/BMP280/BMP280Data.h> +#include <sensors/BMX160/BMX160Data.h> +#include <sensors/BMX160/BMX160WithCorrectionData.h> +#include <sensors/HX711/HX711Data.h> +#include <sensors/L3GD20/L3GD20Data.h> +#include <sensors/LIS3DSH/LIS3DSHData.h> +#include <sensors/LIS3MDL/LIS3MDLData.h> +#include <sensors/MBLoadCell/MBLoadCellData.h> +#include <sensors/MPU9250/MPU9250Data.h> +#include <sensors/MS5803/MS5803Data.h> +#include <sensors/SensorData.h> +#include <sensors/UBXGPS/UBXGPSData.h> +#include <sensors/UbloxGPS/UbloxGPSData.h> +#include <sensors/VN100/VN100Data.h> +#include <sensors/analog/BatteryVoltageSensorData.h> +#include <sensors/analog/CurrentSensorData.h> +#include <sensors/analog/pressure/MPXHZ6130A/MPXHZ6130AData.h> +#include <sensors/analog/pressure/honeywell/HSCMAND015PAData.h> +#include <sensors/analog/pressure/honeywell/HSCMRNN030PAData.h> +#include <sensors/analog/pressure/honeywell/HSCMRNN160KAData.h> +#include <sensors/analog/pressure/honeywell/SSCDANN030PAAData.h> +#include <sensors/analog/pressure/honeywell/SSCDRRN015PDAData.h> + +#include <fstream> +#include <iostream> + +/** + * @brief This file includes all the types the logdecoder script will decode. + * + * All logged classes inside Boardcore should be reported here. + */ + +namespace Boardcore +{ + +namespace LogTypes +{ + +void registerTypes(Deserializer& ds) +{ + ds.registerType<ServoData>(); + ds.registerType<NASState>(); + ds.registerType<LoggingString>(); + ds.registerType<InternalADCData>(); + ds.registerType<EventData>(); + ds.registerType<LoggerStats>(); + ds.registerType<MavlinkStatus>(); + ds.registerType<Xbee::XbeeStatus>(); + ds.registerType<TaskStatsResult>(); + ds.registerType<ADS1118Data>(); + ds.registerType<ADS131M04Data>(); + ds.registerType<BME280Data>(); + ds.registerType<BMP280Data>(); + ds.registerType<BMX160Data>(); + ds.registerType<BMX160WithCorrectionData>(); + ds.registerType<HX711Data>(); + ds.registerType<L3GD20Data>(); + ds.registerType<LIS3DSHData>(); + ds.registerType<LIS3MDLData>(); + ds.registerType<MBLoadCellData>(); + ds.registerType<MPU9250Data>(); + ds.registerType<MS5803Data>(); + ds.registerType<TemperatureData>(); + ds.registerType<UbloxGPSData>(); + ds.registerType<UBXGPSData>(); + ds.registerType<VN100Data>(); + ds.registerType<BatteryVoltageSensorData>(); + ds.registerType<CurrentSensorData>(); + ds.registerType<MPXHZ6130AData>(); + ds.registerType<HSCMAND015PAData>(); + ds.registerType<HSCMRNN030PAData>(); + ds.registerType<HSCMRNN160KAData>(); + ds.registerType<SSCDANN030PAAData>(); + ds.registerType<SSCDRRN015PDAData>(); +} + +} // namespace LogTypes + +} // namespace Boardcore diff --git a/src/shared/logger/Logger.cpp b/src/shared/logger/Logger.cpp index 2709fbcc2f7d3a6e9bf860f180f7036200b4a433..31876f6014c64a9e77bae874c4b26fcd7442725b 100644 --- a/src/shared/logger/Logger.cpp +++ b/src/shared/logger/Logger.cpp @@ -42,10 +42,10 @@ using namespace miosix; namespace Boardcore { -int Logger::start() +bool Logger::start() { if (started) - return fileNumber; + return true; // Find the proper log filename base on the current files on the disk string filename; @@ -68,13 +68,13 @@ int Logger::start() { fileNumber = -1; TRACE("Error opening %s file\n", filename.c_str()); - throw runtime_error("Error opening log file"); + return false; } setbuf(file, NULL); // Disable buffering for the file stream - // The boring part, start threads one by one and if they fail, undo + // The boring part, start threads one by one and if they fail, undo. // Perhaps excessive defensive programming as thread creation failure is - // highly unlikely (only if ram is full) + // highly unlikely (only if ram is full). packTh = Thread::create(packThreadLauncher, skywardStack(16 * 1024), 1, this, Thread::JOINABLE); @@ -82,7 +82,7 @@ int Logger::start() { fclose(file); TRACE("Error creating pack thread\n"); - throw runtime_error("Error creating pack thread"); + return false; } writeTh = Thread::create(writeThreadLauncher, skywardStack(16 * 1024), 1, @@ -101,13 +101,13 @@ int Logger::start() fullBufferList.pop(); // Remove nullptr fclose(file); TRACE("Error creating write thread\n"); - throw runtime_error("Error creating write thread"); + return false; } started = true; stats.logNumber = fileNumber; - return fileNumber; + return true; } void Logger::stop() @@ -301,7 +301,6 @@ LoggerResult Logger::logImpl(const char* name, const void* data, { if (started == false) { - TRACE("Attempting to log %s but the Logger is not started!\n", name); stats.droppedSamples++; // Signal that we are trying to write to a closed log diff --git a/src/shared/logger/Logger.h b/src/shared/logger/Logger.h index 9679b837dea71c0d03de3f1bdb79719105c09278..f1ad95f7b077151a555348400c4ff10d82aa98ce 100644 --- a/src/shared/logger/Logger.h +++ b/src/shared/logger/Logger.h @@ -59,15 +59,17 @@ public: /** * @brief Call this function to start the logger. * - * When this function returns, the logger is started, and subsequent calls - * to log will actually log the data. + * The function tryies to start the logger. It first opens the log file and + * then create the pack and write threads. If it fails on one of this + * operation, the logger is not started. + * + * Use getCurrentLogNumber to retrieve the log file number. * * Blocking call. May take a long time. * - * \throws runtime_error if the log could not be opened. - * \return log number. + * \return true if the logger was started correctly. */ - int start(); + bool start(); /** * @brief Call this function to stop the logger. diff --git a/src/shared/math/SkyQuaternion.h b/src/shared/math/SkyQuaternion.h deleted file mode 100644 index 24b424bc85b489f6c9df96ad3fc5f8de5faaa7e1..0000000000000000000000000000000000000000 --- a/src/shared/math/SkyQuaternion.h +++ /dev/null @@ -1,88 +0,0 @@ -/* Copyright (c) 2020 Skyward Experimental Rocketry - * Author: Marco Cella - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#pragma once - -#include <Eigen/Dense> - -namespace Boardcore -{ - -/** - * @brief Class for managing quaternions. - * Convention used: qx, qy, qz, qw (scalar element as last element) - */ -class SkyQuaternion -{ - -public: - SkyQuaternion(); - - /** - * @brief Transform a vector of euler angles ZYX to quaternion. - * - * @param degeul the vector of euler angles to be transformed [deg] - * - * @return transformed quaternion - */ - Eigen::Vector4f eul2quat(Eigen::Vector3f degeul); - - /** - * @brief Transform a quaternion to a vector of euler angles. - * - * @param quat the quaternion to be transformed - * - * @return transformed vector of euler angles [deg] - */ - Eigen::Vector3f quat2eul(Eigen::Vector4f quat); - - /** - * @brief Transform a rotation matrix to a quaternion. - * - * @param R the rotation matrix to be transformed (3x3) - * - * @return transformed quaternion - */ - Eigen::Vector4f rotm2quat(Eigen::Matrix3f R); - - /** - * @brief Normalize a quaternion. - * - * @param quat the quaternion to be normalized - * - * @return boolean indicating if the operation succeded or not - */ - bool quatnormalize(Eigen::Vector4f& quat); - - /** - * @brief Compute the product of two quaternions. - * - * @param q1 the first factor - * @param q2 the second factor - * - * @return the resulting quaternions product - */ - Eigen::Vector4f quatProd(const Eigen::Vector4f q1, - const Eigen::Vector4f q2); -}; - -} // namespace Boardcore diff --git a/src/shared/radio/MavlinkDriver/MavlinkDriver.h b/src/shared/radio/MavlinkDriver/MavlinkDriver.h index fee9e9b5d0df1b9bb3372b01a2c419b4aa34cd46..79a63e0cc9e6b940ebac1a3acd1d74c14338efa8 100644 --- a/src/shared/radio/MavlinkDriver/MavlinkDriver.h +++ b/src/shared/radio/MavlinkDriver/MavlinkDriver.h @@ -250,7 +250,8 @@ bool MavlinkDriver<PktLength, OutQueueSize>::enqueueMsg( const mavlink_message_t& msg) { // Convert mavlink message to char array - uint8_t msgtempBuf[PktLength]; + // Use fixed buffer size to avoid overflows + uint8_t msgtempBuf[256]; int msgLen = mavlink_msg_to_send_buffer(msgtempBuf, &msg); // Append message to the queue @@ -347,11 +348,12 @@ void MavlinkDriver<PktLength, OutQueueSize>::runSender() // Get the first packet in the queue, without removing it pkt = outQueue.get(); - uint64_t age = (uint64_t)miosix::getTick() - pkt.timestamp(); // If the packet is ready or too old, send it + uint64_t age = TimestampTimer::getInstance().getTimestamp() - + pkt.getTimestamp(); if (pkt.isReady() || age >= outBufferMaxAge) { - outQueue.pop(); // remove from queue + outQueue.pop(); // Remove the packet from queue LOG_DEBUG(logger, "Sending packet. Size: {} (age: {})", pkt.size(), age); diff --git a/src/shared/radio/SX1278/SX1278.cpp b/src/shared/radio/SX1278/SX1278.cpp index 52a91f3ba06520aa4b18974468319a22256b6774..bed1ae549d6daf57f3b77e8f6f0787c3efa9b5d9 100644 --- a/src/shared/radio/SX1278/SX1278.cpp +++ b/src/shared/radio/SX1278/SX1278.cpp @@ -31,6 +31,8 @@ namespace Boardcore using namespace SX1278Defs; +long long now() { return miosix::getTick() * 1000 / miosix::TICK_FREQ; } + // Default values for registers constexpr uint8_t REG_OP_MODE_DEFAULT = RegOpMode::LONG_RANGE_MODE_FSK | RegOpMode::MODULATION_TYPE_FSK | @@ -247,10 +249,14 @@ ssize_t SX1278::receive(uint8_t *pkt, size_t max_len) bool SX1278::send(uint8_t *pkt, size_t len) { - // Packets longer than 255 are not supported - if (len > 255) + // Packets longer than FIFO_LEN (-1 for the len byte) are not supported + if (len > SX1278Defs::FIFO_LEN - 1) return false; + // This shouldn't be needed, but for some reason the device "lies" about + // being ready, so lock up if we are going too fast + rateLimitTx(); + bus_mgr.lock(SX1278BusManager::Mode::MODE_TX); // Wait for TX ready @@ -267,11 +273,24 @@ bool SX1278::send(uint8_t *pkt, size_t len) // Wait for packet sent bus_mgr.waitForIrq(RegIrqFlags::PACKET_SENT); bus_mgr.unlock(); + + last_tx = now(); return true; } void SX1278::handleDioIRQ() { bus_mgr.handleDioIRQ(); } +void SX1278::rateLimitTx() +{ + const long long RATE_LIMIT = 2; + + long long delta = now() - last_tx; + if (delta <= RATE_LIMIT) + { + miosix::Thread::sleep(RATE_LIMIT - delta); + } +} + uint8_t SX1278::getVersion() { SPITransaction spi(bus_mgr.getBus(), SPITransaction::WriteBit::INVERTED); diff --git a/src/shared/radio/SX1278/SX1278.h b/src/shared/radio/SX1278/SX1278.h index 6fb4c355d36f81331e7299e6a291e58c737e4156..3d89a9f9494a95ddcae4871f84a8c21132571066 100644 --- a/src/shared/radio/SX1278/SX1278.h +++ b/src/shared/radio/SX1278/SX1278.h @@ -228,6 +228,8 @@ public: public: using Mode = SX1278BusManager::Mode; + void rateLimitTx(); + void setBitrate(int bitrate); void setFreqDev(int freq_dev); void setFreqRF(int freq_rf); @@ -240,6 +242,7 @@ public: void waitForIrq(uint16_t mask); + long long last_tx = 0; SX1278BusManager bus_mgr; PrintLogger logger = Logging::getLogger("sx1278"); }; diff --git a/src/shared/radio/SX1278/SX1278Defs.h b/src/shared/radio/SX1278/SX1278Defs.h index cd50d09a8c3294659f1c1186f2ad1fd0512937c6..06954099644e8a7b6d6b6566e0ad7d2ef1118f3b 100644 --- a/src/shared/radio/SX1278/SX1278Defs.h +++ b/src/shared/radio/SX1278/SX1278Defs.h @@ -33,6 +33,11 @@ namespace Boardcore namespace SX1278Defs { +/** + * @brief Length of the internal FIFO + */ +constexpr int FIFO_LEN = 64; + /** * @brief Main oscillator frequency (Hz) */ diff --git a/src/shared/radio/Xbee/Xbee.h b/src/shared/radio/Xbee/Xbee.h index ea748a68ff96aee0b6a58a78a4c9b447a3a3c41d..abb1be5015ba548258ab5207dd6abccff0bbd00a 100644 --- a/src/shared/radio/Xbee/Xbee.h +++ b/src/shared/radio/Xbee/Xbee.h @@ -38,7 +38,7 @@ using miosix::FastMutex; #ifndef USE_MOCK_PERIPHERALS using GpioType = miosix::GpioPin; #else -#include <utils/testutils/MockGpioPin.h> +#include <utils/TestUtils/MockGpioPin.h> using GpioType = MockGpioPin; #endif diff --git a/src/shared/radio/Xbee/XbeeStatus.h b/src/shared/radio/Xbee/XbeeStatus.h index 05f25880a39fa721c3453a6cce5597fdb2679925..ae0381dd915392ee8aa35e2a95f94a6a6d2baed2 100644 --- a/src/shared/radio/Xbee/XbeeStatus.h +++ b/src/shared/radio/Xbee/XbeeStatus.h @@ -22,7 +22,7 @@ #pragma once -#include <math/Stats.h> +#include <utils/Stats/Stats.h> #include <cstdint> #include <cstdio> diff --git a/src/shared/scheduler/TaskScheduler.cpp b/src/shared/scheduler/TaskScheduler.cpp index 26dc700a04091a73c7d9d9c33a38db9f3a4612d6..0518b34d9e8b23c4a65efc67804e1b1edbd87125 100644 --- a/src/shared/scheduler/TaskScheduler.cpp +++ b/src/shared/scheduler/TaskScheduler.cpp @@ -40,14 +40,6 @@ TaskScheduler::TaskScheduler() bool TaskScheduler::addTask(function_t function, uint32_t period, uint8_t id, Policy policy, int64_t startTick) { - // Perion must be grater than zero! - if (period <= 0 && policy != Policy::ONE_SHOT) - { - LOG_ERR(logger, - "Trying to add a task non one shot with an invalid period"); - return false; - } - Lock<FastMutex> lock(mutex); // Register the task into the map @@ -69,6 +61,18 @@ bool TaskScheduler::addTask(function_t function, uint32_t period, uint8_t id, return result.second; } +bool TaskScheduler::addTask(function_t function, uint32_t period, Policy policy, + int64_t startTick) +{ + uint8_t id = 1; + + auto it = tasks.cbegin(), end = tasks.cend(); + for (; it != end && id == it->first; ++it, ++id) + ; + + return addTask(function, period, id, policy, startTick); +} + bool TaskScheduler::removeTask(uint8_t id) { Lock<FastMutex> lock(mutex); @@ -99,6 +103,8 @@ bool TaskScheduler::removeTask(uint8_t id) bool TaskScheduler::start() { + // This check is necessary to prevent task normalization if the scheduler is + // already stopped if (running) return false; @@ -128,7 +134,7 @@ vector<TaskStatsResult> TaskScheduler::getTaskStats() return result; } -void TaskScheduler ::normalizeTasks() +void TaskScheduler::normalizeTasks() { int64_t currentTick = getTick(); diff --git a/src/shared/scheduler/TaskScheduler.h b/src/shared/scheduler/TaskScheduler.h index 9a3ba6fe09c0940c59c20dcf643a2e5d282534c8..6eb0664158761b6cbc2b1eae124244429d890d94 100644 --- a/src/shared/scheduler/TaskScheduler.h +++ b/src/shared/scheduler/TaskScheduler.h @@ -25,7 +25,7 @@ #include <ActiveObject.h> #include <Singleton.h> #include <diagnostic/PrintLogger.h> -#include <math/Stats.h> +#include <utils/Stats/Stats.h> #include <cstdint> #include <list> @@ -110,6 +110,25 @@ public: Policy policy = Policy::SKIP, int64_t startTick = miosix::getTick()); + /** + * @brief Add a task function to the scheduler with an auto generated id. + * + * Note that each task has it's own unique ID, even one shot tasks! + * Therefore, if a task already exists with the same id, the function will + * fail and return false. + * + * For one shot tasks, the period is useless and not used. + * + * @param function Function to be called periodically. + * @param period Inter call period. + * @param policy Task policy, default is SKIP. + * @param startTick First activation time, useful for synchronizing tasks. + * @return true if the task was added successfully. + */ + bool addTask(function_t function, uint32_t period, + Policy policy = Policy::SKIP, + int64_t startTick = miosix::getTick()); + /** * @brief Removes the task identified by the given id if it exists. * diff --git a/src/shared/scheduler/TaskSchedulerData.h b/src/shared/scheduler/TaskSchedulerData.h index 35b04bbddc1627f8e6477ce6ebc1e44bdd13df4a..0cbe3865763d3d144b304ce2af5e26ea62dcb070 100644 --- a/src/shared/scheduler/TaskSchedulerData.h +++ b/src/shared/scheduler/TaskSchedulerData.h @@ -22,7 +22,7 @@ #pragma once -#include <math/Stats.h> +#include <utils/Stats/Stats.h> #include <cstdint> #include <ostream> diff --git a/src/shared/sensors/ADS1118/ADS1118.cpp b/src/shared/sensors/ADS1118/ADS1118.cpp index 93724f64a2177cfdb5dd801c500deaee0d427dc8..b49ceb3c61a1da619856134ae200177a034ffb4d 100644 --- a/src/shared/sensors/ADS1118/ADS1118.cpp +++ b/src/shared/sensors/ADS1118/ADS1118.cpp @@ -236,7 +236,7 @@ void ADS1118::readChannel(int8_t nextChannel, int8_t prevChannel) } // Convert and save the value if last written configuration is valid - if (prevChannel >= 0) + if (prevChannel > INVALID_CHANNEL && prevChannel < NUM_OF_CHANNELS) { int16_t rawValue = swapBytes16(transferData); diff --git a/src/shared/sensors/BME280/BME280.cpp b/src/shared/sensors/BME280/BME280.cpp index 53f2b2160f01eaee81a6151f4d4fd931fd4ef362..c8fd6e5400f5209f54a43a2897d990a0505d585a 100644 --- a/src/shared/sensors/BME280/BME280.cpp +++ b/src/shared/sensors/BME280/BME280.cpp @@ -66,21 +66,47 @@ bool BME280::init() loadCompensationParameters(); - // Read once the temperature to compute fineTemperature - setConfiguration(BME280_CONFIG_TEMP_SINGLE); - miosix::Thread::sleep( - calculateMaxMeasurementTime(BME280_CONFIG_TEMP_SINGLE)); - readTemperature(); + // Set the configuration 10 times to be sure + for (int i = 0; i < 10; i++) + { + // Read once the temperature to compute fineTemperature + setConfiguration(BME280_CONFIG_TEMP_SINGLE); + miosix::Thread::sleep( + calculateMaxMeasurementTime(BME280_CONFIG_TEMP_SINGLE)); + readTemperature(); - setConfiguration(); + setConfiguration(); + } + + // Set a sleep time to allow the sensor to change internally the data + miosix::Thread::sleep(100); - BME280Config readBackConfig = readConfiguration(); + // I create the config state which represents the logic or of all the + // 5 readConfiguration controls (We perform 5 checks to avoid that the + // sensor is busy implicating in wrong responses) + bool readConfigResult = false; + BME280Config readBackConfig; + + for (int i = 0; i < 10; i++) + { + readBackConfig = readConfiguration(); + // Check if the configration on the device matches ours + if (config.bytes.ctrlHumidity == readBackConfig.bytes.ctrlHumidity && + config.bytes.ctrlPressureAndTemperature == + readBackConfig.bytes.ctrlPressureAndTemperature && + config.bytes.config == readBackConfig.bytes.config) + { + readConfigResult = true; + break; + } + + // After the check i sleep 100 milliseconds + miosix::Thread::sleep(20); + } - // Check if the configration on the device matches ours - if (config.bytes.ctrlHumidity != readBackConfig.bytes.ctrlHumidity || - config.bytes.ctrlPressureAndTemperature != - readBackConfig.bytes.ctrlPressureAndTemperature || - config.bytes.config != readBackConfig.bytes.config) + // If after the 5 iterations the sensor didn't report the configuration + // set I can report the init error + if (!readConfigResult) { LOG_ERR(logger, "Device configuration incorrect, setup failed"); diff --git a/src/shared/sensors/BMX160/BMX160.cpp b/src/shared/sensors/BMX160/BMX160.cpp index 594c6a1854e8f2ece95d032c431f96f46a5f8083..287f9b5231dc7a08d87240f595a291b75a13089e 100644 --- a/src/shared/sensors/BMX160/BMX160.cpp +++ b/src/shared/sensors/BMX160/BMX160.cpp @@ -23,6 +23,7 @@ #include "BMX160.h" #include <assert.h> +#include <utils/Constants.h> namespace Boardcore { @@ -598,25 +599,24 @@ MagnetometerData BMX160::buildMagData(BMX160Defs::MagRaw data, AccelerometerData BMX160::buildAccData(BMX160Defs::AccRaw data, uint64_t timestamp) { - return AccelerometerData{timestamp, data.x * accSensibility * EARTH_GRAVITY, - data.y * accSensibility * EARTH_GRAVITY, - data.z * accSensibility * EARTH_GRAVITY}; + using namespace Constants; + + return AccelerometerData{timestamp, data.x * accSensibility * g, + data.y * accSensibility * g, + data.z * accSensibility * g}; } GyroscopeData BMX160::buildGyrData(BMX160Defs::GyrRaw data, uint64_t timestamp) { + using namespace Constants; + if (config.gyroscopeUnit == BMX160Config::GyroscopeMeasureUnit::DEG) - { return GyroscopeData{timestamp, data.x * gyrSensibility, data.y * gyrSensibility, data.z * gyrSensibility}; - } else - { - return GyroscopeData{timestamp, - data.x * gyrSensibility * DEGREES_TO_RADIANS, - data.y * gyrSensibility * DEGREES_TO_RADIANS, - data.z * gyrSensibility * DEGREES_TO_RADIANS}; - } + return GyroscopeData{timestamp, data.x * gyrSensibility * g, + data.y * gyrSensibility * g, + data.z * gyrSensibility * g}; } const char* BMX160::debugErr(SPITransaction& spi) diff --git a/src/shared/sensors/BMX160/BMX160Data.h b/src/shared/sensors/BMX160/BMX160Data.h index 273ed10aa2f13339362a90b95247784e091931e0..5347ad81cdd5cbbded71cbdea25b7033c350f333 100644 --- a/src/shared/sensors/BMX160/BMX160Data.h +++ b/src/shared/sensors/BMX160/BMX160Data.h @@ -45,13 +45,9 @@ struct BMX160Data : public AccelerometerData, static std::string header() { return "accelerationTimestamp,accelerationX,accelerationY," - "accelerationZ,gyro_" - "timestamp," - "angularVelocityX," - "angularVelocityY," - "angularVelocityZ,magneticFieldTimestamp,magneticFieldX," - "magneticFieldY," - "magneticFieldZ\n"; + "accelerationZ,angularVelocityTimestamp,angularVelocityX," + "angularVelocityY,angularVelocityZ,magneticFieldTimestamp," + "magneticFieldX,magneticFieldY,magneticFieldZ\n"; } void print(std::ostream& os) const diff --git a/src/shared/sensors/BMX160/BMX160WithCorrection.h b/src/shared/sensors/BMX160/BMX160WithCorrection.h index 627685ec1fec4a5c62dc6907cfe3a2178d40bd0f..fa139ef6bca95e2b6296a97be26185c050fedfd4 100644 --- a/src/shared/sensors/BMX160/BMX160WithCorrection.h +++ b/src/shared/sensors/BMX160/BMX160WithCorrection.h @@ -24,6 +24,7 @@ #include <diagnostic/PrintLogger.h> #include <sensors/BMX160/BMX160.h> +#include <sensors/calibration/AxisOrientation.h> #include <sensors/calibration/BiasCalibration.h> #include <sensors/calibration/SixParameterCalibration.h> diff --git a/src/shared/sensors/HX711/HX711.cpp b/src/shared/sensors/HX711/HX711.cpp index b08b99495064c948fb05c1af2d48101d3fd6659d..8e3fa92694daa960e71db8a1463147e11bdd3e9d 100644 --- a/src/shared/sensors/HX711/HX711.cpp +++ b/src/shared/sensors/HX711/HX711.cpp @@ -63,19 +63,34 @@ HX711Data HX711::sampleImpl() sckPin.alternateFunction(sckAlternateFunction); if (sample & static_cast<int32_t>(0x800000)) - sample |= 0xFF << 24; + sample |= static_cast<uint32_t>(0xFF) << 24; if (sample == static_cast<int32_t>(0xFFFFFFFF)) return lastSample; return {TimestampTimer::getInstance().getTimestamp(), - (sample + offset) / scale}; + static_cast<float>(sample - offset) * scale}; } +void HX711::computeScale(float value, float sample) +{ + // Convert the sample in raw measurement with current scale factor + sample = sample / scale; + + // Update the scale such that the sample corresponds to the given value + this->scale = value / sample; +} + +void HX711::computeScale(float value) { computeScale(value, lastSample.load); } + void HX711::setScale(float scale) { this->scale = scale; } -void HX711::setZero() { offset = -lastSample.weight * scale; } +float HX711::getScale() { return scale; } + +void HX711::setOffset(float offset) { this->offset = offset / scale; } + +void HX711::updateOffset(float offset) { this->offset += offset / scale; } -void HX711::setZero(float offset) { this->offset = offset * scale; } +float HX711::getOffset() { return offset; } -} // namespace Boardcore \ No newline at end of file +} // namespace Boardcore diff --git a/src/shared/sensors/HX711/HX711.h b/src/shared/sensors/HX711/HX711.h index 24d699ae5fef8857acb4c0152cd71792d1c8a783..4674ea16e84c255470bb80ec54aebb997d624665 100644 --- a/src/shared/sensors/HX711/HX711.h +++ b/src/shared/sensors/HX711/HX711.h @@ -65,23 +65,52 @@ public: bool selfTest() override; /** - * @brief Sets the scale value. + * @brief Calculates the scale value such that the load cell's output + * matches the given value. * - * This value is used to convert the raw data into weight. + * The value is used to compute the scale coefficient in this way: + * scale = value / (sample - offset) + * + * @param value Value that the load cell should read now. + * @param sample Sensor sample used to compute the scale. + */ + void computeScale(float value, float sample); + + /** + * @brief Same as computeScale but uses the last sample. + */ + void computeScale(float value); + + /** + * @brief Simply changes the scale. + * + * @param scale New scale value. */ void setScale(float scale); /** - * @brief Sets the offset to zero the current measurement. + * @brief Returns the current scale. */ - void setZero(); + float getScale(); /** * @brief Sets the offset to the given value. * - * @param offset Offset in Kg. + * @param offset Offset that will be removed from the measurement. + */ + void setOffset(float offset); + + /** + * @brief Updates the offset by adding it to the current offset. + * + * @param offset Offset that will be removed from the measurement. + */ + void updateOffset(float offset); + + /** + * @brief Return the current offset. */ - void setZero(float offset); + float getOffset(); private: HX711Data sampleImpl() override; diff --git a/src/shared/sensors/HX711/HX711Data.h b/src/shared/sensors/HX711/HX711Data.h index 6d4f4940229b86fd157f6b4b133ee6b7ec8c9d14..49e25db1942c56331a90e9bbe6cd29f21741607d 100644 --- a/src/shared/sensors/HX711/HX711Data.h +++ b/src/shared/sensors/HX711/HX711Data.h @@ -32,17 +32,17 @@ struct HX711Data : public LoadCellData { HX711Data() : LoadCellData{0, 0} {} - HX711Data(uint64_t weightTimestamp, float weight) - : LoadCellData{weightTimestamp, weight} + HX711Data(uint64_t loadTimestamp, float load) + : LoadCellData{loadTimestamp, load} { } - static std::string header() { return "weightTimestamp,weight\n"; } + static std::string header() { return "loadTimestamp,load\n"; } void print(std::ostream& os) const { - os << weightTimestamp << "," << weight << "\n"; + os << loadTimestamp << "," << load << "\n"; } }; -} // namespace Boardcore \ No newline at end of file +} // namespace Boardcore diff --git a/src/shared/sensors/L3GD20/L3GD20.h b/src/shared/sensors/L3GD20/L3GD20.h index bdd7c78fda3d8d82fffaddd95d0f3a915613be65..802b0ed2fefad0331afcd5da5598178984e4eadc 100644 --- a/src/shared/sensors/L3GD20/L3GD20.h +++ b/src/shared/sensors/L3GD20/L3GD20.h @@ -25,10 +25,10 @@ #include <drivers/spi/SPIDriver.h> #include <drivers/timer/TimestampTimer.h> -#include <math/Vec3.h> #include <miosix.h> #include <sensors/Sensor.h> +#include <Eigen/Core> #include <array> #include "L3GD20Data.h" @@ -220,9 +220,8 @@ public: int16_t y = buf[2] | buf[3] << 8; int16_t z = buf[4] | buf[5] << 8; - Vec3 rads = toRadiansPerSecond(x, y, z); - lastFifo[0] = {lastSampleTimestamp, rads.getX(), rads.getY(), - rads.getZ()}; + Eigen::Vector3f rads = toRadiansPerSecond(x, y, z); + lastFifo[0] = {lastSampleTimestamp, rads(0), rads(1), rads(2)}; } else // FIFO is enabled @@ -243,7 +242,7 @@ public: { bool rmv; // Check for duplicates: there seems to be a bug where the - // sensor occasionaly shifts out the same sample two times in a + // sensor occasionally shifts out the same sample two times in a // row: discard one if (i < fifoLevel - 1) { @@ -276,7 +275,7 @@ public: // Ignoring possible duplicates, the timestamp of the ith sample // in the fifo is: // ts(i) = ts(fifoWatermark) + (i - fifoWatermark)*dt; - Vec3 rads = + Eigen::Vector3f rads = toRadiansPerSecond(buf[i * 6] | buf[i * 6 + 1] << 8, buf[i * 6 + 2] | buf[i * 6 + 3] << 8, buf[i * 6 + 4] | buf[i * 6 + 5] << 8); @@ -284,7 +283,7 @@ public: lastFifo[i - duplicates] = L3GD20Data{ lastInterruptTimestamp + ((int)i - (int)fifoWatermark - (int)duplicates) * dt, - rads.getX(), rads.getY(), rads.getZ()}; + rads(0), rads(1), rads(2)}; } lastFifoLevel = fifoLevel - duplicates; @@ -294,9 +293,10 @@ public: } private: - Vec3 toRadiansPerSecond(int16_t x, int16_t y, int16_t z) + Eigen::Vector3f toRadiansPerSecond(int16_t x, int16_t y, int16_t z) { - return Vec3(x * sensitivity, y * sensitivity, z * sensitivity); + return Eigen::Vector3f(x * sensitivity, y * sensitivity, + z * sensitivity); } static constexpr float SENSITIVITY_250 = 0.00875f; diff --git a/src/shared/sensors/MAX31855/MAX31855.cpp b/src/shared/sensors/MAX31855/MAX31855.cpp index 137cf2569ceb93d17e72fbfa07a38243daa0563f..3a723a6d8a7f46878fafd41162925a734fccf6ab 100644 --- a/src/shared/sensors/MAX31855/MAX31855.cpp +++ b/src/shared/sensors/MAX31855/MAX31855.cpp @@ -59,7 +59,7 @@ bool MAX31855::checkConnection() if ((sample[1] & 0x7) != 0) { lastError = SensorErrors::SELF_TEST_FAIL; - LOG_ERR(logger, "Self test failed, the termocouple is not connected"); + LOG_ERR(logger, "Self test failed, the thermocouple is not connected"); return false; } diff --git a/src/shared/sensors/MBLoadCell/MBLoadCell.cpp b/src/shared/sensors/MBLoadCell/MBLoadCell.cpp index 1874fff78cb19a8b22ff69cefb07e4c1462c9bb2..7f943cd5c2078d7d26c0d0b4ecf2db0433c67416 100644 --- a/src/shared/sensors/MBLoadCell/MBLoadCell.cpp +++ b/src/shared/sensors/MBLoadCell/MBLoadCell.cpp @@ -152,28 +152,28 @@ void MBLoadCell::printData() if (maxPrint) { - TRACE("NEW MAX %.2f (ts: %.3f [s])\n", maxWeight.weight, - maxWeight.weightTimestamp / 1000000.0); + TRACE("NEW MAX %.2f (ts: %.3f [s])\n", maxWeight.load, + maxWeight.loadTimestamp / 1000000.0); maxPrint = false; } if (minPrint) { - TRACE("NEW MIN %.2f (ts: %.3f [s])\n", minWeight.weight, - minWeight.weightTimestamp / 1000000.0); + TRACE("NEW MIN %.2f (ts: %.3f [s])\n", minWeight.load, + minWeight.loadTimestamp / 1000000.0); minPrint = false; } } -Data MBLoadCell::getMaxWeight() { return maxWeight; } +MBLoadCellData MBLoadCell::getMaxWeight() { return maxWeight; } -Data MBLoadCell::getMinWeight() { return minWeight; } +MBLoadCellData MBLoadCell::getMinWeight() { return minWeight; } bool MBLoadCell::selfTest() { return true; } -Data MBLoadCell::sampleImpl() +MBLoadCellData MBLoadCell::sampleImpl() { - Data value; + MBLoadCellData value; switch (settings.mode) { case LoadCellModes::CONT_MOD_T: @@ -191,7 +191,7 @@ Data MBLoadCell::sampleImpl() } // Memorizing also the maximum gross weight registered - if (!maxWeight.valid || maxWeight.weight < value.weight) + if (!maxWeight.valid || maxWeight.load < value.load) { maxWeight = value; maxSetted = true; @@ -203,7 +203,7 @@ Data MBLoadCell::sampleImpl() } // Memorizing also the minimum gross weight registered - if (!minWeight.valid || minWeight.weight > value.weight) + if (!minWeight.valid || minWeight.load > value.load) { minWeight = value; minSetted = true; @@ -217,23 +217,23 @@ Data MBLoadCell::sampleImpl() return value; } -Data MBLoadCell::sampleContModT() +MBLoadCellData MBLoadCell::sampleContModT() { DataModT data; receive(&data); - return Data(atof(data.weight) / 10.0); + return MBLoadCellData(atof(data.weight) / 10.0); } -Data MBLoadCell::sampleContModTd() +MBLoadCellData MBLoadCell::sampleContModTd() { DataModTd data; receive(&data); - return Data(atof(data.weightT) / 10.0); + return MBLoadCellData(atof(data.weightT) / 10.0); } -Data MBLoadCell::sampleAsciiModTd() +MBLoadCellData MBLoadCell::sampleAsciiModTd() { DataAsciiRequest request; @@ -262,7 +262,7 @@ Data MBLoadCell::sampleAsciiModTd() else { // Taking the value returned - return Data(stof(response.substr(3, 6)) / 10.0); + return MBLoadCellData(stof(response.substr(3, 6)) / 10.0); } } diff --git a/src/shared/sensors/MBLoadCell/MBLoadCell.h b/src/shared/sensors/MBLoadCell/MBLoadCell.h index aefe3c6808653ca2ca93abd0742feae5d8f23273..d8e57f5f65bdded510a9ad6830bed13d1c053106 100644 --- a/src/shared/sensors/MBLoadCell/MBLoadCell.h +++ b/src/shared/sensors/MBLoadCell/MBLoadCell.h @@ -47,7 +47,7 @@ namespace Boardcore * - ASCII-modTd: bidirectional mode that consists in sending a request and * receiving a response with the data requested or an error message */ -class MBLoadCell : public Sensor<Data> +class MBLoadCell : public Sensor<MBLoadCellData> { public: /** @@ -86,12 +86,12 @@ public: /** * @brief Returns a copy of the max weight detected. */ - Data getMaxWeight(); + MBLoadCellData getMaxWeight(); /** * @brief Returns a copy of the min weight detected. */ - Data getMinWeight(); + MBLoadCellData getMinWeight(); bool selfTest() override; @@ -102,22 +102,22 @@ protected: * * @return The weight measured from the load cell. */ - Data sampleImpl() override; + MBLoadCellData sampleImpl() override; /** * @brief Sampling in the "continuous Mod T" mode. */ - Data sampleContModT(void); + MBLoadCellData sampleContModT(void); /** * @brief Sampling in the "continuous Mod Td" mode. */ - Data sampleContModTd(void); + MBLoadCellData sampleContModTd(void); /** * @brief Sampling in the "ASCII Mod Td" mode. */ - Data sampleAsciiModTd(void); + MBLoadCellData sampleAsciiModTd(void); /** * @brief Forges a request for the ascii mode. @@ -158,8 +158,8 @@ protected: private: MBLoadCellSettings settings; ///< Contains all the configuration - Data maxWeight; ///< Maximum weight detected by the load cell - Data minWeight; ///< Minimum weight detected by the load cell + MBLoadCellData maxWeight; ///< Maximum weight detected by the load cell + MBLoadCellData minWeight; ///< Minimum weight detected by the load cell bool maxSetted; bool maxPrint; bool minSetted; diff --git a/src/shared/sensors/MBLoadCell/MBLoadCellData.h b/src/shared/sensors/MBLoadCell/MBLoadCellData.h index c576fa20e15f691d11f8a060e6545f449c786310..3c9c12495040743ac5dc2bfd0a7647f62115ee94 100644 --- a/src/shared/sensors/MBLoadCell/MBLoadCellData.h +++ b/src/shared/sensors/MBLoadCell/MBLoadCellData.h @@ -22,9 +22,9 @@ #pragma once -#include <drivers/timer/TimestampTimer.h> #include <utils/Debug.h> +#include <cstdio> #include <map> #include "sensors/SensorData.h" @@ -87,24 +87,25 @@ enum ReturnsStates * @brief Structure that stores a data value, with his timestamp and his * validity. */ -struct Data : public LoadCellData +struct MBLoadCellData : public LoadCellData { bool valid = false; - Data() : LoadCellData{0, 0.0}, valid(false) {} + MBLoadCellData() : LoadCellData{0, 0.0}, valid(false) {} - explicit Data(float data) - : LoadCellData{TimestampTimer::getInstance().getTimestamp(), data}, - valid(true) + explicit MBLoadCellData(float data) : MBLoadCellData{0, data} {} + + explicit MBLoadCellData(uint64_t loadTimestamp, float data) + : LoadCellData{loadTimestamp, data}, valid(true) { } - static std::string header() { return "weightTimestamp,weight\n"; } + static std::string header() { return "loadTimestamp,weight\n"; } void print(std::ostream& os) const { if (valid) - os << weightTimestamp / 1000000.0 << "," << weight << "\n"; + os << loadTimestamp / 1000000.0 << "," << load << "\n"; } }; @@ -115,10 +116,10 @@ struct MBLoadCellSettings { LoadCellModes mode; bool grossMode; - Data peakWeight; - Data setpoint1; - Data setpoint2; - Data setpoint3; + MBLoadCellData peakWeight; + MBLoadCellData setpoint1; + MBLoadCellData setpoint2; + MBLoadCellData setpoint3; /** * @brief Updates the correct value with the data passed. Also, memorizes @@ -129,16 +130,16 @@ struct MBLoadCellSettings switch (val) { case PEAK_WEIGHT: - peakWeight = Data(data); + peakWeight = MBLoadCellData(data); break; case GET_SETPOINT_1: - setpoint1 = Data(data); + setpoint1 = MBLoadCellData(data); break; case GET_SETPOINT_2: - setpoint2 = Data(data); + setpoint2 = MBLoadCellData(data); break; case GET_SETPOINT_3: - setpoint3 = Data(data); + setpoint3 = MBLoadCellData(data); break; default: break; @@ -151,22 +152,22 @@ struct MBLoadCellSettings void print() const { /*if (netWeight.valid) - TRACE("Net Weight : %f [Kg]\n", netWeight.data); + TRACE("Net Weight : %f [Kg]\n", netWeight.load); if (grossWeight.valid) - TRACE("Gross Weight : %f [Kg]\n", grossWeight.data); + TRACE("Gross Weight : %f [Kg]\n", grossWeight.load); */ if (peakWeight.valid) - TRACE("Peak Weight : %f [Kg]\n", peakWeight.weight); + TRACE("Peak Weight : %f [Kg]\n", peakWeight.load); if (setpoint1.valid) - TRACE("Setpoint 1 : %f [Kg]\n", setpoint1.weight); + TRACE("Setpoint 1 : %f [Kg]\n", setpoint1.load); if (setpoint2.valid) - TRACE("Setpoint 2 : %f [Kg]\n", setpoint2.weight); + TRACE("Setpoint 2 : %f [Kg]\n", setpoint2.load); if (setpoint3.valid) - TRACE("Setpoint 3 : %f [Kg]\n", setpoint3.weight); + TRACE("Setpoint 3 : %f [Kg]\n", setpoint3.load); } }; @@ -223,7 +224,7 @@ struct DataAsciiRequest checksum ^= str[i]; } - itoa(checksum, ck, 16); + sprintf(ck, "%x", checksum); } /** diff --git a/src/shared/sensors/MPU9250/MPU9250.cpp b/src/shared/sensors/MPU9250/MPU9250.cpp index 4f0d1eaa1ed0e0849ef1fbd41080a0d32a12e5dd..0348e7afe5ae76c272fc2cbea109439f795dbcea 100644 --- a/src/shared/sensors/MPU9250/MPU9250.cpp +++ b/src/shared/sensors/MPU9250/MPU9250.cpp @@ -29,11 +29,12 @@ namespace Boardcore { -MPU9250::MPU9250(SPISlave spiSlave_, unsigned short samplingRate_, - MPU9250GyroFSR gyroFsr_, MPU9250AccelFSR accelFsr_, - SPI::ClockDivider highSpeedSpiClockDivider_) - : spiSlave(spiSlave_), samplingRate(samplingRate_), gyroFsr(gyroFsr_), - accelFsr(accelFsr_), highSpeedSpiClockDivider(highSpeedSpiClockDivider_) +MPU9250::MPU9250(SPIBusInterface& bus, miosix::GpioPin cs, SPIBusConfig config, + unsigned short samplingRate, MPU9250GyroFSR gyroFsr, + MPU9250AccelFSR accelFsr, + SPI::ClockDivider highSpeedSpiClockDivider) + : spiSlave(bus, cs, config), samplingRate(samplingRate), gyroFsr(gyroFsr), + accelFsr(accelFsr), highSpeedSpiClockDivider(highSpeedSpiClockDivider) { } @@ -434,7 +435,7 @@ void MPU9250::writeSPIWithDelay(SPITransaction& transaction, uint8_t reg, float MPU9250::normalizeAcceleration(int16_t rawValue) { return static_cast<float>(rawValue) / 32768.0f * - ACCELERATION_FS_MAP[accelFsr >> 3] * EARTH_GRAVITY; + ACCELERATION_FS_MAP[accelFsr >> 3] * Constants::g; } // Page 33 of register map document @@ -446,7 +447,7 @@ float MPU9250::normalizeTemperature(int16_t rawValue) float MPU9250::normalizeGyroscope(int16_t rawValue) { return static_cast<float>(rawValue) / 32768.0f * - GYROSCOPE_FS_MAP[gyroFsr >> 3] * DEGREES_TO_RADIANS; + GYROSCOPE_FS_MAP[gyroFsr >> 3] * Constants::DEGREES_TO_RADIANS; } float MPU9250::normalizeMagnetometer(int16_t rawValue, float adjustmentCoeff) diff --git a/src/shared/sensors/MPU9250/MPU9250.h b/src/shared/sensors/MPU9250/MPU9250.h index e6cf27e7f66ab86346e95c59c6605f841098270c..039eda52f5177318bdea77a24679c5e31079e1cb 100644 --- a/src/shared/sensors/MPU9250/MPU9250.h +++ b/src/shared/sensors/MPU9250/MPU9250.h @@ -189,14 +189,16 @@ public: /** * @brief Instantiates the driver * - * @param highSpeedSpiClockDivider_ Clocl diver for 20MHz SPI communication + * @param highSpeedSpiClockDivider Clocl diver for 20MHz SPI communication * with the device */ explicit MPU9250( - SPISlave spiSlave_, unsigned short samplingRate_ = 100, - MPU9250GyroFSR gyroFsr_ = GYRO_FSR_250DPS, - MPU9250AccelFSR accelFsr_ = ACCEL_FSR_2G, - SPI::ClockDivider highSpeedSpiClockDivider_ = SPI::ClockDivider::DIV_4); + SPIBusInterface& bus, miosix::GpioPin cs, + SPIBusConfig config = getDefaultSPIConfig(), + unsigned short samplingRate = 100, + MPU9250GyroFSR gyroFsr = GYRO_FSR_250DPS, + MPU9250AccelFSR accelFsr = ACCEL_FSR_2G, + SPI::ClockDivider highSpeedSpiClockDivider = SPI::ClockDivider::DIV_4); /** * @brief Constructs the default config for SPI Bus. diff --git a/src/shared/sensors/MS5803/MS5803.cpp b/src/shared/sensors/MS5803/MS5803.cpp index 1a2735f642e15deb572aa2e0c8bcd7be29d67636..52eec1bb6c3f0c15e833b75eaea66205c26b56e2 100644 --- a/src/shared/sensors/MS5803/MS5803.cpp +++ b/src/shared/sensors/MS5803/MS5803.cpp @@ -28,15 +28,9 @@ namespace Boardcore { -MS5803::MS5803(SPISlave spiSlave_, uint16_t temperatureDivider_) - : spiSlave(spiSlave_), temperatureDivider(temperatureDivider_) -{ -} - -MS5803::MS5803(SPIBusInterface& spiBus_, miosix::GpioPin cs_, - SPIBusConfig spiConfig_, uint16_t temperatureDivider_) - : spiSlave(spiBus_, cs_, spiConfig_), - temperatureDivider(temperatureDivider_) +MS5803::MS5803(SPIBusInterface& spiBus, miosix::GpioPin cs, + SPIBusConfig spiConfig, uint16_t temperatureDivider) + : spiSlave(spiBus, cs, spiConfig), temperatureDivider(temperatureDivider) { } diff --git a/src/shared/sensors/MS5803/MS5803.h b/src/shared/sensors/MS5803/MS5803.h index 427cb78ca57671c6afda72e3c4e4c0941f106132..6065ebcbab862231cf4fcd37e12d857cb4119198 100644 --- a/src/shared/sensors/MS5803/MS5803.h +++ b/src/shared/sensors/MS5803/MS5803.h @@ -73,9 +73,8 @@ public: static constexpr uint8_t TIMEOUT = 5; - explicit MS5803(SPISlave spiSlave_, uint16_t temperatureDivider_ = 1); - MS5803(SPIBusInterface& spiBus_, miosix::GpioPin cs_, - SPIBusConfig spiConfig_, uint16_t temperatureDivider_ = 1); + MS5803(SPIBusInterface& spiBus, miosix::GpioPin cs, + SPIBusConfig spiConfig = {}, uint16_t temperatureDivider = 1); bool init() override; diff --git a/src/shared/sensors/MS5803/MS5803Data.h b/src/shared/sensors/MS5803/MS5803Data.h index 596dcec13c84878696b79119a5be30b1be0f5a3a..10eb959bd39c60b15e4c1abac74986f517d7ce3d 100644 --- a/src/shared/sensors/MS5803/MS5803Data.h +++ b/src/shared/sensors/MS5803/MS5803Data.h @@ -55,7 +55,7 @@ struct MS5803Data : public PressureData, TemperatureData static std::string header() { - return "pressureTimestamp,press,temperatureTimestamp,temp\n"; + return "pressureTimestamp,pressure,temperatureTimestamp,temperature\n"; } void print(std::ostream& os) const diff --git a/src/shared/sensors/SensorData.h b/src/shared/sensors/SensorData.h index de72201bf50d2fc64cb19bdc744238cc8076aa4f..2a2a2ecb08270a9b4e107871e0480b4bb7b42f79 100644 --- a/src/shared/sensors/SensorData.h +++ b/src/shared/sensors/SensorData.h @@ -1,5 +1,5 @@ -/* Copyright (c) 2020 Skyward Experimental Rocketry - * Author: Luca Conterio +/* Copyright (c) 2020-2022 Skyward Experimental Rocketry + * Authors: Luca Conterio, Alberto Nidasio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -56,8 +56,15 @@ struct TimestampData struct LoadCellData { - uint64_t weightTimestamp; - float weight; + uint64_t loadTimestamp; + float load; + + static std::string header() { return "loadTimestamp,load\n"; } + + void print(std::ostream& os) const + { + os << loadTimestamp << "," << load << "\n"; + } }; struct TemperatureData diff --git a/src/shared/sensors/SensorManager.cpp b/src/shared/sensors/SensorManager.cpp index 39f775e97e642c8699797cbc15da24150fae855d..53ac4adc512f65aea68ca0e313ee6b51b3984870 100644 --- a/src/shared/sensors/SensorManager.cpp +++ b/src/shared/sensors/SensorManager.cpp @@ -50,7 +50,12 @@ SensorManager::~SensorManager() delete scheduler; } -bool SensorManager::start() { return scheduler->start() && initResult; } +bool SensorManager::start() +{ + if (customScheduler) + scheduler->start(); + return initResult; +} void SensorManager::stop() { scheduler->stop(); } @@ -173,11 +178,9 @@ bool SensorManager::init(const SensorMap_t& sensorsMap) samplersMap[sensor] = newSampler; if (currentSamplerId == MAX_TASK_ID) - { LOG_WARN(logger, "Max task ID (255) reached in task scheduler, IDs " "will start again from 0"); - } currentSamplerId++; } diff --git a/src/shared/sensors/SensorManager.h b/src/shared/sensors/SensorManager.h index 4fb828a37ce87b05c59321dc78902a54a5a58fd3..3f6cd41900795f9ebcee5c4a26ca9b0891598177 100644 --- a/src/shared/sensors/SensorManager.h +++ b/src/shared/sensors/SensorManager.h @@ -59,8 +59,14 @@ public: */ ~SensorManager(); + /** + * @brief Starts the task scheduler. + */ bool start(); + /** + * @brief Starts the task scheduler. + */ void stop(); /** @@ -95,7 +101,7 @@ public: const vector<TaskStatsResult> getSamplersStats(); private: - SensorManager(const SensorManager&) = delete; + SensorManager(const SensorManager&) = delete; SensorManager& operator=(const SensorManager&) = delete; /** diff --git a/src/shared/sensors/UbloxGPS/UbloxGPS.cpp b/src/shared/sensors/UbloxGPS/UbloxGPS.cpp new file mode 100644 index 0000000000000000000000000000000000000000..dff24046dfefeca63f88c4499f1d0b2bf4c9f9ce --- /dev/null +++ b/src/shared/sensors/UbloxGPS/UbloxGPS.cpp @@ -0,0 +1,537 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Authors: Davide Bonomini, Davide Mor, Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include "UbloxGPS.h" + +#include <diagnostic/StackLogger.h> +#include <drivers/serial.h> +#include <drivers/timer/TimestampTimer.h> +#include <fcntl.h> +#include <filesystem/file_access.h> + +using namespace miosix; + +namespace Boardcore +{ + +UbloxGPS::UbloxGPS(int baudrate_, uint8_t sampleRate_, int serialPortNum_, + const char* serialPortName_, int defaultBaudrate_) + : baudrate(baudrate_), sampleRate(sampleRate_), + serialPortNumber(serialPortNum_), serialPortName(serialPortName_), + defaultBaudrate(defaultBaudrate_) +{ + // Prepare the gps file path with the specified name + strcpy(gpsFilePath, "/dev/"); + strcat(gpsFilePath, serialPortName); +} + +bool UbloxGPS::init() +{ + // Change the baud rate from the default value + if (!serialCommuinicationSetup()) + { + return false; + } + + Thread::sleep(10); + + // Reset configuration to default + // TODO: maybe move this on serial communication setup + if (!resetConfiguration()) + { + return false; + } + + // Disable NMEA messages + if (!disableNMEAMessages()) + { + return false; + } + + Thread::sleep(100); + + // Set GNSS configuration + if (!setGNSSConfiguration()) + { + return false; + } + + Thread::sleep(100); + + // Enable UBX messages + if (!enableUBXMessages()) + { + return false; + } + + Thread::sleep(100); + + // Set rate + if (!setRate()) + { + return false; + } + + return true; +} + +bool UbloxGPS::selfTest() { return true; } + +UbloxGPSData UbloxGPS::sampleImpl() +{ + Lock<FastMutex> l(mutex); + return threadSample; +} + +void UbloxGPS::run() +{ + /** + * UBX message structure: + * - 2B: Preamble + * - 1B: Message class + * - 1B: Message id + * - 2B: Payload length + * - lB: Payload + * - 2B: Checksum + */ + uint8_t message[6 + UBX_MAX_PAYLOAD_LENGTH + 2]; + uint16_t payloadLength; + + while (!shouldStop()) + { + StackLogger::getInstance().updateStack(THID_GPS); + + // Try to read the message + if (!readUBXMessage(message, payloadLength)) + { + LOG_DEBUG(logger, "Unable to read a UBX message"); + continue; + } + + // Parse the message + if (!parseUBXMessage(message)) + { + LOG_DEBUG(logger, + "UBX message not recognized (class:0x{02x}, id: 0x{02x})", + message[2], message[3]); + } + } +} + +void UbloxGPS::ubxChecksum(uint8_t* msg, int len) +{ + uint8_t ck_a = 0, ck_b = 0; + + // The length must be valid, at least 8 bytes (preamble, msg, length, + // checksum) + if (len <= 8) + { + return; + } + + // Checksum calculation from byte 2 to end of payload + for (int i = 2; i < len - 2; i++) + { + ck_a = ck_a + msg[i]; + ck_b = ck_b + ck_a; + } + + msg[len - 2] = ck_a; + msg[len - 1] = ck_b; +} + +bool UbloxGPS::writeUBXMessage(uint8_t* message, int length) +{ + // Compute the checksum + ubxChecksum(message, length); + + // Write configuration + if (write(gpsFile, message, length) < 0) + { + LOG_ERR(logger, + "Failed to write ubx message (class:0x{02x}, id: 0x{02x})", + message[2], message[3]); + return false; + } + + return true; +} + +bool UbloxGPS::serialCommuinicationSetup() +{ + intrusive_ref_ptr<DevFs> devFs = FilesystemManager::instance().getDevFs(); + + // Change the baudrate only if it is different than the default + if (baudrate != defaultBaudrate) + { + // Close the gps file if already opened + devFs->remove(serialPortName); + + // Open the serial port device with the default boudrate + if (!devFs->addDevice(serialPortName, + intrusive_ref_ptr<Device>(new STM32Serial( + serialPortNumber, defaultBaudrate)))) + { + LOG_ERR(logger, + "[gps] Faild to open serial port {0} with baudrate {1} as " + "file {2}", + serialPortNumber, defaultBaudrate, serialPortName); + return false; + } + + // Open the gps file + if ((gpsFile = open(gpsFilePath, O_RDWR)) < 0) + { + LOG_ERR(logger, "Failed to open gps file {}", gpsFilePath); + return false; + } + + // Change boudrate + if (!setBaudrate()) + { + return false; + }; + + // Close the gps file + if (close(gpsFile) < 0) + { + LOG_ERR(logger, "Failed to close gps file {}", gpsFilePath); + return false; + } + + // Close the serial port + if (!devFs->remove(serialPortName)) + { + LOG_ERR(logger, "Failed to close serial port {} as file {}", + serialPortNumber, serialPortName); + return false; + } + } + + // Reopen the serial port with the configured boudrate + if (!devFs->addDevice(serialPortName, + intrusive_ref_ptr<Device>( + new STM32Serial(serialPortNumber, baudrate)))) + { + LOG_ERR(logger, + "Faild to open serial port {} with baudrate {} as file {}\n", + serialPortNumber, defaultBaudrate, serialPortName); + return false; + } + + // Reopen the gps file + if ((gpsFile = open(gpsFilePath, O_RDWR)) < 0) + { + LOG_ERR(logger, "Failed to open gps file {}", gpsFilePath); + return false; + } + + return true; +} + +bool UbloxGPS::resetConfiguration() +{ + uint8_t ubx_cfg_prt[RESET_CONFIG_MSG_LEN] = { + 0Xb5, 0x62, // Preamble + 0x06, 0x04, // Message UBX-CFG-RST + 0x04, 0x00, // Length + 0x00, 0x00, // navBbrMask (Hot start) + 0x00, // Hardware reset immediately + 0x00, // Reserved + 0xff, 0xff // Checksum + }; + + return writeUBXMessage(ubx_cfg_prt, RESET_CONFIG_MSG_LEN); +} + +bool UbloxGPS::setBaudrate() +{ + uint8_t ubx_cfg_prt[SET_BAUDRATE_MSG_LEN] = { + 0Xb5, 0x62, // Preamble + 0x06, 0x8a, // Message UBX-CFG-VALSET + 0x0c, 0x00, // Length + 0x00, // Version + 0xff, // All layers + 0x00, 0x00, // Reserved + 0x01, 0x00, 0x52, 0x40, // Configuration item key ID + 0xff, 0xff, 0xff, 0xff, // Value + 0xff, 0xff // Checksum + }; + + // Prepare boud rate + ubx_cfg_prt[14] = baudrate; + ubx_cfg_prt[15] = baudrate >> 8; + ubx_cfg_prt[16] = baudrate >> 16; + ubx_cfg_prt[17] = baudrate >> 24; + + return writeUBXMessage(ubx_cfg_prt, SET_BAUDRATE_MSG_LEN); +} + +bool UbloxGPS::disableNMEAMessages() +{ + uint8_t ubx_cfg_valset[DISABLE_NMEA_MESSAGES_MSG_LEN] = { + 0Xb5, 0x62, // Preamble + 0x06, 0x8a, // Message UBX-CFG-VALSET + 0x22, 0x00, // Length + 0x00, // Version + 0xff, // All layers + 0x00, 0x00, // Reserved + 0xbb, 0x00, 0x91, 0x20, // CFG-MSGOUT-NMEA_ID_GGA_UART1 key ID + 0x00, // CFG-MSGOUT-NMEA_ID_GGA_UART1 value + 0xca, 0x00, 0x91, 0x20, // CFG-MSGOUT-NMEA_ID_GLL_UART1 key ID + 0x00, // CFG-MSGOUT-NMEA_ID_GLL_UART1 value + 0xc0, 0x00, 0x91, 0x20, // CFG-MSGOUT-NMEA_ID_GSA_UART1 key ID + 0x00, // CFG-MSGOUT-NMEA_ID_GSA_UART1 value + 0xc5, 0x00, 0x91, 0x20, // CFG-MSGOUT-NMEA_ID_GSV_UART1 key ID + 0x00, // CFG-MSGOUT-NMEA_ID_GSV_UART1 value + 0xac, 0x00, 0x91, 0x20, // CFG-MSGOUT-NMEA_ID_RMC_UART1 key ID + 0x00, // CFG-MSGOUT-NMEA_ID_RMC_UART1 value + 0xb1, 0x00, 0x91, 0x20, // CFG-MSGOUT-NMEA_ID_VTG_UART1 key ID + 0x00, // CFG-MSGOUT-NMEA_ID_VTG_UART1 value + 0xff, 0xff // Checksum + }; + + return writeUBXMessage(ubx_cfg_valset, DISABLE_NMEA_MESSAGES_MSG_LEN); +} + +bool UbloxGPS::setGNSSConfiguration() +{ + uint8_t ubx_cfg_valset[SET_GNSS_CONF_LEN] = { + 0Xb5, 0x62, // Preamble + 0x06, 0x8a, // Message UBX-CFG-VALSET + 0x09, 0x00, // Length + 0x00, // Version + 0x07, // All layers + 0x00, 0x00, // Reserved + 0x21, 0x00, 0x11, 0x20, // CFG-NAVSPG-DYNMODEL key ID + 0x08, // CFG-NAVSPG-DYNMODEL value + 0xff, 0xff // Checksum + }; + + return writeUBXMessage(ubx_cfg_valset, SET_GNSS_CONF_LEN); +} + +bool UbloxGPS::enableUBXMessages() +{ + uint8_t ubx_cfg_valset[ENABLE_UBX_MESSAGES_MSG_LEN] = { + 0Xb5, 0x62, // Preamble + 0x06, 0x8a, // Message UBX-CFG-VALSET + 0x09, 0x00, // Length + 0x00, // Version + 0xff, // All layers + 0x00, 0x00, // Reserved + 0x07, 0x00, 0x91, 0x20, // CFG-MSGOUT-UBX_NAV_PVT_UART1 key ID + 0x01, // CFG-MSGOUT-UBX_NAV_PVT_UART1 value + 0xff, 0xff // Checksum + }; + + return writeUBXMessage(ubx_cfg_valset, ENABLE_UBX_MESSAGES_MSG_LEN); +} + +bool UbloxGPS::setRate() +{ + uint16_t rate = 1000 / sampleRate; + LOG_DEBUG(logger, "Rate: {}", rate); + + uint8_t ubx_cfg_valset[SET_RATE_MSG_LEN] = { + 0Xb5, 0x62, // Preamble + 0x06, 0x8a, // Message UBX-CFG-VALSET + 0x0a, 0x00, // Length + 0x00, // Version + 0x07, // All layers + 0x00, 0x00, // Reserved + 0x01, 0x00, 0x21, 0x30, // CFG-RATE-MEAS key ID + 0xff, 0xff, // CFG-RATE-MEAS value + 0xff, 0xff // Checksum + }; + + // Prepare rate + ubx_cfg_valset[14] = rate; + ubx_cfg_valset[15] = rate >> 8; + + return writeUBXMessage(ubx_cfg_valset, SET_RATE_MSG_LEN); +} + +bool UbloxGPS::readUBXMessage(uint8_t* message, uint16_t& payloadLength) +{ + // Read preamble + do + { + // Read util the first byte of the preamble + do + { + if (read(gpsFile, &message[0], 1) <= 0) // No more data available + { + return false; + } + } while (message[0] != PREAMBLE[0]); + + // Read the next byte + if (read(gpsFile, &message[1], 1) <= 0) // No more data available + { + return false; + } + } while (message[1] != PREAMBLE[1]); // Continue + + // Read message class and ID + if (read(gpsFile, &message[2], 1) <= 0) + { + return false; + } + if (read(gpsFile, &message[3], 1) <= 0) + { + return false; + } + + // Read length + if (read(gpsFile, &message[4], 2) <= 0) + { + return false; + } + payloadLength = message[4] | (message[5] << 8); + if (payloadLength > UBX_MAX_PAYLOAD_LENGTH) + { + return false; + } + + // Read paylaod and checksum + for (auto i = 0; i < payloadLength + 2; i++) + { + if (read(gpsFile, &message[6 + i], 1) <= 0) + { + return false; + } + } + + // Verify the checksum + uint8_t msgChecksum1 = message[6 + payloadLength]; + uint8_t msgChecksum2 = message[6 + payloadLength + 1]; + ubxChecksum(message, 6 + payloadLength + 2); + if (msgChecksum1 != message[6 + payloadLength] || + msgChecksum2 != message[6 + payloadLength + 1]) + { + LOG_ERR(logger, "Message checksum verification failed"); + return false; + } + + return true; +} + +bool UbloxGPS::parseUBXMessage(uint8_t* message) +{ + switch (message[2]) // Message class + { + case 0x01: // UBX-NAV + return parseUBXNAVMessage(message); + case 0x05: // UBX-ACK + return parseUBXACKMessage(message); + } + return false; +} + +bool UbloxGPS::parseUBXNAVMessage(uint8_t* message) +{ + switch (message[3]) // Message id + { + case 0x07: // UBX-NAV-PVT + // Lock the threadSample variable + Lock<FastMutex> l(mutex); + + // Latitude + int32_t rawLatitude = message[6 + 28] | message[6 + 29] << 8 | + message[6 + 30] << 16 | message[6 + 31] << 24; + threadSample.latitude = (float)rawLatitude / 1e7; + + // Longitude + int32_t rawLongitude = message[6 + 24] | message[6 + 25] << 8 | + message[6 + 26] << 16 | + message[6 + 27] << 24; + threadSample.longitude = (float)rawLongitude / 1e7; + + // Height + int32_t rawHeight = message[6 + 32] | message[6 + 33] << 8 | + message[6 + 34] << 16 | message[6 + 35] << 24; + threadSample.height = (float)rawHeight / 1e3; + + // Velocity north + int32_t rawVelocityNorth = message[6 + 48] | message[6 + 49] << 8 | + message[6 + 50] << 16 | + message[6 + 51] << 24; + threadSample.velocityNorth = (float)rawVelocityNorth / 1e3; + + // Velocity east + int32_t rawVelocityEast = message[6 + 52] | message[6 + 53] << 8 | + message[6 + 54] << 16 | + message[6 + 55] << 24; + threadSample.velocityEast = (float)rawVelocityEast / 1e3; + + // Velocity down + int32_t rawVelocityDown = message[6 + 56] | message[6 + 57] << 8 | + message[6 + 58] << 16 | + message[6 + 59] << 24; + threadSample.velocityDown = (float)rawVelocityDown / 1e3; + + // Speed + int32_t rawSpeed = message[6 + 60] | message[6 + 61] << 8 | + message[6 + 62] << 16 | message[6 + 63] << 24; + threadSample.speed = (float)rawSpeed / 1e3; + + // Track (heading of motion) + int32_t rawTrack = message[6 + 64] | message[6 + 65] << 8 | + message[6 + 66] << 16 | message[6 + 67] << 24; + threadSample.track = (float)rawTrack / 1e5; + + // Number of satellite + threadSample.satellites = (uint8_t)message[6 + 23]; + + // Fix (every type of fix accepted) + threadSample.fix = message[6 + 20] != 0; + + // Timestamp + threadSample.gpsTimestamp = + TimestampTimer::getInstance().getTimestamp(); + + return true; + } + + return false; +} + +bool UbloxGPS::parseUBXACKMessage(uint8_t* message) +{ + switch (message[3]) // Message id + { + case 0x00: // UBX-ACK-NAC + LOG_DEBUG(logger, + "Received NAC for message (class:0x{02x}, id: 0x{02x})", + message[6], message[7]); + return true; + case 0x01: // UBX-ACK-ACK + LOG_DEBUG(logger, + "Received ACK for message (class:0x{02x}, id: 0x{02x})", + message[6], message[7]); + return true; + } + return false; +} + +} // namespace Boardcore diff --git a/src/shared/sensors/UbloxGPS/UbloxGPS.h b/src/shared/sensors/UbloxGPS/UbloxGPS.h new file mode 100644 index 0000000000000000000000000000000000000000..9495bc6f5fe792774f0bb9a59c32f35c6e97e5b8 --- /dev/null +++ b/src/shared/sensors/UbloxGPS/UbloxGPS.h @@ -0,0 +1,151 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Authors: Davide Bonomini, Davide Mor, Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <ActiveObject.h> +#include <diagnostic/PrintLogger.h> +#include <miosix.h> +#include <sensors/Sensor.h> + +#include "UbloxGPSData.h" + +namespace Boardcore +{ + +/** + * @brief Driver for Ublox GPSs + * + * This driver handles communication and setup with Ublox GPSs. It uses the + * binary UBX protocol to retrieve and parse navigation data quicker than using + * the string based NMEA. + * + * At initialization it configures the device with the specified baudrate, reset + * the configuration and sets up UBX messages and GNSS parameters. + * + * Communication with the device is performed through a file, the driver opens + * the serial port under the filepath /dev/<serialPortName>. + * There is no need for the file to be setted up beforhand. + * + * This driver was written for a NEO-M9N gps with the latest version of UBX. + */ +class UbloxGPS : public Sensor<UbloxGPSData>, public ActiveObject +{ +public: + /** + * @brief Construct a new UbloxGPS object + * + * @param boudrate_ Baudrate to communicate with the device (max: 921600, + * min: 4800 for NEO-M9N) + * @param sampleRate_ GPS sample rate (max: 25 for NEO-M9N) + * @param serialPortName_ Name of the file for the gps device + * @param defaultBaudrate_ Startup baudrate (38400 for NEO-M9N) + */ + UbloxGPS(int boudrate_ = 921600, uint8_t sampleRate_ = 10, + int serialPortNumber_ = 2, const char *serialPortName_ = "gps", + int defaultBaudrate_ = 38400); + + /** + * @brief Sets up the serial port baudrate, disables the NMEA messages, + * configures GNSS options and enables UBX-PVT message + * + * @return True if the operation succeeded + */ + bool init() override; + + /** + * @brief Read a single message form the GPS, waits 2 sample cycle + * + * @return True if a message was sampled + */ + bool selfTest() override; + +private: + UbloxGPSData sampleImpl() override; + + void run() override; + + void ubxChecksum(uint8_t *msg, int len); + + /** + * @brief Compute the checksum and write the message to the device + */ + bool writeUBXMessage(uint8_t *message, int length); + + /** + * @brief Sets up the serial port with the correct baudrate + * + * Opens the serial port with the defaul baudrate and changes it to + * the value specified in the constructor, then it reopens the serial port. + * If the device is already using the correct baudrate this won't have + * effect. However if the gps is using a different baudrate the diver won't + * be able to communicate. + */ + bool serialCommuinicationSetup(); + + bool resetConfiguration(); + + bool setBaudrate(); + + bool disableNMEAMessages(); + + bool setGNSSConfiguration(); + + bool enableUBXMessages(); + + bool setRate(); + + bool readUBXMessage(uint8_t *message, uint16_t &payloadLength); + + bool parseUBXMessage(uint8_t *message); + + bool parseUBXNAVMessage(uint8_t *message); + + bool parseUBXACKMessage(uint8_t *message); + + const int baudrate; // [baud] + const uint8_t sampleRate; // [Hz] + const int serialPortNumber; + const char *serialPortName; + const int defaultBaudrate; // [baud] + + char gpsFilePath[16]; ///< Allows for a filename of up to 10 characters + int gpsFile = -1; + + mutable miosix::FastMutex mutex; + UbloxGPSData threadSample{}; + + static constexpr int SET_BAUDRATE_MSG_LEN = 20; + static constexpr int RESET_CONFIG_MSG_LEN = 12; + static constexpr int DISABLE_NMEA_MESSAGES_MSG_LEN = 42; + static constexpr int SET_GNSS_CONF_LEN = 17; + static constexpr int ENABLE_UBX_MESSAGES_MSG_LEN = 17; + static constexpr int SET_RATE_MSG_LEN = 18; + + static constexpr uint8_t PREAMBLE[] = {0xb5, 0x62}; + + static constexpr int UBX_MAX_PAYLOAD_LENGTH = 92; + + PrintLogger logger = Logging::getLogger("ubloxgps"); +}; + +} // namespace Boardcore diff --git a/src/shared/sensors/UbloxGPS/UbloxGPSData.h b/src/shared/sensors/UbloxGPS/UbloxGPSData.h new file mode 100644 index 0000000000000000000000000000000000000000..7f5570000b78b915a956b96e754d784e08f7611f --- /dev/null +++ b/src/shared/sensors/UbloxGPS/UbloxGPSData.h @@ -0,0 +1,47 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Author: Davide Bonomini + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <sensors/SensorData.h> + +namespace Boardcore +{ + +struct UbloxGPSData : public GPSData +{ + static std::string header() + { + return "gps_timestamp,latitude,longitude,height,velocity_north," + "velocity_east,velocity_down,speed,track,num_satellites,fix\n"; + } + + void print(std::ostream &os) const + { + os << gpsTimestamp << "," << latitude << "," << longitude << "," + << height << "," << velocityNorth << "," << velocityEast << "," + << velocityDown << "," << speed << "," << track << "," + << (int)satellites << "," << (int)fix << "\n"; + } +}; + +} // namespace Boardcore diff --git a/src/shared/sensors/VN100/VN100.cpp b/src/shared/sensors/VN100/VN100.cpp index cb97c2b0818efdf597c3e0b270b774e67c212c96..eb9d3e9043d62522ec566c40e501d139403ee1d4 100644 --- a/src/shared/sensors/VN100/VN100.cpp +++ b/src/shared/sensors/VN100/VN100.cpp @@ -27,7 +27,8 @@ namespace Boardcore { -VN100::VN100(unsigned int portNumber, BaudRates baudRate, CRCOptions crc) +VN100::VN100(USARTType *portNumber, USARTInterface::Baudrate baudRate, + CRCOptions crc) : portNumber(portNumber), baudRate(baudRate), crc(crc) { } @@ -128,8 +129,7 @@ bool VN100::sampleRaw() } // Send the IMU sampling command - if (!(serialInterface->send(preSampleImuString->c_str(), - preSampleImuString->length()))) + if (!(serialInterface->writeString(preSampleImuString->c_str()))) { LOG_WARN(logger, "Unable to sample due to serial communication error"); return false; @@ -177,13 +177,6 @@ bool VN100::closeAndReset() return false; } - // Close the serial - if (!(serialInterface->closeSerial())) - { - LOG_WARN(logger, "Impossible to close vn100 serial communication"); - return false; - } - isInit = false; // Free the recvString memory @@ -224,8 +217,7 @@ VN100Data VN100::sampleImpl() } // Returns Quaternion, Magnetometer, Accelerometer and Gyro - if (!(serialInterface->send(preSampleImuString->c_str(), - preSampleImuString->length()))) + if (!(serialInterface->writeString(preSampleImuString->c_str()))) { // If something goes wrong i return the last sampled data return lastSample; @@ -257,8 +249,7 @@ VN100Data VN100::sampleImpl() // Returns Magnetometer, Accelerometer, Gyroscope, Temperature and Pressure // (UNCOMPENSATED) DO NOT USE THESE MAGNETOMETER, ACCELEROMETER AND // GYROSCOPE VALUES - if (!(serialInterface->send(preSampleTempPressString->c_str(), - preSampleTempPressString->length()))) + if (!(serialInterface->writeString(preSampleTempPressString->c_str()))) { // If something goes wrong i return the last sampled data return lastSample; @@ -312,16 +303,10 @@ bool VN100::disableAsyncMessages(bool waitResponse) bool VN100::configDefaultSerialPort() { // Initial default settings - serialInterface = new VN100Serial( - portNumber, static_cast<unsigned int>(BaudRates::Baud_115200)); + serialInterface = new USART(portNumber, USARTInterface::Baudrate::B115200); // Check correct serial init - if (!serialInterface->init()) - { - return false; - } - - return true; + return serialInterface->init(); } /** @@ -333,7 +318,7 @@ bool VN100::configUserSerialPort() std::string command; // I format the command to change baud rate - command = fmt::format("{}{}", "VNWRG,5,", baudRate); + command = fmt::format("{}{}", "VNWRG,5,", static_cast<int>(baudRate)); // I can send the command if (!sendStringCommand(command)) @@ -341,23 +326,14 @@ bool VN100::configUserSerialPort() return false; } - // I can close the serial - serialInterface->closeSerial(); - // Destroy the serial object delete serialInterface; // I can open the serial with user's baud rate - serialInterface = - new VN100Serial(portNumber, static_cast<unsigned int>(baudRate)); + serialInterface = new USART(portNumber, baudRate); // Check correct serial init - if (!serialInterface->init()) - { - return false; - } - - return true; + return serialInterface->init(); } bool VN100::setCrc(bool waitResponse) @@ -443,11 +419,13 @@ bool VN100::selfTestImpl() if (!sendStringCommand("VNRRG,01")) { + LOG_WARN(logger, "Unable to send string command"); return false; } if (!recvStringCommand(recvString, recvStringMaxDimension)) { + LOG_WARN(logger, "Unable to receive string command"); return false; } @@ -661,14 +639,14 @@ bool VN100::sendStringCommand(std::string command) } // I send the final command - if (!(serialInterface->send(command.c_str(), command.length() + 1))) + if (!serialInterface->writeString(command.c_str())) { return false; } // Wait some time // TODO dimension the time - miosix::Thread::sleep(1); + miosix::Thread::sleep(500); return true; } @@ -677,7 +655,7 @@ bool VN100::recvStringCommand(char *command, int maxLength) { int i = 0; // Read the buffer - if (!(serialInterface->recv(command, maxLength))) + if (!(serialInterface->read(command, maxLength))) { return false; } diff --git a/src/shared/sensors/VN100/VN100.h b/src/shared/sensors/VN100/VN100.h index 9d40dc304d1de2101beecf71664ecbb7a806fb30..aa61dbdd9b458a96b411047e66452912337e6adc 100644 --- a/src/shared/sensors/VN100/VN100.h +++ b/src/shared/sensors/VN100/VN100.h @@ -26,7 +26,7 @@ * @brief Driver for the VN100S IMU. * * The VN100S sensor is a calibrated IMU which includes accelerometer, - * magnetometer, gyroscope, barometer and temperature sensor. ùThe device + * magnetometer, gyroscope, barometer and temperature sensor. The device * provides also a calibration matrix and an anti-drift matrix for the gyroscope * values. The goal of this driver though is to interface the sensor in its * basic use. Things like asynchronous data and anti-drift techniques haven't @@ -58,7 +58,7 @@ #include <utils/Debug.h> #include "VN100Data.h" -#include "VN100Serial.h" +#include "drivers/usart/USART.h" namespace Boardcore { @@ -69,19 +69,6 @@ namespace Boardcore class VN100 : public Sensor<VN100Data> { public: - enum class BaudRates : unsigned int - { - Baud_9600 = 9600, - Baud_19200 = 19200, - Baud_38400 = 38400, - Baud_57600 = 57600, - Baud_115200 = 115200, - Baud_128000 = 128000, - Baud_230400 = 230400, - Baud_460800 = 460800, - Baud_921600 = 921600 - }; - enum class CRCOptions : uint8_t { CRC_NO = 0x00, @@ -93,12 +80,13 @@ public: * @brief Constructor. * * @param USART port number. - * @param BaudRate different from the sensor's default. + * @param BaudRate different from the sensor's default [9600, 19200, 38400, + * 57600, 115200, 128000, 230400, 460800, 921600]. * @param Redundancy check option. */ - VN100(unsigned int portNumber = defaultPortNumber, - BaudRates baudRate = BaudRates::Baud_115200, - CRCOptions crc = CRCOptions::CRC_ENABLE_8); + VN100(USARTType *portNumber = USART2, + USART::Baudrate baudRate = USART::Baudrate::B921600, + CRCOptions crc = CRCOptions::CRC_ENABLE_8); bool init() override; @@ -236,8 +224,8 @@ private: */ uint16_t calculateChecksum16(uint8_t *message, int length); - unsigned int portNumber; - BaudRates baudRate; + USARTType *portNumber; + USART::Baudrate baudRate; CRCOptions crc; bool isInit = false; @@ -267,11 +255,10 @@ private: * @brief Serial interface that is needed to communicate * with the sensor via ASCII codes. */ - VN100Serial *serialInterface = nullptr; + USARTInterface *serialInterface = nullptr; PrintLogger logger = Logging::getLogger("vn100"); - static const unsigned int defaultPortNumber = 2; static const unsigned int recvStringMaxDimension = 200; }; } // namespace Boardcore diff --git a/src/shared/sensors/VN100/VN100Data.h b/src/shared/sensors/VN100/VN100Data.h index 5671733e8a479513c71b05df7dcc317f15c20bc0..210a3b82d77b5788afeb00a808a345c7dc1f6fad 100644 --- a/src/shared/sensors/VN100/VN100Data.h +++ b/src/shared/sensors/VN100/VN100Data.h @@ -53,6 +53,7 @@ struct VN100Data : public QuaternionData, /** * @brief Void parameters constructor */ + // cppcheck-suppress uninitDerivedMemberVar VN100Data() : QuaternionData{0, 0.0, 0.0, 0.0, 0.0}, MagnetometerData{0, 0.0, 0.0, 0.0}, @@ -67,35 +68,24 @@ struct VN100Data : public QuaternionData, * @param single data structures for all the data */ // cppcheck-suppress passedByValue + // cppcheck-suppress uninitDerivedMemberVar VN100Data(QuaternionData quat, MagnetometerData magData, AccelerometerData accData, GyroscopeData gyro, TemperatureData temp, PressureData pres) - : QuaternionData{quat.quatTimestamp, quat.quatX, quat.quatY, quat.quatZ, - quat.quatW}, - MagnetometerData{magData.magneticFieldTimestamp, - magData.magneticFieldX, magData.magneticFieldY, - magData.magneticFieldZ}, - AccelerometerData{accData.accelerationTimestamp, - accData.accelerationX, accData.accelerationY, - accData.accelerationZ}, - GyroscopeData{gyro.angularVelocityTimestamp, gyro.angularVelocityX, - gyro.angularVelocityY, gyro.angularVelocityZ}, - TemperatureData{temp.temperatureTimestamp, temp.temperature}, - PressureData{pres.pressureTimestamp, pres.pressure} + : QuaternionData(quat), MagnetometerData(magData), + AccelerometerData(accData), GyroscopeData(gyro), + TemperatureData(temp), PressureData(pres) { } static std::string header() { - return "quatTimestamp,quatX,quatY,quatZ,quatW," - "magneticFieldTimestamp,magneticFieldX," - "magneticFieldY,magneticFieldZ," + return "quatTimestamp,quatX,quatY,quatZ,quatW,magneticFieldTimestamp," + "magneticFieldX,magneticFieldY,magneticFieldZ," "accelerationTimestamp,accelerationX,accelerationY," - "accelerationZ,gyro_" - "timestamp," - "angularVelocityX," - "angularVelocityY,angularVelocityZ" - "temperatureTimestamp,temp,pressureTimestamp,press\n"; + "accelerationZ,gyro_timestamp,angularVelocityX,angularVelocityY," + "angularVelocityZ,temperatureTimestamp,temperature," + "pressureTimestamp,pressure\n"; } void print(std::ostream& os) const diff --git a/src/shared/sensors/analog/loadcell/LoadCell.h b/src/shared/sensors/analog/AnalogLoadCell.h similarity index 85% rename from src/shared/sensors/analog/loadcell/LoadCell.h rename to src/shared/sensors/analog/AnalogLoadCell.h index 76d678ce2304542247fb0fa57b33aed73972ff4d..f554bee330b2595a382b9c5cddc7bb7a44699282 100644 --- a/src/shared/sensors/analog/loadcell/LoadCell.h +++ b/src/shared/sensors/analog/AnalogLoadCell.h @@ -26,15 +26,15 @@ #include <functional> -#include "LoadCellData.h" +#include "AnalogLoadCellData.h" namespace Boardcore { -class LoadCellSensor : public Sensor<LoadCellData> +class AnalogLoadCell : public Sensor<AnalogLoadCellData> { public: - LoadCellSensor(std::function<std::pair<uint64_t, float>()> getVoltage, + AnalogLoadCell(std::function<std::pair<uint64_t, float>()> getVoltage, const float mVtoV, const unsigned int fullScale, const float supplyVoltage = 5) : getVoltage(getVoltage), @@ -46,11 +46,11 @@ public: bool selfTest() override { return true; }; - LoadCellData sampleImpl() override + AnalogLoadCellData sampleImpl() override { - LoadCellData data; + AnalogLoadCellData data; - std::tie(data.timestamp, data.voltage) = getVoltage(); + std::tie(data.loadTimestamp, data.voltage) = getVoltage(); data.load = data.voltage / conversionCoeff; diff --git a/src/shared/sensors/analog/loadcell/LoadCellData.h b/src/shared/sensors/analog/AnalogLoadCellData.h similarity index 85% rename from src/shared/sensors/analog/loadcell/LoadCellData.h rename to src/shared/sensors/analog/AnalogLoadCellData.h index 665ae93111656de0f1c756d2a4003d27093fd1fc..8cb8a7274ad3e2cd0101b43a2b7c70742addaa04 100644 --- a/src/shared/sensors/analog/loadcell/LoadCellData.h +++ b/src/shared/sensors/analog/AnalogLoadCellData.h @@ -22,22 +22,20 @@ #pragma once -#include <ostream> +#include <sensors/SensorData.h> namespace Boardcore { -struct LoadCellData +struct AnalogLoadCellData : LoadCellData { - uint64_t timestamp; float voltage; - float load; - static std::string header() { return "timestamp,voltage,load\n"; } + static std::string header() { return "loadTimestamp,load,voltage\n"; } void print(std::ostream& os) const { - os << timestamp << "," << voltage << "," << load << "\n"; + os << loadTimestamp << "," << load << "," << voltage << "\n"; } }; diff --git a/src/shared/sensors/analog/battery/BatteryVoltageSensor.h b/src/shared/sensors/analog/BatteryVoltageSensor.h similarity index 100% rename from src/shared/sensors/analog/battery/BatteryVoltageSensor.h rename to src/shared/sensors/analog/BatteryVoltageSensor.h diff --git a/src/shared/sensors/analog/battery/BatteryVoltageSensorData.h b/src/shared/sensors/analog/BatteryVoltageSensorData.h similarity index 100% rename from src/shared/sensors/analog/battery/BatteryVoltageSensorData.h rename to src/shared/sensors/analog/BatteryVoltageSensorData.h diff --git a/src/shared/sensors/analog/current/CurrentSensor.h b/src/shared/sensors/analog/CurrentSensor.h similarity index 100% rename from src/shared/sensors/analog/current/CurrentSensor.h rename to src/shared/sensors/analog/CurrentSensor.h diff --git a/src/shared/sensors/analog/current/CurrentSensorData.h b/src/shared/sensors/analog/CurrentSensorData.h similarity index 100% rename from src/shared/sensors/analog/current/CurrentSensorData.h rename to src/shared/sensors/analog/CurrentSensorData.h diff --git a/src/shared/sensors/analog/pressure/honeywell/SSCDRRN015PDA.h b/src/shared/sensors/analog/pressure/honeywell/SSCDRRN015PDA.h index 24e9dd136665c371bf48b6a8ac3b2054ee4424de..1e2785023c487613b58f094d19113e9d5fe51c6f 100644 --- a/src/shared/sensors/analog/pressure/honeywell/SSCDRRN015PDA.h +++ b/src/shared/sensors/analog/pressure/honeywell/SSCDRRN015PDA.h @@ -22,7 +22,7 @@ #pragma once -#include <math/Stats.h> +#include <utils/Stats/Stats.h> #include "HoneywellPressureSensor.h" #include "SSCDRRN015PDAData.h" diff --git a/src/shared/sensors/calibration/AxisOrientation.h b/src/shared/sensors/calibration/AxisOrientation.h new file mode 100644 index 0000000000000000000000000000000000000000..7b6924e7f9ebb9878995dcec88e2cc65345b8fd4 --- /dev/null +++ b/src/shared/sensors/calibration/AxisOrientation.h @@ -0,0 +1,233 @@ +/* Copyright (c) 2021-2022 Skyward Experimental Rocketry + * Authors: Riccardo Musso, Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <sensors/Sensor.h> + +#include <Eigen/Core> +#include <Eigen/Geometry> + +namespace Boardcore +{ + +/** + * This enum act like versors towards the chosen axis. + * + * X, Y and Z always set according to the right hand rule, so that: + * X is the index + * Y is the second finger + * Z is the thumb + * + */ +enum class Direction : uint8_t +{ + POSITIVE_X = 0, + NEGATIVE_X, + POSITIVE_Y, + NEGATIVE_Y, + POSITIVE_Z, + NEGATIVE_Z, +}; + +constexpr const char* humanFriendlyDirection[]{ + "North", "South", "East", "West", "Down", "Up", +}; + +inline Eigen::Vector3f orientationToVector(Direction val) +{ + switch (val) + { + case Direction::POSITIVE_X: + return {1, 0, 0}; + case Direction::NEGATIVE_X: + return {-1, 0, 0}; + case Direction::POSITIVE_Y: + return {0, 1, 0}; + case Direction::NEGATIVE_Y: + return {0, -1, 0}; + case Direction::POSITIVE_Z: + return {0, 0, 1}; + case Direction::NEGATIVE_Z: + return {0, 0, -1}; + default: + /* never happens, added just to shut up the warnings */ + return {0, 0, 0}; + } +} + +/** + * This struct represents in the most general way any kind of transformation of + * the reference frame (axis X, Y and Z). + * This data type is intended to simplify the code, so you shouldn't instantiate + * this struct directly, but rather use the structures AxisAngleOrientation or + * AxisOrthoOrientation that will be automatically corrected to this one, thanks + * to the implicit cast in favor of AxisOrientation. + * + * For example: + * + * AxisAngleOrientation angles ( PI/2, PI, 0); + * AxisOrthoOrientation ortho ( Direction::NEGATIVE_X, + * Direction::POSITIVE_Z ); + * + * // The implicit cast is supported and recommended + * AxisOrientation converted1 = angles; + * AxisOrientation converted2 = ortho; + * + * // Now we can use the generated matrix: + * Eigen::Vector3f zeta = convertedX.getMatrix() * Eigen::Vector3f { 0, 0, 1 } + * + */ +struct AxisOrientation +{ + Eigen::Matrix3f mat; + + AxisOrientation() : mat(Eigen::Matrix3f::Identity()) {} + + AxisOrientation(Eigen::Matrix3f _mat) : mat(_mat) {} + + void setMatrix(Eigen::Matrix3f _mat) { mat = _mat; } + + Eigen::Matrix3f getMatrix() const { return mat; } +}; + +/** + * This struct uses the three angles yaw, pitch and roll to define the + * transformation of the reference frame, so according to N.E.D standard we get: + * + * X (north) + * / + * / + * .----> Y (east) + * | + * | + * v + * Z (down) + * + * Where: + * Yaw is rotation of Z axis + * Pitch is rotation of Y axis + * Roll is rotation of X axis + */ +struct AxisAngleOrientation +{ + float yaw, pitch, roll; + + AxisAngleOrientation() : yaw(0), pitch(0), roll(0) {} + + AxisAngleOrientation(float _yaw, float _pitch, float _roll) + : yaw(_yaw), pitch(_pitch), roll(_roll) + { + } + + operator AxisOrientation() const { return AxisOrientation(getMatrix()); } + + Eigen::Matrix3f getMatrix() const + { + return (Eigen::AngleAxisf(yaw, Eigen::Vector3f{0, 0, 1}) * + Eigen::AngleAxisf(pitch, Eigen::Vector3f{0, 1, 0}) * + Eigen::AngleAxisf(roll, Eigen::Vector3f{1, 0, 0})) + .toRotationMatrix(); + } +}; + +/** + * This struct represents the orientation of the reference frame relative + * to X, Y, Z in the start orientation. + * If we know the orientation of the X and Y axis, using the right hand rule + * we can infer the Z axis. + * + * For example, if the base reference is: + * + * z + * ^ + * | + * | + * /----> y + * / + * x + * + * Then if we set x = NEGATIVE_Y, y = POSITIVE_Z, we get: + * + * y z + * ^ ^ + * | / + * | / + * x <----/ + * + */ +struct AxisOrthoOrientation +{ + Direction xAxis, yAxis; + + AxisOrthoOrientation() + : xAxis(Direction::POSITIVE_X), yAxis(Direction::POSITIVE_Y) + { + } + + AxisOrthoOrientation(Direction _xAxis, Direction _yAxis) + : xAxis(_xAxis), yAxis(_yAxis) + { + } + + operator AxisOrientation() const { return AxisOrientation(getMatrix()); } + + Eigen::Matrix3f getMatrix() const + { + Eigen::Vector3f vx, vy, vz; + + vx = orientationToVector(xAxis); + vy = orientationToVector(yAxis); + vz = vx.cross(vy); + + Eigen::Matrix3f mat; + mat.col(0) << vx; + mat.col(1) << vy; + mat.col(2) << vz; + return mat; + } +}; + +/** + * This struct represents axis orientation relative to a reference system. + * Operatively it simply combines two transformations. It is particularly useful + * to obtain an AxisOrientation from a reference system generally not N.E.D. + */ +struct AxisRelativeOrientation +{ + AxisOrientation systemOrientation, orientation; + + AxisRelativeOrientation(const AxisOrientation& _systemOrientation, + const AxisOrientation& _orientation) + : systemOrientation(_systemOrientation), orientation(_orientation) + { + } + + operator AxisOrientation() const { return {getMatrix()}; } + + Eigen::Matrix3f getMatrix() const + { + return systemOrientation.getMatrix() * orientation.getMatrix(); + } +}; + +} // namespace Boardcore diff --git a/src/shared/sensors/calibration/BiasCalibration.h b/src/shared/sensors/calibration/BiasCalibration.h index 5fc2ed071900c11446e6e5d25c27f34ba050e2b2..be84f860e0118b37dff9bc6e4b6c600ff74aa473 100644 --- a/src/shared/sensors/calibration/BiasCalibration.h +++ b/src/shared/sensors/calibration/BiasCalibration.h @@ -1,5 +1,5 @@ -/* Copyright (c) 2020 Skyward Experimental Rocketry - * Author: Riccardo Musso +/* Copyright (c) 2020-2022 Skyward Experimental Rocketry + * Authors: Riccardo Musso, Alberto Nidasio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -23,93 +23,163 @@ #pragma once #include <sensors/SensorData.h> +#include <sensors/calibration/AxisOrientation.h> +#include <sensors/calibration/Calibration.h> +#include <sensors/calibration/SensorDataExtra.h> #include <Eigen/Core> -#include "Calibration.h" -#include "SensorDataExtra.h" - namespace Boardcore { /** - * This is the dumbest type of calibration possible: it stores a 3d vector - * (called "bias") that will be added to every measurement. - * During the calibration phase it will use a given reference vector (for - * example the gravitational acceleration for the accelerometer), and every time - * you'll feed the model with a new value, you have to give it the orientation - * of the sensor, so it can guess the bias. + * @brief Applies an offset to the data + * + * It stores a vector that will be added to every measurement. + * Note: The type T must implement >> and << operators with a Vector3f. + * + * @tparam T Type used for data items. */ template <typename T> class BiasCorrector : public ValuesCorrector<T> { - public: - BiasCorrector() : bias(0, 0, 0) {} - BiasCorrector(const Eigen::Vector3f& _bias) : bias(_bias) {} - - void operator>>(Eigen::Vector3f& rhs) { rhs = bias; } + BiasCorrector(); - void operator<<(const Eigen::Vector3f& rhs) { bias = rhs; } + BiasCorrector(const Eigen::Vector3f& bias); - void setIdentity() override { bias = {0, 0, 0}; } + void operator>>(Eigen::Vector3f& rhs); - T correct(const T& data) const override - { - Eigen::Vector3f tmp; - T out; + void operator<<(const Eigen::Vector3f& rhs); - data >> tmp; - tmp += bias; - out << tmp; + void setIdentity() override; - return out; - } + T correct(const T& data) const override; private: - Eigen::Vector3f bias; + Eigen::Vector3f bias = Eigen::Vector3f::Zero(); }; +/** + * @brief This is the dumbest type of calibration possible: an offset. + * + * During the calibration phase it will use a given reference vector (for + * example the gravitational acceleration), and every time you'll feed the model + * with a new value, you have to give it the orientation of the sensor, so it + * can guess the bias. + * + * @tparam T + */ template <typename T> class BiasCalibration - : public AbstractCalibrationModel<T, BiasCorrector<T>, AxisOrientation> + : public AbstractCalibration<T, BiasCorrector<T>, AxisOrientation> { public: - BiasCalibration() : sum(0, 0, 0), ref(0, 0, 0), numSamples(0) {} - - void setReferenceVector(Eigen::Vector3f vec) { ref = vec; } - Eigen::Vector3f getReferenceVector() { return ref; } - - /** - * BiasCalibration accepts an indefinite number of samples, - * so feed(...) never returns false. - */ - bool feed(const T& measured, const AxisOrientation& transform) override - { - Eigen::Vector3f vec; - measured >> vec; - - sum += (transform.getMatrix().transpose() * ref) - vec; - numSamples++; - - return true; - } - - bool feed(const T& measured) - { - return feed(measured, AxisOrthoOrientation()); - } - - BiasCorrector<T> computeResult() - { - if (numSamples == 0) - return {Eigen::Vector3f{0, 0, 0}}; - return {sum / numSamples}; - } + BiasCalibration(); + + void setReferenceVector(Eigen::Vector3f vec); + + Eigen::Vector3f getReferenceVector(); + + bool feed(const T& measured, const AxisOrientation& transform) override; + + bool feed(const T& measured); + + BiasCorrector<T> computeResult(); private: Eigen::Vector3f sum, ref; unsigned numSamples; }; +template <typename T> +BiasCorrector<T>::BiasCorrector() +{ +} + +template <typename T> +BiasCorrector<T>::BiasCorrector(const Eigen::Vector3f& bias) : bias(bias) +{ +} + +template <typename T> +void BiasCorrector<T>::operator>>(Eigen::Vector3f& rhs) +{ + rhs = bias; +} + +template <typename T> +void BiasCorrector<T>::operator<<(const Eigen::Vector3f& rhs) +{ + bias = rhs; +} + +template <typename T> +void BiasCorrector<T>::setIdentity() +{ + bias = {0, 0, 0}; +} + +template <typename T> +T BiasCorrector<T>::correct(const T& data) const +{ + T output; + Eigen::Vector3f tmp; + + data >> tmp; + tmp += bias; + output << tmp; + + return output; +} + +template <typename T> +BiasCalibration<T>::BiasCalibration() + : sum(0, 0, 0), ref(0, 0, 0), numSamples(0) +{ +} + +template <typename T> +void BiasCalibration<T>::setReferenceVector(Eigen::Vector3f vec) +{ + ref = vec; +} + +template <typename T> +Eigen::Vector3f BiasCalibration<T>::getReferenceVector() +{ + return ref; +} + +/** + * BiasCalibration accepts an indefinite number of samples, + * so feed(...) never returns false. + */ +template <typename T> +bool BiasCalibration<T>::feed(const T& measured, + const AxisOrientation& transform) +{ + Eigen::Vector3f vec; + measured >> vec; + + sum += (transform.getMatrix().transpose() * ref) - vec; + numSamples++; + + return true; +} + +template <typename T> +bool BiasCalibration<T>::feed(const T& measured) +{ + return feed(measured, AxisOrthoOrientation()); +} + +template <typename T> +BiasCorrector<T> BiasCalibration<T>::computeResult() +{ + if (numSamples == 0) + return {Eigen::Vector3f{0, 0, 0}}; + return {sum / numSamples}; +} + } // namespace Boardcore diff --git a/src/shared/sensors/calibration/Calibration.h b/src/shared/sensors/calibration/Calibration.h index 8fa838df73d4de676d18c9256e4f9d86bb21caec..64673246b4ef425db93d3cddd2219d48533ee8da 100644 --- a/src/shared/sensors/calibration/Calibration.h +++ b/src/shared/sensors/calibration/Calibration.h @@ -1,5 +1,5 @@ -/* Copyright (c) 2021 Skyward Experimental Rocketry - * Author: Riccardo Musso +/* Copyright (c) 2021-2022 Skyward Experimental Rocketry + * Authors: Riccardo Musso, Alberto Nidasio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -22,315 +22,67 @@ #pragma once -#include <sensors/Sensor.h> - #include <Eigen/Core> -#include <Eigen/Geometry> namespace Boardcore { /** - * This class is used to adjust the values given by sensors during the flight. - * An object can be obtained only via deserialization or if produced by an - * instance of the "CalibrationModel" class. - * - * T is the type of sensor data the model is applied to (e.g. - * GyroscopeData, MagnetometerData). This is needed because certain models could - * work slightly differently depending on it - * - * Note: derived classes of ValuesCorrector<XX> MUST implement the following - * operators (to make possible to store and load the coefficients): + * @brief This class is used to adjust the values given by a sensor. * - * operator << (const XX& t); - * operator >> (XX& t); - * - * where XX is a datatype that can fully contain all the coefficients used - * by the function correct(input). - * - * Also, an empty constructor must create a neutral instance (identity - * transformation). + * @tparam T Type of sensor data the model is applied to (e.g. + * GyroscopeData, MagnetometerData). */ template <typename T> class ValuesCorrector { public: /** - * This method will sets the internal coefficients so that the corrected + * @brief Sets the internal coefficients so that the corrected * values are exactly the same of the inputted ones. */ virtual void setIdentity() = 0; + /** + * @brief Applies the correction to the given input. + */ virtual T correct(const T& input) const = 0; }; /** - * AbstractCalibrationModel represents a "factory" of ValuesCorrector instances, - * and it's necessary to create one. You will always use one of its derived - * classes, of course. + * @brief Utility to compute calibration parameters for a specific sensor. * - * Values given to the feed() function are needed for the training of the model. + * The calibration appens in two phases: + * - Sample collection: feed is called to store values; + * - Coefficient computation: computeResult is used to calculate the correction + * parameters. * - * T is the sensor data type - * C is a class that extends ValuesCorrector<T> + * @tparam T Sensor's data type used in the feed function. + * @tparam C ValuesCorrector returned by computeResult. */ template <typename T, typename C, typename... AdditionalFeedParams> -class AbstractCalibrationModel +class AbstractCalibration { public: /** - * Gives to the model a single measurement to store and use to produce the - * adapted ValueCorrector instance + * @brief Stores the measurement for further processing. * * @returns false if the model can't accept the given data (usually because - * the internal buffers are full) + * the internal buffers are full). */ virtual bool feed(const T& measurement, const AdditionalFeedParams&... params) = 0; /** - * Creates the best ValuesCorrector instance for the given measurements. - * Note: you must feed some data to the model before getting the result! + * @brief Uses the recorded measurements to computer the correction + * parameters needed to correct sensor's data. + * + * Note: You need to feed the algorithm enough samples. Otherwise the method + * returns an ineffective corrector. + * + * @return ValuesCorrector containing the correction parameters. */ virtual C computeResult() = 0; - - virtual ~AbstractCalibrationModel(){}; -}; - -/** - * This class acts like a Sensor driver but incorporates both a Sensor<T> - * instance and a ValuesCorrector. It can be useful to add a calibration step to - * alredy existing code that uses the Sensor API. - */ -template <typename SensorData> -class SensorWrapper : public Sensor<SensorData> -{ - using S = Sensor<SensorData>; - using C = ValuesCorrector<SensorData>; - -public: - SensorWrapper(S* _sensor, C* _calib) : sensor(_sensor), calib(_calib) {} - - S* getSensor() { return sensor; } - void setSensor(S* s) { sensor = s; } - - C* getValuesCorrector() { return calib; } - void setValuesCorrector(C* c) { calib = c; } - - void init() override { sensor->init(); } - - bool test() override { return sensor->test(); } - - SensorData sampleImpl() override - { - return calib->correct(sensor->sampleImpl()); - } - - SensorErrors getWrappedSensorError() { return sensor->getLastError(); } - -private: - S* sensor; - C* calib; -}; - -/** - * This enum act like versors towards the chosen axis. - * - * X, Y and Z always set according to the right hand rule, so that: - * X is the index - * Y is the second finger - * Z is the thumb - * - */ -enum class Direction : uint8_t -{ - POSITIVE_X = 0, - NEGATIVE_X, - POSITIVE_Y, - NEGATIVE_Y, - POSITIVE_Z, - NEGATIVE_Z, -}; - -constexpr const char* humanFriendlyDirection[]{ - "North", "South", "East", "West", "Down", "Up", -}; - -inline Eigen::Vector3f orientationToVector(Direction val) -{ - switch (val) - { - case Direction::POSITIVE_X: - return {1, 0, 0}; - case Direction::NEGATIVE_X: - return {-1, 0, 0}; - case Direction::POSITIVE_Y: - return {0, 1, 0}; - case Direction::NEGATIVE_Y: - return {0, -1, 0}; - case Direction::POSITIVE_Z: - return {0, 0, 1}; - case Direction::NEGATIVE_Z: - return {0, 0, -1}; - default: - /* never happens, added just to shut up the warnings */ - return {0, 0, 0}; - } -} - -/** - * This struct represents in the most general way any kind of transformation of - * the reference frame (axis X, Y and Z). - * This data type is intended to simplify the code, so you shouldn't instantiate - * this struct directly, but rather use the structures AxisAngleOrientation or - * AxisOrthoOrientation that will be automatically corrected to this one, thanks - * to the implicit cast in favor of AxisOrientation. - * - * For example: - * - * AxisAngleOrientation angles ( PI/2, PI, 0); - * AxisOrthoOrientation ortho ( Direction::NEGATIVE_X, - * Direction::POSITIVE_Z ); - * - * // The implicit cast is supported and recommended - * AxisOrientation converted1 = angles; - * AxisOrientation converted2 = ortho; - * - * // Now we can use the generated matrix: - * Eigen::Vector3f zeta = convertedX.getMatrix() * Eigen::Vector3f { 0, 0, 1 } - * - */ -struct AxisOrientation -{ - Eigen::Matrix3f mat; - - AxisOrientation() : mat(Eigen::Matrix3f::Identity()) {} - - AxisOrientation(Eigen::Matrix3f _mat) : mat(_mat) {} - - void setMatrix(Eigen::Matrix3f _mat) { mat = _mat; } - - Eigen::Matrix3f getMatrix() const { return mat; } -}; - -/** - * This struct uses the three angles yaw, pitch and roll to define the - * transformation of the reference frame, so according to N.E.D standard we get: - * - * X (north) - * / - * / - * .----> Y (east) - * | - * | - * v - * Z (down) - * - * Where: - * Yaw is rotation of Z axis - * Pitch is rotation of Y axis - * Roll is rotation of X axis - */ -struct AxisAngleOrientation -{ - float yaw, pitch, roll; - - AxisAngleOrientation() : yaw(0), pitch(0), roll(0) {} - - AxisAngleOrientation(float _yaw, float _pitch, float _roll) - : yaw(_yaw), pitch(_pitch), roll(_roll) - { - } - - operator AxisOrientation() const { return AxisOrientation(getMatrix()); } - - Eigen::Matrix3f getMatrix() const - { - return (Eigen::AngleAxisf(yaw, Eigen::Vector3f{0, 0, 1}) * - Eigen::AngleAxisf(pitch, Eigen::Vector3f{0, 1, 0}) * - Eigen::AngleAxisf(roll, Eigen::Vector3f{1, 0, 0})) - .toRotationMatrix(); - } -}; - -/** - * This struct represents the orientation of the reference frame relative - * to X, Y, Z in the start orientation. - * If we know the orientation of the X and Y axis, using the right hand rule - * we can infer the Z axis. - * - * For example, if the base reference is: - * - * z - * ^ - * | - * | - * /----> y - * / - * x - * - * Then if we set x = NEGATIVE_Y, y = POSITIVE_Z, we get: - * - * y z - * ^ ^ - * | / - * | / - * x <----/ - * - */ -struct AxisOrthoOrientation -{ - Direction xAxis, yAxis; - - AxisOrthoOrientation() - : xAxis(Direction::POSITIVE_X), yAxis(Direction::POSITIVE_Y) - { - } - - AxisOrthoOrientation(Direction _xAxis, Direction _yAxis) - : xAxis(_xAxis), yAxis(_yAxis) - { - } - - operator AxisOrientation() const { return AxisOrientation(getMatrix()); } - - Eigen::Matrix3f getMatrix() const - { - Eigen::Vector3f vx, vy, vz; - - vx = orientationToVector(xAxis); - vy = orientationToVector(yAxis); - vz = vx.cross(vy); - - Eigen::Matrix3f mat; - mat.col(0) << vx; - mat.col(1) << vy; - mat.col(2) << vz; - return mat; - } -}; - -/** - * This struct represents axis orientation relative to a reference system. - * Operatively it simply combines two transformations. It is particularly useful - * to obtain an AxisOrientation from a reference system generally not N.E.D. - */ -struct AxisRelativeOrientation -{ - AxisOrientation systemOrientation, orientation; - - AxisRelativeOrientation(const AxisOrientation& _systemOrientation, - const AxisOrientation& _orientation) - : systemOrientation(_systemOrientation), orientation(_orientation) - { - } - - operator AxisOrientation() const { return {getMatrix()}; } - - Eigen::Matrix3f getMatrix() const - { - return systemOrientation.getMatrix() * orientation.getMatrix(); - } }; } // namespace Boardcore diff --git a/src/shared/sensors/calibration/HardIronCalibration.h b/src/shared/sensors/calibration/HardIronCalibration.h index 720f9a7edaae543870665af1e92f54ce9a4484ca..2cd616ac58379671d3958859f3eea3d12585ff64 100644 --- a/src/shared/sensors/calibration/HardIronCalibration.h +++ b/src/shared/sensors/calibration/HardIronCalibration.h @@ -68,7 +68,7 @@ private: template <unsigned MaxSamples> class HardIronCalibration - : public AbstractCalibrationModel<MagnetometerData, HardIronCorrector> + : public AbstractCalibration<MagnetometerData, HardIronCorrector> { public: HardIronCalibration() : samples(), numSamples(0) {} diff --git a/src/shared/sensors/calibration/SensorDataExtra.cpp b/src/shared/sensors/calibration/SensorDataExtra.cpp index f8ffd7ea87afd7c786ca4b59b98bf88453ad62c9..ed69028b1d522410bb381d021002bcd39bc0739c 100644 --- a/src/shared/sensors/calibration/SensorDataExtra.cpp +++ b/src/shared/sensors/calibration/SensorDataExtra.cpp @@ -34,6 +34,13 @@ void operator<<(AccelerometerData& lhs, const Vector3f& rhs) lhs.accelerationZ = rhs[2]; } +void operator<<(Eigen::Vector3f& lhs, const AccelerometerData& rhs) +{ + lhs[0] = rhs.accelerationX; + lhs[1] = rhs.accelerationY; + lhs[2] = rhs.accelerationZ; +} + void operator<<(GyroscopeData& lhs, const Vector3f& rhs) { lhs.angularVelocityX = rhs[0]; @@ -41,6 +48,13 @@ void operator<<(GyroscopeData& lhs, const Vector3f& rhs) lhs.angularVelocityZ = rhs[2]; } +void operator<<(Eigen::Vector3f& lhs, const GyroscopeData& rhs) +{ + lhs[0] = rhs.angularVelocityX; + lhs[1] = rhs.angularVelocityY; + lhs[2] = rhs.angularVelocityZ; +} + void operator<<(MagnetometerData& lhs, const Vector3f& rhs) { lhs.magneticFieldX = rhs[0]; @@ -48,25 +62,35 @@ void operator<<(MagnetometerData& lhs, const Vector3f& rhs) lhs.magneticFieldZ = rhs[2]; } -void operator>>(const AccelerometerData& lhs, Vector3f& rhs) +void operator<<(Eigen::Vector3f& lhs, const MagnetometerData& rhs) +{ + lhs[0] = rhs.magneticFieldX; + lhs[1] = rhs.magneticFieldY; + lhs[2] = rhs.magneticFieldZ; +} + +void operator>>(const AccelerometerData& lhs, Eigen::Vector3f& rhs) { - rhs[0] = lhs.accelerationX; - rhs[1] = lhs.accelerationY; - rhs[2] = lhs.accelerationZ; + rhs << lhs; } -void operator>>(const GyroscopeData& lhs, Vector3f& rhs) +void operator>>(const Eigen::Vector3f& lhs, AccelerometerData& rhs) +{ + rhs << lhs; +} + +void operator>>(const GyroscopeData& lhs, Eigen::Vector3f& rhs) { rhs << lhs; } + +void operator>>(const Eigen::Vector3f& lhs, GyroscopeData& rhs) { rhs << lhs; } + +void operator>>(const MagnetometerData& lhs, Eigen::Vector3f& rhs) { - rhs[0] = lhs.angularVelocityX; - rhs[1] = lhs.angularVelocityY; - rhs[2] = lhs.angularVelocityZ; + rhs << lhs; } -void operator>>(const MagnetometerData& lhs, Vector3f& rhs) +void operator>>(const Eigen::Vector3f& lhs, MagnetometerData& rhs) { - rhs[0] = lhs.magneticFieldX; - rhs[1] = lhs.magneticFieldY; - rhs[2] = lhs.magneticFieldZ; + rhs << lhs; } } // namespace Boardcore diff --git a/src/shared/sensors/calibration/SensorDataExtra.h b/src/shared/sensors/calibration/SensorDataExtra.h index 0885a3a5ead12f3eb67c718451031f4388a882f8..cc52c472f14c904ff56913c3bb7d8e54ef8f6bae 100644 --- a/src/shared/sensors/calibration/SensorDataExtra.h +++ b/src/shared/sensors/calibration/SensorDataExtra.h @@ -46,16 +46,26 @@ namespace Boardcore void operator<<(AccelerometerData& lhs, const Eigen::Vector3f& rhs); +void operator<<(Eigen::Vector3f& lhs, const AccelerometerData& rhs); + void operator<<(GyroscopeData& lhs, const Eigen::Vector3f& rhs); +void operator<<(Eigen::Vector3f& lhs, const GyroscopeData& rhs); + void operator<<(MagnetometerData& lhs, const Eigen::Vector3f& rhs); +void operator<<(Eigen::Vector3f& lhs, const MagnetometerData& rhs); + void operator>>(const AccelerometerData& lhs, Eigen::Vector3f& rhs); +void operator>>(const Eigen::Vector3f& lhs, AccelerometerData& rhs); + void operator>>(const GyroscopeData& lhs, Eigen::Vector3f& rhs); -void operator>>(const MagnetometerData& lhs, Eigen::Vector3f& rhs); +void operator>>(const Eigen::Vector3f& lhs, GyroscopeData& rhs); void operator>>(const MagnetometerData& lhs, Eigen::Vector3f& rhs); +void operator>>(const Eigen::Vector3f& lhs, MagnetometerData& rhs); + } // namespace Boardcore diff --git a/src/shared/sensors/calibration/SixParameterCalibration.h b/src/shared/sensors/calibration/SixParameterCalibration.h index b080020861fb33ac622ad94047d82b4d71ba859d..8296a1ac974718cc02c0b174d1cd2c4f56a1482f 100644 --- a/src/shared/sensors/calibration/SixParameterCalibration.h +++ b/src/shared/sensors/calibration/SixParameterCalibration.h @@ -89,8 +89,7 @@ private: template <typename T, unsigned MaxSamples> class SixParameterCalibration - : public AbstractCalibrationModel<T, SixParameterCorrector<T>, - AxisOrientation> + : public AbstractCalibration<T, SixParameterCorrector<T>, AxisOrientation> { public: SixParameterCalibration() : samples(), ref(1, 0, 0), numSamples(0) {} diff --git a/src/shared/sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.cpp b/src/shared/sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.cpp new file mode 100644 index 0000000000000000000000000000000000000000..c4d49d703425c8ec7d84fb1052e135b0b655ae97 --- /dev/null +++ b/src/shared/sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.cpp @@ -0,0 +1,159 @@ +/* Copyright (c) 2021-2022 Skyward Experimental Rocketry + * Authors: Riccardo Musso, Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include "SoftAndHardIronCalibration.h" + +#include <sensors/calibration/SensorDataExtra.h> + +#include <iostream> + +using namespace Eigen; + +namespace Boardcore +{ + +SoftAndHardIronCorrector::SoftAndHardIronCorrector() + : SoftAndHardIronCorrector({0, 0, 0}, {1, 1, 1}) +{ +} + +SoftAndHardIronCorrector::SoftAndHardIronCorrector(const Vector3f& offset, + const Vector3f& gain) + : offset(offset), gain(gain) +{ +} + +void SoftAndHardIronCorrector::setIdentity() +{ + offset = {0, 0, 0}; + gain = {1, 1, 1}; +} + +MagnetometerData SoftAndHardIronCorrector::correct( + const MagnetometerData& sample) const +{ + MagnetometerData output; + Vector3f tmp; + + tmp << sample; + tmp = (tmp + offset).cwiseProduct(gain); + output << tmp; + + return output; +} + +Eigen::Vector3f SoftAndHardIronCorrector::getOffset() const { return offset; } + +void SoftAndHardIronCorrector::setOffset(const Eigen::Vector3f& offset) +{ + this->offset = offset; +} + +Eigen::Vector3f SoftAndHardIronCorrector::getGain() const { return gain; } + +void SoftAndHardIronCorrector::setGain(const Eigen::Vector3f& gain) +{ + this->gain = gain; +} + +SoftAndHardIronCalibration::SoftAndHardIronCalibration() {} + +bool SoftAndHardIronCalibration::feed(const MagnetometerData& data) +{ + // Let S a matrix of Nx7 composed as [x^2, y^2, z^2, x, y, z, 1] + // D need to be S^T * S + // To avoid storing all measurements we just need to incrementally add to D + + Vector3f vector; + vector << data; + Vector<float, 7> S; + // cppcheck-suppress constStatement + S << vector.cwiseProduct(vector), vector, 1; + + for (int i = 0; i < 7; i++) + for (int j = 0; j < 7; j++) + D(i, j) += S(i) * S(j); + + return true; +} + +SoftAndHardIronCorrector SoftAndHardIronCalibration::computeResult() +{ + // Compute eigen value and vectors of D + SelfAdjointEigenSolver<Matrix<float, 7, 7>> solver(D); + auto eigenValues = solver.eigenvalues(); + + // Find the smallest eigen value and vector + float minValue = eigenValues[0]; + int minIdx = 0; + + for (int i = 0; i < eigenValues.rows(); i++) + if (minValue > eigenValues[i]) + { + minValue = eigenValues[i]; + minIdx = i; + } + Eigen::Matrix<float, 7, 1> vec = solver.eigenvectors().col(minIdx); + + // Invert the vector if necessary + float det = vec[0] * vec[1] * vec[2]; + if (det) + { + vec *= -1; + det *= -1; + } + + // Compute offset and gain + Vector3f offset{vec[3] / vec[0] / 2, vec[4] / vec[1] / 2, + vec[5] / vec[2] / 2}; + Vector3f gain = (vec.block(0, 0, 3, 1) / cbrt(det)).cwiseSqrt(); + + return {offset, gain}; +} + +void operator<<(Eigen::Matrix<float, 3, 2>& lhs, + const SoftAndHardIronCorrector& rhs) +{ + lhs.col(0) = rhs.getOffset(); + lhs.col(0) = rhs.getGain(); +} + +void operator<<(SoftAndHardIronCorrector& lhs, + const Eigen::Matrix<float, 3, 2>& rhs) +{ + lhs.setOffset(rhs.col(0)); + lhs.setGain(rhs.col(1)); +} + +void operator>>(const Eigen::Matrix<float, 3, 2>& lhs, + SoftAndHardIronCorrector& rhs) +{ + rhs << lhs; +} + +void operator>>(const SoftAndHardIronCorrector& lhs, + Eigen::Matrix<float, 3, 2>& rhs) +{ + rhs << lhs; +} + +} // namespace Boardcore diff --git a/src/shared/sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h b/src/shared/sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h new file mode 100644 index 0000000000000000000000000000000000000000..61b36c35fdcb91117a604a5f4cecc278ee5794f4 --- /dev/null +++ b/src/shared/sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h @@ -0,0 +1,108 @@ +/* Copyright (c) 2021-2022 Skyward Experimental Rocketry + * Authors: Riccardo Musso, Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ +#pragma once + +#include <sensors/SensorData.h> +#include <sensors/calibration/Calibration.h> + +#include <Eigen/Core> +#include <Eigen/Eigenvalues> +#include <vector> + +namespace Boardcore +{ + +/* + * The Soft-iron calibration removes the measurement error given by both the + * Hard and Soft Iron distortion of the magnetic field. + */ +class SoftAndHardIronCorrector : public ValuesCorrector<MagnetometerData> +{ +public: + SoftAndHardIronCorrector(); + + SoftAndHardIronCorrector(const Eigen::Vector3f& offset, + const Eigen::Vector3f& gain); + + void setIdentity() override; + + MagnetometerData correct(const MagnetometerData& sample) const override; + + Eigen::Vector3f getOffset() const; + + void setOffset(const Eigen::Vector3f& offset); + + Eigen::Vector3f getGain() const; + + void setGain(const Eigen::Vector3f& gain); + +private: + Eigen::Vector3f offset; + Eigen::Vector3f gain; +}; + +/** + * @brief Soft and hard iron calibration utility. + * + * Fits a non-rotated ellipsoid to the calibration data and then derives the + * correction parameters. + * + * Reference: + * https://www.st.com/resource/en/design_tip/dt0059-ellipsoid-or-sphere-fitting-for-sensor-calibration-stmicroelectronics.pdf + * + * @tparam MaxSamples + */ +class SoftAndHardIronCalibration + : public AbstractCalibration<MagnetometerData, SoftAndHardIronCorrector> +{ +public: + SoftAndHardIronCalibration(); + + bool feed(const MagnetometerData& data) override; + + /** + * @brief Uses the recorded measurements to compute the correction + * parameters needed to correct sensor's data. + * + * Note: Feed at leas 9 measurements! + * + * @return SoftAndHardIronCorrector containing the correction parameters. + */ + SoftAndHardIronCorrector computeResult() override; + +private: + Eigen::Matrix<float, 7, 7> D = Eigen::Matrix<float, 7, 7>::Zero(); +}; + +void operator<<(Eigen::Matrix<float, 3, 2>& lhs, + const SoftAndHardIronCorrector& rhs); + +void operator<<(SoftAndHardIronCorrector& lhs, + const Eigen::Matrix<float, 3, 2>& rhs); + +void operator>>(const Eigen::Matrix<float, 3, 2>& lhs, + SoftAndHardIronCorrector& rhs); + +void operator>>(const SoftAndHardIronCorrector& lhs, + Eigen::Matrix<float, 3, 2>& rhs); + +} // namespace Boardcore diff --git a/src/shared/sensors/calibration/SoftIronCalibration.h b/src/shared/sensors/calibration/SoftIronCalibration.h deleted file mode 100644 index 3b6dba4a9e4dc761798c71343191c7f68b994843..0000000000000000000000000000000000000000 --- a/src/shared/sensors/calibration/SoftIronCalibration.h +++ /dev/null @@ -1,194 +0,0 @@ -/* Copyright (c) 2021 Skyward Experimental Rocketry - * Author: Riccardo Musso - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#pragma once - -#include <sensors/SensorData.h> - -#include <Eigen/Core> -#include <Eigen/Eigenvalues> - -#include "Calibration.h" - -namespace Boardcore -{ - -/* - * The Soft-iron calibration removes the measurement error given by both the - * Hard and Soft Iron distortion of the magnetic field. - */ -class SoftIronCorrector : public ValuesCorrector<MagnetometerData> -{ -public: - SoftIronCorrector() : SoftIronCorrector({1, 1, 1}, {0, 0, 0}) {} - - SoftIronCorrector(const Eigen::Vector3f& _p, const Eigen::Vector3f& _q) - : p(_p), q(_q) - { - } - - void setIdentity() override - { - p = {1, 1, 1}; - q = {0, 0, 0}; - } - - void operator>>(Eigen::Matrix<float, 3, 2>& rhs) - { - rhs.col(0) = p.transpose(); - rhs.col(1) = q.transpose(); - } - - void operator<<(const Eigen::Matrix<float, 3, 2>& rhs) - { - p = rhs.col(0).transpose(); - q = rhs.col(1).transpose(); - } - - MagnetometerData correct(const MagnetometerData& input) const override - { - MagnetometerData output; - Eigen::Vector3f vec; - - input >> vec; - vec = vec.cwiseProduct(p) + q; - output << vec; - - return output; - } - -private: - Eigen::Vector3f p, q; -}; - -template <unsigned MaxSamples> -class SoftIronCalibration - : public AbstractCalibrationModel<MagnetometerData, SoftIronCorrector> -{ -public: - SoftIronCalibration() : samples(), numSamples(0) {} - - bool feed(const MagnetometerData& data) override - { - if (numSamples >= MaxSamples) - return false; - - Eigen::Vector3f vec; - data >> vec; - - for (int i = 0; i < 3; ++i) - { - samples(numSamples, i) = vec[i] * vec[i]; - samples(numSamples, i + 3) = vec[i]; - } - samples(numSamples, 6) = 1; - - numSamples++; - return true; - } - - SoftIronCorrector computeResult() override - { - using Mx = Eigen::Matrix<float, 7, 7>; - using Vec7 = Eigen::Matrix<float, 7, 1>; - - float minValue, det; - int minIdx; - - Eigen::MatrixXf tmp1, tmp2; - Eigen::Vector3f p, q; - Vec7 vec; - Mx mat; - - tmp1 = samples.block(0, 0, numSamples, 7); - tmp2 = tmp1.transpose(); - - // mat = tmp2 * tmp1; - - /* - * tmp1: N x 7 - * tmp2: 7 x N - * - * Big matrices multiplication: if done with Eigen, the program crashes - * (bus fault), so we'll do that with good old for's - */ - for (unsigned i = 0; i < 7; ++i) - { - for (unsigned j = 0; j < 7; ++j) - { - mat(i, j) = 0; - - for (unsigned k = 0; k < numSamples; ++k) - { - mat(i, j) += tmp2(i, k) * tmp1(k, j); - } - } - } - - Eigen::SelfAdjointEigenSolver<Mx> solver(mat); - auto eigenvalues = solver.eigenvalues(); - - minValue = eigenvalues[0]; - minIdx = 0; - - for (int i = 1; i < eigenvalues.rows(); ++i) - { - if (minValue > eigenvalues[i]) - { - minValue = eigenvalues[i]; - minIdx = i; - } - } - - vec = solver.eigenvectors().col(minIdx); - det = vec[0] * vec[1] * vec[2]; - - if (det < 0) - { - vec *= -1; - det *= -1; - } - - p = vec.block(0, 0, 3, 1) / cbrt(det); - q = {vec[3] / vec[0] / 2, vec[4] / vec[1] / 2, vec[5] / vec[2] / 2}; - - p[0] = sqrt(p[0]); - p[1] = sqrt(p[1]); - p[2] = sqrt(p[2]); - - q = q.cwiseProduct(p); - - return {p, q}; - } - - Eigen::Matrix<float, MaxSamples, 7> getSamples() { return samples; } - -private: - /* - * The matrix contains x, y, z, x^2, y^2, z^2 and a column of 1s - * row. Its shape is (N x 7) - */ - Eigen::Matrix<float, MaxSamples, 7> samples; - unsigned numSamples; -}; - -} // namespace Boardcore diff --git a/src/shared/sensors/calibration/TwelveParameterCalibration.h b/src/shared/sensors/calibration/TwelveParameterCalibration.h index 061fbbb25d61f7e76a4f1091a32f17aa00f10b9a..832eac3d4466512f365aeee270d0946915f7d184 100644 --- a/src/shared/sensors/calibration/TwelveParameterCalibration.h +++ b/src/shared/sensors/calibration/TwelveParameterCalibration.h @@ -97,8 +97,8 @@ private: template <typename T, unsigned MaxSamples> class TwelveParameterCalibration - : public AbstractCalibrationModel<T, TwelveParameterCorrector<T>, - AxisOrientation> + : public AbstractCalibration<T, TwelveParameterCorrector<T>, + AxisOrientation> { public: TwelveParameterCalibration() : samples(), numSamples(0), ref({1, 0, 0}) {} diff --git a/src/shared/utils/aero/AeroUtils.cpp b/src/shared/utils/AeroUtils/AeroUtils.cpp similarity index 76% rename from src/shared/utils/aero/AeroUtils.cpp rename to src/shared/utils/AeroUtils/AeroUtils.cpp index 434236c180f0f7fe4bdfd2a55330d5ecab9887c9..c984cdfc247550036c4d88eef544fc4cf4d68663 100644 --- a/src/shared/utils/aero/AeroUtils.cpp +++ b/src/shared/utils/AeroUtils/AeroUtils.cpp @@ -22,15 +22,19 @@ #include "AeroUtils.h" +#include <utils/Constants.h> + +using namespace Eigen; + namespace Boardcore { -namespace aeroutils +namespace Aeroutils { float relAltitude(float pressure, float pressureRef, float temperatureRef) { - using namespace constants; + using namespace Constants; return temperatureRef / a * (1 - powf(pressure / pressureRef, nInv)); } @@ -38,7 +42,7 @@ float relAltitude(float pressure, float pressureRef, float temperatureRef) float relDensity(float pressure, float pressureRef, float altitudeRef, float temperatureRef) { - using namespace constants; + using namespace Constants; return pressure / (R * a * altitudeRef + R * temperatureRef * powf(pressure / pressureRef, nInv)); @@ -46,7 +50,7 @@ float relDensity(float pressure, float pressureRef, float altitudeRef, float mslPressure(float pressureRef, float temperatureRef, float altitudeRef) { - using namespace constants; + using namespace Constants; float T0 = mslTemperature(temperatureRef, altitudeRef); return pressureRef / powf(1 - a * altitudeRef / T0, n); @@ -54,16 +58,28 @@ float mslPressure(float pressureRef, float temperatureRef, float altitudeRef) float mslTemperature(float temperatureRef, float altitudeRef) { - return temperatureRef + (altitudeRef * constants::a); + return temperatureRef + (altitudeRef * Constants::a); } float verticalSpeed(float p, float dpDt, float pRef, float tRef) { - using namespace constants; + using namespace Constants; return -(tRef * dpDt * powf(p / pRef, nInv)) / (a * n * p); } -} // namespace aeroutils +Vector2f geodetic2NED(const Vector2f& gpsData, const Vector2f& offset) +{ + float mPerDegLat = 111132.95225; + float mPerDegLon = + fabsf(111412.87733 * cosf(gpsData[0] * Constants::DEGREES_TO_RADIANS)); + + return { + mPerDegLat * (gpsData[0] - offset[0]), + mPerDegLon * (gpsData[1] - offset[1]), + }; +} + +} // namespace Aeroutils } // namespace Boardcore diff --git a/src/shared/utils/aero/AeroUtils.h b/src/shared/utils/AeroUtils/AeroUtils.h similarity index 73% rename from src/shared/utils/aero/AeroUtils.h rename to src/shared/utils/AeroUtils/AeroUtils.h index 24f0d3edde0b4cf1a22a78410941decde2398e7a..80abb8dac650fc6054e359b34e65442bf1402d41 100644 --- a/src/shared/utils/aero/AeroUtils.h +++ b/src/shared/utils/AeroUtils/AeroUtils.h @@ -22,34 +22,18 @@ #pragma once +#include <Eigen/Core> #include <cmath> namespace Boardcore { -namespace aeroutils +namespace Aeroutils { -namespace constants -{ - -// Troposphere temperature gradient [deg/m] -constexpr float a = 0.0065f; - -// Acceleration of gravity [m^s^2] -constexpr float g = 9.80665f; - -// Air gas constant [J/(Kg*K])] -constexpr float R = 287.05f; - -constexpr float n = g / (R * a); -constexpr float nInv = (R * a) / g; - -} // namespace constants - /** * Returns the current altitude with respect to a reference altitude for the - * given pressure, using International Standard Atmosphere model. + * given pressure, using International Standard Atmosphere model. * * @warning This function is valid for altitudes below 11000 meters above sea * level @@ -57,10 +41,10 @@ constexpr float nInv = (R * a) / g; * altitude. It does not provide altitude above mean sea level unless the * reference is, in fact, the sea level. * - * @param pressure Current pressure [Pascal] - * @param pressureRef Pressure at reference altitude (must be > 0) [Pascal] - * @param temperatureRef Temperature at reference altitude [Kelvin] - * @return Current altitude with respect to the reference altitude [meters] + * @param pressure Current pressure [Pas] + * @param pressureRef Pressure at reference altitude (must be > 0) [Pa] + * @param temperatureRef Temperature at reference altitude [K] + * @return Current altitude with respect to the reference altitude [m] */ float relAltitude(float pressure, float pressureRef, float temperatureRef); @@ -68,10 +52,10 @@ float relAltitude(float pressure, float pressureRef, float temperatureRef); * Returns the current air density with respect to a reference density and * temperature, using the Internation Standard Atmosphere model. * - * @param pressure Current atmospheric pressure [Pascal] - * @param pressureRef Pressure at reference altitude (must be > 0) [Pascal] + * @param pressure Current atmospheric pressure [Pa] + * @param pressureRef Pressure at reference altitude (must be > 0) [Pa] * @param altitudeRef Reference altitude [m] - * @param temperatureRef Temperature at reference altitude [Kelvin] + * @param temperatureRef Temperature at reference altitude [K] * @return Current air density [Kg/m^3] */ float relDensity(float pressure, float pressureRef, float altitudeRef, @@ -85,11 +69,10 @@ float relDensity(float pressure, float pressureRef, float altitudeRef, * @warning This function is valid for altitudes below 11000 meters above sea * level * - * @param pressureRef Pressure at reference altitude [Pascal] - * @param temperatureRef Temperature at reference altitude. Must be > 0 - * [Kelvin] - * @param altitudeRef Reference altitude [meters] - * @return Pressure at mean sea level [pascal] + * @param pressureRef Pressure at reference altitude [Pa] + * @param temperatureRef Temperature at reference altitude. Must be > 0 [K] + * @param altitudeRef Reference altitude [m] + * @return Pressure at mean sea level [Pa] */ float mslPressure(float pressureRef, float temperatureRef, float altitudeRef); @@ -100,9 +83,9 @@ float mslPressure(float pressureRef, float temperatureRef, float altitudeRef); * @warning This function is valid for altitudes below 11000 meters above sea * level * - * @param temperatureRef Temperature at reference altitude [Kelvin] - * @param altitudeRef Reference altitude [meters] - * @return Temperature at mean sea level [Kelvin] + * @param temperatureRef Temperature at reference altitude [K] + * @param altitudeRef Reference altitude [m] + * @return Temperature at mean sea level [K] */ float mslTemperature(float temperatureRef, float altitudeRef); @@ -114,13 +97,24 @@ float mslTemperature(float temperatureRef, float altitudeRef); * level * * @param p Current pressure (must be > 0) [Pa] - * @param dpDt [Rate of change of pressure [Pa/s]] + * @param dpDt Rate of change of pressure [Pa/s] * @param pRef Reference pressure (must be > 0) [Pa] * @param tRef Reference temperature [K] * @return Vertical speed, positive upwards [m/s] */ float verticalSpeed(float p, float dpDt, float pRef, float tRef); -} // namespace aeroutils +/** + * @brief Converts decimal degrees of latitude and longitude into displacement + * in meters between two positions the with an ellipsoidal earth model. + * + * @param position1 Latitude and longitude of current position [lat lon][deg] + * @param position2 Initial position used as an offset [lat lon][deg] + * @return Distance between the two coordinates [n e][m] + */ +Eigen::Vector2f geodetic2NED(const Eigen::Vector2f& position1, + const Eigen::Vector2f& position2); + +} // namespace Aeroutils } // namespace Boardcore diff --git a/src/shared/utils/ButtonHandler.h b/src/shared/utils/ButtonHandler.h deleted file mode 100644 index c803f4201d12e87b05a4e859364e397abfe4a3ed..0000000000000000000000000000000000000000 --- a/src/shared/utils/ButtonHandler.h +++ /dev/null @@ -1,129 +0,0 @@ -/* Copyright (c) 2015-2018 Skyward Experimental Rocketry - * Author: Luca Erbetta - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#pragma once - -#include <diagnostic/PrintLogger.h> - -#include <functional> - -#include "ActiveObject.h" - -using miosix::Thread; - -namespace Boardcore -{ - -static const int TICK_LENGTH = 100; // milliseconds -static const int LONG_PRESS_TICKS = 10; -static const int VERY_LONG_PRESS_TICKS = 50; - -enum class ButtonPress -{ - DOWN, // Called as soon as the button is pressed - UP, // Called when the button is released - SHORT, // Short press - LONG, // Long press - VERY_LONG // Very long press -}; - -/** - * @brief Class that detects if a button is pressed, long pressed or long-long - * pressed and calls a callback in each case - * - * @tparam Button GPIO of the button - */ -template <typename Button> -class ButtonHandler : public ActiveObject -{ -public: - using ButtonCallback = std::function<void(uint8_t, ButtonPress)>; - - static bool isPressed() { return Button::value(); } - - ButtonHandler(uint8_t btnId, ButtonCallback callback) - : btnId(btnId), callback(callback) - { - Button::mode(miosix::Mode::INPUT); - } - - ~ButtonHandler() {} - -protected: - void run() override - { - while (!shouldStop() && callback) - { - if (Button::value()) - { - if (!pressed && callback) - { - callback(btnId, ButtonPress::DOWN); - } - - pressedTicks++; - pressed = true; - } - else if (pressed) // if the button was unpressed since - // the last operation - { - if (pressedTicks >= VERY_LONG_PRESS_TICKS) - { - LOG_DEBUG(logger, "Button pressed (very long) (%d ticks)", - pressedTicks); - - callback(btnId, ButtonPress::VERY_LONG); - } - else if (pressedTicks >= LONG_PRESS_TICKS) - { - LOG_DEBUG(logger, "Button pressed (long) (%d ticks)", - pressedTicks); - callback(btnId, ButtonPress::LONG); - } - else - { - LOG_DEBUG(logger, "Button pressed (short) (%d ticks)", - pressedTicks); - callback(btnId, ButtonPress::SHORT); - } - - callback(btnId, ButtonPress::UP); - - pressed = false; - pressedTicks = 0; - } - - Thread::sleep(TICK_LENGTH); - } - } - -private: - uint8_t btnId = 0; - bool pressed = false; - int pressedTicks = 0; - - ButtonCallback callback; - - PrintLogger logger = Logging::getLogger("buttonhandler"); -}; - -} // namespace Boardcore diff --git a/src/shared/utils/ButtonHandler/ButtonHandler.cpp b/src/shared/utils/ButtonHandler/ButtonHandler.cpp new file mode 100644 index 0000000000000000000000000000000000000000..56a0e5df40a5f5e5b2d12d4c3c80e2f0bfb00774 --- /dev/null +++ b/src/shared/utils/ButtonHandler/ButtonHandler.cpp @@ -0,0 +1,100 @@ +/* Copyright (c) 2022 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include "ButtonHandler.h" + +#include <functional> + +namespace Boardcore +{ + +bool ButtonHandler::registerButtonCallback(miosix::GpioPin pin, + ButtonCallback callback) +{ + // Try to insert the callback + auto result = callbacks.insert({pin, {callback, false, 0}}); + + // Check if the insertion took place + if (result.second) + { + return scheduler.addTask( + std::bind(&ButtonHandler::periodicButtonValueCheck, this, pin), + SAMPLE_PERIOD, TaskScheduler::Policy::SKIP); + } + + return false; +} + +bool ButtonHandler::start() { return scheduler.start(); } + +void ButtonHandler::stop() { scheduler.stop(); } + +ButtonHandler::ButtonHandler() +{ + // Start the scheduler immediately + scheduler.start(); +} + +void ButtonHandler::periodicButtonValueCheck(miosix::GpioPin pin) +{ + // Make sure the pin informations are still present + if (callbacks.find(pin) == callbacks.end()) + return; + + // Retrieve the pin information + const ButtonCallback &callback = std::get<0>(callbacks[pin]); + bool &wasPressed = std::get<1>(callbacks[pin]); + int &pressedTicks = std::get<2>(callbacks[pin]); + + // Read the current button status + // Note: The button is assumed to be pressed if the pin value is low + // (pulldown) + bool isNowPressed = !pin.value(); + + if (isNowPressed) + { + // If the pin was not pressed before it has just been pressed + if (!wasPressed && callback) + callback(ButtonEvent::PRESSED); + + // Increment the tick + pressedTicks++; + } + // If the button was pressed before and just released + else if (wasPressed) + { + if (pressedTicks >= VERY_LONG_PRESS_TICKS) + callback(ButtonEvent::VERY_LONG_PRESS); + else if (pressedTicks >= LONG_PRESS_TICKS) + callback(ButtonEvent::LONG_PRESS); + else + callback(ButtonEvent::SHORT_PRESS); + + // Reset the ticks + pressedTicks = 0; + } + + // Save the current button status + wasPressed = isNowPressed; +} + +} // namespace Boardcore diff --git a/src/shared/utils/ButtonHandler/ButtonHandler.h b/src/shared/utils/ButtonHandler/ButtonHandler.h new file mode 100644 index 0000000000000000000000000000000000000000..2ff97a9beb9e4acb7e6e98c45ee88550fa3deddf --- /dev/null +++ b/src/shared/utils/ButtonHandler/ButtonHandler.h @@ -0,0 +1,140 @@ +/* Copyright (c) 2015-2022 Skyward Experimental Rocketry + * Authors: Luca Erbetta, Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <Singleton.h> +#include <miosix.h> +#include <scheduler/TaskScheduler.h> + +#include <map> + +namespace Boardcore +{ + +enum class ButtonEvent +{ + PRESSED, // Called as soon as the button is pressed + SHORT_PRESS, // Called as soon as the button is released + LONG_PRESS, // Called as soon as the button is released + VERY_LONG_PRESS // Called as soon as the button is released +}; + +/** + * @brief Comparison operator between GpioPins used for std::map. + */ +struct GpioPinCompare +{ + bool operator()(const miosix::GpioPin& lhs, + const miosix::GpioPin& rhs) const + { + if (lhs.getPort() == rhs.getPort()) + return lhs.getNumber() < rhs.getNumber(); + return lhs.getPort() < rhs.getPort(); + } +}; + +/** + * @brief Utility to detects if buttons are pressed, long pressed or long-long + * pressed and calls a callback in each case + * + * Note: The ButtonHandler assumes the all the buttons to be pulldown meaning + * that when the button is pressed, the pin is assumed low. + * + * TODO: Allow to set pullup or pulldown configuration for each registered pin. + */ +class ButtonHandler : public Singleton<ButtonHandler> +{ + friend Singleton<ButtonHandler>; + + static constexpr uint32_t SAMPLE_PERIOD = 100; // 10Hz + static constexpr int LONG_PRESS_TICKS = 10; // 1s + static constexpr int VERY_LONG_PRESS_TICKS = 50; // 5s + +public: + using ButtonCallback = std::function<void(ButtonEvent)>; + + /** + * @brief Registers a callback on the specified pin. + * + * @param pin Pin to listen to. + * @param callback Function to call on button events. + * @return False if another callback was already registered for the pin. + */ + bool registerButtonCallback(miosix::GpioPin pin, ButtonCallback callback); + + /** + * @brief Unregisters the callback associated with the specified pin, if + * any. + * + * @param pin Pin whose callback function is to be removed. + * @return True if a callback was present and removed for the given pin. + */ + bool unregisterButtonCallback(miosix::GpioPin pin); + + /** + * @brief Starts the ButtonHandler's task scheduler. + * + * Note that the scheduler is started as soon as the ButtonHandler is first + * used. + * + * @return Whether the task scheduler was started or not. + */ + bool start(); + + /** + * @brief Stops the ButtonHandler's task scheduler. + */ + void stop(); + +private: + ButtonHandler(); + + /** + * @brief This function is added to the scheduler for every pin registered + * in the ButtonHandler. + * + * @param pin Pin whose value need to be checked. + */ + void periodicButtonValueCheck(miosix::GpioPin pin); + + TaskScheduler scheduler; + + /** + * @brief Map of all the callbacks registered in the ButtonHandler. + * + * The key is the GpioPin for which the callback is registered. To used + * GpioPin as a map key, the GpioPinCompare operator was defined as + * explained here: + * https://stackoverflow.com/questions/1102392/how-can-i-use-stdmaps-with-user-defined-types-as-key + * + * The type stored is a tuple containing: + * - The button callback function; + * - Whether or not the button was pressed in the last check iteration; + * - The relative tick of the last pin value change. + */ + std::map<miosix::GpioPin, std::tuple<ButtonCallback, bool, int>, + GpioPinCompare> + callbacks; +}; + +} // namespace Boardcore diff --git a/src/shared/utils/CSVReader/CSVReader.h b/src/shared/utils/CSVReader/CSVReader.h index 32388292c5890d88e3fa43182e049ac89ddaa477..a8f59a5efae736e8de7fb31193163d7fb4964f16 100644 --- a/src/shared/utils/CSVReader/CSVReader.h +++ b/src/shared/utils/CSVReader/CSVReader.h @@ -123,6 +123,9 @@ private: /** * @brief Iterable parser of CSV files. * + * If the CSV file has an header row, you must specify true as the second + * parameter in the constructor. + * * Given the file name, reads the contents as elements of type Data. Can be used * with CSVIterator to iterate through all the CSV rows. * You can retrieve all data inside the file as a vector with collect(). @@ -134,7 +137,14 @@ template <typename Data> class CSVParser { public: - CSVParser(const char* fileName) : fileStream(fileName) {} + CSVParser(const char* fileName, bool hasHeader = false) + : fileStream(fileName) + { + // If the file has the header, ignore everithing in the first line + if (hasHeader) + fileStream.ignore(std::numeric_limits<std::streamsize>::max(), + '\n'); + } ~CSVParser() { fileStream.close(); } diff --git a/src/shared/utils/ClockUtils.h b/src/shared/utils/ClockUtils.h index 6e7c449b53e0cf8a2e0098fce331c331f900cb4c..3d671d9f48311aae9bae40d24b5f022e6d9d8e8e 100644 --- a/src/shared/utils/ClockUtils.h +++ b/src/shared/utils/ClockUtils.h @@ -341,6 +341,8 @@ inline bool ClockUtils::enablePeripheralClock(void* peripheral) return false; } + RCC_SYNC(); + return true; } @@ -575,6 +577,8 @@ inline bool ClockUtils::disablePeripheralClock(void* peripheral) return false; } + RCC_SYNC(); + return true; } diff --git a/src/shared/utils/Constants.h b/src/shared/utils/Constants.h index a2f8ca35922c623794e83229466daa013344b856..ddc1e810901159f6b92d95041a8a3606dd8ef939 100644 --- a/src/shared/utils/Constants.h +++ b/src/shared/utils/Constants.h @@ -25,13 +25,23 @@ namespace Boardcore { -static constexpr const float PI = 3.14159265f; -static constexpr const float EARTH_GRAVITY = 9.80665f; -static constexpr const float EARTH_RADIUS = 6371.0f * 1000.0f; // [m] +namespace Constants +{ + +static constexpr const float PI = 3.14159265f; // [rad] static constexpr const float DEGREES_TO_RADIANS = PI / 180.0f; static constexpr const float RADIANS_TO_DEGREES = 180.0f / PI; -static constexpr const float KNOTS_TO_MPS = 0.514444; -static constexpr const float MSL_PRESSURE = 101325.0f; // [Pa] -static constexpr const float MSL_TEMPERATURE = 288.15f; // [Kelvin] + +static constexpr const float g = 9.80665f; // [m^s^2] + +static constexpr float a = 0.0065f; // Troposphere temperature gradient [deg/m] +static constexpr float R = 287.05f; // Air gas constant [J/Kg/K] +static constexpr float n = g / (R * a); +static constexpr float nInv = (R * a) / g; + +static constexpr const float MSL_PRESSURE = 101325.0f; // [Pa] +static constexpr const float MSL_TEMPERATURE = 288.15f; // [Kelvin] + +} // namespace Constants } // namespace Boardcore diff --git a/src/shared/utils/MovingAverage.h b/src/shared/utils/MovingAverage.h index 94e8fda20d5577c21c9de585a6f3081547c2a0e3..6a6efd1bbcf6790efa89a17919146cb6bc770b37 100644 --- a/src/shared/utils/MovingAverage.h +++ b/src/shared/utils/MovingAverage.h @@ -45,12 +45,21 @@ public: value += newValue * movingAverageCoeff; } + void reset() { value = 0; } + + void setN(int movingAverageN) + { + this->movingAverageN = movingAverageN; + movingAverageCoeff = 1 / (float)movingAverageN; + movingAverageCompCoeff = 1 - movingAverageCoeff; + } + private: T value = 0; - const int movingAverageN = 20; - const float movingAverageCoeff = 1 / (float)movingAverageN; - const float movingAverageCompCoeff = 1 - movingAverageCoeff; + int movingAverageN = 20; + float movingAverageCoeff = 1 / (float)movingAverageN; + float movingAverageCompCoeff = 1 - movingAverageCoeff; }; } // namespace Boardcore diff --git a/src/shared/math/SkyQuaternion.cpp b/src/shared/utils/SkyQuaternion/SkyQuaternion.cpp similarity index 51% rename from src/shared/math/SkyQuaternion.cpp rename to src/shared/utils/SkyQuaternion/SkyQuaternion.cpp index d8f678341d333d84a30f2bdf127ecf8b57a2b79c..1af74a6b622c2c287b4e602167ac5d0d0e1335af 100644 --- a/src/shared/math/SkyQuaternion.cpp +++ b/src/shared/utils/SkyQuaternion/SkyQuaternion.cpp @@ -1,5 +1,5 @@ -/* Copyright (c) 2020 Skyward Experimental Rocketry - * Author: Marco Cella +/* Copyright (c) 2022 Skyward Experimental Rocketry + * Authors: Marco Cella, Alberto Nidasio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -32,13 +32,15 @@ using namespace Eigen; namespace Boardcore { -SkyQuaternion::SkyQuaternion() {} +namespace SkyQuaternion +{ -Vector4f SkyQuaternion::eul2quat(Vector3f degeul) // ZYX rotation +Vector4f eul2quat(Vector3f euler) { - float yaw = degeul(0) * DEGREES_TO_RADIANS; - float pitch = degeul(1) * DEGREES_TO_RADIANS; - float roll = degeul(2) * DEGREES_TO_RADIANS; + // Euler angles are ZYX + float yaw = euler(0) * Constants::DEGREES_TO_RADIANS; + float pitch = euler(1) * Constants::DEGREES_TO_RADIANS; + float roll = euler(2) * Constants::DEGREES_TO_RADIANS; float cyaw = cosf(yaw * 0.5F); float syaw = sinf(yaw * 0.5F); @@ -52,52 +54,45 @@ Vector4f SkyQuaternion::eul2quat(Vector3f degeul) // ZYX rotation float qz = syaw * cpitch * croll - cyaw * spitch * sroll; float qw = cyaw * cpitch * croll + syaw * spitch * sroll; - Vector4f quat(qx, qy, qz, qw); - - return quat; + return Vector4f(qx, qy, qz, qw); } -Vector3f SkyQuaternion::quat2eul(Vector4f quater) +Vector3f quat2eul(Vector4f quat) { - float qx = quater(0); - float qy = quater(1); - float qz = quater(2); - float qw = quater(3); + // Euler angles are ZYX + float qx = quat(0); + float qy = quat(1); + float qz = quat(2); + float qw = quat(3); float yaw = atan2f(2.0F * (qx * qy + qw * qz), powf(qw, 2) + powf(qx, 2) - powf(qy, 2) - powf(qz, 2)); float a = -2.0F * (qx * qz - qw * qy); if (a > 1.0F) - { a = 1.0F; - } else if (a < -1.0F) - { a = -1.0F; - } float pitch = asinf(a); float roll = atan2f(2.0F * (qy * qz + qw * qx), powf(qw, 2) - powf(qx, 2) - powf(qy, 2) + powf(qz, 2)); - Vector3f eul(roll, pitch, yaw); - - return eul * 180.0F / PI; + return Vector3f(roll, pitch, yaw) * Constants::RADIANS_TO_DEGREES; } -Vector4f SkyQuaternion::rotm2quat(Matrix3f R) +Vector4f rotationMatrix2quat(Matrix3f rtm) { - float r11 = R(0, 0); - float r12 = R(0, 1); - float r13 = R(0, 2); - float r21 = R(1, 0); - float r22 = R(1, 1); - float r23 = R(1, 2); - float r31 = R(2, 0); - float r32 = R(2, 1); - float r33 = R(2, 2); + float r11 = rtm(0, 0); + float r12 = rtm(0, 1); + float r13 = rtm(0, 2); + float r21 = rtm(1, 0); + float r22 = rtm(1, 1); + float r23 = rtm(1, 2); + float r31 = rtm(2, 0); + float r32 = rtm(2, 1); + float r33 = rtm(2, 2); float qx; float qy; @@ -105,63 +100,63 @@ Vector4f SkyQuaternion::rotm2quat(Matrix3f R) float qw; float tr = r11 + r22 + r33; - float r = sqrt(1 + tr); - if (r32 - r23 > 0) - { - qx = 0.5 * sqrt(1 + r11 - r22 - r33); - } - else if (r32 - r23 < 0) + if (tr > 0) { - qx = -0.5 * sqrt(1 + r11 - r22 - r33); - } - else - { - qx = 0; - } + float sqtrp1 = sqrt(1 + tr); - if (r13 - r31 > 0) - { - qy = 0.5 * sqrt(1 - r11 + r22 - r33); - } - else if (r13 - r31 < 0) - { - qy = -0.5 * sqrt(1 - r11 + r22 - r33); + qx = (r23 - r32) / (2.0 * sqtrp1); + qy = (r31 - r13) / (2.0 * sqtrp1); + qz = (r12 - r21) / (2.0 * sqtrp1); + qw = 0.5 * sqtrp1; } else { - qy = 0; - } + if ((r22 > r11) && (r22 > r33)) + { + float sqdip1 = sqrt(r22 - r11 - r33 + 1.0); - if (r21 - r12 > 0) - { - qz = 0.5 * sqrt(1 - r11 - r22 + r33); - } - else if (r21 - r12 < 0) - { - qz = -0.5 * sqrt(1 - r11 - r22 + r33); - } - else - { - qz = 0; - } + qy = 0.5 * sqdip1; - qw = 0.5 * r; + if (sqdip1 != 0) + sqdip1 = 0.5 / sqdip1; - if (qw < 0) - { - qx = -qx; - qy = -qy; - qz = -qz; - qw = -qw; - } + qx = (r12 + r21) * sqdip1; + qz = (r23 + r32) * sqdip1; + qw = (r31 - r13) * sqdip1; + } + else if (r33 > r11) + { + float sqdip1 = sqrt(r33 - r11 - r22 + 1.0); + + qz = 0.5 * sqdip1; + + if (sqdip1 != 0) + sqdip1 = 0.5 / sqdip1; + + qx = (r31 + r13) * sqdip1; + qy = (r23 + r32) * sqdip1; + qw = (r12 - r21) * sqdip1; + } + else + { + float sqdip1 = sqrt(r11 - r22 - r33 + 1.0); - Vector4f quat(qx, qy, qz, qw); + qx = 0.5 * sqdip1; - return quat / quat.norm(); + if (sqdip1 != 0) + sqdip1 = 0.5 / sqdip1; + + qy = (r12 + r21) * sqdip1; + qz = (r31 + r13) * sqdip1; + qw = (r23 - r32) * sqdip1; + } + } + + return Vector4f(qx, qy, qz, qw); } -bool SkyQuaternion::quatnormalize(Vector4f& quat) +bool quatNormalize(Vector4f& quat) { float den = sqrt(powf(quat(0), 2) + powf(quat(1), 2) + powf(quat(2), 2) + powf(quat(3), 2)); @@ -173,28 +168,21 @@ bool SkyQuaternion::quatnormalize(Vector4f& quat) return true; } -Vector4f SkyQuaternion::quatProd(const Vector4f q1, const Vector4f q2) +Vector4f quatProd(const Vector4f q1, const Vector4f q2) { - float q1x = q1(0); - float q1y = q1(1); - float q1z = q1(2); - Vector3f qv1(q1x, q1y, q1z); - float q1w = q1(3); - - float q2x = q2(0); - float q2y = q2(1); - float q2z = q2(2); - Vector3f qv2(q2x, q2y, q2z); - float q2w = q2(3); + Vector3f qv1 = q1.head<3>(); + float qs1 = q1(3); + Vector3f qv2 = q2.head<3>(); + float qs2 = q2(3); Vector4f quater; // cppcheck-suppress constStatement - quater << q1w * qv2 + q2w * qv1 - qv1.cross(qv2), q1w * q2w - qv1.dot(qv2); - float quaternionNorm = sqrt(quater(0) * quater(0) + quater(1) * quater(1) + - quater(2) * quater(2) + quater(3) * quater(3)); - quater = quater / quaternionNorm; + quater << qs1 * qv2 + qs2 * qv1 - qv1.cross(qv2), qs1 * qs2 - qv1.dot(qv2); + quater.normalize(); return quater; } +} // namespace SkyQuaternion + } // namespace Boardcore diff --git a/src/shared/utils/SkyQuaternion/SkyQuaternion.h b/src/shared/utils/SkyQuaternion/SkyQuaternion.h new file mode 100644 index 0000000000000000000000000000000000000000..fecab90dcc1aa9e9541bc3ac8de59470a354ac18 --- /dev/null +++ b/src/shared/utils/SkyQuaternion/SkyQuaternion.h @@ -0,0 +1,81 @@ +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Author: Marco Cella + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <Eigen/Dense> + +namespace Boardcore +{ + +/** + * @brief Functions for managing quaternions. + * + * Convention used: x, y, z, w (scalar element as last element) + */ +namespace SkyQuaternion +{ + +/** + * @brief Transform a vector of euler angles (ZYX) to quaternion. + * + * @param euler The vector of euler angles to be transformed [deg] + * @return Transformed quaternion [x, y, z, w] + */ +Eigen::Vector4f eul2quat(Eigen::Vector3f euler); + +/** + * @brief Transform a quaternion to a vector of euler angles (ZYX). + * + * @param quat The quaternion to be transformed [x, y, z, w] + * @return Transformed vector of euler angles [deg] + */ +Eigen::Vector3f quat2eul(Eigen::Vector4f quat); + +/** + * @brief Transform a rotation matrix to a quaternion. + * + * @param rtm The rotation matrix to be transformed (3x3) + * @return Transformed quaternion [x, y, z, w] + */ +Eigen::Vector4f rotationMatrix2quat(Eigen::Matrix3f rtm); + +/** + * @brief Normalize a quaternion. + * + * @param quat The quaternion to be normalized [x, y, z, w] + * @return True if the operation succeeded or not + */ +bool quatNormalize(Eigen::Vector4f& quat); + +/** + * @brief Compute the product of two quaternions. + * + * @param q1 First factor [x, y, z, w] + * @param q2 Second factor [x, y, z, w] + * @return The resulting quaternions product [x, y, z, w] + */ +Eigen::Vector4f quatProd(const Eigen::Vector4f q1, const Eigen::Vector4f q2); + +}; // namespace SkyQuaternion + +} // namespace Boardcore diff --git a/src/shared/math/Stats.cpp b/src/shared/utils/Stats/Stats.cpp similarity index 100% rename from src/shared/math/Stats.cpp rename to src/shared/utils/Stats/Stats.cpp diff --git a/src/shared/math/Stats.h b/src/shared/utils/Stats/Stats.h similarity index 100% rename from src/shared/math/Stats.h rename to src/shared/utils/Stats/Stats.h diff --git a/src/shared/utils/testutils/FakeSpiTypedef.h b/src/shared/utils/TestUtils/FakeSpiTypedef.h similarity index 98% rename from src/shared/utils/testutils/FakeSpiTypedef.h rename to src/shared/utils/TestUtils/FakeSpiTypedef.h index 46b770c44e3b82eb406d54e5718aac248b2b9b61..dae48554ab5c4ac85e0d6e642f7e723a77b48be0 100644 --- a/src/shared/utils/testutils/FakeSpiTypedef.h +++ b/src/shared/utils/TestUtils/FakeSpiTypedef.h @@ -22,7 +22,7 @@ #pragma once #include <miosix.h> -#include <utils/testutils/MockGpioPin.h> +#include <utils/TestUtils/MockGpioPin.h> #include <cstdint> #include <vector> diff --git a/src/shared/utils/testutils/MockGpioPin.h b/src/shared/utils/TestUtils/MockGpioPin.h similarity index 100% rename from src/shared/utils/testutils/MockGpioPin.h rename to src/shared/utils/TestUtils/MockGpioPin.h diff --git a/src/shared/utils/testutils/MockSPIBus.h b/src/shared/utils/TestUtils/MockSPIBus.h similarity index 98% rename from src/shared/utils/testutils/MockSPIBus.h rename to src/shared/utils/TestUtils/MockSPIBus.h index 9bdc6129e9e2c13f547fe2bada8c1259738d8b71..2fdf6a948ac106fb0fd09338f850c8ef186d2ca4 100644 --- a/src/shared/utils/testutils/MockSPIBus.h +++ b/src/shared/utils/TestUtils/MockSPIBus.h @@ -57,7 +57,7 @@ namespace Boardcore "SpiBusInterface must be built using MockGpioPin (-DUSE_MOCK_PERIPHERALS)" #endif -#include <utils/testutils/MockGpioPin.h> +#include <utils/TestUtils/MockGpioPin.h> using miosix::FastMutex; using miosix::Lock; @@ -69,10 +69,10 @@ public: ~MockSPIBus() {} // Delete copy/move contructors/operators - MockSPIBus(const MockSPIBus&) = delete; + MockSPIBus(const MockSPIBus&) = delete; MockSPIBus& operator=(const MockSPIBus&) = delete; - MockSPIBus(MockSPIBus&&) = delete; + MockSPIBus(MockSPIBus&&) = delete; MockSPIBus& operator=(MockSPIBus&&) = delete; /** diff --git a/src/shared/utils/testutils/TestHelper.cpp b/src/shared/utils/TestUtils/TestHelper.cpp similarity index 100% rename from src/shared/utils/testutils/TestHelper.cpp rename to src/shared/utils/TestUtils/TestHelper.cpp diff --git a/src/shared/utils/testutils/TestHelper.h b/src/shared/utils/TestUtils/TestHelper.h similarity index 96% rename from src/shared/utils/testutils/TestHelper.h rename to src/shared/utils/TestUtils/TestHelper.h index 660838fc04fcf103dd581f828e80b99443e6fe23..2e7592f4273969efca7d3ea6a4d38c70b7d02695 100644 --- a/src/shared/utils/testutils/TestHelper.h +++ b/src/shared/utils/TestUtils/TestHelper.h @@ -98,7 +98,7 @@ bool testFSMTransition(FSM_type& fsm, const Event& ev, template <class FSM_type> bool testFSMAsyncTransition(FSM_type& fsm, const Event& ev, uint8_t topic, void (FSM_type::*expectedState)(const Event&), - EventBroker& broker = sEventBroker) + EventBroker& broker = EventBroker::getInstance()) { broker.post(ev, topic); // Wait for the event to be handled @@ -154,7 +154,7 @@ bool testHSMTransition(HSM_type& hsm, const Event& ev, template <class HSM_type> bool testHSMAsyncTransition(HSM_type& hsm, const Event& ev, uint8_t topic, State (HSM_type::*expectedState)(const Event&), - EventBroker& broker = sEventBroker) + EventBroker& broker = EventBroker::getInstance()) { broker.post(ev, topic); // Wait for the event to be handled @@ -183,7 +183,7 @@ bool testHSMAsyncTransition(HSM_type& hsm, const Event& ev, uint8_t topic, */ bool expectEvent(uint8_t eventId, uint8_t topic, long long when, long long uncertainty = EVENT_TIMING_UNCERTAINTY, - EventBroker& broker = sEventBroker); + EventBroker& broker = EventBroker::getInstance()); /** * @brief Waits until the specified event is received or a timeout expires @@ -197,6 +197,6 @@ bool expectEvent(uint8_t eventId, uint8_t topic, long long when, * false if the timeout has expired */ bool waitForEvent(uint8_t event, uint8_t topic, long long timeout = 0, - EventBroker& broker = sEventBroker); + EventBroker& broker = EventBroker::getInstance()); } // namespace Boardcore diff --git a/src/shared/utils/testutils/TestSensor.h b/src/shared/utils/TestUtils/TestSensor.h similarity index 83% rename from src/shared/utils/testutils/TestSensor.h rename to src/shared/utils/TestUtils/TestSensor.h index 09cc692d12cd7d08446a9567110de3bb68cc2e18..07b19e98b8c415c4f98e4388d1e42b02dec8414b 100644 --- a/src/shared/utils/testutils/TestSensor.h +++ b/src/shared/utils/TestUtils/TestSensor.h @@ -29,9 +29,6 @@ #include <cmath> -using miosix::getTick; -using miosix::TICK_FREQ; - namespace Boardcore { @@ -40,11 +37,14 @@ struct TestData : public TimestampData float value; TestData(float v) - : TimestampData{static_cast<uint64_t>(getTick())}, value(v) + : TimestampData{static_cast<uint64_t>(miosix::getTick())}, value(v) { } - TestData() : TimestampData{static_cast<uint64_t>(getTick())}, value(0.0) {} + TestData() + : TimestampData{static_cast<uint64_t>(miosix::getTick())}, value(0.0) + { + } }; class TestSensor : public Sensor<TestData> @@ -60,8 +60,9 @@ public: TestData sampleImpl() override { TRACE("[TestSensor] sampleImpl() \n"); - return TestData(10 * sin(PI * static_cast<float>(getTick()) / - static_cast<float>(TICK_FREQ))); + return TestData( + 10 * sin(Constants::PI * static_cast<float>(miosix::getTick()) / + static_cast<float>(miosix::TICK_FREQ))); } }; diff --git a/src/shared/utils/testutils/ThroughputCalculator.h b/src/shared/utils/TestUtils/ThroughputCalculator.h similarity index 100% rename from src/shared/utils/testutils/ThroughputCalculator.h rename to src/shared/utils/TestUtils/ThroughputCalculator.h diff --git a/src/shared/utils/collections/CircularBuffer.h b/src/shared/utils/collections/CircularBuffer.h index fa6a2e58286996793ee9c7b949b26da61e4cb1db..6182bdcc5b7967af9b90570dada7d62c635438ce 100644 --- a/src/shared/utils/collections/CircularBuffer.h +++ b/src/shared/utils/collections/CircularBuffer.h @@ -32,7 +32,7 @@ namespace Boardcore { /** - * Implementation of an non-synchronized circular buffer + * Implementation of an non-synchronized circular buffer. */ template <typename T, unsigned int Size> class CircularBuffer @@ -41,21 +41,25 @@ class CircularBuffer public: CircularBuffer() {} + virtual ~CircularBuffer() {} /** - * Puts a copy of the element in the buffer - * @param elem element + * @brief Puts a copy of the element in the buffer. + * + * @param elem Element to be added to the queue. + * @return The element added. */ virtual T& put(const T& elem) { buffer[writePtr] = elem; T& added = buffer[writePtr]; + // Advance the read pointer if the two pointers match if (!empty && writePtr == readPtr) - { readPtr = (readPtr + 1) % Size; - } + + // Advance the write pointer writePtr = (writePtr + 1) % Size; empty = false; @@ -64,20 +68,19 @@ public: } /** - * Gets an element from the buffer, without removing it - * Index starts from the oldest element in the buffer: get(0) returns the - * same element as get() + * @brief Gets an element from the buffer, without removing it. * - * @warning Remember to catch the exception! - * @throw range_error if index >= count() + * Index starts from the oldest element in the buffer. + * get() returns the first element. * - * @param i Index of the elemnt to get, starting from the oldest - * - * @return the element + * @warning Remember to catch the exception! + * @throw range_error if index >= count(). + * @param i Index of the element to get, starting from the oldest. + * @return The element. */ - virtual T& get(unsigned int i) + virtual T& get(unsigned int i = 0) { - if (i < CircularBuffer<T, Size>::count()) + if (i < count()) { int ptr = (readPtr + i) % Size; return buffer[ptr]; @@ -89,33 +92,18 @@ public: /** * @brief Returns the last element added in the buffer. * - * @throw range_error if buffer is empty * @warning Remember to catch the exception! - * @return the element + * @throw range_error if buffer is empty. + * @return The element. */ virtual T& last() { return get(count() - 1); } /** - * Gets the first element from the buffer, without removing it - * @throw range_error if buffer is empty - * @warning Remember to catch the exception! - * @return the element - */ - virtual T& get() - { - if (!empty) - { - return buffer[readPtr]; - } - else - throw range_error("CircularBuffer is empty!"); - } - - /** - * Pops the first element in the buffer. - * @throw range_error if buffer is empty + * @brief Pops the first element in the buffer. + * * @warning Remember to catch the exception! - * @return the element that has been popped + * @throw range_error if buffer is empty. + * @return The element that has been popped. */ virtual const T& pop() { @@ -133,8 +121,9 @@ public: } /** - * Counts the elements in the buffer - * @return number of elements in the buffer + * @brief Counts the elements in the buffer. + * + * @return Number of elements in the buffer. */ virtual size_t count() const { @@ -157,16 +146,19 @@ public: return CircularBuffer<T, Size>::count() == Size; } /** - * Returns the maximum number of elements that can be stored in the buffer - * @return buffer size + * @brief Returns the maximum number of elements that can be stored in the + * buffer. + * + * @return Buffer size. */ size_t getSize() const { return Size; } protected: T buffer[Size]; - size_t writePtr = 0, readPtr = 0; - bool empty = true; + size_t writePtr = 0; + size_t readPtr = 0; + bool empty = true; }; } // namespace Boardcore diff --git a/src/shared/utils/collections/IRQCircularBuffer.h b/src/shared/utils/collections/IRQCircularBuffer.h index a6a02e63664400c7b5efc1fc9af1287613862598..e27d15e2a37652df6e822d29fd8d3ff0cd3af104 100644 --- a/src/shared/utils/collections/IRQCircularBuffer.h +++ b/src/shared/utils/collections/IRQCircularBuffer.h @@ -34,7 +34,8 @@ namespace Boardcore { /** - * Implementation of a synchronized circular buffer + * Implementation of a synchronized circular buffer that can be used inside + * interrupt service routines. */ template <typename T, unsigned int Size> class IRQCircularBuffer : public CircularBuffer<T, Size> @@ -44,8 +45,7 @@ class IRQCircularBuffer : public CircularBuffer<T, Size> public: /** - * Puts a copy of the element in the buffer - * @param elem element + * @brief Puts a copy of the element in the buffer. */ T& put(const T& elem) override { @@ -55,37 +55,28 @@ public: } /** - * Gets the first element from the buffer, without removing it - * @warning Remember to catch the exception! - * @return the element - * @throws range_error if buffer is empty - */ - T& get() override - { - FastInterruptDisableLock d; - return Super::get(); - } - - /** - * Gets an element from the buffer, without removing it - * Index starts at the element returned by get() or pop(): get(0) is - * the same as get() + * @brief Gets an element from the buffer, without removing it. + * + * Index starts from the oldest element in the buffer. + * get() returns the first element. * * @warning Remember to catch the exception! - * @return the element - * @throws range_error if buffer is empty + * @throw range_error if index >= count(). + * @param i Index of the element to get, starting from the oldest. + * @return The element. */ - T& get(unsigned int i) override + T& get(unsigned int i = 0) override { FastInterruptDisableLock d; return Super::get(i); } /** - * Pops the first element in the buffer. + * @brief Pops the first element in the buffer. + * * @warning Remember to catch the exception! - * @return the element that has been popped - * @throws range_error if buffer is empty + * @throw range_error if buffer is empty. + * @return The element that has been popped. */ const T& pop() override { @@ -94,8 +85,9 @@ public: } /** - * Counts the elements in the buffer - * @return number of elements in the buffer + * @brief Counts the elements in the buffer. + * + * @return Number of elements in the buffer. */ size_t count() const override { @@ -116,9 +108,9 @@ public: } /** - * Puts a copy of the element in the buffer - * Only to be called inside an ISR or with interrupts disabled - * @param elem element + * @brief Puts a copy of the element in the buffer. + * + * @warning Only to be called inside an ISR or with interrupts disabled. */ T& IRQput(const T& elem) { @@ -127,8 +119,10 @@ public: } /** - * Puts a copy of the element in the buffer - * Only to be called inside an ISR or with interrupts disabled + * @brief Puts a copy of the element in the buffer. + * + * @warning Only to be called inside an ISR or with interrupts disabled. + * * @param elem element * @param hppw Set to true if the woken thread is higher priority than the * current one, unchanged otherwise @@ -137,68 +131,59 @@ public: { if (waiting && (waiting->IRQgetPriority() > Thread::IRQgetCurrentThread()->IRQgetPriority())) - { hppw = true; - } IRQwakeWaitingThread(); return Super::put(elem); } /** - * Gets the first element from the buffer, without removing it - * Only to be called inside an ISR or with interrupts disabled - * @warning Remember to catch the exception! - * @return the element - * @throws range_error if buffer is empty - */ - T& IRQget() { return Super::get(); } - - /** - * Gets an element from the buffer, without removing it - * Index starts at the element returned by get() or pop(): get(0) is - * the same as get() - * @warning Only to be called inside an ISR or with interrupts disabled + * @brief Gets an element from the buffer, without removing it. + * + * @warning Only to be called inside an ISR or with interrupts disabled. + * + * Index starts from the oldest element in the buffer. + * get() returns the first element. + * * @warning Remember to catch the exception! - * @return the element - * @throws range_error if buffer is empty + * @throw range_error if index >= count(). + * @param i Index of the element to get, starting from the oldest. + * @return The element. */ - T& IRQget(unsigned int i) { return Super::get(i); } + T& IRQget(unsigned int i = 0) { return Super::get(i); } /** - * Pops the first element in the buffer. - * @warning Only to be called inside an ISR or with interrupts disabled + * @brief Pops the first element in the buffer. + * + * @warning Only to be called inside an ISR or with interrupts disabled. + * * @warning Remember to catch the exception! - * @return the element that has been popped - * @throws range_error if buffer is empty + * @throw range_error if buffer is empty. + * @return The element that has been popped. */ const T& IRQpop() { return Super::pop(); } /** - * Counts the elements in the buffer - * @warning Only to be called inside an ISR or with interrupts disabled - * @return number of elements in the buffer + * @brief Counts the elements in the buffer. + * + * @warning Only to be called inside an ISR or with interrupts disabled. + * + * @return Number of elements in the buffer. */ size_t IRQcount() const { return Super::count(); } /** - * @brief Returns true if the buffer is empty - * - * @warning Only to be called inside an ISR or with interrupts disabled - * @return empty or not + * @warning Only to be called inside an ISR or with interrupts disabled. */ bool IRQisEmpty() const { return Super::isEmpty(); } /** - * @brief Returns true if the buffer is full - * - * @warning Only to be called inside an ISR or with interrupts disabled - * @return buffer full or not + * @warning Only to be called inside an ISR or with interrupts disabled. */ bool IRQisFull() const { return Super::isFull(); } /** - * @brief Waits until the buffer contains at least one element + * @brief Waits until the buffer contains at least one element. */ void waitUntilNotEmpty() { @@ -228,4 +213,5 @@ private: Thread* waiting = nullptr; }; -} // namespace Boardcore \ No newline at end of file + +} // namespace Boardcore diff --git a/src/shared/utils/collections/SyncCircularBuffer.h b/src/shared/utils/collections/SyncCircularBuffer.h index 9e05227df79823f100b0ff915530c451fd0862b8..cab438fbb629c00aa98122ed9b3512deca329e57 100644 --- a/src/shared/utils/collections/SyncCircularBuffer.h +++ b/src/shared/utils/collections/SyncCircularBuffer.h @@ -55,37 +55,28 @@ public: } /** - * Gets the first element from the buffer, without removing it - * @warning Remember to catch the exception! - * @return the element - * @throws range_error if buffer is empty - */ - T& get() override - { - Lock<FastMutex> l(mutex); - return Super::get(); - } - - /** - * Gets an element from the buffer, without removing it - * Index starts at the element returned by get() or pop(): get(0) is - * the same as get() + * @brief Gets an element from the buffer, without removing it. + * + * Index starts from the oldest element in the buffer. + * get() returns the first element. * * @warning Remember to catch the exception! - * @return the element - * @throws range_error if buffer is empty + * @throw range_error if index >= count(). + * @param i Index of the element to get, starting from the oldest. + * @return The element. */ - T& get(unsigned int i) override + T& get(unsigned int i = 0) override { Lock<FastMutex> l(mutex); return Super::get(i); } /** - * Pops the first element in the buffer. + * @brief Pops the first element in the buffer. + * * @warning Remember to catch the exception! - * @return the element that has been popped - * @throws range_error if buffer is empty + * @throw range_error if buffer is empty. + * @return The element that has been popped. */ const T& pop() override { @@ -94,8 +85,9 @@ public: } /** - * Counts the elements in the buffer - * @return number of elements in the buffer + * @brief Counts the elements in the buffer. + * + * @return Number of elements in the buffer. */ size_t count() const override { @@ -116,7 +108,7 @@ public: } /** - * @brief Waits until the buffer contains at least one element + * @brief Waits until the buffer contains at least one element. */ void waitUntilNotEmpty() { diff --git a/src/shared/utils/collections/SyncPacketQueue.h b/src/shared/utils/collections/SyncPacketQueue.h index ff5e7b360a3f612cf73feae05e0283267d96108f..974979585047db698365923d7077444ea6f0d9a5 100644 --- a/src/shared/utils/collections/SyncPacketQueue.h +++ b/src/shared/utils/collections/SyncPacketQueue.h @@ -1,5 +1,5 @@ -/* Copyright (c) 2019 Skyward Experimental Rocketry - * Author: Alvise de'Faveri Tron +/* Copyright (c) 2019-2022 Skyward Experimental Rocketry + * Authors: Alvise de'Faveri Tron, Davide Mor, Alberto Nidasio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -22,6 +22,7 @@ #pragma once +#include <drivers/timer/TimestampTimer.h> #include <miosix.h> #include <utils/Debug.h> @@ -46,10 +47,9 @@ namespace Boardcore * @brief The Packet class is used for packing together messages with variable * lengths into a fixed size packet. Useful for telemetry. * - * The buffer can only be appended, read or flushed. The caller can also mark - * the packet as ready to be sent. + * Data can only be appended to the payload. The packet can be marked ready. * - * @tparam len Maximum length for the packet. + * @tparam len Packet's payload length. */ template <unsigned int len> class Packet @@ -59,7 +59,7 @@ public: /** * @brief Reserves a fixed length for the packet. */ - Packet() : msgCounter(0), ts(0), ready(false) { content.reserve(len); }; + Packet() { content.reserve(len); }; /** * @brief Clears the buffer. @@ -67,24 +67,25 @@ public: ~Packet() { content.clear(); }; /** - * @brief Try to append a given message to the packet. + * @brief Append a given message to the packet. * - * If it's the first message, also set the timestamp. + * If the message can't fit inside the remaining space only the first bytes + * are copied. * * @param msg The message to be appended. - * @param msgLen Length of msg. - * @return true if the message was appended correctly. - * @return false if there isn't enough space for the message. + * @param msgLen Length of the message. + * @return How many bytes were actually appended. */ - bool tryAppend(const uint8_t* msg, const size_t msgLen); + size_t append(const uint8_t* msg, size_t msgLen); /** - * @brief Mark the packet as ready to be sent. + * @brief Mark the packet as ready. */ inline void markAsReady() { ready = true; } /** - * @brief Copies the content of the buffer at a given address. + * @brief Copies the content of the payload at the given address. + * * @param buf Where to copy the content. * @return How many bytes where copied, i.e. the size of the packet. */ @@ -111,12 +112,12 @@ public: inline bool isReady() const { return ready; } /** - * @return The timestamp of the first successful call to tryAppend(). + * @return The timestamp of the first successful call to append(). */ - inline uint64_t timestamp() const { return ts; } + inline uint64_t getTimestamp() const { return timestamp; } /** - * @return The occupied portion of the buffer (bytes). + * @return The occupied portion of the buffer [bytes]. */ inline size_t size() const { return content.size(); } @@ -126,7 +127,7 @@ public: inline unsigned int maxSize() const { return len; } /** - * @return How many times the tryAppend() function has been called + * @return How many times the append() function has been called * successfully. */ inline unsigned int getMsgCount() const { return msgCounter; } @@ -134,7 +135,7 @@ public: /** * @brief Print information about this object. * - * @param os For example, std::cout + * @param os For example, std::cout. */ void print(std::ostream& os) const; @@ -142,32 +143,29 @@ public: std::vector<uint8_t> content; private: - unsigned int msgCounter; - uint64_t ts; - bool ready; + unsigned int msgCounter = 0; + uint64_t timestamp = 0; + bool ready = false; }; template <unsigned int len> -bool Packet<len>::tryAppend(const uint8_t* msg, const size_t msgLen) +size_t Packet<len>::append(const uint8_t* msg, size_t msgLen) { - if (msgLen == 0 || content.size() + msgLen > len) - { - return false; - } - else + size_t remaining = len - content.size(); + msgLen = std::min(remaining, msgLen); + + if (msgLen != 0) { // Set the packet's timestamp when the first message is inserted if (content.size() == 0) - { - ts = miosix::getTick(); - } + timestamp = TimestampTimer::getInstance().getTimestamp(); // Append the message to the packet content.insert(content.end(), msg, msg + msgLen); msgCounter++; - - return true; } + + return msgLen; } template <unsigned int len> @@ -175,7 +173,7 @@ void Packet<len>::clear() { content.clear(); msgCounter = 0; - ts = 0; + timestamp = 0; ready = false; } @@ -189,194 +187,167 @@ size_t Packet<len>::dump(uint8_t* buf) template <unsigned int len> void Packet<len>::print(std::ostream& os) const { - os << "timestamp=" << ts << ", ready=" << ready + os << "timestamp=" << timestamp << ", ready=" << ready << ", size=" << content.size() << ", msgCounter=" << msgCounter << ", content= "; for (auto const& i : content) - { os << i; - } os << '\n'; } -/****************************************************************************** - * @brief A SyncPacketQueue is a SyncCircularBuffer of Packets. The difference - * is that you pop() Packets but you append() bytes. The bytes will be appended - * to the first available packet. This class is suitable for synchronization - * between two threads. +/** + * @brief A SyncPacketQueue is a SyncCircularBuffer of Packets. * - * @tparam pktLen Maximum length of each packet. (bytes) - * @tparam pktNum Total number of packets. - ******************************************************************************/ + * The difference is that you pop() Packets but you append() bytes. The bytes + * will be appended to the first available packet and the next ones. + * This class is suitable for synchronization between two threads. + * + * @tparam pktLen Maximum length of each packet [bytes]. + * @tparam pktNum Total number of packets. + */ template <unsigned int pktLen, unsigned int pktNum> class SyncPacketQueue { - using Pkt = Packet<pktLen>; - public: /** - * @brief Try to append a given message to the last packet. If there isn't - * enough space, the packet is marked as ready and the message is appended - * to the next packet. If there are no more available packets, the oldest - * one is overwritten. + * @brief Try to append a given message to the packets queue. * - * @param msg the message to be appended - * @param msgLen length of msg - * @return true if the message was appended correctly - * @return false if there isn't enough space for the message + * The message is appended to the last packet and if the space isn't enough, + * it is divided into successive packets. If there are no more available + * packets, the oldest one is overwritten. + * + * The message isn't added to the queue if there is no space considering all + * the queue packets. + * + * @param msg The message to be appended. + * @param msgLen Length of the message [bytes]. + * @return True if the message was appended. */ - int put(uint8_t* msg, size_t msgLen) + bool put(uint8_t* msg, size_t msgLen) { - int dropped = 0; + // Check if the message is empty + if (msgLen == 0) + return false; - if (msgLen == 0 || msgLen > pktLen) - { - return -1; - } + // Check if the queue can hold the packet + if (msgLen > pktLen * pktNum) + return false; { + // Lock the mutex on the buffer Lock<FastMutex> l(mutex); + // Add an element if there isn't any if (buffer.count() == 0) - { - buffer.put(Pkt{}); - } + buffer.put({}); - Pkt& last = buffer.last(); + // Write all the packet + while (msgLen > 0) + { + // If the last packet is ready append a new one + if (buffer.last().isReady()) + buffer.put({}); - bool added = false; + // Append what data is possible to the last packet + size_t appendedLength = buffer.last().append(msg, msgLen); - // Add to the current packet only if it isn't already ready - if (!last.isReady()) - { - added = last.tryAppend(msg, msgLen); - } + // If the packet is full mark it as ready + if (buffer.last().isFull()) + buffer.last().markAsReady(); - // If already ready or cannot fit the new data, add to a new packet - if (!added) - { - // Mark the packet as ready (in the case it wasn't already) - last.markAsReady(); - - if (buffer.isFull()) - { - // We have dropped a packet - ++dropped; - } - // Add a new packet and fill that instead - Pkt& newpkt = buffer.put(Pkt{}); - - if (!newpkt.tryAppend(msg, msgLen)) - { - TRACE("Packet is too big!\n"); - return -1; - } + // Go forward in the data + msgLen -= appendedLength; + msg += appendedLength; } + } - // Mark as ready if the packet is full - if (buffer.last().isFull()) - { - buffer.last().markAsReady(); - } + // Wake all waiting threads + condVerNotEmpty.broadcast(); - cvNotempty.broadcast(); - return dropped; - } + return true; } /** - * @return a copy of the oldest packet, without removing it from the queue. + * @return The oldest packet, without removing it from the queue. */ - const Pkt& get() + const Packet<pktLen>& get() { Lock<FastMutex> l(mutex); return buffer.get(); } /** - * @return the oldest packet, removing it from the queue. + * @return The oldest packet, removing it from the queue. */ - const Pkt& pop() + const Packet<pktLen>& pop() { Lock<FastMutex> l(mutex); return buffer.pop(); } /** - * @return true if all the packets have been marked as ready. + * @return True if all the packets have been marked as ready. */ bool isFull() { Lock<FastMutex> l(mutex); if (buffer.count() > 0) - { return buffer.isFull() && buffer.last().isReady(); - } else - { return false; - } } /** - * @return true if all the packets are completely empty. + * @return True if all the packets are completely empty. */ bool isEmpty() { Lock<FastMutex> l(mutex); - return buffer.isEmpty(); } /** * @brief Blocks the calling thread until the queue is not empty. + * * Returns immediately if already not empty. */ void waitUntilNotEmpty() { Lock<FastMutex> l(mutex); if (buffer.isEmpty()) - { - cvNotempty.wait(mutex); - } + condVerNotEmpty.wait(mutex); } /** - * @return the number of packets that are ready to be sent. + * @return The number of packets that are ready to be sent. */ size_t countReady() { Lock<FastMutex> l(mutex); if (!buffer.isEmpty()) - { return buffer.last().isReady() ? buffer.count() : buffer.count() - 1; - } else - { return 0; - } } /** - * @return the number of packets in use, that are either fully or partially + * @return The number of packets in use, that are either fully or partially * filled. */ size_t countNotEmpty() { Lock<FastMutex> l(mutex); - return buffer.count(); } private: FastMutex mutex; - ConditionVariable cvNotempty; - - CircularBuffer<Pkt, pktNum> buffer; + ConditionVariable condVerNotEmpty; + CircularBuffer<Packet<pktLen>, pktNum> buffer; }; } // namespace Boardcore diff --git a/src/shared/utils/collections/SyncQueue.h b/src/shared/utils/collections/SyncQueue.h index c1b4ccc9a38b4bc2e6ae0d34fd375f5dd0b1eaf2..5f340bba650b6e8f5238e4cc8b8be02ef7c070ef 100644 --- a/src/shared/utils/collections/SyncQueue.h +++ b/src/shared/utils/collections/SyncQueue.h @@ -39,7 +39,7 @@ public: int len(); private: - SynchronizedQueue(const SynchronizedQueue&) = delete; + SynchronizedQueue(const SynchronizedQueue&) = delete; SynchronizedQueue& operator=(const SynchronizedQueue&) = delete; std::list<T> queue; miosix::Mutex mMutex; diff --git a/src/shared/utils/collections/contiguous_queue.h b/src/shared/utils/collections/contiguous_queue.h index 039482f1516f9b29505973222a6275f6a6b46044..bc63a664d11bb5240eda743fe7750f340ea1cb18 100644 --- a/src/shared/utils/collections/contiguous_queue.h +++ b/src/shared/utils/collections/contiguous_queue.h @@ -128,7 +128,7 @@ public: } private: - ContiguousQueue(const ContiguousQueue&) = delete; + ContiguousQueue(const ContiguousQueue&) = delete; ContiguousQueue& operator=(const ContiguousQueue&) = delete; T elements[N] = {0}; diff --git a/src/shared/utils/gui/NavController.h b/src/shared/utils/gui/NavController.h index 528692b3342b3f807465d64099345eb356fea937..617af98d72b4cf4914a08b2a9098e2188ae830cd 100644 --- a/src/shared/utils/gui/NavController.h +++ b/src/shared/utils/gui/NavController.h @@ -23,7 +23,7 @@ #pragma once #include <diagnostic/PrintLogger.h> -#include <utils/ButtonHandler.h> +#include <utils/ButtonHandler/ButtonHandler.h> #include <utils/Debug.h> #include <vector> @@ -66,43 +66,37 @@ public: } } - void onButtonPress(ButtonPress press) + void onButtonEvent(ButtonEvent press) { switch (press) { - case ButtonPress::SHORT: - if (vecSelectable.size() > 0) - { - selectNext(); - } - break; - case ButtonPress::DOWN: + case ButtonEvent::PRESSED: if (selectedIndex < vecSelectable.size()) - { vecSelectable.at(selectedIndex) ->performInteraction(Interaction::BTN_DOWN); - } + if (vecSelectable.size() > 0) + selectNext(); break; - case ButtonPress::UP: + case ButtonEvent::SHORT_PRESS: if (selectedIndex < vecSelectable.size()) - { vecSelectable.at(selectedIndex) ->performInteraction(Interaction::BTN_UP); - } + if (vecSelectable.size() > 0) + selectNext(); break; - case ButtonPress::LONG: + case ButtonEvent::LONG_PRESS: if (selectedIndex < vecSelectable.size()) - { vecSelectable.at(selectedIndex) ->performInteraction(Interaction::CLICK); - } + if (vecSelectable.size() > 0) + selectNext(); break; - case ButtonPress::VERY_LONG: + case ButtonEvent::VERY_LONG_PRESS: if (selectedIndex < vecSelectable.size()) - { vecSelectable.at(selectedIndex) ->performInteraction(Interaction::LONG_CLICK); - } + if (vecSelectable.size() > 0) + selectNext(); break; default: break; diff --git a/src/shared/utils/gui/ScreenManager.h b/src/shared/utils/gui/ScreenManager.h index 4553fed13ea7126b924a1d4c9d9df008c3957092..ce731f437f638bdf904b44e0f5d1fb89ff11a69b 100644 --- a/src/shared/utils/gui/ScreenManager.h +++ b/src/shared/utils/gui/ScreenManager.h @@ -65,7 +65,7 @@ public: } } - void onButtonPress(ButtonPress press) { controller.onButtonPress(press); } + void onButtonEvent(ButtonEvent press) { controller.onButtonEvent(press); } mxgui::DrawingContext& getDrawingContext() { return dc; } @@ -86,7 +86,7 @@ protected: drawViewTree(screens[activeScreen], dc); - Thread::sleepUntil(start + refreshInterval); + miosix::Thread::sleepUntil(start + refreshInterval); } } diff --git a/src/tests/drivers/test-servo.cpp b/src/tests/actuators/test-buzzer.cpp similarity index 55% rename from src/tests/drivers/test-servo.cpp rename to src/tests/actuators/test-buzzer.cpp index 10eaf9c81ba9e43dbdcfdc7c9db94e9b12ec7801..fc6f6fc6bafecae80a801f91b1115223891c31f7 100644 --- a/src/tests/drivers/test-servo.cpp +++ b/src/tests/actuators/test-buzzer.cpp @@ -1,5 +1,5 @@ -/* Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta, Alberto Nidasio +/* Copyright (c) 2022 Skyward Experimental Rocketry + * Author: Alberto Nidasio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -20,47 +20,33 @@ * THE SOFTWARE. */ -#include <drivers/servo/Servo.h> +#include <actuators/Buzzer.h> #include <miosix.h> -#include <utils/ClockUtils.h> -using namespace Boardcore; using namespace miosix; - -using ps1 = Gpio<GPIOB_BASE, 4>; -using ps2 = Gpio<GPIOA_BASE, 7>; -using ps3 = Gpio<GPIOB_BASE, 8>; +using namespace Boardcore; int main() { - ps1::mode(Mode::ALTERNATE); - ps2::mode(Mode::ALTERNATE); - ps3::mode(Mode::ALTERNATE); - - ps1::alternateFunction(2); - ps2::alternateFunction(2); - ps3::alternateFunction(3); - - Servo s1(TIM3, TimerUtils::Channel::CHANNEL_1); - Servo s2(TIM3, TimerUtils::Channel::CHANNEL_2); - Servo s3(TIM10, TimerUtils::Channel::CHANNEL_1); + Buzzer buzzer(TIM4, TimerUtils::Channel::CHANNEL_2); - s1.enable(); - s2.enable(); - s3.enable(); + printf("Now beeping for 2 seconds\n"); + buzzer.oneTimeToggle(2 * 1000); + Thread::sleep(3 * 1000); - float pos[] = {0, 0.5, 1.0}; + printf("Now continuosly toggle the buzzer every 500ms for 5 seconds"); + buzzer.continuoslyToggle(500, 500); + Thread::sleep(6 * 1000); - s1.setPosition(pos[0]); - s2.setPosition(pos[1]); - s3.setPosition(pos[2]); + printf("Now continuosly toggle the buzzer every 200ms for 5 seconds"); + buzzer.continuoslyToggle(200, 200); + Thread::sleep(6 * 1000); - for (int i = 0;; i++) - { - s1.setPosition(pos[i % 3]); - s2.setPosition(pos[(i + 1) % 3]); - s3.setPosition(pos[(i + 2) % 3]); + printf("Now manually turning on the buzzer for 1 second\n"); + buzzer.on(); + Thread::sleep(1000); + buzzer.off(); - Thread::sleep(2000); - } + while (true) + Thread::sleep(1000); } diff --git a/src/tests/drivers/test-hbridge.cpp b/src/tests/actuators/test-hbridge.cpp similarity index 98% rename from src/tests/drivers/test-hbridge.cpp rename to src/tests/actuators/test-hbridge.cpp index 0f2eb7c18ce9de005af15b1a216743ce02529f58..a2aa0438aa91015c899273a37c38ff87458def55 100644 --- a/src/tests/drivers/test-hbridge.cpp +++ b/src/tests/actuators/test-hbridge.cpp @@ -20,7 +20,7 @@ * THE SOFTWARE. */ -#include <drivers/hbridge/HBridge.h> +#include <actuators/HBridge/HBridge.h> #include <miosix.h> #include <utils/Debug.h> diff --git a/src/tests/actuators/test-servo.cpp b/src/tests/actuators/test-servo.cpp new file mode 100644 index 0000000000000000000000000000000000000000..3606640a5468e02a6e8b2dae060b440640a70bbe --- /dev/null +++ b/src/tests/actuators/test-servo.cpp @@ -0,0 +1,101 @@ +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Authors: Luca Erbetta, Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <actuators/Servo/Servo.h> +#include <miosix.h> +#include <scheduler/TaskScheduler.h> +#include <utils/ClockUtils.h> + +#include <iostream> + +/** + * The test uses 4 gpio pins: + * - PB4: TIM3-CH1 + * - PA7: TIM3-CH2 + * - PC8: TIM3-CH3 + * - PB7: TIM4-CH2 + */ + +using namespace Boardcore; +using namespace miosix; + +GpioPin pin1(GPIOB_BASE, 4); +GpioPin pin2(GPIOA_BASE, 7); +GpioPin pin3(GPIOC_BASE, 8); +GpioPin pin4(GPIOB_BASE, 7); + +Servo s1(TIM3, TimerUtils::Channel::CHANNEL_1); +Servo s2(TIM3, TimerUtils::Channel::CHANNEL_2); +Servo s3(TIM3, TimerUtils::Channel::CHANNEL_3); +Servo s4(TIM4, TimerUtils::Channel::CHANNEL_2); + +// Position to cycle through for the servo 1, 2 and 3 +float positions[] = {0, 0.5, 1.0}; +int lastPosition = 0; + +void moveServo() +{ + s1.setPosition(positions[lastPosition % 3]); + s2.setPosition(positions[(lastPosition + 1) % 3]); + s3.setPosition(positions[(lastPosition + 2) % 3]); + + lastPosition++; +} + +int main() +{ + pin1.mode(Mode::ALTERNATE); + pin1.alternateFunction(2); + pin2.mode(Mode::ALTERNATE); + pin2.alternateFunction(2); + pin3.mode(Mode::ALTERNATE); + pin3.alternateFunction(2); + pin4.mode(Mode::ALTERNATE); + pin4.alternateFunction(2); + + // Enable the timers + s1.enable(); + s2.enable(); + s3.enable(); + s4.enable(); + + // Start a periodic task to move the first three servos + TaskScheduler scheduler; + scheduler.addTask(&moveServo, 2 * 1000, 1); + scheduler.start(); + + // Control the fourth servo manually + float percentage; + + printf("Please enter a percentage for the servo "); + printf("or anything else to exit: "); + + while (std::cin >> percentage) + { + s4.setPosition(percentage / 100); + + printf("Position set to: %.2f%%\n", percentage); + + printf("Please enter a percentage for the servo "); + printf("or anything else to exit: "); + } +} diff --git a/src/tests/drivers/stepper/test-stepper.cpp b/src/tests/actuators/test-stepper.cpp similarity index 99% rename from src/tests/drivers/stepper/test-stepper.cpp rename to src/tests/actuators/test-stepper.cpp index 3b27f5f0f565a98e6c41882148f92e31f2ef2ae2..707f227892df7545e5aec8b6a9ef174322ed8e46 100644 --- a/src/tests/drivers/stepper/test-stepper.cpp +++ b/src/tests/actuators/test-stepper.cpp @@ -20,7 +20,7 @@ * THE SOFTWARE. */ -#include <drivers/stepper/Stepper.h> +#include <actuators/Stepper.h> using namespace miosix; using namespace Boardcore; @@ -125,4 +125,4 @@ int main() doRoutine(stepper); printf("The stepper is now disabled\n"); } -} \ No newline at end of file +} diff --git a/src/tests/kalman/test-kalman-data.h b/src/tests/algorithms/Kalman/test-kalman-data.h similarity index 100% rename from src/tests/kalman/test-kalman-data.h rename to src/tests/algorithms/Kalman/test-kalman-data.h diff --git a/src/tests/kalman/test-kalman-eigen-benchmark.cpp b/src/tests/algorithms/Kalman/test-kalman-eigen-benchmark.cpp similarity index 95% rename from src/tests/kalman/test-kalman-eigen-benchmark.cpp rename to src/tests/algorithms/Kalman/test-kalman-eigen-benchmark.cpp index 33d468abe355c658e0903c5ec7c5ca845241332c..7b47c1b28b1e716ed826ce55c6ae9787b6e9e106 100644 --- a/src/tests/kalman/test-kalman-eigen-benchmark.cpp +++ b/src/tests/algorithms/Kalman/test-kalman-eigen-benchmark.cpp @@ -25,12 +25,12 @@ #define EIGEN_RUNTIME_NO_MALLOC -#include <algorithms/kalman/KalmanEigen.h> +#include <algorithms/Kalman/Kalman.h> #include <drivers/timer/GeneralPurposeTimer.h> #include <drivers/timer/TimerUtils.h> #include <kernel/kernel.h> -#include <math/SkyQuaternion.h> #include <util/util.h> +#include <utils/SkyQuaternion/SkyQuaternion.h> #include <iostream> @@ -71,7 +71,7 @@ int main() Matrix<float, p, 1> y(p); // vector with p elements (only one in this case) - KalmanEigen<float, n, p>::KalmanConfig config; + Kalman<float, n, p>::KalmanConfig config; config.F = F; config.H = H; config.Q = Q; @@ -79,7 +79,7 @@ int main() config.P = P; config.x = x0; - KalmanEigen<float, n, p> filter(config); + Kalman<float, n, p> filter(config); float lastTime = 0.0; // Variable to save the time of the last sample diff --git a/src/tests/algorithms/NAS/test-attitude-parafoil.cpp b/src/tests/algorithms/NAS/test-attitude-parafoil.cpp new file mode 100644 index 0000000000000000000000000000000000000000..4f8af2b692b965d8873f721c62d57308931475a8 --- /dev/null +++ b/src/tests/algorithms/NAS/test-attitude-parafoil.cpp @@ -0,0 +1,141 @@ +/* Copyright (c) 2022 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <algorithms/NAS/NAS.h> +#include <algorithms/NAS/StateInitializer.h> +#include <miosix.h> +#include <sensors/MPU9250/MPU9250.h> +#include <sensors/SensorManager.h> +#include <utils/SkyQuaternion/SkyQuaternion.h> + +#include <iostream> + +using namespace miosix; +using namespace Boardcore; +using namespace Eigen; + +NASConfig getEKConfig(); +void setInitialOrientation(); +void imuInit(); +void imuStep(); + +Vector3f nedMag = Vector3f(0.4747, 0.0276, 0.8797); + +NAS* nas; + +SPIBus spi1(SPI1); +MPU9250* imu = nullptr; + +int main() +{ + imuInit(); + + nas = new NAS(getEKConfig()); + setInitialOrientation(); + + TaskScheduler scheduler; + scheduler.addTask(imuStep, 20); + scheduler.start(); + + while (true) + Thread::sleep(1000); +} + +NASConfig getEKConfig() +{ + NASConfig config; + + config.T = 0.02f; + config.SIGMA_BETA = 0.0001f; + config.SIGMA_W = 0.3f; + config.SIGMA_MAG = 0.1f; + config.SIGMA_GPS = 10.0f; + config.SIGMA_BAR = 4.3f; + config.SIGMA_POS = 10.0; + config.SIGMA_VEL = 10.0; + config.P_POS = 1.0f; + config.P_POS_VERTICAL = 10.0f; + config.P_VEL = 1.0f; + config.P_VEL_VERTICAL = 10.0f; + config.P_ATT = 0.01f; + config.P_BIAS = 0.01f; + config.SATS_NUM = 6.0f; + config.NED_MAG = nedMag; + + return config; +} + +void setInitialOrientation() +{ + Eigen::Matrix<float, 13, 1> x; + + // Set quaternions + Eigen::Vector4f q = SkyQuaternion::eul2quat({0, 0, 0}); + x(6) = q(0); + x(7) = q(1); + x(8) = q(2); + x(9) = q(3); + + nas->setX(x); +} + +void imuInit() +{ + imu = new MPU9250(spi1, sensors::mpu9250::cs::getPin()); + imu->init(); +} + +void imuStep() +{ + imu->sample(); + auto data = imu->getLastSample(); + Vector3f acceleration(data.accelerationX, data.accelerationY, + data.accelerationZ); + Vector3f angularVelocity(data.angularVelocityX, data.angularVelocityY, + data.angularVelocityZ); + Vector3f magneticField(data.magneticFieldX, data.magneticFieldY, + data.magneticFieldZ); + + // Calibration + { + Vector3f bias(0.0218462708018154, 0.0281755574886535, + 0.0264119470499244); + angularVelocity -= bias; + Vector3f offset(15.9850903462129, -15.6775071377074, -33.8438469147423); + magneticField -= offset; + magneticField = {magneticField[1], magneticField[0], -magneticField[2]}; + } + + acceleration.normalize(); + magneticField.normalize(); + + // Predict step + nas->predictGyro(angularVelocity); + + // Correct step + nas->correctMag(magneticField); + nas->correctAcc(acceleration); + + auto nasState = nas->getState(); + printf("w%fwa%fab%fbc%fc\n", nasState.qw, nasState.qx, nasState.qy, + nasState.qz); +} diff --git a/src/tests/algorithms/NAS/test-attitude-stack.cpp b/src/tests/algorithms/NAS/test-attitude-stack.cpp new file mode 100644 index 0000000000000000000000000000000000000000..beacab318c26c6005805ca9f7bc24aa7e1147e6b --- /dev/null +++ b/src/tests/algorithms/NAS/test-attitude-stack.cpp @@ -0,0 +1,167 @@ +/* Copyright (c) 2022 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <algorithms/NAS/NAS.h> +#include <algorithms/NAS/StateInitializer.h> +#include <miosix.h> +#include <sensors/BMX160/BMX160.h> +#include <sensors/SensorManager.h> +#include <utils/SkyQuaternion/SkyQuaternion.h> + +#include <iostream> + +using namespace miosix; +using namespace Boardcore; +using namespace Eigen; + +NASConfig getEKConfig(); +void setInitialOrientation(); +void imuInit(); +void imuStep(); + +Vector3f nedMag = Vector3f(0.4747, 0.0276, 0.8797); + +NAS* nas; + +SPIBus spi1(SPI1); +BMX160* imu = nullptr; + +int main() +{ + imuInit(); + + nas = new NAS(getEKConfig()); + setInitialOrientation(); + + TaskScheduler scheduler; + scheduler.addTask(imuStep, 20); + scheduler.start(); + + while (true) + Thread::sleep(1000); +} + +NASConfig getEKConfig() +{ + NASConfig config; + + config.T = 0.02f; + config.SIGMA_BETA = 0.0001f; + config.SIGMA_W = 0.3f; + config.SIGMA_MAG = 0.1f; + config.SIGMA_GPS = 10.0f; + config.SIGMA_BAR = 4.3f; + config.SIGMA_POS = 10.0; + config.SIGMA_VEL = 10.0; + config.P_POS = 1.0f; + config.P_POS_VERTICAL = 10.0f; + config.P_VEL = 1.0f; + config.P_VEL_VERTICAL = 10.0f; + config.P_ATT = 0.01f; + config.P_BIAS = 0.01f; + config.SATS_NUM = 6.0f; + config.NED_MAG = nedMag; + + return config; +} + +void setInitialOrientation() +{ + Eigen::Matrix<float, 13, 1> x; + + // Set quaternions + Eigen::Vector4f q = SkyQuaternion::eul2quat({0, 0, 0}); + x(6) = q(0); + x(7) = q(1); + x(8) = q(2); + x(9) = q(3); + + nas->setX(x); +} + +void imuInit() +{ + SPIBusConfig spiConfig; + spiConfig.clockDivider = SPI::ClockDivider::DIV_8; + + BMX160Config bmx_config; + bmx_config.fifoMode = BMX160Config::FifoMode::HEADER; + bmx_config.fifoWatermark = 80; + bmx_config.fifoInterrupt = BMX160Config::FifoInterruptPin::PIN_INT1; + + bmx_config.temperatureDivider = 1; + + bmx_config.accelerometerRange = BMX160Config::AccelerometerRange::G_16; + + bmx_config.gyroscopeRange = BMX160Config::GyroscopeRange::DEG_1000; + + bmx_config.accelerometerDataRate = BMX160Config::OutputDataRate::HZ_100; + bmx_config.gyroscopeDataRate = BMX160Config::OutputDataRate::HZ_100; + bmx_config.magnetometerRate = BMX160Config::OutputDataRate::HZ_100; + + bmx_config.gyroscopeUnit = BMX160Config::GyroscopeMeasureUnit::RAD; + + imu = new BMX160(spi1, miosix::sensors::bmx160::cs::getPin(), bmx_config, + spiConfig); + imu->init(); +} + +void imuStep() +{ + imu->sample(); + auto data = imu->getLastSample(); + Vector3f acceleration(data.accelerationX, data.accelerationY, + data.accelerationZ); + Vector3f angularVelocity(data.angularVelocityX, data.angularVelocityY, + data.angularVelocityZ); + Vector3f magneticField(data.magneticFieldX, data.magneticFieldY, + data.magneticFieldZ); + + // Calibration + { + Vector3f offset{-1.63512255486542, 3.46523431469979, -3.08516033954451}; + angularVelocity = angularVelocity - offset; + angularVelocity = angularVelocity / 180 * Constants::PI / 10; + Vector3f b{21.5356818859811, -22.7697302909894, -2.68219304319269}; + Matrix3f A{{0.688760050772712, 0, 0}, + {0, 0.637715211784480, 0}, + {0, 0, 2.27669720320908}}; + magneticField = (magneticField - b).transpose() * A; + } + + acceleration.normalize(); + magneticField.normalize(); + + // Predict step + nas->predictGyro(angularVelocity); + + // Correct step + nas->correctMag(magneticField); + nas->correctAcc(acceleration); + + // std::cout << acceleration.transpose() << angularVelocity.transpose() + // << magneticField.transpose() << std::endl; + + auto nasState = nas->getState(); + printf("w%fwa%fab%fbc%fc\n", nasState.qw, nasState.qx, nasState.qy, + nasState.qz); +} diff --git a/src/tests/algorithms/NAS/test-nas-parafoil.cpp b/src/tests/algorithms/NAS/test-nas-parafoil.cpp new file mode 100644 index 0000000000000000000000000000000000000000..57fd167c839ec3bdef2d24c69417ddb7f118aab1 --- /dev/null +++ b/src/tests/algorithms/NAS/test-nas-parafoil.cpp @@ -0,0 +1,183 @@ +/* Copyright (c) 2022 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <algorithms/NAS/NAS.h> +#include <algorithms/NAS/StateInitializer.h> +#include <logger/Logger.h> +#include <miosix.h> +#include <sensors/MPU9250/MPU9250.h> +#include <sensors/SensorManager.h> +#include <sensors/UbloxGPS/UbloxGPS.h> +#include <utils/AeroUtils/AeroUtils.h> +#include <utils/SkyQuaternion/SkyQuaternion.h> + +#include <iostream> + +using namespace miosix; +using namespace Boardcore; +using namespace Eigen; + +NASConfig getEKConfig(); +void setInitialOrientation(); +void init(); +void step(); +void print(); + +Vector3f nedMag = Vector3f(0.4747, 0.0276, 0.8797); +Vector2f startPos = Vector2f(45.501141, 9.156281); + +NAS* nas; + +SPIBus spi1(SPI1); +MPU9250* imu = nullptr; +UbloxGPS* gps = nullptr; + +int main() +{ + init(); + + nas = new NAS(getEKConfig()); + setInitialOrientation(); + + TaskScheduler scheduler; + scheduler.addTask(step, 20, TaskScheduler::Policy::RECOVER); + scheduler.addTask(print, 250); + scheduler.start(); + + while (true) + Thread::sleep(1000); +} + +NASConfig getEKConfig() +{ + NASConfig config; + + config.T = 0.02f; + config.SIGMA_BETA = 0.0001f; + config.SIGMA_W = 0.3f; + config.SIGMA_MAG = 0.1f; + config.SIGMA_GPS = 10.0f; + config.SIGMA_BAR = 4.3f; + config.SIGMA_POS = 10.0; + config.SIGMA_VEL = 10.0; + config.P_POS = 1.0f; + config.P_POS_VERTICAL = 10.0f; + config.P_VEL = 1.0f; + config.P_VEL_VERTICAL = 10.0f; + config.P_ATT = 0.01f; + config.P_BIAS = 0.01f; + config.SATS_NUM = 6.0f; + config.NED_MAG = nedMag; + + return config; +} + +void setInitialOrientation() +{ + Matrix<float, 13, 1> x = Matrix<float, 13, 1>::Zero(); + + // Set quaternions + Vector4f q = SkyQuaternion::eul2quat({0, 0, 0}); + x(6) = q(0); + x(7) = q(1); + x(8) = q(2); + x(9) = q(3); + + nas->setX(x); +} + +void init() +{ + imu = new MPU9250(spi1, sensors::mpu9250::cs::getPin()); + imu->init(); + + gps = new UbloxGPS(38400, 10, 2, "gps", 38400); + gps->init(); + gps->start(); + + Logger::getInstance().start(); +} + +void step() +{ + imu->sample(); + gps->sample(); + auto imuData = imu->getLastSample(); + auto gpsData = gps->getLastSample(); + + Vector3f acceleration(imuData.accelerationX, imuData.accelerationY, + imuData.accelerationZ); + Vector3f angularVelocity(imuData.angularVelocityX, imuData.angularVelocityY, + imuData.angularVelocityZ); + Vector3f magneticField(imuData.magneticFieldX, imuData.magneticFieldY, + imuData.magneticFieldZ); + + Vector2f gpsPos(gpsData.latitude, gpsData.longitude); + gpsPos = Aeroutils::geodetic2NED(gpsPos, startPos); + Vector2f gpsVel(gpsData.velocityNorth, gpsData.velocityNorth); + Vector4f gpsCorrection; + // cppcheck-suppress constStatement + gpsCorrection << gpsPos, gpsVel; + + // Calibration + { + Vector3f biasAcc(-0.1255, 0.2053, -0.2073); + acceleration -= biasAcc; + Vector3f bias(-0.0291, 0.0149, 0.0202); + angularVelocity -= bias; + Vector3f offset(15.9850903462129, -15.6775071377074, -33.8438469147423); + magneticField -= offset; + magneticField = {magneticField[1], magneticField[0], -magneticField[2]}; + } + + // Predict step + nas->predictGyro(angularVelocity); + if (gpsPos[0] < 1e3 && gpsPos[0] > -1e3 && gpsPos[1] < 1e3 && + gpsPos[1] > -1e3) + nas->predictAcc(acceleration); + + // Correct step + magneticField.normalize(); + nas->correctMag(magneticField); + acceleration.normalize(); + nas->correctAcc(acceleration); + if (gpsData.fix) + nas->correctGPS(gpsCorrection); + nas->correctBaro(100000, 110000, 20 + 273.5); + + auto nasState = nas->getState(); + Logger::getInstance().log(imuData); + Logger::getInstance().log(gpsData); + Logger::getInstance().log(nasState); +} + +void print() +{ + auto gpsData = gps->getLastSample(); + auto nasState = nas->getState(); + + Vector2f gpsPos(gpsData.latitude, gpsData.longitude); + gpsPos = Aeroutils::geodetic2NED(gpsPos, startPos); + + printf("%d, %f, %f, %f, %f, %f, %f\n", gpsData.fix, gpsPos[0], gpsPos[1], + nasState.n, nasState.e, nasState.vn, nasState.ve); +} diff --git a/src/tests/algorithms/NAS/test-nas-stack.cpp b/src/tests/algorithms/NAS/test-nas-stack.cpp new file mode 100644 index 0000000000000000000000000000000000000000..e72a1c9830b63682cffff7968cbc27106b85a61f --- /dev/null +++ b/src/tests/algorithms/NAS/test-nas-stack.cpp @@ -0,0 +1,216 @@ +/* Copyright (c) 2022 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <algorithms/NAS/NAS.h> +#include <algorithms/NAS/StateInitializer.h> +#include <logger/Logger.h> +#include <miosix.h> +#include <sensors/BMX160/BMX160.h> +#include <sensors/MS5803/MS5803.h> +#include <sensors/SensorManager.h> +#include <sensors/UbloxGPS/UbloxGPS.h> +#include <utils/AeroUtils/AeroUtils.h> +#include <utils/Constants.h> +#include <utils/SkyQuaternion/SkyQuaternion.h> + +#include <iostream> + +using namespace miosix; +using namespace Boardcore; +using namespace Eigen; + +NASConfig getEKConfig(); +void setInitialOrientation(); +void init(); +void step(); +void print(); + +Vector3f nedMag = Vector3f(0.4747, 0.0276, 0.8797); +Vector2f startPos = Vector2f(45.501141, 9.156281); + +NAS* nas; + +SPIBus spi1(SPI1); +BMX160* imu = nullptr; +UbloxGPS* gps = nullptr; +MS5803* baro = nullptr; + +int main() +{ + init(); + + nas = new NAS(getEKConfig()); + setInitialOrientation(); + + TaskScheduler scheduler; + scheduler.addTask(step, 20, TaskScheduler::Policy::RECOVER); + // scheduler.addTask(print, 250); + scheduler.start(); + + while (true) + Thread::sleep(1000); +} + +NASConfig getEKConfig() +{ + NASConfig config; + + config.T = 0.02f; + config.SIGMA_BETA = 0.0001f; + config.SIGMA_W = 0.3f; + config.SIGMA_MAG = 0.1f; + config.SIGMA_GPS = 10.0f; + config.SIGMA_BAR = 4.3f; + config.SIGMA_POS = 10.0; + config.SIGMA_VEL = 10.0; + config.P_POS = 1.0f; + config.P_POS_VERTICAL = 10.0f; + config.P_VEL = 1.0f; + config.P_VEL_VERTICAL = 10.0f; + config.P_ATT = 0.01f; + config.P_BIAS = 0.01f; + config.SATS_NUM = 6.0f; + config.NED_MAG = nedMag; + + return config; +} + +void setInitialOrientation() +{ + Matrix<float, 13, 1> x = Matrix<float, 13, 1>::Zero(); + + // Set quaternions + Vector4f q = SkyQuaternion::eul2quat({0, 0, 0}); + x(6) = q(0); + x(7) = q(1); + x(8) = q(2); + x(9) = q(3); + + nas->setX(x); +} + +void init() +{ + SPIBusConfig spiConfig; + spiConfig.clockDivider = SPI::ClockDivider::DIV_8; + + BMX160Config bmx_config; + bmx_config.fifoMode = BMX160Config::FifoMode::HEADER; + bmx_config.fifoWatermark = 80; + bmx_config.fifoInterrupt = BMX160Config::FifoInterruptPin::PIN_INT1; + + bmx_config.temperatureDivider = 1; + + bmx_config.accelerometerRange = BMX160Config::AccelerometerRange::G_16; + + bmx_config.gyroscopeRange = BMX160Config::GyroscopeRange::DEG_1000; + + bmx_config.accelerometerDataRate = BMX160Config::OutputDataRate::HZ_100; + bmx_config.gyroscopeDataRate = BMX160Config::OutputDataRate::HZ_100; + bmx_config.magnetometerRate = BMX160Config::OutputDataRate::HZ_100; + + bmx_config.gyroscopeUnit = BMX160Config::GyroscopeMeasureUnit::RAD; + + imu = + new BMX160(spi1, sensors::bmx160::cs::getPin(), bmx_config, spiConfig); + imu->init(); + + gps = new UbloxGPS(921600, 10, 2, "gps", 38400); + gps->init(); + gps->start(); + + baro = new MS5803(spi1, sensors::ms5803::cs::getPin()); + baro->init(); + + Logger::getInstance().start(); +} + +void step() +{ + imu->sample(); + gps->sample(); + baro->sample(); + auto imuData = imu->getLastSample(); + auto gpsData = gps->getLastSample(); + auto baroData = baro->getLastSample(); + + Vector3f acceleration(imuData.accelerationX, imuData.accelerationY, + imuData.accelerationZ); + Vector3f angularVelocity(imuData.angularVelocityX, imuData.angularVelocityY, + imuData.angularVelocityZ); + Vector3f magneticField(imuData.magneticFieldX, imuData.magneticFieldY, + imuData.magneticFieldZ); + + Vector2f gpsPos(gpsData.latitude, gpsData.longitude); + gpsPos = Aeroutils::geodetic2NED(gpsPos, startPos); + Vector2f gpsVel(gpsData.velocityNorth, gpsData.velocityEast); + Vector4f gpsCorrection; + // cppcheck-suppress constStatement + gpsCorrection << gpsPos, gpsVel; + + // Calibration + { + Vector3f offset{-1.63512255486542, 3.46523431469979, -3.08516033954451}; + angularVelocity = angularVelocity - offset; + angularVelocity = angularVelocity / 180 * Constants::PI / 10; + Vector3f b{21.5356818859811, -22.7697302909894, -2.68219304319269}; + Matrix3f A{{0.688760050772712, 0, 0}, + {0, 0.637715211784480, 0}, + {0, 0, 2.27669720320908}}; + magneticField = (magneticField - b).transpose() * A; + } + + // Predict step + nas->predictGyro(angularVelocity); + nas->predictAcc(acceleration); + + // Correct step + magneticField.normalize(); + nas->correctMag(magneticField); + acceleration.normalize(); + nas->correctAcc(acceleration); + if (gpsData.fix) + nas->correctGPS(gpsCorrection); + nas->correctBaro(100000, Constants::MSL_PRESSURE, + Constants::MSL_TEMPERATURE); + + auto nasState = nas->getState(); + + Logger::getInstance().log(imuData); + Logger::getInstance().log(gpsData); + Logger::getInstance().log(baroData); + Logger::getInstance().log(nasState); +} + +void print() +{ + auto gpsData = gps->getLastSample(); + auto baroData = baro->getLastSample(); + auto nasState = nas->getState(); + + Vector2f gpsPos(gpsData.latitude, gpsData.longitude); + gpsPos = Aeroutils::geodetic2NED(gpsPos, startPos); + + printf("%d, %f, %f, %f, %f, %f, %f, %f, %f\n", gpsData.fix, gpsPos[0], + gpsPos[1], nasState.n, nasState.e, nasState.d, nasState.vn, + nasState.ve, baroData.pressure); +} diff --git a/src/tests/algorithms/NAS/test-nas-with-triad.cpp b/src/tests/algorithms/NAS/test-nas-with-triad.cpp new file mode 100644 index 0000000000000000000000000000000000000000..59baeb1d1e37a36035e2fe6208ec22617ea8ad2d --- /dev/null +++ b/src/tests/algorithms/NAS/test-nas-with-triad.cpp @@ -0,0 +1,213 @@ +/* Copyright (c) 2022 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <algorithms/NAS/NAS.h> +#include <algorithms/NAS/StateInitializer.h> +#include <miosix.h> +#include <sensors/BMX160/BMX160.h> +#include <sensors/SensorManager.h> +#include <utils/SkyQuaternion/SkyQuaternion.h> + +#include <iostream> + +using namespace miosix; +using namespace Boardcore; +using namespace Eigen; + +NASConfig getEKConfig(); +void bmxInit(); +void bmxCallback(); + +constexpr uint64_t CALIBRATION_TIMEOUT = 5 * 1e6; + +Vector3f nedMag = Vector3f(0.47472049, 0.02757190, 0.87970463); + +StateInitializer* stateInitializer; +NAS* nas; + +SPIBus spi1(SPI1); +BMX160* bmx160 = nullptr; + +Boardcore::SensorManager::SensorMap_t sensorsMap; +Boardcore::SensorManager* sensorManager = nullptr; + +int main() +{ + bmxInit(); + + sensorManager = new SensorManager(sensorsMap); + stateInitializer = new StateInitializer(); + nas = new NAS(getEKConfig()); + + // Logger::getInstance().start(); + TimestampTimer::getInstance().resetTimestamp(); + sensorManager->start(); + + while (true) + Thread::sleep(1000); +} + +NASConfig getEKConfig() +{ + NASConfig config; + + config.T = 0.02f; + config.SIGMA_BETA = 0.0001f; + config.SIGMA_W = 0.3f; + config.SIGMA_MAG = 0.1f; + config.SIGMA_GPS = 10.0f; + config.SIGMA_BAR = 4.3f; + config.SIGMA_POS = 10.0; + config.SIGMA_VEL = 10.0; + config.P_POS = 1.0f; + config.P_POS_VERTICAL = 10.0f; + config.P_VEL = 1.0f; + config.P_VEL_VERTICAL = 10.0f; + config.P_ATT = 0.01f; + config.P_BIAS = 0.01f; + config.SATS_NUM = 6.0f; + + // Normalized magnetic field in Milan + config.NED_MAG = nedMag; + + return config; +} + +void bmxInit() +{ + SPIBusConfig spiConfig; + spiConfig.clockDivider = SPI::ClockDivider::DIV_8; + + BMX160Config bmx_config; + bmx_config.fifoMode = BMX160Config::FifoMode::HEADER; + bmx_config.fifoWatermark = 80; + bmx_config.fifoInterrupt = BMX160Config::FifoInterruptPin::PIN_INT1; + bmx_config.temperatureDivider = 1; + bmx_config.accelerometerRange = BMX160Config::AccelerometerRange::G_16; + bmx_config.gyroscopeRange = BMX160Config::GyroscopeRange::DEG_1000; + bmx_config.accelerometerDataRate = BMX160Config::OutputDataRate::HZ_100; + bmx_config.gyroscopeDataRate = BMX160Config::OutputDataRate::HZ_100; + bmx_config.magnetometerRate = BMX160Config::OutputDataRate::HZ_100; + bmx_config.gyroscopeUnit = BMX160Config::GyroscopeMeasureUnit::RAD; + + bmx160 = new BMX160(spi1, miosix::sensors::bmx160::cs::getPin(), bmx_config, + spiConfig); + + SensorInfo info("BMX160", 20, &bmxCallback); + + sensorsMap.emplace(std::make_pair(bmx160, info)); +} + +void bmxCallback() +{ + static int meanCount = 0; + static bool calibrating = true; + static Vector3f accMean = Vector3f::Zero(); + static Vector3f magMean = Vector3f::Zero(); + + auto data = bmx160->getLastSample(); + Vector3f acceleration(data.accelerationX, data.accelerationY, + data.accelerationZ); + Vector3f angularVelocity(data.angularVelocityX, data.angularVelocityY, + data.angularVelocityZ); + Vector3f magneticField(data.magneticFieldX, data.magneticFieldY, + data.magneticFieldZ); + + // Apply correction + Vector3f acc_b(0.3763 + 0.094, -0.1445 - 0.0229, -0.1010 + 0.0150); + acceleration -= acc_b; + Vector3f gyro_b{-1.63512255486542, 3.46523431469979, -3.08516033954451}; + angularVelocity = angularVelocity - gyro_b; + angularVelocity = angularVelocity / 180 * Constants::PI / 10; + Vector3f mag_b{21.0730033648425, -24.3997259703105, -2.32621524742862}; + Matrix3f A{{0.659926504672263, 0, 0}, + {0, 0.662442130094073, 0}, + {0, 0, 2.28747567094359}}; + magneticField = (magneticField - mag_b).transpose() * A; + + if (calibrating) + { + if (TimestampTimer::getInstance().getTimestamp() < CALIBRATION_TIMEOUT) + { + accMean = (accMean * meanCount + acceleration) / (meanCount + 1); + magMean = (magMean * meanCount + acceleration) / (meanCount + 1); + meanCount++; + } + else + { + // Now the calibration has ended, compute and log the nas state + calibrating = false; + + // Compute the initial nas state + stateInitializer->triad(accMean, magMean, nedMag); + nas->setX(stateInitializer->getInitX()); + + // Save the state and the IMU data + // Logger::getInstance().log(nas->getState()); + data.accelerationX = accMean[0]; + data.accelerationY = accMean[1]; + data.accelerationZ = accMean[2]; + data.magneticFieldX = magMean[0]; + data.magneticFieldY = magMean[1]; + data.magneticFieldZ = magMean[2]; + // Logger::getInstance().log(data); + + // Restart the logger to change log filename + // Logger::getInstance().stop(); + // Logger::getInstance().start(); + } + } + else + { + // Predict step + { + // nas->predictAcc(acceleration); + nas->predictGyro(angularVelocity); + + data.angularVelocityX = angularVelocity[0]; + data.angularVelocityY = angularVelocity[1]; + data.angularVelocityZ = angularVelocity[2]; + } + + // Correct step + { + magneticField.normalize(); + nas->correctMag(magneticField); + + data.magneticFieldX = magneticField[0]; + data.magneticFieldY = magneticField[1]; + data.magneticFieldZ = magneticField[2]; + } + + auto nasState = nas->getState(); + + nasState.timestamp = TimestampTimer::getInstance().getTimestamp(); + data.accelerationTimestamp = nasState.timestamp; + data.magneticFieldTimestamp = nasState.timestamp; + data.angularVelocityTimestamp = nasState.timestamp; + + // Logger::getInstance().log(nasState); + // Logger::getInstance().log(data); + printf("w%fwa%fab%fbc%fc\n", nasState.qw, nasState.qx, nasState.qy, + nasState.qz); + } +} diff --git a/src/tests/algorithms/NAS/test-tmp.cpp b/src/tests/algorithms/NAS/test-tmp.cpp new file mode 100644 index 0000000000000000000000000000000000000000..39fcdbd511705c39361f2a926ef92c15ea662ff9 --- /dev/null +++ b/src/tests/algorithms/NAS/test-tmp.cpp @@ -0,0 +1,89 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <drivers/spi/SPIDriver.h> +#include <drivers/timer/TimestampTimer.h> +#include <miosix.h> +#include <sensors/BMX160/BMX160.h> +#include <sensors/MPU9250/MPU9250.h> +#include <utils/Debug.h> + +using namespace miosix; + +using namespace Boardcore; + +GpioPin mpuCs = GpioPin(GPIOG_BASE, 3); +SPIBus spi1(SPI1); + +MPU9250* mpu = nullptr; +BMX160* bmx = nullptr; + +void initBoard() +{ + // Chip select pin as output starting high + mpuCs.mode(miosix::Mode::OUTPUT); + mpuCs.high(); + + mpu = new MPU9250(spi1, mpuCs); + + SPIBusConfig spiConfig; + spiConfig.clockDivider = SPI::ClockDivider::DIV_8; + + BMX160Config bmxConfig; + bmxConfig.fifoMode = BMX160Config::FifoMode::HEADER; + bmxConfig.fifoWatermark = 80; + bmxConfig.fifoInterrupt = BMX160Config::FifoInterruptPin::PIN_INT1; + bmxConfig.temperatureDivider = 1; + bmxConfig.accelerometerRange = BMX160Config::AccelerometerRange::G_16; + bmxConfig.gyroscopeRange = BMX160Config::GyroscopeRange::DEG_1000; + bmxConfig.accelerometerDataRate = BMX160Config::OutputDataRate::HZ_100; + bmxConfig.gyroscopeDataRate = BMX160Config::OutputDataRate::HZ_100; + bmxConfig.magnetometerRate = BMX160Config::OutputDataRate::HZ_100; + bmxConfig.gyroscopeUnit = BMX160Config::GyroscopeMeasureUnit::RAD; + bmx = new BMX160(spi1, miosix::sensors::bmx160::cs::getPin(), bmxConfig, + spiConfig); + + mpu->init(); + bmx->init(); +} + +int main() +{ + initBoard(); + + auto lastTick = getTick(); + while (true) + { + mpu->sample(); + bmx->sample(); + auto mpuData = mpu->getLastSample(); + auto bmxData = bmx->getLastSample(); + + printf("%f,%f,%f,", mpuData.magneticFieldX, mpuData.magneticFieldY, + mpuData.magneticFieldZ); + printf("%f,%f,%f\n", bmxData.magneticFieldX, bmxData.magneticFieldY, + bmxData.magneticFieldZ); + + Thread::sleepUntil(lastTick + 20); + lastTick = getTick(); + } +} diff --git a/src/tests/algorithms/NAS/test-triad-parafoil.cpp b/src/tests/algorithms/NAS/test-triad-parafoil.cpp new file mode 100644 index 0000000000000000000000000000000000000000..ed93635ffcb04004de8ce7810051b48a21bb08bb --- /dev/null +++ b/src/tests/algorithms/NAS/test-triad-parafoil.cpp @@ -0,0 +1,79 @@ +/* Copyright (c) 2022 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <algorithms/NAS/StateInitializer.h> +#include <miosix.h> +#include <sensors/MPU9250/MPU9250.h> +#include <sensors/SensorManager.h> +#include <sensors/calibration/SensorDataExtra.h> +#include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h> + +#include <cmath> +#include <iostream> + +using namespace miosix; +using namespace Boardcore; +using namespace Eigen; + +void mpuInit(); +void bmxCallback(); + +Vector3f nedMag = Vector3f(0.4747, 0.0276, 0.8797); + +SPIBus spi1(SPI1); +MPU9250* mpu = nullptr; + +int main() +{ + mpu = new MPU9250(spi1, sensors::mpu9250::cs::getPin()); + mpu->init(); + + auto lastTick = getTick(); + while (true) + { + mpu->sample(); + auto data = mpu->getLastSample(); + + Vector3f acceleration(data.accelerationX, data.accelerationY, + data.accelerationZ); + + Vector3f offset(15.9850903462129, -15.6775071377074, -33.8438469147423); + Vector3f magneticField(data.magneticFieldX, data.magneticFieldY, + data.magneticFieldZ); + magneticField -= offset; + magneticField = {magneticField[1], magneticField[0], -magneticField[2]}; + + acceleration.normalize(); + magneticField.normalize(); + + StateInitializer state; + state.triad(acceleration, magneticField, nedMag); + + auto kalmanState = state.getInitX(); + if (!std::isnan(kalmanState(9))) + printf("w%fwa%fab%fbc%fc\n", kalmanState(9), kalmanState(6), + kalmanState(7), kalmanState(8)); + + Thread::sleepUntil(lastTick + 20); + lastTick = getTick(); + } +} diff --git a/src/tests/algorithms/NAS/test-triad.cpp b/src/tests/algorithms/NAS/test-triad.cpp new file mode 100644 index 0000000000000000000000000000000000000000..f5104a1e0f2cb838a1b14f7b58ec072fbabcdcab --- /dev/null +++ b/src/tests/algorithms/NAS/test-triad.cpp @@ -0,0 +1,110 @@ +/* Copyright (c) 2022 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <algorithms/NAS/StateInitializer.h> +#include <miosix.h> +#include <sensors/BMX160/BMX160.h> +#include <sensors/SensorManager.h> +#include <sensors/calibration/SensorDataExtra.h> +#include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h> + +#include <cmath> +#include <iostream> + +using namespace miosix; +using namespace Boardcore; +using namespace Eigen; + +void imuInit(); + +Vector3f nedMag = Vector3f(0.4747, 0.0276, 0.8797); + +SPIBus spi1(SPI1); +BMX160* bmx = nullptr; + +int main() +{ + imuInit(); + bmx->init(); + + auto lastTick = getTick(); + while (true) + { + bmx->sample(); + auto data = bmx->getLastSample(); + + Vector3f acceleration(data.accelerationX, data.accelerationY, + data.accelerationZ); + Vector3f angularVelocity(data.angularVelocityX, data.angularVelocityY, + data.angularVelocityZ); + Vector3f offset{-1.63512255486542, 3.46523431469979, -3.08516033954451}; + angularVelocity = angularVelocity - offset; + angularVelocity = angularVelocity / 180 * Constants::PI / 10; + Vector3f magneticField(data.magneticFieldX, data.magneticFieldY, + data.magneticFieldZ); + Vector3f b{21.5356818859811, -22.7697302909894, -2.68219304319269}; + Matrix3f A{{0.688760050772712, 0, 0}, + {0, 0.637715211784480, 0}, + {0, 0, 2.27669720320908}}; + magneticField = (magneticField - b).transpose() * A; + + acceleration.normalize(); + magneticField.normalize(); + + StateInitializer state; + state.triad(acceleration, magneticField, nedMag); + + auto kalmanState = state.getInitX(); + if (!std::isnan(kalmanState(9))) + printf("w%fwa%fab%fbc%fc\n", kalmanState(9), kalmanState(6), + kalmanState(7), kalmanState(8)); + + Thread::sleepUntil(lastTick + 20); + lastTick = getTick(); + } +} + +void imuInit() +{ + SPIBusConfig spiConfig; + spiConfig.clockDivider = SPI::ClockDivider::DIV_8; + + BMX160Config bmx_config; + bmx_config.fifoMode = BMX160Config::FifoMode::HEADER; + bmx_config.fifoWatermark = 80; + bmx_config.fifoInterrupt = BMX160Config::FifoInterruptPin::PIN_INT1; + + bmx_config.temperatureDivider = 1; + + bmx_config.accelerometerRange = BMX160Config::AccelerometerRange::G_16; + + bmx_config.gyroscopeRange = BMX160Config::GyroscopeRange::DEG_1000; + + bmx_config.accelerometerDataRate = BMX160Config::OutputDataRate::HZ_100; + bmx_config.gyroscopeDataRate = BMX160Config::OutputDataRate::HZ_100; + bmx_config.magnetometerRate = BMX160Config::OutputDataRate::HZ_100; + + bmx_config.gyroscopeUnit = BMX160Config::GyroscopeMeasureUnit::RAD; + + bmx = new BMX160(spi1, miosix::sensors::bmx160::cs::getPin(), bmx_config, + spiConfig); +} diff --git a/src/tests/catch/test-aero.cpp b/src/tests/catch/test-aero.cpp index d9542136f7a385267efb7039f95738b92212b784..6e8ba3a2d5603a3910323cbff12097e014dbff84 100644 --- a/src/tests/catch/test-aero.cpp +++ b/src/tests/catch/test-aero.cpp @@ -24,7 +24,7 @@ #include "catch-tests-entry.cpp" #endif -#include <utils/aero/AeroUtils.h> +#include <utils/AeroUtils/AeroUtils.h> #include <catch2/catch.hpp> @@ -32,7 +32,7 @@ using namespace Boardcore; TEST_CASE("[AeroUtils] mslTemperature") { - using namespace aeroutils; + using namespace Aeroutils; Approx isa_T0 = Approx(288.151).margin(0.001); // 15 deg celsius, 0.01% error allowed @@ -46,7 +46,7 @@ TEST_CASE("[AeroUtils] mslTemperature") TEST_CASE("[AeroUtils] mslPressure") { - using namespace aeroutils; + using namespace Aeroutils; Approx isa_P0 = Approx(101325).epsilon(0.0001); // 15 deg celsius, 0.01% error allowed @@ -61,7 +61,7 @@ TEST_CASE("[AeroUtils] mslPressure") float mslAltitude(float pressure, float pressureRef, float temperatureRef, float zRef) { - using namespace aeroutils; + using namespace Aeroutils; float t0 = mslTemperature(temperatureRef, zRef); return relAltitude(pressure, mslPressure(pressureRef, temperatureRef, zRef), @@ -84,7 +84,7 @@ TEST_CASE("[AeroUtils] relAltitude") TEST_CASE("[AeroUtils] relDensity") { - using namespace aeroutils; + using namespace Aeroutils; REQUIRE(relDensity(101325, 101325, 0, 288.15) == Approx(1.225).epsilon(0.0001)); @@ -100,7 +100,7 @@ TEST_CASE("[AeroUtils] relDensity") TEST_CASE("[AeroUtils] verticalSpeed") { - using namespace aeroutils; + using namespace Aeroutils; const int count = 5; float p[] = {100129.4, 99555.8, 89153.1, 23611.1, 101284.6}; diff --git a/src/tests/catch/test-eventbroker.cpp b/src/tests/catch/test-eventbroker.cpp index 3139a50ed78563fb01404f22ab88ae780139a2ad..18a9eb65c348d484a29c2c7823be366e4cc042d0 100644 --- a/src/tests/catch/test-eventbroker.cpp +++ b/src/tests/catch/test-eventbroker.cpp @@ -29,7 +29,7 @@ #include <events/EventBroker.h> #include <miosix.h> -#include <utils/testutils/TestHelper.h> +#include <utils/TestUtils/TestHelper.h> #include <catch2/catch.hpp> #include <cstdio> @@ -75,26 +75,26 @@ TEST_CASE("EventBroker - Posts to different topics") SECTION("Post event on TOPIC_1, only sub1 should receive it") { - ev.code = EV_A; + ev = EV_A; broker.post(ev, TOPIC_1); REQUIRE(sub1.getTotalCount() == 1); - REQUIRE(sub1.getLastEvent() == ev.code); + REQUIRE(sub1.getLastEvent() == ev); REQUIRE(sub2.getTotalCount() == 0); } SECTION("Post event on TOPIC_2, both sub1 and sub2 should receive it") { - ev.code = EV_B; + ev = EV_B; broker.post(ev, TOPIC_2); REQUIRE(sub1.getTotalCount() == 1); REQUIRE(sub2.getTotalCount() == 1); - REQUIRE(sub1.getLastEvent() == ev.code); - REQUIRE(sub2.getLastEvent() == ev.code); + REQUIRE(sub1.getLastEvent() == ev); + REQUIRE(sub2.getLastEvent() == ev); } SECTION("Post event on TOPIC_3, no one should receive it") { - ev.code = EV_C; + ev = EV_C; broker.post(ev, TOPIC_3); REQUIRE(sub1.getTotalCount() == 0); diff --git a/src/tests/catch/test-kalman-eigen.cpp b/src/tests/catch/test-kalman-eigen.cpp index fc2245c23deaeb8ca49337ab5afee9796fb2d642..71974bebedf31ff6790e7cc7104c9c76b5fd2538 100644 --- a/src/tests/catch/test-kalman-eigen.cpp +++ b/src/tests/catch/test-kalman-eigen.cpp @@ -26,12 +26,12 @@ #define EIGEN_RUNTIME_NO_MALLOC -#include <algorithms/kalman/KalmanEigen.h> +#include <algorithms/Kalman/Kalman.h> #include <catch2/catch.hpp> #include <iostream> -#include "../kalman/test-kalman-data.h" +#include "../algorithms/Kalman/test-kalman-data.h" using namespace Boardcore; using namespace Eigen; @@ -61,10 +61,10 @@ static const Matrix<float, OUTPUTS_DIM, OUTPUTS_DIM> R{10}; // State vector static const Matrix<float, STATES_DIM, 1> x0(INPUT[0], 0.0, 0.0); -static const KalmanEigen<float, STATES_DIM, OUTPUTS_DIM>::KalmanConfig +static const Kalman<float, STATES_DIM, OUTPUTS_DIM>::KalmanConfig getKalmanConfig() { - KalmanEigen<float, STATES_DIM, OUTPUTS_DIM>::KalmanConfig config; + Kalman<float, STATES_DIM, OUTPUTS_DIM>::KalmanConfig config; config.F = F; config.H = H; config.Q = Q; @@ -77,7 +77,7 @@ getKalmanConfig() TEST_CASE("Update test") { - KalmanEigen<float, STATES_DIM, OUTPUTS_DIM> filter(getKalmanConfig()); + Kalman<float, STATES_DIM, OUTPUTS_DIM> filter(getKalmanConfig()); Matrix<float, OUTPUTS_DIM, 1> y{}; float lastTime = TIME[0]; diff --git a/src/tests/catch/test-packetqueue.cpp b/src/tests/catch/test-packetqueue.cpp index 6d09548dc20e7b2cf7323c25d9b654919033c2c8..e0cf2865ac8da3f9dbc691dbcd583c2aaa79ec03 100644 --- a/src/tests/catch/test-packetqueue.cpp +++ b/src/tests/catch/test-packetqueue.cpp @@ -82,10 +82,12 @@ TEST_CASE("Packet tests") SECTION("Adding stuff to packet") { + // Add 5 bytes + REQUIRE(p.append(messageBase, 5)); + uint64_t ts = p.getTimestamp(); - REQUIRE(p.tryAppend(messageBase, 5)); - uint64_t ts = p.timestamp(); - REQUIRE(miosix::getTick() - ts < 5); + REQUIRE(Boardcore::TimestampTimer::getInstance().getTimestamp() - ts < + 5); REQUIRE(p.dump(buf) == 5); COMPARE(buf, BUF_LEN, "01234"); @@ -93,29 +95,21 @@ TEST_CASE("Packet tests") REQUIRE(p.size() == 5); REQUIRE(p.getMsgCount() == 1); - REQUIRE(p.tryAppend(messageBase + 5, 3)); + // Add 3 bytes + REQUIRE(p.append(messageBase + 5, 3)); REQUIRE(p.dump(buf) == 8); COMPARE(buf, BUF_LEN, "01234567"); REQUIRE(p.isEmpty() == false); REQUIRE(p.size() == 8); - REQUIRE_FALSE(p.tryAppend(messageBase + 8, 3)); - REQUIRE(p.dump(buf) == 8); - COMPARE(buf, BUF_LEN, "01234567"); - REQUIRE(p.isEmpty() == false); - REQUIRE(p.size() == 8); - REQUIRE(p.getMsgCount() == 2); - - REQUIRE(p.tryAppend(messageBase + 8, 2)); - REQUIRE(p.dump(buf) == 10); + // Trying to add 3 more bytes, only 2 should be written + REQUIRE(p.append(messageBase + 8, 3) == 2); + REQUIRE(p.dump(buf) == PKT_LEN); COMPARE(buf, BUF_LEN, "0123456789"); REQUIRE(p.isEmpty() == false); - REQUIRE(p.isFull()); - REQUIRE(p.size() == 10); + REQUIRE(p.size() == PKT_LEN); REQUIRE(p.getMsgCount() == 3); - REQUIRE(p.timestamp() == ts); - p.clear(); REQUIRE(p.isEmpty()); REQUIRE(p.isFull() == false); @@ -130,7 +124,7 @@ TEST_CASE("Packet tests") SECTION("Edge cases") { INFO("Adding empty msg"); - REQUIRE_FALSE(p.tryAppend(messageBase, 0)); + REQUIRE_FALSE(p.append(messageBase, 0)); REQUIRE(p.isEmpty()); REQUIRE(p.isFull() == false); REQUIRE(p.isReady() == false); @@ -141,29 +135,16 @@ TEST_CASE("Packet tests") REQUIRE(p.dump(buf) == 0); INFO("Adding too big msg"); - REQUIRE_FALSE(p.tryAppend(messageBase, PKT_LEN + 1)); - - REQUIRE(p.isEmpty()); - REQUIRE(p.isFull() == false); - REQUIRE(p.isReady() == false); - REQUIRE(p.size() == 0); - REQUIRE(p.maxSize() == PKT_LEN); + REQUIRE(p.append(messageBase, PKT_LEN + 1) == PKT_LEN); - REQUIRE(p.getMsgCount() == 0); - REQUIRE(p.dump(buf) == 0); - - INFO("Adding something to full packet") - REQUIRE(p.tryAppend(messageBase, PKT_LEN)); - REQUIRE_FALSE(p.tryAppend(messageBase, 1)); - - REQUIRE(p.isEmpty() == false); + REQUIRE_FALSE(p.isEmpty()); REQUIRE(p.isFull()); - REQUIRE(p.isReady() == false); + REQUIRE_FALSE(p.isReady()); REQUIRE(p.size() == PKT_LEN); REQUIRE(p.maxSize() == PKT_LEN); REQUIRE(p.getMsgCount() == 1); - REQUIRE(p.dump(buf) == 10); + REQUIRE(p.dump(buf) == PKT_LEN); COMPARE(buf, 10, "0123456789"); } } @@ -182,100 +163,142 @@ TEST_CASE("PacketQueue tests") SECTION("Normal operation") { INFO("Adding two elements to first packet"); - REQUIRE(pq.put(messageBase, 4) == 0); - REQUIRE(pq.put(messageBase, 4) == 0); + REQUIRE(pq.put(messageBase, 4)); + REQUIRE(pq.put(messageBase, 4)); + // No packet should be ready REQUIRE(pq.countReady() == 0); REQUIRE(pq.countNotEmpty() == 1); REQUIRE_FALSE(pq.isEmpty()); REQUIRE_FALSE(pq.isFull()); INFO("Adding third element and filling first packet"); - REQUIRE(pq.put(messageBase, 2) == 0); + REQUIRE(pq.put(messageBase, 2)); + // Now one single packet should be filled and ready REQUIRE(pq.countReady() == 1); REQUIRE(pq.countNotEmpty() == 1); REQUIRE_FALSE(pq.isEmpty()); REQUIRE_FALSE(pq.isFull()); + // Check the packet content Packet<PKT_LEN> p = pq.get(); REQUIRE(p.getMsgCount() == 3); REQUIRE(p.isFull()); REQUIRE(p.isReady()); COMPARE(p, "0123012301"); - INFO("Adding element to second packet"); - REQUIRE(pq.put(messageBase + 10, 4) == 0); + INFO("Adding more data to create a second packet"); + REQUIRE(pq.put(messageBase + 10, 4)); + // The second packet should not be ready REQUIRE(pq.countReady() == 1); REQUIRE(pq.countNotEmpty() == 2); REQUIRE_FALSE(pq.isEmpty()); REQUIRE_FALSE(pq.isFull()); REQUIRE_FALSE(pq.buffer.get(1).isReady()); - COMPARE(pq.buffer.get(1), "abcd"); p = pq.get(); // Should still return first packet REQUIRE(p.getMsgCount() == 3); - INFO( - "Adding element not fitting the second packet, added to the third"); - REQUIRE(pq.put(messageBase + 10, 7) == 0); + INFO("Adding more data to create a third packet"); + REQUIRE(pq.put(messageBase + 10, 7)); + p = pq.get(); // Should still return first packet REQUIRE(p.getMsgCount() == 3); - REQUIRE(pq.countReady() == 2); - REQUIRE(pq.countNotEmpty() == 3); - REQUIRE_FALSE(pq.isEmpty()); - REQUIRE_FALSE(pq.isFull()); - + // Check all the packages REQUIRE(pq.buffer.get(0).isReady()); + REQUIRE(pq.buffer.get(0).size() == PKT_LEN); + COMPARE(pq.buffer.get(0), "0123012301"); REQUIRE(pq.buffer.get(1).isReady()); + REQUIRE(pq.buffer.get(1).size() == PKT_LEN); + COMPARE(pq.buffer.get(1), "abcdabcdef"); REQUIRE_FALSE(pq.buffer.get(2).isReady()); + REQUIRE(pq.buffer.get(2).size() == 1); + COMPARE(pq.buffer.get(2), "g"); - COMPARE(pq.buffer.get(0), "0123012301"); - COMPARE(pq.buffer.get(1), "abcd"); - COMPARE(pq.buffer.get(2), "abcdefg"); + // Check the queue stats + REQUIRE(pq.countReady() == 2); + REQUIRE(pq.countNotEmpty() == 3); + REQUIRE_FALSE(pq.isEmpty()); + REQUIRE_FALSE(pq.isFull()); INFO("Popping first element"); p = pq.pop(); // Should still return first packet REQUIRE(p.getMsgCount() == 3); COMPARE(p, "0123012301"); - // Should now return what was the second element - COMPARE(pq.get(), "abcd"); - + // The packets should now be shifted REQUIRE(pq.buffer.get(0).isReady()); + REQUIRE(pq.buffer.get(0).size() == PKT_LEN); + COMPARE(pq.buffer.get(0), "abcdabcdef"); REQUIRE_FALSE(pq.buffer.get(1).isReady()); + REQUIRE(pq.buffer.get(1).size() == 1); + COMPARE(pq.buffer.get(1), "g"); REQUIRE(pq.countReady() == 1); REQUIRE(pq.countNotEmpty() == 2); REQUIRE_FALSE(pq.isEmpty()); REQUIRE_FALSE(pq.isFull()); - INFO("Adding a msg back to the first packet and filling it"); - REQUIRE(pq.put(messageBase, 10) == 0); - REQUIRE(pq.countReady() == 3); + INFO("Adding more data to fill the last packet"); + REQUIRE(pq.put(messageBase, 10)); + REQUIRE(pq.countReady() == 2); REQUIRE(pq.countNotEmpty() == 3); REQUIRE_FALSE(pq.isEmpty()); - REQUIRE(pq.isFull()); + REQUIRE_FALSE(pq.isFull()); - COMPARE(pq.buffer.get(0), "abcd"); - COMPARE(pq.buffer.get(1), "abcdefg"); - COMPARE(pq.buffer.get(2), "0123456789"); + // We should now have three packets + REQUIRE(pq.buffer.get(0).isReady()); + REQUIRE(pq.buffer.get(0).size() == PKT_LEN); + COMPARE(pq.buffer.get(0), "abcdabcdef"); + REQUIRE(pq.buffer.get(1).isReady()); + REQUIRE(pq.buffer.get(1).size() == PKT_LEN); + COMPARE(pq.buffer.get(1), "g012345678"); + REQUIRE_FALSE(pq.buffer.get(2).isReady()); + REQUIRE(pq.buffer.get(2).size() == 1); + COMPARE(pq.buffer.get(2), "9"); + + // If we now add another 10 bytes the last packet, the last byte which + // does not fit should be put in a new packet at the start of the queue + REQUIRE(pq.put(messageBase, 10)); + REQUIRE(pq.countReady() == 2); + REQUIRE(pq.countNotEmpty() == 3); + REQUIRE_FALSE(pq.isEmpty()); + REQUIRE_FALSE(pq.isFull()); + REQUIRE(pq.buffer.get(0).isReady()); + REQUIRE(pq.buffer.get(0).size() == PKT_LEN); + COMPARE(pq.buffer.get(0), "g012345678"); + REQUIRE(pq.buffer.get(1).isReady()); + REQUIRE(pq.buffer.get(1).size() == PKT_LEN); + COMPARE(pq.buffer.get(1), "9012345678"); + REQUIRE_FALSE(pq.buffer.get(2).isReady()); + REQUIRE(pq.buffer.get(2).size() == 1); + COMPARE(pq.buffer.get(2), "9"); + + // And now by adding the last 9 bytes the queue should be marked ready + REQUIRE(pq.put(messageBase + 10, 9)); + REQUIRE(pq.countReady() == 3); + REQUIRE(pq.countNotEmpty() == 3); + REQUIRE(pq.buffer.get(2).isReady()); + REQUIRE(pq.buffer.get(2).size() == PKT_LEN); + COMPARE(pq.buffer.get(2), "9abcdefghi"); INFO("Popping everything"); p = pq.pop(); - COMPARE(p, "abcd"); REQUIRE(p.isReady()); + REQUIRE(p.size() == PKT_LEN); + COMPARE(p, "g012345678"); p = pq.pop(); - COMPARE(p, "abcdefg"); + COMPARE(p, "9012345678"); REQUIRE(p.isReady()); p = pq.pop(); - COMPARE(p, "0123456789"); + COMPARE(p, "9abcdefghi"); REQUIRE(p.isReady()); REQUIRE_FALSE(pq.isFull()); @@ -288,20 +311,20 @@ TEST_CASE("PacketQueue tests") SECTION("Edge cases") { - INFO("Adding too big msg") - REQUIRE(pq.put(messageBase, PKT_LEN + 1) == -1); + INFO("Adding too big msg"); + REQUIRE_FALSE(pq.put(messageBase, PKT_LEN * QUEUE_LEN + 1)); REQUIRE_FALSE(pq.isFull()); REQUIRE(pq.isEmpty()); REQUIRE(pq.countNotEmpty() == 0); REQUIRE(pq.countReady() == 0); - INFO("Adding empty message") - REQUIRE(pq.put(messageBase, 0) == -1); + INFO("Adding empty message"); + REQUIRE_FALSE(pq.put(messageBase, 0)); - INFO("Adding something to full queue") - REQUIRE(pq.put(messageBase, PKT_LEN) == 0); - REQUIRE(pq.put(messageBase + 5, PKT_LEN) == 0); - REQUIRE(pq.put(messageBase + 10, PKT_LEN) == 0); + INFO("Adding something to full queue"); + REQUIRE(pq.put(messageBase, PKT_LEN)); + REQUIRE(pq.put(messageBase + 5, PKT_LEN)); + REQUIRE(pq.put(messageBase + 10, PKT_LEN)); REQUIRE(pq.buffer.count() == 3); for (int i = 0; i < 3; i++) @@ -319,7 +342,7 @@ TEST_CASE("PacketQueue tests") REQUIRE(pq.countNotEmpty() == 3); REQUIRE(pq.countReady() == 3); - INFO("Get/Pop on empty queue") + INFO("Get/Pop on empty queue"); REQUIRE_NOTHROW(pq.pop()); REQUIRE_NOTHROW(pq.pop()); REQUIRE_NOTHROW(pq.pop()); diff --git a/src/tests/catch/test-sensormanager-catch.cpp b/src/tests/catch/test-sensormanager-catch.cpp index 2a23d9685b18bf7ab0d08ade5367c9648a3e9896..1bead9c7c321e4110ac29a6a4c249340a98d9d8d 100644 --- a/src/tests/catch/test-sensormanager-catch.cpp +++ b/src/tests/catch/test-sensormanager-catch.cpp @@ -24,7 +24,7 @@ #include "catch-tests-entry.cpp" #endif -#include <utils/testutils/TestSensor.h> +#include <utils/TestUtils/TestSensor.h> #include <catch2/catch.hpp> #include <iostream> @@ -39,7 +39,7 @@ using namespace Boardcore; static const uint8_t FIRST_TASK_ID = 7; // used to test IDs assignment to tasks -class FailingSensor : public Sensor<TestData> +class FailingSensorCatch : public Sensor<TestData> { bool init() { return true; } @@ -122,7 +122,7 @@ private: /*Enabled=*/true}; // always failing self-test - FailingSensor s5; + FailingSensorCatch s5; SensorInfo s5_info{ /*ID=*/"s5", /*Period=*/2000, diff --git a/src/tests/catch/xbee/MockXbeeSPIBus.h b/src/tests/catch/xbee/MockXbeeSPIBus.h index d54b3167cdecfa1faf8514a19c8a854f5c6aaf1d..44c48d14175a5b647a9a60ffe97241f496fdfb91 100644 --- a/src/tests/catch/xbee/MockXbeeSPIBus.h +++ b/src/tests/catch/xbee/MockXbeeSPIBus.h @@ -24,8 +24,8 @@ #include <radio/Xbee/APIFrameParser.h> #include <radio/Xbee/APIFrames.h> -#include <utils/testutils/MockGpioPin.h> -#include <utils/testutils/MockSPIBus.h> +#include <utils/TestUtils/MockGpioPin.h> +#include <utils/TestUtils/MockSPIBus.h> #include <deque> #include <functional> diff --git a/src/tests/drivers/canbus/test-canbus-2way.cpp b/src/tests/drivers/canbus/test-canbus-2way.cpp index 2178668e0ffa0a9d36734ff77cf5261abda7a7fd..ab566f0e4555090de6499ef6d35544dd73464bf1 100644 --- a/src/tests/drivers/canbus/test-canbus-2way.cpp +++ b/src/tests/drivers/canbus/test-canbus-2way.cpp @@ -26,6 +26,7 @@ // which it will respond #include <utils/Debug.h> +#include <utils/Stats/Stats.h> #include <string> @@ -34,7 +35,6 @@ #include "diagnostic/PrintLogger.h" #include "drivers/canbus/BusLoadEstimation.h" #include "drivers/canbus/Canbus.h" -#include "math/Stats.h" #include "utils/collections/CircularBuffer.h" constexpr uint32_t BAUD_RATE = 1000 * 1000; diff --git a/src/tests/drivers/sx1278/test-sx1278-bench.cpp b/src/tests/drivers/sx1278/test-sx1278-bench.cpp index d484d39cab98c3703d89d27da0c176900cca7690..31db75f4f5c47aa0a847be004e403718df858116 100644 --- a/src/tests/drivers/sx1278/test-sx1278-bench.cpp +++ b/src/tests/drivers/sx1278/test-sx1278-bench.cpp @@ -93,9 +93,6 @@ struct Stats } stats; -/// Interval between transmissions. -const int TX_INTERVAL = 1; - SX1278 *sx1278[2] = {nullptr, nullptr}; struct Msg @@ -115,12 +112,10 @@ void recvLoop(int idx) while (1) { Msg msg; - msg.idx = 0; - msg.dummy_1 = 0; - msg.dummy_2 = 0; - msg.dummy_3 = 0; + memset(&msg, 0, sizeof(msg)); int len = sx1278[idx]->receive((uint8_t *)&msg, sizeof(msg)); + if (len != sizeof(msg)) { stats.recv_errors++; @@ -143,8 +138,6 @@ void sendLoop(int idx) { while (1) { - miosix::Thread::sleep(TX_INTERVAL); - int next_idx = stats.send_count + 1; Msg msg; @@ -269,6 +262,7 @@ int main() std::thread recv([]() { recvLoop(0); }); #endif #ifndef DISABLE_TX + miosix::Thread::sleep(500); std::thread send([]() { sendLoop(1); }); #endif diff --git a/src/tests/drivers/sx1278/test-sx1278-serial.cpp b/src/tests/drivers/sx1278/test-sx1278-serial.cpp index 0078a16b949f927ca05b82cd7dd6f67b2023b40f..22df3fe51535b7a62aefe3bdce39aae66208ec5f 100644 --- a/src/tests/drivers/sx1278/test-sx1278-serial.cpp +++ b/src/tests/drivers/sx1278/test-sx1278-serial.cpp @@ -31,17 +31,17 @@ using namespace Boardcore; using namespace miosix; -SPIBus bus(SPI3); +SPIBus bus(SPI4); -GpioPin sck(GPIOC_BASE, 10); -GpioPin miso(GPIOC_BASE, 11); -GpioPin mosi(GPIOC_BASE, 12); -GpioPin cs(GPIOA_BASE, 1); -GpioPin dio(GPIOC_BASE, 15); +GpioPin sck(GPIOE_BASE, 2); +GpioPin miso(GPIOE_BASE, 5); +GpioPin mosi(GPIOE_BASE, 6); +GpioPin cs(GPIOC_BASE, 1); +GpioPin dio(GPIOF_BASE, 10); SX1278* sx1278 = nullptr; -void __attribute__((used)) EXTI15_IRQHandlerImpl() +void __attribute__((used)) EXTI10_IRQHandlerImpl() { if (sx1278) sx1278->handleDioIRQ(); @@ -54,16 +54,16 @@ void initBoard() miosix::FastInterruptDisableLock dLock; // Enable SPI3 - RCC->APB1ENR |= RCC_APB1ENR_SPI3EN; + RCC->APB2ENR |= RCC_APB2ENR_SPI4EN; // Enable SPI4 bus RCC_SYNC(); // Setup SPI pins sck.mode(miosix::Mode::ALTERNATE); - sck.alternateFunction(6); + sck.alternateFunction(5); miso.mode(miosix::Mode::ALTERNATE); - miso.alternateFunction(6); + miso.alternateFunction(5); mosi.mode(miosix::Mode::ALTERNATE); - mosi.alternateFunction(6); + mosi.alternateFunction(5); cs.mode(miosix::Mode::OUTPUT); dio.mode(miosix::Mode::INPUT); @@ -77,30 +77,35 @@ void initBoard() void recvLoop() { uint8_t msg[256]; - auto console = miosix::DefaultConsole::instance().get(); - while (1) { int len = sx1278->receive(msg, sizeof(msg)); if (len > 0) { - console->writeBlock(msg, len, 0); - // TODO: Flushing? + auto serial = miosix::DefaultConsole::instance().get(); + serial->writeBlock(msg, len, 0); } } } void sendLoop() { - uint8_t msg[256]; - auto console = miosix::DefaultConsole::instance().get(); + // I create a GPIO with the onboard led to tell the user that + // a package is being sent + miosix::GpioPin led(GPIOG_BASE, 13); + led.mode(miosix::Mode::OUTPUT); + led.low(); + uint8_t msg[63]; while (1) { - int len = console->readBlock(msg, sizeof(msg), 0); + auto serial = miosix::DefaultConsole::instance().get(); + int len = serial->readBlock(msg, sizeof(msg), 0); if (len > 0) { + led.high(); sx1278->send(msg, len); + led.low(); } } } @@ -122,13 +127,20 @@ int main() return -1; } + printf("\n[sx1278] Initialization complete!\n"); + std::thread recv([]() { recvLoop(); }); std::thread send([]() { sendLoop(); }); - printf("\n[sx1278] Initialization complete!\n"); + for (;;) + { + miosix::Thread::sleep(100); + } - while (1) - miosix::Thread::wait(); + // God please forgive me + // FIXME(davide.mor): ABSOLUTELY fix this + // miosix::Thread::sleep(20000); + // miosix::reboot(); return 0; } diff --git a/src/tests/drivers/test-MBLoadCell.cpp b/src/tests/drivers/test-MBLoadCell.cpp index 5993c26044a69a6a537e82b5effba75ffee69f74..f5d97bb0da552add766b54542ba97f45b014e32d 100644 --- a/src/tests/drivers/test-MBLoadCell.cpp +++ b/src/tests/drivers/test-MBLoadCell.cpp @@ -21,15 +21,16 @@ */ #include <diagnostic/PrintLogger.h> - -#include "sensors/MBLoadCell/MBLoadCell.h" -#include "string.h" -#include "utils/ButtonHandler.h" +#include <sensors/MBLoadCell/MBLoadCell.h> +#include <string.h> +#include <utils/ButtonHandler/ButtonHandler.h> //#define PRINT_ALL_SAMPLES // To be defined if we want to print all the samples using namespace Boardcore; using namespace miosix; +using namespace std; +using namespace placeholders; using button = miosix::Gpio<GPIOA_BASE, 0>; ///< User button const uint8_t btnUserId = 1; @@ -39,17 +40,17 @@ const uint8_t btnUserId = 1; * button is pressed for a long time, resets the minimum and maximum values of * the recorded weights */ -void buttonCallback(uint8_t btnId, ButtonPress btnPress, MBLoadCell *loadcell) +void buttonCallback(ButtonEvent btnPress, MBLoadCell *loadcell) { - if (btnId == btnUserId && btnPress == ButtonPress::DOWN) + if (btnPress == ButtonEvent::PRESSED) TRACE( "## MAX: %.2f [Kg] (ts: %.3f)\t##\tMIN: %.2f [Kg] (ts: %.3f) ##\n", - loadcell->getMaxWeight().weight, - loadcell->getMaxWeight().weightTimestamp / 1000000.0, - loadcell->getMinWeight().weight, - loadcell->getMinWeight().weightTimestamp / 1000000.0); + loadcell->getMaxWeight().load, + loadcell->getMaxWeight().loadTimestamp / 1000000.0, + loadcell->getMinWeight().load, + loadcell->getMinWeight().load / 1000000.0); - if (btnId == btnUserId && btnPress == ButtonPress::LONG) + if (btnPress == ButtonEvent::LONG_PRESS) loadcell->resetMaxMinWeights(); } @@ -67,14 +68,9 @@ int main() */ MBLoadCell loadCell(LoadCellModes::CONT_MOD_T, 2, 115200); - // Binding the load cell instance to the callback to be called - std::function<void(uint8_t, ButtonPress)> callback = - std::bind(buttonCallback, std::placeholders::_1, std::placeholders::_2, - &loadCell); - // Instanciating the button - ButtonHandler<button> btnHandler(btnUserId, callback); - btnHandler.start(); + ButtonHandler::getInstance().registerButtonCallback( + button::getPin(), bind(buttonCallback, _1, &loadCell)); // Initializing the load cell if (!loadCell.init()) diff --git a/src/tests/drivers/test-dsgamma.cpp b/src/tests/drivers/test-dsgamma.cpp index 03c9555075b41cec6929157105bea79f95b9037a..1445ae8a674aaa467b3713faf1f8eef368ab9694 100644 --- a/src/tests/drivers/test-dsgamma.cpp +++ b/src/tests/drivers/test-dsgamma.cpp @@ -170,8 +170,10 @@ int main() } printf("Bytes received: %d\nDropped: %d,Time:%d ms\n", index, lostBytes, + // cppcheck-suppress uninitvar (int)(endT - startT)); printf("Speed: %.3f KB/s\n", + // cppcheck-suppress uninitvar index / ((endT - startT) / 1024.0f) / 1024.0f); /* for (int i = 0; i < index; i++) { diff --git a/src/tests/drivers/usart/test-usart.cpp b/src/tests/drivers/usart/test-usart.cpp new file mode 100644 index 0000000000000000000000000000000000000000..ce99d3d5a5a764b696598f8b0bf2c1ef49eccd7f --- /dev/null +++ b/src/tests/drivers/usart/test-usart.cpp @@ -0,0 +1,182 @@ +/* Copyright (c) 2022 Skyward Experimental Rocketry + * Author: Emilio Corigliano + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <fmt/format.h> + +#include <cassert> + +#include "drivers/usart/USART.h" +#include "miosix.h" +#include "string" +#include "string.h" +#include "thread" +#include "utils/SerialInterface.h" + +using namespace miosix; +using namespace Boardcore; + +/** + * SETUP: + * - connect the default serial (USART3) to the pc + * - connect usartx_rx to the usarty_tx + * - connect usarty_rx to the usartx_tx + * + * WARNING: If using the STM32SerialWrapper to read, the test passes only if we: + * 1. do a write/writeString with USART or STM32SerialWrapper; + * 2. sleep 10ms or more (or a printf...) + * 3. we finally read from STM32SerialWrapper::read method. + * if we omit the waiting time we end up failing the test. If more data is sent, + * then more baudrates tests will fail (from 2400 to greater). The USART driver + * doesn't have this problem. + */ + +typedef struct +{ + char dataChar; + int dataInt; + float dataFloat; + double dataDouble; + + std::string print() + { + return fmt::format("{},{:d},{:f},{:f}", dataChar, dataInt, dataFloat, + dataDouble); + } +} StructToSend; +StructToSend struct_tx = {'C', 42, 420.69, 48.84}; +char buf_tx[64] = "Testing communication, but very very very loong :D"; +USARTInterface::Baudrate baudrates[] = { + USARTInterface::Baudrate::B2400, USARTInterface::Baudrate::B9600, + USARTInterface::Baudrate::B19200, USARTInterface::Baudrate::B38400, + USARTInterface::Baudrate::B57600, USARTInterface::Baudrate::B115200, + USARTInterface::Baudrate::B230400, USARTInterface::Baudrate::B460800, + USARTInterface::Baudrate::B921600}; + +/** + * Communication: src -> dst + * tests the writeString, write and read methods of the USART drivers + */ +bool testCommunicationSequential(USARTInterface *src, USARTInterface *dst) +{ + char buf_rx[64]; + StructToSend struct_rx; + bool passed = true; + + /************************** SENDING STRING **************************/ + printf("Sending string\n"); + + printf("\t%d--> sent: \t'%s'\n", src->getId(), buf_tx); + src->writeString(buf_tx); + // Thread::sleep(10); // enable to pass the test with STM32SerialWrapper + dst->read(buf_rx, 64); + + printf("\t%d<-- received: \t'%s'\n", dst->getId(), buf_rx); + + if (strcmp(buf_tx, buf_rx) == 0) + { + printf("*** %d -> %d WORKING!\n", src->getId(), dst->getId()); + } + else + { + printf("### %d -> %d ERROR!\n", src->getId(), dst->getId()); + passed = false; + } + + /*********************** SENDING BINARY DATA ************************/ + + printf("Sending binary data\n"); + printf("\t%d--> sent: \t'%s'\n", src->getId(), struct_tx.print().c_str()); + src->write(&struct_tx, sizeof(StructToSend)); + // Thread::sleep(10); // enable to pass the test with STM32SerialWrapper + dst->read(&struct_rx, sizeof(StructToSend)); + printf("\t%d<-- received: \t'%s'\n", dst->getId(), + struct_rx.print().c_str()); + + if (memcmp(&struct_tx, &struct_rx, sizeof(StructToSend)) == 0) + { + printf("*** %d -> %d WORKING!\n", src->getId(), dst->getId()); + } + else + { + printf("### %d -> %d ERROR!\n", src->getId(), dst->getId()); + passed = false; + } + + return passed; +} + +/* Available default pins: + * - USART1: tx=PA9 rx=PA10 + * - USART2: tx=PA2 rx=PA3 + * - USART3: tx=PB10 rx=PB11 + * - UART4: tx=PA0 rx=PA1 + * - UART5: tx=PC12 rx=PD2 + * - USART6: tx=PC6 rx=PC7 + * - UART7: tx=PE8 rx=PE7 + * - UART8: tx=PE1 rx=PE0 + */ +int main() +{ + bool testPassed = true; + printf("*** SERIAL 3 WORKING!\n"); + for (unsigned int iBaud = 0; + iBaud < sizeof(baudrates) / sizeof(baudrates[0]); iBaud++) + { + USARTInterface::Baudrate baudrate = baudrates[iBaud]; + printf("\n\n########################### %d\n", (int)baudrate); + + // declaring the usart peripherals + STM32SerialWrapper usartx(USART1, baudrate, u1rx2::getPin(), + u1tx1::getPin()); + usartx.init(); + + USART usarty(UART4, baudrate); + // usarty.initPins(u5tx::getPin(), 8, u5rx::getPin(), 8); + // usarty.setOversampling(false); + // usarty.setStopBits(1); + // usarty.setWordLength(USART::WordLength::BIT8); + // usarty.setParity(USART::ParityBit::NO_PARITY); + usarty.init(); + + // testing transmission (both char and binary) "serial 1 <- serial 2" + testPassed &= testCommunicationSequential(&usartx, &usarty); + + // testing transmission (both char and binary) "serial 1 -> serial 2" + testPassed &= testCommunicationSequential(&usarty, &usartx); + } + + if (testPassed) + { + printf( + "********************************\n" + "*** TEST PASSED ***\n" + "********************************\n"); + } + else + { + printf( + "################################\n" + "### TEST FAILED ###\n" + "################################\n"); + } + return 0; +} diff --git a/src/tests/drivers/xbee/XbeeTestData.h b/src/tests/drivers/xbee/XbeeTestData.h index e7e6cc4c8f67fdce1f0488892b957bbfa7fe2db7..cde8e70c9d678bd10c26a2907de0d53b0dc59686 100644 --- a/src/tests/drivers/xbee/XbeeTestData.h +++ b/src/tests/drivers/xbee/XbeeTestData.h @@ -22,7 +22,7 @@ #pragma once -#include <math/Stats.h> +#include <utils/Stats/Stats.h> #include <array> #include <ostream> diff --git a/src/tests/drivers/xbee/XbeeTransceiver.h b/src/tests/drivers/xbee/XbeeTransceiver.h index 049b9afe73c538d7db0245505ff6446ed2e30470..3e41850e557367880e47fde90bcd57fc11788527 100644 --- a/src/tests/drivers/xbee/XbeeTransceiver.h +++ b/src/tests/drivers/xbee/XbeeTransceiver.h @@ -28,7 +28,7 @@ #include <radio/Xbee/APIFramesLog.h> #include <radio/Xbee/Xbee.h> #include <utils/Debug.h> -#include <utils/testutils/ThroughputCalculator.h> +#include <utils/TestUtils/ThroughputCalculator.h> #include <functional> diff --git a/src/tests/drivers/xbee/gui/EnergyScanScreen.h b/src/tests/drivers/xbee/gui/EnergyScanScreen.h index 081d375486f11fba2cf4a95af1e08f81d248e069..49259f2d8967279274d3adca993b0bdaec713a8b 100644 --- a/src/tests/drivers/xbee/gui/EnergyScanScreen.h +++ b/src/tests/drivers/xbee/gui/EnergyScanScreen.h @@ -22,8 +22,8 @@ #pragma once -#include <math/Stats.h> #include <mxgui/display.h> +#include <utils/Stats/Stats.h> #include <utils/gui/GridLayout.h> #include <utils/gui/TextView.h> #include <utils/gui/VerticalLayout.h> diff --git a/src/tests/drivers/xbee/gui/StatusScreen.h b/src/tests/drivers/xbee/gui/StatusScreen.h index 19cc9e6e9b8e4224c177b60da5f6299762f82d2b..b397715e0d772a8261ddfbb42f127b320ac9b848 100644 --- a/src/tests/drivers/xbee/gui/StatusScreen.h +++ b/src/tests/drivers/xbee/gui/StatusScreen.h @@ -24,11 +24,11 @@ #include <logger/Logger.h> #include <mxgui/display.h> +#include <utils/TestUtils/ThroughputCalculator.h> #include <utils/gui/GridLayout.h> #include <utils/gui/OptionView.h> #include <utils/gui/TextView.h> #include <utils/gui/VerticalLayout.h> -#include <utils/testutils/ThroughputCalculator.h> #include <cstdint> #include <cstring> diff --git a/src/tests/drivers/xbee/test-xbee-gui.cpp b/src/tests/drivers/xbee/test-xbee-gui.cpp index 7c51a4003d40d767dfc4974177cc431283ee553b..407f90a1dd4c9682e45c5aa504388544b83ea0b0 100644 --- a/src/tests/drivers/xbee/test-xbee-gui.cpp +++ b/src/tests/drivers/xbee/test-xbee-gui.cpp @@ -27,7 +27,7 @@ #include <radio/Xbee/APIFramesLog.h> #include <radio/Xbee/ATCommands.h> #include <radio/Xbee/Xbee.h> -#include <utils/ButtonHandler.h> +#include <utils/ButtonHandler/ButtonHandler.h> #include <array> #include <cstdio> @@ -75,7 +75,6 @@ Xbee::Xbee* xbee = nullptr; ConstSendInterval sndInt{0}; XbeeTransceiver* trans = nullptr; XbeeGUI* gui; -ButtonHandler<GpioUserBtn>* btnHandler; unsigned int markCounter = 1; @@ -145,10 +144,9 @@ int main() // GUI gui = new XbeeGUI(); - btnHandler = new ButtonHandler<GpioUserBtn>( - 0, bind(&ScreenManager::onButtonPress, &gui->screenManager, _2)); - - btnHandler->start(); + ButtonHandler::getInstance().registerButtonCallback( + GpioUserBtn::getPin(), + bind(&ScreenManager::onButtonEvent, &gui->screenManager, _1)); gui->screenConfig.btnStart.addOnInteractionListener(onStartButtonClick); gui->screenConfig.btnEnergy.addOnInteractionListener(onEnergyButtonClick); diff --git a/src/tests/events/fsm/test-fsm.cpp b/src/tests/events/fsm/test-fsm.cpp index 7c119cc16ddddbd6a58914e4f8f422a59a60ac09..b6f2dc4f7e3a8dfd43f198962d74abe988ee9c60 100644 --- a/src/tests/events/fsm/test-fsm.cpp +++ b/src/tests/events/fsm/test-fsm.cpp @@ -33,17 +33,18 @@ int main() { FSMExample fsm; - sEventBroker.start(); // Start broker thread - fsm.start(); // Start FSM thread + EventBroker::getInstance().start(); // Start broker thread + fsm.start(); // Start FSM thread // State machine starts in state S1. Post EV_A to move to S2 - sEventBroker.post(Event{EV_A}, TOPIC_T1); + EventBroker::getInstance().post(Event{EV_A}, TOPIC_T1); // FSM now in State S2 - sEventBroker.post(Event{EV_E}, - TOPIC_T1); // This makes the FSM print hello world + EventBroker::getInstance().post( + Event{EV_E}, + TOPIC_T1); // This makes the FSM print hello world - sEventBroker.post(Event{EV_C}, TOPIC_T1); // Transition to S3 + EventBroker::getInstance().post(Event{EV_C}, TOPIC_T1); // Transition to S3 printf("Waiting for the FSM to transition to S1\n"); Thread::sleep(1100); @@ -53,7 +54,7 @@ int main() // Since previously we've been in state S3, now v == 1 and EV_A will make // the FSM transition to S4 instead of S1 - sEventBroker.post(Event{EV_A}, TOPIC_T1); + EventBroker::getInstance().post(Event{EV_A}, TOPIC_T1); // Now the state machine is in state S4 @@ -68,5 +69,5 @@ int main() // Stop the threds, even though we will never reach this point, but just for // correctness ;) fsm.stop(); - sEventBroker.stop(); + EventBroker::getInstance().stop(); } diff --git a/src/tests/events/fsm/test-fsm.h b/src/tests/events/fsm/test-fsm.h index 7b16d6703e9cc8c6910dd012b2377d5461d7063c..f603cdfec09f72e09864f4dbeaf43e08264c9b6d 100644 --- a/src/tests/events/fsm/test-fsm.h +++ b/src/tests/events/fsm/test-fsm.h @@ -40,8 +40,8 @@ namespace Boardcore */ enum ExampleEvents : uint8_t { - EV_A = EV_FIRST_SIGNAL, // The first event must always have - // value EV_FIRST_SIGNAL + EV_A = EV_FIRST_CUSTOM, // The first event must always have + // value EV_FIRST_CUSTOM EV_B, // Values for the following event can be manually specified or // assigned automatically EV_C, @@ -64,7 +64,7 @@ enum ExampleTopics : uint8_t */ void traceState(uint8_t state, const Event& ev) { - switch (ev.code) + switch (ev) { case EV_ENTRY: { @@ -78,7 +78,7 @@ void traceState(uint8_t state, const Event& ev) } default: { - printf("(S%d) Received event %d\n", state, ev.code); + printf("(S%d) Received event %d\n", state, ev); break; } } @@ -110,10 +110,10 @@ public: FSMExample() : FSM(&FSMExample::state_S1), v(0) { // Subscribe for events posted on TOPIC_T1 - sEventBroker.subscribe(this, TOPIC_T1); + EventBroker::getInstance().subscribe(this, TOPIC_T1); } - ~FSMExample() { sEventBroker.unsubscribe(this); } + ~FSMExample() { EventBroker::getInstance().unsubscribe(this); } private: /* @@ -129,7 +129,7 @@ private: // state machine on the terminal. traceState(STATE_S1, ev); - switch (ev.code) + switch (ev) { // It's always good to add braces to every single case statement, to // avoid problems @@ -169,7 +169,7 @@ private: { traceState(STATE_S2, ev); - switch (ev.code) + switch (ev) { case EV_ENTRY: { @@ -200,7 +200,7 @@ private: { traceState(STATE_S3, ev); - switch (ev.code) + switch (ev) { case EV_ENTRY: { @@ -208,15 +208,15 @@ private: v = 1; // Post EV_D to itself in 1 seconds - delayed_ev_id = - sEventBroker.postDelayed<1000>(Event{EV_D}, TOPIC_T1); + delayed_ev_id = EventBroker::getInstance().postDelayed<1000>( + Event{EV_D}, TOPIC_T1); break; } case EV_EXIT: { // Remove the delayed event in case it has not fired yet - sEventBroker.removeDelayed(delayed_ev_id); + EventBroker::getInstance().removeDelayed(delayed_ev_id); break; } case EV_B: @@ -243,7 +243,7 @@ private: { traceState(STATE_S4, ev); - switch (ev.code) + switch (ev) { case EV_ENTRY: { diff --git a/src/tests/sensors/analog/test-battery-voltage.cpp b/src/tests/sensors/analog/test-battery-voltage.cpp index 1e2065433ee6f6da108ba4e28896aa31294ab755..3bc6d96fbbfb94fdfcad3c9dc762e4a0d27bb020 100644 --- a/src/tests/sensors/analog/test-battery-voltage.cpp +++ b/src/tests/sensors/analog/test-battery-voltage.cpp @@ -23,7 +23,7 @@ #include <drivers/adc/InternalADC.h> #include <drivers/timer/TimestampTimer.h> #include <miosix.h> -#include <sensors/analog/battery/BatteryVoltageSensor.h> +#include <sensors/analog/BatteryVoltageSensor.h> #include <utils/Debug.h> using namespace Boardcore; diff --git a/src/tests/sensors/analog/test-current-sensor.cpp b/src/tests/sensors/analog/test-current-sensor.cpp index 5983e82b6a53b915bf9a63036c847c66c1e1b2a2..b223d9041c2fb626326adb5c3e5bd29cf2063163 100644 --- a/src/tests/sensors/analog/test-current-sensor.cpp +++ b/src/tests/sensors/analog/test-current-sensor.cpp @@ -24,7 +24,7 @@ #include <drivers/timer/TimestampTimer.h> #include <interfaces-impl/gpio_impl.h> #include <miosix.h> -#include <sensors/analog/current/CurrentSensor.h> +#include <sensors/analog/CurrentSensor.h> #include <utils/Debug.h> using namespace miosix; diff --git a/src/tests/sensors/calibration/test-calibration-benchmark.cpp b/src/tests/sensors/calibration/test-calibration-benchmark.cpp index 066536cf73f042b53047cc615ae3ab5c67070f9d..1ed6b0f81de257186fd9c1c858b8648fce157998 100644 --- a/src/tests/sensors/calibration/test-calibration-benchmark.cpp +++ b/src/tests/sensors/calibration/test-calibration-benchmark.cpp @@ -21,9 +21,9 @@ */ /* ACCELEROMETER calibration: please set the chosen one to 1 */ -#define BIAS_CALIBRATION_LOAD_TEST 1 -#define SIX_PARAMETER_CALIBRATION_LOAD_TEST 0 -#define TWELVE_PARAMETER_CALIBRATION_LOAD_TEST 0 +#define BIAS_CALIBRATION_LOAD_TEST +// #define SIX_PARAMETER_CALIBRATION_LOAD_TEST +// #define TWELVE_PARAMETER_CALIBRATION_LOAD_TEST /* Expressed in Hertz: valid values: 1 <= frequency <= 1000 */ #define SAMPLE_FREQUENCY_LOAD_TEST 1000 @@ -35,7 +35,6 @@ #include <sensors/calibration/BiasCalibration.h> #include <sensors/calibration/HardIronCalibration.h> #include <sensors/calibration/SixParameterCalibration.h> -#include <sensors/calibration/SoftIronCalibration.h> #include <sensors/calibration/TwelveParameterCalibration.h> #include <utils/Debug.h> @@ -47,17 +46,17 @@ volatile AccelerometerData testData; int main() { -#if BIAS_CALIBRATION_LOAD_TEST +#ifdef BIAS_CALIBRATION_LOAD_TEST BiasCorrector<AccelerometerData> corrector; TRACE("Using BIAS calibration model.\n"); #endif -#if SIX_PARAMETER_CALIBRATION_LOAD_TEST +#ifdef SIX_PARAMETER_CALIBRATION_LOAD_TEST SixParameterCorrector<AccelerometerData> corrector; TRACE("Using SIX-PARAMETER calibration model.\n"); #endif -#if TWELVE_PARAMETER_CALIBRATION_LOAD_TEST +#ifdef TWELVE_PARAMETER_CALIBRATION_LOAD_TEST TwelveParameterCorrector<AccelerometerData> corrector; TRACE("Using TWELVE-PARAMETER calibration model.\n"); #endif diff --git a/src/tests/sensors/calibration/test-calibration-stats.cpp b/src/tests/sensors/calibration/test-calibration-stats.cpp index a52fabb0f3b7ff29e35d60cbf52b6de16b8d648f..40aa29ad31fb57621dc67c00d6c83193c4b6d30b 100644 --- a/src/tests/sensors/calibration/test-calibration-stats.cpp +++ b/src/tests/sensors/calibration/test-calibration-stats.cpp @@ -23,13 +23,14 @@ #define SENSOR_LIS3DSH_STATS_TEST 1 #include <drivers/spi/SPIDriver.h> -#include <math/Stats.h> #include <miosix.h> +#include <utils/Stats/Stats.h> #if SENSOR_LIS3DSH_STATS_TEST #include <sensors/LIS3DSH/LIS3DSH.h> #endif +#include <sensors/calibration/AxisOrientation.h> #include <sensors/calibration/Calibration.h> using namespace Boardcore; diff --git a/src/tests/sensors/calibration/test-calibration.cpp b/src/tests/sensors/calibration/test-calibration.cpp index 4172dec1c3bface25671de10f8f8a3f9479c511f..9e2429572ddd71e14b969bb8c10f77d473e9a619 100644 --- a/src/tests/sensors/calibration/test-calibration.cpp +++ b/src/tests/sensors/calibration/test-calibration.cpp @@ -39,7 +39,6 @@ #include <sensors/calibration/BiasCalibration.h> #include <sensors/calibration/HardIronCalibration.h> #include <sensors/calibration/SixParameterCalibration.h> -#include <sensors/calibration/SoftIronCalibration.h> #include <sensors/calibration/TwelveParameterCalibration.h> #include <utils/Debug.h> diff --git a/src/tests/sensors/test-hx711.cpp b/src/tests/sensors/test-hx711.cpp index 53e6622f98b409019717fc2ad72ee8a71ce5c89d..f4acb0d76fc4e5c9dee08a4b8a6477bb478a192c 100644 --- a/src/tests/sensors/test-hx711.cpp +++ b/src/tests/sensors/test-hx711.cpp @@ -1,5 +1,5 @@ -/* Copyright (c) 2021 Skyward Experimental Rocketry - * Author: Matteo Pignataro +/* Copyright (c) 2022 Skyward Experimental Rocketry + * Author: Alberto Nidasio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -54,19 +54,19 @@ int main() for (int i = 0; i < 100; i++) { sensor.sample(); - average += sensor.getLastSample().weight; + average += sensor.getLastSample().load; Thread::sleep(12); } average /= 100; - sensor.setZero(-average); + sensor.setOffset(average); sensor.setScale(214000); while (true) { sensor.sample(); - printf("[%.1f] %f\n", sensor.getLastSample().weightTimestamp / 1e6, - sensor.getLastSample().weight); + printf("[%.1f] %f\n", sensor.getLastSample().loadTimestamp / 1e6, + sensor.getLastSample().load); Thread::sleep(10); } diff --git a/src/tests/sensors/test-l3gd20-fifo.cpp b/src/tests/sensors/test-l3gd20-fifo.cpp index 31208a2f48ff4754786a88ab7b99eea51c8b0a57..2b6d46286f8df3ff7f3b620eb4e0a04860642da4 100644 --- a/src/tests/sensors/test-l3gd20-fifo.cpp +++ b/src/tests/sensors/test-l3gd20-fifo.cpp @@ -80,7 +80,7 @@ static constexpr unsigned int FIFO_WATERMARK = 12; // output data rate) is the following: static constexpr float SAMPLE_FREQUENCY = 782.3f; -struct GyroSample +struct GyroSampleFifo { int fifoNum; L3GD20Data gyro; @@ -93,7 +93,7 @@ struct GyroSample // How many samples to collect static constexpr int NUM_SAMPLES = SAMPLE_FREQUENCY * 20; -GyroSample data[NUM_SAMPLES]; +GyroSampleFifo data[NUM_SAMPLES]; int dataCounter = 0; // Last interrupt received timer tick diff --git a/src/tests/sensors/test-max31855.cpp b/src/tests/sensors/test-max31855.cpp index 1c6527d4ed539e1f7668a709cf3a5c5f14caa099..00171ebab0da1cb58c04885821dc96e0b7526754 100644 --- a/src/tests/sensors/test-max31855.cpp +++ b/src/tests/sensors/test-max31855.cpp @@ -1,5 +1,5 @@ -/* Copyright (c) 2021 Skyward Experimental Rocketry - * Author: Matteo Pignataro +/* Copyright (c) 2022 Skyward Experimental Rocketry + * Author: Alberto Nidasio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -27,15 +27,19 @@ using namespace miosix; using namespace Boardcore; -GpioPin sckPin = GpioPin(GPIOE_BASE, 2); -GpioPin misoPin = GpioPin(GPIOE_BASE, 5); -GpioPin csPin = GpioPin(GPIOE_BASE, 4); +GpioPin sckPin = GpioPin(GPIOF_BASE, 7); +GpioPin misoPin = GpioPin(GPIOF_BASE, 8); +GpioPin csPin = GpioPin(GPIOD_BASE, 13); + +GpioPin csMems = GpioPin(GPIOC_BASE, 1); void initBoard() { // Setup gpio pins csPin.mode(Mode::OUTPUT); csPin.high(); + csMems.mode(Mode::OUTPUT); + csMems.high(); sckPin.mode(Mode::ALTERNATE); sckPin.alternateFunction(5); misoPin.mode(Mode::ALTERNATE); @@ -47,7 +51,7 @@ int main() // Enable SPI clock and set gpios initBoard(); - SPIBus spiBus(SPI4); + SPIBus spiBus(SPI5); MAX31855 sensor{spiBus, csPin}; printf("Starting process verification!\n"); @@ -62,7 +66,8 @@ int main() sensor.sample(); TemperatureData sample = sensor.getLastSample(); - printf("%.2f\n", sample.temperature); + printf("[%.2f] %.2f\n", sample.temperatureTimestamp / 1e6, + sample.temperature); Thread::sleep(500); } diff --git a/src/tests/sensors/test-mpu9250.cpp b/src/tests/sensors/test-mpu9250.cpp index ba2ad4ea4f8d1687c18554f12cacf41fa892ff35..03e798fa2ac28a1d7c23b334f8c715b67d2db0ae 100644 --- a/src/tests/sensors/test-mpu9250.cpp +++ b/src/tests/sensors/test-mpu9250.cpp @@ -53,18 +53,11 @@ void initBoard() int main() { // Enable SPI clock and set gpios - initBoard(); - - // SPI configuration setup - - SPIBusConfig spiConfig; - spiConfig.clockDivider = SPI::ClockDivider::DIV_64; - spiConfig.mode = SPI::Mode::MODE_3; - SPIBus spiBus(SPI2); - SPISlave spiSlave(spiBus, csPin, spiConfig); + // initBoard(); // Device initialization - MPU9250 mpu9250(spiSlave); + SPIBus spiBus(SPI1); + MPU9250 mpu9250(spiBus, sensors::mpu9250::cs::getPin()); // Initialize the device bool result = mpu9250.init(); @@ -85,7 +78,7 @@ int main() printf("%lld,%f,%f,%f\n", data.magneticFieldTimestamp, data.magneticFieldX, data.magneticFieldY, data.magneticFieldZ); - // Serial communicaion at 115200 baud takes aprx. 10ms + // Serial communication at 115200 baud takes approximately 10ms // miosix::delayMs(10); } diff --git a/src/tests/sensors/test-ms5803.cpp b/src/tests/sensors/test-ms5803.cpp index f3a8bcc1539d5b4eb9a7255f9056a85d043f6fec..4c00318899a0fd596e002c0baa3e350203ecd876 100644 --- a/src/tests/sensors/test-ms5803.cpp +++ b/src/tests/sensors/test-ms5803.cpp @@ -35,12 +35,9 @@ using namespace miosix; int main() { - SPIBusConfig spiConfig; + // Sample temperature every 10 pressure samples SPIBus spiBus(SPI1); - SPISlave spiSlave(spiBus, miosix::sensors::ms5803::cs::getPin(), spiConfig); - - // Sample temperature every 5 pressure samples - MS5803 sensor(spiSlave, 10); + MS5803 sensor(spiBus, miosix::sensors::ms5803::cs::getPin(), {}, 10); Thread::sleep(100); diff --git a/src/tests/sensors/test-ubloxgps.cpp b/src/tests/sensors/test-ubloxgps.cpp new file mode 100644 index 0000000000000000000000000000000000000000..24b6cc092a73e531765fbbe4897a29133a827014 --- /dev/null +++ b/src/tests/sensors/test-ubloxgps.cpp @@ -0,0 +1,91 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Authors: Davide Bonomini, Davide Mor, Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <drivers/timer/TimestampTimer.h> +#include <miosix.h> +#include <sensors/UbloxGPS/UbloxGPS.h> +#include <utils/Debug.h> + +#include <cstdio> + +using namespace Boardcore; +using namespace miosix; + +#define RATE 4 + +int main() +{ + (void)TimestampTimer::getInstance(); + + printf("Welcome to the ublox test\n"); + + // Keep GPS baud rate at default for easier testing + UbloxGPS gps(921600, RATE, 2, "gps", 38400); + UbloxGPSData dataGPS; + printf("Gps allocated\n"); + + // Init the gps + if (gps.init()) + { + printf("Successful gps initialization\n"); + } + else + { + printf("Failed gps initialization\n"); + } + + // Perform the selftest + if (gps.selfTest()) + { + printf("Successful gps selftest\n"); + } + else + { + printf("Failed gps selftest\n"); + } + + // Start the gps thread + gps.start(); + printf("Gps started\n"); + + while (true) + { + printf("a\n"); + // Give time to the thread + Thread::sleep(1000 / RATE); + + // Sample + gps.sample(); + printf("b\n"); + dataGPS = gps.getLastSample(); + + // Print out the latest sample + printf( + "[gps] timestamp: % 4.3f, fix: %01d lat: % f lon: % f " + "height: %4.1f nsat: %2d speed: %3.2f velN: % 3.2f velE: % 3.2f " + "track %3.1f\n", + (float)dataGPS.gpsTimestamp / 1000000, dataGPS.fix, + dataGPS.latitude, dataGPS.longitude, dataGPS.height, + dataGPS.satellites, dataGPS.speed, dataGPS.velocityNorth, + dataGPS.velocityEast, dataGPS.track); + } +} diff --git a/src/tests/sensors/test-vn100.cpp b/src/tests/sensors/test-vn100.cpp index 6d3c0bdbf7762840613c311729d25a08e6d70ada..df1d1870ec448660c8fe809f3cf3fb41783c7be3 100644 --- a/src/tests/sensors/test-vn100.cpp +++ b/src/tests/sensors/test-vn100.cpp @@ -29,19 +29,11 @@ using namespace Boardcore; int main() { - GpioPin tx(GPIOB_BASE, 6); - GpioPin rx(GPIOB_BASE, 7); VN100Data sample; string sampleRaw; - VN100 sensor{1, VN100::BaudRates::Baud_921600, + VN100 sensor{USART1, USARTInterface::Baudrate::B921600, VN100::CRCOptions::CRC_ENABLE_16}; - tx.mode(Mode::ALTERNATE); - rx.mode(Mode::ALTERNATE); - - tx.alternateFunction(7); - rx.alternateFunction(7); - // Let the sensor start up Thread::sleep(1000); @@ -64,18 +56,19 @@ int main() // Sample and print 100 samples for (int i = 0; i < 100; i++) { - /*sensor.sample(); + sensor.sample(); sample = sensor.getLastSample(); - //printf("%" PRIu64 ", %.3f, %.3f, %.3f\n", - sample.accelerationTimestamp, sample.accelerationX, - sample.accelerationY, sample.accelerationZ); - //printf("%.3f, %.3f, %.3f\n", sample.angularVelocityX, - sample.angularVelocityY, sample.angularVelocityZ);*/ + printf("acc: %" PRIu64 ", %.3f, %.3f, %.3f\n", + sample.accelerationTimestamp, sample.accelerationX, + sample.accelerationY, sample.accelerationZ); + printf("ang: %.3f, %.3f, %.3f\n", sample.angularVelocityX, + sample.angularVelocityY, sample.angularVelocityZ); sensor.sampleRaw(); sampleRaw = sensor.getLastRawSample(); printf("%s\n", sampleRaw.c_str()); // Thread::sleep(100); + printf("\n"); } sensor.closeAndReset(); diff --git a/src/tests/test-eventinjector.cpp b/src/tests/test-eventinjector.cpp index 424637b10027b701572790948541c489e6071e10..f36f11ba2a9bbfb3d0524d4369e69df63849fddc 100644 --- a/src/tests/test-eventinjector.cpp +++ b/src/tests/test-eventinjector.cpp @@ -37,7 +37,7 @@ int main() EventInjector injector; injector.start(); - EventCounter counter(sEventBroker); + EventCounter counter(EventBroker::getInstance()); counter.subscribe({topic}); for (;;) diff --git a/src/tests/test-hsm.cpp b/src/tests/test-hsm.cpp index c23c9d7ef15384575ee2231e2e2d2f3cabe79b56..c33ba46ba6bffbb54c97b8682b79665d4b3275ef 100644 --- a/src/tests/test-hsm.cpp +++ b/src/tests/test-hsm.cpp @@ -44,7 +44,7 @@ using namespace miosix; { \ cout << "------------------------------" << endl; \ cout << "Triggering signal " << #SIGNAL << endl; \ - sEventBroker.post({SIGNAL}, TOPIC_TEST); \ + EventBroker::getInstance().post({SIGNAL}, TOPIC_TEST); \ Thread::sleep(400); \ testValue = HSM.testState(STATE); \ cout << "Check State " << #STATE << " " \ @@ -54,7 +54,7 @@ using namespace miosix; enum TestEvents : uint8_t { - EV_A = EV_FIRST_SIGNAL, + EV_A = EV_FIRST_CUSTOM, EV_B, EV_C, EV_D, @@ -84,14 +84,13 @@ public: bool foo; }; -#define DEBUG_PRINT \ - cout << __func__ << ": event received:" << (int)e.code << endl +#define DEBUG_PRINT cout << __func__ << ": event received:" << (int)e << endl using namespace std; HSMUTTest::HSMUTTest() : HSM(&HSMUTTest::state_initialization) { - sEventBroker.subscribe(this, TOPIC_TEST); + EventBroker::getInstance().subscribe(this, TOPIC_TEST); } State HSMUTTest::state_initialization(const Event& e) @@ -105,7 +104,7 @@ State HSMUTTest::state_S(const Event& e) { State retState = HANDLED; DEBUG_PRINT; - switch (e.code) + switch (e) { case EV_ENTRY: break; @@ -138,7 +137,7 @@ State HSMUTTest::state_S1(const Event& e) { State retState = HANDLED; DEBUG_PRINT; - switch (e.code) + switch (e) { case EV_ENTRY: break; @@ -184,7 +183,7 @@ State HSMUTTest::state_S11(const Event& e) { State retState = HANDLED; DEBUG_PRINT; - switch (e.code) + switch (e) { case EV_ENTRY: break; @@ -220,7 +219,7 @@ State HSMUTTest::state_S2(const Event& e) { State retState = HANDLED; DEBUG_PRINT; - switch (e.code) + switch (e) { case EV_ENTRY: break; @@ -256,7 +255,7 @@ State HSMUTTest::state_S21(const Event& e) { State retState = HANDLED; DEBUG_PRINT; - switch (e.code) + switch (e) { case EV_ENTRY: break; @@ -281,7 +280,7 @@ State HSMUTTest::state_S211(const Event& e) { State retState = HANDLED; DEBUG_PRINT; - switch (e.code) + switch (e) { case EV_ENTRY: break; @@ -309,7 +308,7 @@ State HSMUTTest::state_S211(const Event& e) int main() { - sEventBroker.start(); + EventBroker::getInstance().start(); HSMUTTest& hsm = HSMUTTest::getInstance(); hsm.start(); diff --git a/src/tests/test-max485.cpp b/src/tests/test-max485.cpp index c63a1695a82d48f48a8a3aebe420835a4af2cdf2..18bb9077befedf2d449ad8707bf85a740e3b6dce 100644 --- a/src/tests/test-max485.cpp +++ b/src/tests/test-max485.cpp @@ -37,10 +37,10 @@ * * WIRINGS: * Max485 n1 | stm32f407vg_discovery ser 1 - * DI | PA9 + * DI | PA9 / PB6 * DE | PC9 * RE | PC8 - * RO | PA10 + * RO | PA10 / PB7 * VCC | 3.3/5 V * GND | GND * @@ -56,8 +56,6 @@ * A | A * B | B * - * WARNINGS: - * - the baudrate has to be extremely low. "guaranteed" at 2400 */ #include "string.h" @@ -77,7 +75,8 @@ using ctrlPin2_s2 = miosix::Gpio<GPIOC_BASE, 2>; char msg[64] = "Testing communication :D"; char rcv[64]; -int baudrates[6] = {2400, 3600, 4800, 9600, 19200, 115200}; +int baudrates[] = {2400, 3600, 4800, 9600, 19200, + 115200, 230400, 460800, 921600}; // function for the thread that has to read from serial void readSer(SerialInterface *s) @@ -132,7 +131,8 @@ int main() } printf("*** SERIAL 3 WORKING!\n"); - for (int iBaud = 0; iBaud < 6; iBaud++) + for (unsigned int iBaud = 0; + iBaud < sizeof(baudrates) / sizeof(baudrates[0]); iBaud++) { Thread::sleep(1000); diff --git a/src/tests/test-sensormanager.cpp b/src/tests/test-sensormanager.cpp index 72560d47c3398fcacb0a5a143809b5e747024464..1a8b06648b7c60c46f31c461c5485285622577d1 100644 --- a/src/tests/test-sensormanager.cpp +++ b/src/tests/test-sensormanager.cpp @@ -25,7 +25,7 @@ #include <sensors/Sensor.h> #include <sensors/SensorManager.h> #include <utils/Debug.h> -#include <utils/testutils/TestSensor.h> +#include <utils/TestUtils/TestSensor.h> #include <functional> #include <iostream> diff --git a/src/tests/utils/test-buttonhandler.cpp b/src/tests/utils/test-buttonhandler.cpp new file mode 100644 index 0000000000000000000000000000000000000000..a47acd7bf682edbd120dbb0aa92a84338b277561 --- /dev/null +++ b/src/tests/utils/test-buttonhandler.cpp @@ -0,0 +1,72 @@ +/* Copyright (c) 2022 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <miosix.h> +#include <utils/ButtonHandler/ButtonHandler.h> + +#include <functional> + +using namespace miosix; +using namespace Boardcore; +using namespace std; +using namespace placeholders; + +GpioPin button1 = GpioPin(GPIOG_BASE, 10); +GpioPin button2 = GpioPin(GPIOG_BASE, 9); + +void buttonCallback(ButtonEvent event, int buttonId); + +int main() +{ + button1.mode(Mode::INPUT); + button2.mode(Mode::INPUT); + + ButtonHandler::getInstance().registerButtonCallback( + button1, bind(buttonCallback, _1, 1)); + ButtonHandler::getInstance().registerButtonCallback( + button2, bind(buttonCallback, _1, 2)); + + while (true) + Thread::sleep(1000); +} + +void buttonCallback(ButtonEvent event, int buttonId) +{ + switch (event) + { + case ButtonEvent::PRESSED: + printf("Button %d pressed\n", buttonId); + break; + case ButtonEvent::SHORT_PRESS: + printf("Button %d released, it was a short press\n", buttonId); + break; + case ButtonEvent::LONG_PRESS: + printf("Button %d released, it was a long press\n", buttonId); + break; + case ButtonEvent::VERY_LONG_PRESS: + printf("Button %d released, it was a very long press\n", buttonId); + break; + + default: + break; + } +} \ No newline at end of file