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 7ba04dac130512cce20d22323bcbfdb06ad39a51..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-entry
+ - ./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/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
index b600d2af98677534acbe0414a25b1022a5d7f720..897e004670cc70a17c97858293aba85ad96709ab 100644
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -1,5 +1,59 @@
{
"configurations": [
+ {
+ "name": "stm32f407vg_stm32f4discovery",
+ "cStandard": "c11",
+ "cppStandard": "c++11",
+ "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
+ "defines": [
+ "DEBUG",
+ "_ARCH_CORTEXM4_STM32F4",
+ "_BOARD_STM32F407VG_STM32F4DISCOVERY",
+ "_MIOSIX_BOARDNAME=stm32f407vg_stm32f4discovery",
+ "HSE_VALUE=8000000",
+ "SYSCLK_FREQ_168MHz=168000000",
+ "_MIOSIX",
+ "__cplusplus=201103L"
+ ],
+ "includePath": [
+ "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f407vg_stm32f4discovery",
+ "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f407vg_stm32f4discovery",
+ "${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/stm32f407vg_stm32f4discovery",
+ "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f407vg_stm32f4discovery",
+ "${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",
+ "${workspaceFolder}/libs/fmt",
+ "${workspaceFolder}/src/shared",
+ "${workspaceFolder}/src/tests"
+ ],
+ "limitSymbolsToIncludedHeaders": true
+ },
+ "compileCommands": "${workspaceFolder}/build/compile_commands.json"
+ },
{
"name": "stm32f429zi_stm32f4discovery",
"cStandard": "c11",
@@ -24,6 +78,7 @@
"${workspaceFolder}/libs/mavlink_skyward_lib",
"${workspaceFolder}/libs/fmt/include",
"${workspaceFolder}/libs/eigen",
+ "${workspaceFolder}/libs/tscpp",
"${workspaceFolder}/libs",
"${workspaceFolder}/src/shared",
"${workspaceFolder}/src/tests"
@@ -50,8 +105,113 @@
"${workspaceFolder}/src/tests"
],
"limitSymbolsToIncludedHeaders": true
- },
- "compileCommands": "${workspaceFolder}/build/compile_commands.json"
+ }
+ },
+ {
+ "name": "stm32f429zi_skyward_death_stack_x",
+ "cStandard": "c11",
+ "cppStandard": "c++11",
+ "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
+ "defines": [
+ "DEBUG",
+ "_ARCH_CORTEXM4_STM32F4",
+ "_BOARD_STM32F429ZI_SKYWARD_DEATH_STACK_X",
+ "_MIOSIX_BOARDNAME=stm32f429zi_skyward_death_stack_x",
+ "HSE_VALUE=8000000",
+ "SYSCLK_FREQ_168MHz=168000000",
+ "_MIOSIX",
+ "__cplusplus=201103L"
+ ],
+ "includePath": [
+ "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_x",
+ "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_x",
+ "${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_skyward_death_stack_x",
+ "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_x",
+ "${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",
+ "${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",
+ "${workspaceFolder}/libs/fmt",
+ "${workspaceFolder}/src/shared",
+ "${workspaceFolder}/src/tests"
+ ],
+ "limitSymbolsToIncludedHeaders": true
+ }
}
],
"version": 4
diff --git a/.vscode/launch.json b/.vscode/launch.json
index f6dd88a2243d5f8ed189a89979fe54ac4128d51f..ef12355a4e4b74e6afa893f09f2e5d91cf40dabb 100644
--- a/.vscode/launch.json
+++ b/.vscode/launch.json
@@ -3,7 +3,7 @@
"configurations": [
{
"cwd": "${workspaceRoot}",
- "executable": "${workspaceFolder}/bin/${fileBasenameNoExtension}/${fileBasenameNoExtension}.elf",
+ "executable": "${workspaceFolder}/build/${fileBasenameNoExtension}",
"name": "ST-LINK V2 (current entrypoint)",
"request": "attach",
"type": "cortex-debug",
@@ -17,7 +17,7 @@
},
{
"cwd": "${workspaceRoot}",
- "executable": "${workspaceFolder}/bin/${fileBasenameNoExtension}/${fileBasenameNoExtension}.elf",
+ "executable": "${workspaceFolder}/build/${fileBasenameNoExtension}",
"name": "ST-LINK V1 (current entrypoint)",
"request": "attach",
"type": "cortex-debug",
diff --git a/.vscode/settings.json b/.vscode/settings.json
index bb68ef811f81ac7a0bfddb42a01bda684a1df228..43e514f2603ebb9bb2b90fdab6c1c80067e2f17d 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -98,11 +98,131 @@
"valarray": "cpp",
"ranges": "cpp",
"compare": "cpp",
- "concepts": "cpp"
+ "concepts": "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": [
+ "aelf",
+ "Alessandro",
+ "AMSL",
+ "atthr",
+ "AVDD",
+ "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",
- "miosix"
- ]
+ "GPIOC",
+ "GPIOD",
+ "GPIOE",
+ "GPIOF",
+ "GPIOG",
+ "GPIOS",
+ "gpstr",
+ "Hatt",
+ "HSCMAND",
+ "HSCMRNN",
+ "IDLEIE",
+ "irqn",
+ "irqv",
+ "Kalman",
+ "Katt",
+ "kbps",
+ "ldrex",
+ "LIFCR",
+ "logdecoder",
+ "Luca",
+ "MEKF",
+ "MINC",
+ "miosix",
+ "mkdir",
+ "MPXHZ",
+ "Musso",
+ "NATT",
+ "NBAR",
+ "NDTR",
+ "NGPS",
+ "NMAG",
+ "NMEKF",
+ "nord",
+ "NVIC",
+ "peripehral",
+ "PINC",
+ "Pitot",
+ "Plin",
+ "Qgbw",
+ "Qget",
+ "Qhandle",
+ "Qput",
+ "Qwait",
+ "Qwakeup",
+ "Riccardo",
+ "RXNE",
+ "RXNEIE",
+ "sats",
+ "Satt",
+ "SDRAM",
+ "spitch",
+ "sqdip",
+ "sqtrp",
+ "sroll",
+ "SSCDANN",
+ "SSCDRRN",
+ "stof",
+ "syaw",
+ "TCIE",
+ "TEIE",
+ "testsuite",
+ "TSCPP",
+ "Ublox",
+ "Unsync",
+ "upcounter",
+ "USART",
+ "velnord",
+ "Xbee",
+ "xnord",
+ "yned"
+ ],
+ "cSpell.language": "en",
+ "cSpell.enabled": true
}
diff --git a/CMakeLists.txt b/CMakeLists.txt
index b5369a8580e1b65fa13d3c70e17929555bf10d87..53273bb8481f7b3ac5c2385fd4505fd29a0f90fe 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_stm32f4discovery)
+sbs_target(test-serial stm32f407vg_stm32f4discovery)
add_executable(test-taskscheduler src/tests/scheduler/test-taskscheduler.cpp)
sbs_target(test-taskscheduler stm32f407vg_stm32f4discovery)
@@ -92,11 +92,11 @@ sbs_target(test-trace-logger stm32f429zi_stm32f4discovery)
# Tests - Catch #
#-----------------------------------------------------------------------------#
-add_executable(catch-tests-entry
+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
@@ -105,9 +105,56 @@ add_executable(catch-tests-entry
src/tests/catch/test-sensormanager-catch.cpp
src/tests/catch/xbee/test-xbee-parser.cpp
)
-target_compile_definitions(catch-tests-entry PRIVATE USE_MOCK_PERIPHERALS)
-sbs_target(catch-tests-entry stm32f429zi_stm32f4discovery)
-sbs_catch_test(catch-tests-entry)
+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 #
@@ -281,6 +322,9 @@ sbs_target(test-bmx160 stm32f429zi_skyward_death_stack_x)
add_executable(test-bmx160-with-correction src/tests/sensors/test-bmx160-with-correction.cpp)
sbs_target(test-bmx160-with-correction stm32f429zi_skyward_death_stack_x)
+add_executable(test-hx711 src/tests/sensors/test-hx711.cpp)
+sbs_target(test-hx711 stm32f429zi_stm32f4discovery)
+
add_executable(test-l3gd20 src/tests/sensors/test-l3gd20.cpp)
sbs_target(test-l3gd20 stm32f429zi_stm32f4discovery)
@@ -296,14 +340,17 @@ sbs_target(test-lis3mdl stm32f429zi_skyward_death_stack_x)
add_executable(test-max6675 src/tests/sensors/test-max6675.cpp)
sbs_target(test-max6675 stm32f429zi_stm32f4discovery)
+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)
add_executable(test-ubloxgps-serial src/tests/sensors/test-ubloxgps.cpp)
-sbs_target(test-ubloxgps-serial stm32f407vg_stm32f4discovery)
+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)
@@ -318,3 +365,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 36cf3d102918be929ec4500f01a50c8701105e60..85d11ef2498c72e2528591aabf6a092ebb82b0af 100644
--- a/cmake/boardcore-host.cmake
+++ b/cmake/boardcore-host.cmake
@@ -25,8 +25,11 @@ add_library(boardcore-host STATIC EXCLUDE_FROM_ALL
${SBS_BASE}/src/shared/diagnostic/CpuMeter.cpp
${SBS_BASE}/src/shared/diagnostic/PrintLogger.cpp
- # Radio
- ${SBS_BASE}/src/shared/radio/Xbee/APIFrameParser.cpp
+ # Actuators
+ ${SBS_BASE}/src/shared/actuators/Servo/Servo.cpp
+
+ # Drivers
+ ${SBS_BASE}/src/shared/drivers/timer/TimestampTimer.cpp
# Events
${SBS_BASE}/src/shared/events/EventBroker.cpp
@@ -34,10 +37,8 @@ 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
# Scheduler
${SBS_BASE}/src/shared/scheduler/TaskScheduler.cpp
@@ -46,19 +47,16 @@ add_library(boardcore-host STATIC EXCLUDE_FROM_ALL
${SBS_BASE}/src/shared/sensors/SensorManager.cpp
${SBS_BASE}/src/shared/sensors/SensorSampler.cpp
- # Timer
- ${SBS_BASE}/src/shared/drivers/timer/TimestampTimer.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)
+add_library(SkywardBoardcore::Boardcore::host ALIAS boardcore-host)
target_include_directories(boardcore-host PUBLIC ${SBS_BASE}/src/shared)
target_link_libraries(boardcore-host PUBLIC
- Miosix::Miosix-host
+ Miosix::Miosix::host
TSCPP::TSCPP
Eigen3::Eigen
fmt::fmt-header-only
diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake
index a37df344373e492d13a75e30d802312f17430004..2fdea04d4a7b2628845b1bcfc62952433eee4e2b 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
@@ -72,8 +74,10 @@ foreach(OPT_BOARD ${BOARDS})
${SBS_BASE}/src/shared/sensors/BMP280/BMP280.cpp
${SBS_BASE}/src/shared/sensors/BMX160/BMX160.cpp
${SBS_BASE}/src/shared/sensors/BMX160/BMX160WithCorrection.cpp
+ ${SBS_BASE}/src/shared/sensors/HX711/HX711.cpp
${SBS_BASE}/src/shared/sensors/calibration/SensorDataExtra.cpp
${SBS_BASE}/src/shared/sensors/MAX6675/MAX6675.cpp
+ ${SBS_BASE}/src/shared/sensors/MAX31855/MAX31855.cpp
${SBS_BASE}/src/shared/sensors/MBLoadCell/MBLoadCell.cpp
${SBS_BASE}/src/shared/sensors/MPU9250/MPU9250.cpp
${SBS_BASE}/src/shared/sensors/MS5803/MS5803.cpp
@@ -82,17 +86,21 @@ foreach(OPT_BOARD ${BOARDS})
${SBS_BASE}/src/shared/sensors/UbloxGPS/UbloxGPS.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})
+ add_library(SkywardBoardcore::Boardcore::${OPT_BOARD} ALIAS ${BOARDCORE_LIBRARY})
target_include_directories(${BOARDCORE_LIBRARY} PUBLIC ${SBS_BASE}/src/shared)
target_link_libraries(${BOARDCORE_LIBRARY} PUBLIC
- Miosix::Miosix-${OPT_BOARD}
- Mxgui::Mxgui-${OPT_BOARD}
+ Miosix::Miosix::${OPT_BOARD}
+ Mxgui::Mxgui::${OPT_BOARD}
TSCPP::TSCPP
Eigen3::Eigen
fmt::fmt-header-only
diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index 1043f75367791030a36a6f5f2285482eed9b2aec..5e77e8cd88ceb12f063df6c60b3f9abc5d7c9e4f 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -28,6 +28,7 @@ include(${KPATH}/config/boards.cmake)
add_subdirectory(${SBS_BASE}/libs/miosix-host EXCLUDE_FROM_ALL)
+set(KPATH ${KPATH} CACHE PATH "Path to kernel directory")
add_subdirectory(${SBS_BASE}/libs/mxgui EXCLUDE_FROM_ALL)
add_subdirectory(${SBS_BASE}/libs/tscpp EXCLUDE_FROM_ALL)
diff --git a/cmake/sbs.cmake b/cmake/sbs.cmake
index 17f5f3ef5c0d2372ece9d4d751a70e248268806e..e3d2065733043e139997f535b224820f82e00b8a 100644
--- a/cmake/sbs.cmake
+++ b/cmake/sbs.cmake
@@ -47,7 +47,7 @@ function(sbs_target TARGET OPT_BOARD)
endif()
target_include_directories(${TARGET} PRIVATE src/shared)
if(CMAKE_CROSSCOMPILING)
- target_link_libraries(${TARGET} PRIVATE SkywardBoardcore::Boardcore-${OPT_BOARD})
+ target_link_libraries(${TARGET} PRIVATE SkywardBoardcore::Boardcore::${OPT_BOARD})
add_custom_command(
TARGET ${TARGET} POST_BUILD
COMMAND ${CMAKE_OBJCOPY} -O ihex ${TARGET} ${TARGET}.hex
@@ -56,7 +56,7 @@ function(sbs_target TARGET OPT_BOARD)
VERBATIM
)
else()
- target_link_libraries(${TARGET} PRIVATE SkywardBoardcore::Boardcore-host)
+ target_link_libraries(${TARGET} PRIVATE SkywardBoardcore::Boardcore::host)
endif()
endfunction()
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/miosix-host b/libs/miosix-host
index f93e4e7f8025322c365e649cad96b65faf9d6ab5..ed8a4d8c24e59ce7513b1cebc9b6427fde55c7ce 160000
--- a/libs/miosix-host
+++ b/libs/miosix-host
@@ -1 +1 @@
-Subproject commit f93e4e7f8025322c365e649cad96b65faf9d6ab5
+Subproject commit ed8a4d8c24e59ce7513b1cebc9b6427fde55c7ce
diff --git a/libs/miosix-kernel b/libs/miosix-kernel
index a5d535dd506f30399edb3e315de41e2f97e87c4b..441aca88b44e5438e04abbef1639cd7f9df7bb37 160000
--- a/libs/miosix-kernel
+++ b/libs/miosix-kernel
@@ -1 +1 @@
-Subproject commit a5d535dd506f30399edb3e315de41e2f97e87c4b
+Subproject commit 441aca88b44e5438e04abbef1639cd7f9df7bb37
diff --git a/libs/mxgui b/libs/mxgui
index 3c3954db30418264863e8dda93ef3f8a91e00695..0052374bc85c2bf90753faba61ecc08905658c83 160000
--- a/libs/mxgui
+++ b/libs/mxgui
@@ -1 +1 @@
-Subproject commit 3c3954db30418264863e8dda93ef3f8a91e00695
+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..2523965cd9ce3b7e20457d7d5c1f047d90386c7c 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..670873a182784e3a882b1649fb48e69502e3b164 100644
--- a/scripts/logdecoder/logdecoder.cpp
+++ b/scripts/logdecoder/logdecoder.cpp
@@ -1,59 +1,139 @@
-/***************************************************************************
- * 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(string commandName);
+
+/**
+ * @brief Deserialize a file.
+ *
+ * @param fileName File name complete with extension.
+ * @return Whether the deserialization was successful.
+ */
+bool deserialize(string fileName);
+
+/**
+ * @brief Deserialize all log file in the directory. Assumes the log files named
+ * as logXX.dat.
+ *
+ * Scans for all the 100 possible log files and decode the ones found.
+ *
+ * @return False if an error was encountered.
+ */
+bool deserializeAll();
+
+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;
+ }
-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&) {
+ // File deserialization
+ if (arg1 == "-a" || arg1 == "--all")
+ success = deserializeAll();
+ else
+ success = deserialize(arg1);
+
+ // End
+ if (success)
+ std::cout << "Deserialization completed successfully\n";
+ else
+ std::cout << "Deserialization ended with errors\n";
return 0;
}
+
+void showUsage(string commandName)
+{
+ std::cerr << "Usage: " << commandName << " {-a | <log_file_name> | -h}"
+ << "Options:\n"
+ << "\t-h,--help\t\tShow this help message\n"
+ << "\t-a,--all Deserialize all logs in the current directory "
+ "named as logXX.dat\n"
+ << std::endl;
+}
+
+bool deserialize(string fileName)
+{
+ std::cout << "Deserializing " << fileName << "...\n";
+
+ Deserializer d(fileName);
+ LogTypes::registerTypes(d);
+
+ return d.deserialize();
+}
+
+bool deserializeAll()
+{
+ std::cout << "Deserializing all logs in the current directory...\n";
+
+ for (int i = 0; i < 100; i++)
+ {
+ char fileName[11];
+ sprintf(fileName, "log%02d.dat", i);
+ struct stat st;
+
+ // Check if the current logfile exists
+ if (stat(fileName, &st) != 0)
+ continue;
+
+ // File found
+ if (!deserialize(string(fileName)))
+ return false;
+
+ std::cout << fileName << " deserialized successfully";
+ }
+
+ return true;
+}
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..fbb00378cb7c5c7e0d21d41616847012ce166ad4
--- /dev/null
+++ b/src/entrypoints/imu-calibration.cpp
@@ -0,0 +1,131 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Authors: 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 8a84d3533bec6477c13def0060f364d9e31649d1..89c3260272c61d4c0e2e4247573fc97362176ce3 100644
--- a/src/entrypoints/kernel-testsuite.cpp
+++ b/src/entrypoints/kernel-testsuite.cpp
@@ -187,8 +187,7 @@ int main()
break;
}
// For testing mpu
- unsigned int *m;
- (void)m; // Disable unused warning
+ unsigned int *m __attribute__((unused));
switch (c)
{
case 't':
@@ -427,9 +426,7 @@ void process_test_file_concurrency()
fail("Wrong data from file 2");
}
- // cppcheck-suppress nullPointerRedundantCheck
fclose(f1);
- // cppcheck-suppress nullPointerRedundantCheck
fclose(f2);
pass();
@@ -587,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");
@@ -599,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");
@@ -608,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");
@@ -638,9 +635,8 @@ and test argv pointer passing
static volatile bool t1_v1;
-static void t1_p1(void *argv)
+static void t1_p1(void *argv __attribute__((unused)))
{
- (void)argv;
for (;;)
{
if (Thread::testTerminate())
@@ -742,10 +738,8 @@ Thread::getCurrentThread
static volatile bool t2_v1;
static Thread *t2_p_v1;
-static void t2_p1(void *argv)
+static void t2_p1(void *argv __attribute__((unused)))
{
- (void)argv;
-
// This is to fix a race condition: the immediately after the thread
// creation a yield occurs, t2_p_v1 is not yet assigned so the check fails
Thread::sleep(5);
@@ -762,9 +756,8 @@ static void t2_p1(void *argv)
}
}
-static void t2_p2(void *argv)
+static void t2_p2(void *argv __attribute__((unused)))
{
- (void)argv;
while (Thread::testTerminate() == false)
t2_v1 = true;
}
@@ -836,9 +829,8 @@ getTick()
also tests creation of multiple instances of the same thread
*/
-static void t3_p1(void *argv)
+static void t3_p1(void *argv __attribute__((unused)))
{
- (void)argv;
const int SLEEP_TIME = 100;
for (;;)
{
@@ -855,9 +847,8 @@ static void t3_p1(void *argv)
static volatile bool t3_v2; // Set to true by t3_p2
static volatile bool t3_deleted; // Set when an instance of t3_p2 is deleted
-static void t3_p2(void *argv)
+static void t3_p2(void *argv __attribute__((unused)))
{
- (void)argv;
const int SLEEP_TIME = 15;
for (;;)
{
@@ -959,9 +950,8 @@ Thread::IRQgetPriority
static volatile bool t4_v1;
-static void t4_p1(void *argv)
+static void t4_p1(void *argv __attribute__((unused)))
{
- (void)argv;
for (;;)
{
if (Thread::testTerminate())
@@ -1112,9 +1102,8 @@ static volatile bool t5_v1;
static volatile bool
t5_v2; // False=testing Thread::wait() else Thread::IRQwait()
-static void t5_p1(void *argv)
+static void t5_p1(void *argv __attribute__((unused)))
{
- (void)argv;
for (;;)
{
if (Thread::testTerminate())
@@ -1231,9 +1220,8 @@ static Sequence seq;
static Mutex t6_m1;
static FastMutex t6_m1a;
-static void t6_p1(void *argv)
+static void t6_p1(void *argv __attribute__((unused)))
{
- (void)argv;
t6_m1.lock();
seq.add('1');
Thread::sleep(100);
@@ -1244,9 +1232,8 @@ static void t6_p1(void *argv)
t6_m1.unlock();
}
-static void t6_p2(void *argv)
+static void t6_p2(void *argv __attribute__((unused)))
{
- (void)argv;
t6_m1.lock();
seq.add('2');
Thread::sleep(100);
@@ -1256,9 +1243,8 @@ static void t6_p2(void *argv)
t6_m1.unlock();
}
-static void t6_p3(void *argv)
+static void t6_p3(void *argv __attribute__((unused)))
{
- (void)argv;
t6_m1.lock();
seq.add('3');
Thread::sleep(100);
@@ -1268,9 +1254,8 @@ static void t6_p3(void *argv)
t6_m1.unlock();
}
-static void t6_p4(void *argv)
+static void t6_p4(void *argv __attribute__((unused)))
{
- (void)argv;
t6_m1.lock();
for (;;)
{
@@ -1287,36 +1272,32 @@ static void t6_p4(void *argv)
}
}
-static void t6_p1a(void *argv)
+static void t6_p1a(void *argv __attribute__((unused)))
{
- (void)argv;
t6_m1a.lock();
seq.add('1');
Thread::sleep(100);
t6_m1a.unlock();
}
-static void t6_p2a(void *argv)
+static void t6_p2a(void *argv __attribute__((unused)))
{
- (void)argv;
t6_m1a.lock();
seq.add('2');
Thread::sleep(100);
t6_m1a.unlock();
}
-static void t6_p3a(void *argv)
+static void t6_p3a(void *argv __attribute__((unused)))
{
- (void)argv;
t6_m1a.lock();
seq.add('3');
Thread::sleep(100);
t6_m1a.unlock();
}
-static void t6_p4a(void *argv)
+static void t6_p4a(void *argv __attribute__((unused)))
{
- (void)argv;
t6_m1a.lock();
for (;;)
{
@@ -1337,9 +1318,8 @@ static volatile bool t6_v1;
static Mutex t6_m2;
static FastMutex t6_m2a;
-static void t6_p5(void *argv)
+static void t6_p5(void *argv __attribute__((unused)))
{
- (void)argv;
for (;;)
{
if (Thread::testTerminate())
@@ -1354,9 +1334,8 @@ static void t6_p5(void *argv)
}
}
-static void t6_p5a(void *argv)
+static void t6_p5a(void *argv __attribute__((unused)))
{
- (void)argv;
for (;;)
{
if (Thread::testTerminate())
@@ -1395,9 +1374,8 @@ static void t6_p6a(void *argv)
static Mutex t6_m5(Mutex::RECURSIVE);
static FastMutex t6_m5a(FastMutex::RECURSIVE);
-static void *t6_p7(void *argv)
+static void *t6_p7(void *argv __attribute__((unused)))
{
- (void)argv;
if (t6_m5.tryLock() == false)
return reinterpret_cast<void *>(1); // 1 = locked
t6_m5.unlock();
@@ -1412,9 +1390,8 @@ bool checkIft6_m5IsLocked()
return reinterpret_cast<int>(result) == 0 ? false : true;
}
-static void *t6_p7a(void *argv)
+static void *t6_p7a(void *argv __attribute__((unused)))
{
- (void)argv;
if (t6_m5a.tryLock() == false)
return reinterpret_cast<void *>(1); // 1 = locked
t6_m5a.unlock();
@@ -1896,9 +1873,8 @@ FIXME: The overloaded versions of IRQput and IRQget are not tested
static Queue<char, 4> t8_q1;
static Queue<char, 4> t8_q2;
-static void t8_p1(void *argv)
+static void t8_p1(void *argv __attribute__((unused)))
{
- (void)argv;
for (;;)
{
if (Thread::testTerminate())
@@ -2157,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
@@ -2176,9 +2152,8 @@ void t10_f2()
}
}
-void t10_p1(void *argv)
+void t10_p1(void *argv __attribute__((unused)))
{
- (void)argv;
t10_f2();
fail("Exception not thrown");
}
@@ -2204,9 +2179,8 @@ tests class MemoryProfiling
static volatile unsigned int t11_v1; // Free heap after spawning thread
-void t11_p1(void *argv)
+void t11_p1(void *argv __attribute__((unused)))
{
- (void)argv;
if (MemoryProfiling::getStackSize() != STACK_SMALL)
fail("getStackSize (2)");
// Check that getCurrentFreeHeap returns the same value from different
@@ -2256,18 +2230,13 @@ Additional test for priority inheritance
Mutex t12_m1;
Mutex t12_m2;
-void t12_p1(void *argv)
+void t12_p1(void *argv __attribute__((unused)))
{
- (void)argv;
Lock<Mutex> l1(t12_m1);
Lock<Mutex> l2(t12_m2);
}
-void t12_p2(void *argv)
-{
- (void)argv;
- Lock<Mutex> l(t12_m1);
-}
+void t12_p2(void *argv __attribute__((unused))) { Lock<Mutex> l(t12_m1); }
void test_12()
{
@@ -2485,9 +2454,8 @@ static int t15_v3;
static ConditionVariable t15_c1;
static Mutex t15_m1;
-void t15_p1(void *argv)
+void t15_p1(void *argv __attribute__((unused)))
{
- (void)argv;
for (int i = 0; i < 10; i++)
{
Lock<Mutex> l(t15_m1);
@@ -2497,9 +2465,8 @@ void t15_p1(void *argv)
}
}
-void t15_p2(void *argv)
+void t15_p2(void *argv __attribute__((unused)))
{
- (void)argv;
for (int i = 0; i < 10; i++)
{
Lock<Mutex> l(t15_m1);
@@ -2622,9 +2589,8 @@ void *t16_p2(void *argv)
pthread_cond_t t16_c1 = PTHREAD_COND_INITIALIZER;
-void *t16_p3(void *argv)
+void *t16_p3(void *argv __attribute__((unused)))
{
- (void)argv;
Thread::sleep(30);
if (pthread_mutex_trylock(&t16_m1) != 0)
fail("cond_wait did not release mutex");
@@ -2638,9 +2604,8 @@ void *t16_p3(void *argv)
pthread_cond_t t16_c2;
-void *t16_p4(void *argv)
+void *t16_p4(void *argv __attribute__((unused)))
{
- (void)argv;
Thread::sleep(30);
if (pthread_mutex_trylock(&t16_m1) != 0)
fail("cond_wait did not release mutex (2)");
@@ -3093,9 +3058,8 @@ static void be()
bq.bufferEmptied();
}
-static void t19_p1(void *argv)
+static void t19_p1(void *argv __attribute__((unused)))
{
- (void)argv;
Thread::sleep(50);
{
FastInterruptDisableLock dLock;
@@ -3628,9 +3592,8 @@ struct t22_s1
int b;
};
-static void t22_t2(void *argv)
+static void t22_t2(void *argv __attribute__((unused)))
{
- (void)argv;
while (Thread::testTerminate() == false)
{
t22_v5 = true;
@@ -4208,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>();
@@ -4428,9 +4391,8 @@ Also tests concurrent write by opening and writing 3 files from 3 threads
*/
static volatile bool fs_1_error;
-static void fs_t1_p1(void *argv)
+static void fs_t1_p1(void *argv __attribute__((unused)))
{
- (void)argv;
FILE *f;
if ((f = fopen("/sd/testdir/file_1.txt", "w")) == NULL)
{
@@ -4460,9 +4422,8 @@ static void fs_t1_p1(void *argv)
}
}
-static void fs_t1_p2(void *argv)
+static void fs_t1_p2(void *argv __attribute__((unused)))
{
- (void)argv;
FILE *f;
if ((f = fopen("/sd/testdir/file_2.txt", "w")) == NULL)
{
@@ -4492,9 +4453,8 @@ static void fs_t1_p2(void *argv)
}
}
-static void fs_t1_p3(void *argv)
+static void fs_t1_p3(void *argv __attribute__((unused)))
{
- (void)argv;
FILE *f;
if ((f = fopen("/sd/testdir/file_3.txt", "w")) == NULL)
{
@@ -4570,6 +4530,7 @@ static void fs_test_1()
i = 0;
for (;;)
{
+ // cppcheck-suppress nullPointerRedundantCheck
j = fread(buf, 1, 1024, f);
if (j == 0)
break;
@@ -4589,6 +4550,7 @@ static void fs_test_1()
i = 0;
for (;;)
{
+ // cppcheck-suppress nullPointerRedundantCheck
j = fread(buf, 1, 1024, f);
if (j == 0)
break;
@@ -4608,6 +4570,7 @@ static void fs_test_1()
i = 0;
for (;;)
{
+ // cppcheck-suppress nullPointerRedundantCheck
j = fread(buf, 1, 1024, f);
if (j == 0)
break;
@@ -4633,6 +4596,7 @@ static void fs_test_1()
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
@@ -4645,11 +4609,9 @@ 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");
- // 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:
@@ -4660,45 +4622,33 @@ 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");
- // cppcheck-suppress nullPointerRedundantCheck
if (ftell(f) != (21 * 128 - 1))
{
iprintf("ftell=%ld\n", ftell(f));
fail("ftell() 1");
}
- // cppcheck-suppress nullPointerRedundantCheck
fseek(f, 21 + 17, SEEK_SET); // Seek to 002\n
- // cppcheck-suppress nullPointerRedundantCheck
if ((fgetc(f) != '0') | (fgetc(f) != '0') | (fgetc(f) != '2') |
- // cppcheck-suppress nullPointerRedundantCheck
(fgetc(f) != '\n'))
fail("fgetc SEEK_SET");
- // 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
- // cppcheck-suppress nullPointerRedundantCheck
if ((fgetc(f) != '0') | (fgetc(f) != '5') | (fgetc(f) != '3') |
- // cppcheck-suppress nullPointerRedundantCheck
(fgetc(f) != '\n'))
fail("fgetc SEEK_CUR");
- // cppcheck-suppress nullPointerRedundantCheck
if (ftell(f) != (21 * 53))
{
iprintf("ftell=%ld\n", ftell(f));
fail("ftell() 2");
}
- // cppcheck-suppress nullPointerRedundantCheck
if (fclose(f) != 0)
fail("Can't close r2 file_4.txt");
// Testing remove()
@@ -4792,7 +4742,6 @@ 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");
- // cppcheck-suppress nullPointerRedundantCheck
fclose(f);
if (unlink((d + filename2).c_str()))
fail("unlink 3");
@@ -4842,10 +4791,10 @@ 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");
}
- // cppcheck-suppress nullPointerRedundantCheck
if (fclose(f) != 0)
fail("close 1");
@@ -4856,11 +4805,11 @@ 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);
}
- // cppcheck-suppress nullPointerRedundantCheck
if (fclose(f) != 0)
fail("close 2");
delete[] buf;
@@ -5063,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)
@@ -5162,9 +5112,8 @@ tests:
context switch speed
*/
-static void b2_p1(void *argv)
+static void b2_p1(void *argv __attribute__((unused)))
{
- (void)argv;
for (;;)
{
if (Thread::testTerminate())
@@ -5219,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.
*/
@@ -5310,14 +5259,13 @@ quit:
//
/*
tests:
-Mutex lonk/unlock time
+Mutex lock/unlock time
*/
volatile bool b4_end = false;
-void b4_t1(void *argv)
+void b4_t1(void *argv __attribute__((unused)))
{
- (void)argv;
Thread::sleep(1000);
b4_end = true;
}
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 74%
rename from src/shared/drivers/servo/Servo.cpp
rename to src/shared/actuators/Servo/Servo.cpp
index ce0f9330209bbd1137bcc5000443a8fcda21618c..b4d9b8e647261844027f406fcf268e04bae68611 100644
--- a/src/shared/drivers/servo/Servo.cpp
+++ b/src/shared/actuators/Servo/Servo.cpp
@@ -27,26 +27,57 @@
namespace Boardcore
{
+#ifndef COMPILE_FOR_HOST
+
Servo::Servo(TIM_TypeDef* const timer, TimerUtils::Channel pwmChannel,
unsigned int minPulse, unsigned int maxPulse,
- unsigned int frequency)
+ unsigned int frequency, unsigned int resetPulse)
: pwm(timer, frequency), pwmChannel(pwmChannel), minPulse(minPulse),
- maxPulse(maxPulse), frequency(frequency)
+ maxPulse(maxPulse), resetPulse(resetPulse), frequency(frequency)
+{
+ setPosition(resetPulse);
+}
+
+Servo::Servo(TIM_TypeDef* const timer, TimerUtils::Channel pwmChannel,
+ unsigned int minPulse, unsigned int maxPulse,
+ unsigned int frequency)
+ : Servo(timer, pwmChannel, minPulse, maxPulse, frequency, minPulse)
{
- setPosition(0);
}
void Servo::enable() { pwm.enableChannel(pwmChannel); }
void Servo::disable() { pwm.disableChannel(pwmChannel); }
+void Servo::reset() { setPosition(resetPulse); }
+
+#else
+
+Servo::Servo(unsigned int minPulse, unsigned int maxPulse,
+ unsigned int frequency)
+ : minPulse(minPulse), maxPulse(maxPulse), frequency(frequency),
+ resetPulse(minPulse)
+{
+ setPosition(0);
+}
+
+void Servo::enable() {}
+
+void Servo::disable() {}
+
+#endif
+
void Servo::setPosition(float position)
{
float pulse = minPulse + (maxPulse - minPulse) * position;
float dutyCycle = pulse * frequency / 1000000.0f;
+#ifndef COMPILE_FOR_HOST
pwm.setDutyCycle(pwmChannel, dutyCycle);
+#else
+ this->dutyCycle = dutyCycle;
+#endif
}
void Servo::setPosition90Deg(float degrees) { setPosition(degrees / 90); }
@@ -59,7 +90,11 @@ void Servo::setPosition360Deg(float degrees) { setPosition(degrees / 360); }
float Servo::getPosition()
{
+#ifndef COMPILE_FOR_HOST
float dutyCycle = pwm.getDutyCycle(pwmChannel);
+#else
+ float dutyCycle = this->dutyCycle;
+#endif
float pulse = dutyCycle * 1000000.0f / frequency;
diff --git a/src/shared/drivers/servo/Servo.h b/src/shared/actuators/Servo/Servo.h
similarity index 81%
rename from src/shared/drivers/servo/Servo.h
rename to src/shared/actuators/Servo/Servo.h
index f070f196b1a7a2d2af03534ac90f18f591b6e661..43c1ca54a708a0171dab661297ce97a1944dc6a1 100644
--- a/src/shared/drivers/servo/Servo.h
+++ b/src/shared/actuators/Servo/Servo.h
@@ -20,7 +20,9 @@
* THE SOFTWARE.
*/
+#ifndef COMPILE_FOR_HOST
#include <drivers/timer/PWM.h>
+#endif
#pragma once
@@ -54,6 +56,7 @@ namespace Boardcore
class Servo
{
public:
+#ifndef COMPILE_FOR_HOST
/**
* @brief Prepare the timer and sets the PWM output to the minimum.
*
@@ -73,6 +76,25 @@ public:
explicit Servo(TIM_TypeDef* const timer, TimerUtils::Channel pwmChannel,
unsigned int minPulse = 1000, unsigned int maxPulse = 2000,
unsigned int frequency = 50);
+ /**
+ * @brief Prepare the timer and sets the PWM output to the minimum.
+ *
+ * @see Servo::Servo
+ *
+ * @param timer Timer peripheral used for the PWM signal.
+ * @param pwmChannel Timer's channel used for the PWM signal.
+ * @param frequency Frequency of the PWM driving the H-bridge.
+ * @param minPulse Minimum signal pulse in microseconds.
+ * @param maxPulse Maximum signal pulse in microseconds.
+ * @param resetPulse Reset signal pulse in microseconds.
+ */
+ explicit Servo(TIM_TypeDef* const timer, TimerUtils::Channel pwmChannel,
+ unsigned int minPulse, unsigned int maxPulse,
+ unsigned int frequency, unsigned int resetPulse);
+#else
+ explicit Servo(unsigned int minPulse = 1000, unsigned int maxPulse = 2000,
+ unsigned int frequency = 50);
+#endif
/**
* @brief Starts producing the PWM signal.
@@ -84,6 +106,11 @@ public:
*/
void disable();
+ /**
+ * @brief Moves the servo to the reset position.
+ */
+ void reset();
+
/**
* @brief Set the position of the servomotor.
*
@@ -120,11 +147,16 @@ private:
Servo& operator=(const Servo&) = delete;
Servo(const Servo& s) = delete;
+#ifndef COMPILE_FOR_HOST
PWM pwm;
TimerUtils::Channel pwmChannel;
+#else
+ float dutyCycle;
+#endif
float minPulse;
float maxPulse;
+ float resetPulse;
float frequency;
};
diff --git a/src/shared/drivers/stepper/Stepper.h b/src/shared/actuators/Stepper.h
similarity index 95%
rename from src/shared/drivers/stepper/Stepper.h
rename to src/shared/actuators/Stepper.h
index fa22b4641be1ce4e4a98c9ce85eac281ea360bce..b08639af1075f1d3534df3fd5543f3fe0e5e6b25 100644
--- a/src/shared/drivers/stepper/Stepper.h
+++ b/src/shared/actuators/Stepper.h
@@ -20,9 +20,11 @@
* THE SOFTWARE.
*/
+#pragma once
+
#include <interfaces-impl/gpio_impl.h>
#include <interfaces/delays.h>
-#include <utils/testutils/MockGpioPin.h>
+#include <utils/TestUtils/MockGpioPin.h>
namespace Boardcore
{
@@ -74,6 +76,12 @@ public:
*/
void setMicrostepping(Microstep microstep);
+ /**
+ * @brief Zeros the driver's internal position.
+ *
+ */
+ void zeroPosition();
+
/**
* @brief Move the stepper motor by the specified amount of steps.
*/
@@ -184,6 +192,8 @@ inline void Stepper::setMicrostepping(Microstep microstep)
}
}
+void Stepper::zeroPosition() { currentPosition = 0; }
+
inline void Stepper::move(int32_t steps)
{
if (speed == 0)
@@ -229,7 +239,7 @@ inline uint32_t Stepper::getCurrentPosition() { return currentPosition; }
inline float Stepper::getCurrentDegPosition()
{
- return currentPosition * stepAngle;
+ return currentPosition * stepAngle / getMicrosteppingValue();
}
inline int Stepper::getMicrosteppingValue()
@@ -251,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
new file mode 100644
index 0000000000000000000000000000000000000000..56d31830cdc7218ecd795ab34e003e57261a13d8
--- /dev/null
+++ b/src/shared/algorithms/Algorithm.h
@@ -0,0 +1,70 @@
+/* Copyright (c) 2020 Skyward Experimental Rocketry
+ * Author: Luca Conterio
+ *
+ * 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
+
+namespace Boardcore
+{
+
+class Algorithm
+{
+public:
+ /**
+ * @brief Initializes the Algorithm object, must be called as soon as the
+ * object is created.
+ * */
+ virtual bool init() = 0;
+
+ /**
+ * @brief Starts the execution of the algorithm and set the running flag to
+ * true.
+ * */
+ void begin() { running = true; }
+
+ /**
+ * @brief Terminates the algorithm's execution and sets the running flag to
+ * false.
+ * */
+ void end() { running = false; }
+
+ /**
+ * @brief Checks wether the algorithm is in a running state or not, and
+ * eventually calls the @see{step} routine.
+ * */
+ void update()
+ {
+ if (running)
+ {
+ step();
+ }
+ }
+
+protected:
+ /**
+ * @brief The actual algorithm step.
+ */
+ virtual void step() = 0;
+
+ bool running = false;
+};
+
+} // namespace Boardcore
diff --git a/src/shared/kalman/KalmanEigen.h b/src/shared/algorithms/Kalman/Kalman.h
similarity index 97%
rename from src/shared/kalman/KalmanEigen.h
rename to src/shared/algorithms/Kalman/Kalman.h
index 214e1184e9f75f2d7835fde22f99826ce533d2c3..204311d7532230955459247fcc4ece54e2777785 100644
--- a/src/shared/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/PID.h b/src/shared/algorithms/PID.h
index 727f1e5477eddb3f51eed688e824775d2d2ea706..5f460f4381ee7e9e9e9aa16886dda6850ca620b6 100644
--- a/src/shared/algorithms/PID.h
+++ b/src/shared/algorithms/PID.h
@@ -47,16 +47,17 @@ public:
double update(double error)
{
- double p = Kp * error;
+ lastP = Kp * error;
if (!saturation)
{
i = i + Ki * Ts * error;
}
- double u = p + i;
+ double u = lastP + i;
- return antiWindUp(u);
+ lastOutput = antiWindUp(u);
+ return lastOutput;
}
/**
@@ -84,13 +85,21 @@ public:
double getI() { return i; }
+ double getLastP() { return lastP; }
+
+ double getLastOutput() { return lastOutput; }
+
+ bool isSaturated() { return saturation; }
+
double Kp; // Proportional factor.
double Ki; // Integral factor.
double Ts; // Sampling period.
double uMin, uMax; // Anti-windup limits.
private:
- double i = 0; // Integral contribution.
+ double i = 0; // Integral contribution.
+ double lastP = 0; // Last computer proportional contribution.
+ double lastOutput; // Last computed controller output.
bool saturation = false;
};
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 3ad39429b7dd4ff38d476f2ef0ad3eb77bf64f3a..d8b253b77e4aa9264b0227fd32926af43ef1ee36 100644
--- a/src/shared/diagnostic/PrintLogger.h
+++ b/src/shared/diagnostic/PrintLogger.h
@@ -27,7 +27,6 @@
#include <fmt/format.h>
#include <logger/Logger.h>
#include <utils/Constants.h>
-#include <utils/Unused.h>
#include <utils/collections/CircularBuffer.h>
#include <memory>
@@ -101,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/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 de60c5ed54ae18cfb7156e3a4f31826e6f434ee7..8d2077fca048d27085f7f3f64447ac7ca5222663 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..00428f59ac6fc23490b067bef83a21c51e26b58d 100644
--- a/src/shared/drivers/timer/BasicTimer.h
+++ b/src/shared/drivers/timer/BasicTimer.h
@@ -85,7 +85,7 @@ public:
explicit BasicTimer(TIM_TypeDef *timer);
/**
- * @brief Resets the registers and disables the peripheral clock.
+ * @brief Disables the peripheral clock.
*/
~BasicTimer();
@@ -207,11 +207,7 @@ 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; }
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..b6f8a6aca9e5ec5b1fa782d36cbf18a010c802d8 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,8 +84,8 @@ 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());
}
void PWM::setTimerConfiguration()
diff --git a/src/shared/drivers/timer/PWM.h b/src/shared/drivers/timer/PWM.h
index 988613fa78738e2bbbd93d15c244c937cc3b2645..b25e508ef893510f406e89cfd47161ca5ef9911c 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
diff --git a/src/shared/drivers/timer/TimerUtils.h b/src/shared/drivers/timer/TimerUtils.h
index b575dcb16a35b3a11e0f42e2f60aa2cf039ba1a6..64d7e6801881e12f836fa62de423f83e0ae8477d 100644
--- a/src/shared/drivers/timer/TimerUtils.h
+++ b/src/shared/drivers/timer/TimerUtils.h
@@ -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.cpp b/src/shared/drivers/timer/TimestampTimer.cpp
index 7c0739fadaf48ba13172009ab36bfa41cc520321..8faa0167753afc9907b8d245aeeda165f5fb4971 100644
--- a/src/shared/drivers/timer/TimestampTimer.cpp
+++ b/src/shared/drivers/timer/TimestampTimer.cpp
@@ -27,27 +27,21 @@
namespace Boardcore
{
-void TimestampTimer::resetTimestamp()
+TimestampTimer::TimestampTimer()
{
-#ifndef COMPILE_FOR_HOST
- timer.setCounter(0);
-#endif
+ initTimestampTimer();
+ enableTimestampTimer();
}
#ifndef COMPILE_FOR_HOST
+
TIM_TypeDef *TimestampTimer::getTimer() { return timer.getTimer(); }
-#endif
-TimestampTimer::TimestampTimer()
-{
- initTimestampTimer();
- enableTimestampTimer();
-}
+void TimestampTimer::resetTimestamp() { timer.setCounter(0); }
// TODO: Keep support for STM32F103
void TimestampTimer::initTimestampTimer()
{
-#ifndef COMPILE_FOR_HOST
{
miosix::FastInterruptDisableLock dLock;
// Enable TIM2 peripheral clock
@@ -60,7 +54,6 @@ void TimestampTimer::initTimestampTimer()
// Generate an update event to apply the new prescaler value
timer.generateUpdate();
-#endif
PrintLogger logger = Logging::getLogger("timestamptimer");
LOG_INFO(logger, "Initialized timestamp timer");
@@ -68,12 +61,28 @@ void TimestampTimer::initTimestampTimer()
void TimestampTimer::enableTimestampTimer()
{
-#ifndef COMPILE_FOR_HOST
timer.enable();
-#endif
PrintLogger logger = Logging::getLogger("timestamptimer");
LOG_INFO(logger, "Enabled timestamp timer");
}
+#else
+
+void TimestampTimer::resetTimestamp() {}
+
+void TimestampTimer::initTimestampTimer()
+{
+ PrintLogger logger = Logging::getLogger("timestamptimer");
+ LOG_INFO(logger, "Initialized timestamp timer [COMPILE_FOR_HOST]");
+}
+
+void TimestampTimer::enableTimestampTimer()
+{
+ PrintLogger logger = Logging::getLogger("timestamptimer");
+ LOG_INFO(logger, "Enabled timestamp timer [COMPILE_FOR_HOST]");
+}
+
+#endif
+
} // namespace Boardcore
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/HSM.h b/src/shared/events/HSM.h
index 01020e669b8edfb256cdf0a07b4aae32ee794999..435aef317a4cfd59ab49a6b0958a49e883d4fa90 100644
--- a/src/shared/events/HSM.h
+++ b/src/shared/events/HSM.h
@@ -381,8 +381,8 @@ private:
StateHandler target = this->state;
State retState;
- State s = (static_cast<T*>(this)->*temp)({EV_EMPTY});
- UNUSED(s); // Avoid warning when not compiling for DEBUG
+ State s __attribute__((unused)) =
+ (static_cast<T*>(this)->*temp)({EV_EMPTY});
D(assert(s == TRAN));
do
diff --git a/src/shared/events/utils/EventCounter.h b/src/shared/events/utils/EventCounter.h
index db1f43bedcec3001abbdb066729c0198d39d5f3f..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,17 +87,13 @@ 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; }
protected:
// Do nothing
- void handleEvent(const Event& ev) override
- {
- // Avoid unused argument warning
- (void)ev;
- };
+ void handleEvent(const Event& ev __attribute__((unused))) override{};
private:
EventBroker& broker;
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..e0382dffcaec69b821faa0b7f28b81f0095c0a4a 100644
--- a/src/shared/logger/Deserializer.h
+++ b/src/shared/logger/Deserializer.h
@@ -27,6 +27,7 @@
#include <cstdio>
#include <fstream>
+#include <iostream>
#include <limits>
#include <ostream>
#include <string>
@@ -51,151 +52,155 @@ typedef std::numeric_limits<float> flt;
class Deserializer
{
public:
- Deserializer(std::string logfile, std::string prefix = "")
- : prefix(prefix), logFile(logfile),
- logFileWithExt(prefix + logFile + ".dat")
- {
- }
+ 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
+ * Register a type to be deserialized, and the associated print function.
*
- * @param t the object to be deserialized
- * @param fncPrint function that prints the deserialized data on the
+ * @param t The object to be deserialized.
+ * @param fncPrint Function that prints the deserialized data on the
* provided output stream.
+ * @param header Optional CSV header text.
*/
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;
- }
+ std::string header = "");
- char cFilename[128];
- sprintf(cFilename, "%s%s_%s.csv", prefix.c_str(), logFile.c_str(),
- typeid(T).name());
+ /**
+ * @brief Deserializes the provided file.
+ *
+ * @return Whether the deserialization was successful.
+ */
+ bool deserialize();
- std::string filename(cFilename);
+ void close();
- std::ofstream* stream = new std::ofstream();
- stream->open(filename);
+private:
+ bool closed = false;
- if (!stream->is_open())
- {
- printf("Error opening file %s.\n", filename.c_str());
- perror("Error is:");
- delete stream;
- return false;
- }
+ std::vector<std::ofstream*> fileStreams;
+ tscpp::TypePoolStream tps;
+
+ std::string fileName;
+};
+
+Deserializer::Deserializer(std::string fileName) : fileName(fileName) {}
- fileStreams.push_back(stream);
- stream->precision(flt::max_digits10); // Set stream precision to
- // maximum float precision
- // Print the header
- if (header.length() > 0)
+Deserializer::~Deserializer()
+{
+ if (!closed)
+ for (auto it = fileStreams.begin(); it != fileStreams.end(); it++)
{
- *stream << header;
+ (*it)->close();
+ delete *it;
}
+}
- using namespace std::placeholders; // for _1
+template <typename T>
+bool Deserializer::registerType(
+ std::function<void(T& t, std::ostream& os)> fncPrint, std::string header)
+{
+ if (closed)
+ {
+ printf("Error: Deserializer is closed.\n");
+ return false;
+ }
- std::function<void(T & t)> callback =
- std::bind(fncPrint, _1, std::ref(*stream));
+ char cFilename[128];
+ sprintf(cFilename, "%s_%s.csv", fileName.c_str(), typeid(T).name());
- tps.registerType<T>(callback);
+ std::string filename(cFilename);
- return true;
+ std::ofstream* stream = new std::ofstream();
+ stream->open(filename);
+
+ if (!stream->is_open())
+ {
+ printf("Error opening file %s.\n", filename.c_str());
+ perror("Error is:");
+ delete stream;
+ return false;
}
- /**
- * @brief Deserializes the provided file.
- *
- * @return Wheter the deserialization was successful.
- */
- bool deserialize()
+ fileStreams.push_back(stream);
+ stream->precision(flt::max_digits10); // Set stream precision to
+ // maximum float precision
+ // Print the header
+ if (header.length() > 0)
{
- if (closed)
- {
- return false;
- }
+ *stream << header;
+ }
+
+ using namespace std::placeholders; // for _1
+
+ std::function<void(T & t)> callback =
+ std::bind(fncPrint, _1, std::ref(*stream));
- bool success = true;
- std::string unknownTypeName;
+ tps.registerType<T>(callback);
+
+ return true;
+}
+
+bool Deserializer::deserialize()
+{
+ if (closed)
+ return false;
+
+ bool success = true;
+ std::string unknownTypeName;
+
+ std::ifstream file(fileName);
+
+ // Check if the file exists
+ if (!file)
+ {
+ std::cout << fileName << " does not exists." << std::endl;
+ return false;
+ }
- struct stat st;
- if (stat(logFileWithExt.c_str(), &st) != 0)
+ tscpp::UnknownInputArchive ia(file, tps);
+ int i = 0;
+ while (success)
+ {
+ try
{
- printf("File %s does not exists.\n", logFileWithExt.c_str());
- return false;
+ ia.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();
+ break;
}
- catch (tscpp::TscppException& ex)
+ 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;
- }
+ unknownTypeName = ex.name();
+ success = false;
+ std::cout << "Unknown type found: " << unknownTypeName
+ << std::endl;
+ break;
}
}
- file.close();
- return success;
}
- void close()
+ file.close();
+ return success;
+}
+
+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)->close();
+ delete *it;
}
}
-
-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..bb2a1c06dfee3da621fe2676f56cdce841af3904
--- /dev/null
+++ b/src/shared/logger/LogTypes.h
@@ -0,0 +1,125 @@
+/* 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 <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/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
+{
+
+template <typename T>
+void print(T& t, std::ostream& os)
+{
+ t.print(os);
+}
+
+template <typename T>
+void registerType(Deserializer& ds)
+{
+ ds.registerType<T>(print<T>, T::header());
+}
+
+void registerTypes(Deserializer& 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<VN100Data>(ds);
+ registerType<BatteryVoltageSensorData>(ds);
+ registerType<CurrentSensorData>(ds);
+ registerType<LoadCellData>(ds);
+ registerType<MPXHZ6130AData>(ds);
+ registerType<HSCMAND015PAData>(ds);
+ registerType<HSCMRNN030PAData>(ds);
+ registerType<HSCMRNN160KAData>(ds);
+ registerType<SSCDANN030PAAData>(ds);
+ registerType<SSCDRRN015PDAData>(ds);
+}
+
+} // 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/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 fe621101c837e69e7bee8ad661de96ad1116a03e..c8fd6e5400f5209f54a43a2897d990a0505d585a 100644
--- a/src/shared/sensors/BME280/BME280.cpp
+++ b/src/shared/sensors/BME280/BME280.cpp
@@ -47,8 +47,8 @@ const BME280::BME280Config BME280::BME280_CONFIG_TEMP_SINGLE = {
SKIPPED, 0, 0, FORCED_MODE, SKIPPED,
OVERSAMPLING_1, 0, FILTER_OFF, STB_TIME_0_5};
-BME280::BME280(SPISlave spiSlave_, BME280Config config_)
- : spiSlave(spiSlave_), config(config_)
+BME280::BME280(SPISlave spiSlave, BME280Config config)
+ : spiSlave(spiSlave), config(config)
{
}
diff --git a/src/shared/sensors/BME280/BME280.h b/src/shared/sensors/BME280/BME280.h
index 7ab2c6cdd39e4d1973091a4c4c40d24e546369db..7da38ced045595836d85fcd9b59925f25f9d579d 100644
--- a/src/shared/sensors/BME280/BME280.h
+++ b/src/shared/sensors/BME280/BME280.h
@@ -155,8 +155,8 @@ public:
static const BME280Config
BME280_CONFIG_TEMP_SINGLE; ///< Temperature enabled in forced mode
- explicit BME280(SPISlave spiSlave_,
- BME280Config config_ = BME280_CONFIG_ALL_ENABLED);
+ explicit BME280(SPISlave spiSlave,
+ BME280Config config = BME280_CONFIG_ALL_ENABLED);
/**
* @brief Initialize the device with the specified configuration
diff --git a/src/shared/sensors/BMP280/BMP280.cpp b/src/shared/sensors/BMP280/BMP280.cpp
index 11839de28598dcfc60d049bc5985e4cba3c8f283..35ad4ee5fddbd32667e6b361bd80f76d364094dc 100644
--- a/src/shared/sensors/BMP280/BMP280.cpp
+++ b/src/shared/sensors/BMP280/BMP280.cpp
@@ -45,8 +45,8 @@ const BMP280::BMP280Config BMP280::BMP280_CONFIG_ALL_ENABLED = {0,
const BMP280::BMP280Config BMP280::BMP280_CONFIG_TEMP_SINGLE = {
0, 0, FORCED_MODE, SKIPPED, OVERSAMPLING_1, 0, FILTER_OFF, STB_TIME_0_5};
-BMP280::BMP280(SPISlave spiSlave_, BMP280Config config_)
- : spiSlave(spiSlave_), config(config_)
+BMP280::BMP280(SPISlave spiSlave, BMP280Config config)
+ : spiSlave(spiSlave), config(config)
{
}
diff --git a/src/shared/sensors/BMP280/BMP280.h b/src/shared/sensors/BMP280/BMP280.h
index 385a40695fc386cc647cd051431198c150f9b835..8b3263dfaa376c6108bec7bf03ef154bace53423 100644
--- a/src/shared/sensors/BMP280/BMP280.h
+++ b/src/shared/sensors/BMP280/BMP280.h
@@ -147,8 +147,8 @@ public:
///< enabled in
///< forced mode
- explicit BMP280(SPISlave spiSlave_,
- BMP280Config config_ = BMP280_CONFIG_ALL_ENABLED);
+ explicit BMP280(SPISlave spiSlave,
+ BMP280Config config = BMP280_CONFIG_ALL_ENABLED);
/**
* @brief Initialize the device with the specified configuration
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
new file mode 100644
index 0000000000000000000000000000000000000000..8e3fa92694daa960e71db8a1463147e11bdd3e9d
--- /dev/null
+++ b/src/shared/sensors/HX711/HX711.cpp
@@ -0,0 +1,96 @@
+/* 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 "HX711.h"
+
+#include <drivers/timer/TimestampTimer.h>
+#include <interfaces-impl/gpio_impl.h>
+#include <miosix.h>
+
+namespace Boardcore
+{
+
+HX711::HX711(SPIBusInterface &bus, miosix::GpioPin sckPin, SPIBusConfig config,
+ unsigned char sckAlternateFunction)
+ : bus(bus), sckPin(sckPin), config(config),
+ sckAlternateFunction(sckAlternateFunction)
+{
+}
+
+SPIBusConfig HX711::getDefaultSPIConfig()
+{
+ SPIBusConfig spiConfig{};
+ spiConfig.clockDivider = SPI::ClockDivider::DIV_128;
+ spiConfig.mode = SPI::Mode::MODE_1;
+ return spiConfig;
+}
+
+bool HX711::init() { return true; }
+
+bool HX711::selfTest() { return true; }
+
+HX711Data HX711::sampleImpl()
+{
+ int32_t sample;
+
+ bus.configure(config);
+ sample = bus.read16() << 8;
+ sample |= bus.read();
+ sckPin.mode(miosix::Mode::OUTPUT);
+ sckPin.high();
+ miosix::delayUs(1);
+ sckPin.low();
+ sckPin.mode(miosix::Mode::ALTERNATE);
+ sckPin.alternateFunction(sckAlternateFunction);
+
+ if (sample & static_cast<int32_t>(0x800000))
+ sample |= static_cast<uint32_t>(0xFF) << 24;
+
+ if (sample == static_cast<int32_t>(0xFFFFFFFF))
+ return lastSample;
+
+ return {TimestampTimer::getInstance().getTimestamp(),
+ 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; }
+
+float HX711::getScale() { return scale; }
+
+void HX711::setOffset(float offset) { this->offset = offset / scale; }
+
+void HX711::updateOffset(float offset) { this->offset += offset / scale; }
+
+float HX711::getOffset() { return offset; }
+
+} // namespace Boardcore
diff --git a/src/shared/sensors/HX711/HX711.h b/src/shared/sensors/HX711/HX711.h
new file mode 100644
index 0000000000000000000000000000000000000000..4674ea16e84c255470bb80ec54aebb997d624665
--- /dev/null
+++ b/src/shared/sensors/HX711/HX711.h
@@ -0,0 +1,127 @@
+/* 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/spi/SPIDriver.h>
+#include <sensors/Sensor.h>
+
+#include "HX711Data.h"
+
+namespace Boardcore
+{
+
+/**
+ * @brief Load cell transducer.
+ *
+ * HX711 is a precision 24-bit analog-to-digital converter (ADC) designed for
+ * weigh scales and industrial control applications to interface directly with a
+ * bridge sensor.
+ *
+ * The input multiplexer selects either Channel A or B differential input to the
+ * low-noise programmable gain amplifier (PGA). Channel A can be programmed with
+ * a gain of 128 or 64, corresponding to a full-scale differential input voltage
+ * of ±20mV or ±40mV respectively, when a 5V supply is connected to AVDD analog
+ * power supply pin. Channel B has a fixed gain of 32. On-chip power supply
+ * regulator eliminates the need for an external supply regulator to provide
+ * analog power for the ADC and the sensor. Clock input is flexible. It can be
+ * from an external clock source, a crystal, or the on-chip oscillator that does
+ * not require any external component. On-chip power-on-reset circuitry
+ * simplifies digital interface initialization. There is no programming needed
+ * for the internal registers. All controls to the HX711 are through the pins.
+ *
+ * Reference: https://github.com/bogde/HX711
+ */
+class HX711 : public Sensor<HX711Data>
+{
+public:
+ HX711(SPIBusInterface& bus, miosix::GpioPin sckPin,
+ SPIBusConfig config = getDefaultSPIConfig(),
+ unsigned char sckAlternateFunction = 5);
+
+ static SPIBusConfig getDefaultSPIConfig();
+
+ bool init() override;
+
+ bool selfTest() override;
+
+ /**
+ * @brief Calculates the scale value such that the load cell's output
+ * matches the given value.
+ *
+ * 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 Returns the current scale.
+ */
+ float getScale();
+
+ /**
+ * @brief Sets the offset to the given value.
+ *
+ * @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.
+ */
+ float getOffset();
+
+private:
+ HX711Data sampleImpl() override;
+
+ SPIBusInterface& bus;
+ miosix::GpioPin sckPin;
+ const SPIBusConfig config;
+ unsigned char sckAlternateFunction;
+
+ float scale = 1;
+ int32_t offset = 0;
+};
+
+} // namespace Boardcore
diff --git a/src/shared/utils/Unused.h b/src/shared/sensors/HX711/HX711Data.h
similarity index 70%
rename from src/shared/utils/Unused.h
rename to src/shared/sensors/HX711/HX711Data.h
index 5540e82257824b05063fe131935d5e3e355fe759..49e25db1942c56331a90e9bbe6cd29f21741607d 100644
--- a/src/shared/utils/Unused.h
+++ b/src/shared/sensors/HX711/HX711Data.h
@@ -1,5 +1,6 @@
+
/* Copyright (c) 2019 Skyward Experimental Rocketry
- * Author: Alvise de' Faveri Tron
+ * 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
@@ -22,4 +23,26 @@
#pragma once
-#define UNUSED(x) (void)(x)
+#include <sensors/SensorData.h>
+
+namespace Boardcore
+{
+
+struct HX711Data : public LoadCellData
+{
+ HX711Data() : LoadCellData{0, 0} {}
+
+ HX711Data(uint64_t loadTimestamp, float load)
+ : LoadCellData{loadTimestamp, load}
+ {
+ }
+
+ static std::string header() { return "loadTimestamp,load\n"; }
+
+ void print(std::ostream& os) const
+ {
+ os << loadTimestamp << "," << load << "\n";
+ }
+};
+
+} // 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
new file mode 100644
index 0000000000000000000000000000000000000000..3a723a6d8a7f46878fafd41162925a734fccf6ab
--- /dev/null
+++ b/src/shared/sensors/MAX31855/MAX31855.cpp
@@ -0,0 +1,113 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: Angelo Zangari
+ *
+ * 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 "MAX31855.h"
+
+#include <drivers/timer/TimestampTimer.h>
+
+namespace Boardcore
+{
+
+MAX31855::MAX31855(SPIBusInterface& bus, miosix::GpioPin cs,
+ SPIBusConfig config)
+ : slave(bus, cs, config)
+{
+}
+
+SPIBusConfig MAX31855::getDefaultSPIConfig()
+{
+ SPIBusConfig spiConfig{};
+ spiConfig.clockDivider = SPI::ClockDivider::DIV_32;
+ spiConfig.mode = SPI::Mode::MODE_1;
+ return spiConfig;
+}
+
+bool MAX31855::init() { return true; }
+
+bool MAX31855::selfTest() { return true; }
+
+bool MAX31855::checkConnection()
+{
+ uint16_t sample[2];
+
+ {
+ SPITransaction spi{slave};
+ spi.read(sample, sizeof(sample));
+ }
+
+ // Bits D0, D1 and D2 go high if thermocouple is open or shorted either to
+ // gnd or Vcc
+ if ((sample[1] & 0x7) != 0)
+ {
+ lastError = SensorErrors::SELF_TEST_FAIL;
+ LOG_ERR(logger, "Self test failed, the thermocouple is not connected");
+ return false;
+ }
+
+ return true;
+}
+
+TemperatureData MAX31855::sampleImpl()
+{
+ uint16_t sample;
+
+ {
+ SPITransaction spi{slave};
+ sample = spi.read16();
+ }
+
+ TemperatureData result{};
+ result.temperatureTimestamp = TimestampTimer::getInstance().getTimestamp();
+
+ // Extract data bits
+ sample = sample >> 2;
+
+ // Convert the integer and decimal part separetly
+ result.temperature = static_cast<float>(sample >> 2);
+ result.temperature += static_cast<float>(sample & 0x3) * 0.25;
+
+ return result;
+}
+
+TemperatureData MAX31855::readInternalTemperature()
+{
+ uint16_t sample[2];
+
+ {
+ SPITransaction spi{slave};
+ spi.read(sample, sizeof(sample));
+ }
+
+ TemperatureData result{};
+ result.temperatureTimestamp = TimestampTimer::getInstance().getTimestamp();
+
+ // Extract data bits
+ sample[1] = sample[1] >> 4;
+
+ // Convert the integer and decimal part separetly
+ result.temperature = static_cast<float>(sample[1] >> 4);
+ result.temperature += static_cast<float>(sample[1] & 0xF) * 0.0625;
+
+ return result;
+}
+
+} // namespace Boardcore
diff --git a/src/shared/sensors/MAX31855/MAX31855.h b/src/shared/sensors/MAX31855/MAX31855.h
new file mode 100644
index 0000000000000000000000000000000000000000..89aa3484f0dc2e589c35f96329bfac0536d22896
--- /dev/null
+++ b/src/shared/sensors/MAX31855/MAX31855.h
@@ -0,0 +1,81 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: Angelo Zangari
+ *
+ * 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 <drivers/spi/SPIDriver.h>
+#include <sensors/Sensor.h>
+
+namespace Boardcore
+{
+/**
+ * @brief MAX31855 termocouple sensor driver.
+ */
+class MAX31855 : public Sensor<TemperatureData>
+{
+public:
+ /**
+ * @brief Constructor.
+ *
+ * @param bus The Spi bus.
+ * @param cs The CS pin to lower when we need to sample.
+ * @param config The SPI configuration.
+ */
+ MAX31855(SPIBusInterface &bus, miosix::GpioPin cs,
+ SPIBusConfig config = getDefaultSPIConfig());
+
+ /**
+ * Constructs the default config for SPI Bus.
+ *
+ * @returns The default SPIBusConfig.
+ */
+ static SPIBusConfig getDefaultSPIConfig();
+
+ bool init();
+
+ /**
+ * @brief Checks wheter the termocouple is open.
+ *
+ * @return True if the termocouple is connected.
+ */
+ bool selfTest();
+
+ /**
+ * @brief Checks whether the thermocouple is connected or not.
+ */
+ bool checkConnection();
+
+ /**
+ * @brief Read the device internal temperature (cold junction).
+ */
+ TemperatureData readInternalTemperature();
+
+private:
+ TemperatureData sampleImpl() override;
+
+ SPISlave slave;
+
+ PrintLogger logger = Logging::getLogger("max31855");
+};
+
+} // namespace Boardcore
diff --git a/src/shared/sensors/MAX6675/MAX6675.cpp b/src/shared/sensors/MAX6675/MAX6675.cpp
index 08ca8e3016c3b32b8f33f7a57938419db8c90141..b7e5e5b67869773ca5b9f65a381f472af36146d8 100644
--- a/src/shared/sensors/MAX6675/MAX6675.cpp
+++ b/src/shared/sensors/MAX6675/MAX6675.cpp
@@ -55,7 +55,7 @@ bool MAX6675::checkConnection()
// The third bit (D2) indicates wheter the termocouple is connected or not
// It is high if open
- if ((sample % 0x2) != 0)
+ if ((sample & 0x4) != 0)
{
lastError = SensorErrors::SELF_TEST_FAIL;
LOG_ERR(logger, "Self test failed, the termocouple is not connected");
@@ -74,19 +74,16 @@ TemperatureData MAX6675::sampleImpl()
sample = spi.read16();
}
+ TemperatureData result{};
+ result.temperatureTimestamp = TimestampTimer::getInstance().getTimestamp();
+
// Extract bits 14-3
sample &= 0x7FF8;
sample >>= 3;
- TemperatureData result{};
- result.temperatureTimestamp = TimestampTimer::getInstance().getTimestamp();
-
- // Convert the sample value
- result.temperature =
- static_cast<unsigned int>(sample >> 2); // Integer part
- sample &= 0x0003; // Take the floating point part
- result.temperature +=
- static_cast<float>(sample * 0.25); // Floating point part
+ // Convert the integer and decimal part separetly
+ result.temperature = static_cast<float>(sample >> 2);
+ result.temperature += static_cast<float>(sample & 0x3) * 0.25;
return result;
}
diff --git a/src/shared/sensors/MAX6675/MAX6675.h b/src/shared/sensors/MAX6675/MAX6675.h
index 1276549318e5caa0a479faddf02b46b2b3661d45..0ee3e01bdf07bf807ee76a6287c80d7cb6d53733 100644
--- a/src/shared/sensors/MAX6675/MAX6675.h
+++ b/src/shared/sensors/MAX6675/MAX6675.h
@@ -31,34 +31,22 @@ namespace Boardcore
{
/**
- * @brief MAX6675 termocouple sensor driver.
+ * @brief MAX6675 thermocouple sensor driver.
*/
class MAX6675 : public Sensor<TemperatureData>
{
public:
- /**
- * @brief Constructor.
- *
- * @param bus The Spi bus.
- * @param cs The CS pin to lower when we need to sample.
- * @param config The SPI configuration.
- */
MAX6675(SPIBusInterface &bus, miosix::GpioPin cs,
SPIBusConfig config = getDefaultSPIConfig());
- /**
- * Constructs the default config for SPI Bus.
- *
- * @returns The default SPIBusConfig.
- */
static SPIBusConfig getDefaultSPIConfig();
bool init();
/**
- * @brief Checks wheter the termocouple is open.
+ * @brief Checks whether the thermocouple is connected or not.
*
- * @return True if the termocouple is connected.
+ * @return True if the thermocouple is connected.
*/
bool selfTest();
@@ -70,7 +58,7 @@ public:
private:
TemperatureData sampleImpl() override;
- SPISlave slave;
+ const SPISlave slave;
PrintLogger logger = Logging::getLogger("max6675");
};
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 978c6772ef16f11632482f686f45c1467d596202..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,25 +87,25 @@ enum ReturnsStates
* @brief Structure that stores a data value, with his timestamp and his
* validity.
*/
-struct Data : public DigitalLoadCellData
+struct MBLoadCellData : public LoadCellData
{
bool valid = false;
- Data() : DigitalLoadCellData{0, 0.0}, valid(false) {}
+ MBLoadCellData() : LoadCellData{0, 0.0}, valid(false) {}
- explicit Data(float data)
- : DigitalLoadCellData{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";
}
};
@@ -116,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
@@ -130,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;
@@ -152,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);
}
};
@@ -224,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 a0e76f7aa0a207fa38c96edf5aee385280a1b9cf..d40024e73622d384aacdd9bddf89b3a76307e217 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
@@ -54,10 +54,17 @@ struct TimestampData
uint64_t timestamp;
};
-struct DigitalLoadCellData
+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 eb15921551a1744e778d8b93c51575dac5413682..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 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..1874bc082084e05bf9d16b54dda9586724f36bb1 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();
/**
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/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 df4de132c0bc67bafcb539c9eb7107f13b8f1331..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/Kalman.h>
#include <drivers/timer/GeneralPurposeTimer.h>
#include <drivers/timer/TimerUtils.h>
-#include <kalman/KalmanEigen.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 77ecb04bd8d0e5d4f7c2ac2d6cff0fe3e0cf45fc..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 <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..aa2beb55466235a36a0f3336034b1c3bf8bf5262 100644
--- a/src/tests/catch/test-packetqueue.cpp
+++ b/src/tests/catch/test-packetqueue.cpp
@@ -152,7 +152,7 @@ TEST_CASE("Packet tests")
REQUIRE(p.getMsgCount() == 0);
REQUIRE(p.dump(buf) == 0);
- INFO("Adding something to full packet")
+ INFO("Adding something to full packet");
REQUIRE(p.tryAppend(messageBase, PKT_LEN));
REQUIRE_FALSE(p.tryAppend(messageBase, 1));
@@ -288,17 +288,17 @@ TEST_CASE("PacketQueue tests")
SECTION("Edge cases")
{
- INFO("Adding too big msg")
+ INFO("Adding too big msg");
REQUIRE(pq.put(messageBase, PKT_LEN + 1) == -1);
REQUIRE_FALSE(pq.isFull());
REQUIRE(pq.isEmpty());
REQUIRE(pq.countNotEmpty() == 0);
REQUIRE(pq.countReady() == 0);
- INFO("Adding empty message")
+ INFO("Adding empty message");
REQUIRE(pq.put(messageBase, 0) == -1);
- INFO("Adding something to full queue")
+ 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);
@@ -319,7 +319,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/catch/xbee/test-xbee-parser.cpp b/src/tests/catch/xbee/test-xbee-parser.cpp
index e783c98051dadf7ab9c191f5f9d9e5c16887d1db..924d96851e8970f3729ef57d13f65de28ae78be1 100644
--- a/src/tests/catch/xbee/test-xbee-parser.cpp
+++ b/src/tests/catch/xbee/test-xbee-parser.cpp
@@ -330,8 +330,8 @@ TEST_CASE("Frame serialization")
txOrig.setTransmitOptions(0x40);
REQUIRE(txOrig.getTrasmitOptions() == 0x40);
- uint8_t* rfData = txOrig.getRFDataPointer();
- UNUSED(rfData); // TODO: Check rfData
+ uint8_t* rfData __attribute__((unused)) = txOrig.getRFDataPointer();
+ // TODO: Check rfData
REQUIRE(txOrig.getRFDataLength() == 0);
REQUIRE(txOrig.getFrameDataLength() == 13);
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/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 26229c35e1286fa63a27154407547ca6a6ec8d8e..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>
@@ -122,7 +122,7 @@ struct EnergyScanData
EnergyScanData() = default;
- EnergyScanData(long long ts, array<int, 30> scan)
+ EnergyScanData(long long ts, const array<int, 30> scan)
{
for (int i = 0; i < 30; i++)
{
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 dacc4bca74194bae6ef50bec5a0dd4857f049cbc..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);
@@ -159,25 +157,22 @@ int main()
gui->screenEnergy.btnStop.addOnInteractionListener(onStopButtonClick);
gui->screenEnergy.btnMark.addOnInteractionListener(onMarkButtonClick);
gui->screenEnergy.btnReset.addOnInteractionListener(
- [&](View* d, Interaction action)
+ [&](View* d __attribute__((unused)), Interaction action)
{
- UNUSED(d);
if (action == Interaction::CLICK)
gui->screenEnergy.resetStats();
});
gui->screenEnd.tvF.addOnInteractionListener(
- [&](View* d, Interaction action)
+ [&](View* d __attribute__((unused)), Interaction action)
{
- UNUSED(d);
if (action == Interaction::CLICK)
gui->screenManager.showScreen(XbeeGUI::SCREEN_RESPECT);
});
gui->screenEnd.tvReset.addOnInteractionListener(
- [&](View* d, Interaction action)
+ [&](View* d __attribute__((unused)), Interaction action)
{
- UNUSED(d);
if (action == Interaction::CLICK)
miosix::reboot();
});
@@ -218,9 +213,8 @@ int main()
}
}
-void onStartButtonClick(View* btn, Interaction action)
+void onStartButtonClick(View* btn __attribute__((unused)), Interaction action)
{
- UNUSED(btn);
if (action == Interaction::CLICK)
{
@@ -267,9 +261,8 @@ void onStopButtonClick(View* btn, Interaction action)
}
}
-void onMarkButtonClick(View* btn, Interaction action)
+void onMarkButtonClick(View* btn __attribute__((unused)), Interaction action)
{
- UNUSED(btn);
if (action == Interaction::CLICK)
{
Mark m{getTick(), markCounter++};
@@ -283,9 +276,8 @@ void onMarkButtonClick(View* btn, Interaction action)
}
}
-void onEnergyButtonClick(View* btn, Interaction action)
+void onEnergyButtonClick(View* btn __attribute__((unused)), Interaction action)
{
- UNUSED(btn);
if (action == Interaction::CLICK)
{
energyScanner.start();
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-bmx160-with-correction.cpp b/src/tests/sensors/test-bmx160-with-correction.cpp
index 27e005d273636ecb3605dd73b644ca7020ede208..32c0363c182d739ca6e7e98b1715f5660c8bfaf6 100644
--- a/src/tests/sensors/test-bmx160-with-correction.cpp
+++ b/src/tests/sensors/test-bmx160-with-correction.cpp
@@ -160,10 +160,8 @@ int main()
return 0;
}
-void bmx160Sample(void *args)
+void bmx160Sample(void *args __attribute__((unused)))
{
- UNUSED(args);
-
while (!stopSamplingThread)
{
// Sample the bmx160
diff --git a/src/tests/sensors/test-hx711.cpp b/src/tests/sensors/test-hx711.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f4acb0d76fc4e5c9dee08a4b8a6477bb478a192c
--- /dev/null
+++ b/src/tests/sensors/test-hx711.cpp
@@ -0,0 +1,73 @@
+/* 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 <drivers/timer/TimestampTimer.h>
+#include <miosix.h>
+#include <sensors/HX711/HX711.h>
+
+using namespace miosix;
+using namespace Boardcore;
+
+GpioPin sckPin = GpioPin(GPIOB_BASE, 3);
+GpioPin misoPin = GpioPin(GPIOB_BASE, 4);
+
+void initBoard()
+{
+ // Setup gpio pins
+ sckPin.mode(Mode::ALTERNATE);
+ sckPin.alternateFunction(5);
+ misoPin.mode(Mode::ALTERNATE);
+ misoPin.alternateFunction(5);
+}
+
+int main()
+{
+ // Enable SPI clock and set gpios
+ initBoard();
+
+ SPIBus spiBus(SPI1);
+ HX711 sensor{spiBus, sckPin};
+ sensor.setScale(1.0f);
+
+ // Average 100 samples and then apply the offset
+ printf("Measuring offset...\n");
+ float average = 0;
+ for (int i = 0; i < 100; i++)
+ {
+ sensor.sample();
+ average += sensor.getLastSample().load;
+ Thread::sleep(12);
+ }
+ average /= 100;
+ sensor.setOffset(average);
+ sensor.setScale(214000);
+
+ while (true)
+ {
+ sensor.sample();
+
+ 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-lis3mdl.cpp b/src/tests/sensors/test-lis3mdl.cpp
index b1117912111407c0f38b00f948884057bec72647..af8abf3d3535ddc8fe187479abd88a8dbb857070 100644
--- a/src/tests/sensors/test-lis3mdl.cpp
+++ b/src/tests/sensors/test-lis3mdl.cpp
@@ -76,8 +76,7 @@ int main()
while (true)
{
sensor.sample();
- LIS3MDLData data = sensor.getLastSample();
- UNUSED(data);
+ LIS3MDLData data __attribute__((unused)) = sensor.getLastSample();
TRACE("%f C | x: %f | y: %f | z %f\n", data.temperature,
data.magneticFieldX, data.magneticFieldY, data.magneticFieldZ);
miosix::Thread::sleep(2000);
diff --git a/src/tests/sensors/test-max31855.cpp b/src/tests/sensors/test-max31855.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..00171ebab0da1cb58c04885821dc96e0b7526754
--- /dev/null
+++ b/src/tests/sensors/test-max31855.cpp
@@ -0,0 +1,75 @@
+/* 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 <drivers/timer/TimestampTimer.h>
+#include <miosix.h>
+#include <sensors/MAX31855/MAX31855.h>
+
+using namespace miosix;
+using namespace Boardcore;
+
+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);
+ misoPin.alternateFunction(5);
+}
+
+int main()
+{
+ // Enable SPI clock and set gpios
+ initBoard();
+
+ SPIBus spiBus(SPI5);
+ MAX31855 sensor{spiBus, csPin};
+
+ printf("Starting process verification!\n");
+
+ if (!sensor.selfTest())
+ {
+ printf("Sensor self test failed!\n");
+ }
+
+ while (true)
+ {
+ sensor.sample();
+ TemperatureData sample = sensor.getLastSample();
+
+ printf("[%.2f] %.2f\n", sample.temperatureTimestamp / 1e6,
+ sample.temperature);
+
+ Thread::sleep(500);
+ }
+ return 0;
+}
diff --git a/src/tests/sensors/test-max6675.cpp b/src/tests/sensors/test-max6675.cpp
index 48f73fd292473cd23190f39f6b3b971e3fbff523..be541c114bb5f38e1bd8f53634cf0dd71d501a63 100644
--- a/src/tests/sensors/test-max6675.cpp
+++ b/src/tests/sensors/test-max6675.cpp
@@ -29,7 +29,6 @@ using namespace Boardcore;
GpioPin sckPin = GpioPin(GPIOE_BASE, 2);
GpioPin misoPin = GpioPin(GPIOE_BASE, 5);
-GpioPin mosiPin = GpioPin(GPIOE_BASE, 6);
GpioPin csPin = GpioPin(GPIOE_BASE, 4);
void initBoard()
@@ -41,8 +40,6 @@ void initBoard()
sckPin.alternateFunction(5);
misoPin.mode(Mode::ALTERNATE);
misoPin.alternateFunction(5);
- mosiPin.mode(Mode::ALTERNATE);
- mosiPin.alternateFunction(5);
}
int main()
@@ -69,5 +66,4 @@ int main()
Thread::sleep(500);
}
- return 0;
}
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
index 0c611daf4e7ffc2ccabcc56da70d989496e1d45f..24b6cc092a73e531765fbbe4897a29133a827014 100644
--- a/src/tests/sensors/test-ubloxgps.cpp
+++ b/src/tests/sensors/test-ubloxgps.cpp
@@ -69,15 +69,17 @@ int main()
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
- TRACE(
+ 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",
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 1eba767a4811a93b971c234b4fa6d8b13e398d67..c33ba46ba6bffbb54c97b8682b79665d4b3275ef 100644
--- a/src/tests/test-hsm.cpp
+++ b/src/tests/test-hsm.cpp
@@ -37,16 +37,14 @@ using namespace miosix;
#define TOPIC_TEST 1
-#define CHECK_INIT() \
- bool testValue = false; \
- (void)testValue
+#define CHECK_INIT() bool testValue __attribute__((unused)) = false;
#define CHECK_STATE(HSM, SIGNAL, STATE) \
do \
{ \
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 << " " \
@@ -56,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,
@@ -86,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)
@@ -107,7 +104,7 @@ State HSMUTTest::state_S(const Event& e)
{
State retState = HANDLED;
DEBUG_PRINT;
- switch (e.code)
+ switch (e)
{
case EV_ENTRY:
break;
@@ -140,7 +137,7 @@ State HSMUTTest::state_S1(const Event& e)
{
State retState = HANDLED;
DEBUG_PRINT;
- switch (e.code)
+ switch (e)
{
case EV_ENTRY:
break;
@@ -186,7 +183,7 @@ State HSMUTTest::state_S11(const Event& e)
{
State retState = HANDLED;
DEBUG_PRINT;
- switch (e.code)
+ switch (e)
{
case EV_ENTRY:
break;
@@ -222,7 +219,7 @@ State HSMUTTest::state_S2(const Event& e)
{
State retState = HANDLED;
DEBUG_PRINT;
- switch (e.code)
+ switch (e)
{
case EV_ENTRY:
break;
@@ -258,7 +255,7 @@ State HSMUTTest::state_S21(const Event& e)
{
State retState = HANDLED;
DEBUG_PRINT;
- switch (e.code)
+ switch (e)
{
case EV_ENTRY:
break;
@@ -283,7 +280,7 @@ State HSMUTTest::state_S211(const Event& e)
{
State retState = HANDLED;
DEBUG_PRINT;
- switch (e.code)
+ switch (e)
{
case EV_ENTRY:
break;
@@ -311,49 +308,36 @@ State HSMUTTest::state_S211(const Event& e)
int main()
{
- sEventBroker.start();
+ EventBroker::getInstance().start();
HSMUTTest& hsm = HSMUTTest::getInstance();
hsm.start();
CHECK_INIT();
- // cppcheck-suppress unreadVariable
// cppcheck-suppress assertWithSideEffect
CHECK_STATE(hsm, EV_G, &HSMUTTest::state_S11);
- // cppcheck-suppress unreadVariable
// cppcheck-suppress assertWithSideEffect
CHECK_STATE(hsm, EV_I, &HSMUTTest::state_S11);
- // cppcheck-suppress unreadVariable
// cppcheck-suppress assertWithSideEffect
CHECK_STATE(hsm, EV_A, &HSMUTTest::state_S11);
- // cppcheck-suppress unreadVariable
// cppcheck-suppress assertWithSideEffect
CHECK_STATE(hsm, EV_D, &HSMUTTest::state_S11);
- // cppcheck-suppress unreadVariable
// cppcheck-suppress assertWithSideEffect
CHECK_STATE(hsm, EV_D, &HSMUTTest::state_S11);
- // cppcheck-suppress unreadVariable
// cppcheck-suppress assertWithSideEffect
CHECK_STATE(hsm, EV_B, &HSMUTTest::state_S11);
- // cppcheck-suppress unreadVariable
// cppcheck-suppress assertWithSideEffect
CHECK_STATE(hsm, EV_C, &HSMUTTest::state_S211);
- // cppcheck-suppress unreadVariable
// cppcheck-suppress assertWithSideEffect
CHECK_STATE(hsm, EV_E, &HSMUTTest::state_S11);
- // cppcheck-suppress unreadVariable
// cppcheck-suppress assertWithSideEffect
CHECK_STATE(hsm, EV_E, &HSMUTTest::state_S11);
- // cppcheck-suppress unreadVariable
// cppcheck-suppress assertWithSideEffect
CHECK_STATE(hsm, EV_G, &HSMUTTest::state_S211);
- // cppcheck-suppress unreadVariable
// cppcheck-suppress assertWithSideEffect
CHECK_STATE(hsm, EV_B, &HSMUTTest::state_S211);
- // cppcheck-suppress unreadVariable
// cppcheck-suppress assertWithSideEffect
CHECK_STATE(hsm, EV_I, &HSMUTTest::state_S211);
- // cppcheck-suppress unreadVariable
// cppcheck-suppress assertWithSideEffect
CHECK_STATE(hsm, EV_I, &HSMUTTest::state_S211);
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 ee235db75ad919be5512fcbf0fe114c5f76b439e..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>
@@ -259,7 +259,7 @@ int main()
// TEST SENSORS WITH FIFO
- const uint32_t fifoSize = 20; // cppcheck-suppress unreadVariable
+ const uint32_t fifoSize = 20;
MySensorFIFO s;
@@ -269,9 +269,8 @@ int main()
{
s.sample();
- MySensorDataFIFO data = fifoProxy.getLastSample();
-
- UNUSED(data);
+ MySensorDataFIFO data __attribute__((unused)) =
+ fifoProxy.getLastSample();
TRACE("AccelProxy : %llu %f %f %f \n", data.accelerationTimestamp,
data.accelerationX, data.accelerationY, data.accelerationZ);
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