diff --git a/.project b/.project new file mode 100644 index 0000000..0c54847 --- /dev/null +++ b/.project @@ -0,0 +1,11 @@ + + + Flight-Computer + + + + + + + + diff --git a/.vscode/launch.json b/.vscode/launch.json index 4219b1c..5d8a422 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -24,7 +24,7 @@ "device": "STM32F411RE", "interface": "swd", "runToEntryPoint": "main", - "preLaunchTask": "Build", + // "preLaunchTask": "Build", } ] diff --git a/.vscode/settings.json b/.vscode/settings.json index c0de84c..fc23654 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,4 +1,5 @@ { "cortex-debug.gdbPath.linux": "/usr/bin/arm-none-eabi-gdb", - "cortex-debug.stutilPath.linux": "/usr/bin/st-util" + "cortex-debug.stutilPath.linux": "/usr/bin/st-util", + "cortex-debug.variableUseNaturalFormat": true } \ No newline at end of file diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 180f7c5..ee461a2 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -10,11 +10,11 @@ "detail": "Build" }, { - "label": "Clean Build", + "label": "Clean", "group": "build", "type": "shell", "command": "make", - "args": [ "-j", "-C", "./Code/", "cleanbuild"], + "args": [ "-j", "-C", "./Code/", "clean"], "detail": "Do a clean build" }, { diff --git a/Code/.cproject b/Code/.cproject index e550f10..e17e536 100644 --- a/Code/.cproject +++ b/Code/.cproject @@ -14,6 +14,29 @@ +<<<<<<< HEAD + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +======= +>>>>>>> main @@ -92,9 +219,20 @@ +<<<<<<< HEAD + + + + + + + + +======= +>>>>>>> main \ No newline at end of file diff --git a/Code/.mxproject b/Code/.mxproject index 453870f..849d6be 100644 --- a/Code/.mxproject +++ b/Code/.mxproject @@ -1,8 +1,8 @@ [PreviousLibFiles] -LibFiles=Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_adc.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_adc_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_adc.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_rcc.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_rcc_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_bus.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_rcc.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_system.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_utils.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash_ramfunc.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_gpio.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_gpio_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_gpio.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_dma_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_dma.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_dma.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_dmamux.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_pwr.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_pwr_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_pwr.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_cortex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_cortex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal.h;Drivers\STM32F4xx_HAL_Driver\Inc\Legacy\stm32_hal_legacy.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_def.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_exti.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_exti.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_i2c.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_i2c.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_i2c_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_spi.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_spi.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_uart.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_usart.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_pcd.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_pcd_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_usb.h;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_adc.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_adc_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_rcc.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_rcc_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash_ramfunc.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_gpio.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_dma_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_dma.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pwr.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pwr_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_cortex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_exti.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_i2c.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_i2c_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_spi.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_uart.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pcd.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pcd_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_ll_usb.c;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_adc.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_adc_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_adc.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_rcc.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_rcc_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_bus.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_rcc.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_system.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_utils.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash_ramfunc.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_gpio.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_gpio_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_gpio.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_dma_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_dma.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_dma.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_dmamux.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_pwr.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_pwr_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_pwr.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_cortex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_cortex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal.h;Drivers\STM32F4xx_HAL_Driver\Inc\Legacy\stm32_hal_legacy.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_def.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_exti.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_exti.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_i2c.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_i2c.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_i2c_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_spi.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_spi.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_uart.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_usart.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_pcd.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_pcd_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_usb.h;Drivers\CMSIS\Device\ST\STM32F4xx\Include\stm32f446xx.h;Drivers\CMSIS\Device\ST\STM32F4xx\Include\stm32f4xx.h;Drivers\CMSIS\Device\ST\STM32F4xx\Include\system_stm32f4xx.h;Drivers\CMSIS\Device\ST\STM32F4xx\Include\system_stm32f4xx.h;Drivers\CMSIS\Device\ST\STM32F4xx\Source\Templates\system_stm32f4xx.c;Drivers\CMSIS\Include\cachel1_armv7.h;Drivers\CMSIS\Include\cmsis_armcc.h;Drivers\CMSIS\Include\cmsis_armclang.h;Drivers\CMSIS\Include\cmsis_armclang_ltm.h;Drivers\CMSIS\Include\cmsis_compiler.h;Drivers\CMSIS\Include\cmsis_gcc.h;Drivers\CMSIS\Include\cmsis_iccarm.h;Drivers\CMSIS\Include\cmsis_version.h;Drivers\CMSIS\Include\core_armv81mml.h;Drivers\CMSIS\Include\core_armv8mbl.h;Drivers\CMSIS\Include\core_armv8mml.h;Drivers\CMSIS\Include\core_cm0.h;Drivers\CMSIS\Include\core_cm0plus.h;Drivers\CMSIS\Include\core_cm1.h;Drivers\CMSIS\Include\core_cm23.h;Drivers\CMSIS\Include\core_cm3.h;Drivers\CMSIS\Include\core_cm33.h;Drivers\CMSIS\Include\core_cm35p.h;Drivers\CMSIS\Include\core_cm4.h;Drivers\CMSIS\Include\core_cm55.h;Drivers\CMSIS\Include\core_cm7.h;Drivers\CMSIS\Include\core_cm85.h;Drivers\CMSIS\Include\core_sc000.h;Drivers\CMSIS\Include\core_sc300.h;Drivers\CMSIS\Include\core_starmc1.h;Drivers\CMSIS\Include\mpu_armv7.h;Drivers\CMSIS\Include\mpu_armv8.h;Drivers\CMSIS\Include\pac_armv81.h;Drivers\CMSIS\Include\pmu_armv8.h;Drivers\CMSIS\Include\tz_context.h; +LibFiles=Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_adc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_rcc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_rcc_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_bus.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_rcc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_system.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_utils.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_flash.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_flash_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_flash_ramfunc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_gpio.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_gpio_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_gpio.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_dma_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_dma.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_dma.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_dmamux.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pwr.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pwr_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_pwr.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_cortex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_cortex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal.h;Drivers/STM32F4xx_HAL_Driver/Inc/Legacy/stm32_hal_legacy.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_def.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_exti.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_exti.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_i2c.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_i2c.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_i2c_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_spi.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_spi.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_uart.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_usart.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pcd.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pcd_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_usb.h;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ramfunc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_gpio.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_cortex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_exti.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_uart.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_usb.c;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_adc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_rcc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_rcc_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_bus.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_rcc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_system.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_utils.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_flash.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_flash_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_flash_ramfunc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_gpio.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_gpio_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_gpio.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_dma_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_dma.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_dma.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_dmamux.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pwr.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pwr_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_pwr.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_cortex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_cortex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal.h;Drivers/STM32F4xx_HAL_Driver/Inc/Legacy/stm32_hal_legacy.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_def.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_exti.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_exti.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_i2c.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_i2c.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_i2c_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_spi.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_spi.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_uart.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_usart.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pcd.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pcd_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_usb.h;Drivers/CMSIS/Device/ST/STM32F4xx/Include/stm32f446xx.h;Drivers/CMSIS/Device/ST/STM32F4xx/Include/stm32f4xx.h;Drivers/CMSIS/Device/ST/STM32F4xx/Include/system_stm32f4xx.h;Drivers/CMSIS/Device/ST/STM32F4xx/Include/system_stm32f4xx.h;Drivers/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c;Drivers/CMSIS/Include/core_armv8mml.h;Drivers/CMSIS/Include/core_starmc1.h;Drivers/CMSIS/Include/core_cm1.h;Drivers/CMSIS/Include/cmsis_armclang_ltm.h;Drivers/CMSIS/Include/core_cm0plus.h;Drivers/CMSIS/Include/core_cm33.h;Drivers/CMSIS/Include/core_cm7.h;Drivers/CMSIS/Include/cmsis_gcc.h;Drivers/CMSIS/Include/core_cm23.h;Drivers/CMSIS/Include/mpu_armv8.h;Drivers/CMSIS/Include/cmsis_armcc.h;Drivers/CMSIS/Include/pac_armv81.h;Drivers/CMSIS/Include/cmsis_iccarm.h;Drivers/CMSIS/Include/core_cm55.h;Drivers/CMSIS/Include/pmu_armv8.h;Drivers/CMSIS/Include/core_armv81mml.h;Drivers/CMSIS/Include/core_cm0.h;Drivers/CMSIS/Include/core_cm3.h;Drivers/CMSIS/Include/mpu_armv7.h;Drivers/CMSIS/Include/core_cm85.h;Drivers/CMSIS/Include/tz_context.h;Drivers/CMSIS/Include/cmsis_compiler.h;Drivers/CMSIS/Include/cmsis_version.h;Drivers/CMSIS/Include/core_sc300.h;Drivers/CMSIS/Include/core_armv8mbl.h;Drivers/CMSIS/Include/core_cm4.h;Drivers/CMSIS/Include/core_sc000.h;Drivers/CMSIS/Include/cachel1_armv7.h;Drivers/CMSIS/Include/core_cm35p.h;Drivers/CMSIS/Include/cmsis_armclang.h; [PreviousUsedCubeIDEFiles] -SourceFiles=Core/Src/main.c;Core/Src/stm32f4xx_it.c;Core/Src/stm32f4xx_hal_msp.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_adc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ramfunc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_gpio.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_cortex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_exti.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_uart.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_usb.c;Drivers/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c;Core/Src/system_stm32f4xx.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_adc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ramfunc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_gpio.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_cortex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_exti.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_uart.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_usb.c;Drivers/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c;Core/Src/system_stm32f4xx.c;;; +SourceFiles=Core/Src/main.c;Core/Src/stm32f4xx_it.c;Core/Src/stm32f4xx_hal_msp.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ramfunc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_gpio.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_cortex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_exti.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_uart.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_usb.c;Drivers/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c;Core/Src/system_stm32f4xx.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ramfunc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_gpio.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_cortex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_exti.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_uart.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_usb.c;Drivers/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c;Core/Src/system_stm32f4xx.c;;; HeaderPath=Drivers/STM32F4xx_HAL_Driver/Inc;Drivers/STM32F4xx_HAL_Driver/Inc/Legacy;Drivers/CMSIS/Device/ST/STM32F4xx/Include;Drivers/CMSIS/Include;Core/Inc; CDefines=USE_HAL_DRIVER;STM32F446xx;USE_HAL_DRIVER;USE_HAL_DRIVER; diff --git a/Code/Code.ioc b/Code/Code.ioc index e68c34c..2f6757c 100644 --- a/Code/Code.ioc +++ b/Code/Code.ioc @@ -8,19 +8,32 @@ ADC1.master=1 CAD.formats= CAD.pinconfig= CAD.provider= +Dma.Request0=UART4_RX +Dma.RequestsNb=1 +Dma.UART4_RX.0.Direction=DMA_PERIPH_TO_MEMORY +Dma.UART4_RX.0.FIFOMode=DMA_FIFOMODE_DISABLE +Dma.UART4_RX.0.Instance=DMA1_Stream2 +Dma.UART4_RX.0.MemDataAlignment=DMA_MDATAALIGN_BYTE +Dma.UART4_RX.0.MemInc=DMA_MINC_ENABLE +Dma.UART4_RX.0.Mode=DMA_CIRCULAR +Dma.UART4_RX.0.PeriphDataAlignment=DMA_PDATAALIGN_BYTE +Dma.UART4_RX.0.PeriphInc=DMA_PINC_DISABLE +Dma.UART4_RX.0.Priority=DMA_PRIORITY_LOW +Dma.UART4_RX.0.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode File.Version=6 KeepUserPlacement=false Mcu.CPN=STM32F446RET6 Mcu.Family=STM32F4 Mcu.IP0=ADC1 -Mcu.IP1=I2C2 -Mcu.IP2=NVIC -Mcu.IP3=RCC -Mcu.IP4=SPI1 -Mcu.IP5=SYS -Mcu.IP6=UART4 -Mcu.IP7=USB_OTG_FS -Mcu.IPNb=8 +Mcu.IP1=DMA +Mcu.IP2=I2C2 +Mcu.IP3=NVIC +Mcu.IP4=RCC +Mcu.IP5=SPI1 +Mcu.IP6=SYS +Mcu.IP7=UART4 +Mcu.IP8=USB_OTG_FS +Mcu.IPNb=9 Mcu.Name=STM32F446R(C-E)Tx Mcu.Package=LQFP64 Mcu.Pin0=PC13 @@ -67,6 +80,7 @@ Mcu.UserName=STM32F446RETx MxCube.Version=6.16.1 MxDb.Version=DB.6.0.161 NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.DMA1_Stream2_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false NVIC.ForceEnableDMAVector=true NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false @@ -140,9 +154,10 @@ PB4.GPIOParameters=GPIO_Label PB4.GPIO_Label=LORA_CS PB4.Locked=true PB4.Signal=GPIO_Output -PB5.GPIOParameters=GPIO_Label +PB5.GPIOParameters=PinState,GPIO_Label PB5.GPIO_Label=BARO_CS PB5.Locked=true +PB5.PinState=GPIO_PIN_SET PB5.Signal=GPIO_Output PB6.GPIOParameters=GPIO_Label PB6.GPIO_Label=FLASH_CS @@ -235,7 +250,7 @@ ProjectManager.LastFirmware=true ProjectManager.LibraryCopy=1 ProjectManager.MainLocation=Core/Src ProjectManager.NoMain=false -ProjectManager.PreviousToolchain= +ProjectManager.PreviousToolchain=STM32CubeIDE ProjectManager.ProjectBuild=false ProjectManager.ProjectFileName=Code.ioc ProjectManager.ProjectName=Code @@ -246,8 +261,9 @@ ProjectManager.TargetToolchain=Makefile ProjectManager.ToolChainLocation= ProjectManager.UAScriptAfterPath= ProjectManager.UAScriptBeforePath= -ProjectManager.UnderRoot=false -ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_ADC1_Init-ADC1-false-HAL-true,4-MX_I2C2_Init-I2C2-false-HAL-true,5-MX_SPI1_Init-SPI1-false-HAL-true,6-MX_UART4_Init-UART4-false-HAL-true,7-MX_USB_OTG_FS_PCD_Init-USB_OTG_FS-false-HAL-true +<<<<<<< HEAD +ProjectManager.UnderRoot=true +ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_DMA_Init-DMA-false-HAL-true,4-MX_ADC1_Init-ADC1-false-HAL-true,5-MX_I2C2_Init-I2C2-false-HAL-true,6-MX_SPI1_Init-SPI1-false-HAL-true,7-MX_UART4_Init-UART4-false-HAL-true,8-MX_USB_OTG_FS_PCD_Init-USB_OTG_FS-false-HAL-true RCC.AHBFreq_Value=180000000 RCC.APB1CLKDivider=RCC_HCLK_DIV4 RCC.APB1Freq_Value=45000000 @@ -319,13 +335,16 @@ SH.S_TIM1_CH2.0=TIM1_CH2 SH.S_TIM1_CH2.ConfNb=1 SH.S_TIM1_CH3.0=TIM1_CH3 SH.S_TIM1_CH3.ConfNb=1 -SPI1.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_2 -SPI1.CalculateBaudRate=45.0 MBits/s +SPI1.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_16 +SPI1.CLKPhase=SPI_PHASE_1EDGE +SPI1.CLKPolarity=SPI_POLARITY_LOW +SPI1.CalculateBaudRate=5.625 MBits/s SPI1.Direction=SPI_DIRECTION_2LINES -SPI1.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate,BaudRatePrescaler +SPI1.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate,BaudRatePrescaler,CLKPhase,CLKPolarity SPI1.Mode=SPI_MODE_MASTER SPI1.VirtualType=VM_MASTER -UART4.IPParameters=VirtualMode +UART4.BaudRate=9600 +UART4.IPParameters=VirtualMode,BaudRate UART4.VirtualMode=Asynchronous USB_OTG_FS.IPParameters=VirtualMode USB_OTG_FS.VirtualMode=Device_Only diff --git a/Code/Core/Inc/DataContainer.h b/Code/Core/Inc/DataContainer.h index 7867236..3c063df 100644 --- a/Code/Core/Inc/DataContainer.h +++ b/Code/Core/Inc/DataContainer.h @@ -8,6 +8,8 @@ #ifndef INC_DATACONTAINER_H_ #define INC_DATACONTAINER_H_ +#include + /** * @class DataContainer * @brief Central data storage and communication hub for all subsystems. @@ -29,10 +31,66 @@ class DataContainer { private: - public: + /* IMU Data*/ + float LSM6DSV320GyroX_dps; + float LSM6DSV320GyroY_dps; + float LSM6DSV320GyroZ_dps; + + float LSM6DSV320LowGAccelX_mps2; + float LSM6DSV320LowGAccelY_mps2; + float LSM6DSV320LowGAccelZ_mps2; + + float LSM6DSV320HighGAccelX_mps2; + float LSM6DSV320HighGAccelY_mps2; + float LSM6DSV320HighGAccelZ_mps2; + + /* Barometer Data*/ + float MS560702BA03Temperature_C; + float MS560702BA03Pressure_hPA; + float MS560702BA03Altitude_m; + + /* GPS Data */ + // Latitude in decimal degrees (+N, -S) + float GPSLatitude; + // Longitude in decimal degrees (+E, -W) + float GPSLongitude; + // Altitude in meters (from GPS) + float GPSAltitude_m; + // GPS fix status: 0 = invalid, 1 = GPS fix, 2 = DGPS + int GPSFix; + // Number of satellites used in fix + int GPSNumSatellites; + // UTC time string from GPS (hhmmss.sss) + char GPSUTCTime[16]; + + /* Quaternion Data */ + float quaternionW; + float quaternionX; + float quaternionY; + float quaternionZ; + + float quaternionNorm; + + /* Euler Angles */ + float roll; + float pitch; + float yaw; + + /* Kalman Filter Data */ + float KalmanFilterPositionX_m; + float KalmanFilterPositionY_m; + float KalmanFilterPositionZ_m; + + float KalmanFilterVelocityX_mps; + float KalmanFilterVelocityY_mps; + float KalmanFilterVelocityZ_mps; + + float KalmanFilterAccelerationX_mps2; + float KalmanFilterAccelerationY_mps2; + float KalmanFilterAccelerationZ_mps2; }; -#endif /* INC_DATACONTAINER_H_ */ \ No newline at end of file +#endif /* INC_DATACONTAINER_H_ */ diff --git a/Code/Core/Inc/Devices/GPS.h b/Code/Core/Inc/Devices/GPS.h new file mode 100644 index 0000000..3b330ea --- /dev/null +++ b/Code/Core/Inc/Devices/GPS.h @@ -0,0 +1,102 @@ +/* + * GPS.h + * + * Created on: Jan 08, 2026 + * Author: colin + */ + +#ifndef SRC_DEVICES_GPS_H_ +#define SRC_DEVICES_GPS_H_ + +#include "main.h" +#include "DataContainer.h" +#include + +#define GPS_DMA_BUFFER_SIZE 512 +#define GPS_UBX_MAX_PAYLOAD 256 + +class GPS { +public: + GPS(DataContainer* data, UART_HandleTypeDef* uart, uint8_t* dmaBuffer); + + int deviceInit(); + int updateDevice(); + +private: + DataContainer* data; + UART_HandleTypeDef* uart; + uint8_t* dmaBuffer; + + uint16_t currentBufferPos; + uint16_t lastDmaBufferPos; + + // UBX message buffer + uint8_t ubxBuffer[GPS_UBX_MAX_PAYLOAD]; + uint16_t ubxIndex; + + // UBX-NAV-PVT structure (92 bytes payload) + struct __attribute__((packed)) UBX_NAV_PVT { + uint32_t iTOW; // GPS time of week (ms) + uint16_t year; // Year (UTC) + uint8_t month; // Month (1-12) + uint8_t day; // Day (1-31) + uint8_t hour; // Hour (0-23) + uint8_t min; // Minute (0-59) + uint8_t sec; // Second (0-60) + uint8_t valid; // Validity flags + uint32_t tAcc; // Time accuracy estimate (ns) + int32_t nano; // Fraction of second (-1e9 to 1e9) + uint8_t fixType; // GPS fix type (0=none, 3=3D fix) + uint8_t flags; // Fix status flags + uint8_t flags2; // Additional flags + uint8_t numSV; // Number of satellites + int32_t lon; // Longitude (1e-7 deg) + int32_t lat; // Latitude (1e-7 deg) + int32_t height; // Height above ellipsoid (mm) + int32_t hMSL; // Height above MSL (mm) + uint32_t hAcc; // Horizontal accuracy (mm) + uint32_t vAcc; // Vertical accuracy (mm) + int32_t velN; // North velocity (mm/s) + int32_t velE; // East velocity (mm/s) + int32_t velD; // Down velocity (mm/s) + int32_t gSpeed; // Ground speed (mm/s) + int32_t headMot; // Heading of motion (1e-5 deg) + uint32_t sAcc; // Speed accuracy (mm/s) + uint32_t headAcc; // Heading accuracy (1e-5 deg) + uint16_t pDOP; // Position DOP (0.01) + uint8_t reserved1[6]; // Reserved + int32_t headVeh; // Heading of vehicle (1e-5 deg) + int16_t magDec; // Magnetic declination (1e-2 deg) + uint16_t magAcc; // Magnetic declination accuracy (1e-2 deg) + }; + + // UBX parser state machine + enum UBXParseState { + WAIT_SYNC1, + WAIT_SYNC2, + GET_CLASS, + GET_ID, + GET_LENGTH1, + GET_LENGTH2, + GET_PAYLOAD, + GET_CHECKSUM1, + GET_CHECKSUM2 + }; + + UBXParseState parseState; + uint8_t msgClass; + uint8_t msgId; + uint16_t payloadLength; + uint16_t payloadIndex; + uint8_t ckA, ckB; + + // Private methods + void processDMAData(); + bool parseUBXByte(uint8_t byte); + void processUBXMessage(); + void sendUBXCommand(const uint8_t* cmd, uint16_t len); + void calculateChecksum(const uint8_t* data, uint16_t len, uint8_t* ckA, uint8_t* ckB); + void configureGPS(); +}; + +#endif /* SRC_DEVICES_GPS_H_ */ \ No newline at end of file diff --git a/Code/Core/Inc/Devices/I2CDevice.h b/Code/Core/Inc/Devices/I2CDevice.h new file mode 100644 index 0000000..0b8d84e --- /dev/null +++ b/Code/Core/Inc/Devices/I2CDevice.h @@ -0,0 +1,109 @@ +/* + * I2CDevice.h + * + * Created on: Oct 29, 2025 + * Author: colin + */ + +#ifndef INC_DEVICES_I2CDEVICE_H_ +#define INC_DEVICES_I2CDEVICE_H_ + +#include /* Needed for i2c */ + +#include "DataContainer.h" + +/** + * @class I2CDevice + * @brief Abstract base class for I2C-based peripheral devices. + * + * The I2CDevice class provides a generic interface for communicating + * with I2C peripherals on an STM32 microcontroller. It manages + * low-level I2C read and write transactions while allowing derived + * classes to implement device-specific initialization and logic. + * + * Derived classes should implement `deviceInit()` to configure the + * specific hardware device (e.g., sensors, EEPROMs, ADCs). + * * + * @note The I2C peripheral must be initialized before using this class. + */ +class I2CDevice { +public: + /** + * @brief Construct a new I2CDevice object. + * + * Initializes a generic I2C device interface and stores a reference + * to the shared DataContainer for system-wide access. + * The specific I2C peripheral and device address should be + * configured by the derived class or initialization routine. + * + * @param data Pointer to the main DataContainer used for sharing system data. + */ + I2CDevice(DataContainer* data); + + /** + * @brief Initialize the I2C device. + * + * Must be implemented by derived classes to configure device registers, + * verify communication, or perform startup routines. + * + * @return int Status code (0 for success, negative for failure). + */ + virtual int deviceInit() = 0; + + /** + * @brief Perform periodic device tasks or updates. + * + * This pure virtual method should be implemented by derived I2C device classes + * to execute regular operations that need to run in the main loop, such as: + * - Reading sensor data + * - Updating internal state + * - Performing background processing or checks + * + * The method is intended to be called periodically, typically from the main + * system loop or a scheduler. + * + * @return int Status code (0 for success, negative for failure). + */ + virtual int updateDevice() = 0; + +private: + I2C_HandleTypeDef *i2cHandler; // I2C peripheral handle. + uint8_t address; // 7-bit or 8-bit I2C device address. + +protected: + DataContainer* data; // Pointer to the shared DataContainer. + + /** + * @brief Read data from an I2C device register. + * + * Reads one or more bytes from a specific register of the I2C device. + * + * @param reg Register address to read from. + * @param data Pointer to the buffer to store the received data. + * @param len Number of bytes to read (default = 1). + * @return HAL_StatusTypeDef HAL_OK if successful, or an appropriate HAL error code. + * + * @note This function assumes the I2C peripheral has been properly + * initialized before calling. It should typically be used + * by higher-level device-specific read functions. + */ + HAL_StatusTypeDef readI2C(uint16_t reg, uint8_t *data, uint16_t len=1); + + /** + * @brief Write data to an I2C device register. + * + * Writes one or more bytes to a specific register of the I2C device. + * + * @param reg Register address to write to. + * @param data Pointer to the buffer containing the data to write. + * @param len Number of bytes to write (default = 1). + * @return HAL_StatusTypeDef HAL_OK if successful, or an appropriate HAL error code. + * + * @note This function assumes the I2C peripheral has been properly + * initialized before calling. It should typically be used + * by higher-level device-specific write functions. + */ + HAL_StatusTypeDef writeI2C(uint8_t reg, uint8_t *data, uint8_t len=1); +}; + +#endif /* INC_DEVICES_I2CDEVICE_H_ */ diff --git a/Code/Core/Inc/Devices/SPIDevice.h b/Code/Core/Inc/Devices/SPIDevice.h new file mode 100644 index 0000000..7597fa2 --- /dev/null +++ b/Code/Core/Inc/Devices/SPIDevice.h @@ -0,0 +1,126 @@ +/* + * SPIDevice.h + * + * Created on: Oct 29, 2025 + * Author: colin + */ + +#ifndef INC_DEVICES_SPIDEVICE_H_ +#define INC_DEVICES_SPIDEVICE_H_ + +#include /* Needed for memcpy */ +#include /* Needed for spi */ + +#include "DataContainer.h" + +/** + * @class SPIDevice + * @brief Abstract base class for SPI-based peripheral devices. + * + * The SPIDevice class provides a generic interface for communicating + * with devices over the SPI bus on an STM32 microcontroller. It handles + * low-level SPI transactions such as reading from and writing to device + * registers while allowing derived classes to implement device-specific + * initialization and functionality. + * + * This class is designed to be inherited by specific SPI device drivers + * (e.g., sensors, memory chips, ADCs, etc.) which will implement their + * own initialization and data handling routines. + * + * @note This class assumes SPI is configured and initialized externally + * before calling any read or write operations. + */ +class SPIDevice { +public: + + /** + * @brief Construct a new SPIDevice object. + * + * Initializes a generic SPI device interface and stores references to + * the SPI peripheral, chip select (CS) pin, and main DataContainer. + * This constructor does not perform any hardware initialization; it + * simply sets up references for derived device classes. + * + * @param data Pointer to the main DataContainer used for sharing system data. + * @param spi Pointer to the SPI handle (SPI_HandleTypeDef) used for communication. + * @param port Pointer to the GPIO port that controls the chip select (CS) pin. + * @param pin GPIO pin number used for the chip select (CS) signal. + */ + SPIDevice(DataContainer* data, SPI_HandleTypeDef *spi, GPIO_TypeDef *port, uint16_t pin); + + /** + * @brief Initialize the SPI device. + * + * Must be implemented by derived classes to configure device registers, + * verify communication, or perform startup routines. + * + * @return int Status code (0 for success, negative for failure). + */ + virtual int deviceInit() = 0; + + /** + * @brief Perform periodic device tasks or updates. + * + * This pure virtual method should be implemented by derived SPI device classes + * to execute regular operations that need to run in the main loop, such as: + * - Reading sensor data + * - Updating internal state + * - Performing background processing or checks + * + * The method is intended to be called periodically, typically from the main + * system loop or a scheduler. + * + * @return int Status code (0 for success, negative for failure). + */ + virtual int updateDevice() = 0; + +private: + HAL_StatusTypeDef status; // SPI read/write status + +protected: + DataContainer* data; // Pointer to the shared DataContainer. + + SPI_HandleTypeDef *spiHandler; // SPI peripheral handle. + GPIO_TypeDef *chipSelectPort; // Pointer to the port connected to the Chip Select Pin + uint16_t chipSelectPin; // The Chip Select Pin + + /** + * @brief Read data from an SPI device register. + * + * Sends a register address to the SPI device and reads back one or more bytes + * of data. The chip select (CS) line is automatically asserted low before the + * transaction and released high afterward. + * + * @param reg Register address to read from. + * @param data Pointer to the buffer where the received data will be stored. + * @param len Number of bytes to read (default = 1). + * @return HAL_StatusTypeDef HAL_OK if successful, or an appropriate HAL error code. + * + * @note This function assumes the SPI peripheral has been properly initialized + * before calling. It should typically be used by higher-level device + * functions to read configuration or sensor data. + */ + HAL_StatusTypeDef readSPI(uint8_t reg, uint8_t *data, uint8_t len=1); + + + /** + * @brief Write data to an SPI device register. + * + * Sends a register address followed by one or more bytes of data to the SPI + * device. The chip select (CS) line is automatically asserted low before the + * transaction and released high afterward. + * + * @param reg Register address to write to. + * @param data Pointer to the buffer containing the data to send. + * @param len Number of bytes to write (default = 1). + * @return HAL_StatusTypeDef HAL_OK if successful, or an appropriate HAL error code. + * + * @note This function assumes the SPI peripheral has been properly initialized + * before calling. It should typically be used by higher-level device + * functions to configure registers or send commands. + */ + HAL_StatusTypeDef writeSPI(uint8_t reg, uint8_t *data, uint8_t len=1); + +}; + +#endif /* INC_DEVICES_SPIDEVICE_H_ */ diff --git a/Code/Core/Inc/Devices/SPI_Devices/LSM6DSV320.h b/Code/Core/Inc/Devices/SPI_Devices/LSM6DSV320.h new file mode 100644 index 0000000..e0801c4 --- /dev/null +++ b/Code/Core/Inc/Devices/SPI_Devices/LSM6DSV320.h @@ -0,0 +1,391 @@ +/* + * LSM6DSV320.h + * + * Created on: Oct 29, 2025 + * Author: colin + */ + +#ifndef SRC_DEVICES_SPI_DEVICES_LSM6DSV320_H_ +#define SRC_DEVICES_SPI_DEVICES_LSM6DSV320_H_ + +#include "Devices/SPIDevice.h" + +#include "defines.h" + +// Register Address +#define LSM6DSV320_FUNC_CFG_ACCESS 0x01 +#define LSM6DSV320_PIN_CTRL 0x02 +#define LSM6DSV320_IF_CFG 0x03 +/* Reserved 0x04 - 0x05 */ +#define LSM6DSV320_ODR_TRIG_CFG 0x06 +#define LSM6DSV320_FIFO_CTRL1 0x07 +#define LSM6DSV320_FIFO_CTRL2 0x08 +#define LSM6DSV320_FIFO_CTRL3 0x09 +#define LSM6DSV320_FIFO_CTRL4 0x0A +#define LSM6DSV320_COUNTER_BDR_REG1 0x0B +#define LSM6DSV320_COUNTER_BDR_REG2 0x0C +#define LSM6DSV320_INT1_CTLR 0x0D +#define LSM6DSV320_INT2_CTLR 0x0E +#define LSM6DSV320_WHO_AM_I 0x0F +#define LSM6DSV320_CTRL1 0x10 +#define LSM6DSV320_CTRL2 0x11 +#define LSM6DSV320_CTRL3 0x12 +#define LSM6DSV320_CTRL4 0x13 +#define LSM6DSV320_CTRL5 0x14 +#define LSM6DSV320_CTRL6 0x15 +#define LSM6DSV320_CTRL7 0x16 +#define LSM6DSV320_CTRL8 0x17 +#define LSM6DSV320_CTRL9 0x18 +#define LSM6DSV320_CTRL10 0x19 +#define LSM6DSV320_CTRL_STATUS 0x1A +#define LSM6DSV320_FIFO_STATUS1 0x1B +#define LSM6DSV320_FIFO_STATUS2 0x1C +#define LSM6DSV320_ALL_INT_REG 0x1D +#define LSM6DSV320_STATUS_REG 0x1E +/* Reserved 0x1F */ +#define LSM6DSV320_OUT_TEMP_L 0x20 +#define LSM6DSV320_OUT_TEMP_H 0x21 +#define LSM6DSV320_OUTX_L_G 0x22 +#define LSM6DSV320_OUTX_H_G 0x23 +#define LSM6DSV320_OUTY_L_G 0x24 +#define LSM6DSV320_OUTY_H_G 0x25 +#define LSM6DSV320_OUTZ_L_G 0x26 +#define LSM6DSV320_OUTZ_H_G 0x27 +#define LSM6DSV320_OUTX_L_A 0x28 +#define LSM6DSV320_OUTX_H_A 0x29 +#define LSM6DSV320_OUTY_L_A 0x2A +#define LSM6DSV320_OUTY_H_A 0x2B +#define LSM6DSV320_OUTZ_L_A 0x2C +#define LSM6DSV320_OUTZ_H_A 0x2D +#define LSM6DSV320_UI_OUTX_L_G_OIS_EIS 0x2E +#define LSM6DSV320_UI_OUTX_H_G_OIS_EIS 0x2F +#define LSM6DSV320_UI_OUTY_L_G_OIS_EIS 0x30 +#define LSM6DSV320_UI_OUTY_H_G_OIS_EIS 0x31 +#define LSM6DSV320_UI_OUTZ_L_G_OIS_EIS 0x32 +#define LSM6DSV320_UI_OUTZ_H_G_OIS_EIS 0x33 +#define LSM6DSV320_UI_OUTX_L_A_OIS_HG 0x34 +#define LSM6DSV320_UI_OUTX_H_A_OIS_HG 0x35 +#define LSM6DSV320_UI_OUTY_L_A_OIS_HG 0x36 +#define LSM6DSV320_UI_OUTY_H_A_OIS_HG 0x37 +#define LSM6DSV320_UI_OUTZ_L_A_OIS_HG 0x38 +#define LSM6DSV320_UI_OUTZ_H_A_OIS_HG 0x39 +/* Reserved 0x3A - 0x3F */ +#define LSM6DSV320_TIMESTAMP0 0x40 +#define LSM6DSV320_TIMESTAMP1 0x41 +#define LSM6DSV320_TIMESTAMP2 0x42 +#define LSM6DSV320_TIMESTAMP3 0x43 +#define LSM6DSV320_UI_STATUS_REG_OIS 0x44 +#define LSM6DSV320_WAKE_UP_SRC 0x45 +#define LSM6DSV320_TAP_SRC 0x46 +#define LSM6DSV320_D6D_SRC 0x47 +#define LSM6DSV320_STATUS_MASTER_MAINPAGE 0x48 +#define LSM6DSV320_EMB_FUNC_MASTER_MAINPAGE 0x49 +#define LSM6DSV320_FSM_MASTER_MAINPAGE 0x4A +#define LSM6DSV320_MLS_MASTER_MAINPAGE 0x4B +#define LSM6DSV320_HG_WAKE_UP_SRC 0x4C +#define LSM6DSV320_CTRL2_XL_HG 0x4D +#define LSM6DSV320_CTRL1_XL_HG 0x4E +#define LSM6DSV320_INTERNAL_FREQ_FINE 0x4F +#define LSM6DSV320_FUNCTIONS_ENABLE 0x50 +/* Reserved 0x51 */ +#define LSM6DSV320_HG_FUNCTIONS_ENABLE 0x52 +#define LSM6DSV320_HG_WAKE_UP_THS 0x53 +#define LSM6DSV320_INACTIVITY_DUR 0x54 +#define LSM6DSV320_INACTIVITY_THS 0x55 +#define LSM6DSV320_TAP_CFG0 0x56 +#define LSM6DSV320_TAP_CFG1 0x57 +#define LSM6DSV320_TAP_CFG2 0x58 +#define LSM6DSV320_TAP_THS_6D 0x59 +#define LSM6DSV320_TAP_DUR 0x5A +#define LSM6DSV320_WAKE_UP_THS 0x5B +#define LSM6DSV320_WAKE_UP_DUR 0x5C +#define LSM6DSV320_FREE_FALL 0x5D +#define LSM6DSV320_MD1_CFG 0x5E +#define LSM6DSV320_MD2_CFG 0x5F +/* Reserved 0x60 - 0x61 */ +#define LSM6DSV320_HAODR_CFG 0x62 +#define LSM6DSV320_EMB_FUNC_CTRL 0x63 +#define LSM6DSV320_UI_HANDSHAKE_CTRL 0x64 +#define LSM6DSV320_UI_SPI2_SHARED_0 0x65 +#define LSM6DSV320_UI_SPI2_SHARED_1 0x66 +#define LSM6DSV320_UI_SPI2_SHARED_2 0x67 +#define LSM6DSV320_UI_SPI2_SHARED_3 0x68 +#define LSM6DSV320_UI_SPI2_SHARED_4 0x69 +#define LSM6DSV320_UI_SPI2_SHARED_5 0x6A +#define LSM6DSV320_CTRL_EIS 0x6B +#define LSM6DSV320_XL_HG_X_OFS_USR 0x6C +#define LSM6DSV320_XL_HG_Y_OFS_USR 0x6D +#define LSM6DSV320_XL_HG_Z_OFS_USR 0x6E +#define LSM6DSV320_UI_INT_OIS 0x6F +#define LSM6DSV320_UI_CTRL1_OIS 0x70 +#define LSM6DSV320_UI_CTRL2_OIS 0x71 +#define LSM6DSV320_UI_CTRL3_OIS 0x72 +#define LSM6DSV320_X_OFS_USR 0x73 +#define LSM6DSV320_Y_OFS_USR 0x74 +#define LSM6DSV320_Z_OFS_USR 0x75 +/* Reserved 0x76 - 0x77 */ +#define LSM6DSV320_FIFO_DATA_OUT_TAG 0x78 +#define LSM6DSV320_FIFO_DATA_OUT_X_L 0x79 +#define LSM6DSV320_FIFO_DATA_OUT_X_H 0x7A +#define LSM6DSV320_FIFO_DATA_OUT_Y_L 0x7B +#define LSM6DSV320_FIFO_DATA_OUT_Y_H 0x7C +#define LSM6DSV320_FIFO_DATA_OUT_Z_L 0x7D +#define LSM6DSV320_FIFO_DATA_OUT_Z_H 0x7E + + +#define LSM6DSV320_ID 0x73 +// #define LSM6DSV320_ID 0x64 + +// Sensitivity +#define LSM6DSV320_LOW_G_SENSITIVITY_2G 0.000061f // +/- 2g 0.061 mg/LSB = 0.000061 g/LSB +#define LSM6DSV320_LOW_G_SENSITIVITY_4G 0.000122f // +/- 4g 0.122 mg/LSB = 0.000122 g/LSB +#define LSM6DSV320_LOW_G_SENSITIVITY_8G 0.000244f // +/- 8g 0.244 mg/LSB = 0.000244 g/LSB +#define LSM6DSV320_LOW_G_SENSITIVITY_16G 0.000488f // +/- 16g 0.488 mg/LSB = 0.000488 g/LSB + +#define LSM6DSV320_HIGH_G_SENSITIVITY_32G 0.000976f // +/- 32g 1.952 mg/LSB = 0.000976 g/LSB +#define LSM6DSV320_HIGH_G_SENSITIVITY_64G 0.001952f // +/- 64g 3.904 mg/LSB = 0.001952 g/LSB +#define LSM6DSV320_HIGH_G_SENSITIVITY_128G 0.003904f // +/- 128g 7.808 mg/LSB = 0.003904 g/LSB +#define LSM6DSV320_HIGH_G_SENSITIVITY_256G 0.007808f // +/- 256g 15.616 mg/LSB = 0.007808 g/LSB +#define LSM6DSV320_HIGH_G_SENSITIVITY_320G 0.010417f // +/- 320g 20.833 mg/LSB = 0.010417 g/LSB + +#define LSM6DSV320_GYRO_SENSITIVITY_250DPS 0.00875f // +/- 250dgps 8.75 mdps/LSB = 0.00875 dps/LSB +#define LSM6DSV320_GYRO_SENSITIVITY_500DPS 0.0175f // +/- 500dgps 17.5 mdps/LSB = 0.0175 dps/LSB +#define LSM6DSV320_GYRO_SENSITIVITY_1000DPS 0.0175f // +/- 1000dgps 35 mdps/LSB = 0.0175 dps/LSB +#define LSM6DSV320_GYRO_SENSITIVITY_2000DPS 0.07f // +/- 2000dgps 70 mdps/LSB = 0.07 dps/LSB +#define LSM6DSV320_GYRO_SENSITIVITY_4000DPS 0.14f // +/- 4000dgps 140 mdps/LSB = 0.14 dps/LSB + + +/** + * @class LSM6DSV320 + * @brief Driver for the LSM6DSV320 Inertial Measurement Unit (IMU). + * + * This class implements an SPI-based driver for the **LSM6DSV320** 6-axis IMU, + * which includes two 3-axis accelerometer (low-g and high-g) and a 3-axis gyroscope. + * It provides enumerations for all major configuration options such as + * full-scale range, output data rate (ODR), and operating mode. + * + * The driver communicates via SPI using the `SPIDevice` base class and + * implements a device-specific initialization routine (`deviceInit()`). + * + * Configuration options are based on the official STMicroelectronics datasheet. + * Default values are selected for typical performance-oriented operation. + * + * @see STMicroelectronics LSM6DSV320 Datasheet, Rev 2.0, March 2025 + */ +class LSM6DSV320: public SPIDevice { +public: + + /** + * @brief Construct a new LSM6DSV320 device. + * + * Initializes the LSM6DSV320 SPI driver interface and sets default + * configuration parameters for the IMU. The SPI interface, CS port, + * and pin are passed to the base `SPIDevice` class. + * + * @param data Pointer to the main DataContainer used for shared system data. + * @param spi Pointer to the SPI handle (SPI_HandleTypeDef). + * @param port Pointer to the GPIO port controlling the chip select (CS) pin. + * @param pin GPIO pin number used for chip select (CS). + */ + LSM6DSV320(DataContainer* data, SPI_HandleTypeDef *spi, GPIO_TypeDef *port, uint16_t pin); + + /** + * @brief Initialize the LSM6DSV320 device. + * + * Configures the IMU by writing default settings to key control registers, + * verifying communication with the device, and preparing it for operation. + * + * @return int Status code (0 for success, negative for failure). + */ + int deviceInit() override; + + /** + * @brief Perform periodic updates for the LSM6DSV320 IMU. + * + * This method should be called regularly (e.g., in the main control loop) + * to read sensor data from the accelerometer and gyroscope, process it + * as needed, and update internal state or shared DataContainer values. + * + * Typical tasks performed by this method include: + * - Reading raw accelerometer and gyroscope registers via SPI + * - Converting raw readings into physical units using sensitivity constants + * - Updating the main DataContainer with latest sensor values + * - Performing basic filtering or status checks if required + * + * @return int Status code: + * - 0: Update successful + * - Negative value: Error reading data or SPI communication failure + * + * @note This function does not initialize the device; `deviceInit()` must + * be called first. + */ + int updateDevice() override; + +private: + // ------------------------------------------------------------- + // Low-G Accelerometer Configuration + // ------------------------------------------------------------- + + /** + * @enum LSM6DSV320_LOW_G_RANGE + * @brief Low-g accelerometer full-scale range options. + * + * Defined in datasheet (Table 70, page 71). + */ + enum LSM6DSV320_LOW_G_RANGE { + LOW_G_RANGE_2G = 0b00, + LOW_G_RANGE_4G = 0b01, + LOW_G_RANGE_8G = 0b10, + LOW_G_RANGE_16G = 0b11 + } lowGRange = LOW_G_RANGE_16G; + + /** + * @enum LSM6DSV320_LOW_G_OP_MODE + * @brief Low-g accelerometer operating mode options. + * + * Defined in datasheet (Table 53, page 65). + */ + enum LSM6DSV320_LOW_G_OP_MODE { + LOW_G_HIGH_PERFORMANCE = 0b000, // High-performance mode (default) + LOW_G_HIGH_ACCURACY = 0b001, // High-accuracy ODR mode + // Reserved + LOW_G_ODR_TRIGGERED = 0b011, // ODR-triggered mode + LOW_G_LOW_POWER1 = 0b100, // Low-power mode 1 + LOW_G_LOW_POWER2 = 0b101, // Low-power mode 2 + LOW_G_LOW_POWER3 = 0b110, // Low-power mode 3 + LOW_G_NORMAL = 0b111 // Normal + } lowGOPMode = LOW_G_HIGH_ACCURACY; + + /** + * @enum LSM6DSV320_LOW_G_ODR + * @brief Low-g accelerometer output data rate options. + * + * Defined in datasheet (Table 54, page 65). + */ + enum LSM6DSV320_LOW_G_ODR { + LOW_G_ODR_POWER_DOWN = 0b0000, // Power-down (default) + LOW_G_ODR_1_875HZ = 0b0001, // 1.875 Hz (low-power mode) + LOW_G_ODR_7_5HZ = 0b0010, // 7.5 Hz (high-performance, normal mode) + LOW_G_ODR_15HZ = 0b0011, // 15 Hz (low-power, high-performance, normal mode) + LOW_G_ODR_30HZ = 0b0100, // 30 Hz (low-power, high-performance, normal mode) + LOW_G_ODR_60HZ = 0b0101, // 60 Hz (low-power, high-performance, normal mode) + LOW_G_ODR_120HZ = 0b0110, // 120 Hz (low-power, high-performance, normal mode) + LOW_G_ODR_240HZ = 0b0111, // 240 Hz (low-power, high-performance, normal mode) + LOW_G_ODR_480HZ = 0b1000, // 480 Hz (high-performance, normal mode) + LOW_G_ODR_960HZ = 0b1001, // 960 Hz (high-performance, normal mode) + LOW_G_ODR_1_92KHZ = 0b1010, // 1.92 kHz (high-performance mode) + LOW_G_ODR_7_68KHZ = 0b1011 // 7.68 kHz (high-performance mode) + } lowGODR = LOW_G_ODR_960HZ; + + + // ------------------------------------------------------------- + // High-G Accelerometer Configuration + // ------------------------------------------------------------- + + /** + * @enum LSM6DSV320_HIGH_G_RANGE + * @brief High-g accelerometer full-scale range options. + * + * Defined in datasheet (Table 150, page 91). + */ + enum LSM6DSV320_HIGH_G_RANGE { + HIGH_G_RANGE_32G = 0b000, + HIGH_G_RANGE_64G = 0b001, + HIGH_G_RANGE_128G = 0b010, + HIGH_G_RANGE_256G = 0b011, + HIGH_G_RANGE_320G = 0b100, + } highGRange = HIGH_G_RANGE_32G; + + /** + * @enum LSM6DSV320_HIGH_G_ODR + * @brief High-g accelerometer output data rate options. + * + * Defined in datasheet (Table 149, page 91). + */ + enum LSM6DSV320_HIGH_G_ODR { + HIGH_G_ODR_POWER_DOWN = 0b000, // Power-down (default) + // Reserved + // Reserved + HIGH_G_ODR_480_HZ = 0b011, // 480 Hz + HIGH_G_ODR_960_HZ = 0b100, // 960 Hz + HIGH_G_ODR_1_92_KHZ = 0b101, // 1.92 kHz + HIGH_G_ODR_3_84_KHZ = 0b110, // 3.84 kHz + HIGH_G_ODR_7_68_KHZ = 0b111, // 7.68 kHz + } highGODR = HIGH_G_ODR_960_HZ; + + + // ------------------------------------------------------------- + // Gyroscope Configuration + // ------------------------------------------------------------- + + /** + * @enum LSM6DSV320_GYRO_RANGE + * @brief Gyroscope full-scale range options. + * + * Defined in datasheet (Table 65, page 69). + */ + enum LSM6DSV320_GYRO_RANGE { + GYRO_RANGE_250DPS = 0b001, + GYRO_RANGE_500DPS = 0b010, + GYRO_RANGE_1000DPS = 0b011, + GYRO_RANGE_2000DPS = 0b100, + GYRO_RANGE_4000DPS = 0b101 + } gyroRange = GYRO_RANGE_250DPS; + + /** + * @enum LSM6DSV320_GYRO_OP_MODE + * @brief Gyroscope operating mode options. + * + * Defined in datasheet (Table 53, page 65). + */ + enum LSM6DSV320_GYRO_OP_MODE { + GYRO_HIGH_PERFORMANCE = 0b000, // High-performance mode (default) + GYRO_HIGH_ACCURACY = 0b001, // High-accuracy ODR mode + // Reserved + GYRO_ODR_TRIGGERED = 0b011, // ODR-triggered mode + GYRO_SLEEP = 0b100, // Sleep mode + GYRO_LOW_POWER = 0b101, // Low-power mode + } gyroOPMode = GYRO_HIGH_ACCURACY; + + /** + * @enum LSM6DSV320_GYRO_ODR + * @brief Gyroscope output data rate options. + * + * Defined in datasheet (Table 54, page 65). + */ + enum LSM6DSV320_GYRO_ODR { + GYRO_ODR_POWER_DOWN = 0b0000, // Power-down (default) + GYRO_ODR_7_5HZ = 0b0010, // 7.5 Hz (high-performance mode) + GYRO_ODR_15HZ = 0b0011, // 15 Hz (low-power, high-performance mode) + GYRO_ODR_30HZ = 0b0100, // 30 Hz (low-power, high-performance mode) + GYRO_ODR_60HZ = 0b0101, // 60 Hz (low-power, high-performance mode) + GYRO_ODR_120HZ = 0b0110, // 120 Hz (low-power, high-performance mode) + GYRO_ODR_240HZ = 0b0111, // 240 Hz (low-power, high-performance mode) + GYRO_ODR_480HZ = 0b1000, // 480 Hz (high-performance mode) + GYRO_ODR_960HZ = 0b1001, // 960 Hz (high-performance mode) + GYRO_ODR_1_92KHZ = 0b1010, // 1.92 kHz (high-performance mode) + GYRO_ODR_3_58KHZ = 0b1011, // 3.58 kHz (high-performance mode) + GYRO_ODR_7_68KHZ = 0b1100 // 7.68 kHz (high-performance mode) + } gyroODR = GYRO_ODR_960HZ; + + // ------------------------------------------------------------- + // Sensitivity Constants + // ------------------------------------------------------------- + float lowGSensitivity; + float highGSensitivity; + float gyroSensitivity; + + + // ------------------------------------------------------------- + // Bias Constants + // ------------------------------------------------------------- + int numOfSamples = 5000; + + float gyroXBias; + float gyroYBias; + float gyroZBias; + + uint8_t buffer[6]; +}; + +#endif /* SRC_DEVICES_SPI_DEVICES_LSM6DSV320_H_ */ diff --git a/Code/Core/Inc/Devices/SPI_Devices/MS560702BA03.h b/Code/Core/Inc/Devices/SPI_Devices/MS560702BA03.h new file mode 100644 index 0000000..80e74a3 --- /dev/null +++ b/Code/Core/Inc/Devices/SPI_Devices/MS560702BA03.h @@ -0,0 +1,151 @@ +/* + * MS560702BA03.h + * + * Created on: Oct 29, 2025 + * Author: colin + */ + +#ifndef SRC_DEVICES_SPI_DEVICES_MS560702BA03_H_ +#define SRC_DEVICES_SPI_DEVICES_MS560702BA03_H_ + +#include + +#include "Devices/SPIDevice.h" + +#include "defines.h" +#include "utils.h" + +// Address +#define MS5607_ADDRESS 0b1110111 << 1 // or 0b1110110 if CSB is low + +// Commands +#define MS5607_RESET 0x1E +#define MS5607_CONVERT_D1 0x40 // Pressure conversion +#define MS5607_CONVERT_D2 0x50 // Temperature conversion +#define MS5607_ADC_READ 0x00 +#define MS5607_PROM_READ 0xA0 // Base address for PROM read + + +/** + * @class MS560702BA03 + * @brief Driver for the MS560702BA03 Barometer. + * + * This class implements an SPI-based driver for the MS560702BA03 Barometer, + * It provides enumerations for the configuration options such output data rate (ODR), and operating mode. + * + * The driver communicates via SPI using the `SPIDevice` base class and + * implements a device-specific initialization routine (`deviceInit()`). + * + * Configuration options are based on the official STMicroelectronics datasheet. + * Default values are selected for typical performance-oriented operation. + * + * @see TE MS560702BA03 Datasheet, Rev B3, March 2024 + */ +class MS560702BA03: public SPIDevice { +public: + + /** + * @brief Construct a new MS560702BA03 device. + * + * Initializes the MS560702BA03 SPI driver interface and sets default + * configuration parameters for the Barometer. The SPI interface, CS port, + * and pin are passed to the base `SPIDevice` class. + * + * @param data Pointer to the main DataContainer used for shared system data. + * @param spi Pointer to the SPI handle (SPI_HandleTypeDef). + * @param port Pointer to the GPIO port controlling the chip select (CS) pin. + * @param pin GPIO pin number used for chip select (CS). + */ + MS560702BA03(DataContainer* data, SPI_HandleTypeDef *spi, GPIO_TypeDef *port, uint16_t pin); + + /** + * @brief Initialize the MS560702BA03 device. + * + * Configures the IMU by writing default settings to key control registers, + * verifying communication with the device, and preparing it for operation. + * + * @return int Status code (0 for success, negative for failure). + */ + int deviceInit() override; + + /** + * @brief Perform periodic updates for the MS560702BA03 IMU. + * + * This method should be called regularly (e.g., in the main control loop) + * to read sensor data from the accelerometer and gyroscope, process it + * as needed, and update internal state or shared DataContainer values. + * + * Typical tasks performed by this method include: + * - Reading ADC registers via SPI + * - Converting raw readings into physical units using sensitivity constants + * - Updating the main DataContainer with latest sensor values + * + * @return int Status code: + * - 0: Update successful + * - Negative value: Error reading data or SPI communication failure + * + * @note This function does not initialize the device; `deviceInit()` must + * be called first. + */ + int updateDevice() override; + + /** + * @brief Start the pressure measurement conversion. + * + * This method should be called before `updateDevice()` to start the pressure measurement. + * + * @note This function must be called prior to `updateDevice()` to ensure valid data is read. + */ + void startConversion(); + +private: + + uint16_t C[7]; // Calibration coefficients + + uint32_t D1, D2; + int32_t dT, TEMP, P; + int64_t OFF, SENS; + + /** + * @brief Read all factory calibration coefficients from the MS5607 PROM. + * + * This function reads the seven 16-bit calibration values (C0–C6) stored + * in the sensor’s PROM using the PROM READ command sequence. These values + * are required for computing temperature and pressure from raw ADC data. + * + * @note Each coefficient is read via SPI using two bytes. + * + * @return 0 If all coefficients were successfully read. + * @return -1 If any SPI transaction fails. + */ + int readProm(); + + /** + * @brief Trigger an ADC conversion and read the resulting 24-bit raw value. + * + * This function sends a conversion command to the MS5607, waits the + * appropriate conversion time, and then reads the 24-bit raw ADC output. + * + * @param cmd The ADC conversion command (e.g., D1/D2 with OSR setting). + * + * @return uint32_t The 24-bit raw ADC result assembled into a 32-bit integer. + */ + uint32_t readADC(uint8_t cmd); + + enum MS5607_OSR { + OSR_256 = 0b000, + OSR_512 = 0b001, + OSR_1024 = 0b010, + OSR_2048 = 0b011, + OSR_4096 = 0b100 + } osr = OSR_256; + + uint32_t conversionTime_us; + uint32_t conversionStart_us; + uint32_t delay; + uint32_t now_us; + + uint8_t buffer[3]; +}; + +#endif /* SRC_DEVICES_SPI_DEVICES_MS560702BA03_H_ */ diff --git a/Code/Core/Inc/Subsystems/Navigation.h b/Code/Core/Inc/Subsystems/Navigation.h new file mode 100644 index 0000000..5fe3fa6 --- /dev/null +++ b/Code/Core/Inc/Subsystems/Navigation.h @@ -0,0 +1,148 @@ +/* + * Navigation.h + * + * Created on: Oct 27, 2025 + * Author: colin + */ + +#ifndef SRC_SUBSYSTEMS_NAVIGATION_H_ +#define SRC_SUBSYSTEMS_NAVIGATION_H_ + +#include "Subsystem.h" +#include "main.h" + +#include "Devices/SPI_Devices/LSM6DSV320.h" +#include "Devices/SPI_Devices/MS560702BA03.h" +#include "Devices/GPS.h" + +#include "matrix.hpp" +#include "utils.h" +#include "defines.h" + +#define GYRO_THRESHOLD_DPS 0.1f + +/** + * @class Navigation + * @brief Handles flight navigation logic and sensor data fusion. + * + * The Navigation subsystem is responsible for processing and combining data + * from various sensors (e.g., IMU, barometer, magnetometer) to estimate + * the vehicle’s orientation, position, and velocity in real-time. + * + * This class inherits from the base `Subsystem` interface and implements + * the required `init()` and `update()` methods to manage its setup and + * periodic updates within the main control loop. + * + * Typical tasks include: + * - Initializing navigation sensors + * - Running sensor fusion algorithms (e.g., Kalman or complementary filter) + * - Updating the main DataContainer with current state estimates + */ +class Navigation: public Subsystem { +public: + /** + * @brief Navigation subsystem constructor. + * + * @param data Reference to the main DataContainer for shared data access. + * @param spiBus Reference to the SPI Bus that is connected to the sensors + * @param uart Refrences the UART that is connected to the GPS + */ + Navigation(DataContainer* data, SPI_HandleTypeDef* spiBus, UART_HandleTypeDef* uart, uint8_t* gpsRxBuffer); + + /** + * @brief Initializes the navigation subsystem. + * + * This method sets up required sensors, allocates necessary buffers, + * and prepares the system for navigation updates. + * + * @return 0 on success, or a negative error code on failure. + */ + int init() override; + + /** + * @brief Updates navigation data and state estimation. + * + * This function should be called periodically (e.g., at a fixed update rate) + * to process new sensor data, compute navigation estimates, and store + * the results in the shared DataContainer. + * + * @return 0 on success, or a negative error code if the update fails. + */ + int update() override; + +private: + // Sensors + LSM6DSV320 imu; // LSM6DSV320 IMU for measuring Low-G Accelerations, High-G Accelerations, and Angular Rates + MS560702BA03 baro; // MS560702BA03 Barometer for measuring Barometric Altitude, and Temperature + GPS gps; // GPR for Absolute Positioning: Latitude, Longitude, Altitude + + // Orientation Tracking + float quaternionWDot; + float quaternionXDot; + float quaternionYDot; + float quaternionZDot; + + float rollRate_rad; + float pitchRate_rad; + float yawRate_rad; + + float siny_cosp; + float cosy_cosp; + float sinp; + float sinr_cosp; + float cosr_cosp; + + void integrateQuaternion(); + void initializeQuaternion(); + + void rotateVectorByQuaternion(Matrix& vec); + + // Kalman Filter + Matrix lowG; + Matrix highG; + + Matrix x; // State vector + Matrix Z; // Measurements + Matrix F; // State transition matrix + Matrix H; // Observation matrix + Matrix Q; // Process noise covariance + Matrix R; // Measurement noise covariance + Matrix P; // Estimate error covariance + Matrix K; // Kalman gain + + Matrix S; + Matrix I; + + float processNoise = 0.01f; + + float lowGNoise = 0.01f; + float highGNoise = 0.01f; + float baroNoise = 0.01f; + + /** + * @brief Initializes the Kalman Filter + * + * This function setups up the necessary matrices and variables for the Kalman Filter. + */ + void initKalmanFilter(); + + void updateKalmanFilter(); + + void runKalmanFilter(); + + bool isKalmanFilterInit = false; + + // Timing + float lastLoop; // Time of start of previous loop in ms + float freq; // Frequency of the loop = 1/dt + float now; // Current time in ms + float dt_ms; // Time since last loop in ms + float dt_s; // Time since last loop in s + float dt2, dt3, dt4; // dt^2, dt^3, dt^4 + + float dt_us; + float freq_us; + float last_us; +}; + +#endif /* SRC_SUBSYSTEMS_NAVIGATION_H_ */ diff --git a/Code/Core/Inc/defines.h b/Code/Core/Inc/defines.h new file mode 100644 index 0000000..b14adeb --- /dev/null +++ b/Code/Core/Inc/defines.h @@ -0,0 +1,27 @@ +/* + * defines.h + * + * Created on: Oct 29, 2025 + * Author: colin + */ + +#ifndef INC_DEFINES_H_ +#define INC_DEFINES_H_ + +#define GRAVITY 9.81f + +// Unit Conversions + +#define DEG_TO_RAD M_PI / 180.0f +#define RAD_TO_DEG 180.0f / M_PI + +#define FEET_TO_METER 0.3048f +#define METER_TO_FEET 3.28084 + +#define NAVIGATION_TARGET_FREQ 500 // hz +#define NAVIGATION_TARGET_DT 1/NAVIGATION_TARGET_FREQ + +#define KALMAN_FILTER_NUM_OF_STATES 9 // Position (X, Y, Z), Velocity (X, Y, Z), Acceleration (X, Y, Z) +#define KALMAN_FILTER_NUM_OF_MEASUREMENTS 7 // Low G Acceleration (X, Y, Z), High G Acceleration (X, Y, Z), Barometric Altitude + +#endif /* INC_DEFINES_H_ */ diff --git a/Code/Core/Inc/matrix.hpp b/Code/Core/Inc/matrix.hpp new file mode 100644 index 0000000..46465af --- /dev/null +++ b/Code/Core/Inc/matrix.hpp @@ -0,0 +1,197 @@ +#pragma once + +#include +#include +#include + +template +class Matrix { +private: + T data[ROWS][COLS]; + +public: + // Default constructor (zero initialization) + Matrix() { + for (int i = 0; i < ROWS; ++i) + for (int j = 0; j < COLS; ++j) + data[i][j] = T(0); + } + + Matrix(std::initializer_list> init) { + int i = 0; + for (auto row : init) { + if (i >= ROWS) + break; + int j = 0; + for (auto val : row) { + if (j >= COLS) + break; + data[i][j++] = val; + } + ++i; + } + // Fill remaining elements with zero + for (; i < ROWS; ++i) + for (int j = 0; j < COLS; ++j) + data[i][j] = T(0); + } + + // Flat initializer list constructor (row-major order) + Matrix(std::initializer_list init) { + int count = 0; + for (T val : init) { + if (count >= ROWS * COLS) + break; + data[count / COLS][count % COLS] = val; + ++count; + } + // Fill any remaining elements with 0 + for (; count < ROWS * COLS; ++count) { + data[count / COLS][count % COLS] = T(0); + } + } + + // Access operators + T& operator()(int row, int col) { + // if (row < 0 || row >= ROWS || col < 0 || col >= COLS) + // throw std::out_of_range("Matrix index out of bounds"); + return data[row][col]; + } + + const T& operator()(int row, int col) const { + // if (row < 0 || row >= ROWS || col < 0 || col >= COLS) + // throw std::out_of_range("Matrix index out of bounds"); + return data[row][col]; + } + + T& operator()(int index) { + // static_assert(ROWS == 1 || COLS == 1, "1D indexing only allowed for vectors"); + // if (index < 0 || index >= ROWS * COLS) + // throw std::out_of_range("Index out of bounds"); + return (ROWS == 1) ? data[0][index] : data[index][0]; + } + + const T& operator()(int index) const { + static_assert(ROWS == 1 || COLS == 1, "1D indexing only allowed for vectors"); + if (index < 0 || index >= ROWS * COLS) + throw std::out_of_range("Index out of bounds"); + return (ROWS == 1) ? data[0][index] : data[index][0]; + } + + + // Addition + Matrix operator+(const Matrix& rhs) const { + Matrix result; + for (int i = 0; i < ROWS; ++i) + for (int j = 0; j < COLS; ++j) + result(i, j) = data[i][j] + rhs(i, j); + return result; + } + + // Subtraction + Matrix operator-(const Matrix& rhs) const { + Matrix result; + for (int i = 0; i < ROWS; ++i) + for (int j = 0; j < COLS; ++j) + result(i, j) = data[i][j] - rhs(i, j); + return result; + } + + // Multiplication + template + Matrix operator*(const Matrix& rhs) const { + Matrix result; + for (int i = 0; i < ROWS; ++i) + for (int j = 0; j < OTHER_COLS; ++j) + for (int k = 0; k < COLS; ++k) + result(i, j) += data[i][k] * rhs(k, j); + return result; + } + + Matrix& operator*=(T scalar) { + for (int i = 0; i < ROWS; ++i) + for (int j = 0; j < COLS; ++j) + data[i][j] *= scalar; + return *this; + } + + // Transpose + Matrix transpose() const { + Matrix result; + for (int i = 0; i < ROWS; ++i) + for (int j = 0; j < COLS; ++j) + result(j, i) = data[i][j]; + return result; + } + + // Inverse (only for square matrices) + Matrix inverse() const { + static_assert(ROWS == COLS, "Inverse only defined for square matrices"); + const int N = ROWS; + Matrix aug; + + // Copy the input matrix and append identity + for (int i = 0; i < N; ++i) + for (int j = 0; j < N; ++j) + aug(i, j) = data[i][j]; + + Matrix inv; + for (int i = 0; i < N; ++i) + inv(i, i) = T(1); + + // Gauss-Jordan elimination + for (int i = 0; i < N; ++i) { + // Pivot + T pivot = aug(i, i); + // if (std::abs(pivot) < 1e-10) + // throw std::runtime_error("Matrix is singular and cannot be inverted"); + + for (int j = 0; j < N; ++j) { + aug(i, j) /= pivot; + inv(i, j) /= pivot; + } + + for (int k = 0; k < N; ++k) { + if (k == i) continue; + T factor = aug(k, i); + for (int j = 0; j < N; ++j) { + aug(k, j) -= factor * aug(i, j); + inv(k, j) -= factor * inv(i, j); + } + } + } + + return inv; + } + + Matrix identity() const { + static_assert(ROWS == COLS, "Identity matrix must be square"); + Matrix result; + for (int i = 0; i < ROWS; ++i) + for (int j = 0; j < COLS; ++j) + result(i, j) = (i == j) ? T(1) : T(0); + return result; + } + + void setIdentity() { + static_assert(ROWS == COLS, "Identity matrix must be square"); + for (int i = 0; i < ROWS; ++i) + for (int j = 0; j < COLS; ++j) + data[i][j] = (i == j) ? T(1) : T(0); + } + + void setZero() { + for (int i = 0; i < ROWS; ++i) + for (int j = 0; j < COLS; ++j) + data[i][j] = T(0); + } + + // Print utility + void print() const { + for (int i = 0; i < ROWS; ++i) { + for (int j = 0; j < COLS; ++j) + std::cout << std::setw(8) << data[i][j] << " "; + std::cout << "\n"; + } + } +}; \ No newline at end of file diff --git a/Code/Core/Inc/stm32f4xx_it.h b/Code/Core/Inc/stm32f4xx_it.h index 4da0d77..af45aca 100644 --- a/Code/Core/Inc/stm32f4xx_it.h +++ b/Code/Core/Inc/stm32f4xx_it.h @@ -55,6 +55,7 @@ void SVC_Handler(void); void DebugMon_Handler(void); void PendSV_Handler(void); void SysTick_Handler(void); +void DMA1_Stream2_IRQHandler(void); void OTG_FS_IRQHandler(void); /* USER CODE BEGIN EFP */ diff --git a/Code/Core/Inc/usbHelper.h b/Code/Core/Inc/usbHelper.h index ce548de..498ad59 100644 --- a/Code/Core/Inc/usbHelper.h +++ b/Code/Core/Inc/usbHelper.h @@ -5,15 +5,12 @@ #include "tusb.h" -#define MAX_TICK_MSG_LEN 40 -static char usb_tx_buffer[MAX_TICK_MSG_LEN]; - /** * @brief Sends a string over the TinyUSB CDC interface. * * @param str Pointer to the character array (string) to send. * @param len Length of the string (in bytes). * @retval The number of bytes successfully written to the USB buffer. */ -size_t cdcSendMessage(const char* str, size_t len); +size_t cdcSendMessage(char* str, size_t len); #endif /* INC_USBHELPER_H_ */ \ No newline at end of file diff --git a/Code/Core/Inc/utils.h b/Code/Core/Inc/utils.h new file mode 100644 index 0000000..143d449 --- /dev/null +++ b/Code/Core/Inc/utils.h @@ -0,0 +1,25 @@ +/* + * utils.h + * + * Created on: Dec 1, 2025 + * Author: colin + */ + +#ifndef INC_UTILS_H_ +#define INC_UTILS_H_ + +#include "stm32f4xx.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void DWT_Init(void); +uint32_t micros(void); +void delay_us(uint32_t us); + +#ifdef __cplusplus +} +#endif + +#endif /* INC_UTILS_H_ */ diff --git a/Code/Core/Src/Devices/GPS.cpp b/Code/Core/Src/Devices/GPS.cpp new file mode 100644 index 0000000..8ee424c --- /dev/null +++ b/Code/Core/Src/Devices/GPS.cpp @@ -0,0 +1,329 @@ +/* + * GPS.cpp + * + * Created on: Jan 08, 2026 + * Author: colin + */ + +#include "Devices/GPS.h" +#include + +GPS::GPS(DataContainer* data, UART_HandleTypeDef* uart, uint8_t* dmaBuffer) +{ + this->uart = uart; + this->dmaBuffer = dmaBuffer; + + this->currentBufferPos = 0; + this->lastDmaBufferPos = 0; + + this->ubxIndex = 0; + this->parseState = WAIT_SYNC1; + this->payloadIndex = 0; + + // Initialize GPS data in DataContainer + this->data = data; + data->GPSLatitude = 0.0f; + data->GPSLongitude = 0.0f; + data->GPSAltitude_m = 0.0f; + data->GPSFix = 0; + data->GPSNumSatellites = 0; + memset(data->GPSUTCTime, 0, sizeof(data->GPSUTCTime)); +} + +int GPS::deviceInit() +{ + // Wait for GPS to boot up + HAL_Delay(500); + + // Configure GPS for UBX protocol + configureGPS(); + + return 0; +} + +void GPS::calculateChecksum(const uint8_t* data, uint16_t len, uint8_t* ckA, uint8_t* ckB) +{ + *ckA = 0; + *ckB = 0; + + for (uint16_t i = 0; i < len; i++) + { + *ckA += data[i]; + *ckB += *ckA; + } +} + +void GPS::sendUBXCommand(const uint8_t* cmd, uint16_t len) +{ + HAL_UART_Transmit(uart, (uint8_t*)cmd, len, 100); +} + +void GPS::configureGPS() +{ + // Change baud rate from 9600 to 115200 + uint8_t cfgPrtBaud[] = { + 0xB5, 0x62, // Sync chars + 0x06, 0x00, // CFG-PRT + 0x14, 0x00, // Length = 20 + 0x01, // PortID = UART1 + 0x00, // Reserved + 0x00, 0x00, // txReady (not used) + 0xD0, 0x08, 0x00, 0x00, // Mode: 8N1 + 0x00, 0xC2, 0x01, 0x00, // Baud: 115200 (0x0001C200) + 0x07, 0x00, // inProtoMask: UBX + NMEA + RTCM + 0x01, 0x00, // outProtoMask: UBX only + 0x00, 0x00, // flags + 0x00, 0x00, // Reserved + 0x00, 0x00 // Checksum placeholder + }; + + // Calculate checksum for CFG-PRT (skip sync chars) + calculateChecksum(&cfgPrtBaud[2], 24, &cfgPrtBaud[26], &cfgPrtBaud[27]); + sendUBXCommand(cfgPrtBaud, sizeof(cfgPrtBaud)); + + HAL_Delay(100); + + // Now reconfigure UART on MCU side to 115200 + uart->Init.BaudRate = 115200; + if (HAL_UART_Init(uart) != HAL_OK) + { + Error_Handler(); + } + + HAL_Delay(100); + + // Disable NMEA 0183 output (UBX only) + uint8_t cfgPrt[] = { + 0xB5, 0x62, // Sync + 0x06, 0x00, // CFG-PRT + 0x14, 0x00, // Length = 20 + 0x01, // PortID = UART1 + 0x00, // Reserved + 0x00, 0x00, // txReady + 0xD0, 0x08, 0x00, 0x00, // Mode: 8N1 + 0x00, 0xC2, 0x01, 0x00, // Baud: 115200 + 0x01, 0x00, // inProtoMask: UBX only + 0x01, 0x00, // outProtoMask: UBX only + 0x00, 0x00, // flags + 0x00, 0x00, // Reserved + 0x00, 0x00 // Checksum + }; + calculateChecksum(&cfgPrt[2], 24, &cfgPrt[26], &cfgPrt[27]); + sendUBXCommand(cfgPrt, sizeof(cfgPrt)); + + HAL_Delay(100); + + // Set navigation rate to 10 Hz + uint8_t cfgRate[] = { + 0xB5, 0x62, // Sync + 0x06, 0x08, // CFG-RATE + 0x06, 0x00, // Length = 6 + 0x64, 0x00, // measRate = 100 ms (10 Hz) + 0x01, 0x00, // navRate = 1 + 0x01, 0x00, // timeRef = UTC + 0x00, 0x00 // Checksum + }; + calculateChecksum(&cfgRate[2], 10, &cfgRate[12], &cfgRate[13]); + sendUBXCommand(cfgRate, sizeof(cfgRate)); + + HAL_Delay(100); + + // Enable UBX-NAV-PVT message + uint8_t cfgMsg[] = { + 0xB5, 0x62, // Sync + 0x06, 0x01, // CFG-MSG + 0x03, 0x00, // Length = 3 + 0x01, 0x07, // NAV-PVT (Class 0x01, ID 0x07) + 0x01, // Rate: 1 (every solution) + 0x00, 0x00 // Checksum + }; + calculateChecksum(&cfgMsg[2], 7, &cfgMsg[9], &cfgMsg[10]); + sendUBXCommand(cfgMsg, sizeof(cfgMsg)); + + HAL_Delay(100); + + // Save configuration to flash (optional but recommended) + uint8_t cfgSave[] = { + 0xB5, 0x62, // Sync + 0x06, 0x09, // CFG-CFG (save configuration) + 0x0D, 0x00, // Length = 13 + 0x00, 0x00, 0x00, 0x00, // Clear mask + 0xFF, 0xFF, 0x00, 0x00, // Save mask (all) + 0x00, 0x00, 0x00, 0x00, // Load mask + 0x17, // Device mask: BBR, Flash, EEPROM, SPI Flash + 0x00, 0x00 // Checksum + }; + calculateChecksum(&cfgSave[2], 17, &cfgSave[19], &cfgSave[20]); + sendUBXCommand(cfgSave, sizeof(cfgSave)); + + HAL_Delay(500); // Wait for save to complete +} + +int GPS::updateDevice() +{ + processDMAData(); + return 0; +} + +void GPS::processDMAData() +{ + // Get current DMA write position + currentBufferPos = GPS_DMA_BUFFER_SIZE - __HAL_DMA_GET_COUNTER(uart->hdmarx); + + // Process new data + if (currentBufferPos != lastDmaBufferPos) + { + if (currentBufferPos > lastDmaBufferPos) + { + // Normal case: no wrap-around + for (uint16_t i = lastDmaBufferPos; i < currentBufferPos; i++) + { + if (parseUBXByte(dmaBuffer[i])) + { + processUBXMessage(); + } + } + } + else + { + // Wrap-around case + for (uint16_t i = lastDmaBufferPos; i < GPS_DMA_BUFFER_SIZE; i++) + { + if (parseUBXByte(dmaBuffer[i])) + { + processUBXMessage(); + } + } + for (uint16_t i = 0; i < currentBufferPos; i++) + { + if (parseUBXByte(dmaBuffer[i])) + { + processUBXMessage(); + } + } + } + + lastDmaBufferPos = currentBufferPos; + } +} + +bool GPS::parseUBXByte(uint8_t byte) +{ + switch (parseState) + { + case WAIT_SYNC1: + if (byte == 0xB5) + { + parseState = WAIT_SYNC2; + ubxIndex = 0; + } + break; + + case WAIT_SYNC2: + if (byte == 0x62) + { + parseState = GET_CLASS; + ckA = 0; + ckB = 0; + } + else + { + parseState = WAIT_SYNC1; + } + break; + + case GET_CLASS: + msgClass = byte; + ubxBuffer[ubxIndex++] = byte; + ckA += byte; + ckB += ckA; + parseState = GET_ID; + break; + + case GET_ID: + msgId = byte; + ubxBuffer[ubxIndex++] = byte; + ckA += byte; + ckB += ckA; + parseState = GET_LENGTH1; + break; + + case GET_LENGTH1: + payloadLength = byte; + ubxBuffer[ubxIndex++] = byte; + ckA += byte; + ckB += ckA; + parseState = GET_LENGTH2; + break; + + case GET_LENGTH2: + payloadLength |= (byte << 8); + ubxBuffer[ubxIndex++] = byte; + ckA += byte; + ckB += ckA; + payloadIndex = 0; + + if (payloadLength > 0 && payloadLength < GPS_UBX_MAX_PAYLOAD) + parseState = GET_PAYLOAD; + else if (payloadLength == 0) + parseState = GET_CHECKSUM1; + else + parseState = WAIT_SYNC1; // Payload too large + break; + + case GET_PAYLOAD: + ubxBuffer[ubxIndex++] = byte; + ckA += byte; + ckB += ckA; + payloadIndex++; + + if (payloadIndex >= payloadLength) + parseState = GET_CHECKSUM1; + break; + + case GET_CHECKSUM1: + if (byte == ckA) + { + parseState = GET_CHECKSUM2; + } + else + { + parseState = WAIT_SYNC1; // Checksum A error + } + break; + + case GET_CHECKSUM2: + parseState = WAIT_SYNC1; + if (byte == ckB) + { + return true; // Valid message received! + } + // Checksum B error + break; + } + + return false; +} + +void GPS::processUBXMessage() +{ + // Check if this is NAV-PVT (Class 0x01, ID 0x07) + if (msgClass == 0x01 && msgId == 0x07 && payloadLength == 92) + { + // Cast payload to UBX_NAV_PVT structure (skip class, id, length bytes) + UBX_NAV_PVT* pvt = (UBX_NAV_PVT*)&ubxBuffer[4]; + + // Update DataContainer with GPS data + data->GPSLatitude = pvt->lat * 1e-7f; + data->GPSLongitude = pvt->lon * 1e-7f; + data->GPSAltitude_m = pvt->hMSL * 0.001f; // mm to meters + data->GPSFix = pvt->fixType; + data->GPSNumSatellites = pvt->numSV; + + // Format UTC time as "HHMMSS.sss" + snprintf(data->GPSUTCTime, sizeof(data->GPSUTCTime), + "%02d%02d%02d.%03d", + pvt->hour, pvt->min, pvt->sec, + (int)(pvt->nano / 1000000)); + } +} \ No newline at end of file diff --git a/Code/Core/Src/Devices/I2CDevice.cpp b/Code/Core/Src/Devices/I2CDevice.cpp new file mode 100644 index 0000000..c34684c --- /dev/null +++ b/Code/Core/Src/Devices/I2CDevice.cpp @@ -0,0 +1,23 @@ +/* + * I2CDevice.cpp + * + * Created on: Oct 29, 2025 + * Author: colin + */ + +#include "Devices/I2CDevice.h" + +I2CDevice::I2CDevice(DataContainer* data) +{ + this->data = data; +} + +HAL_StatusTypeDef I2CDevice::readI2C(uint16_t reg, uint8_t *data, uint16_t len) +{ + return HAL_I2C_Mem_Read(this->i2cHandler, this->address, reg, I2C_MEMADD_SIZE_8BIT, data, len, HAL_MAX_DELAY); +} + +HAL_StatusTypeDef I2CDevice::writeI2C(uint8_t reg, uint8_t *data, uint8_t len) +{ + return HAL_I2C_Mem_Write(this->i2cHandler, this->address, reg, I2C_MEMADD_SIZE_8BIT, data, len, HAL_MAX_DELAY); +} diff --git a/Code/Core/Src/Devices/SPIDevice.cpp b/Code/Core/Src/Devices/SPIDevice.cpp new file mode 100644 index 0000000..5bcd2b7 --- /dev/null +++ b/Code/Core/Src/Devices/SPIDevice.cpp @@ -0,0 +1,55 @@ +/* + * I2CDevice.cpp + * + * Created on: Oct 29, 2025 + * Author: colin + */ + +#include "Devices/SPIDevice.h" + +SPIDevice::SPIDevice(DataContainer* data, SPI_HandleTypeDef *spi, GPIO_TypeDef *port, uint16_t pin) +{ + this->data = data; + this->spiHandler = spi; + this->chipSelectPort = port; + this->chipSelectPin = pin; +} + +HAL_StatusTypeDef SPIDevice::readSPI(uint8_t reg, uint8_t *data, uint8_t len) +{ + uint8_t tx[len+1]; + uint8_t rx[len+1]; + + tx[0] = reg; + for (int i=1; i < len+1; i++) tx[i] = 0x00; + for (int i=0; i < len+1; i++) rx[i] = 0x00; + + // Pull the SPI Chip Select pin low to select the correct device + HAL_GPIO_WritePin(this->chipSelectPort, this->chipSelectPin, GPIO_PIN_RESET); + + status = HAL_SPI_TransmitReceive(this->spiHandler, tx, rx, len+1, 10); + + // Pull the SPI Chip Select pin high to deselect the device + HAL_GPIO_WritePin(this->chipSelectPort, this->chipSelectPin, GPIO_PIN_SET); + + memcpy(data, rx + 1, len); + + return status; +} + +HAL_StatusTypeDef SPIDevice::writeSPI(uint8_t reg, uint8_t *data, uint8_t len) +{ + uint8_t buffer[len + 1]; + buffer[0] = reg; + memcpy(buffer + 1, data, len); + + // Pull the SPI Chip Select pin low to select the correct device + HAL_GPIO_WritePin(this->chipSelectPort, this->chipSelectPin, GPIO_PIN_RESET); + + status = HAL_SPI_Transmit(this->spiHandler, buffer, len+1, 10); + + // Pull the SPI Chip Select pin high to deselect the device + HAL_GPIO_WritePin(this->chipSelectPort, this->chipSelectPin, GPIO_PIN_SET); + + return status; +} diff --git a/Code/Core/Src/Devices/SPI_Devices/LSM6DSV320.cpp b/Code/Core/Src/Devices/SPI_Devices/LSM6DSV320.cpp new file mode 100644 index 0000000..aea7e57 --- /dev/null +++ b/Code/Core/Src/Devices/SPI_Devices/LSM6DSV320.cpp @@ -0,0 +1,302 @@ +/* + * LSM6DSV320.cpp + * + * Created on: Oct 29, 2025 + * Author: colin + */ + +#include "Devices/SPI_Devices/LSM6DSV320.h" + +LSM6DSV320::LSM6DSV320(DataContainer* data, SPI_HandleTypeDef *spi, GPIO_TypeDef *port, uint16_t pin) + : SPIDevice(data, spi, port, pin) +{ + data->LSM6DSV320GyroX_dps = 0.0f; + data->LSM6DSV320GyroY_dps = 0.0f; + data->LSM6DSV320GyroZ_dps = 0.0f; + + data->LSM6DSV320LowGAccelX_mps2 = 0.0f; + data->LSM6DSV320LowGAccelY_mps2 = 0.0f; + data->LSM6DSV320LowGAccelZ_mps2 = 0.0f; + + data->LSM6DSV320HighGAccelX_mps2 = 0.0f; + data->LSM6DSV320HighGAccelY_mps2 = 0.0f; + data->LSM6DSV320HighGAccelZ_mps2 = 0.0f; + + gyroXBias = 0.0f; + gyroYBias = 0.0f; + gyroZBias = 0.0f; +} + +int LSM6DSV320::deviceInit() +{ + uint8_t whoAmI; + + readSPI(LSM6DSV320_WHO_AM_I | 0x80, &whoAmI, 1); + if (whoAmI != LSM6DSV320_ID) { +// return -1; + } + + /* Sensor Configuration */ + uint8_t config = 0; + uint8_t out[2] = {0,0}; + + // IF_CFG page 58, table 32 + config = 0; + config |= 0 << 7; // SDA_PU_EN disconnects the SDA pull-up (default) + config |= 0 << 6; // SHUB_PU_EN Disables I2C plus pull-ups (default) + config |= 0 << 5; // ASF_CTRL turns off anti-spike filter (default) + config |= 0 << 4; // H_LACTIVE sets interrupts to active high (default) + config |= 0 << 3; // PP_OD sets interrupts to push-pull (default) + config |= 0 << 2; // SIM sets the SPI interface more to 4 wire (default) + config |= 0 << 1; // Reserved + config |= 0 << 0; // I2C_I3C_DISABLE disables I2C and I3C interface (default) + writeSPI(LSM6DSV320_IF_CFG, &config); + out[0] = 0; out[1] = 0; + readSPI(LSM6DSV320_IF_CFG | 0x80, out); + + HAL_Delay(1); + + // CTRL1 page 65, table 52 + config = 0; + config |= this->lowGOPMode << 4; // OP_MODE_XL + config |= this->lowGODR << 0; // ODR_XL + writeSPI(LSM6DSV320_CTRL1, &config); + out[0] = 0; out[1] = 0; + readSPI(LSM6DSV320_CTRL1 | 0x80, out); + + HAL_Delay(1); + + // CTRL2 page 66, table 55 + config = 0; + config |= this->gyroOPMode << 4; // OP_MODE_G + config |= this->gyroODR << 0; // ODR_G + writeSPI(LSM6DSV320_CTRL2, &config); + out[0] = 0; out[1] = 0; + readSPI(LSM6DSV320_CTRL2 | 0x80, out); + + HAL_Delay(1); + + // CTRL3 page 67, table 58 + config = 0; + config |= 0b0 << 7; // BOOT set to normal mode + config |= 0b1 << 6; // BDU set to + config |= 0b1 << 2; // IF_INC set to + config |= 0b0 << 0; // SW_RESET set to normal mode + writeSPI(LSM6DSV320_CTRL3, &config); + out[0] = 0; out[1] = 0; + readSPI(LSM6DSV320_CTRL3 | 0x80, out); + + HAL_Delay(1); + + // CTRL4 page 68, table 60 + config = 0; + config |= 0b0 << 4; // INT2_on_INT1 + config |= 0b0 << 3; // DRDY_MASK set to disabled + config |= 0b0 << 2; // INT2_DRDY_TEMP set to disabled + config |= 0b0 << 1; // DRDY_PULSED set data-ready latched mode + config |= 0b0 << 0; // INT2_IN_LH set embedded functions active low + writeSPI(LSM6DSV320_CTRL4, &config); + out[0] = 0; out[1] = 0; + readSPI(LSM6DSV320_CTRL4 | 0x80, out); + + HAL_Delay(1); + + // CTRL5 page 69, table 62 + config = 0; + config |= 0b0 << 7; // IF2_TA0_PID + config |= 0b00 << 2; // BUS_ACT_SEC sets the bus available time selection for IBT to 50 us (default) + config |= 0b0 << 0; // INT_EN_SPI disables INT pin when I3C is enabled (default) + writeSPI(LSM6DSV320_CTRL5, &config); + out[0] = 0; out[1] = 0; + readSPI(LSM6DSV320_CTRL5 | 0x80, out); + + HAL_Delay(1); + + // CTRL6 page 69, table 64 + config = 0; + config |= 0b110 << 4; // LPF1_G_BW sets the low-pass filter for gyroscope bandwidth selection + config |= this->gyroRange << 0; // FS_G + writeSPI(LSM6DSV320_CTRL6, &config); + out[0] = 0; out[1] = 0; + readSPI(LSM6DSV320_CTRL6 | 0x80, out); + + HAL_Delay(1); + + // CTRL7 page 70, table 67 + config = 0; + config |= 0b0 << 7; // INT1_DRDY_XL disabled high-g data-ready interrupt on INT1 pin (default) + config |= 0b0 << 6; // INT2_DRDY_XL disables high-g data-ready interrupt on INT2 pin (default) + config |= 0b1 << 0; // LPF1_G_EN enables low-pass filter for gyroscope (default) + writeSPI(LSM6DSV320_CTRL7, &config); + out[0] = 0; out[1] = 0; + readSPI(LSM6DSV320_CTRL7 | 0x80, out); + + HAL_Delay(1); + + // CTRL8 page 71, table 69 + config = 0; + config |= 0b000 << 5; // HP_LPF2_XL_BW set to ODR / 4 + config |= this->lowGRange << 0; // FS_XL + writeSPI(LSM6DSV320_CTRL8, &config); + out[0] = 0; out[1] = 0; + readSPI(LSM6DSV320_CTRL8 | 0x80, out); + + HAL_Delay(1); + + // CTRL9 page 72, table 72 + config = 0; + config |= 0b0 << 6; // HP_REF_MODE_XL disables the high-pass filter reference mode (default) + config |= 0b0 << 5; // XL_FASTSETTL_MODE disables the fast-settling mode (default) + config |= 0b0 << 4; // HP_SLOPE_XL_EN selects low-pass filter slope (default) + config |= 0b1 << 3; // LPF2_XL_EN + config |= 0b0 << 1; // USR_OFF_W sets user offsets to be 2^-10 g/LSB (default) + config |= 0b0 << 0; // USR_OFF_ON_OUT bypasses user offset correction block (default) + writeSPI(LSM6DSV320_CTRL9, &config); + out[0] = 0; out[1] = 0; + readSPI(LSM6DSV320_CTRL9 | 0x80, out); + + HAL_Delay(1); + + // CTRL10 page 74, table 74 + config = 0; + config |= 0b0 << 6; // EMB_FUNC_DEBUG disables the embedded functions + config |= 0b00 << 2; // ST_G sets the normal self-test mode (default) + config |= 0b00 << 0; // ST_XL sets the normal self-test mode (default) + writeSPI(LSM6DSV320_CTRL10, &config); + out[0] = 0; out[1] = 0; + readSPI(LSM6DSV320_CTRL10 | 0x80, out); + + // CTRL1_XL_HG page 91, table 147 + config = 0; + config |= 0b1 << 7; // XL_HG_REGOUT_EN enable high-g outputs + config |= 0b0 << 6; // HG_USR_OUFF_OU_OUT disabled high-g offsets + config |= this->highGODR << 3; // ODR_XL_HG + config |= this->highGRange << 0; // FS_XL_HG + writeSPI(LSM6DSV320_CTRL1_XL_HG, &config); + out[0] = 0; out[1] = 0; + readSPI(LSM6DSV320_CTRL1_XL_HG | 0x80, out); + + HAL_Delay(1); + + // CTRL2_XL_HG page 91, table 145 + config = 0; + config |= 0b0 << 4; // HG_USR_OFF_ON_WU doesn't drive the offset values into high-g wake-up and shock + config |= 0b00 << 0; // XL_HG_ST sets the normal self-test mode (default) + writeSPI(LSM6DSV320_CTRL2_XL_HG, &config); + out[0] = 0; out[1] = 0; + readSPI(LSM6DSV320_CTRL2_XL_HG | 0x80, out); + + HAL_Delay(1); + + /* Configure Sensitivities */ + + // Set the according Low G Sensitivity + switch (lowGRange) { + case LOW_G_RANGE_2G: this->lowGSensitivity = LSM6DSV320_LOW_G_SENSITIVITY_2G * GRAVITY; break; + case LOW_G_RANGE_4G: this->lowGSensitivity = LSM6DSV320_LOW_G_SENSITIVITY_4G * GRAVITY; break; + case LOW_G_RANGE_8G: this->lowGSensitivity = LSM6DSV320_LOW_G_SENSITIVITY_8G * GRAVITY; break; + case LOW_G_RANGE_16G: this->lowGSensitivity = LSM6DSV320_LOW_G_SENSITIVITY_16G * GRAVITY; break; + default: return false; + } + + // Set the according High G Sensitivity + switch (highGRange) { + case HIGH_G_RANGE_32G: this->highGSensitivity = LSM6DSV320_HIGH_G_SENSITIVITY_32G * GRAVITY; break; + case HIGH_G_RANGE_64G: this->highGSensitivity = LSM6DSV320_HIGH_G_SENSITIVITY_64G * GRAVITY; break; + case HIGH_G_RANGE_128G: this->highGSensitivity = LSM6DSV320_HIGH_G_SENSITIVITY_128G * GRAVITY; break; + case HIGH_G_RANGE_256G: this->highGSensitivity = LSM6DSV320_HIGH_G_SENSITIVITY_256G * GRAVITY; break; + case HIGH_G_RANGE_320G: this->highGSensitivity = LSM6DSV320_HIGH_G_SENSITIVITY_320G * GRAVITY; break; + default: return false; + } + + // Set the according Gyroscope Sensitivity + switch (gyroRange) { + case GYRO_RANGE_250DPS: this->gyroSensitivity = LSM6DSV320_GYRO_SENSITIVITY_250DPS; break; + case GYRO_RANGE_500DPS: this->gyroSensitivity = LSM6DSV320_GYRO_SENSITIVITY_500DPS; break; + case GYRO_RANGE_1000DPS: this->gyroSensitivity = LSM6DSV320_GYRO_SENSITIVITY_1000DPS; break; + case GYRO_RANGE_2000DPS: this->gyroSensitivity = LSM6DSV320_GYRO_SENSITIVITY_2000DPS; break; + case GYRO_RANGE_4000DPS: this->gyroSensitivity = LSM6DSV320_GYRO_SENSITIVITY_4000DPS; break; + default: return false; + } + + + /* Gyroscope Calibration */ + float sumOfGyroX = 0; + float sumOfGyroY = 0; + float sumOfGyroZ = 0; + // Do a basic calibration of the gyro by reading 100 samples and averaging them + for (int i = 0; i < numOfSamples; i++) { + // Read Gyroscope Measurements + readSPI(LSM6DSV320_OUTX_L_G | 0x80, &this->buffer[0]); + readSPI(LSM6DSV320_OUTX_H_G | 0x80, &this->buffer[1]); + readSPI(LSM6DSV320_OUTY_L_G | 0x80, &this->buffer[2]); + readSPI(LSM6DSV320_OUTY_H_G | 0x80, &this->buffer[3]); + readSPI(LSM6DSV320_OUTZ_L_G | 0x80, &this->buffer[4]); + readSPI(LSM6DSV320_OUTZ_H_G | 0x80, &this->buffer[5]); + + // Cast measurements to floats + this->data->LSM6DSV320GyroX_dps = ((float) (int16_t) (this->buffer[0] | this->buffer[1] << 8)) * this->gyroSensitivity; + this->data->LSM6DSV320GyroY_dps = ((float) (int16_t) (this->buffer[2] | this->buffer[3] << 8)) * this->gyroSensitivity; + this->data->LSM6DSV320GyroZ_dps = ((float) (int16_t) (this->buffer[4] | this->buffer[5] << 8)) * this->gyroSensitivity; + + sumOfGyroX += this->data->LSM6DSV320GyroX_dps; + sumOfGyroY += this->data->LSM6DSV320GyroY_dps; + sumOfGyroZ += this->data->LSM6DSV320GyroZ_dps; + + HAL_Delay(1); + } + + // Set the bias + this->gyroXBias = sumOfGyroX / numOfSamples; + this->gyroYBias = sumOfGyroY / numOfSamples; + this->gyroZBias = sumOfGyroZ / numOfSamples; + + // Read 1 sample of each accelerometer + updateDevice(); + + return 0; +} + +int LSM6DSV320::updateDevice() +{ + // Read Gyroscope Measurements + readSPI(LSM6DSV320_OUTX_L_G | 0x80, &this->buffer[0]); + readSPI(LSM6DSV320_OUTX_H_G | 0x80, &this->buffer[1]); + readSPI(LSM6DSV320_OUTY_L_G | 0x80, &this->buffer[2]); + readSPI(LSM6DSV320_OUTY_H_G | 0x80, &this->buffer[3]); + readSPI(LSM6DSV320_OUTZ_L_G | 0x80, &this->buffer[4]); + readSPI(LSM6DSV320_OUTZ_H_G | 0x80, &this->buffer[5]); + + // Cast measurements to floats + this->data->LSM6DSV320GyroX_dps = (((float) (int16_t) (this->buffer[0] | this->buffer[1] << 8)) * this->gyroSensitivity) - gyroXBias; + this->data->LSM6DSV320GyroY_dps = (((float) (int16_t) (this->buffer[2] | this->buffer[3] << 8)) * this->gyroSensitivity) - gyroYBias; + this->data->LSM6DSV320GyroZ_dps = (((float) (int16_t) (this->buffer[4] | this->buffer[5] << 8)) * this->gyroSensitivity) - gyroZBias; + + // Read Low G Accelerometer Measurements + readSPI(LSM6DSV320_OUTX_L_A | 0x80, &this->buffer[0]); + readSPI(LSM6DSV320_OUTX_H_A | 0x80, &this->buffer[1]); + readSPI(LSM6DSV320_OUTY_L_A | 0x80, &this->buffer[2]); + readSPI(LSM6DSV320_OUTY_H_A | 0x80, &this->buffer[3]); + readSPI(LSM6DSV320_OUTZ_L_A | 0x80, &this->buffer[4]); + readSPI(LSM6DSV320_OUTZ_H_A | 0x80, &this->buffer[5]); + + // Cast measurements to floats + this->data->LSM6DSV320LowGAccelX_mps2 = ((float) (int16_t) (this->buffer[0] | this->buffer[1] << 8)) * this->lowGSensitivity; + this->data->LSM6DSV320LowGAccelY_mps2 = ((float) (int16_t) (this->buffer[2] | this->buffer[3] << 8)) * this->lowGSensitivity; + this->data->LSM6DSV320LowGAccelZ_mps2 = ((float) (int16_t) (this->buffer[4] | this->buffer[5] << 8)) * this->lowGSensitivity; + + // Read High G Accelerometer Measurements + readSPI(LSM6DSV320_UI_OUTX_L_A_OIS_HG | 0x80, &this->buffer[0]); + readSPI(LSM6DSV320_UI_OUTX_H_A_OIS_HG | 0x80, &this->buffer[1]); + readSPI(LSM6DSV320_UI_OUTY_L_A_OIS_HG | 0x80, &this->buffer[2]); + readSPI(LSM6DSV320_UI_OUTY_H_A_OIS_HG | 0x80, &this->buffer[3]); + readSPI(LSM6DSV320_UI_OUTZ_L_A_OIS_HG | 0x80, &this->buffer[4]); + readSPI(LSM6DSV320_UI_OUTZ_H_A_OIS_HG | 0x80, &this->buffer[5]); + + // Cast measurements to floats + this->data->LSM6DSV320HighGAccelX_mps2 = ((float) (int16_t) (this->buffer[0] | this->buffer[1] << 8)) * this->highGSensitivity; + this->data->LSM6DSV320HighGAccelY_mps2 = ((float) (int16_t) (this->buffer[2] | this->buffer[3] << 8)) * this->highGSensitivity; + this->data->LSM6DSV320HighGAccelZ_mps2 = ((float) (int16_t) (this->buffer[4] | this->buffer[5] << 8)) * this->highGSensitivity; + + return 0; +} diff --git a/Code/Core/Src/Devices/SPI_Devices/MS560702BA03.cpp b/Code/Core/Src/Devices/SPI_Devices/MS560702BA03.cpp new file mode 100644 index 0000000..eae390f --- /dev/null +++ b/Code/Core/Src/Devices/SPI_Devices/MS560702BA03.cpp @@ -0,0 +1,90 @@ +/* + * MS560702BA03.cpp + * + * Created on: Oct 29, 2025 + * Author: colin + */ + +#include "Devices/SPI_Devices/MS560702BA03.h" + +MS560702BA03::MS560702BA03(DataContainer* data, SPI_HandleTypeDef *spi, GPIO_TypeDef *port, uint16_t pin) + : SPIDevice(data, spi, port, pin) +{ + data->MS560702BA03Temperature_C = 0.0f; + data->MS560702BA03Pressure_hPA = 0.0f; + data->MS560702BA03Altitude_m = 0.0f; + +} + +int MS560702BA03::deviceInit() +{ + if (writeSPI(MS5607_RESET, nullptr, 0) != HAL_OK) { + return false; + } + + HAL_Delay(3); // Max reset time + + switch (osr) { + case OSR_256: conversionTime_us = 700; break; + case OSR_512: conversionTime_us = 1000; break; + case OSR_1024: conversionTime_us = 2000; break; + case OSR_2048: conversionTime_us = 5000; break; + case OSR_4096: conversionTime_us = 9000; break; + default: return false; + } + + return readProm(); +} + +int MS560702BA03::readProm() { + for (uint8_t i = 0; i < 7; i++) { + if (readSPI(MS5607_PROM_READ + (i * 2), buffer, 2) != HAL_OK) + return -1; + C[i] = (buffer[0] << 8) | buffer[1]; + } + return 0; +} + +uint32_t MS560702BA03::readADC(uint8_t cmd) { + writeSPI(cmd, nullptr, 0); + delay_us(conversionTime_us); + + readSPI(MS5607_ADC_READ, buffer, 3); + + return (buffer[0] << 16) | (buffer[1] << 8) | buffer[2]; +} + +int MS560702BA03::updateDevice() +{ + now_us = micros(); + delay = now_us - conversionStart_us; + if (delay < conversionTime_us) + delay_us(conversionTime_us - delay); + + readSPI(MS5607_ADC_READ, buffer, 3); + D1 = (buffer[0] << 16) | (buffer[1] << 8) | buffer[2]; + + // D1 = readADC(MS5607_CONVERT_D1 | (osr << 1)); + D2 = readADC(MS5607_CONVERT_D2 | (osr << 1)); + + dT = D2 - ((uint32_t)C[5] << 8); + TEMP = 2000 + ((int64_t)dT * C[6]) / (1 << 23); + + OFF = ((int64_t)C[2] << 17) + (((int64_t)C[4] * dT) >> 6); + SENS = ((int64_t)C[1] << 16) + (((int64_t)C[3] * dT) >> 7); + + P = (((D1 * SENS) >> 21) - OFF) >> 15; + + this->data->MS560702BA03Temperature_C = TEMP / 100.0f; + this->data->MS560702BA03Pressure_hPA = P / 100.0f; + this->data->MS560702BA03Altitude_m = (1-powf(this->data->MS560702BA03Pressure_hPA/1013.25, 0.190284))*145366.45 * FEET_TO_METER; + + return 0; +} + +void MS560702BA03::startConversion() +{ + writeSPI(MS5607_CONVERT_D1 | (osr << 1), nullptr, 0); + + conversionStart_us = micros(); +} \ No newline at end of file diff --git a/Code/Core/Src/Subsystems/Navigation.cpp b/Code/Core/Src/Subsystems/Navigation.cpp new file mode 100644 index 0000000..02579a3 --- /dev/null +++ b/Code/Core/Src/Subsystems/Navigation.cpp @@ -0,0 +1,444 @@ +/* + * Navigation.cpp + * + * Created on: Oct 27, 2025 + * Author: colin + */ + +#include "Subsystems/Navigation.h" + +float softThreshold(float value, float threshold) +{ + if (fabsf(value) < threshold) + return value * (fabsf(value) / threshold); // Linear taper + return value; +} + +Navigation::Navigation(DataContainer* data, SPI_HandleTypeDef* spiBus, UART_HandleTypeDef* uart, uint8_t* gpsRxBuffer) + : Subsystem(data), + imu(data, spiBus, SPI_CS1_GPIO_Port, SPI_CS1_Pin), + baro(data, spiBus, BARO_CS_GPIO_Port, BARO_CS_Pin), + gps(data, uart, gpsRxBuffer) +{ + data->KalmanFilterPositionX_m = 0.0f; + data->KalmanFilterPositionY_m = 0.0f; + data->KalmanFilterPositionZ_m = 0.0f; + + data->KalmanFilterVelocityX_mps = 0.0f; + data->KalmanFilterVelocityY_mps = 0.0f; + data->KalmanFilterVelocityZ_mps = 0.0f; + + data->KalmanFilterAccelerationX_mps2 = 0.0f; + data->KalmanFilterAccelerationY_mps2 = 0.0f; + data->KalmanFilterAccelerationZ_mps2 = 0.0f; + + lastLoop = HAL_GetTick(); + last_us = micros(); +} + + +int Navigation::init() +{ + + if (imu.deviceInit() < 0) + { + return -1; + } + + if (baro.deviceInit() < 0) + { + return -1; + } + + if (gps.deviceInit() < 0) + { + return -1; + } + + initializeQuaternion(); + + return 0; +} + + +int Navigation::update() +{ + now = micros(); + + // Handle micros() overflow + if (now > lastLoop) + { + dt_us = now - lastLoop; + dt_s = dt_us / 1000000.0f; + freq = 1.0f / dt_s; + lastLoop = now; + } + // If now < lastLoop, micros() has overflowed and pretend that the dt didn't change + + dt2 = dt_s * dt_s; + dt3 = dt2 * dt_s; + dt4 = dt3 * dt_s; + + // ------------------------------------------------------------- + // Read Sensor Data + // ------------------------------------------------------------- + + baro.startConversion(); + + imu.updateDevice(); + + lowG(0) = data->LSM6DSV320LowGAccelX_mps2; + lowG(1) = data->LSM6DSV320LowGAccelY_mps2; + lowG(2) = data->LSM6DSV320LowGAccelZ_mps2; + + highG(0) = data->LSM6DSV320HighGAccelX_mps2; + highG(1) = data->LSM6DSV320HighGAccelY_mps2; + highG(2) = data->LSM6DSV320HighGAccelZ_mps2; + + // ------------------------------------------------------------- + // Quaterion Intergration + // ------------------------------------------------------------- + rollRate_rad = data->LSM6DSV320GyroY_dps * DEG_TO_RAD; + pitchRate_rad = data->LSM6DSV320GyroX_dps * DEG_TO_RAD; + yawRate_rad = data->LSM6DSV320GyroZ_dps * DEG_TO_RAD; + + // integrate quaternion + integrateQuaternion(); + + // Rotate Accelerometer data by quaterion to get it to earth reference frame + rotateVectorByQuaternion(lowG); + rotateVectorByQuaternion(highG); + + baro.updateDevice(); + gps.updateDevice(); + + // ------------------------------------------------------------- + // Kalman Filter + // ------------------------------------------------------------- + + if (!isKalmanFilterInit) + initKalmanFilter(); + + updateKalmanFilter(); + runKalmanFilter(); + + data->KalmanFilterPositionX_m = x(0); + data->KalmanFilterAccelerationX_mps2 = x(1); + data->KalmanFilterVelocityX_mps = x(2); + + data->KalmanFilterPositionY_m = x(3); + data->KalmanFilterAccelerationY_mps2 = x(4); + data->KalmanFilterVelocityY_mps = x(5); + + data->KalmanFilterPositionZ_m = x(6); + data->KalmanFilterAccelerationZ_mps2 = x(7); + data->KalmanFilterVelocityZ_mps = x(8); + + return 0; +} + +void Navigation::rotateVectorByQuaternion(Matrix& vec) +{ + // Rotate vector from body frame to earth frame using quaternion + // Formula: v' = q * v * q^(-1) + // For unit quaternions: q^(-1) = q* (conjugate) + + float qw = data->quaternionW; + float qx = data->quaternionX; + float qy = data->quaternionY; + float qz = data->quaternionZ; + + float vx = vec(0); + float vy = vec(1); + float vz = vec(2); + + // Optimized rotation: v' = v + 2*r x (r x v + w*v) + // where r = [qx, qy, qz] and w = qw + + // First cross product: r x v + float cx = qy * vz - qz * vy; + float cy = qz * vx - qx * vz; + float cz = qx * vy - qy * vx; + + // Add w*v + cx += qw * vx; + cy += qw * vy; + cz += qw * vz; + + // Second cross product: r x (r x v + w*v) + float rx = qy * cz - qz * cy; + float ry = qz * cx - qx * cz; + float rz = qx * cy - qy * cx; + + // Final result: v + 2 * (r x (r x v + w*v)) + vec(0) = vx + 2.0f * rx; + vec(1) = vy + 2.0f * ry; + vec(2) = vz + 2.0f * rz; +} + +void Navigation::integrateQuaternion() +{ + pitchRate_rad = softThreshold(pitchRate_rad, GYRO_THRESHOLD_DPS); + rollRate_rad = softThreshold(rollRate_rad, GYRO_THRESHOLD_DPS); + yawRate_rad = softThreshold(yawRate_rad, GYRO_THRESHOLD_DPS); + + quaternionWDot = 0.5f * (-pitchRate_rad * data->quaternionX - rollRate_rad * data->quaternionY - yawRate_rad * data->quaternionZ); + quaternionXDot = 0.5f * ( pitchRate_rad * data->quaternionW + yawRate_rad * data->quaternionY - rollRate_rad * data->quaternionZ); + quaternionYDot = 0.5f * ( rollRate_rad * data->quaternionW - yawRate_rad * data->quaternionX + pitchRate_rad * data->quaternionZ); + quaternionZDot = 0.5f * ( yawRate_rad * data->quaternionW + rollRate_rad * data->quaternionX - pitchRate_rad * data->quaternionY); + + data->quaternionW += quaternionWDot * dt_s; + data->quaternionX += quaternionXDot * dt_s; + data->quaternionY += quaternionYDot * dt_s; + data->quaternionZ += quaternionZDot * dt_s; + + data->quaternionNorm = sqrtf( data->quaternionW * data->quaternionW + + data->quaternionX * data->quaternionX + + data->quaternionY * data->quaternionY + + data->quaternionZ * data->quaternionZ); + + if (data->quaternionNorm > 1e-6f) + { + data->quaternionW /= data->quaternionNorm; + data->quaternionX /= data->quaternionNorm; + data->quaternionY /= data->quaternionNorm; + data->quaternionZ /= data->quaternionNorm; + } else { + data->quaternionW = 1.0f; + data->quaternionX = 0.0f; + data->quaternionY = 0.0f; + data->quaternionZ = 0.0f; + } + + siny_cosp = 2.0f * (data->quaternionW * data->quaternionY - data->quaternionZ * data->quaternionX); + cosy_cosp = 1.0f - 2.0f * (data->quaternionX * data->quaternionX + data->quaternionY * data->quaternionY); + data->roll = atan2f(siny_cosp, cosy_cosp) * RAD_TO_DEG; + + // Pitch (rotation about X-axis) + sinp = 2.0f * (data->quaternionW * data->quaternionX + data->quaternionY * data->quaternionZ); + if (fabsf(sinp) >= 1.0f) + data->pitch = copysignf(M_PI / 2.0f, sinp) * RAD_TO_DEG; // Use 90 degrees if out of range + else + data->pitch = asinf(sinp) * RAD_TO_DEG; + + // Yaw (rotation about Z-axis) + sinr_cosp = 2.0f * (data->quaternionW * data->quaternionZ + data->quaternionX * data->quaternionY); + cosr_cosp = 1.0f - 2.0f * (data->quaternionY * data->quaternionY + data->quaternionZ * data->quaternionZ); + data->yaw = atan2f(sinr_cosp, cosr_cosp) * RAD_TO_DEG; +} + +void Navigation::initializeQuaternion() +{ + float norm = sqrtf( data->LSM6DSV320LowGAccelX_mps2 * data->LSM6DSV320LowGAccelX_mps2 + + data->LSM6DSV320LowGAccelY_mps2 * data->LSM6DSV320LowGAccelY_mps2 + + data->LSM6DSV320LowGAccelZ_mps2 * data->LSM6DSV320LowGAccelZ_mps2); + + if (norm < 1e-6f) + { + data->quaternionW = 1.0f; + data->quaternionX = 0.0f; + data->quaternionY = 0.0f; + data->quaternionZ = 0.0f; + } + + float ax = data->LSM6DSV320LowGAccelX_mps2 / norm; + float ay = data->LSM6DSV320LowGAccelY_mps2 / norm; + float az = data->LSM6DSV320LowGAccelZ_mps2 / norm; + + data->quaternionW = sqrtf(1 + ax + ay + az) / 2; + data->quaternionX = (ay - az) / (4 * data->quaternionW); + data->quaternionY = (az - ax) / (4 * data->quaternionW); + data->quaternionZ = (ax - ay) / (4 * data->quaternionW); + + // Calculate initial roll and pitch from accelerometer + data->pitch = atan2f(-ax, ay); + data->yaw = atan2f(-az, -ay); + data->roll = 0.0f; + + // Convert to quaternion + float cr = cosf(data->roll * 0.5f); + float sr = sinf(data->roll * 0.5f); + float cp = cosf(data->pitch * 0.5f); + float sp = sinf(data->pitch * 0.5f); + float cy = cosf(data->yaw * 0.5f); + float sy = sinf(data->yaw * 0.5f); + + // Quaternion from Euler angles + data->quaternionW = cr * cp * cy + sr * sp * sy; + data->quaternionX = cr * sp * cy + sr * cp * sy; + data->quaternionY = sr * cp * cy - cr * sp * sy; + data->quaternionZ = cr * cp * sy - sr * sp * cy; +} + +void Navigation::initKalmanFilter() +{ + dt_s = NAVIGATION_TARGET_DT; + dt2 = dt_s * dt_s; + dt3 = dt2 * dt_s; + dt4 = dt3 * dt_s; + + // Initialize Kalman Filter Matricies + + // Initial State + x.setZero(); + x(2) = lowG(0); // Acc X + x(5) = lowG(1); // Acc Y + x(6) = data->MS560702BA03Altitude_m; // Pos Z Set the initial Barometric Altitude to the current altitude + x(8) = lowG(2); // Acc Z + + // State Transition + F.setIdentity(); + // For each axis block: + // pos' = pos + vel*dt + 0.5*acc*dt^2 + // vel' = vel + acc*dt + // acc' = acc + + // X Axis + F(0,1) = dt_s; + F(0,2) = (dt_s * dt_s) / 2.0f; + F(1,2) = dt_s; + + // Y Axis + F(3,4) = dt_s; + F(3,5) = (dt_s * dt_s) / 2.0f; + F(4,5) = dt_s; + + // Z Axis + F(6,7) = dt_s; + F(6,8) = (dt_s * dt_s) / 2.0f; + F(7,8) = dt_s; + + // Get Initial Measurements + Z.setZero(); + Z(0) = data->LSM6DSV320LowGAccelX_mps2; + Z(1) = data->LSM6DSV320LowGAccelY_mps2; + Z(2) = data->LSM6DSV320LowGAccelZ_mps2; + Z(3) = data->LSM6DSV320HighGAccelX_mps2; + Z(4) = data->LSM6DSV320HighGAccelY_mps2; + Z(5) = data->LSM6DSV320HighGAccelZ_mps2; + Z(6) = data->MS560702BA03Altitude_m; + + // Observation + H.setZero(); + H(0, 2) = H(1, 5) = H(2, 8) = H(3, 2) = H(4, 5) = H(5, 8) = H(6, 6) = 1.0f; + + // Process Noise + Q.setZero(); + Q(0,0) = dt4 / 4.0f; + Q(0,1) = dt3 / 2.0f; Q(1,0) = Q(0,1); + Q(0,2) = dt2 / 2.0f; Q(2,0) = Q(0,2); + Q(1,1) = dt2; + Q(1,2) = dt_s; Q(2,1) = Q(1,2); + Q(2,2) = 1.0f; // keep small baseline + + Q(3,3) = dt4 / 4.0f; + Q(3,4) = dt3 / 2.0f; Q(4,3) = Q(3,4); + Q(3,5) = dt2 / 2.0f; Q(5,3) = Q(3,5); + Q(4,4) = dt2; + Q(4,5) = dt_s; Q(5,4) = Q(4,5); + Q(5,5) = 1.0f; // keep small baseline + + Q(6,6) = dt4 / 4.0f; + Q(6,7) = dt3 / 2.0f; Q(7,6) = Q(6,7); + Q(6,8) = dt2 / 2.0f; Q(8,6) = Q(6,8); + Q(7,7) = dt2; + Q(7,8) = dt_s; Q(8,7) = Q(7,8); + Q(8,8) = 1.0f; // keep small baseline + + // copy blocks for Y (3..5) and Z (6..8) + + Q *= processNoise; + + // Measurement Noise + R.setIdentity(); + R(0, 0) = R(1, 1) = R(2, 2) = powf(lowGNoise, 2); + R(3, 3) = R(4, 4) = R(5, 5) = powf(highGNoise, 2); + R(6, 6) = powf(baroNoise, 2); + + // Estimate Error + P.setIdentity(); + P *= 1.0; // initial uncertainty + + I.setIdentity(); + + isKalmanFilterInit = true; + + return; +} + +void Navigation::updateKalmanFilter() +{ + // Update Measurements + Z(0) = lowG(0); + Z(1) = lowG(1); + Z(2) = lowG(2); + Z(3) = highG(0); + Z(4) = highG(1); + Z(5) = highG(2); + Z(6) = data->MS560702BA03Altitude_m; + + // Update State Transition Matrix + + // X Axis + F(0,1) = dt_s; + F(0,2) = (dt_s * dt_s) / 2.0f; + F(1,2) = dt_s; + + // Y Axis + F(3,4) = dt_s; + F(3,5) = (dt_s * dt_s) / 2.0f; + F(4,5) = dt_s; + + // Z Axis + F(6,7) = dt_s; + F(6,8) = (dt_s * dt_s) / 2.0f; + F(7,8) = dt_s; + + // Update Process Noise + Q.setZero(); + Q(0,0) = dt4 / 4.0f; + Q(0,1) = dt3 / 2.0f; Q(1,0) = Q(0,1); + Q(0,2) = dt2 / 2.0f; Q(2,0) = Q(0,2); + Q(1,1) = dt2; + Q(1,2) = dt_s; Q(2,1) = Q(1,2); + Q(2,2) = 1.0f; // keep small baseline + + Q(3,3) = dt4 / 4.0f; + Q(3,4) = dt3 / 2.0f; Q(4,3) = Q(3,4); + Q(3,5) = dt2 / 2.0f; Q(5,3) = Q(3,5); + Q(4,4) = dt2; + Q(4,5) = dt_s; Q(5,4) = Q(4,5); + Q(5,5) = 1.0f; // keep small baseline + + Q(6,6) = dt4 / 4.0f; + Q(6,7) = dt3 / 2.0f; Q(7,6) = Q(6,7); + Q(6,8) = dt2 / 2.0f; Q(8,6) = Q(6,8); + Q(7,7) = dt2; + Q(7,8) = dt_s; Q(8,7) = Q(7,8); + Q(8,8) = 1.0f; // keep small baseline + + Q *= processNoise; + + return; +} + +void Navigation::runKalmanFilter() +{ + // Predict State ~0.5 ms + x = F * x; + + // Predict Error Covariance ~3 ms + S = H * P * H.transpose() + R; + + // Compute Kalman Gain ~5 ms + // Prefer an LDLT or LLT solve over explicit inverse: + K = (P * H.transpose()) * S.inverse(); + + // Compute the Estimate ~0.5 ms + x = x + K * (Z - H * x); + + // Computer the Error covariance ~10 ms + P = (I - K * H) * P * (I - K * H).transpose() + K * R * K.transpose(); + + return; +} diff --git a/Code/Core/Src/main.cpp b/Code/Core/Src/main.cpp index 549a728..145b7a1 100644 --- a/Code/Core/Src/main.cpp +++ b/Code/Core/Src/main.cpp @@ -22,13 +22,21 @@ /* Private includes ----------------------------------------------------------*/ /* USER CODE BEGIN Includes */ +#include +#include +#include #include + #include "tusb.h" #include "dfuBootloader.h" #include "usbHelper.h" #include "DataContainer.h" +#include "Subsystems/Navigation.h" + +#include "utils.h" + /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ @@ -53,16 +61,34 @@ I2C_HandleTypeDef hi2c2; SPI_HandleTypeDef hspi1; UART_HandleTypeDef huart4; +DMA_HandleTypeDef hdma_uart4_rx; PCD_HandleTypeDef hpcd_USB_OTG_FS; /* USER CODE BEGIN PV */ +#define USB_BUF_LEN 512 + +char usbTxBuffer[USB_BUF_LEN]; +char usbRxBuffer[USB_BUF_LEN]; + +uint16_t usbTxBufferLen; +uint16_t usbRxBufferLen; + +#define GPS_BUFFER_SIZE 512 + +uint8_t gpsRxBuffer[GPS_BUFFER_SIZE]; + +DataContainer data; + +Navigation nav(&data, &hspi1, &huart4, gpsRxBuffer); + /* USER CODE END PV */ /* Private function prototypes -----------------------------------------------*/ extern "C" void SystemClock_Config(void); static void MX_GPIO_Init(void); +static void MX_DMA_Init(void); static void MX_ADC1_Init(void); static void MX_I2C2_Init(void); static void MX_SPI1_Init(void); @@ -83,7 +109,6 @@ static void MX_USB_OTG_FS_PCD_Init(void); */ int main(void) { - /* USER CODE BEGIN 1 */ dfuCheckAndJumpBootloader(); @@ -108,6 +133,7 @@ int main(void) /* Initialize all configured peripherals */ MX_GPIO_Init(); + MX_DMA_Init(); MX_ADC1_Init(); MX_I2C2_Init(); MX_SPI1_Init(); @@ -115,11 +141,27 @@ int main(void) MX_USB_OTG_FS_PCD_Init(); /* USER CODE BEGIN 2 */ + HAL_UART_Receive_DMA(&huart4, gpsRxBuffer, GPS_BUFFER_SIZE); + tud_init(BOARD_TUD_RHPORT); - uint32_t currentTick; - uint32_t lastSendTick = 0; - const uint32_t sendIntervalMs = 5000; + cdcSendMessage("Welcome to the Pioneer Rocketry Flight Computer!\r\n", USB_BUF_LEN); + + DWT_Init(); + + if (nav.init() < 0) + { + usbTxBufferLen = snprintf((char*)usbTxBuffer, USB_BUF_LEN, "Error while Initializing Navigation!\r\n"); + cdcSendMessage(usbTxBuffer, usbTxBufferLen); + while (1) + { + // Send Error Message over USB CDC + cdcSendMessage(usbTxBuffer, usbTxBufferLen); + HAL_Delay(1000); + } + } + + cdcSendMessage("Initialization Complete \r\n", USB_BUF_LEN); /* USER CODE END 2 */ @@ -131,18 +173,10 @@ int main(void) /* USER CODE BEGIN 3 */ - tud_task(); - - currentTick = HAL_GetTick(); // Get the current system tick (ms) - - // Check if it's time to send data - if (currentTick - lastSendTick >= sendIntervalMs) - { - lastSendTick = currentTick; + nav.update(); + // HAL_Delay(1); - int len = snprintf(usb_tx_buffer, MAX_TICK_MSG_LEN, "Tick: %lu ms\r\n", currentTick); - cdcSendMessage(usb_tx_buffer, len); - } + tud_task(); } /* USER CODE END 3 */ } @@ -309,7 +343,7 @@ static void MX_SPI1_Init(void) hspi1.Init.CLKPolarity = SPI_POLARITY_LOW; hspi1.Init.CLKPhase = SPI_PHASE_1EDGE; hspi1.Init.NSS = SPI_NSS_SOFT; - hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_2; + hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_16; hspi1.Init.FirstBit = SPI_FIRSTBIT_MSB; hspi1.Init.TIMode = SPI_TIMODE_DISABLE; hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE; @@ -340,7 +374,7 @@ static void MX_UART4_Init(void) /* USER CODE END UART4_Init 1 */ huart4.Instance = UART4; - huart4.Init.BaudRate = 115200; + huart4.Init.BaudRate = 9600; huart4.Init.WordLength = UART_WORDLENGTH_8B; huart4.Init.StopBits = UART_STOPBITS_1; huart4.Init.Parity = UART_PARITY_NONE; @@ -392,6 +426,22 @@ static void MX_USB_OTG_FS_PCD_Init(void) } +/** + * Enable DMA controller clock + */ +static void MX_DMA_Init(void) +{ + + /* DMA controller clock enable */ + __HAL_RCC_DMA1_CLK_ENABLE(); + + /* DMA interrupt init */ + /* DMA1_Stream2_IRQn interrupt configuration */ + HAL_NVIC_SetPriority(DMA1_Stream2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(DMA1_Stream2_IRQn); + +} + /** * @brief GPIO Initialization Function * @param None @@ -416,8 +466,10 @@ static void MX_GPIO_Init(void) |PRYO1_TRIGGER_Pin|LORA_RESET_Pin, GPIO_PIN_RESET); /*Configure GPIO pin Output Level */ - HAL_GPIO_WritePin(GPIOB, LORA_CS_Pin|BARO_CS_Pin|FLASH_CS_Pin|FLASH_RESET_Pin - |FLASH_WP_Pin, GPIO_PIN_RESET); + HAL_GPIO_WritePin(GPIOB, LORA_CS_Pin|FLASH_CS_Pin|FLASH_RESET_Pin|FLASH_WP_Pin, GPIO_PIN_RESET); + + /*Configure GPIO pin Output Level */ + HAL_GPIO_WritePin(BARO_CS_GPIO_Port, BARO_CS_Pin, GPIO_PIN_SET); /*Configure GPIO pins : SPI_CS1_Pin LORA_DIO0_Pin PYRO3_TRIGGER_Pin PRYO2_TRIGGER_Pin PRYO1_TRIGGER_Pin LORA_RESET_Pin */ @@ -479,7 +531,7 @@ static void MX_GPIO_Init(void) void tud_dfu_runtime_reboot_to_dfu_cb(void) { - const char* reboot_msg = "Rebooting into DFU mode...\r\n"; + char reboot_msg[] = "Rebooting into DFU mode...\r\n"; cdcSendMessage(reboot_msg, strlen(reboot_msg)); // Ensure message is sent @@ -516,6 +568,10 @@ void tud_dfu_manifest_cb(uint8_t alt) NVIC_SystemReset(); } +void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) +{ + +} /* USER CODE END 4 */ diff --git a/Code/Core/Src/stm32f4xx_hal_msp.c b/Code/Core/Src/stm32f4xx_hal_msp.c index 855b1a3..e87f488 100644 --- a/Code/Core/Src/stm32f4xx_hal_msp.c +++ b/Code/Core/Src/stm32f4xx_hal_msp.c @@ -23,6 +23,7 @@ /* USER CODE BEGIN Includes */ /* USER CODE END Includes */ +extern DMA_HandleTypeDef hdma_uart4_rx; /* Private typedef -----------------------------------------------------------*/ /* USER CODE BEGIN TD */ @@ -318,6 +319,25 @@ void HAL_UART_MspInit(UART_HandleTypeDef* huart) GPIO_InitStruct.Alternate = GPIO_AF8_UART4; HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); + /* UART4 DMA Init */ + /* UART4_RX Init */ + hdma_uart4_rx.Instance = DMA1_Stream2; + hdma_uart4_rx.Init.Channel = DMA_CHANNEL_4; + hdma_uart4_rx.Init.Direction = DMA_PERIPH_TO_MEMORY; + hdma_uart4_rx.Init.PeriphInc = DMA_PINC_DISABLE; + hdma_uart4_rx.Init.MemInc = DMA_MINC_ENABLE; + hdma_uart4_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE; + hdma_uart4_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE; + hdma_uart4_rx.Init.Mode = DMA_CIRCULAR; + hdma_uart4_rx.Init.Priority = DMA_PRIORITY_LOW; + hdma_uart4_rx.Init.FIFOMode = DMA_FIFOMODE_DISABLE; + if (HAL_DMA_Init(&hdma_uart4_rx) != HAL_OK) + { + Error_Handler(); + } + + __HAL_LINKDMA(huart,hdmarx,hdma_uart4_rx); + /* USER CODE BEGIN UART4_MspInit 1 */ /* USER CODE END UART4_MspInit 1 */ @@ -348,6 +368,8 @@ void HAL_UART_MspDeInit(UART_HandleTypeDef* huart) */ HAL_GPIO_DeInit(GPIOC, GPIO_PIN_10|GPIO_PIN_11); + /* UART4 DMA DeInit */ + HAL_DMA_DeInit(huart->hdmarx); /* USER CODE BEGIN UART4_MspDeInit 1 */ /* USER CODE END UART4_MspDeInit 1 */ diff --git a/Code/Core/Src/stm32f4xx_it.c b/Code/Core/Src/stm32f4xx_it.c index 91920b6..1d7376a 100644 --- a/Code/Core/Src/stm32f4xx_it.c +++ b/Code/Core/Src/stm32f4xx_it.c @@ -58,6 +58,7 @@ /* USER CODE END 0 */ /* External variables --------------------------------------------------------*/ +extern DMA_HandleTypeDef hdma_uart4_rx; extern PCD_HandleTypeDef hpcd_USB_OTG_FS; /* USER CODE BEGIN EV */ @@ -201,6 +202,20 @@ void SysTick_Handler(void) /* please refer to the startup file (startup_stm32f4xx.s). */ /******************************************************************************/ +/** + * @brief This function handles DMA1 stream2 global interrupt. + */ +void DMA1_Stream2_IRQHandler(void) +{ + /* USER CODE BEGIN DMA1_Stream2_IRQn 0 */ + + /* USER CODE END DMA1_Stream2_IRQn 0 */ + HAL_DMA_IRQHandler(&hdma_uart4_rx); + /* USER CODE BEGIN DMA1_Stream2_IRQn 1 */ + + /* USER CODE END DMA1_Stream2_IRQn 1 */ +} + /** * @brief This function handles USB On The Go FS global interrupt. */ diff --git a/Code/Core/Src/usbHelper.cpp b/Code/Core/Src/usbHelper.cpp index d3ed654..880ec34 100644 --- a/Code/Core/Src/usbHelper.cpp +++ b/Code/Core/Src/usbHelper.cpp @@ -6,7 +6,7 @@ * @param len Length of the string (in bytes). * @retval The number of bytes successfully written to the USB buffer. */ -size_t cdcSendMessage(const char* str, size_t len) +size_t cdcSendMessage(char* str, size_t len) { // 1. Check if the USB CDC interface is connected to the host if (!tud_cdc_connected()) diff --git a/Code/Core/Src/utils.c b/Code/Core/Src/utils.c new file mode 100644 index 0000000..4ac012e --- /dev/null +++ b/Code/Core/Src/utils.c @@ -0,0 +1,29 @@ +/* + * utils.c + * + * Created on: Dec 1, 2025 + * Author: colin + */ + +#include "utils.h" +#include "stm32f4xx.h" + +void DWT_Init(void) +{ + CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; // Enable trace + DWT->CYCCNT = 0; // Reset counter + DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk; // Start counter +} + +uint32_t micros(void) +{ + return DWT->CYCCNT / (SystemCoreClock / 1000000); +} + +void delay_us(uint32_t us) +{ + uint32_t cycles = (SystemCoreClock / 1000000) * us; + uint32_t start = DWT->CYCCNT; + + while ((DWT->CYCCNT - start) < cycles); +} diff --git a/Code/Debug/Core/Src/subdir.mk b/Code/Debug/Core/Src/subdir.mk deleted file mode 100644 index 864ba39..0000000 --- a/Code/Debug/Core/Src/subdir.mk +++ /dev/null @@ -1,63 +0,0 @@ -################################################################################ -# Automatically-generated file. Do not edit! -# Toolchain: GNU Tools for STM32 (13.3.rel1) -################################################################################ - -# Add inputs and outputs from these tool invocations to the build variables -CPP_SRCS += \ -../Core/Src/Subsystem.cpp \ -../Core/Src/dfuBootloader.cpp \ -../Core/Src/main.cpp \ -../Core/Src/usbHelper.cpp \ -../Core/Src/usb_descriptors.cpp - -C_SRCS += \ -../Core/Src/board_api.c \ -../Core/Src/stm32f4xx_hal_msp.c \ -../Core/Src/stm32f4xx_it.c \ -../Core/Src/syscalls.c \ -../Core/Src/sysmem.c \ -../Core/Src/system_stm32f4xx.c - -C_DEPS += \ -./Core/Src/board_api.d \ -./Core/Src/stm32f4xx_hal_msp.d \ -./Core/Src/stm32f4xx_it.d \ -./Core/Src/syscalls.d \ -./Core/Src/sysmem.d \ -./Core/Src/system_stm32f4xx.d - -OBJS += \ -./Core/Src/Subsystem.o \ -./Core/Src/board_api.o \ -./Core/Src/dfuBootloader.o \ -./Core/Src/main.o \ -./Core/Src/stm32f4xx_hal_msp.o \ -./Core/Src/stm32f4xx_it.o \ -./Core/Src/syscalls.o \ -./Core/Src/sysmem.o \ -./Core/Src/system_stm32f4xx.o \ -./Core/Src/usbHelper.o \ -./Core/Src/usb_descriptors.o - -CPP_DEPS += \ -./Core/Src/Subsystem.d \ -./Core/Src/dfuBootloader.d \ -./Core/Src/main.d \ -./Core/Src/usbHelper.d \ -./Core/Src/usb_descriptors.d - - -# Each subdirectory must supply rules for building sources it contributes -Core/Src/%.o Core/Src/%.su Core/Src/%.cyclo: ../Core/Src/%.cpp Core/Src/subdir.mk - arm-none-eabi-g++ "$<" -mcpu=cortex-m4 -std=gnu++14 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F446xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I../lib/TinyUSB/src -I../lib/TinyUSB/hw -O0 -ffunction-sections -fdata-sections -fno-exceptions -fno-rtti -fno-use-cxa-atexit -Wall -fstack-usage -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@" -Core/Src/%.o Core/Src/%.su Core/Src/%.cyclo: ../Core/Src/%.c Core/Src/subdir.mk - arm-none-eabi-gcc "$<" -mcpu=cortex-m4 -std=gnu11 -g3 -DDEBUG -DUSE_HAL_DRIVER -DSTM32F446xx -c -I../Core/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc -I../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I../Drivers/CMSIS/Include -I../lib/TinyUSB/hw -I../lib/TinyUSB/src -O0 -ffunction-sections -fdata-sections -Wall -fstack-usage -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb -o "$@" - -clean: clean-Core-2f-Src - -clean-Core-2f-Src: - -$(RM) ./Core/Src/Subsystem.cyclo ./Core/Src/Subsystem.d ./Core/Src/Subsystem.o ./Core/Src/Subsystem.su ./Core/Src/board_api.cyclo ./Core/Src/board_api.d ./Core/Src/board_api.o ./Core/Src/board_api.su ./Core/Src/dfuBootloader.cyclo ./Core/Src/dfuBootloader.d ./Core/Src/dfuBootloader.o ./Core/Src/dfuBootloader.su ./Core/Src/main.cyclo ./Core/Src/main.d ./Core/Src/main.o ./Core/Src/main.su ./Core/Src/stm32f4xx_hal_msp.cyclo ./Core/Src/stm32f4xx_hal_msp.d ./Core/Src/stm32f4xx_hal_msp.o ./Core/Src/stm32f4xx_hal_msp.su ./Core/Src/stm32f4xx_it.cyclo ./Core/Src/stm32f4xx_it.d ./Core/Src/stm32f4xx_it.o ./Core/Src/stm32f4xx_it.su ./Core/Src/syscalls.cyclo ./Core/Src/syscalls.d ./Core/Src/syscalls.o ./Core/Src/syscalls.su ./Core/Src/sysmem.cyclo ./Core/Src/sysmem.d ./Core/Src/sysmem.o ./Core/Src/sysmem.su ./Core/Src/system_stm32f4xx.cyclo ./Core/Src/system_stm32f4xx.d ./Core/Src/system_stm32f4xx.o ./Core/Src/system_stm32f4xx.su ./Core/Src/usbHelper.cyclo ./Core/Src/usbHelper.d ./Core/Src/usbHelper.o ./Core/Src/usbHelper.su ./Core/Src/usb_descriptors.cyclo ./Core/Src/usb_descriptors.d ./Core/Src/usb_descriptors.o ./Core/Src/usb_descriptors.su - -.PHONY: clean-Core-2f-Src - diff --git a/Code/Makefile b/Code/Makefile index 5a54037..e7b1bbb 100644 --- a/Code/Makefile +++ b/Code/Makefile @@ -22,7 +22,7 @@ TARGET = Code # debug build? DEBUG = 1 # optimization -OPT = -Og +OPT = -O0 ####################################### @@ -163,6 +163,7 @@ LDSCRIPT = STM32F446XX_FLASH.ld LIBS = -lc -lm -lnosys LIBDIR = LDFLAGS = $(MCU) -specs=nano.specs -T$(LDSCRIPT) $(LIBDIR) $(LIBS) -Wl,-Map=$(BUILD_DIR)/$(TARGET).map,--cref -Wl,--gc-sections +LDFLAGS += -u _printf_float # default action: build all all: $(BUILD_DIR)/$(TARGET).elf $(BUILD_DIR)/$(TARGET).hex $(BUILD_DIR)/$(TARGET).bin diff --git a/PCB/Flight Computer-backups/Flight Computer-2025-06-24_212714.zip b/PCB/Flight Computer-backups/Flight Computer-2025-06-24_212714.zip deleted file mode 100644 index 0c8dfe4..0000000 Binary files a/PCB/Flight Computer-backups/Flight Computer-2025-06-24_212714.zip and /dev/null differ diff --git a/PCB/Flight Computer-backups/Flight Computer-2025-10-28_211611.zip b/PCB/Flight Computer-backups/Flight Computer-2025-10-28_211611.zip new file mode 100644 index 0000000..61a59e1 Binary files /dev/null and b/PCB/Flight Computer-backups/Flight Computer-2025-10-28_211611.zip differ diff --git a/PCB/Flight Computer.kicad_prl b/PCB/Flight Computer.kicad_prl index 11a927c..141b652 100644 --- a/PCB/Flight Computer.kicad_prl +++ b/PCB/Flight Computer.kicad_prl @@ -1,6 +1,6 @@ { "board": { - "active_layer": 0, + "active_layer": 5, "active_layer_preset": "", "auto_track_width": true, "hidden_netclasses": [], @@ -49,7 +49,7 @@ "conflict_shadows", "shapes" ], - "visible_layers": "00000000_00000000_00002a80_aaaaafff", + "visible_layers": "00000000_00000000_00002a80_aaaa00ab", "zone_display_mode": 1 }, "git": {