From 4210c9c10472cc19e2561eea3304657f77265dc7 Mon Sep 17 00:00:00 2001 From: dyldonahue <98500199+dyldonahue@users.noreply.github.com> Date: Mon, 17 Jun 2024 13:25:21 -0400 Subject: [PATCH] Develop/fsae (#186) * switch to pca9539 (untested) (#175) * switch to pca9539 (untested) * update pdu to improved api * fix addr * new reg name * disable shutdown * removed debugging stuff * bump --------- Co-authored-by: Scott A <89099102+Sabramz@users.noreply.github.com> * Feature/pedal tune (#180) * prepped for pedal tuning + added break check when going to drive mode * added Scott's implementation for bspd pre check * added defcon 5 fault * fuck * remoced torque send * pedals calibrated * fix bit * im stupid * new pedals * added debug * poop farty * Auto stash before merge of "feature/pedal-tune" and "origin/feature/pedal-tune" * move tsms status * reset queuue * fixed stuff --------- Co-authored-by: Jack Rubacha * Finally get mph working! (#183) * dti back and mph calcs * queue fixes and got a backwards erpm reading * use correct units * fix up mph, tested recieve and send, not validated connection --------- Co-authored-by: Scott A <89099102+Sabramz@users.noreply.github.com> * Monitor fixes (#181) * add lv monitor again * remove bad line * fix missed line in rebase * fix up math * regen fix issue * fix send torque * Bms fault fixes (#182) * fix bms fault, get rid of write fan battbox * add note * fix bspd prefault * adjust value for proper scale * fix mph, bspd prefault * fix nero * add brake filter * Debounce tsms (#178) * tsms debounce with nertimer * you saw nothing * tsms debounce with nertimer * you saw nothing * add tsms debounce --------- Co-authored-by: Jack Rubacha --------- Co-authored-by: Jack Rubacha Co-authored-by: Scott A <89099102+Sabramz@users.noreply.github.com> --- .mxproject | 2 +- Core/Inc/cerberus_conf.h | 19 +- Core/Inc/dti.h | 15 +- Core/Inc/fault.h | 2 + Core/Inc/monitor.h | 9 + Core/Inc/mpu.h | 8 +- Core/Inc/nero.h | 2 + Core/Inc/pdu.h | 11 +- Core/Inc/queues.h | 4 + Core/Inc/stm32f4xx_hal_conf.h | 2 +- Core/Src/bms.c | 1 + Core/Src/can_handler.c | 13 +- Core/Src/dti.c | 110 +++++----- Core/Src/main.c | 35 ++-- Core/Src/monitor.c | 279 +++++++++++++++++-------- Core/Src/mpu.c | 11 +- Core/Src/nero.c | 8 +- Core/Src/pdu.c | 373 +++++++++++++++++----------------- Core/Src/state_machine.c | 34 +++- Core/Src/stm32f4xx_hal_msp.c | 35 +++- Core/Src/torque.c | 52 ++++- Drivers/Embedded-Base | 2 +- Makefile | 3 +- cerberus.ioc | 39 ++-- 24 files changed, 664 insertions(+), 405 deletions(-) diff --git a/.mxproject b/.mxproject index 59fb5666..f3909065 100644 --- a/.mxproject +++ b/.mxproject @@ -1,5 +1,5 @@ [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_can.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_iwdg.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_iwdg.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_tim.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_tim_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_uart.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_usart.h;Middlewares/Third_Party/FreeRTOS/Source/include/croutine.h;Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h;Middlewares/Third_Party/FreeRTOS/Source/include/event_groups.h;Middlewares/Third_Party/FreeRTOS/Source/include/FreeRTOS.h;Middlewares/Third_Party/FreeRTOS/Source/include/list.h;Middlewares/Third_Party/FreeRTOS/Source/include/message_buffer.h;Middlewares/Third_Party/FreeRTOS/Source/include/mpu_prototypes.h;Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h;Middlewares/Third_Party/FreeRTOS/Source/include/portable.h;Middlewares/Third_Party/FreeRTOS/Source/include/projdefs.h;Middlewares/Third_Party/FreeRTOS/Source/include/queue.h;Middlewares/Third_Party/FreeRTOS/Source/include/semphr.h;Middlewares/Third_Party/FreeRTOS/Source/include/stack_macros.h;Middlewares/Third_Party/FreeRTOS/Source/include/StackMacros.h;Middlewares/Third_Party/FreeRTOS/Source/include/stream_buffer.h;Middlewares/Third_Party/FreeRTOS/Source/include/task.h;Middlewares/Third_Party/FreeRTOS/Source/include/timers.h;Middlewares/Third_Party/FreeRTOS/Source/include/atomic.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/freertos_mpool.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/freertos_os2.h;Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.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_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_can.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_iwdg.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_uart.c;Middlewares/Third_Party/FreeRTOS/Source/croutine.c;Middlewares/Third_Party/FreeRTOS/Source/event_groups.c;Middlewares/Third_Party/FreeRTOS/Source/list.c;Middlewares/Third_Party/FreeRTOS/Source/queue.c;Middlewares/Third_Party/FreeRTOS/Source/stream_buffer.c;Middlewares/Third_Party/FreeRTOS/Source/tasks.c;Middlewares/Third_Party/FreeRTOS/Source/timers.c;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c;Middlewares/Third_Party/FreeRTOS/Source/portable/MemMang/heap_4.c;Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/port.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_can.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_iwdg.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_iwdg.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_tim.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_tim_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_uart.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_usart.h;Middlewares/Third_Party/FreeRTOS/Source/include/croutine.h;Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h;Middlewares/Third_Party/FreeRTOS/Source/include/event_groups.h;Middlewares/Third_Party/FreeRTOS/Source/include/FreeRTOS.h;Middlewares/Third_Party/FreeRTOS/Source/include/list.h;Middlewares/Third_Party/FreeRTOS/Source/include/message_buffer.h;Middlewares/Third_Party/FreeRTOS/Source/include/mpu_prototypes.h;Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h;Middlewares/Third_Party/FreeRTOS/Source/include/portable.h;Middlewares/Third_Party/FreeRTOS/Source/include/projdefs.h;Middlewares/Third_Party/FreeRTOS/Source/include/queue.h;Middlewares/Third_Party/FreeRTOS/Source/include/semphr.h;Middlewares/Third_Party/FreeRTOS/Source/include/stack_macros.h;Middlewares/Third_Party/FreeRTOS/Source/include/StackMacros.h;Middlewares/Third_Party/FreeRTOS/Source/include/stream_buffer.h;Middlewares/Third_Party/FreeRTOS/Source/include/task.h;Middlewares/Third_Party/FreeRTOS/Source/include/timers.h;Middlewares/Third_Party/FreeRTOS/Source/include/atomic.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/freertos_mpool.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/freertos_os2.h;Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h;Drivers/CMSIS/Device/ST/STM32F4xx/Include/stm32f405xx.h;Drivers/CMSIS/Device/ST/STM32F4xx/Include/stm32f4xx.h;Drivers/CMSIS/Device/ST/STM32F4xx/Include/system_stm32f4xx.h;Drivers/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c;Drivers/CMSIS/Include/cmsis_compiler.h;Drivers/CMSIS/Include/tz_context.h;Drivers/CMSIS/Include/core_cm1.h;Drivers/CMSIS/Include/cmsis_armcc.h;Drivers/CMSIS/Include/core_cm0plus.h;Drivers/CMSIS/Include/core_sc300.h;Drivers/CMSIS/Include/core_cm3.h;Drivers/CMSIS/Include/core_cm33.h;Drivers/CMSIS/Include/core_cm7.h;Drivers/CMSIS/Include/mpu_armv7.h;Drivers/CMSIS/Include/core_cm4.h;Drivers/CMSIS/Include/mpu_armv8.h;Drivers/CMSIS/Include/core_cm0.h;Drivers/CMSIS/Include/core_armv8mbl.h;Drivers/CMSIS/Include/cmsis_iccarm.h;Drivers/CMSIS/Include/cmsis_gcc.h;Drivers/CMSIS/Include/cmsis_armclang.h;Drivers/CMSIS/Include/core_sc000.h;Drivers/CMSIS/Include/cmsis_version.h;Drivers/CMSIS/Include/core_armv8mml.h;Drivers/CMSIS/Include/core_cm23.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_can.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_iwdg.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_iwdg.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_tim.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_tim_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_uart.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_usart.h;Middlewares/Third_Party/FreeRTOS/Source/include/croutine.h;Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h;Middlewares/Third_Party/FreeRTOS/Source/include/event_groups.h;Middlewares/Third_Party/FreeRTOS/Source/include/FreeRTOS.h;Middlewares/Third_Party/FreeRTOS/Source/include/list.h;Middlewares/Third_Party/FreeRTOS/Source/include/message_buffer.h;Middlewares/Third_Party/FreeRTOS/Source/include/mpu_prototypes.h;Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h;Middlewares/Third_Party/FreeRTOS/Source/include/portable.h;Middlewares/Third_Party/FreeRTOS/Source/include/projdefs.h;Middlewares/Third_Party/FreeRTOS/Source/include/queue.h;Middlewares/Third_Party/FreeRTOS/Source/include/semphr.h;Middlewares/Third_Party/FreeRTOS/Source/include/stack_macros.h;Middlewares/Third_Party/FreeRTOS/Source/include/StackMacros.h;Middlewares/Third_Party/FreeRTOS/Source/include/stream_buffer.h;Middlewares/Third_Party/FreeRTOS/Source/include/task.h;Middlewares/Third_Party/FreeRTOS/Source/include/timers.h;Middlewares/Third_Party/FreeRTOS/Source/include/atomic.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/freertos_mpool.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/freertos_os2.h;Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.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_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_can.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_iwdg.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_uart.c;Middlewares/Third_Party/FreeRTOS/Source/croutine.c;Middlewares/Third_Party/FreeRTOS/Source/event_groups.c;Middlewares/Third_Party/FreeRTOS/Source/list.c;Middlewares/Third_Party/FreeRTOS/Source/queue.c;Middlewares/Third_Party/FreeRTOS/Source/stream_buffer.c;Middlewares/Third_Party/FreeRTOS/Source/tasks.c;Middlewares/Third_Party/FreeRTOS/Source/timers.c;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c;Middlewares/Third_Party/FreeRTOS/Source/portable/MemMang/heap_4.c;Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/port.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_can.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_iwdg.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_iwdg.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_tim.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_tim_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_uart.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_usart.h;Middlewares/Third_Party/FreeRTOS/Source/include/croutine.h;Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h;Middlewares/Third_Party/FreeRTOS/Source/include/event_groups.h;Middlewares/Third_Party/FreeRTOS/Source/include/FreeRTOS.h;Middlewares/Third_Party/FreeRTOS/Source/include/list.h;Middlewares/Third_Party/FreeRTOS/Source/include/message_buffer.h;Middlewares/Third_Party/FreeRTOS/Source/include/mpu_prototypes.h;Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h;Middlewares/Third_Party/FreeRTOS/Source/include/portable.h;Middlewares/Third_Party/FreeRTOS/Source/include/projdefs.h;Middlewares/Third_Party/FreeRTOS/Source/include/queue.h;Middlewares/Third_Party/FreeRTOS/Source/include/semphr.h;Middlewares/Third_Party/FreeRTOS/Source/include/stack_macros.h;Middlewares/Third_Party/FreeRTOS/Source/include/StackMacros.h;Middlewares/Third_Party/FreeRTOS/Source/include/stream_buffer.h;Middlewares/Third_Party/FreeRTOS/Source/include/task.h;Middlewares/Third_Party/FreeRTOS/Source/include/timers.h;Middlewares/Third_Party/FreeRTOS/Source/include/atomic.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/freertos_mpool.h;Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/freertos_os2.h;Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h;Drivers/CMSIS/Device/ST/STM32F4xx/Include/stm32f405xx.h;Drivers/CMSIS/Device/ST/STM32F4xx/Include/stm32f4xx.h;Drivers/CMSIS/Device/ST/STM32F4xx/Include/system_stm32f4xx.h;Drivers/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c;Drivers/CMSIS/Include/mpu_armv7.h;Drivers/CMSIS/Include/core_cm23.h;Drivers/CMSIS/Include/mpu_armv8.h;Drivers/CMSIS/Include/core_cm4.h;Drivers/CMSIS/Include/core_cm0.h;Drivers/CMSIS/Include/cmsis_armcc.h;Drivers/CMSIS/Include/cmsis_version.h;Drivers/CMSIS/Include/core_cm0plus.h;Drivers/CMSIS/Include/core_sc300.h;Drivers/CMSIS/Include/core_armv8mml.h;Drivers/CMSIS/Include/tz_context.h;Drivers/CMSIS/Include/core_cm3.h;Drivers/CMSIS/Include/cmsis_compiler.h;Drivers/CMSIS/Include/core_sc000.h;Drivers/CMSIS/Include/cmsis_gcc.h;Drivers/CMSIS/Include/core_cm33.h;Drivers/CMSIS/Include/core_armv8mbl.h;Drivers/CMSIS/Include/core_cm1.h;Drivers/CMSIS/Include/core_cm7.h;Drivers/CMSIS/Include/cmsis_armclang.h;Drivers/CMSIS/Include/cmsis_iccarm.h; [PreviousUsedCubeIDEFiles] SourceFiles=Core\Src\main.c;Core\Src\freertos.c;Core\Src\stm32f4xx_it.c;Core\Src\stm32f4xx_hal_msp.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_can.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_tim.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_tim_ex.c;Middlewares\Third_Party\FreeRTOS\Source\croutine.c;Middlewares\Third_Party\FreeRTOS\Source\event_groups.c;Middlewares\Third_Party\FreeRTOS\Source\list.c;Middlewares\Third_Party\FreeRTOS\Source\queue.c;Middlewares\Third_Party\FreeRTOS\Source\stream_buffer.c;Middlewares\Third_Party\FreeRTOS\Source\tasks.c;Middlewares\Third_Party\FreeRTOS\Source\timers.c;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.c;Middlewares\Third_Party\FreeRTOS\Source\portable\MemMang\heap_4.c;Middlewares\Third_Party\FreeRTOS\Source\portable\GCC\ARM_CM4F\port.c;Drivers\CMSIS\Device\ST\STM32F4xx\Source\Templates\system_stm32f4xx.c;Core\Src\system_stm32f4xx.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_can.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_tim.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_tim_ex.c;Middlewares\Third_Party\FreeRTOS\Source\croutine.c;Middlewares\Third_Party\FreeRTOS\Source\event_groups.c;Middlewares\Third_Party\FreeRTOS\Source\list.c;Middlewares\Third_Party\FreeRTOS\Source\queue.c;Middlewares\Third_Party\FreeRTOS\Source\stream_buffer.c;Middlewares\Third_Party\FreeRTOS\Source\tasks.c;Middlewares\Third_Party\FreeRTOS\Source\timers.c;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.c;Middlewares\Third_Party\FreeRTOS\Source\portable\MemMang\heap_4.c;Middlewares\Third_Party\FreeRTOS\Source\portable\GCC\ARM_CM4F\port.c;Drivers\CMSIS\Device\ST\STM32F4xx\Source\Templates\system_stm32f4xx.c;Core\Src\system_stm32f4xx.c;;;Middlewares\Third_Party\FreeRTOS\Source\croutine.c;Middlewares\Third_Party\FreeRTOS\Source\event_groups.c;Middlewares\Third_Party\FreeRTOS\Source\list.c;Middlewares\Third_Party\FreeRTOS\Source\queue.c;Middlewares\Third_Party\FreeRTOS\Source\stream_buffer.c;Middlewares\Third_Party\FreeRTOS\Source\tasks.c;Middlewares\Third_Party\FreeRTOS\Source\timers.c;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.c;Middlewares\Third_Party\FreeRTOS\Source\portable\MemMang\heap_4.c;Middlewares\Third_Party\FreeRTOS\Source\portable\GCC\ARM_CM4F\port.c; diff --git a/Core/Inc/cerberus_conf.h b/Core/Inc/cerberus_conf.h index 2916f6be..59523746 100644 --- a/Core/Inc/cerberus_conf.h +++ b/Core/Inc/cerberus_conf.h @@ -5,20 +5,22 @@ #define FUSES_SAMPLE_DELAY 1000 /* ms */ #define SHUTDOWN_MONITOR_DELAY 500 /* ms */ #define NERO_DELAY_TIME 100 /* ms*/ +#define LV_READ_DELAY 1000 #define SERIAL_MONITOR_DELAY #define CAN_ROUTER_DELAY #define CAN_DISPATCH_DELAY 5 -#define BMS_CAN_MONITOR_DELAY 7000 +#define BMS_CAN_MONITOR_DELAY 4000 #define STATE_MACHINE_DELAY #define TORQUE_CALC_DELAY #define FAULT_HANDLE_DELAY /* Pedal tuning */ #define PEDALS_SAMPLE_DELAY 20 /* ms */ -#define ACCEL1_OFFSET 2767 -#define ACCEL1_MAX_VAL 3319 -#define ACCEL2_OFFSET 1920 -#define ACCEL2_MAX_VAL 3382 +#define ACCEL1_OFFSET 980 +#define ACCEL1_MAX_VAL 1866 +#define ACCEL2_OFFSET 1780 +#define ACCEL2_MAX_VAL 3365 +#define PEDAL_BRAKE_THRESH 650 /* Torque Tuning */ #define MAX_TORQUE 220 /* Nm * 10 */ @@ -50,16 +52,17 @@ #define WATCHDOG_Pin GPIO_PIN_15 #define WATCHDOG_GPIO_Port GPIOB -#define CANID_PEDAL_SENSOR 0x002 -// TODO: GET CORRECT CAN ID -#define CANID_IMU 0x003 + #define CANID_TEMP_SENSOR 0x004 #define CANID_TORQUE_MSG 0x005 #define CANID_OUTBOUND_MSG 0xA55 #define CANID_FUSE 0x111 #define CANID_SHUTDOWN_LOOP 0x123 +#define CANID_IMU_ACCEL 0x506 +#define CANID_IMU_GYRO 0x507 #define CANID_NERO_MSG 0x501 #define CANID_FAULT_MSG 0x502 +#define CANID_LV_MONITOR 0x503 #define CANID_PEDALS_ACCEL_MSG 0x504 #define CANID_PEDALS_BRAKE_MSG 0x505 #define CANID_EXTRA_MSG 0x701 // Reserved for MPU debug message, see yaml for format \ No newline at end of file diff --git a/Core/Inc/dti.h b/Core/Inc/dti.h index a71bcad1..28530a4d 100644 --- a/Core/Inc/dti.h +++ b/Core/Inc/dti.h @@ -22,9 +22,10 @@ #define DTI_CANID_TEMPS_FAULT 0x456 /* Controller Temp, Motor Temp, Faults */ #define DTI_CANID_ID_IQ 0x476 /* Id, Iq values */ #define DTI_CANID_SIGNALS 0x496 /* Throttle signal, Brake signal, IO, Drive enable */ +#define DTI_QUEUE_SIZE 5 -#define WHEEL_CIRCUMFERENCE 1.2767 /* meters */ -#define GEAR_RATIO 4.3 /* unitless */ +#define TIRE_DIAMETER 16 /* inches */ +#define GEAR_RATIO 47 / 13.0 /* unitless */ #define POLE_PAIRS 10 /* unitless */ typedef struct @@ -44,13 +45,13 @@ typedef struct } dti_t; // TODO: Expand GET interface -uint32_t dti_get_rpm(dti_t* dti); +int32_t dti_get_rpm(dti_t* dti); /* Utilities for Decoding CAN message */ -// extern osThreadId_t dti_router_handle; -// extern const osThreadAttr_t dti_router_attributes; -// extern osMessageQueueId_t dti_router_queue; -// void vDTIRouter(void* pv_params); +extern osThreadId_t dti_router_handle; +extern const osThreadAttr_t dti_router_attributes; +extern osMessageQueueId_t dti_router_queue; +void vDTIRouter(void* pv_params); dti_t* dti_init(); diff --git a/Core/Inc/fault.h b/Core/Inc/fault.h index 2b7bacc1..f8d3de48 100644 --- a/Core/Inc/fault.h +++ b/Core/Inc/fault.h @@ -21,6 +21,8 @@ typedef enum { INVALID_TRANSITION_FAULT = 0x400, BMS_CAN_MONITOR_FAULT = 0x800, BUTTONS_MONITOR_FAULT = 0xF00, + BSPD_PREFAULT = 0x1000, + LV_MONITOR_FAULT = 0x2000, MAX_FAULTS } fault_code_t; diff --git a/Core/Inc/monitor.h b/Core/Inc/monitor.h index 8541e7d9..87c0623e 100644 --- a/Core/Inc/monitor.h +++ b/Core/Inc/monitor.h @@ -5,6 +5,10 @@ #include "stm32f4xx_hal.h" #include "stdbool.h" +void vLVMonitor(void* pv_params); +extern osThreadId lv_monitor_handle; +extern const osThreadAttr_t lv_monitor_attributes; + /* Defining Temperature Monitor Task */ void vTempMonitor(void* pv_params); extern osThreadId_t temp_monitor_handle; @@ -25,6 +29,11 @@ void vIMUMonitor(void* pv_params); extern osThreadId_t imu_monitor_handle; extern const osThreadAttr_t imu_monitor_attributes; +/* Task for Monitoring the TSMS sense on PDU CTRL expander*/ +void vTsmsMonitor(void* pv_params); +extern osThreadId_t tsms_monitor_handle; +extern const osThreadAttr_t tsms_monitor_attributes; + /* Task for Monitoring the Fuses on PDU */ void vFusingMonitor(void* pv_params); extern osThreadId_t fusing_monitor_handle; diff --git a/Core/Inc/mpu.h b/Core/Inc/mpu.h index 06c8a378..750335d0 100644 --- a/Core/Inc/mpu.h +++ b/Core/Inc/mpu.h @@ -13,6 +13,10 @@ typedef struct { ADC_HandleTypeDef* pedals_adc; uint32_t pedal_dma_buf[4]; + + ADC_HandleTypeDef* lv_adc; + uint32_t lv_dma_buf; + GPIO_TypeDef* led_gpio; GPIO_TypeDef* watchdog_gpio; sht30_t* temp_sensor; @@ -29,7 +33,7 @@ typedef struct } brake_adc_channels_t; -mpu_t* init_mpu(I2C_HandleTypeDef* hi2c, ADC_HandleTypeDef* pedals_adc, GPIO_TypeDef* led_gpio, +mpu_t* init_mpu(I2C_HandleTypeDef* hi2c, ADC_HandleTypeDef* pedals_adc, ADC_HandleTypeDef* lv_adc, GPIO_TypeDef* led_gpio, GPIO_TypeDef* watchdog_gpio); int8_t write_rled(mpu_t* mpu, bool status); @@ -44,6 +48,8 @@ int8_t pet_watchdog(mpu_t* mpu); void read_pedals(mpu_t* mpu, uint32_t pedal_buf[4]); +void read_lv_voltage(mpu_t* mpu, uint32_t *lv_buf); + int8_t read_temp_sensor(mpu_t* mpu, uint16_t* temp, uint16_t* humidity); int8_t read_accel(mpu_t* mpu, uint16_t accel[3]); diff --git a/Core/Inc/nero.h b/Core/Inc/nero.h index d2aa2f7d..43c6e980 100644 --- a/Core/Inc/nero.h +++ b/Core/Inc/nero.h @@ -47,6 +47,8 @@ void select_nero_index(); */ void set_mph(int8_t new_mph); +void set_nero_home_mode(); + /* * Emits the state of NERO */ diff --git a/Core/Inc/pdu.h b/Core/Inc/pdu.h index 07970173..2a4394fa 100644 --- a/Core/Inc/pdu.h +++ b/Core/Inc/pdu.h @@ -2,16 +2,15 @@ #define PDU_H #include "cmsis_os.h" -#include "pi4ioe.h" -#include "max7314.h" +#include "pca9539.h" #include #include typedef struct { I2C_HandleTypeDef* hi2c; osMutexId_t* mutex; - max7314_t* shutdown_expander; - max7314_t* ctrl_expander; + pca9539_t* shutdown_expander; + pca9539_t* ctrl_expander; osTimerId rtds_timer; } pdu_t; @@ -40,7 +39,7 @@ typedef enum { MAX_FUSES } fuse_t; -int8_t read_fuse(pdu_t* pdu, fuse_t fuse, bool* status); +int8_t read_fuses(pdu_t* pdu, bool status[MAX_FUSES]); int8_t read_tsms_sense(pdu_t* pdu, bool* status); @@ -60,6 +59,6 @@ typedef enum { MAX_SHUTDOWN_STAGES } shutdown_stage_t; -int8_t read_shutdown(pdu_t* pdu, shutdown_stage_t stage, bool* status); +int8_t read_shutdown(pdu_t* pdu, bool status[MAX_SHUTDOWN_STAGES]); #endif /* PDU_H */ diff --git a/Core/Inc/queues.h b/Core/Inc/queues.h index a4517897..1af69c1b 100644 --- a/Core/Inc/queues.h +++ b/Core/Inc/queues.h @@ -28,4 +28,8 @@ typedef struct { extern osMessageQueueId_t pedal_data_queue; +extern osMessageQueueId_t break_state_queue; + +bool get_brake_state(); + #endif // QUEUES_H diff --git a/Core/Inc/stm32f4xx_hal_conf.h b/Core/Inc/stm32f4xx_hal_conf.h index f431bf12..e7df4a42 100644 --- a/Core/Inc/stm32f4xx_hal_conf.h +++ b/Core/Inc/stm32f4xx_hal_conf.h @@ -214,7 +214,7 @@ #define MAC_ADDR5 0U /* Definition of the Ethernet driver buffers size and count */ -#define ETH_RX_BUF_SIZE /* buffer size for receive */ +#define ETH_RX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for receive */ #define ETH_TX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for transmit */ #define ETH_RXBUFNB 4U /* 4 Rx buffers of size ETH_RX_BUF_SIZE */ #define ETH_TXBUFNB 4U /* 4 Tx buffers of size ETH_TX_BUF_SIZE */ diff --git a/Core/Src/bms.c b/Core/Src/bms.c index 29a16f7d..d248f605 100644 --- a/Core/Src/bms.c +++ b/Core/Src/bms.c @@ -27,6 +27,7 @@ void bms_fault_callback() { fault_data_t fault_data = { .id = BMS_CAN_MONITOR_FAULT, .severity = DEFCON1 }; /*TO-DO: update severity*/ fault_data.diag = "Failing To Receive CAN Messages from Shepherd"; + osTimerStart(bms->bms_monitor_timer, BMS_CAN_MONITOR_DELAY); queue_fault(&fault_data); } diff --git a/Core/Src/can_handler.c b/Core/Src/can_handler.c index 5b886a47..f298775c 100644 --- a/Core/Src/can_handler.c +++ b/Core/Src/can_handler.c @@ -21,6 +21,7 @@ #include #include "stdio.h" #include +#include "dti.h" #define CAN_MSG_QUEUE_SIZE 50 /* messages */ static osMessageQueueId_t can_outbound_queue; @@ -75,14 +76,16 @@ void can1_callback(CAN_HandleTypeDef* hcan) switch (new_msg.id) { /* Messages Relevant to Motor Controller */ case DTI_CANID_ERPM: - case DTI_CANID_CURRENTS: - case DTI_CANID_TEMPS_FAULT: - case DTI_CANID_ID_IQ: + // case DTI_CANID_CURRENTS: + // case DTI_CANID_TEMPS_FAULT: + // case DTI_CANID_ID_IQ: // case DTI_CANID_SIGNALS: - // osMessageQueuePut(dti_router_queue, &new_msg, 0U, 0U); - // break; + osMessageQueuePut(dti_router_queue, &new_msg, 0U, 0U); + break; case BMS_DCL_MSG: + //printf("Recieved dcl"); osMessageQueuePut(bms_monitor_queue, &new_msg, 0U, 0U); + break; default: break; } diff --git a/Core/Src/dti.c b/Core/Src/dti.c index 1c176608..275466de 100644 --- a/Core/Src/dti.c +++ b/Core/Src/dti.c @@ -35,10 +35,6 @@ dti_t* dti_init() mc->mutex = osMutexNew(&dti_mutex_attributes); assert(mc->mutex); - /* Create Queue for CAN signaling */ - // dti_router_queue = osMessageQueueNew(CAN_QUEUE_SIZE, sizeof(can_msg_t), NULL); - // assert(dti_router_queue); - return mc; } @@ -221,66 +217,66 @@ void dti_set_drive_enable(bool drive_enable) queue_can_msg(msg); } -uint32_t dti_get_rpm(dti_t* mc) +int32_t dti_get_rpm(dti_t* mc) { - uint32_t rpm; + int32_t rpm; osMutexAcquire(*mc->mutex, osWaitForever); rpm = mc->rpm; + //printf("Rpm %ld",rpm); osMutexRelease(*mc->mutex); return rpm; } /* Inbound Task-specific Info */ -// osThreadId_t dti_router_handle; -// const osThreadAttr_t dti_router_attributes -// = { .name = "DTIRouter", .stack_size = 64 * 8, .priority = (osPriority_t)osPriorityHigh }; - -// static void dti_set_rpm(dti_t *mc, can_msg_t msg) -// { -// /* ERPM is first four bytes of can message in big endian format */ -// uint32_t erpm = 0; -// for (int i = 0; i < 4; i++) { -// erpm |= msg.data[i] << (8 * (3 - i)); -// } - -// uint32_t rpm = erpm / POLE_PAIRS; - -// osMutexAcquire(*mc->mutex, osWaitForever); -// mc->rpm = rpm; -// osMutexRelease(*mc->mutex); -// } - -// void vDTIRouter(void* pv_params) -// { -// can_msg_t message; -// osStatus_t status; -// // fault_data_t fault_data = { .id = DTI_ROUTING_FAULT, .severity = DEFCON2 }; - -// dti_t *mc = (dti_t *)pv_params; - -// for (;;) { -// /* Wait until new CAN message comes into queue */ -// status = osMessageQueueGet(dti_router_queue, &message, NULL, osWaitForever); -// if (status == osOK) -// { -// switch (message.id) -// { -// case DTI_CANID_ERPM: -// dti_set_rpm(mc, message); -// break; -// case DTI_CANID_CURRENTS: -// break; -// case DTI_CANID_TEMPS_FAULT: -// break; -// case DTI_CANID_ID_IQ: -// break; -// case DTI_CANID_SIGNALS: -// break; -// default: -// break; -// } -// } -// } -// } +osThreadId_t dti_router_handle; +const osThreadAttr_t dti_router_attributes + = { .name = "DTIRouter", .stack_size = 64 * 8, .priority = (osPriority_t)osPriorityHigh }; + +static void dti_record_rpm(dti_t *mc, can_msg_t msg) +{ + /* ERPM is first four bytes of can message in big endian format */ + int32_t erpm = (msg.data[0] << 24) + (msg.data[1] << 16) + (msg.data[2] << 8) + (msg.data[3]); + + int32_t rpm = erpm / POLE_PAIRS; + + //printf("\n\nRPM Rec: %ld\n\n", rpm); + + osMutexAcquire(*mc->mutex, osWaitForever); + mc->rpm = rpm; + osMutexRelease(*mc->mutex); +} + +void vDTIRouter(void* pv_params) +{ + can_msg_t message; + osStatus_t status; + // fault_data_t fault_data = { .id = DTI_ROUTING_FAULT, .severity = DEFCON2 }; + + dti_t *mc = (dti_t *)pv_params; + + for (;;) { + /* Wait until new CAN message comes into queue */ + status = osMessageQueueGet(dti_router_queue, &message, NULL, osWaitForever); + if (status == osOK) + { + switch (message.id) + { + case DTI_CANID_ERPM: + dti_record_rpm(mc, message); + break; + case DTI_CANID_CURRENTS: + break; + case DTI_CANID_TEMPS_FAULT: + break; + case DTI_CANID_ID_IQ: + break; + case DTI_CANID_SIGNALS: + break; + default: + break; + } + } + } +} diff --git a/Core/Src/main.c b/Core/Src/main.c index 1a004af2..04b1c778 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -60,6 +60,7 @@ /* Private variables ---------------------------------------------------------*/ ADC_HandleTypeDef hadc1; ADC_HandleTypeDef hadc3; +DMA_HandleTypeDef hdma_adc1; DMA_HandleTypeDef hdma_adc3; CAN_HandleTypeDef hcan1; @@ -82,6 +83,8 @@ const osThreadAttr_t defaultTask_attributes = { osMessageQueueId_t brakelight_signal; osMessageQueueId_t pedal_data_queue; osMessageQueueId_t imu_queue; +osMessageQueueId_t dti_router_queue; + /* USER CODE END PV */ @@ -133,6 +136,7 @@ int _write(int file, char* ptr, int len) { */ int main(void) { + /* USER CODE BEGIN 1 */ printf("BOOT\r\n"); /* USER CODE END 1 */ @@ -176,7 +180,7 @@ int main(void) /* I'm kinda defining mutexes here lol */ /* Create Interfaces to Represent Relevant Hardware */ - mpu_t *mpu = init_mpu(&hi2c1, &hadc3, GPIOC, GPIOB); + mpu_t *mpu = init_mpu(&hi2c1, &hadc3, &hadc1, GPIOC, GPIOB); pdu_t *pdu = init_pdu(&hi2c2); assert(pdu); dti_t *mc = dti_init(); @@ -200,6 +204,8 @@ int main(void) brakelight_signal = osMessageQueueNew(15, sizeof(bool), NULL); imu_queue = osMessageQueueNew(IMU_QUEUE_SIZE, sizeof(imu_data_t), NULL); pedal_data_queue = osMessageQueueNew(PEDAL_DATA_QUEUE_SIZE, sizeof(pedals_t), NULL); + dti_router_queue = osMessageQueueNew(DTI_QUEUE_SIZE, sizeof(can_msg_t), NULL); + assert(dti_router_queue); /* USER CODE END RTOS_QUEUES */ /* Create the thread(s) */ @@ -208,21 +214,25 @@ int main(void) /* USER CODE BEGIN RTOS_THREADS */ /* Monitors */ + lv_monitor_handle = osThreadNew(vLVMonitor, mpu, &lv_monitor_attributes); + assert(lv_monitor_handle); // temp_monitor_handle = osThreadNew(vTempMonitor, mpu, &temp_monitor_attributes); - //assert(temp_monitor_handle); + // assert(temp_monitor_handle); //imu_monitor_handle = osThreadNew(vIMUMonitor, mpu, &imu_monitor_attributes); //assert(imu_monitor_handle); steeringio_buttons_monitor_handle = osThreadNew(vSteeringIOButtonsMonitor, wheel, &steeringio_buttons_monitor_attributes); pedals_monitor_handle = osThreadNew(vPedalsMonitor, mpu, &pedals_monitor_attributes); assert(pedals_monitor_handle); + tsms_monitor_handle = osThreadNew(vTsmsMonitor, pdu, &tsms_monitor_attributes); + assert(tsms_monitor_handle); fusing_monitor_handle = osThreadNew(vFusingMonitor, pdu, &fusing_monitor_attributes); assert(fusing_monitor_handle); - shutdown_monitor_handle = osThreadNew(vShutdownMonitor, pdu, &shutdown_monitor_attributes); - assert(shutdown_monitor_handle); + // shutdown_monitor_handle = osThreadNew(vShutdownMonitor, pdu, &shutdown_monitor_attributes); + // assert(shutdown_monitor_handle); /* Messaging */ - //dti_router_handle = osThreadNew(vDTIRouter, mc, &dti_router_attributes); - //assert(dti_router_handle); + dti_router_handle = osThreadNew(vDTIRouter, mc, &dti_router_attributes); + assert(dti_router_handle); can_dispatch_handle = osThreadNew(vCanDispatch, NULL, &can_dispatch_attributes); assert(can_dispatch_handle); bms_monitor_handle = osThreadNew(vBMSCANMonitor, NULL, &bms_monitor_attributes); @@ -252,6 +262,7 @@ int main(void) osKernelStart(); /* We should never get here as control is now taken by the scheduler */ + /* Infinite loop */ /* USER CODE BEGIN WHILE */ while (1) @@ -330,14 +341,14 @@ static void MX_ADC1_Init(void) hadc1.Instance = ADC1; hadc1.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV6; hadc1.Init.Resolution = ADC_RESOLUTION_12B; - hadc1.Init.ScanConvMode = DISABLE; - hadc1.Init.ContinuousConvMode = DISABLE; + hadc1.Init.ScanConvMode = ENABLE; + hadc1.Init.ContinuousConvMode = ENABLE; hadc1.Init.DiscontinuousConvMode = DISABLE; hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START; hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT; hadc1.Init.NbrOfConversion = 1; - hadc1.Init.DMAContinuousRequests = DISABLE; + hadc1.Init.DMAContinuousRequests = ENABLE; hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV; if (HAL_ADC_Init(&hadc1) != HAL_OK) { @@ -346,7 +357,7 @@ static void MX_ADC1_Init(void) /** Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time. */ - sConfig.Channel = ADC_CHANNEL_15; + sConfig.Channel = ADC_CHANNEL_8; sConfig.Rank = 1; sConfig.SamplingTime = ADC_SAMPLETIME_3CYCLES; if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) @@ -654,8 +665,8 @@ static void MX_GPIO_Init(void) GPIO_InitStruct.Pull = GPIO_PULLUP; HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); - /*Configure GPIO pins : PB0 PB1 */ - GPIO_InitStruct.Pin = GPIO_PIN_0|GPIO_PIN_1; + /*Configure GPIO pin : PB1 */ + GPIO_InitStruct.Pin = GPIO_PIN_1; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Pull = GPIO_PULLUP; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); diff --git a/Core/Src/monitor.c b/Core/Src/monitor.c index ea92c243..4775036d 100644 --- a/Core/Src/monitor.c +++ b/Core/Src/monitor.c @@ -1,32 +1,72 @@ #include "monitor.h" +#include "c_utils.h" #include "can_handler.h" #include "cerberus_conf.h" #include "fault.h" +#include "lsm6dso.h" #include "mpu.h" #include "pdu.h" #include "queues.h" #include "serial_monitor.h" #include "sht30.h" -#include "c_utils.h" +#include "state_machine.h" +#include "steeringio.h" #include "stm32f405xx.h" #include "task.h" -#include "lsm6dso.h" #include "timer.h" -#include "serial_monitor.h" -#include "state_machine.h" -#include "steeringio.h" #include -#include #include #include +#include /* Parameters for the pedal monitoring task */ -#define MAX_ADC_VAL_12b 4096 +#define MAX_ADC_VAL_12b 4096 // DEBUG: threshold may need adjusting #define PEDAL_DIFF_THRESH 30 #define PEDAL_FAULT_TIME 500 /* ms */ + static bool tsms = false; +static bool brake_state = false; + + +osThreadId lv_monitor_handle; +const osThreadAttr_t lv_monitor_attributes = { + .name = "LVMonitor", + .stack_size = 32 * 8, + .priority = (osPriority_t)osPriorityHigh4, +}; + +void vLVMonitor(void* pv_params) +{ + mpu_t* mpu = (mpu_t*)pv_params; + fault_data_t fault_data = { .id = LV_MONITOR_FAULT, .severity = DEFCON5 }; + can_msg_t msg = { .id = CANID_LV_MONITOR, .len = 4, .data = { 0 } }; + + uint32_t v_int; + + for (;;) { + read_lv_voltage(mpu, &v_int); + + // scale up then truncate + // convert to out of 24 volts + // since 12 bits / 4096 + // Magic number bc idk the resistors on the voltage divider + float v_dec = v_int * 8.967; + + // get final voltage + v_int = (uint32_t) (v_dec *10.0); + + //endian_swap(&v_int, sizeof(v_int)); + memcpy(msg.data, &v_int, msg.len); + if (queue_can_msg(msg)) { + fault_data.diag = "Failed to send steering buttons can message"; + queue_fault(&fault_data); + } + + osDelay(LV_READ_DELAY); + } +} osThreadId_t temp_monitor_handle; const osThreadAttr_t temp_monitor_attributes = { @@ -35,12 +75,17 @@ const osThreadAttr_t temp_monitor_attributes = { .priority = (osPriority_t)osPriorityHigh1, }; +bool get_brake_state() +{ + return brake_state; +} + void vTempMonitor(void* pv_params) { fault_data_t fault_data = { .id = ONBOARD_TEMP_FAULT, .severity = DEFCON5 }; can_msg_t temp_msg = { .id = CANID_TEMP_SENSOR, .len = 4, .data = { 0 } }; - mpu_t *mpu = (mpu_t *)pv_params; + mpu_t* mpu = (mpu_t*)pv_params; for (;;) { /* Take measurement */ @@ -76,17 +121,20 @@ const osThreadAttr_t pedals_monitor_attributes = { .priority = (osPriority_t)osPriorityRealtime1, }; -void eval_pedal_fault(uint16_t accel_1, uint16_t accel_2, nertimer_t *diff_timer, nertimer_t *sc_timer, nertimer_t *oc_timer, fault_data_t *fault_data) +void eval_pedal_fault(uint16_t accel_1, uint16_t accel_2, nertimer_t* diff_timer, + nertimer_t* sc_timer, nertimer_t* oc_timer, fault_data_t* fault_data) { /* Fault - open circuit (Max ADC value + some a lil bit) */ - if ((accel_1 > (MAX_ADC_VAL_12b - 20) || accel_2 > (MAX_ADC_VAL_12b - 20)) && is_timer_active(oc_timer)) { + if ((accel_1 > (MAX_ADC_VAL_12b - 20) || accel_2 > (MAX_ADC_VAL_12b - 20)) + && is_timer_active(oc_timer)) { if (is_timer_expired(oc_timer)) { if ((accel_1 == MAX_ADC_VAL_12b || accel_2 == MAX_ADC_VAL_12b)) { fault_data->diag = "Pedal open circuit fault - max acceleration value "; queue_fault(fault_data); } } - } else if ((accel_1 > (MAX_ADC_VAL_12b - 20) || accel_2 > (MAX_ADC_VAL_12b - 20)) && !is_timer_active(oc_timer)) { + } else if ((accel_1 > (MAX_ADC_VAL_12b - 20) || accel_2 > (MAX_ADC_VAL_12b - 20)) + && !is_timer_active(oc_timer)) { start_timer(oc_timer, PEDAL_FAULT_TIME); } else { cancel_timer(oc_timer); @@ -94,8 +142,10 @@ void eval_pedal_fault(uint16_t accel_1, uint16_t accel_2, nertimer_t *diff_timer /* Fault - short circuit */ if ((accel_1 < 500 || accel_2 < 500) && is_timer_active(sc_timer)) { + if (is_timer_expired(sc_timer)) { - if ((accel_1 < 500 || accel_2 < 500 )) { + + if ((accel_1 < 500 || accel_2 < 500)) { fault_data->diag = "Pedal short circuit fault - no acceleration value "; queue_fault(fault_data); } @@ -112,6 +162,7 @@ void eval_pedal_fault(uint16_t accel_1, uint16_t accel_2, nertimer_t *diff_timer /* Fault - difference between pedal sensing values */ if ((abs(accel_1_norm - accel_2_norm) > PEDAL_DIFF_THRESH) && is_timer_active(diff_timer)) { + /* starting diff timer */ if (is_timer_expired(diff_timer)) { if ((abs(accel_1_norm - accel_2_norm) > PEDAL_DIFF_THRESH)) { @@ -119,7 +170,8 @@ void eval_pedal_fault(uint16_t accel_1, uint16_t accel_2, nertimer_t *diff_timer queue_fault(fault_data); } } - } else if ((abs(accel_1_norm - accel_2_norm) > PEDAL_DIFF_THRESH) && !is_timer_active(diff_timer)) { + } else if ((abs(accel_1_norm - accel_2_norm) > PEDAL_DIFF_THRESH) + && !is_timer_active(diff_timer)) { start_timer(diff_timer, PEDAL_FAULT_TIME); } else { cancel_timer(diff_timer); @@ -129,12 +181,13 @@ void eval_pedal_fault(uint16_t accel_1, uint16_t accel_2, nertimer_t *diff_timer void vPedalsMonitor(void* pv_params) { const uint8_t num_samples = 10; - enum {ACCELPIN_2, ACCELPIN_1, BRAKEPIN_1, BRAKEPIN_2}; + static const uint8_t buffer_size = 10; + enum { ACCELPIN_2, ACCELPIN_1, BRAKEPIN_1, BRAKEPIN_2 }; static pedals_t sensor_data; - fault_data_t fault_data = { .id = ONBOARD_PEDAL_FAULT, .severity = DEFCON1 }; - can_msg_t accel_pedals_msg = { .id = CANID_PEDALS_ACCEL_MSG, .len = 8, .data = { 0 } }; - can_msg_t brake_pedals_msg = { .id = CANID_PEDALS_BRAKE_MSG, .len = 8, .data = { 0 } }; + fault_data_t fault_data = { .id = ONBOARD_PEDAL_FAULT, .severity = DEFCON1 }; + can_msg_t accel_pedals_msg = { .id = CANID_PEDALS_ACCEL_MSG, .len = 8, .data = { 0 } }; + can_msg_t brake_pedals_msg = { .id = CANID_PEDALS_BRAKE_MSG, .len = 8, .data = { 0 } }; uint32_t adc_data[4]; bool is_braking = false; @@ -147,10 +200,10 @@ void vPedalsMonitor(void* pv_params) cancel_timer(&oc_timer_accelerator); /* Handle ADC Data for two input accelerator value and two input brake value*/ - mpu_t *mpu = (mpu_t *)pv_params; - + mpu_t* mpu = (mpu_t*)pv_params; uint8_t counter = 0; + static int index = 0; for (;;) { read_pedals(mpu, adc_data); @@ -158,33 +211,57 @@ void vPedalsMonitor(void* pv_params) /* Evaluate Pedal Faulting Conditions */ eval_pedal_fault(adc_data[ACCELPIN_1], adc_data[ACCELPIN_2], &diff_timer_accelerator, &sc_timer_accelerator, &oc_timer_accelerator, &fault_data); //eval_pedal_fault(adc_data[BRAKEPIN_1], adc_data[BRAKEPIN_1], &diff_timer_brake, &sc_timer_brake, &oc_timer_brake, &fault_data); - + /* Offset adjusted per pedal sensor, clamp to be above 0 */ uint16_t accel_val1 = (int16_t)adc_data[ACCELPIN_1] - ACCEL1_OFFSET <= 0 ? 0 : (uint16_t)(adc_data[ACCELPIN_1] - ACCEL1_OFFSET) * 100 / (ACCEL1_MAX_VAL - ACCEL1_OFFSET); - //printf("Accel 1: %d\r\n", max_pedal1); + // printf("Accel 1: %d\r\n", max_pedal1); uint16_t accel_val2 = (int16_t)adc_data[ACCELPIN_2] - ACCEL2_OFFSET <= 0 ? 0 : (uint16_t)(adc_data[ACCELPIN_2] - ACCEL2_OFFSET) * 100 / (ACCEL2_MAX_VAL - ACCEL2_OFFSET); - //printf("Accel 2: %d\r\n",max_pedal2); + // printf("Accel 2: %d\r\n",max_pedal2); uint16_t accel_val = (uint16_t)(accel_val1 + accel_val2) / 2; //printf("Avg Pedal Val: %d\r\n\n", accel_val); + /* Raw ADC for tuning */ + //printf("Accel 1: %ld\r\n", adc_data[ACCELPIN_1]); + //printf("Accel 2: %ld\r\n", adc_data[ACCELPIN_2]); + /* Brakelight Control */ - //printf("Brake 1: %ld\r\n", adc_data[BRAKEPIN_1]); - //printf("Brake 2: %ld\r\n", adc_data[BRAKEPIN_2]); + // printf("Brake 1: %ld\r\n", adc_data[BRAKEPIN_1]); + // printf("Brake 2: %ld\r\n", adc_data[BRAKEPIN_2]); + + static float buffer[10] = {0}; + + uint16_t brake_avg = (adc_data[BRAKEPIN_1] + adc_data[BRAKEPIN_2]) / 2; + // Add the new value to the buffer + buffer[index] = brake_avg; - is_braking = (adc_data[BRAKEPIN_1] + adc_data[BRAKEPIN_2]) / 2 > 650; + // Increment the index, wrapping around if necessary + index = (index + 1) % buffer_size; + + // Calculate the average of the buffer + float sum = 0.0; + for (int i = 0; i < buffer_size; ++i) { + sum += buffer[i]; + } + float average_brake = sum / buffer_size; + + is_braking = average_brake > PEDAL_BRAKE_THRESH; + brake_state = is_braking; osMessageQueuePut(brakelight_signal, &is_braking, 0U, 0U); + //osMessageQueueReset(break_state_queue); + //osMessageQueuePut(break_state_queue, &is_braking, 0U, 0U); /* Low Pass Filter */ sensor_data.accelerator_value = (sensor_data.accelerator_value + (accel_val)) / num_samples; - sensor_data.brake_value = (sensor_data.brake_value + (adc_data[BRAKEPIN_1] + adc_data[BRAKEPIN_2]) / 2) / num_samples; + sensor_data.brake_value + = average_brake / 10.0; // still divide by 10 since we multiple by 10 on other end /* Publish to Onboard Pedals Queue */ + //printf("Accel pedal queue %d", sensor_data.accelerator_value); osStatus_t check = osMessageQueuePut(pedal_data_queue, &sensor_data, 0U, 0U); - if(check != 0) - { + if (check != 0) { fault_data.diag = "Failed to push pedal data to queue"; queue_fault(&fault_data); } @@ -193,17 +270,17 @@ void vPedalsMonitor(void* pv_params) counter += 1; if (counter >= 5) { counter = 0; - endian_swap(&adc_data[ACCELPIN_1], sizeof(adc_data[ACCELPIN_1])); - endian_swap(&adc_data[ACCELPIN_2], sizeof(adc_data[ACCELPIN_2])); - memcpy(accel_pedals_msg.data, &adc_data, accel_pedals_msg.len); - queue_can_msg(accel_pedals_msg); - - endian_swap(&adc_data[BRAKEPIN_1], sizeof(adc_data[BRAKEPIN_1])); - endian_swap(&adc_data[BRAKEPIN_2], sizeof(adc_data[BRAKEPIN_2])); - memcpy(brake_pedals_msg.data, adc_data + 2, brake_pedals_msg.len); - queue_can_msg(brake_pedals_msg); + endian_swap(&adc_data[ACCELPIN_1], sizeof(adc_data[ACCELPIN_1])); + endian_swap(&adc_data[ACCELPIN_2], sizeof(adc_data[ACCELPIN_2])); + memcpy(accel_pedals_msg.data, &adc_data, accel_pedals_msg.len); + queue_can_msg(accel_pedals_msg); + + endian_swap(&adc_data[BRAKEPIN_1], sizeof(adc_data[BRAKEPIN_1])); + endian_swap(&adc_data[BRAKEPIN_2], sizeof(adc_data[BRAKEPIN_2])); + memcpy(brake_pedals_msg.data, adc_data + 2, brake_pedals_msg.len); + queue_can_msg(brake_pedals_msg); } - + osDelay(PEDALS_SAMPLE_DELAY); } } @@ -220,17 +297,17 @@ void vIMUMonitor(void* pv_params) const uint8_t num_samples = 10; static imu_data_t sensor_data; fault_data_t fault_data = { .id = IMU_FAULT, .severity = DEFCON5 }; - can_msg_t imu_accel_msg = { .id = CANID_IMU, .len = 6, .data = { 0 } }; - can_msg_t imu_gyro_msg = { .id = CANID_IMU, .len = 6, .data = { 0 } }; + can_msg_t imu_accel_msg = { .id = CANID_IMU_ACCEL, .len = 6, .data = { 0 } }; + can_msg_t imu_gyro_msg = { .id = CANID_IMU_GYRO, .len = 6, .data = { 0 } }; - mpu_t *mpu = (mpu_t *)pv_params; + mpu_t* mpu = (mpu_t*)pv_params; for (;;) { // serial_print("IMU Task\r\n"); /* Take measurement */ uint16_t accel_data[3] = { 0 }; uint16_t gyro_data[3] = { 0 }; - if (read_accel(mpu,accel_data)) { + if (read_accel(mpu, accel_data)) { fault_data.diag = "Failed to get IMU acceleration"; queue_fault(&fault_data); } @@ -261,10 +338,10 @@ void vIMUMonitor(void* pv_params) /* Send CAN message */ memcpy(imu_accel_msg.data, &sensor_data, imu_accel_msg.len); - //if (queue_can_msg(imu_accel_msg)) { + // if (queue_can_msg(imu_accel_msg)) { // fault_data.diag = "Failed to send CAN message"; // queue_fault(&fault_data); - //} + // } memcpy(imu_gyro_msg.data, &sensor_data, imu_gyro_msg.len); if (queue_can_msg(imu_gyro_msg)) { @@ -277,6 +354,48 @@ void vIMUMonitor(void* pv_params) } } +osThreadId_t tsms_monitor_handle; +const osThreadAttr_t tsms_monitor_attributes = { + .name = "TsmsMonitor", + .stack_size = 32 * 8, + .priority = (osPriority_t)osPriorityHigh, +}; + +void vTsmsMonitor(void *pv_params) { + fault_data_t fault_data = { .id = FUSE_MONITOR_FAULT, .severity = DEFCON5 }; + pdu_t* pdu = (pdu_t*)pv_params; + + bool tsms_status = false; + nertimer_t tsms_debounce_timer = { .active = false }; + + for (;;) { + /* If we got a reliable TSMS reading, handle transition to and out of ACTIVE*/ + if(!read_tsms_sense(pdu, &tsms_status)) { + printf("Checking pdu"); + + // Timer has not been started, and there is a change in TSMS, so start the timer + if (tsms != tsms_status && !is_timer_active(&tsms_debounce_timer)) { + start_timer(&tsms_debounce_timer, 500); + } + // During debouncing, the tsms reading changes, so end the debounce period + else if (tsms == tsms_status && !is_timer_expired(&tsms_debounce_timer)) { + cancel_timer(&tsms_debounce_timer); + } + // The TSMS reading has been consistent thorughout the debounce period + else if (is_timer_expired(&tsms_debounce_timer)) { + tsms = tsms_status; + } + if (get_func_state() == ACTIVE && tsms == false) { + set_home_mode(); + } + } else { + queue_fault(&fault_data); + } + + osDelay(100); + } +} + osThreadId_t fusing_monitor_handle; const osThreadAttr_t fusing_monitor_attributes = { .name = "FusingMonitor", @@ -289,26 +408,30 @@ void vFusingMonitor(void* pv_params) fault_data_t fault_data = { .id = FUSE_MONITOR_FAULT, .severity = DEFCON5 }; can_msg_t fuse_msg = { .id = CANID_FUSE, .len = 2, .data = { 0 } }; pdu_t* pdu = (pdu_t*)pv_params; - bool fuses[MAX_FUSES] = { 0 }; uint16_t fuse_buf; + bool fuses[MAX_FUSES] + = { 0 }; - struct __attribute__((__packed__)){ - uint8_t fuse_1; - uint8_t fuse_2; - } fuse_data; + struct __attribute__((__packed__)) { + uint8_t fuse_1; + uint8_t fuse_2; + } fuse_data; for (;;) { fuse_buf = 0; - for (fuse_t fuse = 0; fuse < MAX_FUSES; fuse++) { - read_fuse(pdu, fuse, &fuses[fuse]); /* Actually read the fuse */ + if (read_fuses(pdu, fuses)) { + fault_data.diag = "Failed to read fuses"; + queue_fault(&fault_data); + } + for (fuse_t fuse = 0; fuse < MAX_FUSES; fuse++) { fuse_buf |= fuses[fuse] << fuse; /* Sets the bit at position `fuse` to the state of the fuse */ } // serial_print("Fuses:\t%X\r\n", fuse_buf); - + fuse_data.fuse_1 = fuse_buf & 0xFF; fuse_data.fuse_2 = (fuse_buf >> 8) & 0xFF; @@ -316,13 +439,13 @@ void vFusingMonitor(void* pv_params) fuse_data.fuse_1 = reverse_bits(fuse_data.fuse_1); fuse_data.fuse_2 = reverse_bits(fuse_data.fuse_2); - memcpy(fuse_msg.data, &fuse_data, fuse_msg.len); if (queue_can_msg(fuse_msg)) { fault_data.diag = "Failed to send CAN message"; queue_fault(&fault_data); } + osDelay(FUSES_SAMPLE_DELAY); } } @@ -341,20 +464,21 @@ void vShutdownMonitor(void* pv_params) pdu_t* pdu = (pdu_t*)pv_params; bool shutdown_loop[MAX_SHUTDOWN_STAGES] = { 0 }; uint16_t shutdown_buf; - bool tsms_status = false; - struct __attribute__((__packed__)){ - uint8_t shut_1; - uint8_t shut_2; - } shutdown_data ; + struct __attribute__((__packed__)) { + uint8_t shut_1; + uint8_t shut_2; + } shutdown_data; for (;;) { shutdown_buf = 0; - for (shutdown_stage_t stage = 0; stage < MAX_SHUTDOWN_STAGES; stage++) { - read_shutdown(pdu, stage, &shutdown_loop[stage]); /* Actually read the shutdown loop stage state */ - + if (read_shutdown(pdu,shutdown_loop)) { + fault_data.diag = "Failed to read shutdown buffer"; + queue_fault(&fault_data); + } + for (shutdown_stage_t stage = 0; stage < MAX_SHUTDOWN_STAGES; stage++) { shutdown_buf |= shutdown_loop[stage] << stage; /* Sets the bit at position `stage` to the state of the stage */ @@ -370,24 +494,13 @@ void vShutdownMonitor(void* pv_params) shutdown_data.shut_2 = reverse_bits(shutdown_data.shut_1); shutdown_data.shut_2 = reverse_bits(shutdown_data.shut_2); - memcpy(shutdown_msg.data, &shutdown_data, shutdown_msg.len); if (queue_can_msg(shutdown_msg)) { fault_data.diag = "Failed to send CAN message"; queue_fault(&fault_data); } - /* If we got a reliable TSMS reading, handle transition to and out of ACTIVE*/ - if(!read_tsms_sense(pdu, &tsms_status)) { - tsms = tsms_status; - if (get_func_state() == ACTIVE && tsms == 0) { - set_home_mode(); - } - } - - // serial_print("TSMS: %d\r\n", tsms_status); - - osDelay(SHUTDOWN_MONITOR_DELAY); + osDelay(SHUTDOWN_MONITOR_DELAY); } } @@ -401,8 +514,8 @@ const osThreadAttr_t steeringio_buttons_monitor_attributes = { void vSteeringIOButtonsMonitor(void* pv_params) { button_data_t buttons; - steeringio_t *wheel = (steeringio_t *)pv_params; - can_msg_t msg = { .id = 0x680, .len = 8, .data = { 0 } }; + steeringio_t* wheel = (steeringio_t*)pv_params; + can_msg_t msg = { .id = 0x680, .len = 8, .data = { 0 } }; fault_data_t fault_data = { .id = BUTTONS_MONITOR_FAULT, .severity = DEFCON5 }; for (;;) { @@ -415,20 +528,15 @@ void vSteeringIOButtonsMonitor(void* pv_params) uint8_t button_7 = !HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_0); uint8_t button_8 = !HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_1); - //serial_print("%d, %d, %d, %d, %d, %d, %d, %d \r\n", button_1, button_2, button_3, button_4, button_5, button_6, button_7, button_8); + // serial_print("%d, %d, %d, %d, %d, %d, %d, %d \r\n", button_1, button_2, button_3, + // button_4, button_5, button_6, button_7, button_8); // 1, 2, 0 ,0 , 0, 0 // serial_print("\r\n"); - uint8_t button_data = (button_1 << 7) | - (button_2 << 6) | - (button_3 << 5) | - (button_4 << 4) | - (button_5 << 3) | - (button_6 << 2) | - (button_7 << 1) | - (button_8); + uint8_t button_data = (button_1 << 7) | (button_2 << 6) | (button_3 << 5) | (button_4 << 4) + | (button_5 << 3) | (button_6 << 2) | (button_7 << 1) | (button_8); buttons.data[0] = button_data; @@ -445,7 +553,8 @@ void vSteeringIOButtonsMonitor(void* pv_params) } } -bool get_tsms() { +bool get_tsms() +{ return tsms; } diff --git a/Core/Src/mpu.c b/Core/Src/mpu.c index 02d545a7..428a51f1 100644 --- a/Core/Src/mpu.c +++ b/Core/Src/mpu.c @@ -14,11 +14,12 @@ static osMutexAttr_t mpu_i2c_mutex_attr; static osMutexAttr_t mpu_adc_mutex_attr; -mpu_t* init_mpu(I2C_HandleTypeDef* hi2c, ADC_HandleTypeDef* pedals_adc, +mpu_t* init_mpu(I2C_HandleTypeDef* hi2c, ADC_HandleTypeDef* pedals_adc, ADC_HandleTypeDef* lv_adc, GPIO_TypeDef* led_gpio, GPIO_TypeDef* watchdog_gpio) { assert(hi2c); assert(pedals_adc); + assert(lv_adc); assert(led_gpio); assert(watchdog_gpio); @@ -28,6 +29,7 @@ mpu_t* init_mpu(I2C_HandleTypeDef* hi2c, ADC_HandleTypeDef* pedals_adc, mpu->hi2c = hi2c; mpu->pedals_adc = pedals_adc; + mpu->lv_adc = lv_adc; mpu->led_gpio = led_gpio; mpu->watchdog_gpio = watchdog_gpio; @@ -39,6 +41,8 @@ mpu_t* init_mpu(I2C_HandleTypeDef* hi2c, ADC_HandleTypeDef* pedals_adc, assert(!HAL_ADC_Start_DMA(mpu->pedals_adc, mpu->pedal_dma_buf, sizeof(mpu->pedal_dma_buf)/sizeof(uint32_t))); + assert(!HAL_ADC_Start_DMA(mpu->lv_adc, &mpu->lv_dma_buf, sizeof(mpu->lv_dma_buf)/sizeof(uint32_t))); + /* Initialize the IMU */ mpu->imu = malloc(sizeof(lsm6dso_t)); assert(mpu->imu); @@ -102,6 +106,11 @@ int8_t pet_watchdog(mpu_t* mpu) return 0; } +void read_lv_voltage(mpu_t* mpu, uint32_t *lv_buf) +{ + memcpy(lv_buf, &mpu->lv_dma_buf, sizeof(mpu->lv_dma_buf)); +} + void read_pedals(mpu_t* mpu, uint32_t pedal_buf[4]) { memcpy(pedal_buf, mpu->pedal_dma_buf, sizeof(mpu->pedal_dma_buf)); diff --git a/Core/Src/nero.c b/Core/Src/nero.c index 8446335f..01677ea6 100644 --- a/Core/Src/nero.c +++ b/Core/Src/nero.c @@ -4,6 +4,7 @@ #include "can_handler.h" #include "state_machine.h" #include "serial_monitor.h" +#include "queues.h" #include "c_utils.h" #include "stdio.h" #include "cerberus_conf.h" @@ -48,6 +49,7 @@ void increment_nero_index() { } void set_mph(int8_t new_mph) { + //printf("mph %d", new_mph); mph = new_mph; } @@ -86,7 +88,7 @@ void select_nero_index() { uint8_t max_drive_states = MAX_DRIVE_STATES - 1; // Account for reverse and pit being the same screen - if (nero_state.nero_index > 0 && nero_state.nero_index < max_drive_states && get_tsms()) { + if (nero_state.nero_index > 0 && nero_state.nero_index < max_drive_states && get_tsms() && get_brake_state()) { state_request.id = FUNCTIONAL; state_request.state.functional = ACTIVE; queue_state_transition(state_request); @@ -104,6 +106,10 @@ void select_nero_index() { } } +void set_nero_home_mode() { + nero_state.home_mode = true; +} + void set_home_mode() { state_req_t state_request = {.id = FUNCTIONAL, .state.functional = READY}; if (!get_tsms()) { diff --git a/Core/Src/pdu.c b/Core/Src/pdu.c index 09af71cf..60bbce4c 100644 --- a/Core/Src/pdu.c +++ b/Core/Src/pdu.c @@ -1,41 +1,38 @@ #include "pdu.h" #include "serial_monitor.h" #include -#include #include +#include #define PUMP_CTRL 0 #define RADFAN_CTRL 1 #define BRKLIGHT_CTRL 2 #define BATBOXFAN_CTRL 3 -#define RTDS_CTRL 15 -#define TSMS_CTRL 0x04 -#define SMBALERT 0x05 -#define RTDS_CTRL 15 -#define MUTEX_TIMEOUT osWaitForever /* ms */ - -#define SHUTDOWN_ADDR 0x20 -#define CTRL_ADDR 0x24 -#define RTDS_DURATION 2500 +#define RTDS_CTRL 7 // PORT 17 BANK 1 (so read with 1_REG) +// #define TSMS_CTRL 0x04 +// #define SMBALERT 0x05 +#define MUTEX_TIMEOUT osWaitForever /* ms */ -#define PIN_MODE_OUTPUT 0 -#define PIN_MODE_INPUT 1 +#define SHUTDOWN_ADDR PCA_I2C_ADDR_3 +#define CTRL_ADDR PCA_I2C_ADDR_2 +#define RTDS_DURATION 2500 static osMutexAttr_t pdu_mutex_attributes; static void rtds_shutoff_cb(void* pv_params) { - pdu_t *pdu = (pdu_t *)pv_params; + pdu_t* pdu = (pdu_t*)pv_params; osStatus_t stat = osMutexAcquire(pdu->mutex, MUTEX_TIMEOUT); if (stat) return; /* write RTDS over i2c */ - HAL_StatusTypeDef error = max7314_set_pin_state(pdu->ctrl_expander, RTDS_CTRL, false); - if(error != HAL_OK) { - osMutexRelease(pdu->mutex); - return; - } + HAL_StatusTypeDef error + = pca9539_write_pin(pdu->ctrl_expander, PCA_OUTPUT_1_REG, RTDS_CTRL, false); + if (error != HAL_OK) { + osMutexRelease(pdu->mutex); + return; + } osMutexRelease(pdu->mutex); } @@ -51,55 +48,66 @@ pdu_t* init_pdu(I2C_HandleTypeDef* hi2c) pdu->hi2c = hi2c; /* Initialize Shutdown GPIO Expander */ - pdu->shutdown_expander = malloc(sizeof(max7314_t)); - assert(pdu->shutdown_expander); - pdu->shutdown_expander->dev_addr = SHUTDOWN_ADDR; - max7314_init(pdu->shutdown_expander, pdu->hi2c); - - // Blink disabled, global intensity enabled - uint8_t config_data = 0b01001100; - HAL_StatusTypeDef status = max7314_write_config(pdu->shutdown_expander, &config_data); - if (status != HAL_OK) { - printf("\n\rshutdown init fail\n\r"); - free(pdu->shutdown_expander); - free(pdu); - return NULL; - } + pdu->shutdown_expander = malloc(sizeof(pca9539_t)); + assert(pdu->shutdown_expander); + // pca9539_init(pdu->shutdown_expander, pdu->hi2c, SHUTDOWN_ADDR); + // if (status != HAL_OK) { + // printf("\n\rshutdown init fail\n\r"); + // free(pdu->shutdown_expander); + // free(pdu); + // return NULL; + // } + + // all shutdown expander things are inputs + // uint8_t shutdown_config_directions = 0b00000000; + // HAL_StatusTypeDef status = pca9539_write_reg(pdu->shutdown_expander, PCA_DIRECTION_0_REG, + // shutdown_config_directions); + // if (status != HAL_OK) { + // printf("\n\rshutdown write fail\n\r"); + // free(pdu->shutdown_expander); + // free(pdu); + // return NULL; + // } + // status + // = pca9539_write_reg(pdu->shutdown_expander, PCA_DIRECTION_1_REG, + // shutdown_config_directions); if (status != HAL_OK) { printf("\n\rshutdown wrtie 2 fail\n\r"); + // free(pdu->shutdown_expander); + // free(pdu); + // return NULL; + // } + + /* Initialize Control GPIO Expander */ - pdu->ctrl_expander = malloc(sizeof(max7314_t)); - assert(pdu->ctrl_expander); - pdu->ctrl_expander->dev_addr = CTRL_ADDR; - max7314_init(pdu->ctrl_expander, pdu->hi2c); - - /* Same as shutdown */ - status = max7314_write_config(pdu->ctrl_expander, &config_data); - if (status != HAL_OK) { - printf("\n\rcntrl init fail\n\r"); - free(pdu->shutdown_expander); - free(pdu); - return NULL; - } - - /* Set global intensity */ - status = max7314_set_global_intensity(pdu->ctrl_expander, 0); - if (status != HAL_OK) { - printf("\n\rSet global intensity fail\n\r"); + pdu->ctrl_expander = malloc(sizeof(pca9539_t)); + assert(pdu->ctrl_expander); + pca9539_init(pdu->ctrl_expander, pdu->hi2c, CTRL_ADDR); + + // write everything OFF, FAULT 1 is off + uint8_t buf = 0b00000010; + pca9539_write_reg(pdu->ctrl_expander, PCA_OUTPUT_0_REG, buf); + pca9539_write_reg(pdu->ctrl_expander, PCA_OUTPUT_1_REG, buf); + + // pin 0 to the right + buf = 0b11110000; + HAL_StatusTypeDef status + = pca9539_write_reg(pdu->ctrl_expander, PCA_DIRECTION_0_REG, buf); + if (status != HAL_OK) { + printf("\n\rcntrl init fail\n\r"); free(pdu->ctrl_expander); - free(pdu->shutdown_expander); - free(pdu); - return NULL; - } - - // set pins 15, 3, 2, 1, 0 to outputs - uint8_t pin_config[2] = {0b11110000, 0b01111111}; - if (max7314_set_pin_modes(pdu->ctrl_expander, pin_config)) { - printf("\n\rset pin modes fail\n\r"); - free(pdu->ctrl_expander); - free(pdu->shutdown_expander); - free(pdu); - return NULL; - } + free(pdu); + return NULL; + } + // pin 0 to the right + buf = 0b01111111; + status + = pca9539_write_reg(pdu->ctrl_expander, PCA_DIRECTION_1_REG, buf); + if (status != HAL_OK) { + printf("\n\rcntrl init fail\n\r"); + free(pdu->ctrl_expander); + free(pdu); + return NULL; + } /* Create Mutex */ pdu->mutex = osMutexNew(&pdu_mutex_attributes); @@ -107,17 +115,6 @@ pdu_t* init_pdu(I2C_HandleTypeDef* hi2c) pdu->rtds_timer = osTimerNew(&rtds_shutoff_cb, osTimerOnce, pdu, NULL); - assert(!max7314_set_pin_state(pdu->ctrl_expander, RTDS_CTRL, false)); - - // DEBUG To test RTDS - //sound_rtds(pdu); - - // DEBUG brakelight - //write_brakelight(pdu, true); - - // DEBUG pump - //write_pump(pdu, false); - return pdu; } @@ -133,11 +130,12 @@ int8_t write_pump(pdu_t* pdu, bool status) } /* write pump over i2c */ - HAL_StatusTypeDef error = max7314_set_pin_state(pdu->ctrl_expander, PUMP_CTRL, status); - if(error != HAL_OK) { - osMutexRelease(pdu->mutex); - return error; - } + HAL_StatusTypeDef error + = pca9539_write_pin(pdu->ctrl_expander, PCA_OUTPUT_0_REG, PUMP_CTRL, status); + if (error != HAL_OK) { + osMutexRelease(pdu->mutex); + return error; + } osMutexRelease(pdu->mutex); return 0; @@ -153,11 +151,12 @@ int8_t write_fault(pdu_t* pdu, bool status) return stat; /* write radiator over i2c */ - HAL_StatusTypeDef error = max7314_set_pin_state(pdu->ctrl_expander, RADFAN_CTRL, status); - if(error != HAL_OK) { - osMutexRelease(pdu->mutex); - return error; - } + HAL_StatusTypeDef error + = pca9539_write_pin(pdu->ctrl_expander, PCA_OUTPUT_0_REG, RADFAN_CTRL, status); + if (error != HAL_OK) { + osMutexRelease(pdu->mutex); + return error; + } osMutexRelease(pdu->mutex); return 0; @@ -173,11 +172,12 @@ int8_t write_brakelight(pdu_t* pdu, bool status) return stat; /* write brakelight over i2c */ - HAL_StatusTypeDef error = max7314_set_pin_state(pdu->ctrl_expander, BRKLIGHT_CTRL, status); - if(error != HAL_OK) { - osMutexRelease(pdu->mutex); - return error; - } + HAL_StatusTypeDef error + = pca9539_write_pin(pdu->ctrl_expander, PCA_OUTPUT_0_REG, BRKLIGHT_CTRL, status); + if (error != HAL_OK) { + osMutexRelease(pdu->mutex); + return error; + } osMutexRelease(pdu->mutex); return 0; @@ -193,11 +193,11 @@ int8_t write_fan_battbox(pdu_t* pdu, bool status) return stat; /* write fan over i2c */ - HAL_StatusTypeDef error = max7314_set_pin_state(pdu->ctrl_expander, BATBOXFAN_CTRL, status); - if(error != HAL_OK) { - osMutexRelease(pdu->mutex); - return error; - } + HAL_StatusTypeDef error = pca9539_write_pin(pdu->ctrl_expander, PCA_OUTPUT_0_REG, 2, status); + if (error != HAL_OK) { + osMutexRelease(pdu->mutex); + return error; + } osMutexRelease(pdu->mutex); return 0; @@ -205,7 +205,7 @@ int8_t write_fan_battbox(pdu_t* pdu, bool status) int8_t sound_rtds(pdu_t* pdu) { - if (!pdu) + if (!pdu) return -1; osStatus_t stat = osMutexAcquire(pdu->mutex, MUTEX_TIMEOUT); @@ -215,18 +215,26 @@ int8_t sound_rtds(pdu_t* pdu) osTimerStart(pdu->rtds_timer, RTDS_DURATION); /* write RTDS over i2c */ - HAL_StatusTypeDef error = max7314_set_pin_state(pdu->ctrl_expander, RTDS_CTRL, true); - if(error != HAL_OK) { - osMutexRelease(pdu->mutex); - return error; - } + HAL_StatusTypeDef error + = pca9539_write_pin(pdu->ctrl_expander, PCA_OUTPUT_1_REG, RTDS_CTRL, true); + if (error != HAL_OK) { + osMutexRelease(pdu->mutex); + return error; + } osMutexRelease(pdu->mutex); return 0; } -int8_t read_fuse(pdu_t* pdu, fuse_t fuse, bool* status) +static void deconstruct_buf(uint8_t data, bool config[8]) +{ + for (uint8_t i = 0; i < 8; i++) { + config[i] = (data >> i) & 1; + } +} + +int8_t read_fuses(pdu_t* pdu, bool status[MAX_FUSES]) { if (!pdu) return -1; @@ -235,45 +243,35 @@ int8_t read_fuse(pdu_t* pdu, fuse_t fuse, bool* status) if (stat) return stat; - uint16_t pin; - switch (fuse) - { - case FUSE_BATTBOX: - pin = 4; - break; - case FUSE_LVBOX: - pin = 5; - break; - case FUSE_FAN_RADIATOR: - pin = 6; - break; - case FUSE_MC: - pin = 7; - break; - case FUSE_FAN_BATTBOX: - pin = 8; - break; - case FUSE_PUMP: - pin = 9; - break; - case FUSE_DASHBOARD: - pin = 10; - break; - case FUSE_BRAKELIGHT: - pin = 11; - break; - case FUSE_BRB: - pin = 12; - break; - default: - return -1; + uint8_t bank0_d = 0; + HAL_StatusTypeDef error = pca9539_read_reg(pdu->ctrl_expander, PCA_INPUT_0_REG, &bank0_d); + if (error != HAL_OK) { + osMutexRelease(pdu->mutex); + return error; } - HAL_StatusTypeDef error = max7314_read_pin(pdu->ctrl_expander, pin, status); - if(error != HAL_OK) { - osMutexRelease(pdu->mutex); - return error; - } + uint8_t bank1_d = 0; + error = pca9539_read_reg(pdu->ctrl_expander, PCA_INPUT_1_REG, &bank1_d); + if (error != HAL_OK) { + osMutexRelease(pdu->mutex); + return error; + } + + bool bank0[8]; + deconstruct_buf(bank0_d, bank0); + + bool bank1[8]; + deconstruct_buf(bank1_d, bank1); + + status[FUSE_BATTBOX] = bank0[4]; + status[FUSE_LVBOX] = bank0[5]; + status[FUSE_FAN_RADIATOR] = bank0[6]; + status[FUSE_MC] = bank0[7]; + status[FUSE_FAN_BATTBOX] = bank1[0]; + status[FUSE_PUMP] = bank1[1]; + status[FUSE_DASHBOARD] = bank1[2]; + status[FUSE_BRAKELIGHT] = bank1[3]; + status[FUSE_BRB] = bank1[4]; osMutexRelease(pdu->mutex); return 0; @@ -289,18 +287,21 @@ int8_t read_tsms_sense(pdu_t* pdu, bool* status) return stat; /* read pin over i2c */ - uint8_t tsms_pin = 14; - HAL_StatusTypeDef error = max7314_read_pin(pdu->ctrl_expander, tsms_pin, status); - if(error != HAL_OK) { - osMutexRelease(pdu->mutex); - return error; - } + uint8_t tsms_pin = 6; + uint8_t config = 0; + HAL_StatusTypeDef error + = pca9539_read_pin(pdu->ctrl_expander, PCA_INPUT_1_REG, tsms_pin, &config); + if (error != HAL_OK) { + osMutexRelease(pdu->mutex); + return error; + } + *status = config; osMutexRelease(pdu->mutex); return 0; } -int8_t read_shutdown(pdu_t* pdu, shutdown_stage_t stage, bool* status) +int8_t read_shutdown(pdu_t* pdu, bool status[MAX_SHUTDOWN_STAGES]) { if (!pdu) return -1; @@ -309,54 +310,43 @@ int8_t read_shutdown(pdu_t* pdu, shutdown_stage_t stage, bool* status) if (stat) return stat; - uint16_t pin; - switch (stage) - { - case CKPT_BRB_CLR: - pin = 0; - break; - case BMS_OK: - pin = 2; - break; - case INERTIA_SW_OK: - pin = 3; - break; - case SPARE_GPIO1_OK: - pin = 4; - break; - case IMD_OK: - pin = 5; - break; - case BSPD_OK: - pin = 8; - break; - case BOTS_OK: - pin = 13; - break; - case HVD_INTLK_OK: - pin = 14; - break; - case HVC_INTLK_OK: - pin = 15; - break; - default: - return -1; + uint8_t bank0_d = 0; + HAL_StatusTypeDef error = pca9539_read_reg(pdu->shutdown_expander, PCA_INPUT_0_REG, &bank0_d); + if (error != HAL_OK) { + osMutexRelease(pdu->mutex); + return error; + } + + uint8_t bank1_d = 0; + error = pca9539_read_reg(pdu->shutdown_expander, PCA_INPUT_1_REG, &bank1_d); + if (error != HAL_OK) { + osMutexRelease(pdu->mutex); + return error; } - // read pin over i2c - HAL_StatusTypeDef error = max7314_read_pin(pdu->shutdown_expander, pin, status); - if(error != HAL_OK) { - osMutexRelease(pdu->mutex); - return error; - } + bool bank0[8]; + deconstruct_buf(bank0_d, bank0); + + bool bank1[8]; + deconstruct_buf(bank1_d, bank1); + + status[CKPT_BRB_CLR] = bank0[0]; + status[BMS_OK] = bank0[2]; + status[INERTIA_SW_OK] = bank0[3]; + status[SPARE_GPIO1_OK] = bank0[4]; + status[IMD_OK] = bank0[5]; + status[BSPD_OK] = bank1[0]; + status[BOTS_OK] = bank1[5]; + status[HVD_INTLK_OK] = bank1[6]; + status[HVC_INTLK_OK] = bank1[7]; osMutexRelease(pdu->mutex); return 0; } -int8_t write_rtds(pdu_t* pdu, bool status) +int8_t write_rtds(pdu_t* pdu, bool status) { - if (!pdu) + if (!pdu) return -1; osStatus_t stat = osMutexAcquire(pdu->mutex, MUTEX_TIMEOUT); @@ -364,11 +354,12 @@ int8_t write_rtds(pdu_t* pdu, bool status) return stat; /* write fan over i2c */ - HAL_StatusTypeDef error = max7314_set_pin_state(pdu->ctrl_expander, RTDS_CTRL, status); - if(error != HAL_OK) { - osMutexRelease(pdu->mutex); - return error; - } + HAL_StatusTypeDef error + = pca9539_write_pin(pdu->ctrl_expander, PCA_OUTPUT_1_REG, RTDS_CTRL, status); + if (error != HAL_OK) { + osMutexRelease(pdu->mutex); + return error; + } osMutexRelease(pdu->mutex); return 0; diff --git a/Core/Src/state_machine.c b/Core/Src/state_machine.c index 2ca22cbf..f2f24283 100644 --- a/Core/Src/state_machine.c +++ b/Core/Src/state_machine.c @@ -2,7 +2,9 @@ #include "fault.h" #include "can_handler.h" #include "serial_monitor.h" +#include "nero.h" #include "pdu.h" +#include "queues.h" #include #include #include @@ -41,6 +43,7 @@ const osThreadAttr_t sm_director_attributes = }; static osMessageQueueId_t state_trans_queue; +osMessageQueueId_t break_state_queue; void fault_callback() { @@ -66,6 +69,9 @@ int queue_state_transition(state_req_t new_state) /* If we are turning ON the motor, blare RTDS */ if (cerberus_state.drive == NOT_DRIVING) { + if (!get_brake_state()) { + return 0; + } serial_print("CALLING RTDS"); sound_rtds(pdu); } @@ -91,24 +97,31 @@ int queue_state_transition(state_req_t new_state) case READY: /* Turn off high power peripherals */ serial_print("going to ready"); - write_fan_battbox(pdu, false); + //write_fan_battbox(pdu, false); write_pump(pdu, false); write_fault(pdu, true); + cerberus_state.drive = OFF; + printf("READY\r\n"); break; case ACTIVE: /* Turn on high power peripherals */ - write_fan_battbox(pdu, true); + //write_fan_battbox(pdu, true); write_pump(pdu, true); write_fault(pdu, true); sound_rtds(pdu); // TEMPORARY + printf("ACTIVE STATE\r\n"); break; case FAULTED: /* Turn off high power peripherals */ - write_fan_battbox(pdu, true); + //write_fan_battbox(pdu, true); write_pump(pdu, false); write_fault(pdu, false); + // DO NOT CHANGE WITHOUT CHECKING the bms fault timer, as if BMS timer is later could result in a fault/unfault/fault flicker. osTimerStart(fault_timer, 5000); HAL_IWDG_Refresh(&hiwdg); + cerberus_state.drive = OFF; + set_nero_home_mode(); + printf("FAULTED\r\n"); break; default: // Do Nothing @@ -136,6 +149,7 @@ void vStateMachineDirector(void* pv_params) cerberus_state.drive = NOT_DRIVING; state_trans_queue = osMessageQueueNew(STATE_TRANS_QUEUE_SIZE, sizeof(state_req_t), NULL); + break_state_queue = osMessageQueueNew(1, sizeof(bool), NULL); pdu_t *pdu_1 = (pdu_t *)pv_params; @@ -169,6 +183,14 @@ void vStateMachineDirector(void* pv_params) /* If we are turning ON the motor, blare RTDS */ if (cerberus_state.drive == NOT_DRIVING) { + + /* make sure foot is on break */ + bool break_state = false; + //osMessageQueueGet(break_state_queue, &break_state, NULL, 0); + if (!break_state) { + continue; + } + serial_print("CALLING RTDS"); sound_rtds(pdu); } @@ -194,19 +216,19 @@ void vStateMachineDirector(void* pv_params) case READY: /* Turn off high power peripherals */ serial_print("going to ready"); - write_fan_battbox(pdu, false); + //write_fan_battbox(pdu, false); write_pump(pdu, true); write_fault(pdu, true); break; case ACTIVE: /* Turn on high power peripherals */ - write_fan_battbox(pdu, true); + //write_fan_battbox(pdu, true); write_pump(pdu, true); write_fault(pdu, true); break; case FAULTED: /* Turn off high power peripherals */ - write_fan_battbox(pdu, false); + //write_fan_battbox(pdu, false); write_pump(pdu, false); write_fault(pdu, false); HAL_IWDG_Refresh(&hiwdg); diff --git a/Core/Src/stm32f4xx_hal_msp.c b/Core/Src/stm32f4xx_hal_msp.c index dc083352..a5ba7a82 100644 --- a/Core/Src/stm32f4xx_hal_msp.c +++ b/Core/Src/stm32f4xx_hal_msp.c @@ -20,10 +20,11 @@ /* Includes ------------------------------------------------------------------*/ #include "main.h" - /* USER CODE BEGIN Includes */ /* USER CODE END Includes */ +extern DMA_HandleTypeDef hdma_adc1; + extern DMA_HandleTypeDef hdma_adc3; /* Private typedef -----------------------------------------------------------*/ @@ -64,6 +65,7 @@ extern DMA_HandleTypeDef hdma_adc3; */ void HAL_MspInit(void) { + /* USER CODE BEGIN MspInit 0 */ /* USER CODE END MspInit 0 */ @@ -98,14 +100,40 @@ void HAL_ADC_MspInit(ADC_HandleTypeDef* hadc) __HAL_RCC_ADC1_CLK_ENABLE(); __HAL_RCC_GPIOC_CLK_ENABLE(); + __HAL_RCC_GPIOB_CLK_ENABLE(); /**ADC1 GPIO Configuration PC5 ------> ADC1_IN15 + PB0 ------> ADC1_IN8 */ GPIO_InitStruct.Pin = GPIO_PIN_5; GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; GPIO_InitStruct.Pull = GPIO_PULLUP; HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); + GPIO_InitStruct.Pin = GPIO_PIN_0; + GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + /* ADC1 DMA Init */ + /* ADC1 Init */ + hdma_adc1.Instance = DMA2_Stream4; + hdma_adc1.Init.Channel = DMA_CHANNEL_0; + hdma_adc1.Init.Direction = DMA_PERIPH_TO_MEMORY; + hdma_adc1.Init.PeriphInc = DMA_PINC_DISABLE; + hdma_adc1.Init.MemInc = DMA_MINC_ENABLE; + hdma_adc1.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD; + hdma_adc1.Init.MemDataAlignment = DMA_MDATAALIGN_WORD; + hdma_adc1.Init.Mode = DMA_CIRCULAR; + hdma_adc1.Init.Priority = DMA_PRIORITY_LOW; + hdma_adc1.Init.FIFOMode = DMA_FIFOMODE_DISABLE; + if (HAL_DMA_Init(&hdma_adc1) != HAL_OK) + { + Error_Handler(); + } + + __HAL_LINKDMA(hadc,DMA_Handle,hdma_adc1); + /* USER CODE BEGIN ADC1_MspInit 1 */ /* USER CODE END ADC1_MspInit 1 */ @@ -174,9 +202,14 @@ void HAL_ADC_MspDeInit(ADC_HandleTypeDef* hadc) /**ADC1 GPIO Configuration PC5 ------> ADC1_IN15 + PB0 ------> ADC1_IN8 */ HAL_GPIO_DeInit(GPIOC, GPIO_PIN_5); + HAL_GPIO_DeInit(GPIOB, GPIO_PIN_0); + + /* ADC1 DMA DeInit */ + HAL_DMA_DeInit(hadc->DMA_Handle); /* USER CODE BEGIN ADC1_MspDeInit 1 */ /* USER CODE END ADC1_MspDeInit 1 */ diff --git a/Core/Src/torque.c b/Core/Src/torque.c index f96af11e..c45cc646 100644 --- a/Core/Src/torque.c +++ b/Core/Src/torque.c @@ -42,10 +42,15 @@ static void linear_accel_to_torque(float accel, uint16_t* torque) *torque = (uint16_t)(accel * MAX_TORQUE); } -static void rpm_to_mph(uint32_t rpm, float* mph) +static float rpm_to_mph(int32_t rpm) { /* Convert RPM to MPH */ - *mph = (rpm / 60) * WHEEL_CIRCUMFERENCE * 2.237 / GEAR_RATIO; + // rpm * gear ratio = wheel rpm + // tire diamter (in) to miles --> tire diamter miles + // wheel rpm * 60 --> wheel rph + // tire diamter miles * pi --> tire circumference + // rph * wheel circumference miles --> mph + return (rpm / (GEAR_RATIO))*60 * (TIRE_DIAMETER / 63360.0)*M_PI; } static void limit_accel_to_torque(float mph, float accel, uint16_t* torque) @@ -112,18 +117,27 @@ void vCalcTorque(void* pv_params) assert(delay_time < MAX_COMMAND_DELAY); pedals_t pedal_data; uint16_t torque = 0; + float mph = 0; osStatus_t stat; + bool motor_disabled = false; dti_t *mc = (dti_t *)pv_params; for (;;) { stat = osMessageQueueGet(pedal_data_queue, &pedal_data, 0U, delay_time); - float accelerator_value = (float) pedal_data.accelerator_value / 10.0; + float accelerator_value = (float) pedal_data.accelerator_value / 10.0; // 0 to 1 + float brake_value = (float) pedal_data.brake_value * 10.0; // ACTUAL PSI /* If we receive a new message within the time frame, calc new torque */ if (stat == osOK) - { + { + int32_t rpm = dti_get_rpm(mc); + //printf("rpm %ld", rpm); + mph = rpm_to_mph(rpm); + //printf("mph %d", (int8_t) mph); + set_mph(mph); + func_state_t func_state = get_func_state(); if (func_state != ACTIVE) { @@ -131,10 +145,33 @@ void vCalcTorque(void* pv_params) continue; } - drive_state_t drive_state = AUTOCROSS;//get_drive_state(); + /* EV.4.7: If brakes are engaged and APPS signals more than 25% pedal travel, disable power + to the motor(s). Re-enable when accelerator has less than 5% pedal travel. */ + //fault_data_t fault_data = { .id = BSPD_PREFAULT, .severity = DEFCON5 }; + /* 600 is an arbitrary threshold to consider the brakes mechanically activated */ + if (brake_value > 600 && (accelerator_value) > 0.25) + { + printf("\n\n\n\rENTER MOTOR DISABLED\r\n\n\n"); + motor_disabled = true; + torque = 0; + } - float mph; - rpm_to_mph(dti_get_rpm(mc), &mph); + if (motor_disabled) + { + printf("\nMotor disabled\n"); + if (accelerator_value < 0.05) + { + motor_disabled = false; + printf("\n\nMotor reenabled, queuing fault\n\n"); + //queue_fault(&fault_data); + } else { + torque = 0; + dti_set_torque(torque); + continue; + } + } + + drive_state_t drive_state = AUTOCROSS;//get_drive_state(); switch (drive_state) { @@ -159,7 +196,6 @@ void vCalcTorque(void* pv_params) serial_print("torque: %d\r\n", torque); /* Send whatever torque command we have on record */ - set_mph(mph); dti_set_torque(torque); } } diff --git a/Drivers/Embedded-Base b/Drivers/Embedded-Base index 41cbd057..7172a1e6 160000 --- a/Drivers/Embedded-Base +++ b/Drivers/Embedded-Base @@ -1 +1 @@ -Subproject commit 41cbd0573af8cad8091b3cbc2fcf9083cc0cad53 +Subproject commit 7172a1e65a2f6411dd130dcb5f69cb30f4b9dda8 diff --git a/Makefile b/Makefile index dc8d33bf..e8160754 100644 --- a/Makefile +++ b/Makefile @@ -1,5 +1,5 @@ ########################################################################################################################## -# File automatically-generated by tool: [projectgenerator] version: [4.2.0-B44] date: [Fri Apr 26 01:59:51 EDT 2024] +# File automatically-generated by tool: [projectgenerator] version: [4.3.0-B58] date: [Wed Jun 05 04:33:49 GMT 2024] ########################################################################################################################## # ------------------------------------------------ @@ -81,6 +81,7 @@ Drivers/Embedded-Base/general/src/sht30.c \ Drivers/Embedded-Base/general/src/max7314.c \ Drivers/Embedded-Base/platforms/stm32f405/src/can.c \ Drivers/Embedded-Base/general/src/pi4ioe.c \ +Drivers/Embedded-Base/general/src/pca9539.c \ Drivers/Embedded-Base/middleware/src/timer.c \ Drivers/Embedded-Base/middleware/src/ringbuffer.c \ Drivers/Embedded-Base/middleware/src/c_utils.c \ diff --git a/cerberus.ioc b/cerberus.ioc index c1b244e2..86a014e0 100644 --- a/cerberus.ioc +++ b/cerberus.ioc @@ -1,10 +1,13 @@ #MicroXplorer Configuration settings - do not modify -ADC1.Channel-1\#ChannelRegularConversion=ADC_CHANNEL_15 +ADC1.Channel-2\#ChannelRegularConversion=ADC_CHANNEL_8 ADC1.ClockPrescaler=ADC_CLOCK_SYNC_PCLK_DIV6 -ADC1.IPParameters=Rank-1\#ChannelRegularConversion,master,Channel-1\#ChannelRegularConversion,SamplingTime-1\#ChannelRegularConversion,NbrOfConversionFlag,ClockPrescaler +ADC1.ContinuousConvMode=ENABLE +ADC1.DMAContinuousRequests=ENABLE +ADC1.IPParameters=Rank-2\#ChannelRegularConversion,Channel-2\#ChannelRegularConversion,SamplingTime-2\#ChannelRegularConversion,NbrOfConversionFlag,master,ClockPrescaler,ScanConvMode,ContinuousConvMode,DMAContinuousRequests ADC1.NbrOfConversionFlag=1 -ADC1.Rank-1\#ChannelRegularConversion=1 -ADC1.SamplingTime-1\#ChannelRegularConversion=ADC_SAMPLETIME_3CYCLES +ADC1.Rank-2\#ChannelRegularConversion=1 +ADC1.SamplingTime-2\#ChannelRegularConversion=ADC_SAMPLETIME_3CYCLES +ADC1.ScanConvMode=ENABLE ADC1.master=1 ADC3.Channel-0\#ChannelRegularConversion=ADC_CHANNEL_2 ADC3.Channel-1\#ChannelRegularConversion=ADC_CHANNEL_3 @@ -40,6 +43,16 @@ CAN1.IPParameters=CalculateTimeQuantum,CalculateTimeBit,CalculateBaudRate,Presca CAN1.NART=DISABLE CAN1.Prescaler=2 CAN1.TXFP=DISABLE +Dma.ADC1.1.Direction=DMA_PERIPH_TO_MEMORY +Dma.ADC1.1.FIFOMode=DMA_FIFOMODE_DISABLE +Dma.ADC1.1.Instance=DMA2_Stream4 +Dma.ADC1.1.MemDataAlignment=DMA_MDATAALIGN_WORD +Dma.ADC1.1.MemInc=DMA_MINC_ENABLE +Dma.ADC1.1.Mode=DMA_CIRCULAR +Dma.ADC1.1.PeriphDataAlignment=DMA_PDATAALIGN_WORD +Dma.ADC1.1.PeriphInc=DMA_PINC_DISABLE +Dma.ADC1.1.Priority=DMA_PRIORITY_LOW +Dma.ADC1.1.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode Dma.ADC3.0.Direction=DMA_PERIPH_TO_MEMORY Dma.ADC3.0.FIFOMode=DMA_FIFOMODE_DISABLE Dma.ADC3.0.Instance=DMA2_Stream0 @@ -51,9 +64,11 @@ Dma.ADC3.0.PeriphInc=DMA_PINC_DISABLE Dma.ADC3.0.Priority=DMA_PRIORITY_MEDIUM Dma.ADC3.0.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode Dma.Request0=ADC3 -Dma.RequestsNb=1 -FREERTOS.IPParameters=Tasks01 +Dma.Request1=ADC1 +Dma.RequestsNb=2 +FREERTOS.IPParameters=Tasks01,configUSE_PREEMPTION FREERTOS.Tasks01=defaultTask,24,128,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL +FREERTOS.configUSE_PREEMPTION=0 File.Version=6 GPIO.groupedBy=Group By Peripherals IWDG.IPParameters=Prescaler @@ -113,8 +128,8 @@ Mcu.PinsNb=33 Mcu.ThirdPartyNb=0 Mcu.UserConstants= Mcu.UserName=STM32F405RGTx -MxCube.Version=6.10.0 -MxDb.Version=DB.6.0.100 +MxCube.Version=6.11.1 +MxDb.Version=DB.6.0.111 NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false NVIC.CAN1_RX0_IRQn=true\:10\:0\:true\:false\:true\:true\:true\:true\:true NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false @@ -164,10 +179,8 @@ PA7.Locked=true PA7.Signal=GPIO_Input PA9.Locked=true PA9.Signal=USB_OTG_FS_VBUS -PB0.GPIOParameters=GPIO_PuPd -PB0.GPIO_PuPd=GPIO_PULLUP PB0.Locked=true -PB0.Signal=GPIO_Input +PB0.Signal=ADCx_IN8 PB1.GPIOParameters=GPIO_PuPd PB1.GPIO_PuPd=GPIO_PULLUP PB1.Locked=true @@ -269,12 +282,14 @@ SH.ADCx_IN0.0=ADC3_IN0,IN0 SH.ADCx_IN0.ConfNb=1 SH.ADCx_IN1.0=ADC3_IN1,IN1 SH.ADCx_IN1.ConfNb=1 -SH.ADCx_IN15.0=ADC1_IN15,IN15 +SH.ADCx_IN15.0=ADC1_IN15 SH.ADCx_IN15.ConfNb=1 SH.ADCx_IN2.0=ADC3_IN2,IN2 SH.ADCx_IN2.ConfNb=1 SH.ADCx_IN3.0=ADC3_IN3,IN3 SH.ADCx_IN3.ConfNb=1 +SH.ADCx_IN8.0=ADC1_IN8,IN8 +SH.ADCx_IN8.ConfNb=1 USART3.IPParameters=VirtualMode USART3.VirtualMode=VM_ASYNC VP_FREERTOS_VS_CMSIS_V2.Mode=CMSIS_V2