From 4560da95def38af1f8e2773e5a6e70231cd75841 Mon Sep 17 00:00:00 2001 From: Aditya Anand <90068582+AdityaAsGithub@users.noreply.github.com> Date: Sat, 9 Dec 2023 17:37:33 -0500 Subject: [PATCH] Fixed L471 not Building, Renaming Navigation (#76) * Added LF71 to Cmake, modified necessary HALs to support nav * Fixed include path error * Renamed Navigation to Torque Vectoring, changed old TV to Torque Vectoring (FPGA) --------- Co-authored-by: AdityaAsGithub --- .vscode/launch.json | 18 +- CMakeLists.txt | 11 +- common/daq/can_config.json | 4 +- common/daq/per_dbc.dbc | 36 +- common/faults/fault_config.json | 4 +- common/faults/fault_nodes.h | 2 +- common/faults/faults.h | 2 +- common/phal_L4/CMakeLists.txt | 1 + common/phal_L4/adc/adc.c | 9 +- common/phal_L4/dma/dma.h | 14 +- common/phal_L4/eeprom/eeprom.h | 6 +- common/phal_L4/eeprom_spi/eeprom_spi.h | 6 +- common/phal_L4/spi/spi.h | 13 + common/psched/CMakeLists.txt | 15 + source/dashboard/can/can_parse.c | 8 +- source/dashboard/can/can_parse.h | 8 +- source/driveline/can/can_parse.c | 8 +- source/driveline/can/can_parse.h | 8 +- source/l4_testing/can/can_parse.c | 8 +- source/l4_testing/can/can_parse.h | 8 +- source/main_module/can/can_parse.c | 8 +- source/main_module/can/can_parse.h | 8 +- source/navigation/can/can_parse.c | 86 ---- source/navigation/can/can_parse.h | 277 ----------- source/navigation/main.c | 446 ----------------- source/navigation/main.h | 53 -- source/precharge/can/can_parse.c | 8 +- source/precharge/can/can_parse.h | 8 +- source/torque_vector/CMakeLists.txt | 7 +- .../bmi/bmi088.c | 0 .../bmi/bmi088.h | 0 .../bmm/bmm150.c | 2 +- .../bmm/bmm150.h | 0 .../bsxlite/bsxlite_interface.h | 0 .../bsxlite/libalgobsx.a | Bin .../bsxlite/memory_footprint.txt | 0 source/torque_vector/can/can_parse.c | 146 ++---- source/torque_vector/can/can_parse.h | 341 ++++++------- .../{navigation => torque_vector}/gps/gps.c | 2 +- .../{navigation => torque_vector}/gps/gps.h | 0 .../{navigation => torque_vector}/imu/imu.c | 2 +- .../{navigation => torque_vector}/imu/imu.h | 0 source/torque_vector/main.c | 455 ++++++++++++++---- source/torque_vector/main.h | 83 ++-- .../{navigation => torque_vector}/sfs/SFS.c | 0 .../{navigation => torque_vector}/sfs/SFS.h | 0 .../sfs/SFS_data.c | 0 .../sfs/rtwtypes.h | 0 .../sfs_pp/sfs_pp.c | 2 +- .../sfs_pp/sfs_pp.h | 0 source/torque_vector_fpga/CMakeLists.txt | 17 + .../bitstream/bitstream.c | 0 .../bitstream/bitstream.h | 0 source/torque_vector_fpga/can/can_parse.c | 176 +++++++ source/torque_vector_fpga/can/can_parse.h | 270 +++++++++++ source/torque_vector_fpga/main.c | 177 +++++++ source/torque_vector_fpga/main.h | 46 ++ 57 files changed, 1434 insertions(+), 1375 deletions(-) delete mode 100644 source/navigation/can/can_parse.c delete mode 100644 source/navigation/can/can_parse.h delete mode 100644 source/navigation/main.c delete mode 100644 source/navigation/main.h rename source/{navigation => torque_vector}/bmi/bmi088.c (100%) rename source/{navigation => torque_vector}/bmi/bmi088.h (100%) rename source/{navigation => torque_vector}/bmm/bmm150.c (99%) rename source/{navigation => torque_vector}/bmm/bmm150.h (100%) rename source/{navigation => torque_vector}/bsxlite/bsxlite_interface.h (100%) rename source/{navigation => torque_vector}/bsxlite/libalgobsx.a (100%) rename source/{navigation => torque_vector}/bsxlite/memory_footprint.txt (100%) rename source/{navigation => torque_vector}/gps/gps.c (99%) rename source/{navigation => torque_vector}/gps/gps.h (100%) rename source/{navigation => torque_vector}/imu/imu.c (99%) rename source/{navigation => torque_vector}/imu/imu.h (100%) rename source/{navigation => torque_vector}/sfs/SFS.c (100%) rename source/{navigation => torque_vector}/sfs/SFS.h (100%) rename source/{navigation => torque_vector}/sfs/SFS_data.c (100%) rename source/{navigation => torque_vector}/sfs/rtwtypes.h (100%) rename source/{navigation => torque_vector}/sfs_pp/sfs_pp.c (95%) rename source/{navigation => torque_vector}/sfs_pp/sfs_pp.h (100%) create mode 100644 source/torque_vector_fpga/CMakeLists.txt rename source/{torque_vector => torque_vector_fpga}/bitstream/bitstream.c (100%) rename source/{torque_vector => torque_vector_fpga}/bitstream/bitstream.h (100%) create mode 100644 source/torque_vector_fpga/can/can_parse.c create mode 100644 source/torque_vector_fpga/can/can_parse.h create mode 100644 source/torque_vector_fpga/main.c create mode 100644 source/torque_vector_fpga/main.h diff --git a/.vscode/launch.json b/.vscode/launch.json index 1ffa5daa..fcae579e 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -61,9 +61,9 @@ "preLaunchTask": "build", "runToEntryPoint": "main" }, - { "name": "Navigation", + { "name": "Torque Vectoring", "cwd": "${workspaceRoot}", - "executable": "./output/navigation/navigation.elf", + "executable": "./output/torque_vector/torque_vector.elf", "request": "launch", "type": "cortex-debug", "servertype": "openocd", @@ -75,7 +75,7 @@ ], "debuggerArgs": [ "-d", - "${workspaceFolder}/source/navigation" + "${workspaceFolder}/source/torque_vector" ], "preLaunchTask": "build", "runToEntryPoint": "main" @@ -156,9 +156,9 @@ // "preLaunchTask": "build", "runToEntryPoint": "main" }, - { "name": "Torque Vectoring", + { "name": "Torque Vectoring (FPGA)", "cwd": "${workspaceRoot}", - "executable": "./output/torque_vector/torque_vector.elf", + "executable": "./output/torque_vector_fpga/torque_vector_fpga.elf", "request": "launch", "type": "cortex-debug", "servertype": "openocd", @@ -170,7 +170,7 @@ ], "debuggerArgs": [ "-d", - "${workspaceFolder}/source/torque_vector" + "${workspaceFolder}/source/torque_vector_fpga" ], "preLaunchTask": "build", "runToEntryPoint": "main" @@ -289,9 +289,9 @@ "preLaunchTask": "build", "runToEntryPoint": "main" }, - { "name": "BL App - Torque Vectoring", + { "name": "BL App - Torque Vectoring (FPGA)", "cwd": "${workspaceRoot}", - "executable": "./output/torque_vector/BL_torque_vector.elf", + "executable": "./output/torque_vector_fpga/BL_torque_vector_fpga.elf", "request": "launch", "type": "cortex-debug", "servertype": "openocd", @@ -303,7 +303,7 @@ ], "debuggerArgs": [ "-d", - "${workspaceFolder}/source/torque_vector" + "${workspaceFolder}/source/torque_vector_fpga" ], "preLaunchTask": "build", "runToEntryPoint": "main" diff --git a/CMakeLists.txt b/CMakeLists.txt index 3ef8dfd7..1895448f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -43,6 +43,7 @@ include(cmake/FindSTM32HAL.cmake) # See cmake/FindCMSIS.cmake and cmake/FindSTM32HAL.cmake for constructing additional libraries make_cmsis_library(CMSIS_L432 STM32L4xx STM32L432xx ${CMAKE_SOURCE_DIR}/common/STM32CubeL4/Drivers/CMSIS) +make_cmsis_library(CMSIS_L471 STM32L4xx STM32L471xx ${CMAKE_SOURCE_DIR}/common/STM32CubeL4/Drivers/CMSIS) make_cmsis_library(CMSIS_L496 STM32L4xx STM32L496xx ${CMAKE_SOURCE_DIR}/common/STM32CubeL4/Drivers/CMSIS) make_cmsis_library(CMSIS_F407 STM32F4xx STM32F407xx ${CMAKE_SOURCE_DIR}/common/STM32CubeF4/Drivers/CMSIS) make_cmsis_library(CMSIS_F732 STM32F7xx STM32F732xx ${CMAKE_SOURCE_DIR}/common/STM32CubeF7/Drivers/CMSIS) @@ -86,10 +87,14 @@ add_subdirectory(source/bootloader) add_subdirectory(source/l4_testing) add_subdirectory(source/f4_testing) add_subdirectory(source/f7_testing) -add_subdirectory(source/torque_vector) add_subdirectory(source/precharge) -# add_subdirectory(source/driveline) +add_subdirectory(source/torque_vector) add_subdirectory(source/dashboard) +add_subdirectory(source/pdu) + + +# Deprecated Components +# add_subdirectory(source/driveline) +# add_subdirectory(source/torque_vector_fpga) # add_subdirectory(source/template) # add_subdirectory(source/bms) -add_subdirectory(source/pdu) diff --git a/common/daq/can_config.json b/common/daq/can_config.json index deb8ce3e..c82ad538 100644 --- a/common/daq/can_config.json +++ b/common/daq/can_config.json @@ -211,7 +211,7 @@ ] }, { - "node_name":"Torque_Vector", + "node_name":"Torque_Vector_fpga", "node_ssa":2, "tx":[ { "msg_name":"torque_request", @@ -583,7 +583,7 @@ ] }, { - "node_name":"Navigation", + "node_name":"torque_vector", "node_ssa": 55, "tx":[ { "msg_name":"gps_velocity", diff --git a/common/daq/per_dbc.dbc b/common/daq/per_dbc.dbc index 6b506b48..382bdeff 100644 --- a/common/daq/per_dbc.dbc +++ b/common/daq/per_dbc.dbc @@ -33,7 +33,7 @@ NS_ : BS_: -BU_: Main_Module Torque_Vector Driveline Precharge OrionBMS Navigation Dashboard Steering Acceleration bootloader BMS_LV BITSTREAM Charger DAQ Precharge ARDUINO TEST_NODE TEST_NODE_2 DAQ +BU_: Main_Module Torque_Vector_fpga Driveline Precharge OrionBMS torque_vector Dashboard Steering Acceleration bootloader BMS_LV BITSTREAM Charger DAQ Precharge ARDUINO TEST_NODE TEST_NODE_2 DAQ BO_ 2214598913 main_hb: 2 Main_Module @@ -129,17 +129,17 @@ BO_ 2148059649 fault_sync_main_module: 3 Main_Module BO_ 2550136769 daq_response_MAIN_MODULE: 8 Main_Module SG_ daq_response : 0|64@1+ (1,0) [0|0] "" Vector__XXX -BO_ 2214592578 torque_request: 6 Torque_Vector +BO_ 2214592578 torque_request: 6 Torque_Vector_fpga SG_ rear_right : 36|12@1+ (1,0) [0|0] "" Vector__XXX SG_ rear_left : 24|12@1+ (1,0) [0|0] "" Vector__XXX SG_ front_right : 12|12@1+ (1,0) [0|0] "" Vector__XXX SG_ front_left : 0|12@1+ (1,0) [0|0] "" Vector__XXX -BO_ 2147490050 bitstream_flash_status: 1 Torque_Vector +BO_ 2147490050 bitstream_flash_status: 1 Torque_Vector_fpga SG_ flash_timeout_rx : 1|1@1+ (1,0) [0|0] "" Vector__XXX SG_ flash_success : 0|1@1+ (1,0) [0|0] "" Vector__XXX -BO_ 2148059714 fault_sync_torque_vector: 3 Torque_Vector +BO_ 2148059714 fault_sync_torque_vector_fpga: 3 Torque_Vector_fpga SG_ latched : 16|1@1+ (1,0) [0|0] "" Vector__XXX SG_ idx : 0|16@1+ (1,0) [0|0] "" Vector__XXX @@ -315,64 +315,64 @@ BO_ 2348812088 orion_errors: 4 OrionBMS SG_ charger_safety_relay : 1|1@1+ (1,0) [0|0] "" Vector__XXX SG_ discharge_limit_enforce : 0|1@1+ (1,0) [0|0] "" Vector__XXX -BO_ 2348810935 gps_velocity: 8 Navigation +BO_ 2348810935 gps_velocity: 8 torque_vector SG_ gps_vel_total : 48|16@1- (0.01,0) [0|0] "m/s" Vector__XXX SG_ gps_vel_d : 32|16@1- (0.01,0) [0|0] "m/s" Vector__XXX SG_ gps_vel_e : 16|16@1- (0.01,0) [0|0] "m/s" Vector__XXX SG_ gps_vel_n : 0|16@1- (0.01,0) [0|0] "m/s" Vector__XXX -BO_ 2348819255 gps_position: 8 Navigation +BO_ 2348819255 gps_position: 8 torque_vector SG_ height : 48|16@1- (0.01,0) [0|0] "m" Vector__XXX SG_ gps_pos_z : 32|16@1- (0.01,0) [0|0] "m" Vector__XXX SG_ gps_pos_y : 16|16@1- (0.01,0) [0|0] "m" Vector__XXX SG_ gps_pos_x : 0|16@1- (0.01,0) [0|0] "m" Vector__XXX -BO_ 2348819319 gps_coordinates: 8 Navigation +BO_ 2348819319 gps_coordinates: 8 torque_vector SG_ longitude : 32|32@1- (1,0) [0|0] "deg" Vector__XXX SG_ latitude : 0|32@1- (1,0) [0|0] "deg" Vector__XXX -BO_ 2348810999 imu_gyro: 6 Navigation +BO_ 2348810999 imu_gyro: 6 torque_vector SG_ imu_gyro_z : 32|16@1- (0.01,0) [0|0] "rad/s" Vector__XXX SG_ imu_gyro_y : 16|16@1- (0.01,0) [0|0] "rad/s" Vector__XXX SG_ imu_gyro_x : 0|16@1- (0.01,0) [0|0] "rad/s" Vector__XXX -BO_ 2348819383 imu_accel: 6 Navigation +BO_ 2348819383 imu_accel: 6 torque_vector SG_ imu_accel_z : 32|16@1- (0.01,0) [0|0] "m/s^2" Vector__XXX SG_ imu_accel_y : 16|16@1- (0.01,0) [0|0] "m/s^2" Vector__XXX SG_ imu_accel_x : 0|16@1- (0.01,0) [0|0] "m/s^2" Vector__XXX -BO_ 2348819447 bmm_mag: 6 Navigation +BO_ 2348819447 bmm_mag: 6 torque_vector SG_ bmm_mag_z : 32|16@1- (1,0) [0|0] "uT" Vector__XXX SG_ bmm_mag_y : 16|16@1- (1,0) [0|0] "uT" Vector__XXX SG_ bmm_mag_x : 0|16@1- (1,0) [0|0] "uT" Vector__XXX -BO_ 2348902711 sfs_pos: 6 Navigation +BO_ 2348902711 sfs_pos: 6 torque_vector SG_ sfs_pos_z : 32|16@1- (0.01,0) [0|0] "m" Vector__XXX SG_ sfs_pos_y : 16|16@1- (0.01,0) [0|0] "m" Vector__XXX SG_ sfs_pos_x : 0|16@1- (0.01,0) [0|0] "m" Vector__XXX -BO_ 2348902775 sfs_vel: 6 Navigation +BO_ 2348902775 sfs_vel: 6 torque_vector SG_ sfs_vel_z : 32|16@1- (0.01,0) [0|0] "m/s" Vector__XXX SG_ sfs_vel_y : 16|16@1- (0.01,0) [0|0] "m/s" Vector__XXX SG_ sfs_vel_x : 0|16@1- (0.01,0) [0|0] "m/s" Vector__XXX -BO_ 2348902839 sfs_acc: 6 Navigation +BO_ 2348902839 sfs_acc: 6 torque_vector SG_ sfs_acc_z : 32|16@1- (0.01,0) [0|0] "m/s^2" Vector__XXX SG_ sfs_acc_y : 16|16@1- (0.01,0) [0|0] "m/s^2" Vector__XXX SG_ sfs_acc_x : 0|16@1- (0.01,0) [0|0] "m/s^2" Vector__XXX -BO_ 2348902903 sfs_ang: 8 Navigation +BO_ 2348902903 sfs_ang: 8 torque_vector SG_ sfs_ang_d : 48|16@1- (0.0001,0) [0|0] "" Vector__XXX SG_ sfs_ang_c : 32|16@1- (0.0001,0) [0|0] "" Vector__XXX SG_ sfs_ang_b : 16|16@1- (0.0001,0) [0|0] "" Vector__XXX SG_ sfs_ang_a : 0|16@1- (0.0001,0) [0|0] "" Vector__XXX -BO_ 2348902967 sfs_ang_vel: 6 Navigation +BO_ 2348902967 sfs_ang_vel: 6 torque_vector SG_ sfs_ang_vel_z : 32|16@1- (0.0001,0) [0|0] "" Vector__XXX SG_ sfs_ang_vel_y : 16|16@1- (0.0001,0) [0|0] "" Vector__XXX SG_ sfs_ang_vel_x : 0|16@1- (0.0001,0) [0|0] "" Vector__XXX -BO_ 2348819895 throttle_remapped: 4 Navigation +BO_ 2348819895 throttle_remapped: 4 torque_vector SG_ remap_k_rr : 16|16@1- (1,0) [0|0] "none" Vector__XXX SG_ remap_k_rl : 0|16@1- (1,0) [0|0] "none" Vector__XXX @@ -834,11 +834,11 @@ BO_ 2483032050 daq_command_TEST_NODE: 8 DAQ CM_ BU_ Main_Module ""; -CM_ BU_ Torque_Vector ""; +CM_ BU_ Torque_Vector_fpga ""; CM_ BU_ Driveline ""; CM_ BU_ Precharge ""; CM_ BU_ OrionBMS ""; -CM_ BU_ Navigation ""; +CM_ BU_ torque_vector ""; CM_ BU_ Dashboard ""; CM_ BU_ Steering ""; CM_ BU_ Acceleration ""; diff --git a/common/faults/fault_config.json b/common/faults/fault_config.json index 203876f7..314b3b5c 100644 --- a/common/faults/fault_config.json +++ b/common/faults/fault_config.json @@ -447,8 +447,8 @@ ] }, { - "node_name": "tv", - "can_name": "torque_vector", + "node_name": "tv_old", + "can_name": "torque_vector_fpga", "faults": [ { "fault_name": "tv_offline", diff --git a/common/faults/fault_nodes.h b/common/faults/fault_nodes.h index 27f100a5..09878fc9 100644 --- a/common/faults/fault_nodes.h +++ b/common/faults/fault_nodes.h @@ -8,7 +8,7 @@ #define NODE_DRIVELINE_FRONT 1 #define NODE_DASHBOARD 2 #define NODE_PRECHARGE 3 -#define NODE_TV 4 +#define NODE_TV_OLD 4 #define NODE_TEST 5 //END AUTO NODE DEFS diff --git a/common/faults/faults.h b/common/faults/faults.h index e5228853..63cd9a44 100644 --- a/common/faults/faults.h +++ b/common/faults/faults.h @@ -24,7 +24,7 @@ #define TOTAL_DRIVELINE_FRONT_FAULTS 4 #define TOTAL_DASHBOARD_FAULTS 6 #define TOTAL_PRECHARGE_FAULTS 28 -#define TOTAL_TV_FAULTS 1 +#define TOTAL_TV_OLD_FAULTS 1 #define TOTAL_TEST_FAULTS 4 #define TOTAL_MCU_NUM 6 #define TOTAL_NUM_FAULTS 52 diff --git a/common/phal_L4/CMakeLists.txt b/common/phal_L4/CMakeLists.txt index 58d28ccb..ba864be2 100644 --- a/common/phal_L4/CMakeLists.txt +++ b/common/phal_L4/CMakeLists.txt @@ -17,4 +17,5 @@ MACRO(MAKE_PHAL_LIBRARY lib_name lib_link) ENDMACRO(MAKE_PHAL_LIBRARY) MAKE_PHAL_LIBRARY(PHAL_L432 CMSIS_L432) +MAKE_PHAL_LIBRARY(PHAL_L471 CMSIS_L471) MAKE_PHAL_LIBRARY(PHAL_L496 CMSIS_L496) diff --git a/common/phal_L4/adc/adc.c b/common/phal_L4/adc/adc.c index 98199c65..5d482281 100644 --- a/common/phal_L4/adc/adc.c +++ b/common/phal_L4/adc/adc.c @@ -1,7 +1,7 @@ /** * @file adc.h * @author Luke Oxley (lcoxley@purdue.edu) - * @brief + * @brief * @version 0.1 * @date 2021-12-27 */ @@ -41,7 +41,7 @@ bool PHAL_initADC(ADC_TypeDef* adc, ADCInitConfig_t* config, ADCChannelConfig_t #ifdef STM32L432xx #define ADC_COMMON ADC1_COMMON - #elif STM32L496xx + #elif STM32L496xx || STM32L471xx #define ADC_COMMON ADC123_COMMON #else #error "STM32 Arch not currently supported for ADC" @@ -58,7 +58,7 @@ bool PHAL_initADC(ADC_TypeDef* adc, ADCInitConfig_t* config, ADCChannelConfig_t adc->CFGR &= ~(ADC_CFGR_CONT); adc->CFGR |= (config->cont_conv_mode << ADC_CFGR_CONT_Pos) & ADC_CFGR_CONT_Msk; - // Overrun + // Overrun adc->CFGR &= ~(ADC_CFGR_OVRMOD); adc->CFGR |= (config->overrun << ADC_CFGR_OVRMOD_Pos) & ADC_CFGR_OVRMOD_Msk; @@ -77,7 +77,7 @@ bool PHAL_initADC(ADC_TypeDef* adc, ADCInitConfig_t* config, ADCChannelConfig_t // DMA configuration if (config->dma_mode != ADC_DMA_OFF) { - adc->CFGR |= (ADC_CFGR_DMAEN) | + adc->CFGR |= (ADC_CFGR_DMAEN) | (((config->dma_mode == ADC_DMA_CIRCULAR) << ADC_CFGR_DMACFG_Pos) & ADC_CFGR_DMACFG); } else adc->CFGR &= ~(ADC_CFGR_DMAEN); @@ -149,4 +149,3 @@ uint16_t PHAL_readADC(ADC_TypeDef* adc) { return (uint16_t) (adc->DR & ADC_DR_RDATA_Msk); } - diff --git a/common/phal_L4/dma/dma.h b/common/phal_L4/dma/dma.h index 766f1b59..6e3b5986 100644 --- a/common/phal_L4/dma/dma.h +++ b/common/phal_L4/dma/dma.h @@ -13,6 +13,8 @@ #ifdef STM32L496xx #include "stm32l496xx.h" +#elif STM32L471xx +#include "stm32l471xx.h" #elif STM32L432xx #include "stm32l432xx.h" #else @@ -48,7 +50,7 @@ typedef struct { /** * @brief Initialize DMA peripheral to set m2m, p2p, or p2m with set size * and length of txfer - * + * * @param init -> Address of initialization structure * @return true -> Successful init (no clashing params) * @return false -> Init not complete (parameters clash) @@ -57,35 +59,35 @@ bool PHAL_initDMA(dma_init_t* init); /** * @brief Start txfer after sucessful DMA peripheral initialization - * + * * @param init -> Address of initialization structure */ void PHAL_startTxfer(dma_init_t* init); /** * @brief Stop txfer - * + * * @param init -> Address of initialization structure */ void PHAL_stopTxfer(dma_init_t* init); /** * @brief Re-enable DMA txfer after error ISR fires - * + * * @param init -> Address of initialization structure */ void PHAL_reEnable(dma_init_t* init); /** * @brief Set memory address for DMA transfer. In Mem to Mem this acts as the source address - * + * * @param init -> Address of initialization structure */ void PHAL_DMA_setMemAddress(dma_init_t* init, const uint32_t address); /** * @brief Set transfer length for DMA transaction - * + * * @param init -> Address of initialization structure */ void PHAL_DMA_setTxferLength(dma_init_t* init, const uint32_t length); diff --git a/common/phal_L4/eeprom/eeprom.h b/common/phal_L4/eeprom/eeprom.h index 5f4eb2c0..bdeea852 100644 --- a/common/phal_L4/eeprom/eeprom.h +++ b/common/phal_L4/eeprom/eeprom.h @@ -5,6 +5,8 @@ #ifdef STM32L496xx #include "stm32l496xx.h" +#elif STM32L471xx +#include "stm32l432xx.h" #elif STM32L432xx #include "stm32l432xx.h" #else @@ -47,7 +49,7 @@ #define S_BCMP 63 #define M_INIT 0 -#define M_VERSION (M_INIT + S_INIT) +#define M_VERSION (M_INIT + S_INIT) #define M_FNAME (M_VERSION + S_VERSION) #define M_ADDR (M_FNAME + S_FNAME) #define M_LEN (M_ADDR + S_ADDR) @@ -76,7 +78,7 @@ * +-----------------------------------------+ * + PAGE 63 + * +-----------------------------------------+ - * + * */ // Structures diff --git a/common/phal_L4/eeprom_spi/eeprom_spi.h b/common/phal_L4/eeprom_spi/eeprom_spi.h index 36b082bc..b218a02f 100644 --- a/common/phal_L4/eeprom_spi/eeprom_spi.h +++ b/common/phal_L4/eeprom_spi/eeprom_spi.h @@ -12,6 +12,8 @@ #ifdef STM32L496xx #include "stm32l496xx.h" +#elif STM32L471xx +#include "stm32l471xx.h" #elif STM32L432xx #include "stm32l432xx.h" #else @@ -55,7 +57,7 @@ #define S_BCMP 63 #define M_INIT 0 -#define M_VERSION (M_INIT + S_INIT) +#define M_VERSION (M_INIT + S_INIT) #define M_FNAME (M_VERSION + S_VERSION) #define M_ADDR (M_FNAME + S_FNAME) #define M_LEN (M_ADDR + S_ADDR) @@ -84,7 +86,7 @@ * +-----------------------------------------+ * + PAGE 63 + * +-----------------------------------------+ - * + * */ // Structures diff --git a/common/phal_L4/spi/spi.h b/common/phal_L4/spi/spi.h index d2e6e9b6..307aa5b4 100644 --- a/common/phal_L4/spi/spi.h +++ b/common/phal_L4/spi/spi.h @@ -61,6 +61,19 @@ bool PHAL_SPI_init(SPI_InitConfig_t *handle); */ bool PHAL_SPI_transfer(SPI_InitConfig_t *spi, const uint8_t *out_data, const uint32_t data_len, const uint8_t *in_data); +/** + * @brief SPI handle + * + * @param spi SPI handle + * @param out_data Address of data buffer to put on MOSI line + * @param txlen Number of SPI Packets to Send + * @param rxlen Number of SPI Packets to Receive + * @param in_data Address of data buffer to put data coming in MISO line + * @return true Successfully completed non-DMA SPI transaction + * @return false Unable to complete non-DMA SPI transaction + */ +bool PHAL_SPI_transfer_noDMA(SPI_InitConfig_t *spi, const uint8_t *out_data, uint32_t txlen, uint32_t rxlen, uint8_t *in_data); + /** * @brief Check for current SPI transaction to complete * @param cfg Spi config diff --git a/common/psched/CMakeLists.txt b/common/psched/CMakeLists.txt index 81b0ee06..90f045a4 100644 --- a/common/psched/CMakeLists.txt +++ b/common/psched/CMakeLists.txt @@ -14,6 +14,21 @@ target_include_directories(${TARGET_NAME} PUBLIC ${CMAKE_SOURCE_DIR}) target_link_libraries(${TARGET_NAME} CMSIS_L432) +# Set up for L471 + +set(TARGET_NAME PSCHED_L471) +add_library(${TARGET_NAME}) + +# Find all .c sources in project +file(GLOB_RECURSE glob_sources "*.c") +target_sources(${TARGET_NAME} PRIVATE ${glob_sources}) + +# Find directories for '#include' +# For libraries, these directories are all referenced to the top level firmware directory, CMAKE_SOURCE_DIR +target_include_directories(${TARGET_NAME} PUBLIC ${CMAKE_SOURCE_DIR}) + +target_link_libraries(${TARGET_NAME} CMSIS_L471) + # Set up for L496 set(TARGET_NAME PSCHED_L496) diff --git a/source/dashboard/can/can_parse.c b/source/dashboard/can/can_parse.c index a0324006..286f28b0 100644 --- a/source/dashboard/can/can_parse.c +++ b/source/dashboard/can/can_parse.c @@ -204,9 +204,9 @@ void canRxUpdate() can_data.fault_sync_precharge.latched = msg_data_a->fault_sync_precharge.latched; handleCallbacks(msg_data_a->fault_sync_main_module.idx, msg_data_a->fault_sync_main_module.latched); break; - case ID_FAULT_SYNC_TORQUE_VECTOR: - can_data.fault_sync_torque_vector.idx = msg_data_a->fault_sync_torque_vector.idx; - can_data.fault_sync_torque_vector.latched = msg_data_a->fault_sync_torque_vector.latched; + case ID_FAULT_SYNC_TORQUE_VECTOR_FPGA: + can_data.fault_sync_torque_vector_fpga.idx = msg_data_a->fault_sync_torque_vector_fpga.idx; + can_data.fault_sync_torque_vector_fpga.latched = msg_data_a->fault_sync_torque_vector_fpga.latched; handleCallbacks(msg_data_a->fault_sync_main_module.idx, msg_data_a->fault_sync_main_module.latched); break; case ID_FAULT_SYNC_TEST_NODE: @@ -321,7 +321,7 @@ bool initCANFilter() CAN1->sFilterRegister[8].FR1 = (ID_FAULT_SYNC_DRIVELINE << 3) | 4; CAN1->sFilterRegister[8].FR2 = (ID_FAULT_SYNC_PRECHARGE << 3) | 4; CAN1->FA1R |= (1 << 9); // configure bank 9 - CAN1->sFilterRegister[9].FR1 = (ID_FAULT_SYNC_TORQUE_VECTOR << 3) | 4; + CAN1->sFilterRegister[9].FR1 = (ID_FAULT_SYNC_TORQUE_VECTOR_FPGA << 3) | 4; CAN1->sFilterRegister[9].FR2 = (ID_FAULT_SYNC_TEST_NODE << 3) | 4; CAN1->FA1R |= (1 << 10); // configure bank 10 CAN1->sFilterRegister[10].FR1 = (ID_SET_FAULT << 3) | 4; diff --git a/source/dashboard/can/can_parse.h b/source/dashboard/can/can_parse.h index 67438dfb..c263b357 100644 --- a/source/dashboard/can/can_parse.h +++ b/source/dashboard/can/can_parse.h @@ -46,7 +46,7 @@ #define ID_FAULT_SYNC_MAIN_MODULE 0x8ca01 #define ID_FAULT_SYNC_DRIVELINE 0x8ca83 #define ID_FAULT_SYNC_PRECHARGE 0x8cac4 -#define ID_FAULT_SYNC_TORQUE_VECTOR 0x8ca42 +#define ID_FAULT_SYNC_TORQUE_VECTOR_FPGA 0x8ca42 #define ID_FAULT_SYNC_TEST_NODE 0x8cb7f #define ID_SET_FAULT 0x809c83e #define ID_RETURN_FAULT_CONTROL 0x809c87e @@ -81,7 +81,7 @@ #define DLC_FAULT_SYNC_MAIN_MODULE 3 #define DLC_FAULT_SYNC_DRIVELINE 3 #define DLC_FAULT_SYNC_PRECHARGE 3 -#define DLC_FAULT_SYNC_TORQUE_VECTOR 3 +#define DLC_FAULT_SYNC_TORQUE_VECTOR_FPGA 3 #define DLC_FAULT_SYNC_TEST_NODE 3 #define DLC_SET_FAULT 3 #define DLC_RETURN_FAULT_CONTROL 2 @@ -370,7 +370,7 @@ typedef union { struct { uint64_t idx: 16; uint64_t latched: 1; - } fault_sync_torque_vector; + } fault_sync_torque_vector_fpga; struct { uint64_t idx: 16; uint64_t latched: 1; @@ -560,7 +560,7 @@ typedef struct { struct { uint16_t idx; uint8_t latched; - } fault_sync_torque_vector; + } fault_sync_torque_vector_fpga; struct { uint16_t idx; uint8_t latched; diff --git a/source/driveline/can/can_parse.c b/source/driveline/can/can_parse.c index 545f844c..0be5487e 100644 --- a/source/driveline/can/can_parse.c +++ b/source/driveline/can/can_parse.c @@ -119,9 +119,9 @@ void canRxUpdate() can_data.fault_sync_precharge.latched = msg_data_a->fault_sync_precharge.latched; handleCallbacks(msg_data_a->fault_sync_main_module.idx, msg_data_a->fault_sync_main_module.latched); break; - case ID_FAULT_SYNC_TORQUE_VECTOR: - can_data.fault_sync_torque_vector.idx = msg_data_a->fault_sync_torque_vector.idx; - can_data.fault_sync_torque_vector.latched = msg_data_a->fault_sync_torque_vector.latched; + case ID_FAULT_SYNC_TORQUE_VECTOR_FPGA: + can_data.fault_sync_torque_vector_fpga.idx = msg_data_a->fault_sync_torque_vector_fpga.idx; + can_data.fault_sync_torque_vector_fpga.latched = msg_data_a->fault_sync_torque_vector_fpga.latched; handleCallbacks(msg_data_a->fault_sync_main_module.idx, msg_data_a->fault_sync_main_module.latched); break; case ID_FAULT_SYNC_TEST_NODE: @@ -201,7 +201,7 @@ bool initCANFilter() CAN1->sFilterRegister[4].FR2 = (ID_FAULT_SYNC_DASHBOARD << 3) | 4; CAN1->FA1R |= (1 << 5); // configure bank 5 CAN1->sFilterRegister[5].FR1 = (ID_FAULT_SYNC_PRECHARGE << 3) | 4; - CAN1->sFilterRegister[5].FR2 = (ID_FAULT_SYNC_TORQUE_VECTOR << 3) | 4; + CAN1->sFilterRegister[5].FR2 = (ID_FAULT_SYNC_TORQUE_VECTOR_FPGA << 3) | 4; CAN1->FA1R |= (1 << 6); // configure bank 6 CAN1->sFilterRegister[6].FR1 = (ID_FAULT_SYNC_TEST_NODE << 3) | 4; CAN1->sFilterRegister[6].FR2 = (ID_SET_FAULT << 3) | 4; diff --git a/source/driveline/can/can_parse.h b/source/driveline/can/can_parse.h index 4b33ace0..8c69c161 100644 --- a/source/driveline/can/can_parse.h +++ b/source/driveline/can/can_parse.h @@ -47,7 +47,7 @@ typedef union { #define ID_FAULT_SYNC_MAIN_MODULE 0x8ca01 #define ID_FAULT_SYNC_DASHBOARD 0x8cb05 #define ID_FAULT_SYNC_PRECHARGE 0x8cac4 -#define ID_FAULT_SYNC_TORQUE_VECTOR 0x8ca42 +#define ID_FAULT_SYNC_TORQUE_VECTOR_FPGA 0x8ca42 #define ID_FAULT_SYNC_TEST_NODE 0x8cb7f #define ID_SET_FAULT 0x809c83e #define ID_RETURN_FAULT_CONTROL 0x809c87e @@ -73,7 +73,7 @@ typedef union { #define DLC_FAULT_SYNC_MAIN_MODULE 3 #define DLC_FAULT_SYNC_DASHBOARD 3 #define DLC_FAULT_SYNC_PRECHARGE 3 -#define DLC_FAULT_SYNC_TORQUE_VECTOR 3 +#define DLC_FAULT_SYNC_TORQUE_VECTOR_FPGA 3 #define DLC_FAULT_SYNC_TEST_NODE 3 #define DLC_SET_FAULT 3 #define DLC_RETURN_FAULT_CONTROL 2 @@ -327,7 +327,7 @@ typedef union { struct { uint64_t idx: 16; uint64_t latched: 1; - } fault_sync_torque_vector; + } fault_sync_torque_vector_fpga; struct { uint64_t idx: 16; uint64_t latched: 1; @@ -432,7 +432,7 @@ typedef struct { struct { uint16_t idx; uint8_t latched; - } fault_sync_torque_vector; + } fault_sync_torque_vector_fpga; struct { uint16_t idx; uint8_t latched; diff --git a/source/l4_testing/can/can_parse.c b/source/l4_testing/can/can_parse.c index 793e4785..5c7ab4ca 100644 --- a/source/l4_testing/can/can_parse.c +++ b/source/l4_testing/can/can_parse.c @@ -83,9 +83,9 @@ void canRxUpdate(void) can_data.fault_sync_precharge.latched = msg_data_a->fault_sync_precharge.latched; handleCallbacks(msg_data_a->fault_sync_main_module.idx, msg_data_a->fault_sync_main_module.latched); break; - case ID_FAULT_SYNC_TORQUE_VECTOR: - can_data.fault_sync_torque_vector.idx = msg_data_a->fault_sync_torque_vector.idx; - can_data.fault_sync_torque_vector.latched = msg_data_a->fault_sync_torque_vector.latched; + case ID_FAULT_SYNC_TORQUE_VECTOR_FPGA: + can_data.fault_sync_torque_vector_fpga.idx = msg_data_a->fault_sync_torque_vector_fpga.idx; + can_data.fault_sync_torque_vector_fpga.latched = msg_data_a->fault_sync_torque_vector_fpga.latched; handleCallbacks(msg_data_a->fault_sync_main_module.idx, msg_data_a->fault_sync_main_module.latched); break; case ID_SET_FAULT: @@ -148,7 +148,7 @@ bool initCANFilter() CAN1->sFilterRegister[3].FR2 = (ID_FAULT_SYNC_DASHBOARD << 3) | 4; CAN1->FA1R |= (1 << 4); // configure bank 4 CAN1->sFilterRegister[4].FR1 = (ID_FAULT_SYNC_PRECHARGE << 3) | 4; - CAN1->sFilterRegister[4].FR2 = (ID_FAULT_SYNC_TORQUE_VECTOR << 3) | 4; + CAN1->sFilterRegister[4].FR2 = (ID_FAULT_SYNC_TORQUE_VECTOR_FPGA << 3) | 4; CAN1->FA1R |= (1 << 5); // configure bank 5 CAN1->sFilterRegister[5].FR1 = (ID_SET_FAULT << 3) | 4; CAN1->sFilterRegister[5].FR2 = (ID_RETURN_FAULT_CONTROL << 3) | 4; diff --git a/source/l4_testing/can/can_parse.h b/source/l4_testing/can/can_parse.h index a7311307..7597c1fd 100644 --- a/source/l4_testing/can/can_parse.h +++ b/source/l4_testing/can/can_parse.h @@ -42,7 +42,7 @@ #define ID_FAULT_SYNC_DRIVELINE 0x8ca83 #define ID_FAULT_SYNC_DASHBOARD 0x8cb05 #define ID_FAULT_SYNC_PRECHARGE 0x8cac4 -#define ID_FAULT_SYNC_TORQUE_VECTOR 0x8ca42 +#define ID_FAULT_SYNC_TORQUE_VECTOR_FPGA 0x8ca42 #define ID_SET_FAULT 0x809c83e #define ID_RETURN_FAULT_CONTROL 0x809c87e #define ID_DAQ_COMMAND_TEST_NODE 0x14000ff2 @@ -71,7 +71,7 @@ #define DLC_FAULT_SYNC_DRIVELINE 3 #define DLC_FAULT_SYNC_DASHBOARD 3 #define DLC_FAULT_SYNC_PRECHARGE 3 -#define DLC_FAULT_SYNC_TORQUE_VECTOR 3 +#define DLC_FAULT_SYNC_TORQUE_VECTOR_FPGA 3 #define DLC_SET_FAULT 3 #define DLC_RETURN_FAULT_CONTROL 2 #define DLC_DAQ_COMMAND_TEST_NODE 8 @@ -332,7 +332,7 @@ typedef union { struct { uint64_t idx: 16; uint64_t latched: 1; - } fault_sync_torque_vector; + } fault_sync_torque_vector_fpga; struct { uint64_t id: 16; uint64_t value: 1; @@ -399,7 +399,7 @@ typedef struct { struct { uint16_t idx; uint8_t latched; - } fault_sync_torque_vector; + } fault_sync_torque_vector_fpga; struct { uint16_t id; uint8_t value; diff --git a/source/main_module/can/can_parse.c b/source/main_module/can/can_parse.c index 9be6d5d8..e114a0a1 100644 --- a/source/main_module/can/can_parse.c +++ b/source/main_module/can/can_parse.c @@ -128,9 +128,9 @@ void canRxUpdate() can_data.fault_sync_precharge.latched = msg_data_a->fault_sync_precharge.latched; handleCallbacks(msg_data_a->fault_sync_main_module.idx, msg_data_a->fault_sync_main_module.latched); break; - case ID_FAULT_SYNC_TORQUE_VECTOR: - can_data.fault_sync_torque_vector.idx = msg_data_a->fault_sync_torque_vector.idx; - can_data.fault_sync_torque_vector.latched = msg_data_a->fault_sync_torque_vector.latched; + case ID_FAULT_SYNC_TORQUE_VECTOR_FPGA: + can_data.fault_sync_torque_vector_fpga.idx = msg_data_a->fault_sync_torque_vector_fpga.idx; + can_data.fault_sync_torque_vector_fpga.latched = msg_data_a->fault_sync_torque_vector_fpga.latched; handleCallbacks(msg_data_a->fault_sync_main_module.idx, msg_data_a->fault_sync_main_module.latched); break; case ID_FAULT_SYNC_TEST_NODE: @@ -216,7 +216,7 @@ bool initCANFilter() CAN1->sFilterRegister[5].FR1 = (ID_FAULT_SYNC_DASHBOARD << 3) | 4; CAN1->sFilterRegister[5].FR2 = (ID_FAULT_SYNC_PRECHARGE << 3) | 4; CAN1->FA1R |= (1 << 6); // configure bank 6 - CAN1->sFilterRegister[6].FR1 = (ID_FAULT_SYNC_TORQUE_VECTOR << 3) | 4; + CAN1->sFilterRegister[6].FR1 = (ID_FAULT_SYNC_TORQUE_VECTOR_FPGA << 3) | 4; CAN1->sFilterRegister[6].FR2 = (ID_FAULT_SYNC_TEST_NODE << 3) | 4; CAN1->FA1R |= (1 << 7); // configure bank 7 CAN1->sFilterRegister[7].FR1 = (ID_SET_FAULT << 3) | 4; diff --git a/source/main_module/can/can_parse.h b/source/main_module/can/can_parse.h index aa3acc3e..111ac499 100644 --- a/source/main_module/can/can_parse.h +++ b/source/main_module/can/can_parse.h @@ -48,7 +48,7 @@ #define ID_FAULT_SYNC_DRIVELINE 0x8ca83 #define ID_FAULT_SYNC_DASHBOARD 0x8cb05 #define ID_FAULT_SYNC_PRECHARGE 0x8cac4 -#define ID_FAULT_SYNC_TORQUE_VECTOR 0x8ca42 +#define ID_FAULT_SYNC_TORQUE_VECTOR_FPGA 0x8ca42 #define ID_FAULT_SYNC_TEST_NODE 0x8cb7f #define ID_SET_FAULT 0x809c83e #define ID_RETURN_FAULT_CONTROL 0x809c87e @@ -85,7 +85,7 @@ #define DLC_FAULT_SYNC_DRIVELINE 3 #define DLC_FAULT_SYNC_DASHBOARD 3 #define DLC_FAULT_SYNC_PRECHARGE 3 -#define DLC_FAULT_SYNC_TORQUE_VECTOR 3 +#define DLC_FAULT_SYNC_TORQUE_VECTOR_FPGA 3 #define DLC_FAULT_SYNC_TEST_NODE 3 #define DLC_SET_FAULT 3 #define DLC_RETURN_FAULT_CONTROL 2 @@ -469,7 +469,7 @@ typedef union { struct { uint64_t idx: 16; uint64_t latched: 1; - } fault_sync_torque_vector; + } fault_sync_torque_vector_fpga; struct { uint64_t idx: 16; uint64_t latched: 1; @@ -567,7 +567,7 @@ typedef struct { struct { uint16_t idx; uint8_t latched; - } fault_sync_torque_vector; + } fault_sync_torque_vector_fpga; struct { uint16_t idx; uint8_t latched; diff --git a/source/navigation/can/can_parse.c b/source/navigation/can/can_parse.c deleted file mode 100644 index 17bdd9db..00000000 --- a/source/navigation/can/can_parse.c +++ /dev/null @@ -1,86 +0,0 @@ -/** - * @file can_parse.c - * @author Luke Oxley (lcoxley@purdue.edu) - * @brief Parsing of CAN messages using auto-generated structures with bit-fields - * @version 0.1 - * @date 2021-09-15 - * - * @copyright Copyright (c) 2021 - * - */ -#include "can_parse.h" - -// prototypes -bool initCANFilter(); - -can_data_t can_data; -q_handle_t *q_rx_can_a; -volatile uint32_t last_can_rx_time_ms = 0; - -void initCANParse(q_handle_t *rx_a) -{ - q_rx_can_a = rx_a; - initCANFilter(); -} - -void canRxUpdate() -{ - CanMsgTypeDef_t msg_header; - CanParsedData_t *msg_data_a; - - if (qReceive(q_rx_can_a, &msg_header) == SUCCESS_G) - { - last_can_rx_time_ms = sched.os_ticks; - msg_data_a = (CanParsedData_t *)&msg_header.Data; - /* BEGIN AUTO CASES */ - switch(msg_header.ExtId) - { - default: - __asm__("nop"); - } - /* END AUTO CASES */ - } - - /* BEGIN AUTO STALE CHECKS */ - /* END AUTO STALE CHECKS */ -} - -bool initCANFilter() -{ - CAN1->MCR |= CAN_MCR_INRQ; // Enter back into INIT state (required for changing scale) - uint32_t timeout = 0; - while (!(CAN1->MSR & CAN_MSR_INAK) && ++timeout < PHAL_CAN_INIT_TIMEOUT) - ; - if (timeout == PHAL_CAN_INIT_TIMEOUT) - return false; - - CAN1->FMR |= CAN_FMR_FINIT; // Enter init mode for filter banks - CAN1->FM1R |= 0x07FFFFFF; // Set banks 0-27 to id mode - CAN1->FS1R |= 0x07FFFFFF; // Set banks 0-27 to 32-bit scale - - /* BEGIN AUTO FILTER */ - /* END AUTO FILTER */ - - CAN1->FMR &= ~CAN_FMR_FINIT; // Enable Filters (exit filter init mode) - - // Enter back into NORMAL mode - CAN1->MCR &= ~CAN_MCR_INRQ; - while ((CAN1->MSR & CAN_MSR_INAK) && ++timeout < PHAL_CAN_INIT_TIMEOUT) - ; - - return timeout != PHAL_CAN_INIT_TIMEOUT; -} - -void canProcessRxIRQs(CanMsgTypeDef_t *rx) -{ - CanParsedData_t *msg_data_a; - - msg_data_a = (CanParsedData_t *)rx->Data; - switch (rx->ExtId) - { - /* BEGIN AUTO RX IRQ */ - /* END AUTO RX IRQ */ - default: - __asm__("nop"); - } -} \ No newline at end of file diff --git a/source/navigation/can/can_parse.h b/source/navigation/can/can_parse.h deleted file mode 100644 index d5c72eaa..00000000 --- a/source/navigation/can/can_parse.h +++ /dev/null @@ -1,277 +0,0 @@ -/** - * @file can_parse.h - * @author Luke Oxley (lcoxley@purdue.edu) - * @brief Parsing of CAN messages using auto-generated structures with bit-fields - * @version 0.1 - * @date 2021-09-15 - * - * @copyright Copyright (c) 2021 - * - */ -#ifndef _CAN_PARSE_H_ -#define _CAN_PARSE_H_ - -#include "common/queue/queue.h" -#include "common/psched/psched.h" -#include "common/phal_L4/can/can.h" - -// Make this match the node name within the can_config.json -#define NODE_NAME "Navigation" - -// Message ID definitions -/* BEGIN AUTO ID DEFS */ -#define ID_GPS_VELOCITY 0xc0002b7 -#define ID_GPS_POSITION 0xc002337 -#define ID_GPS_COORDINATES 0xc002377 -#define ID_IMU_GYRO 0xc0002f7 -#define ID_IMU_ACCEL 0xc0023b7 -#define ID_BMM_MAG 0xc0023f7 -#define ID_SFS_POS 0xc016937 -#define ID_SFS_VEL 0xc016977 -#define ID_SFS_ACC 0xc0169b7 -#define ID_SFS_ANG 0xc0169f7 -#define ID_SFS_ANG_VEL 0xc016a37 -#define ID_THROTTLE_REMAPPED 0xc0025b7 -/* END AUTO ID DEFS */ - -// Message DLC definitions -/* BEGIN AUTO DLC DEFS */ -#define DLC_GPS_VELOCITY 8 -#define DLC_GPS_POSITION 8 -#define DLC_GPS_COORDINATES 8 -#define DLC_IMU_GYRO 6 -#define DLC_IMU_ACCEL 6 -#define DLC_BMM_MAG 6 -#define DLC_SFS_POS 6 -#define DLC_SFS_VEL 6 -#define DLC_SFS_ACC 6 -#define DLC_SFS_ANG 8 -#define DLC_SFS_ANG_VEL 6 -#define DLC_THROTTLE_REMAPPED 4 -/* END AUTO DLC DEFS */ - -// Message sending macros -/* BEGIN AUTO SEND MACROS */ -#define SEND_GPS_VELOCITY(queue, gps_vel_n_, gps_vel_e_, gps_vel_d_, gps_vel_total_) do {\ - CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_GPS_VELOCITY, .DLC=DLC_GPS_VELOCITY, .IDE=1};\ - CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ - data_a->gps_velocity.gps_vel_n = gps_vel_n_;\ - data_a->gps_velocity.gps_vel_e = gps_vel_e_;\ - data_a->gps_velocity.gps_vel_d = gps_vel_d_;\ - data_a->gps_velocity.gps_vel_total = gps_vel_total_;\ - qSendToBack(&queue, &msg);\ - } while(0) -#define SEND_GPS_POSITION(queue, gps_pos_x_, gps_pos_y_, gps_pos_z_, height_) do {\ - CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_GPS_POSITION, .DLC=DLC_GPS_POSITION, .IDE=1};\ - CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ - data_a->gps_position.gps_pos_x = gps_pos_x_;\ - data_a->gps_position.gps_pos_y = gps_pos_y_;\ - data_a->gps_position.gps_pos_z = gps_pos_z_;\ - data_a->gps_position.height = height_;\ - qSendToBack(&queue, &msg);\ - } while(0) -#define SEND_GPS_COORDINATES(queue, latitude_, longitude_) do {\ - CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_GPS_COORDINATES, .DLC=DLC_GPS_COORDINATES, .IDE=1};\ - CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ - data_a->gps_coordinates.latitude = latitude_;\ - data_a->gps_coordinates.longitude = longitude_;\ - qSendToBack(&queue, &msg);\ - } while(0) -#define SEND_IMU_GYRO(queue, imu_gyro_x_, imu_gyro_y_, imu_gyro_z_) do {\ - CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_IMU_GYRO, .DLC=DLC_IMU_GYRO, .IDE=1};\ - CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ - data_a->imu_gyro.imu_gyro_x = imu_gyro_x_;\ - data_a->imu_gyro.imu_gyro_y = imu_gyro_y_;\ - data_a->imu_gyro.imu_gyro_z = imu_gyro_z_;\ - qSendToBack(&queue, &msg);\ - } while(0) -#define SEND_IMU_ACCEL(queue, imu_accel_x_, imu_accel_y_, imu_accel_z_) do {\ - CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_IMU_ACCEL, .DLC=DLC_IMU_ACCEL, .IDE=1};\ - CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ - data_a->imu_accel.imu_accel_x = imu_accel_x_;\ - data_a->imu_accel.imu_accel_y = imu_accel_y_;\ - data_a->imu_accel.imu_accel_z = imu_accel_z_;\ - qSendToBack(&queue, &msg);\ - } while(0) -#define SEND_BMM_MAG(queue, bmm_mag_x_, bmm_mag_y_, bmm_mag_z_) do {\ - CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_BMM_MAG, .DLC=DLC_BMM_MAG, .IDE=1};\ - CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ - data_a->bmm_mag.bmm_mag_x = bmm_mag_x_;\ - data_a->bmm_mag.bmm_mag_y = bmm_mag_y_;\ - data_a->bmm_mag.bmm_mag_z = bmm_mag_z_;\ - qSendToBack(&queue, &msg);\ - } while(0) -#define SEND_SFS_POS(queue, sfs_pos_x_, sfs_pos_y_, sfs_pos_z_) do {\ - CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_SFS_POS, .DLC=DLC_SFS_POS, .IDE=1};\ - CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ - data_a->sfs_pos.sfs_pos_x = sfs_pos_x_;\ - data_a->sfs_pos.sfs_pos_y = sfs_pos_y_;\ - data_a->sfs_pos.sfs_pos_z = sfs_pos_z_;\ - qSendToBack(&queue, &msg);\ - } while(0) -#define SEND_SFS_VEL(queue, sfs_vel_x_, sfs_vel_y_, sfs_vel_z_) do {\ - CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_SFS_VEL, .DLC=DLC_SFS_VEL, .IDE=1};\ - CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ - data_a->sfs_vel.sfs_vel_x = sfs_vel_x_;\ - data_a->sfs_vel.sfs_vel_y = sfs_vel_y_;\ - data_a->sfs_vel.sfs_vel_z = sfs_vel_z_;\ - qSendToBack(&queue, &msg);\ - } while(0) -#define SEND_SFS_ACC(queue, sfs_acc_x_, sfs_acc_y_, sfs_acc_z_) do {\ - CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_SFS_ACC, .DLC=DLC_SFS_ACC, .IDE=1};\ - CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ - data_a->sfs_acc.sfs_acc_x = sfs_acc_x_;\ - data_a->sfs_acc.sfs_acc_y = sfs_acc_y_;\ - data_a->sfs_acc.sfs_acc_z = sfs_acc_z_;\ - qSendToBack(&queue, &msg);\ - } while(0) -#define SEND_SFS_ANG(queue, sfs_ang_a_, sfs_ang_b_, sfs_ang_c_, sfs_ang_d_) do {\ - CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_SFS_ANG, .DLC=DLC_SFS_ANG, .IDE=1};\ - CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ - data_a->sfs_ang.sfs_ang_a = sfs_ang_a_;\ - data_a->sfs_ang.sfs_ang_b = sfs_ang_b_;\ - data_a->sfs_ang.sfs_ang_c = sfs_ang_c_;\ - data_a->sfs_ang.sfs_ang_d = sfs_ang_d_;\ - qSendToBack(&queue, &msg);\ - } while(0) -#define SEND_SFS_ANG_VEL(queue, sfs_ang_vel_x_, sfs_ang_vel_y_, sfs_ang_vel_z_) do {\ - CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_SFS_ANG_VEL, .DLC=DLC_SFS_ANG_VEL, .IDE=1};\ - CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ - data_a->sfs_ang_vel.sfs_ang_vel_x = sfs_ang_vel_x_;\ - data_a->sfs_ang_vel.sfs_ang_vel_y = sfs_ang_vel_y_;\ - data_a->sfs_ang_vel.sfs_ang_vel_z = sfs_ang_vel_z_;\ - qSendToBack(&queue, &msg);\ - } while(0) -#define SEND_THROTTLE_REMAPPED(queue, remap_k_rl_, remap_k_rr_) do {\ - CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_THROTTLE_REMAPPED, .DLC=DLC_THROTTLE_REMAPPED, .IDE=1};\ - CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ - data_a->throttle_remapped.remap_k_rl = remap_k_rl_;\ - data_a->throttle_remapped.remap_k_rr = remap_k_rr_;\ - qSendToBack(&queue, &msg);\ - } while(0) -/* END AUTO SEND MACROS */ - -// Stale Checking -#define STALE_THRESH 3 / 2 // 3 / 2 would be 150% of period -/* BEGIN AUTO UP DEFS (Update Period)*/ -/* END AUTO UP DEFS */ - -#define CHECK_STALE(stale, curr, last, period) \ - if (!stale && \ - (curr - last) > period * STALE_THRESH) \ - stale = 1 - -/* BEGIN AUTO CAN ENUMERATIONS */ -/* END AUTO CAN ENUMERATIONS */ - -// Message Raw Structures -/* BEGIN AUTO MESSAGE STRUCTURE */ -typedef union { - struct { - uint64_t gps_vel_n: 16; - uint64_t gps_vel_e: 16; - uint64_t gps_vel_d: 16; - uint64_t gps_vel_total: 16; - } gps_velocity; - struct { - uint64_t gps_pos_x: 16; - uint64_t gps_pos_y: 16; - uint64_t gps_pos_z: 16; - uint64_t height: 16; - } gps_position; - struct { - uint64_t latitude: 32; - uint64_t longitude: 32; - } gps_coordinates; - struct { - uint64_t imu_gyro_x: 16; - uint64_t imu_gyro_y: 16; - uint64_t imu_gyro_z: 16; - } imu_gyro; - struct { - uint64_t imu_accel_x: 16; - uint64_t imu_accel_y: 16; - uint64_t imu_accel_z: 16; - } imu_accel; - struct { - uint64_t bmm_mag_x: 16; - uint64_t bmm_mag_y: 16; - uint64_t bmm_mag_z: 16; - } bmm_mag; - struct { - uint64_t sfs_pos_x: 16; - uint64_t sfs_pos_y: 16; - uint64_t sfs_pos_z: 16; - } sfs_pos; - struct { - uint64_t sfs_vel_x: 16; - uint64_t sfs_vel_y: 16; - uint64_t sfs_vel_z: 16; - } sfs_vel; - struct { - uint64_t sfs_acc_x: 16; - uint64_t sfs_acc_y: 16; - uint64_t sfs_acc_z: 16; - } sfs_acc; - struct { - uint64_t sfs_ang_a: 16; - uint64_t sfs_ang_b: 16; - uint64_t sfs_ang_c: 16; - uint64_t sfs_ang_d: 16; - } sfs_ang; - struct { - uint64_t sfs_ang_vel_x: 16; - uint64_t sfs_ang_vel_y: 16; - uint64_t sfs_ang_vel_z: 16; - } sfs_ang_vel; - struct { - uint64_t remap_k_rl: 16; - uint64_t remap_k_rr: 16; - } throttle_remapped; - uint8_t raw_data[8]; -} __attribute__((packed)) CanParsedData_t; -/* END AUTO MESSAGE STRUCTURE */ - -// contains most up to date received -// type for each variable matches that defined in JSON -/* BEGIN AUTO CAN DATA STRUCTURE */ -typedef struct { -} can_data_t; -/* END AUTO CAN DATA STRUCTURE */ - -extern can_data_t can_data; - -/* BEGIN AUTO EXTERN CALLBACK */ -extern void handleCallbacks(uint16_t id, bool latched); -extern void set_fault_daq(uint16_t id, bool value); -extern void return_fault_control(uint16_t id); -extern void send_fault(uint16_t id, bool latched); -/* END AUTO EXTERN CALLBACK */ - -/* BEGIN AUTO EXTERN RX IRQ */ -/* END AUTO EXTERN RX IRQ */ - -/** - * @brief Setup queue and message filtering - * - * @param q_rx_can RX buffer of CAN messages - */ -void initCANParse(q_handle_t *q_rx_can_a); - -/** - * @brief Pull message off of rx buffer, - * update can_data struct, - * check for stale messages - */ -void canRxUpdate(); - -/** - * @brief Process any rx message callbacks from the CAN Rx IRQ - * - * @param rx rx data from message just recieved - */ -void canProcessRxIRQs(CanMsgTypeDef_t *rx); - -extern volatile uint32_t last_can_rx_time_ms; - -#endif \ No newline at end of file diff --git a/source/navigation/main.c b/source/navigation/main.c deleted file mode 100644 index 55412c6d..00000000 --- a/source/navigation/main.c +++ /dev/null @@ -1,446 +0,0 @@ -/* System Includes */ -#include "stm32l471xx.h" -#include "common/phal_L4/gpio/gpio.h" -#include "common/phal_L4/rcc/rcc.h" -#include "common/phal_L4/spi/spi.h" -#include "common/psched/psched.h" -#include "common/phal_L4/usart/usart.h" - -/* Module Includes */ -#include "bmi088.h" -#include "can_parse.h" -#include "bsxlite_interface.h" -#include "imu.h" -#include "main.h" -#include "gps.h" -#include "bmm150.h" -#include "SFS.h" -#include "SFS_pp.h" - -extern q_handle_t q_tx_can; - -uint8_t collect_test[100] = {0}; - -GPIOInitConfig_t gpio_config[] = { - // Status Indicators - GPIO_INIT_OUTPUT(ERR_LED_GPIO_Port, ERR_LED_Pin, GPIO_OUTPUT_LOW_SPEED), - GPIO_INIT_OUTPUT(CONN_LED_GPIO_Port, CONN_LED_Pin, GPIO_OUTPUT_LOW_SPEED), - GPIO_INIT_OUTPUT(HEARTBEAT_GPIO_Port, HEARTBEAT_Pin, GPIO_OUTPUT_LOW_SPEED), - - // SPI - GPIO_INIT_AF(SPI_SCLK_GPIO_Port, SPI_SCLK_Pin, 5, GPIO_OUTPUT_HIGH_SPEED, GPIO_OUTPUT_PUSH_PULL, GPIO_INPUT_PULL_DOWN), - GPIO_INIT_AF(SPI_MOSI_GPIO_Port, SPI_MOSI_Pin, 5, GPIO_OUTPUT_HIGH_SPEED, GPIO_OUTPUT_PUSH_PULL, GPIO_INPUT_PULL_DOWN), - GPIO_INIT_AF(SPI_MISO_GPIO_Port, SPI_MISO_Pin, 5, GPIO_OUTPUT_HIGH_SPEED, GPIO_OUTPUT_OPEN_DRAIN, GPIO_INPUT_OPEN_DRAIN), - GPIO_INIT_OUTPUT(SPI_CS_ACEL_GPIO_Port, SPI_CS_ACEL_Pin, GPIO_OUTPUT_HIGH_SPEED), - GPIO_INIT_OUTPUT(SPI_CS_GYRO_GPIO_Port, SPI_CS_GYRO_Pin, GPIO_OUTPUT_HIGH_SPEED), - GPIO_INIT_OUTPUT(SPI_CS_MAG_GPIO_Port, SPI_CS_MAG_Pin, GPIO_OUTPUT_HIGH_SPEED), - - // GPS USART - GPIO_INIT_USART3RX_PC5, - GPIO_INIT_USART3TX_PC4, - - // EEPROM - GPIO_INIT_OUTPUT(NAV_EEPROM_CS_GPIO_PORT, NAV_EEPROM_CS_PIN, GPIO_OUTPUT_HIGH_SPEED), - GPIO_INIT_OUTPUT(NAV_WP_GPIO_PORT, NAV_WP_PIN, GPIO_OUTPUT_HIGH_SPEED), - - // CAN - GPIO_INIT_CANRX_PA11, - GPIO_INIT_CANTX_PA12}; - -/* USART Configuration */ -// M9N GPS -dma_init_t usart_gps_tx_dma_config = USART3_TXDMA_CONT_CONFIG(NULL, 1); -dma_init_t usart_gps_rx_dma_config = USART3_RXDMA_CONT_CONFIG(NULL, 2); -usart_init_t huart_gps = { - .baud_rate = 115200, - .word_length = WORD_8, - .hw_flow_ctl = HW_DISABLE, - .mode = MODE_TX_RX, - .stop_bits = SB_ONE, - .parity = PT_NONE, - .obsample = OB_DISABLE, - .ovsample = OV_16, - .adv_feature.rx_inv = false, - .adv_feature.tx_inv = false, - .adv_feature.auto_baud = false, - .adv_feature.data_inv = false, - .adv_feature.msb_first = false, - .adv_feature.overrun = false, - .adv_feature.dma_on_rx_err = false, - .tx_dma_cfg = &usart_gps_tx_dma_config, - .rx_dma_cfg = &usart_gps_rx_dma_config}; - -#define TargetCoreClockrateHz 16000000 -ClockRateConfig_t clock_config = { - .system_source = SYSTEM_CLOCK_SRC_HSI, - .system_clock_target_hz = TargetCoreClockrateHz, - .ahb_clock_target_hz = (TargetCoreClockrateHz / 1), - .apb1_clock_target_hz = (TargetCoreClockrateHz / (1)), - .apb2_clock_target_hz = (TargetCoreClockrateHz / (1)), -}; - -/* Locals for Clock Rates */ -extern uint32_t APB1ClockRateHz; -extern uint32_t APB2ClockRateHz; -extern uint32_t AHBClockRateHz; -extern uint32_t PLLClockRateHz; - -dma_init_t spi_rx_dma_config = SPI1_RXDMA_CONT_CONFIG(NULL, 2); -dma_init_t spi_tx_dma_config = SPI1_TXDMA_CONT_CONFIG(NULL, 1); - -SPI_InitConfig_t spi_config = { - .data_rate = TargetCoreClockrateHz / 64, - .data_len = 8, - .nss_sw = true, - .nss_gpio_port = SPI_CS_MAG_GPIO_Port, - .nss_gpio_pin = SPI_CS_MAG_Pin, - .rx_dma_cfg = NULL, - .tx_dma_cfg = NULL, - // .rx_dma_cfg = &spi_rx_dma_config, - // .tx_dma_cfg = &spi_tx_dma_config, - .periph = SPI1}; - -uint8_t num_iterations = 0; - -// Test Nav Message -GPS_Handle_t testGPSHandle = {}; -vector_3d_t accel_in, gyro_in, mag_in; - -BMI088_Handle_t bmi_config = { - .accel_csb_gpio_port = SPI_CS_ACEL_GPIO_Port, - .accel_csb_pin = SPI_CS_ACEL_Pin, - .accel_range = ACCEL_RANGE_3G, - .accel_odr = ACCEL_ODR_50Hz, - .accel_bwp = ACCEL_OS_NORMAL, - .gyro_csb_gpio_port = SPI_CS_GYRO_GPIO_Port, - .gyro_csb_pin = SPI_CS_GYRO_Pin, - .gyro_datarate = GYRO_DR_100Hz_32Hz, - .gyro_range = GYRO_RANGE_250, - .spi = &spi_config}; -BMM150_Handle_t bmm_config = { - .spi = &spi_config, - .mag_csb_gpio_port = SPI_CS_MAG_GPIO_Port, - .mag_csb_pin = SPI_CS_MAG_Pin}; -IMU_Handle_t imu_h = { - .bmi = &bmi_config, -}; - -/* Function Prototypes */ -void canTxUpdate(void); -void heartBeatLED(void); -void preflightAnimation(void); -void preflightChecks(void); -void sendIMUData(void); -void collectGPSData(void); -void collectMagData(void); -extern void HardFault_Handler(void); -q_handle_t q_tx_can, q_rx_can; - -/* SFS Definitions */ -static ExtU rtU; /* External inputs */ -static ExtY rtY; /* External outputs */ -static RT_MODEL rtM_; -static RT_MODEL *const rtMPtr = &rtM_; /* Real-time model */ -static DW rtDW; /* Observable states */ -static RT_MODEL *const rtM = rtMPtr; - -int main(void) -{ - // memset(spi2_tx_buffer + 8, 255, 100 - 8); - - /* Data Struct Initialization */ - qConstruct(&q_tx_can, sizeof(CanMsgTypeDef_t)); - qConstruct(&q_rx_can, sizeof(CanMsgTypeDef_t)); - - /* HAL Initialization */ - if (0 != PHAL_configureClockRates(&clock_config)) - { - HardFault_Handler(); - } - - /* GPIO initialization */ - if (!PHAL_initGPIO(gpio_config, sizeof(gpio_config) / sizeof(GPIOInitConfig_t))) - { - HardFault_Handler(); - } - - /* USART initialization */ - huart_gps.rx_dma_cfg->circular = true; - if (!PHAL_initUSART(USART3, &huart_gps, APB1ClockRateHz)) - { - HardFault_Handler(); - } - PHAL_usartRxDma(USART3, &huart_gps, (uint16_t *)testGPSHandle.raw_message, 100); - - /* SPI initialization */ - if (!PHAL_SPI_init(&spi_config)) - { - HardFault_Handler(); - } - spi_config.data_rate = APB2ClockRateHz / 16; - - PHAL_writeGPIO(SPI_CS_ACEL_GPIO_Port, SPI_CS_ACEL_Pin, 1); - PHAL_writeGPIO(SPI_CS_GYRO_GPIO_Port, SPI_CS_GYRO_Pin, 1); - PHAL_writeGPIO(SPI_CS_MAG_GPIO_Port, SPI_CS_MAG_Pin, 1); - - // while (1) - // { - // PHAL_usartRxBl(USART3, (uint16_t *)collect_test, 100); - // // PHAL_toggleGPIO(ERR_LED_GPIO_Port, ERR_LED_Pin); - // } - - /* Task Creation */ - schedInit(APB1ClockRateHz); - configureAnim(preflightAnimation, preflightChecks, 74, 1000); - - taskCreateBackground(canTxUpdate); - taskCreateBackground(canRxUpdate); - - taskCreate(heartBeatLED, 500); - - taskCreate(sendIMUData, 10); - taskCreate(collectGPSData, 40); - taskCreate(collectMagData, 40); - taskCreate(SFS_MAIN, 10); - - /* No Way Home */ - schedStart(); - - return 0; -} - -void preflightChecks(void) -{ - static uint16_t state; - - switch (state++) - { - case 0: - if (!PHAL_initCAN(CAN1, false)) - { - HardFault_Handler(); - } - NVIC_EnableIRQ(CAN1_RX0_IRQn); - break; - case 2: - if (!BMM150_readID(&bmm_config)) - { - asm("nop"); - } - break; - case 1: - if (!BMI088_init(&bmi_config)) - { - HardFault_Handler(); - } - break; - case 250: - BMI088_powerOnAccel(&bmi_config); - break; - case 500: - if (!BMI088_initAccel(&bmi_config)) - HardFault_Handler(); - break; - case 700: - /* Pack model data into RTM */ - rtM->dwork = &rtDW; - - /* Initialize model */ - SFS_initialize(rtM); - default: - if (state > 750) - { - if (!imu_init(&imu_h)) - HardFault_Handler(); - initCANParse(&q_rx_can); - registerPreflightComplete(1); - state = 750; // prevent wrap around - } - break; - } -} - -void preflightAnimation(void) -{ - static uint32_t time; - - PHAL_writeGPIO(HEARTBEAT_GPIO_Port, HEARTBEAT_Pin, 0); - PHAL_writeGPIO(ERR_LED_GPIO_Port, ERR_LED_Pin, 0); - PHAL_writeGPIO(CONN_LED_GPIO_Port, CONN_LED_Pin, 0); - - switch (time++ % 6) - { - case 0: - case 5: - PHAL_writeGPIO(HEARTBEAT_GPIO_Port, HEARTBEAT_Pin, 1); - break; - case 1: - case 2: - case 3: - PHAL_writeGPIO(ERR_LED_GPIO_Port, ERR_LED_Pin, 1); - break; - case 4: - PHAL_writeGPIO(CONN_LED_GPIO_Port, CONN_LED_Pin, 1); - break; - } -} -void heartBeatLED(void) -{ - PHAL_toggleGPIO(HEARTBEAT_GPIO_Port, HEARTBEAT_Pin); - - // if ((sched.os_ticks - last_can_rx_time_ms) >= CONN_LED_MS_THRESH) - // PHAL_writeGPIO(CONN_LED_GPIO_Port, CONN_LED_Pin, 0); - // else PHAL_writeGPIO(CONN_LED_GPIO_Port, CONN_LED_Pin, 1); -} - -void sendIMUData(void) -{ - imu_periodic(&imu_h, &rtU); -} - -uint8_t poll_pvt[] = {"0xB5, 0x62, 0x01, 0x07, 0x00, 0x00, 0x08, 0x19"}; - -// Test function for usartRxDma -void collectGPSData(void) -{ - testGPSHandle.messages_received++; - BMI088_readGyro(&bmi_config, &gyro_in); - BMI088_readAccel(&bmi_config, &accel_in); - testGPSHandle.acceleration = accel_in; - testGPSHandle.gyroscope = gyro_in; - parseVelocity(&testGPSHandle, &rtU); - // PHAL_usartRxBl(USART3, (uint16_t *)collect_test, 100); - // PHAL_usartRxDma(USART3, &huart_gps, (uint16_t *)collect_test, 100); - // PHAL_usartTxDma(USART3, &huart_gps, (uint16_t *)poll_pvt, strlen(poll_pvt)); - // while (PHAL_SPI_busy(&spi2_config)) - // ; - // if (num_iterations == 1) - // { - // memset(spi2_tx_buffer, 255, sizeof(spi2_tx_buffer)); - // } - // else - // { - // PHAL_SPI_transfer(&spi2_config, spi2_tx_buffer, 100, spi2_rx_buffer); - // } - - // if (spi2_rx_buffer[0] != 255) - // { - // asm("nop"); - // } - // while (PHAL_SPI_busy(&spi2_config)) - // ; - // num_iterations++; -} - -void collectMagData(void) -{ - BMM150_readMag(&bmm_config, &rtU); -} - -void canTxUpdate(void) -{ - CanMsgTypeDef_t tx_msg; - if (qReceive(&q_tx_can, &tx_msg) == SUCCESS_G) // Check queue for items and take if there is one - { - PHAL_txCANMessage(&tx_msg); - } -} - -void CAN1_RX0_IRQHandler() -{ - if (CAN1->RF0R & CAN_RF0R_FOVR0) // FIFO Overrun - CAN1->RF0R &= !(CAN_RF0R_FOVR0); - - if (CAN1->RF0R & CAN_RF0R_FULL0) // FIFO Full - CAN1->RF0R &= !(CAN_RF0R_FULL0); - - if (CAN1->RF0R & CAN_RF0R_FMP0_Msk) // Release message pending - { - CanMsgTypeDef_t rx; - rx.Bus = CAN1; - - // Get either StdId or ExtId - rx.IDE = CAN_RI0R_IDE & CAN1->sFIFOMailBox[0].RIR; - if (rx.IDE) - { - rx.ExtId = ((CAN_RI0R_EXID | CAN_RI0R_STID) & CAN1->sFIFOMailBox[0].RIR) >> CAN_RI0R_EXID_Pos; - } - else - { - rx.StdId = (CAN_RI0R_STID & CAN1->sFIFOMailBox[0].RIR) >> CAN_RI0R_STID_Pos; - } - - rx.DLC = (CAN_RDT0R_DLC & CAN1->sFIFOMailBox[0].RDTR) >> CAN_RDT0R_DLC_Pos; - - rx.Data[0] = (uint8_t)(CAN1->sFIFOMailBox[0].RDLR >> 0) & 0xFF; - rx.Data[1] = (uint8_t)(CAN1->sFIFOMailBox[0].RDLR >> 8) & 0xFF; - rx.Data[2] = (uint8_t)(CAN1->sFIFOMailBox[0].RDLR >> 16) & 0xFF; - rx.Data[3] = (uint8_t)(CAN1->sFIFOMailBox[0].RDLR >> 24) & 0xFF; - rx.Data[4] = (uint8_t)(CAN1->sFIFOMailBox[0].RDHR >> 0) & 0xFF; - rx.Data[5] = (uint8_t)(CAN1->sFIFOMailBox[0].RDHR >> 8) & 0xFF; - rx.Data[6] = (uint8_t)(CAN1->sFIFOMailBox[0].RDHR >> 16) & 0xFF; - rx.Data[7] = (uint8_t)(CAN1->sFIFOMailBox[0].RDHR >> 24) & 0xFF; - - CAN1->RF0R |= (CAN_RF0R_RFOM0); - - qSendToBack(&q_rx_can, &rx); // Add to queue (qSendToBack is interrupt safe) - } -} - -// void main_module_bl_cmd_CALLBACK(CanParsedData_t *msg_data_a) -// { -// if (can_data.main_module_bl_cmd.cmd == BLCMD_RST) -// Bootloader_ResetForFirmwareDownload(); -// } - -void SFS_MAIN(void) -{ - SFS_pp(&rtU); - - static boolean_T OverrunFlag = false; - - /* Disable interrupts here */ - - /* Check for overrun */ - if (OverrunFlag) - { - rtmSetErrorStatus(rtM, "Overrun"); - return; - } - - OverrunFlag = true; - - /* Save FPU context here (if necessary) */ - /* Re-enable timer or interrupt here */ - /* Set model inputs here */ - - /* Step the model */ - SFS_step(rtM, &rtU, &rtY); - SEND_SFS_POS(q_tx_can, (int16_t)(rtY.pos_VNED[0] * 100), - (int16_t)(rtY.pos_VNED[1] * 100), (int16_t)(rtY.pos_VNED[2] * 100)); - SEND_SFS_VEL(q_tx_can, (int16_t)(rtY.vel_VNED[0] * 100), - (int16_t)(rtY.vel_VNED[1] * 100), (int16_t)(rtY.vel_VNED[2] * 100)); - SEND_SFS_ACC(q_tx_can, (int16_t)(rtY.acc_VNED[0] * 100), - (int16_t)(rtY.acc_VNED[1] * 100), (int16_t)(rtY.acc_VNED[2] * 100)); - SEND_SFS_ANG(q_tx_can, (int16_t)(rtY.ang_NED[0] * 10000), - (int16_t)(rtY.ang_NED[1] * 10000), (int16_t)(rtY.ang_NED[2] * 10000), (int16_t)(rtY.ang_NED[3] * 10000)); - SEND_SFS_ANG_VEL(q_tx_can, (int16_t)(rtY.angvel_VNED[0] * 10000), - (int16_t)(rtY.angvel_VNED[1] * 10000), (int16_t)(rtY.angvel_VNED[2] * 10000)); - /* Get model outputs here */ - - /* Indicate task complete */ - OverrunFlag = false; - - /* Disable interrupts here */ - /* Restore FPU context here (if necessary) */ - /* Enable interrupts here */ -} - -void HardFault_Handler() -{ - PHAL_writeGPIO(ERR_LED_GPIO_Port, ERR_LED_Pin, 1); - while (1) - { - __asm__("nop"); - } -} \ No newline at end of file diff --git a/source/navigation/main.h b/source/navigation/main.h deleted file mode 100644 index 36d43844..00000000 --- a/source/navigation/main.h +++ /dev/null @@ -1,53 +0,0 @@ -/** - * @file main.h - * @author Luke Oxley (lcoxley@purdue.edu) - * @brief Software for controlling navigation - sensor acquisition - * @version 0.1 - * @date 2022-12-08 - * - * @copyright Copyright (c) 2022 - * - */ -#ifndef _MAIN_H_ -#define _MAIN_H_ - -// STM32L471RET - -// Status Indicators -#define ERR_LED_GPIO_Port (GPIOB) -#define ERR_LED_Pin (5) -#define CONN_LED_GPIO_Port (GPIOB) -#define CONN_LED_Pin (7) -#define CONN_LED_MS_THRESH (500) -#define HEARTBEAT_GPIO_Port (GPIOB) -#define HEARTBEAT_Pin (6) - -// SPI IMU -#define SPI_SCLK_GPIO_Port (GPIOA) -#define SPI_SCLK_Pin (5) -#define SPI_MISO_GPIO_Port (GPIOA) -#define SPI_MISO_Pin (6) -#define SPI_MOSI_GPIO_Port (GPIOA) -#define SPI_MOSI_Pin (7) - -#define SPI_CS_ACEL_GPIO_Port (GPIOA) -#define SPI_CS_ACEL_Pin (3) -#define SPI_CS_GYRO_GPIO_Port (GPIOA) -#define SPI_CS_GYRO_Pin (2) -#define SPI_CS_MAG_GPIO_Port (GPIOB) -#define SPI_CS_MAG_Pin (0) - -// USART GPS -#define GPS_RX_GPIO_Port (GPIOC) -#define GPS_RX_Pin (5) -#define GPS_TX_GPIO_Port (GPIOC) -#define GPS_TX_Pin (4) - -// EEPROM -#define NAV_EEPROM_CS_GPIO_PORT (GPIOB) -#define NAV_EEPROM_CS_PIN (12) -#define NAV_WP_GPIO_PORT (GPIOB) -#define NAV_WP_PIN (13) - -#endif \ No newline at end of file diff --git a/source/precharge/can/can_parse.c b/source/precharge/can/can_parse.c index 8c3f15f9..85701d0a 100644 --- a/source/precharge/can/can_parse.c +++ b/source/precharge/can/can_parse.c @@ -237,9 +237,9 @@ void canRxUpdate() can_data.fault_sync_dashboard.latched = msg_data_a->fault_sync_dashboard.latched; handleCallbacks(msg_data_a->fault_sync_main_module.idx, msg_data_a->fault_sync_main_module.latched); break; - case ID_FAULT_SYNC_TORQUE_VECTOR: - can_data.fault_sync_torque_vector.idx = msg_data_a->fault_sync_torque_vector.idx; - can_data.fault_sync_torque_vector.latched = msg_data_a->fault_sync_torque_vector.latched; + case ID_FAULT_SYNC_TORQUE_VECTOR_FPGA: + can_data.fault_sync_torque_vector_fpga.idx = msg_data_a->fault_sync_torque_vector_fpga.idx; + can_data.fault_sync_torque_vector_fpga.latched = msg_data_a->fault_sync_torque_vector_fpga.latched; handleCallbacks(msg_data_a->fault_sync_main_module.idx, msg_data_a->fault_sync_main_module.latched); break; case ID_FAULT_SYNC_TEST_NODE: @@ -346,7 +346,7 @@ bool initCANFilter() CAN1->sFilterRegister[3].FR1 = (ID_FAULT_SYNC_DRIVELINE << 3) | 4; CAN1->sFilterRegister[3].FR2 = (ID_FAULT_SYNC_DASHBOARD << 3) | 4; CAN1->FA1R |= (1 << 4); // configure bank 4 - CAN1->sFilterRegister[4].FR1 = (ID_FAULT_SYNC_TORQUE_VECTOR << 3) | 4; + CAN1->sFilterRegister[4].FR1 = (ID_FAULT_SYNC_TORQUE_VECTOR_FPGA << 3) | 4; CAN1->sFilterRegister[4].FR2 = (ID_FAULT_SYNC_TEST_NODE << 3) | 4; CAN1->FA1R |= (1 << 5); // configure bank 5 CAN1->sFilterRegister[5].FR1 = (ID_SET_FAULT << 3) | 4; diff --git a/source/precharge/can/can_parse.h b/source/precharge/can/can_parse.h index b96c8e40..979ad4de 100644 --- a/source/precharge/can/can_parse.h +++ b/source/precharge/can/can_parse.h @@ -61,7 +61,7 @@ #define ID_FAULT_SYNC_MAIN_MODULE 0x8ca01 #define ID_FAULT_SYNC_DRIVELINE 0x8ca83 #define ID_FAULT_SYNC_DASHBOARD 0x8cb05 -#define ID_FAULT_SYNC_TORQUE_VECTOR 0x8ca42 +#define ID_FAULT_SYNC_TORQUE_VECTOR_FPGA 0x8ca42 #define ID_FAULT_SYNC_TEST_NODE 0x8cb7f #define ID_SET_FAULT 0x809c83e #define ID_RETURN_FAULT_CONTROL 0x809c87e @@ -111,7 +111,7 @@ #define DLC_FAULT_SYNC_MAIN_MODULE 3 #define DLC_FAULT_SYNC_DRIVELINE 3 #define DLC_FAULT_SYNC_DASHBOARD 3 -#define DLC_FAULT_SYNC_TORQUE_VECTOR 3 +#define DLC_FAULT_SYNC_TORQUE_VECTOR_FPGA 3 #define DLC_FAULT_SYNC_TEST_NODE 3 #define DLC_SET_FAULT 3 #define DLC_RETURN_FAULT_CONTROL 2 @@ -536,7 +536,7 @@ typedef union { struct { uint64_t idx: 16; uint64_t latched: 1; - } fault_sync_torque_vector; + } fault_sync_torque_vector_fpga; struct { uint64_t idx: 16; uint64_t latched: 1; @@ -750,7 +750,7 @@ typedef struct { struct { uint16_t idx; uint8_t latched; - } fault_sync_torque_vector; + } fault_sync_torque_vector_fpga; struct { uint16_t idx; uint8_t latched; diff --git a/source/torque_vector/CMakeLists.txt b/source/torque_vector/CMakeLists.txt index 2292cfaf..8d5829fa 100644 --- a/source/torque_vector/CMakeLists.txt +++ b/source/torque_vector/CMakeLists.txt @@ -7,11 +7,14 @@ STRING(TOLOWER ${COMPONENT_NAME} COMPONENT_NAME) set(TARGET_NAME ${COMPONENT_NAME}.elf) add_executable(${TARGET_NAME}) +# Add in bsxlite +target_link_libraries(${TARGET_NAME} ${CMAKE_CURRENT_LIST_DIR}/bsxlite/libalgobsx.a) + # Propreties are set in order to make the common component set_target_properties(${TARGET_NAME} PROPERTIES COMPONENT_NAME ${COMPONENT_NAME} COMPONENT_DIR ${CMAKE_CURRENT_LIST_DIR} - LINKER_SCRIPT "STM32L432KCUx_FLASH" - COMMON_LIBS "CMSIS_L432;PSCHED;QUEUE;PHAL_L432;BOOTLOADER_COMMON_L432;FAULTS" + LINKER_SCRIPT "STM32L496VGTx_FLASH" + COMMON_LIBS "CMSIS_L471;PSCHED_L471;QUEUE;PHAL_L471;libm.a;" ) COMMON_FIRMWARE_COMPONENT(${TARGET_NAME}) diff --git a/source/navigation/bmi/bmi088.c b/source/torque_vector/bmi/bmi088.c similarity index 100% rename from source/navigation/bmi/bmi088.c rename to source/torque_vector/bmi/bmi088.c diff --git a/source/navigation/bmi/bmi088.h b/source/torque_vector/bmi/bmi088.h similarity index 100% rename from source/navigation/bmi/bmi088.h rename to source/torque_vector/bmi/bmi088.h diff --git a/source/navigation/bmm/bmm150.c b/source/torque_vector/bmm/bmm150.c similarity index 99% rename from source/navigation/bmm/bmm150.c rename to source/torque_vector/bmm/bmm150.c index bed02915..3e800c74 100644 --- a/source/navigation/bmm/bmm150.c +++ b/source/torque_vector/bmm/bmm150.c @@ -16,7 +16,7 @@ #include "common_defs.h" #include "stdbool.h" #include "can_parse.h" -#include "SFS_pp.h" +#include "sfs_pp.h" static inline void BMM150_selectMag(BMM150_Handle_t *bmm); bool BMM150_readID(BMM150_Handle_t *bmm); diff --git a/source/navigation/bmm/bmm150.h b/source/torque_vector/bmm/bmm150.h similarity index 100% rename from source/navigation/bmm/bmm150.h rename to source/torque_vector/bmm/bmm150.h diff --git a/source/navigation/bsxlite/bsxlite_interface.h b/source/torque_vector/bsxlite/bsxlite_interface.h similarity index 100% rename from source/navigation/bsxlite/bsxlite_interface.h rename to source/torque_vector/bsxlite/bsxlite_interface.h diff --git a/source/navigation/bsxlite/libalgobsx.a b/source/torque_vector/bsxlite/libalgobsx.a similarity index 100% rename from source/navigation/bsxlite/libalgobsx.a rename to source/torque_vector/bsxlite/libalgobsx.a diff --git a/source/navigation/bsxlite/memory_footprint.txt b/source/torque_vector/bsxlite/memory_footprint.txt similarity index 100% rename from source/navigation/bsxlite/memory_footprint.txt rename to source/torque_vector/bsxlite/memory_footprint.txt diff --git a/source/torque_vector/can/can_parse.c b/source/torque_vector/can/can_parse.c index c481ee22..17bdd9db 100644 --- a/source/torque_vector/can/can_parse.c +++ b/source/torque_vector/can/can_parse.c @@ -4,9 +4,9 @@ * @brief Parsing of CAN messages using auto-generated structures with bit-fields * @version 0.1 * @date 2021-09-15 - * + * * @copyright Copyright (c) 2021 - * + * */ #include "can_parse.h" @@ -14,9 +14,10 @@ bool initCANFilter(); can_data_t can_data; -q_handle_t* q_rx_can_a; +q_handle_t *q_rx_can_a; +volatile uint32_t last_can_rx_time_ms = 0; -void initCANParse(q_handle_t* rx_a) +void initCANParse(q_handle_t *rx_a) { q_rx_can_a = rx_a; initCANFilter(); @@ -25,79 +26,15 @@ void initCANParse(q_handle_t* rx_a) void canRxUpdate() { CanMsgTypeDef_t msg_header; - CanParsedData_t* msg_data_a; + CanParsedData_t *msg_data_a; - if(qReceive(q_rx_can_a, &msg_header) == SUCCESS_G) + if (qReceive(q_rx_can_a, &msg_header) == SUCCESS_G) { - msg_data_a = (CanParsedData_t *) &msg_header.Data; + last_can_rx_time_ms = sched.os_ticks; + msg_data_a = (CanParsedData_t *)&msg_header.Data; /* BEGIN AUTO CASES */ switch(msg_header.ExtId) { - case ID_FRONT_WHEEL_DATA: - can_data.front_wheel_data.left_speed = msg_data_a->front_wheel_data.left_speed; - can_data.front_wheel_data.right_speed = msg_data_a->front_wheel_data.right_speed; - can_data.front_wheel_data.left_normal = msg_data_a->front_wheel_data.left_normal; - can_data.front_wheel_data.right_normal = msg_data_a->front_wheel_data.right_normal; - can_data.front_wheel_data.stale = 0; - can_data.front_wheel_data.last_rx = sched.os_ticks; - break; - case ID_REAR_WHEEL_DATA: - can_data.rear_wheel_data.left_speed = msg_data_a->rear_wheel_data.left_speed; - can_data.rear_wheel_data.right_speed = msg_data_a->rear_wheel_data.right_speed; - can_data.rear_wheel_data.left_normal = msg_data_a->rear_wheel_data.left_normal; - can_data.rear_wheel_data.right_normal = msg_data_a->rear_wheel_data.right_normal; - can_data.rear_wheel_data.stale = 0; - can_data.rear_wheel_data.last_rx = sched.os_ticks; - break; - case ID_BITSTREAM_DATA: - can_data.bitstream_data.d0 = msg_data_a->bitstream_data.d0; - can_data.bitstream_data.d1 = msg_data_a->bitstream_data.d1; - can_data.bitstream_data.d2 = msg_data_a->bitstream_data.d2; - can_data.bitstream_data.d3 = msg_data_a->bitstream_data.d3; - can_data.bitstream_data.d4 = msg_data_a->bitstream_data.d4; - can_data.bitstream_data.d5 = msg_data_a->bitstream_data.d5; - can_data.bitstream_data.d6 = msg_data_a->bitstream_data.d6; - can_data.bitstream_data.d7 = msg_data_a->bitstream_data.d7; - break; - case ID_BITSTREAM_REQUEST: - can_data.bitstream_request.download_request = msg_data_a->bitstream_request.download_request; - can_data.bitstream_request.download_size = msg_data_a->bitstream_request.download_size; - bitstream_request_CALLBACK(msg_data_a); - break; - case ID_FAULT_SYNC_MAIN_MODULE: - can_data.fault_sync_main_module.idx = msg_data_a->fault_sync_main_module.idx; - can_data.fault_sync_main_module.latched = msg_data_a->fault_sync_main_module.latched; - handleCallbacks(msg_data_a->fault_sync_main_module.idx, msg_data_a->fault_sync_main_module.latched); - break; - case ID_FAULT_SYNC_DRIVELINE: - can_data.fault_sync_driveline.idx = msg_data_a->fault_sync_driveline.idx; - can_data.fault_sync_driveline.latched = msg_data_a->fault_sync_driveline.latched; - handleCallbacks(msg_data_a->fault_sync_main_module.idx, msg_data_a->fault_sync_main_module.latched); - break; - case ID_FAULT_SYNC_DASHBOARD: - can_data.fault_sync_dashboard.idx = msg_data_a->fault_sync_dashboard.idx; - can_data.fault_sync_dashboard.latched = msg_data_a->fault_sync_dashboard.latched; - handleCallbacks(msg_data_a->fault_sync_main_module.idx, msg_data_a->fault_sync_main_module.latched); - break; - case ID_FAULT_SYNC_PRECHARGE: - can_data.fault_sync_precharge.idx = msg_data_a->fault_sync_precharge.idx; - can_data.fault_sync_precharge.latched = msg_data_a->fault_sync_precharge.latched; - handleCallbacks(msg_data_a->fault_sync_main_module.idx, msg_data_a->fault_sync_main_module.latched); - break; - case ID_FAULT_SYNC_TEST_NODE: - can_data.fault_sync_test_node.idx = msg_data_a->fault_sync_test_node.idx; - can_data.fault_sync_test_node.latched = msg_data_a->fault_sync_test_node.latched; - handleCallbacks(msg_data_a->fault_sync_main_module.idx, msg_data_a->fault_sync_main_module.latched); - break; - case ID_SET_FAULT: - can_data.set_fault.id = msg_data_a->set_fault.id; - can_data.set_fault.value = msg_data_a->set_fault.value; - set_fault_daq(msg_data_a->set_fault.id, msg_data_a->set_fault.value); - break; - case ID_RETURN_FAULT_CONTROL: - can_data.return_fault_control.id = msg_data_a->return_fault_control.id; - return_fault_control(msg_data_a->return_fault_control.id); - break; default: __asm__("nop"); } @@ -105,72 +42,45 @@ void canRxUpdate() } /* BEGIN AUTO STALE CHECKS */ - CHECK_STALE(can_data.front_wheel_data.stale, - sched.os_ticks, can_data.front_wheel_data.last_rx, - UP_FRONT_WHEEL_DATA); - CHECK_STALE(can_data.rear_wheel_data.stale, - sched.os_ticks, can_data.rear_wheel_data.last_rx, - UP_REAR_WHEEL_DATA); /* END AUTO STALE CHECKS */ } bool initCANFilter() { - CAN1->MCR |= CAN_MCR_INRQ; // Enter back into INIT state (required for changing scale) + CAN1->MCR |= CAN_MCR_INRQ; // Enter back into INIT state (required for changing scale) uint32_t timeout = 0; - while(!(CAN1->MSR & CAN_MSR_INAK) && ++timeout < PHAL_CAN_INIT_TIMEOUT) - ; + while (!(CAN1->MSR & CAN_MSR_INAK) && ++timeout < PHAL_CAN_INIT_TIMEOUT) + ; if (timeout == PHAL_CAN_INIT_TIMEOUT) - return false; + return false; - CAN1->FMR |= CAN_FMR_FINIT; // Enter init mode for filter banks - CAN1->FM1R |= 0x07FFFFFF; // Set banks 0-27 to id mode - CAN1->FS1R |= 0x07FFFFFF; // Set banks 0-27 to 32-bit scale + CAN1->FMR |= CAN_FMR_FINIT; // Enter init mode for filter banks + CAN1->FM1R |= 0x07FFFFFF; // Set banks 0-27 to id mode + CAN1->FS1R |= 0x07FFFFFF; // Set banks 0-27 to 32-bit scale /* BEGIN AUTO FILTER */ - CAN1->FA1R |= (1 << 0); // configure bank 0 - CAN1->sFilterRegister[0].FR1 = (ID_FRONT_WHEEL_DATA << 3) | 4; - CAN1->sFilterRegister[0].FR2 = (ID_REAR_WHEEL_DATA << 3) | 4; - CAN1->FA1R |= (1 << 1); // configure bank 1 - CAN1->sFilterRegister[1].FR1 = (ID_BITSTREAM_DATA << 3) | 4; - CAN1->sFilterRegister[1].FR2 = (ID_BITSTREAM_REQUEST << 3) | 4; - CAN1->FA1R |= (1 << 2); // configure bank 2 - CAN1->sFilterRegister[2].FR1 = (ID_FAULT_SYNC_MAIN_MODULE << 3) | 4; - CAN1->sFilterRegister[2].FR2 = (ID_FAULT_SYNC_DRIVELINE << 3) | 4; - CAN1->FA1R |= (1 << 3); // configure bank 3 - CAN1->sFilterRegister[3].FR1 = (ID_FAULT_SYNC_DASHBOARD << 3) | 4; - CAN1->sFilterRegister[3].FR2 = (ID_FAULT_SYNC_PRECHARGE << 3) | 4; - CAN1->FA1R |= (1 << 4); // configure bank 4 - CAN1->sFilterRegister[4].FR1 = (ID_FAULT_SYNC_TEST_NODE << 3) | 4; - CAN1->sFilterRegister[4].FR2 = (ID_SET_FAULT << 3) | 4; - CAN1->FA1R |= (1 << 5); // configure bank 5 - CAN1->sFilterRegister[5].FR1 = (ID_RETURN_FAULT_CONTROL << 3) | 4; /* END AUTO FILTER */ - CAN1->FMR &= ~CAN_FMR_FINIT; // Enable Filters (exit filter init mode) + CAN1->FMR &= ~CAN_FMR_FINIT; // Enable Filters (exit filter init mode) // Enter back into NORMAL mode CAN1->MCR &= ~CAN_MCR_INRQ; - while((CAN1->MSR & CAN_MSR_INAK) && ++timeout < PHAL_CAN_INIT_TIMEOUT) - ; + while ((CAN1->MSR & CAN_MSR_INAK) && ++timeout < PHAL_CAN_INIT_TIMEOUT) + ; return timeout != PHAL_CAN_INIT_TIMEOUT; } - -void canProcessRxIRQs(CanMsgTypeDef_t* rx) +void canProcessRxIRQs(CanMsgTypeDef_t *rx) { - CanParsedData_t* msg_data_a; + CanParsedData_t *msg_data_a; - msg_data_a = (CanParsedData_t *) rx->Data; - switch(rx->ExtId) + msg_data_a = (CanParsedData_t *)rx->Data; + switch (rx->ExtId) { - /* BEGIN AUTO RX IRQ */ - case ID_BITSTREAM_DATA: - bitstream_data_IRQ(msg_data_a); - break; - /* END AUTO RX IRQ */ - default: - __asm__("nop"); + /* BEGIN AUTO RX IRQ */ + /* END AUTO RX IRQ */ + default: + __asm__("nop"); } -} +} \ No newline at end of file diff --git a/source/torque_vector/can/can_parse.h b/source/torque_vector/can/can_parse.h index 9f65283a..097f95fe 100644 --- a/source/torque_vector/can/can_parse.h +++ b/source/torque_vector/can/can_parse.h @@ -4,9 +4,9 @@ * @brief Parsing of CAN messages using auto-generated structures with bit-fields * @version 0.1 * @date 2021-09-15 - * + * * @copyright Copyright (c) 2021 - * + * */ #ifndef _CAN_PARSE_H_ #define _CAN_PARSE_H_ @@ -16,67 +16,137 @@ #include "common/phal_L4/can/can.h" // Make this match the node name within the can_config.json -#define NODE_NAME "Torque_Vector" +#define NODE_NAME "torque_vector" // Message ID definitions /* BEGIN AUTO ID DEFS */ -#define ID_TORQUE_REQUEST 0x4000042 -#define ID_BITSTREAM_FLASH_STATUS 0x1902 -#define ID_FAULT_SYNC_TORQUE_VECTOR 0x8ca42 -#define ID_FRONT_WHEEL_DATA 0x4000003 -#define ID_REAR_WHEEL_DATA 0x4000043 -#define ID_BITSTREAM_DATA 0x400193e -#define ID_BITSTREAM_REQUEST 0x1000197e -#define ID_FAULT_SYNC_MAIN_MODULE 0x8ca01 -#define ID_FAULT_SYNC_DRIVELINE 0x8ca83 -#define ID_FAULT_SYNC_DASHBOARD 0x8cb05 -#define ID_FAULT_SYNC_PRECHARGE 0x8cac4 -#define ID_FAULT_SYNC_TEST_NODE 0x8cb7f -#define ID_SET_FAULT 0x809c83e -#define ID_RETURN_FAULT_CONTROL 0x809c87e +#define ID_GPS_VELOCITY 0xc0002b7 +#define ID_GPS_POSITION 0xc002337 +#define ID_GPS_COORDINATES 0xc002377 +#define ID_IMU_GYRO 0xc0002f7 +#define ID_IMU_ACCEL 0xc0023b7 +#define ID_BMM_MAG 0xc0023f7 +#define ID_SFS_POS 0xc016937 +#define ID_SFS_VEL 0xc016977 +#define ID_SFS_ACC 0xc0169b7 +#define ID_SFS_ANG 0xc0169f7 +#define ID_SFS_ANG_VEL 0xc016a37 +#define ID_THROTTLE_REMAPPED 0xc0025b7 /* END AUTO ID DEFS */ // Message DLC definitions /* BEGIN AUTO DLC DEFS */ -#define DLC_TORQUE_REQUEST 6 -#define DLC_BITSTREAM_FLASH_STATUS 1 -#define DLC_FAULT_SYNC_TORQUE_VECTOR 3 -#define DLC_FRONT_WHEEL_DATA 8 -#define DLC_REAR_WHEEL_DATA 8 -#define DLC_BITSTREAM_DATA 8 -#define DLC_BITSTREAM_REQUEST 5 -#define DLC_FAULT_SYNC_MAIN_MODULE 3 -#define DLC_FAULT_SYNC_DRIVELINE 3 -#define DLC_FAULT_SYNC_DASHBOARD 3 -#define DLC_FAULT_SYNC_PRECHARGE 3 -#define DLC_FAULT_SYNC_TEST_NODE 3 -#define DLC_SET_FAULT 3 -#define DLC_RETURN_FAULT_CONTROL 2 +#define DLC_GPS_VELOCITY 8 +#define DLC_GPS_POSITION 8 +#define DLC_GPS_COORDINATES 8 +#define DLC_IMU_GYRO 6 +#define DLC_IMU_ACCEL 6 +#define DLC_BMM_MAG 6 +#define DLC_SFS_POS 6 +#define DLC_SFS_VEL 6 +#define DLC_SFS_ACC 6 +#define DLC_SFS_ANG 8 +#define DLC_SFS_ANG_VEL 6 +#define DLC_THROTTLE_REMAPPED 4 /* END AUTO DLC DEFS */ // Message sending macros /* BEGIN AUTO SEND MACROS */ -#define SEND_TORQUE_REQUEST(queue, front_left_, front_right_, rear_left_, rear_right_) do {\ - CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_TORQUE_REQUEST, .DLC=DLC_TORQUE_REQUEST, .IDE=1};\ +#define SEND_GPS_VELOCITY(queue, gps_vel_n_, gps_vel_e_, gps_vel_d_, gps_vel_total_) do {\ + CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_GPS_VELOCITY, .DLC=DLC_GPS_VELOCITY, .IDE=1};\ CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ - data_a->torque_request.front_left = front_left_;\ - data_a->torque_request.front_right = front_right_;\ - data_a->torque_request.rear_left = rear_left_;\ - data_a->torque_request.rear_right = rear_right_;\ + data_a->gps_velocity.gps_vel_n = gps_vel_n_;\ + data_a->gps_velocity.gps_vel_e = gps_vel_e_;\ + data_a->gps_velocity.gps_vel_d = gps_vel_d_;\ + data_a->gps_velocity.gps_vel_total = gps_vel_total_;\ qSendToBack(&queue, &msg);\ } while(0) -#define SEND_BITSTREAM_FLASH_STATUS(queue, flash_success_, flash_timeout_rx_) do {\ - CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_BITSTREAM_FLASH_STATUS, .DLC=DLC_BITSTREAM_FLASH_STATUS, .IDE=1};\ +#define SEND_GPS_POSITION(queue, gps_pos_x_, gps_pos_y_, gps_pos_z_, height_) do {\ + CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_GPS_POSITION, .DLC=DLC_GPS_POSITION, .IDE=1};\ CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ - data_a->bitstream_flash_status.flash_success = flash_success_;\ - data_a->bitstream_flash_status.flash_timeout_rx = flash_timeout_rx_;\ + data_a->gps_position.gps_pos_x = gps_pos_x_;\ + data_a->gps_position.gps_pos_y = gps_pos_y_;\ + data_a->gps_position.gps_pos_z = gps_pos_z_;\ + data_a->gps_position.height = height_;\ qSendToBack(&queue, &msg);\ } while(0) -#define SEND_FAULT_SYNC_TORQUE_VECTOR(queue, idx_, latched_) do {\ - CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_FAULT_SYNC_TORQUE_VECTOR, .DLC=DLC_FAULT_SYNC_TORQUE_VECTOR, .IDE=1};\ +#define SEND_GPS_COORDINATES(queue, latitude_, longitude_) do {\ + CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_GPS_COORDINATES, .DLC=DLC_GPS_COORDINATES, .IDE=1};\ CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ - data_a->fault_sync_torque_vector.idx = idx_;\ - data_a->fault_sync_torque_vector.latched = latched_;\ + data_a->gps_coordinates.latitude = latitude_;\ + data_a->gps_coordinates.longitude = longitude_;\ + qSendToBack(&queue, &msg);\ + } while(0) +#define SEND_IMU_GYRO(queue, imu_gyro_x_, imu_gyro_y_, imu_gyro_z_) do {\ + CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_IMU_GYRO, .DLC=DLC_IMU_GYRO, .IDE=1};\ + CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ + data_a->imu_gyro.imu_gyro_x = imu_gyro_x_;\ + data_a->imu_gyro.imu_gyro_y = imu_gyro_y_;\ + data_a->imu_gyro.imu_gyro_z = imu_gyro_z_;\ + qSendToBack(&queue, &msg);\ + } while(0) +#define SEND_IMU_ACCEL(queue, imu_accel_x_, imu_accel_y_, imu_accel_z_) do {\ + CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_IMU_ACCEL, .DLC=DLC_IMU_ACCEL, .IDE=1};\ + CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ + data_a->imu_accel.imu_accel_x = imu_accel_x_;\ + data_a->imu_accel.imu_accel_y = imu_accel_y_;\ + data_a->imu_accel.imu_accel_z = imu_accel_z_;\ + qSendToBack(&queue, &msg);\ + } while(0) +#define SEND_BMM_MAG(queue, bmm_mag_x_, bmm_mag_y_, bmm_mag_z_) do {\ + CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_BMM_MAG, .DLC=DLC_BMM_MAG, .IDE=1};\ + CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ + data_a->bmm_mag.bmm_mag_x = bmm_mag_x_;\ + data_a->bmm_mag.bmm_mag_y = bmm_mag_y_;\ + data_a->bmm_mag.bmm_mag_z = bmm_mag_z_;\ + qSendToBack(&queue, &msg);\ + } while(0) +#define SEND_SFS_POS(queue, sfs_pos_x_, sfs_pos_y_, sfs_pos_z_) do {\ + CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_SFS_POS, .DLC=DLC_SFS_POS, .IDE=1};\ + CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ + data_a->sfs_pos.sfs_pos_x = sfs_pos_x_;\ + data_a->sfs_pos.sfs_pos_y = sfs_pos_y_;\ + data_a->sfs_pos.sfs_pos_z = sfs_pos_z_;\ + qSendToBack(&queue, &msg);\ + } while(0) +#define SEND_SFS_VEL(queue, sfs_vel_x_, sfs_vel_y_, sfs_vel_z_) do {\ + CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_SFS_VEL, .DLC=DLC_SFS_VEL, .IDE=1};\ + CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ + data_a->sfs_vel.sfs_vel_x = sfs_vel_x_;\ + data_a->sfs_vel.sfs_vel_y = sfs_vel_y_;\ + data_a->sfs_vel.sfs_vel_z = sfs_vel_z_;\ + qSendToBack(&queue, &msg);\ + } while(0) +#define SEND_SFS_ACC(queue, sfs_acc_x_, sfs_acc_y_, sfs_acc_z_) do {\ + CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_SFS_ACC, .DLC=DLC_SFS_ACC, .IDE=1};\ + CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ + data_a->sfs_acc.sfs_acc_x = sfs_acc_x_;\ + data_a->sfs_acc.sfs_acc_y = sfs_acc_y_;\ + data_a->sfs_acc.sfs_acc_z = sfs_acc_z_;\ + qSendToBack(&queue, &msg);\ + } while(0) +#define SEND_SFS_ANG(queue, sfs_ang_a_, sfs_ang_b_, sfs_ang_c_, sfs_ang_d_) do {\ + CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_SFS_ANG, .DLC=DLC_SFS_ANG, .IDE=1};\ + CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ + data_a->sfs_ang.sfs_ang_a = sfs_ang_a_;\ + data_a->sfs_ang.sfs_ang_b = sfs_ang_b_;\ + data_a->sfs_ang.sfs_ang_c = sfs_ang_c_;\ + data_a->sfs_ang.sfs_ang_d = sfs_ang_d_;\ + qSendToBack(&queue, &msg);\ + } while(0) +#define SEND_SFS_ANG_VEL(queue, sfs_ang_vel_x_, sfs_ang_vel_y_, sfs_ang_vel_z_) do {\ + CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_SFS_ANG_VEL, .DLC=DLC_SFS_ANG_VEL, .IDE=1};\ + CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ + data_a->sfs_ang_vel.sfs_ang_vel_x = sfs_ang_vel_x_;\ + data_a->sfs_ang_vel.sfs_ang_vel_y = sfs_ang_vel_y_;\ + data_a->sfs_ang_vel.sfs_ang_vel_z = sfs_ang_vel_z_;\ + qSendToBack(&queue, &msg);\ + } while(0) +#define SEND_THROTTLE_REMAPPED(queue, remap_k_rl_, remap_k_rr_) do {\ + CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_THROTTLE_REMAPPED, .DLC=DLC_THROTTLE_REMAPPED, .IDE=1};\ + CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ + data_a->throttle_remapped.remap_k_rl = remap_k_rl_;\ + data_a->throttle_remapped.remap_k_rr = remap_k_rr_;\ qSendToBack(&queue, &msg);\ } while(0) /* END AUTO SEND MACROS */ @@ -84,12 +154,12 @@ // Stale Checking #define STALE_THRESH 3 / 2 // 3 / 2 would be 150% of period /* BEGIN AUTO UP DEFS (Update Period)*/ -#define UP_FRONT_WHEEL_DATA 10 -#define UP_REAR_WHEEL_DATA 10 /* END AUTO UP DEFS */ -#define CHECK_STALE(stale, curr, last, period) if(!stale && \ - (curr - last) > period * STALE_THRESH) stale = 1 +#define CHECK_STALE(stale, curr, last, period) \ + if (!stale && \ + (curr - last) > period * STALE_THRESH) \ + stale = 1 /* BEGIN AUTO CAN ENUMERATIONS */ /* END AUTO CAN ENUMERATIONS */ @@ -98,72 +168,66 @@ /* BEGIN AUTO MESSAGE STRUCTURE */ typedef union { struct { - uint64_t front_left: 12; - uint64_t front_right: 12; - uint64_t rear_left: 12; - uint64_t rear_right: 12; - } torque_request; - struct { - uint64_t flash_success: 1; - uint64_t flash_timeout_rx: 1; - } bitstream_flash_status; - struct { - uint64_t idx: 16; - uint64_t latched: 1; - } fault_sync_torque_vector; + uint64_t gps_vel_n: 16; + uint64_t gps_vel_e: 16; + uint64_t gps_vel_d: 16; + uint64_t gps_vel_total: 16; + } gps_velocity; struct { - uint64_t left_speed: 16; - uint64_t right_speed: 16; - uint64_t left_normal: 16; - uint64_t right_normal: 16; - } front_wheel_data; + uint64_t gps_pos_x: 16; + uint64_t gps_pos_y: 16; + uint64_t gps_pos_z: 16; + uint64_t height: 16; + } gps_position; struct { - uint64_t left_speed: 16; - uint64_t right_speed: 16; - uint64_t left_normal: 16; - uint64_t right_normal: 16; - } rear_wheel_data; + uint64_t latitude: 32; + uint64_t longitude: 32; + } gps_coordinates; struct { - uint64_t d0: 8; - uint64_t d1: 8; - uint64_t d2: 8; - uint64_t d3: 8; - uint64_t d4: 8; - uint64_t d5: 8; - uint64_t d6: 8; - uint64_t d7: 8; - } bitstream_data; + uint64_t imu_gyro_x: 16; + uint64_t imu_gyro_y: 16; + uint64_t imu_gyro_z: 16; + } imu_gyro; struct { - uint64_t download_request: 1; - uint64_t download_size: 32; - } bitstream_request; + uint64_t imu_accel_x: 16; + uint64_t imu_accel_y: 16; + uint64_t imu_accel_z: 16; + } imu_accel; struct { - uint64_t idx: 16; - uint64_t latched: 1; - } fault_sync_main_module; + uint64_t bmm_mag_x: 16; + uint64_t bmm_mag_y: 16; + uint64_t bmm_mag_z: 16; + } bmm_mag; struct { - uint64_t idx: 16; - uint64_t latched: 1; - } fault_sync_driveline; + uint64_t sfs_pos_x: 16; + uint64_t sfs_pos_y: 16; + uint64_t sfs_pos_z: 16; + } sfs_pos; struct { - uint64_t idx: 16; - uint64_t latched: 1; - } fault_sync_dashboard; + uint64_t sfs_vel_x: 16; + uint64_t sfs_vel_y: 16; + uint64_t sfs_vel_z: 16; + } sfs_vel; struct { - uint64_t idx: 16; - uint64_t latched: 1; - } fault_sync_precharge; + uint64_t sfs_acc_x: 16; + uint64_t sfs_acc_y: 16; + uint64_t sfs_acc_z: 16; + } sfs_acc; struct { - uint64_t idx: 16; - uint64_t latched: 1; - } fault_sync_test_node; + uint64_t sfs_ang_a: 16; + uint64_t sfs_ang_b: 16; + uint64_t sfs_ang_c: 16; + uint64_t sfs_ang_d: 16; + } sfs_ang; struct { - uint64_t id: 16; - uint64_t value: 1; - } set_fault; + uint64_t sfs_ang_vel_x: 16; + uint64_t sfs_ang_vel_y: 16; + uint64_t sfs_ang_vel_z: 16; + } sfs_ang_vel; struct { - uint64_t id: 16; - } return_fault_control; + uint64_t remap_k_rl: 16; + uint64_t remap_k_rr: 16; + } throttle_remapped; uint8_t raw_data[8]; } __attribute__((packed)) CanParsedData_t; /* END AUTO MESSAGE STRUCTURE */ @@ -172,70 +236,12 @@ typedef union { // type for each variable matches that defined in JSON /* BEGIN AUTO CAN DATA STRUCTURE */ typedef struct { - struct { - uint16_t left_speed; - uint16_t right_speed; - uint16_t left_normal; - uint16_t right_normal; - uint8_t stale; - uint32_t last_rx; - } front_wheel_data; - struct { - uint16_t left_speed; - uint16_t right_speed; - uint16_t left_normal; - uint16_t right_normal; - uint8_t stale; - uint32_t last_rx; - } rear_wheel_data; - struct { - uint8_t d0; - uint8_t d1; - uint8_t d2; - uint8_t d3; - uint8_t d4; - uint8_t d5; - uint8_t d6; - uint8_t d7; - } bitstream_data; - struct { - uint8_t download_request; - uint32_t download_size; - } bitstream_request; - struct { - uint16_t idx; - uint8_t latched; - } fault_sync_main_module; - struct { - uint16_t idx; - uint8_t latched; - } fault_sync_driveline; - struct { - uint16_t idx; - uint8_t latched; - } fault_sync_dashboard; - struct { - uint16_t idx; - uint8_t latched; - } fault_sync_precharge; - struct { - uint16_t idx; - uint8_t latched; - } fault_sync_test_node; - struct { - uint16_t id; - uint8_t value; - } set_fault; - struct { - uint16_t id; - } return_fault_control; } can_data_t; /* END AUTO CAN DATA STRUCTURE */ extern can_data_t can_data; /* BEGIN AUTO EXTERN CALLBACK */ -extern void bitstream_request_CALLBACK(CanParsedData_t* msg_data_a); extern void handleCallbacks(uint16_t id, bool latched); extern void set_fault_daq(uint16_t id, bool value); extern void return_fault_control(uint16_t id); @@ -243,15 +249,14 @@ extern void send_fault(uint16_t id, bool latched); /* END AUTO EXTERN CALLBACK */ /* BEGIN AUTO EXTERN RX IRQ */ -extern void bitstream_data_IRQ(CanParsedData_t* msg_data_a); /* END AUTO EXTERN RX IRQ */ /** * @brief Setup queue and message filtering - * + * * @param q_rx_can RX buffer of CAN messages */ -void initCANParse(q_handle_t* q_rx_can_a); +void initCANParse(q_handle_t *q_rx_can_a); /** * @brief Pull message off of rx buffer, @@ -262,9 +267,11 @@ void canRxUpdate(); /** * @brief Process any rx message callbacks from the CAN Rx IRQ - * + * * @param rx rx data from message just recieved */ -void canProcessRxIRQs(CanMsgTypeDef_t* rx); +void canProcessRxIRQs(CanMsgTypeDef_t *rx); + +extern volatile uint32_t last_can_rx_time_ms; #endif \ No newline at end of file diff --git a/source/navigation/gps/gps.c b/source/torque_vector/gps/gps.c similarity index 99% rename from source/navigation/gps/gps.c rename to source/torque_vector/gps/gps.c index ce6e8238..c9f462e2 100644 --- a/source/navigation/gps/gps.c +++ b/source/torque_vector/gps/gps.c @@ -1,7 +1,7 @@ #include #include #include "gps.h" -#include "SFS_pp.h" +#include "sfs_pp.h" #include "SFS.h" union i_Long iLong; diff --git a/source/navigation/gps/gps.h b/source/torque_vector/gps/gps.h similarity index 100% rename from source/navigation/gps/gps.h rename to source/torque_vector/gps/gps.h diff --git a/source/navigation/imu/imu.c b/source/torque_vector/imu/imu.c similarity index 99% rename from source/navigation/imu/imu.c rename to source/torque_vector/imu/imu.c index 9dde266f..9b2b0185 100644 --- a/source/navigation/imu/imu.c +++ b/source/torque_vector/imu/imu.c @@ -10,7 +10,7 @@ */ #include "imu.h" -#include "SFS_pp.h" +#include "sfs_pp.h" extern q_handle_t q_tx_can; /** diff --git a/source/navigation/imu/imu.h b/source/torque_vector/imu/imu.h similarity index 100% rename from source/navigation/imu/imu.h rename to source/torque_vector/imu/imu.h diff --git a/source/torque_vector/main.c b/source/torque_vector/main.c index 1e16e6d8..0ceda2c5 100644 --- a/source/torque_vector/main.c +++ b/source/torque_vector/main.c @@ -1,51 +1,82 @@ /* System Includes */ -#include "stm32l432xx.h" -#include "system_stm32l4xx.h" -#include "can_parse.h" -#include "common/psched/psched.h" -#include "common/phal_L4/can/can.h" -#include "common/phal_L4/quadspi/quadspi.h" +#include "stm32l471xx.h" #include "common/phal_L4/gpio/gpio.h" #include "common/phal_L4/rcc/rcc.h" -#include "common/bootloader/bootloader_common.h" - +#include "common/phal_L4/spi/spi.h" +#include "common/psched/psched.h" +#include "common/phal_L4/usart/usart.h" /* Module Includes */ +#include "bmi088.h" +#include "can_parse.h" +#include "bsxlite_interface.h" +#include "imu.h" #include "main.h" -#include "bitstream.h" +#include "gps.h" +#include "bmm150.h" +#include "SFS.h" +#include "sfs_pp.h" -#include "common/faults/faults.h" +extern q_handle_t q_tx_can; + +uint8_t collect_test[100] = {0}; -/* PER HAL Initilization Structures */ GPIOInitConfig_t gpio_config[] = { - GPIO_INIT_CANRX_PA11, - GPIO_INIT_CANTX_PA12, - // QuadSPI Chip Selects - GPIO_INIT_OUTPUT(QUADSPI_CS_FLASH_GPIO_Port, QUADSPI_CS_FLASH_Pin, GPIO_OUTPUT_LOW_SPEED), - GPIO_INIT_OUTPUT(QUADSPI_CS_FPGA_GPIO_Port, QUADSPI_CS_FPGA_Pin, GPIO_OUTPUT_LOW_SPEED), - // QuadSPI Data/CLK - GPIO_INIT_AF(QUADSPI_CLK_GPIO_Port, QUADSPI_CLK_Pin, 10, GPIO_OUTPUT_LOW_SPEED, GPIO_OUTPUT_OPEN_DRAIN, GPIO_INPUT_OPEN_DRAIN), - GPIO_INIT_AF(QUADSPI_IO0_GPIO_Port, QUADSPI_IO0_Pin, 10, GPIO_OUTPUT_LOW_SPEED, GPIO_OUTPUT_OPEN_DRAIN, GPIO_INPUT_OPEN_DRAIN), - GPIO_INIT_AF(QUADSPI_IO1_GPIO_Port, QUADSPI_IO1_Pin, 10, GPIO_OUTPUT_LOW_SPEED, GPIO_OUTPUT_OPEN_DRAIN, GPIO_INPUT_OPEN_DRAIN), - GPIO_INIT_AF(QUADSPI_IO2_GPIO_Port, QUADSPI_IO2_Pin, 10, GPIO_OUTPUT_LOW_SPEED, GPIO_OUTPUT_OPEN_DRAIN, GPIO_INPUT_OPEN_DRAIN), - GPIO_INIT_AF(QUADSPI_IO3_GPIO_Port, QUADSPI_IO3_Pin, 10, GPIO_OUTPUT_LOW_SPEED, GPIO_OUTPUT_OPEN_DRAIN, GPIO_INPUT_OPEN_DRAIN), - // I2C Bus - GPIO_INIT_AF(I2C_SCL_GPIO_Port, I2C_SCL_Pin, 4, GPIO_OUTPUT_LOW_SPEED, GPIO_OUTPUT_OPEN_DRAIN, GPIO_INPUT_OPEN_DRAIN), - GPIO_INIT_AF(I2C_SDA_GPIO_Port, I2C_SDA_Pin, 4, GPIO_OUTPUT_LOW_SPEED, GPIO_OUTPUT_OPEN_DRAIN, GPIO_INPUT_OPEN_DRAIN), - GPIO_INIT_OUTPUT(I2C_WRITE_CONTROL_GPIO_Port, I2C_WRITE_CONTROL_Pin, GPIO_OUTPUT_LOW_SPEED), - // Status LEDs - GPIO_INIT_OUTPUT(ERROR_LED_GPIO_Port, ERROR_LED_Pin, GPIO_OUTPUT_LOW_SPEED), + // Status Indicators + GPIO_INIT_OUTPUT(ERR_LED_GPIO_Port, ERR_LED_Pin, GPIO_OUTPUT_LOW_SPEED), GPIO_INIT_OUTPUT(CONN_LED_GPIO_Port, CONN_LED_Pin, GPIO_OUTPUT_LOW_SPEED), - GPIO_INIT_OUTPUT(HEARTBEAT_LED_GPIO_Port, HEARTBEAT_LED_Pin, GPIO_OUTPUT_LOW_SPEED) -}; + GPIO_INIT_OUTPUT(HEARTBEAT_GPIO_Port, HEARTBEAT_Pin, GPIO_OUTPUT_LOW_SPEED), + + // SPI + GPIO_INIT_AF(SPI_SCLK_GPIO_Port, SPI_SCLK_Pin, 5, GPIO_OUTPUT_HIGH_SPEED, GPIO_OUTPUT_PUSH_PULL, GPIO_INPUT_PULL_DOWN), + GPIO_INIT_AF(SPI_MOSI_GPIO_Port, SPI_MOSI_Pin, 5, GPIO_OUTPUT_HIGH_SPEED, GPIO_OUTPUT_PUSH_PULL, GPIO_INPUT_PULL_DOWN), + GPIO_INIT_AF(SPI_MISO_GPIO_Port, SPI_MISO_Pin, 5, GPIO_OUTPUT_HIGH_SPEED, GPIO_OUTPUT_OPEN_DRAIN, GPIO_INPUT_OPEN_DRAIN), + GPIO_INIT_OUTPUT(SPI_CS_ACEL_GPIO_Port, SPI_CS_ACEL_Pin, GPIO_OUTPUT_HIGH_SPEED), + GPIO_INIT_OUTPUT(SPI_CS_GYRO_GPIO_Port, SPI_CS_GYRO_Pin, GPIO_OUTPUT_HIGH_SPEED), + GPIO_INIT_OUTPUT(SPI_CS_MAG_GPIO_Port, SPI_CS_MAG_Pin, GPIO_OUTPUT_HIGH_SPEED), + + // GPS USART + GPIO_INIT_USART3RX_PC5, + GPIO_INIT_USART3TX_PC4, + + // EEPROM + GPIO_INIT_OUTPUT(NAV_EEPROM_CS_GPIO_PORT, NAV_EEPROM_CS_PIN, GPIO_OUTPUT_HIGH_SPEED), + GPIO_INIT_OUTPUT(NAV_WP_GPIO_PORT, NAV_WP_PIN, GPIO_OUTPUT_HIGH_SPEED), + + // CAN + GPIO_INIT_CANRX_PA11, + GPIO_INIT_CANTX_PA12}; + +/* USART Configuration */ +// M9N GPS +dma_init_t usart_gps_tx_dma_config = USART3_TXDMA_CONT_CONFIG(NULL, 1); +dma_init_t usart_gps_rx_dma_config = USART3_RXDMA_CONT_CONFIG(NULL, 2); +usart_init_t huart_gps = { + .baud_rate = 115200, + .word_length = WORD_8, + .hw_flow_ctl = HW_DISABLE, + .mode = MODE_TX_RX, + .stop_bits = SB_ONE, + .parity = PT_NONE, + .obsample = OB_DISABLE, + .ovsample = OV_16, + .adv_feature.rx_inv = false, + .adv_feature.tx_inv = false, + .adv_feature.auto_baud = false, + .adv_feature.data_inv = false, + .adv_feature.msb_first = false, + .adv_feature.overrun = false, + .adv_feature.dma_on_rx_err = false, + .tx_dma_cfg = &usart_gps_tx_dma_config, + .rx_dma_cfg = &usart_gps_rx_dma_config}; #define TargetCoreClockrateHz 16000000 ClockRateConfig_t clock_config = { - .system_source =SYSTEM_CLOCK_SRC_HSI, - .system_clock_target_hz =TargetCoreClockrateHz, - .ahb_clock_target_hz =(TargetCoreClockrateHz / 1), - .apb1_clock_target_hz =(TargetCoreClockrateHz / (1)), - .apb2_clock_target_hz =(TargetCoreClockrateHz / (1)), + .system_source = SYSTEM_CLOCK_SRC_HSI, + .system_clock_target_hz = TargetCoreClockrateHz, + .ahb_clock_target_hz = (TargetCoreClockrateHz / 1), + .apb1_clock_target_hz = (TargetCoreClockrateHz / (1)), + .apb2_clock_target_hz = (TargetCoreClockrateHz / (1)), }; /* Locals for Clock Rates */ @@ -54,82 +85,262 @@ extern uint32_t APB2ClockRateHz; extern uint32_t AHBClockRateHz; extern uint32_t PLLClockRateHz; +dma_init_t spi_rx_dma_config = SPI1_RXDMA_CONT_CONFIG(NULL, 2); +dma_init_t spi_tx_dma_config = SPI1_TXDMA_CONT_CONFIG(NULL, 1); + +SPI_InitConfig_t spi_config = { + .data_rate = TargetCoreClockrateHz / 64, + .data_len = 8, + .nss_sw = true, + .nss_gpio_port = SPI_CS_MAG_GPIO_Port, + .nss_gpio_pin = SPI_CS_MAG_Pin, + .rx_dma_cfg = NULL, + .tx_dma_cfg = NULL, + // .rx_dma_cfg = &spi_rx_dma_config, + // .tx_dma_cfg = &spi_tx_dma_config, + .periph = SPI1}; + +uint8_t num_iterations = 0; + +// Test Nav Message +GPS_Handle_t testGPSHandle = {}; +vector_3d_t accel_in, gyro_in, mag_in; + +BMI088_Handle_t bmi_config = { + .accel_csb_gpio_port = SPI_CS_ACEL_GPIO_Port, + .accel_csb_pin = SPI_CS_ACEL_Pin, + .accel_range = ACCEL_RANGE_3G, + .accel_odr = ACCEL_ODR_50Hz, + .accel_bwp = ACCEL_OS_NORMAL, + .gyro_csb_gpio_port = SPI_CS_GYRO_GPIO_Port, + .gyro_csb_pin = SPI_CS_GYRO_Pin, + .gyro_datarate = GYRO_DR_100Hz_32Hz, + .gyro_range = GYRO_RANGE_250, + .spi = &spi_config}; +BMM150_Handle_t bmm_config = { + .spi = &spi_config, + .mag_csb_gpio_port = SPI_CS_MAG_GPIO_Port, + .mag_csb_pin = SPI_CS_MAG_Pin}; +IMU_Handle_t imu_h = { + .bmi = &bmi_config, +}; + /* Function Prototypes */ -void canReceiveTest(); -void canSendTest(); -void Error_Handler(); -void SysTick_Handler(); -void canTxUpdate(); -void blinkTask(); -void PHAL_FaltHandler(); -extern void HardFault_Handler(); - -q_handle_t q_tx_can; -q_handle_t q_rx_can; - -int main (void) +void canTxUpdate(void); +void heartBeatLED(void); +void preflightAnimation(void); +void preflightChecks(void); +void sendIMUData(void); +void collectGPSData(void); +void collectMagData(void); +extern void HardFault_Handler(void); +q_handle_t q_tx_can, q_rx_can; + +/* SFS Definitions */ +static ExtU rtU; /* External inputs */ +static ExtY rtY; /* External outputs */ +static RT_MODEL rtM_; +static RT_MODEL *const rtMPtr = &rtM_; /* Real-time model */ +static DW rtDW; /* Observable states */ +static RT_MODEL *const rtM = rtMPtr; + +int main(void) { - // Main stack pointer is saved as the first entry in the .isr_entry - Bootloader_ConfirmApplicationLaunch(); + // memset(spi2_tx_buffer + 8, 255, 100 - 8); - /* Data Struct init */ + /* Data Struct Initialization */ qConstruct(&q_tx_can, sizeof(CanMsgTypeDef_t)); qConstruct(&q_rx_can, sizeof(CanMsgTypeDef_t)); - /* HAL Initilization */ + /* HAL Initialization */ if (0 != PHAL_configureClockRates(&clock_config)) - PHAL_FaltHandler(); - - if (1 != PHAL_initGPIO(gpio_config, sizeof(gpio_config)/sizeof(GPIOInitConfig_t))) - PHAL_FaltHandler(); + { + HardFault_Handler(); + } - if (1 != PHAL_initCAN(CAN1, false)) - PHAL_FaltHandler(); + /* GPIO initialization */ + if (!PHAL_initGPIO(gpio_config, sizeof(gpio_config) / sizeof(GPIOInitConfig_t))) + { + HardFault_Handler(); + } - if (1 != PHAL_qspiInit()) - PHAL_FaltHandler(); + /* USART initialization */ + huart_gps.rx_dma_cfg->circular = true; + if (!PHAL_initUSART(USART3, &huart_gps, APB1ClockRateHz)) + { + HardFault_Handler(); + } + PHAL_usartRxDma(USART3, &huart_gps, (uint16_t *)testGPSHandle.raw_message, 100); - NVIC_EnableIRQ(CAN1_RX0_IRQn); + /* SPI initialization */ + if (!PHAL_SPI_init(&spi_config)) + { + HardFault_Handler(); + } + spi_config.data_rate = APB2ClockRateHz / 16; - /* Module init */ - // bitstreamInit(); - schedInit(SystemCoreClock); // See Datasheet DS11451 Figure. 4 for clock tree - initCANParse(&q_rx_can); + PHAL_writeGPIO(SPI_CS_ACEL_GPIO_Port, SPI_CS_ACEL_Pin, 1); + PHAL_writeGPIO(SPI_CS_GYRO_GPIO_Port, SPI_CS_GYRO_Pin, 1); + PHAL_writeGPIO(SPI_CS_MAG_GPIO_Port, SPI_CS_MAG_Pin, 1); - PHAL_writeGPIO(HEARTBEAT_LED_GPIO_Port, HEARTBEAT_LED_Pin, 1); + // while (1) + // { + // PHAL_usartRxBl(USART3, (uint16_t *)collect_test, 100); + // // PHAL_toggleGPIO(ERR_LED_GPIO_Port, ERR_LED_Pin); + // } /* Task Creation */ - taskCreate(blinkTask, 500); - taskCreate(bitstream10Hz, 100); - taskCreate(bitstream100Hz, 10); + schedInit(APB1ClockRateHz); + configureAnim(preflightAnimation, preflightChecks, 74, 1000); + taskCreateBackground(canTxUpdate); taskCreateBackground(canRxUpdate); + + taskCreate(heartBeatLED, 500); + + taskCreate(sendIMUData, 10); + taskCreate(collectGPSData, 40); + taskCreate(collectMagData, 40); + taskCreate(SFS_MAIN, 10); + + /* No Way Home */ schedStart(); return 0; } -void blinkTask() +void preflightChecks(void) { - PHAL_toggleGPIO(HEARTBEAT_LED_GPIO_Port, HEARTBEAT_LED_Pin); + static uint16_t state; + + switch (state++) + { + case 0: + if (!PHAL_initCAN(CAN1, false)) + { + HardFault_Handler(); + } + NVIC_EnableIRQ(CAN1_RX0_IRQn); + break; + case 2: + if (!BMM150_readID(&bmm_config)) + { + asm("nop"); + } + break; + case 1: + if (!BMI088_init(&bmi_config)) + { + HardFault_Handler(); + } + break; + case 250: + BMI088_powerOnAccel(&bmi_config); + break; + case 500: + if (!BMI088_initAccel(&bmi_config)) + HardFault_Handler(); + break; + case 700: + /* Pack model data into RTM */ + rtM->dwork = &rtDW; + + /* Initialize model */ + SFS_initialize(rtM); + default: + if (state > 750) + { + if (!imu_init(&imu_h)) + HardFault_Handler(); + initCANParse(&q_rx_can); + registerPreflightComplete(1); + state = 750; // prevent wrap around + } + break; + } +} + +void preflightAnimation(void) +{ + static uint32_t time; + + PHAL_writeGPIO(HEARTBEAT_GPIO_Port, HEARTBEAT_Pin, 0); + PHAL_writeGPIO(ERR_LED_GPIO_Port, ERR_LED_Pin, 0); + PHAL_writeGPIO(CONN_LED_GPIO_Port, CONN_LED_Pin, 0); + + switch (time++ % 6) + { + case 0: + case 5: + PHAL_writeGPIO(HEARTBEAT_GPIO_Port, HEARTBEAT_Pin, 1); + break; + case 1: + case 2: + case 3: + PHAL_writeGPIO(ERR_LED_GPIO_Port, ERR_LED_Pin, 1); + break; + case 4: + PHAL_writeGPIO(CONN_LED_GPIO_Port, CONN_LED_Pin, 1); + break; + } +} +void heartBeatLED(void) +{ + PHAL_toggleGPIO(HEARTBEAT_GPIO_Port, HEARTBEAT_Pin); + + // if ((sched.os_ticks - last_can_rx_time_ms) >= CONN_LED_MS_THRESH) + // PHAL_writeGPIO(CONN_LED_GPIO_Port, CONN_LED_Pin, 0); + // else PHAL_writeGPIO(CONN_LED_GPIO_Port, CONN_LED_Pin, 1); +} + +void sendIMUData(void) +{ + imu_periodic(&imu_h, &rtU); } -void PHAL_FaltHandler() +uint8_t poll_pvt[] = {"0xB5, 0x62, 0x01, 0x07, 0x00, 0x00, 0x08, 0x19"}; + +// Test function for usartRxDma +void collectGPSData(void) { - asm("bkpt"); - HardFault_Handler(); + testGPSHandle.messages_received++; + BMI088_readGyro(&bmi_config, &gyro_in); + BMI088_readAccel(&bmi_config, &accel_in); + testGPSHandle.acceleration = accel_in; + testGPSHandle.gyroscope = gyro_in; + parseVelocity(&testGPSHandle, &rtU); + // PHAL_usartRxBl(USART3, (uint16_t *)collect_test, 100); + // PHAL_usartRxDma(USART3, &huart_gps, (uint16_t *)collect_test, 100); + // PHAL_usartTxDma(USART3, &huart_gps, (uint16_t *)poll_pvt, strlen(poll_pvt)); + // while (PHAL_SPI_busy(&spi2_config)) + // ; + // if (num_iterations == 1) + // { + // memset(spi2_tx_buffer, 255, sizeof(spi2_tx_buffer)); + // } + // else + // { + // PHAL_SPI_transfer(&spi2_config, spi2_tx_buffer, 100, spi2_rx_buffer); + // } + + // if (spi2_rx_buffer[0] != 255) + // { + // asm("nop"); + // } + // while (PHAL_SPI_busy(&spi2_config)) + // ; + // num_iterations++; } -// *** Compulsory CAN Tx/Rx callbacks *** -void bootloader_request_reset_CALLBACK(CanParsedData_t* data) +void collectMagData(void) { - Bootloader_ResetForFirmwareDownload(); + BMM150_readMag(&bmm_config, &rtU); } -void canTxUpdate() +void canTxUpdate(void) { CanMsgTypeDef_t tx_msg; - if (qReceive(&q_tx_can, &tx_msg) == SUCCESS_G) // Check queue for items and take if there is one + if (qReceive(&q_tx_can, &tx_msg) == SUCCESS_G) // Check queue for items and take if there is one { PHAL_txCANMessage(&tx_msg); } @@ -146,32 +357,90 @@ void CAN1_RX0_IRQHandler() if (CAN1->RF0R & CAN_RF0R_FMP0_Msk) // Release message pending { CanMsgTypeDef_t rx; + rx.Bus = CAN1; // Get either StdId or ExtId - if (CAN_RI0R_IDE & CAN1->sFIFOMailBox[0].RIR) + rx.IDE = CAN_RI0R_IDE & CAN1->sFIFOMailBox[0].RIR; + if (rx.IDE) { - rx.ExtId = ((CAN_RI0R_EXID | CAN_RI0R_STID) & CAN1->sFIFOMailBox[0].RIR) >> CAN_RI0R_EXID_Pos; + rx.ExtId = ((CAN_RI0R_EXID | CAN_RI0R_STID) & CAN1->sFIFOMailBox[0].RIR) >> CAN_RI0R_EXID_Pos; } else { - rx.StdId = (CAN_RI0R_STID & CAN1->sFIFOMailBox[0].RIR) >> CAN_TI0R_STID_Pos; + rx.StdId = (CAN_RI0R_STID & CAN1->sFIFOMailBox[0].RIR) >> CAN_RI0R_STID_Pos; } rx.DLC = (CAN_RDT0R_DLC & CAN1->sFIFOMailBox[0].RDTR) >> CAN_RDT0R_DLC_Pos; - rx.Data[0] = (uint8_t) (CAN1->sFIFOMailBox[0].RDLR >> 0) & 0xFF; - rx.Data[1] = (uint8_t) (CAN1->sFIFOMailBox[0].RDLR >> 8) & 0xFF; - rx.Data[2] = (uint8_t) (CAN1->sFIFOMailBox[0].RDLR >> 16) & 0xFF; - rx.Data[3] = (uint8_t) (CAN1->sFIFOMailBox[0].RDLR >> 24) & 0xFF; - rx.Data[4] = (uint8_t) (CAN1->sFIFOMailBox[0].RDHR >> 0) & 0xFF; - rx.Data[5] = (uint8_t) (CAN1->sFIFOMailBox[0].RDHR >> 8) & 0xFF; - rx.Data[6] = (uint8_t) (CAN1->sFIFOMailBox[0].RDHR >> 16) & 0xFF; - rx.Data[7] = (uint8_t) (CAN1->sFIFOMailBox[0].RDHR >> 24) & 0xFF; - - canProcessRxIRQs(&rx); + rx.Data[0] = (uint8_t)(CAN1->sFIFOMailBox[0].RDLR >> 0) & 0xFF; + rx.Data[1] = (uint8_t)(CAN1->sFIFOMailBox[0].RDLR >> 8) & 0xFF; + rx.Data[2] = (uint8_t)(CAN1->sFIFOMailBox[0].RDLR >> 16) & 0xFF; + rx.Data[3] = (uint8_t)(CAN1->sFIFOMailBox[0].RDLR >> 24) & 0xFF; + rx.Data[4] = (uint8_t)(CAN1->sFIFOMailBox[0].RDHR >> 0) & 0xFF; + rx.Data[5] = (uint8_t)(CAN1->sFIFOMailBox[0].RDHR >> 8) & 0xFF; + rx.Data[6] = (uint8_t)(CAN1->sFIFOMailBox[0].RDHR >> 16) & 0xFF; + rx.Data[7] = (uint8_t)(CAN1->sFIFOMailBox[0].RDHR >> 24) & 0xFF; - CAN1->RF0R |= (CAN_RF0R_RFOM0); + CAN1->RF0R |= (CAN_RF0R_RFOM0); qSendToBack(&q_rx_can, &rx); // Add to queue (qSendToBack is interrupt safe) } +} + +// void main_module_bl_cmd_CALLBACK(CanParsedData_t *msg_data_a) +// { +// if (can_data.main_module_bl_cmd.cmd == BLCMD_RST) +// Bootloader_ResetForFirmwareDownload(); +// } + +void SFS_MAIN(void) +{ + SFS_pp(&rtU); + + static boolean_T OverrunFlag = false; + + /* Disable interrupts here */ + + /* Check for overrun */ + if (OverrunFlag) + { + rtmSetErrorStatus(rtM, "Overrun"); + return; + } + + OverrunFlag = true; + + /* Save FPU context here (if necessary) */ + /* Re-enable timer or interrupt here */ + /* Set model inputs here */ + + /* Step the model */ + SFS_step(rtM, &rtU, &rtY); + SEND_SFS_POS(q_tx_can, (int16_t)(rtY.pos_VNED[0] * 100), + (int16_t)(rtY.pos_VNED[1] * 100), (int16_t)(rtY.pos_VNED[2] * 100)); + SEND_SFS_VEL(q_tx_can, (int16_t)(rtY.vel_VNED[0] * 100), + (int16_t)(rtY.vel_VNED[1] * 100), (int16_t)(rtY.vel_VNED[2] * 100)); + SEND_SFS_ACC(q_tx_can, (int16_t)(rtY.acc_VNED[0] * 100), + (int16_t)(rtY.acc_VNED[1] * 100), (int16_t)(rtY.acc_VNED[2] * 100)); + SEND_SFS_ANG(q_tx_can, (int16_t)(rtY.ang_NED[0] * 10000), + (int16_t)(rtY.ang_NED[1] * 10000), (int16_t)(rtY.ang_NED[2] * 10000), (int16_t)(rtY.ang_NED[3] * 10000)); + SEND_SFS_ANG_VEL(q_tx_can, (int16_t)(rtY.angvel_VNED[0] * 10000), + (int16_t)(rtY.angvel_VNED[1] * 10000), (int16_t)(rtY.angvel_VNED[2] * 10000)); + /* Get model outputs here */ + + /* Indicate task complete */ + OverrunFlag = false; + + /* Disable interrupts here */ + /* Restore FPU context here (if necessary) */ + /* Enable interrupts here */ +} + +void HardFault_Handler() +{ + PHAL_writeGPIO(ERR_LED_GPIO_Port, ERR_LED_Pin, 1); + while (1) + { + __asm__("nop"); + } } \ No newline at end of file diff --git a/source/torque_vector/main.h b/source/torque_vector/main.h index f499aca7..334cdb11 100644 --- a/source/torque_vector/main.h +++ b/source/torque_vector/main.h @@ -1,46 +1,53 @@ -#ifndef MAIN_H_ -#define MAIN_H_ +/** + * @file main.h + * @author Luke Oxley (lcoxley@purdue.edu) + * @brief Software for controlling Torque Vectoring PCB + sensor acquisition + * @version 0.1 + * @date 2022-12-08 + * + * @copyright Copyright (c) 2022 + * + */ +#ifndef _MAIN_H_ +#define _MAIN_H_ -#include "common/faults/fault_nodes.h" +// STM32L471RET -#define FAULT_NODE_NAME NODE_TV +// Status Indicators +#define ERR_LED_GPIO_Port (GPIOB) +#define ERR_LED_Pin (5) +#define CONN_LED_GPIO_Port (GPIOB) +#define CONN_LED_Pin (7) +#define CONN_LED_MS_THRESH (500) +#define HEARTBEAT_GPIO_Port (GPIOB) +#define HEARTBEAT_Pin (6) -// QuadSPI CS -#define QUADSPI_CS_FLASH_GPIO_Port (GPIOA) -#define QUADSPI_CS_FLASH_Pin (2) -#define QUADSPI_CS_FPGA_GPIO_Port (GPIOA) -#define QUADSPI_CS_FPGA_Pin (1) -// QuadSPI CLK -#define QUADSPI_CLK_GPIO_Port (GPIOA) -#define QUADSPI_CLK_Pin (3) -// QuadSPI I/O Ports -#define QUADSPI_IO0_GPIO_Port (GPIOB) -#define QUADSPI_IO0_Pin (2) -#define QUADSPI_IO1_GPIO_Port (GPIOB) -#define QUADSPI_IO1_Pin (1) -#define QUADSPI_IO2_GPIO_Port (GPIOA) -#define QUADSPI_IO2_Pin (7) -#define QUADSPI_IO3_GPIO_Port (GPIOA) -#define QUADSPI_IO3_Pin (6) +// SPI IMU +#define SPI_SCLK_GPIO_Port (GPIOA) +#define SPI_SCLK_Pin (5) +#define SPI_MISO_GPIO_Port (GPIOA) +#define SPI_MISO_Pin (6) +#define SPI_MOSI_GPIO_Port (GPIOA) +#define SPI_MOSI_Pin (7) -// FPGA Configuration Reset -#define FPGA_CFG_RST_GPIO_Port (GPIOA) -#define FPGA_CFG_RST_Pin (4) +#define SPI_CS_ACEL_GPIO_Port (GPIOA) +#define SPI_CS_ACEL_Pin (3) +#define SPI_CS_GYRO_GPIO_Port (GPIOA) +#define SPI_CS_GYRO_Pin (2) +#define SPI_CS_MAG_GPIO_Port (GPIOB) +#define SPI_CS_MAG_Pin (0) -// I2C Bus -#define I2C_SCL_GPIO_Port (GPIOB) -#define I2C_SCL_Pin (6) -#define I2C_SDA_GPIO_Port (GPIOB) -#define I2C_SDA_Pin (7) -#define I2C_WRITE_CONTROL_GPIO_Port (GPIOA) -#define I2C_WRITE_CONTROL_Pin (6) +// USART GPS +#define GPS_RX_GPIO_Port (GPIOC) +#define GPS_RX_Pin (5) +#define GPS_TX_GPIO_Port (GPIOC) +#define GPS_TX_Pin (4) -// Status LEDs -#define ERROR_LED_GPIO_Port (GPIOA) -#define ERROR_LED_Pin (15) -#define CONN_LED_GPIO_Port (GPIOB) -#define CONN_LED_Pin (4) -#define HEARTBEAT_LED_GPIO_Port (GPIOB) -#define HEARTBEAT_LED_Pin (5) +// EEPROM +#define NAV_EEPROM_CS_GPIO_PORT (GPIOB) +#define NAV_EEPROM_CS_PIN (12) +#define NAV_WP_GPIO_PORT (GPIOB) +#define NAV_WP_PIN (13) #endif \ No newline at end of file diff --git a/source/navigation/sfs/SFS.c b/source/torque_vector/sfs/SFS.c similarity index 100% rename from source/navigation/sfs/SFS.c rename to source/torque_vector/sfs/SFS.c diff --git a/source/navigation/sfs/SFS.h b/source/torque_vector/sfs/SFS.h similarity index 100% rename from source/navigation/sfs/SFS.h rename to source/torque_vector/sfs/SFS.h diff --git a/source/navigation/sfs/SFS_data.c b/source/torque_vector/sfs/SFS_data.c similarity index 100% rename from source/navigation/sfs/SFS_data.c rename to source/torque_vector/sfs/SFS_data.c diff --git a/source/navigation/sfs/rtwtypes.h b/source/torque_vector/sfs/rtwtypes.h similarity index 100% rename from source/navigation/sfs/rtwtypes.h rename to source/torque_vector/sfs/rtwtypes.h diff --git a/source/navigation/sfs_pp/sfs_pp.c b/source/torque_vector/sfs_pp/sfs_pp.c similarity index 95% rename from source/navigation/sfs_pp/sfs_pp.c rename to source/torque_vector/sfs_pp/sfs_pp.c index 679cb303..b981722f 100644 --- a/source/navigation/sfs_pp/sfs_pp.c +++ b/source/torque_vector/sfs_pp/sfs_pp.c @@ -1,4 +1,4 @@ -#include "SFS_pp.h" +#include "sfs_pp.h" #include "SFS.h" #include "can_parse.h" #include "common_defs.h" diff --git a/source/navigation/sfs_pp/sfs_pp.h b/source/torque_vector/sfs_pp/sfs_pp.h similarity index 100% rename from source/navigation/sfs_pp/sfs_pp.h rename to source/torque_vector/sfs_pp/sfs_pp.h diff --git a/source/torque_vector_fpga/CMakeLists.txt b/source/torque_vector_fpga/CMakeLists.txt new file mode 100644 index 00000000..2292cfaf --- /dev/null +++ b/source/torque_vector_fpga/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 3.13) + +# Setup Component name based on directory +get_filename_component(COMPONENT_NAME ${CMAKE_CURRENT_LIST_DIR} NAME) +STRING(TOLOWER ${COMPONENT_NAME} COMPONENT_NAME) + +set(TARGET_NAME ${COMPONENT_NAME}.elf) +add_executable(${TARGET_NAME}) + +# Propreties are set in order to make the common component +set_target_properties(${TARGET_NAME} PROPERTIES + COMPONENT_NAME ${COMPONENT_NAME} + COMPONENT_DIR ${CMAKE_CURRENT_LIST_DIR} + LINKER_SCRIPT "STM32L432KCUx_FLASH" + COMMON_LIBS "CMSIS_L432;PSCHED;QUEUE;PHAL_L432;BOOTLOADER_COMMON_L432;FAULTS" +) +COMMON_FIRMWARE_COMPONENT(${TARGET_NAME}) diff --git a/source/torque_vector/bitstream/bitstream.c b/source/torque_vector_fpga/bitstream/bitstream.c similarity index 100% rename from source/torque_vector/bitstream/bitstream.c rename to source/torque_vector_fpga/bitstream/bitstream.c diff --git a/source/torque_vector/bitstream/bitstream.h b/source/torque_vector_fpga/bitstream/bitstream.h similarity index 100% rename from source/torque_vector/bitstream/bitstream.h rename to source/torque_vector_fpga/bitstream/bitstream.h diff --git a/source/torque_vector_fpga/can/can_parse.c b/source/torque_vector_fpga/can/can_parse.c new file mode 100644 index 00000000..c481ee22 --- /dev/null +++ b/source/torque_vector_fpga/can/can_parse.c @@ -0,0 +1,176 @@ +/** + * @file can_parse.c + * @author Luke Oxley (lcoxley@purdue.edu) + * @brief Parsing of CAN messages using auto-generated structures with bit-fields + * @version 0.1 + * @date 2021-09-15 + * + * @copyright Copyright (c) 2021 + * + */ +#include "can_parse.h" + +// prototypes +bool initCANFilter(); + +can_data_t can_data; +q_handle_t* q_rx_can_a; + +void initCANParse(q_handle_t* rx_a) +{ + q_rx_can_a = rx_a; + initCANFilter(); +} + +void canRxUpdate() +{ + CanMsgTypeDef_t msg_header; + CanParsedData_t* msg_data_a; + + if(qReceive(q_rx_can_a, &msg_header) == SUCCESS_G) + { + msg_data_a = (CanParsedData_t *) &msg_header.Data; + /* BEGIN AUTO CASES */ + switch(msg_header.ExtId) + { + case ID_FRONT_WHEEL_DATA: + can_data.front_wheel_data.left_speed = msg_data_a->front_wheel_data.left_speed; + can_data.front_wheel_data.right_speed = msg_data_a->front_wheel_data.right_speed; + can_data.front_wheel_data.left_normal = msg_data_a->front_wheel_data.left_normal; + can_data.front_wheel_data.right_normal = msg_data_a->front_wheel_data.right_normal; + can_data.front_wheel_data.stale = 0; + can_data.front_wheel_data.last_rx = sched.os_ticks; + break; + case ID_REAR_WHEEL_DATA: + can_data.rear_wheel_data.left_speed = msg_data_a->rear_wheel_data.left_speed; + can_data.rear_wheel_data.right_speed = msg_data_a->rear_wheel_data.right_speed; + can_data.rear_wheel_data.left_normal = msg_data_a->rear_wheel_data.left_normal; + can_data.rear_wheel_data.right_normal = msg_data_a->rear_wheel_data.right_normal; + can_data.rear_wheel_data.stale = 0; + can_data.rear_wheel_data.last_rx = sched.os_ticks; + break; + case ID_BITSTREAM_DATA: + can_data.bitstream_data.d0 = msg_data_a->bitstream_data.d0; + can_data.bitstream_data.d1 = msg_data_a->bitstream_data.d1; + can_data.bitstream_data.d2 = msg_data_a->bitstream_data.d2; + can_data.bitstream_data.d3 = msg_data_a->bitstream_data.d3; + can_data.bitstream_data.d4 = msg_data_a->bitstream_data.d4; + can_data.bitstream_data.d5 = msg_data_a->bitstream_data.d5; + can_data.bitstream_data.d6 = msg_data_a->bitstream_data.d6; + can_data.bitstream_data.d7 = msg_data_a->bitstream_data.d7; + break; + case ID_BITSTREAM_REQUEST: + can_data.bitstream_request.download_request = msg_data_a->bitstream_request.download_request; + can_data.bitstream_request.download_size = msg_data_a->bitstream_request.download_size; + bitstream_request_CALLBACK(msg_data_a); + break; + case ID_FAULT_SYNC_MAIN_MODULE: + can_data.fault_sync_main_module.idx = msg_data_a->fault_sync_main_module.idx; + can_data.fault_sync_main_module.latched = msg_data_a->fault_sync_main_module.latched; + handleCallbacks(msg_data_a->fault_sync_main_module.idx, msg_data_a->fault_sync_main_module.latched); + break; + case ID_FAULT_SYNC_DRIVELINE: + can_data.fault_sync_driveline.idx = msg_data_a->fault_sync_driveline.idx; + can_data.fault_sync_driveline.latched = msg_data_a->fault_sync_driveline.latched; + handleCallbacks(msg_data_a->fault_sync_main_module.idx, msg_data_a->fault_sync_main_module.latched); + break; + case ID_FAULT_SYNC_DASHBOARD: + can_data.fault_sync_dashboard.idx = msg_data_a->fault_sync_dashboard.idx; + can_data.fault_sync_dashboard.latched = msg_data_a->fault_sync_dashboard.latched; + handleCallbacks(msg_data_a->fault_sync_main_module.idx, msg_data_a->fault_sync_main_module.latched); + break; + case ID_FAULT_SYNC_PRECHARGE: + can_data.fault_sync_precharge.idx = msg_data_a->fault_sync_precharge.idx; + can_data.fault_sync_precharge.latched = msg_data_a->fault_sync_precharge.latched; + handleCallbacks(msg_data_a->fault_sync_main_module.idx, msg_data_a->fault_sync_main_module.latched); + break; + case ID_FAULT_SYNC_TEST_NODE: + can_data.fault_sync_test_node.idx = msg_data_a->fault_sync_test_node.idx; + can_data.fault_sync_test_node.latched = msg_data_a->fault_sync_test_node.latched; + handleCallbacks(msg_data_a->fault_sync_main_module.idx, msg_data_a->fault_sync_main_module.latched); + break; + case ID_SET_FAULT: + can_data.set_fault.id = msg_data_a->set_fault.id; + can_data.set_fault.value = msg_data_a->set_fault.value; + set_fault_daq(msg_data_a->set_fault.id, msg_data_a->set_fault.value); + break; + case ID_RETURN_FAULT_CONTROL: + can_data.return_fault_control.id = msg_data_a->return_fault_control.id; + return_fault_control(msg_data_a->return_fault_control.id); + break; + default: + __asm__("nop"); + } + /* END AUTO CASES */ + } + + /* BEGIN AUTO STALE CHECKS */ + CHECK_STALE(can_data.front_wheel_data.stale, + sched.os_ticks, can_data.front_wheel_data.last_rx, + UP_FRONT_WHEEL_DATA); + CHECK_STALE(can_data.rear_wheel_data.stale, + sched.os_ticks, can_data.rear_wheel_data.last_rx, + UP_REAR_WHEEL_DATA); + /* END AUTO STALE CHECKS */ +} + +bool initCANFilter() +{ + CAN1->MCR |= CAN_MCR_INRQ; // Enter back into INIT state (required for changing scale) + uint32_t timeout = 0; + while(!(CAN1->MSR & CAN_MSR_INAK) && ++timeout < PHAL_CAN_INIT_TIMEOUT) + ; + if (timeout == PHAL_CAN_INIT_TIMEOUT) + return false; + + CAN1->FMR |= CAN_FMR_FINIT; // Enter init mode for filter banks + CAN1->FM1R |= 0x07FFFFFF; // Set banks 0-27 to id mode + CAN1->FS1R |= 0x07FFFFFF; // Set banks 0-27 to 32-bit scale + + /* BEGIN AUTO FILTER */ + CAN1->FA1R |= (1 << 0); // configure bank 0 + CAN1->sFilterRegister[0].FR1 = (ID_FRONT_WHEEL_DATA << 3) | 4; + CAN1->sFilterRegister[0].FR2 = (ID_REAR_WHEEL_DATA << 3) | 4; + CAN1->FA1R |= (1 << 1); // configure bank 1 + CAN1->sFilterRegister[1].FR1 = (ID_BITSTREAM_DATA << 3) | 4; + CAN1->sFilterRegister[1].FR2 = (ID_BITSTREAM_REQUEST << 3) | 4; + CAN1->FA1R |= (1 << 2); // configure bank 2 + CAN1->sFilterRegister[2].FR1 = (ID_FAULT_SYNC_MAIN_MODULE << 3) | 4; + CAN1->sFilterRegister[2].FR2 = (ID_FAULT_SYNC_DRIVELINE << 3) | 4; + CAN1->FA1R |= (1 << 3); // configure bank 3 + CAN1->sFilterRegister[3].FR1 = (ID_FAULT_SYNC_DASHBOARD << 3) | 4; + CAN1->sFilterRegister[3].FR2 = (ID_FAULT_SYNC_PRECHARGE << 3) | 4; + CAN1->FA1R |= (1 << 4); // configure bank 4 + CAN1->sFilterRegister[4].FR1 = (ID_FAULT_SYNC_TEST_NODE << 3) | 4; + CAN1->sFilterRegister[4].FR2 = (ID_SET_FAULT << 3) | 4; + CAN1->FA1R |= (1 << 5); // configure bank 5 + CAN1->sFilterRegister[5].FR1 = (ID_RETURN_FAULT_CONTROL << 3) | 4; + /* END AUTO FILTER */ + + CAN1->FMR &= ~CAN_FMR_FINIT; // Enable Filters (exit filter init mode) + + // Enter back into NORMAL mode + CAN1->MCR &= ~CAN_MCR_INRQ; + while((CAN1->MSR & CAN_MSR_INAK) && ++timeout < PHAL_CAN_INIT_TIMEOUT) + ; + + return timeout != PHAL_CAN_INIT_TIMEOUT; +} + + +void canProcessRxIRQs(CanMsgTypeDef_t* rx) +{ + CanParsedData_t* msg_data_a; + + msg_data_a = (CanParsedData_t *) rx->Data; + switch(rx->ExtId) + { + /* BEGIN AUTO RX IRQ */ + case ID_BITSTREAM_DATA: + bitstream_data_IRQ(msg_data_a); + break; + /* END AUTO RX IRQ */ + default: + __asm__("nop"); + } +} diff --git a/source/torque_vector_fpga/can/can_parse.h b/source/torque_vector_fpga/can/can_parse.h new file mode 100644 index 00000000..9518cc01 --- /dev/null +++ b/source/torque_vector_fpga/can/can_parse.h @@ -0,0 +1,270 @@ +/** + * @file can_parse.h + * @author Luke Oxley (lcoxley@purdue.edu) + * @brief Parsing of CAN messages using auto-generated structures with bit-fields + * @version 0.1 + * @date 2021-09-15 + * + * @copyright Copyright (c) 2021 + * + */ +#ifndef _CAN_PARSE_H_ +#define _CAN_PARSE_H_ + +#include "common/queue/queue.h" +#include "common/psched/psched.h" +#include "common/phal_L4/can/can.h" + +// Make this match the node name within the can_config.json +#define NODE_NAME "Torque_Vector_fpga" + +// Message ID definitions +/* BEGIN AUTO ID DEFS */ +#define ID_TORQUE_REQUEST 0x4000042 +#define ID_BITSTREAM_FLASH_STATUS 0x1902 +#define ID_FAULT_SYNC_TORQUE_VECTOR_FPGA 0x8ca42 +#define ID_FRONT_WHEEL_DATA 0x4000003 +#define ID_REAR_WHEEL_DATA 0x4000043 +#define ID_BITSTREAM_DATA 0x400193e +#define ID_BITSTREAM_REQUEST 0x1000197e +#define ID_FAULT_SYNC_MAIN_MODULE 0x8ca01 +#define ID_FAULT_SYNC_DRIVELINE 0x8ca83 +#define ID_FAULT_SYNC_DASHBOARD 0x8cb05 +#define ID_FAULT_SYNC_PRECHARGE 0x8cac4 +#define ID_FAULT_SYNC_TEST_NODE 0x8cb7f +#define ID_SET_FAULT 0x809c83e +#define ID_RETURN_FAULT_CONTROL 0x809c87e +/* END AUTO ID DEFS */ + +// Message DLC definitions +/* BEGIN AUTO DLC DEFS */ +#define DLC_TORQUE_REQUEST 6 +#define DLC_BITSTREAM_FLASH_STATUS 1 +#define DLC_FAULT_SYNC_TORQUE_VECTOR_FPGA 3 +#define DLC_FRONT_WHEEL_DATA 8 +#define DLC_REAR_WHEEL_DATA 8 +#define DLC_BITSTREAM_DATA 8 +#define DLC_BITSTREAM_REQUEST 5 +#define DLC_FAULT_SYNC_MAIN_MODULE 3 +#define DLC_FAULT_SYNC_DRIVELINE 3 +#define DLC_FAULT_SYNC_DASHBOARD 3 +#define DLC_FAULT_SYNC_PRECHARGE 3 +#define DLC_FAULT_SYNC_TEST_NODE 3 +#define DLC_SET_FAULT 3 +#define DLC_RETURN_FAULT_CONTROL 2 +/* END AUTO DLC DEFS */ + +// Message sending macros +/* BEGIN AUTO SEND MACROS */ +#define SEND_TORQUE_REQUEST(queue, front_left_, front_right_, rear_left_, rear_right_) do {\ + CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_TORQUE_REQUEST, .DLC=DLC_TORQUE_REQUEST, .IDE=1};\ + CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ + data_a->torque_request.front_left = front_left_;\ + data_a->torque_request.front_right = front_right_;\ + data_a->torque_request.rear_left = rear_left_;\ + data_a->torque_request.rear_right = rear_right_;\ + qSendToBack(&queue, &msg);\ + } while(0) +#define SEND_BITSTREAM_FLASH_STATUS(queue, flash_success_, flash_timeout_rx_) do {\ + CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_BITSTREAM_FLASH_STATUS, .DLC=DLC_BITSTREAM_FLASH_STATUS, .IDE=1};\ + CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ + data_a->bitstream_flash_status.flash_success = flash_success_;\ + data_a->bitstream_flash_status.flash_timeout_rx = flash_timeout_rx_;\ + qSendToBack(&queue, &msg);\ + } while(0) +#define SEND_FAULT_SYNC_TORQUE_VECTOR_FPGA(queue, idx_, latched_) do {\ + CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_FAULT_SYNC_TORQUE_VECTOR_FPGA, .DLC=DLC_FAULT_SYNC_TORQUE_VECTOR_FPGA, .IDE=1};\ + CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ + data_a->fault_sync_torque_vector_fpga.idx = idx_;\ + data_a->fault_sync_torque_vector_fpga.latched = latched_;\ + qSendToBack(&queue, &msg);\ + } while(0) +/* END AUTO SEND MACROS */ + +// Stale Checking +#define STALE_THRESH 3 / 2 // 3 / 2 would be 150% of period +/* BEGIN AUTO UP DEFS (Update Period)*/ +#define UP_FRONT_WHEEL_DATA 10 +#define UP_REAR_WHEEL_DATA 10 +/* END AUTO UP DEFS */ + +#define CHECK_STALE(stale, curr, last, period) if(!stale && \ + (curr - last) > period * STALE_THRESH) stale = 1 + +/* BEGIN AUTO CAN ENUMERATIONS */ +/* END AUTO CAN ENUMERATIONS */ + +// Message Raw Structures +/* BEGIN AUTO MESSAGE STRUCTURE */ +typedef union { + struct { + uint64_t front_left: 12; + uint64_t front_right: 12; + uint64_t rear_left: 12; + uint64_t rear_right: 12; + } torque_request; + struct { + uint64_t flash_success: 1; + uint64_t flash_timeout_rx: 1; + } bitstream_flash_status; + struct { + uint64_t idx: 16; + uint64_t latched: 1; + } fault_sync_torque_vector_fpga; + struct { + uint64_t left_speed: 16; + uint64_t right_speed: 16; + uint64_t left_normal: 16; + uint64_t right_normal: 16; + } front_wheel_data; + struct { + uint64_t left_speed: 16; + uint64_t right_speed: 16; + uint64_t left_normal: 16; + uint64_t right_normal: 16; + } rear_wheel_data; + struct { + uint64_t d0: 8; + uint64_t d1: 8; + uint64_t d2: 8; + uint64_t d3: 8; + uint64_t d4: 8; + uint64_t d5: 8; + uint64_t d6: 8; + uint64_t d7: 8; + } bitstream_data; + struct { + uint64_t download_request: 1; + uint64_t download_size: 32; + } bitstream_request; + struct { + uint64_t idx: 16; + uint64_t latched: 1; + } fault_sync_main_module; + struct { + uint64_t idx: 16; + uint64_t latched: 1; + } fault_sync_driveline; + struct { + uint64_t idx: 16; + uint64_t latched: 1; + } fault_sync_dashboard; + struct { + uint64_t idx: 16; + uint64_t latched: 1; + } fault_sync_precharge; + struct { + uint64_t idx: 16; + uint64_t latched: 1; + } fault_sync_test_node; + struct { + uint64_t id: 16; + uint64_t value: 1; + } set_fault; + struct { + uint64_t id: 16; + } return_fault_control; + uint8_t raw_data[8]; +} __attribute__((packed)) CanParsedData_t; +/* END AUTO MESSAGE STRUCTURE */ + +// contains most up to date received +// type for each variable matches that defined in JSON +/* BEGIN AUTO CAN DATA STRUCTURE */ +typedef struct { + struct { + uint16_t left_speed; + uint16_t right_speed; + uint16_t left_normal; + uint16_t right_normal; + uint8_t stale; + uint32_t last_rx; + } front_wheel_data; + struct { + uint16_t left_speed; + uint16_t right_speed; + uint16_t left_normal; + uint16_t right_normal; + uint8_t stale; + uint32_t last_rx; + } rear_wheel_data; + struct { + uint8_t d0; + uint8_t d1; + uint8_t d2; + uint8_t d3; + uint8_t d4; + uint8_t d5; + uint8_t d6; + uint8_t d7; + } bitstream_data; + struct { + uint8_t download_request; + uint32_t download_size; + } bitstream_request; + struct { + uint16_t idx; + uint8_t latched; + } fault_sync_main_module; + struct { + uint16_t idx; + uint8_t latched; + } fault_sync_driveline; + struct { + uint16_t idx; + uint8_t latched; + } fault_sync_dashboard; + struct { + uint16_t idx; + uint8_t latched; + } fault_sync_precharge; + struct { + uint16_t idx; + uint8_t latched; + } fault_sync_test_node; + struct { + uint16_t id; + uint8_t value; + } set_fault; + struct { + uint16_t id; + } return_fault_control; +} can_data_t; +/* END AUTO CAN DATA STRUCTURE */ + +extern can_data_t can_data; + +/* BEGIN AUTO EXTERN CALLBACK */ +extern void bitstream_request_CALLBACK(CanParsedData_t* msg_data_a); +extern void handleCallbacks(uint16_t id, bool latched); +extern void set_fault_daq(uint16_t id, bool value); +extern void return_fault_control(uint16_t id); +extern void send_fault(uint16_t id, bool latched); +/* END AUTO EXTERN CALLBACK */ + +/* BEGIN AUTO EXTERN RX IRQ */ +extern void bitstream_data_IRQ(CanParsedData_t* msg_data_a); +/* END AUTO EXTERN RX IRQ */ + +/** + * @brief Setup queue and message filtering + * + * @param q_rx_can RX buffer of CAN messages + */ +void initCANParse(q_handle_t* q_rx_can_a); + +/** + * @brief Pull message off of rx buffer, + * update can_data struct, + * check for stale messages + */ +void canRxUpdate(); + +/** + * @brief Process any rx message callbacks from the CAN Rx IRQ + * + * @param rx rx data from message just recieved + */ +void canProcessRxIRQs(CanMsgTypeDef_t* rx); + +#endif \ No newline at end of file diff --git a/source/torque_vector_fpga/main.c b/source/torque_vector_fpga/main.c new file mode 100644 index 00000000..1e16e6d8 --- /dev/null +++ b/source/torque_vector_fpga/main.c @@ -0,0 +1,177 @@ +/* System Includes */ +#include "stm32l432xx.h" +#include "system_stm32l4xx.h" +#include "can_parse.h" +#include "common/psched/psched.h" +#include "common/phal_L4/can/can.h" +#include "common/phal_L4/quadspi/quadspi.h" +#include "common/phal_L4/gpio/gpio.h" +#include "common/phal_L4/rcc/rcc.h" +#include "common/bootloader/bootloader_common.h" + + +/* Module Includes */ +#include "main.h" +#include "bitstream.h" + +#include "common/faults/faults.h" + +/* PER HAL Initilization Structures */ +GPIOInitConfig_t gpio_config[] = { + GPIO_INIT_CANRX_PA11, + GPIO_INIT_CANTX_PA12, + // QuadSPI Chip Selects + GPIO_INIT_OUTPUT(QUADSPI_CS_FLASH_GPIO_Port, QUADSPI_CS_FLASH_Pin, GPIO_OUTPUT_LOW_SPEED), + GPIO_INIT_OUTPUT(QUADSPI_CS_FPGA_GPIO_Port, QUADSPI_CS_FPGA_Pin, GPIO_OUTPUT_LOW_SPEED), + // QuadSPI Data/CLK + GPIO_INIT_AF(QUADSPI_CLK_GPIO_Port, QUADSPI_CLK_Pin, 10, GPIO_OUTPUT_LOW_SPEED, GPIO_OUTPUT_OPEN_DRAIN, GPIO_INPUT_OPEN_DRAIN), + GPIO_INIT_AF(QUADSPI_IO0_GPIO_Port, QUADSPI_IO0_Pin, 10, GPIO_OUTPUT_LOW_SPEED, GPIO_OUTPUT_OPEN_DRAIN, GPIO_INPUT_OPEN_DRAIN), + GPIO_INIT_AF(QUADSPI_IO1_GPIO_Port, QUADSPI_IO1_Pin, 10, GPIO_OUTPUT_LOW_SPEED, GPIO_OUTPUT_OPEN_DRAIN, GPIO_INPUT_OPEN_DRAIN), + GPIO_INIT_AF(QUADSPI_IO2_GPIO_Port, QUADSPI_IO2_Pin, 10, GPIO_OUTPUT_LOW_SPEED, GPIO_OUTPUT_OPEN_DRAIN, GPIO_INPUT_OPEN_DRAIN), + GPIO_INIT_AF(QUADSPI_IO3_GPIO_Port, QUADSPI_IO3_Pin, 10, GPIO_OUTPUT_LOW_SPEED, GPIO_OUTPUT_OPEN_DRAIN, GPIO_INPUT_OPEN_DRAIN), + // I2C Bus + GPIO_INIT_AF(I2C_SCL_GPIO_Port, I2C_SCL_Pin, 4, GPIO_OUTPUT_LOW_SPEED, GPIO_OUTPUT_OPEN_DRAIN, GPIO_INPUT_OPEN_DRAIN), + GPIO_INIT_AF(I2C_SDA_GPIO_Port, I2C_SDA_Pin, 4, GPIO_OUTPUT_LOW_SPEED, GPIO_OUTPUT_OPEN_DRAIN, GPIO_INPUT_OPEN_DRAIN), + GPIO_INIT_OUTPUT(I2C_WRITE_CONTROL_GPIO_Port, I2C_WRITE_CONTROL_Pin, GPIO_OUTPUT_LOW_SPEED), + // Status LEDs + GPIO_INIT_OUTPUT(ERROR_LED_GPIO_Port, ERROR_LED_Pin, GPIO_OUTPUT_LOW_SPEED), + GPIO_INIT_OUTPUT(CONN_LED_GPIO_Port, CONN_LED_Pin, GPIO_OUTPUT_LOW_SPEED), + GPIO_INIT_OUTPUT(HEARTBEAT_LED_GPIO_Port, HEARTBEAT_LED_Pin, GPIO_OUTPUT_LOW_SPEED) +}; + +#define TargetCoreClockrateHz 16000000 +ClockRateConfig_t clock_config = { + .system_source =SYSTEM_CLOCK_SRC_HSI, + .system_clock_target_hz =TargetCoreClockrateHz, + .ahb_clock_target_hz =(TargetCoreClockrateHz / 1), + .apb1_clock_target_hz =(TargetCoreClockrateHz / (1)), + .apb2_clock_target_hz =(TargetCoreClockrateHz / (1)), +}; + +/* Locals for Clock Rates */ +extern uint32_t APB1ClockRateHz; +extern uint32_t APB2ClockRateHz; +extern uint32_t AHBClockRateHz; +extern uint32_t PLLClockRateHz; + +/* Function Prototypes */ +void canReceiveTest(); +void canSendTest(); +void Error_Handler(); +void SysTick_Handler(); +void canTxUpdate(); +void blinkTask(); +void PHAL_FaltHandler(); +extern void HardFault_Handler(); + +q_handle_t q_tx_can; +q_handle_t q_rx_can; + +int main (void) +{ + // Main stack pointer is saved as the first entry in the .isr_entry + Bootloader_ConfirmApplicationLaunch(); + + /* Data Struct init */ + qConstruct(&q_tx_can, sizeof(CanMsgTypeDef_t)); + qConstruct(&q_rx_can, sizeof(CanMsgTypeDef_t)); + + /* HAL Initilization */ + if (0 != PHAL_configureClockRates(&clock_config)) + PHAL_FaltHandler(); + + if (1 != PHAL_initGPIO(gpio_config, sizeof(gpio_config)/sizeof(GPIOInitConfig_t))) + PHAL_FaltHandler(); + + if (1 != PHAL_initCAN(CAN1, false)) + PHAL_FaltHandler(); + + if (1 != PHAL_qspiInit()) + PHAL_FaltHandler(); + + NVIC_EnableIRQ(CAN1_RX0_IRQn); + + /* Module init */ + // bitstreamInit(); + schedInit(SystemCoreClock); // See Datasheet DS11451 Figure. 4 for clock tree + initCANParse(&q_rx_can); + + PHAL_writeGPIO(HEARTBEAT_LED_GPIO_Port, HEARTBEAT_LED_Pin, 1); + + /* Task Creation */ + taskCreate(blinkTask, 500); + taskCreate(bitstream10Hz, 100); + taskCreate(bitstream100Hz, 10); + taskCreateBackground(canTxUpdate); + taskCreateBackground(canRxUpdate); + schedStart(); + + return 0; +} + +void blinkTask() +{ + PHAL_toggleGPIO(HEARTBEAT_LED_GPIO_Port, HEARTBEAT_LED_Pin); +} + +void PHAL_FaltHandler() +{ + asm("bkpt"); + HardFault_Handler(); +} + +// *** Compulsory CAN Tx/Rx callbacks *** +void bootloader_request_reset_CALLBACK(CanParsedData_t* data) +{ + Bootloader_ResetForFirmwareDownload(); +} + +void canTxUpdate() +{ + CanMsgTypeDef_t tx_msg; + if (qReceive(&q_tx_can, &tx_msg) == SUCCESS_G) // Check queue for items and take if there is one + { + PHAL_txCANMessage(&tx_msg); + } +} + +void CAN1_RX0_IRQHandler() +{ + if (CAN1->RF0R & CAN_RF0R_FOVR0) // FIFO Overrun + CAN1->RF0R &= !(CAN_RF0R_FOVR0); + + if (CAN1->RF0R & CAN_RF0R_FULL0) // FIFO Full + CAN1->RF0R &= !(CAN_RF0R_FULL0); + + if (CAN1->RF0R & CAN_RF0R_FMP0_Msk) // Release message pending + { + CanMsgTypeDef_t rx; + + // Get either StdId or ExtId + if (CAN_RI0R_IDE & CAN1->sFIFOMailBox[0].RIR) + { + rx.ExtId = ((CAN_RI0R_EXID | CAN_RI0R_STID) & CAN1->sFIFOMailBox[0].RIR) >> CAN_RI0R_EXID_Pos; + } + else + { + rx.StdId = (CAN_RI0R_STID & CAN1->sFIFOMailBox[0].RIR) >> CAN_TI0R_STID_Pos; + } + + rx.DLC = (CAN_RDT0R_DLC & CAN1->sFIFOMailBox[0].RDTR) >> CAN_RDT0R_DLC_Pos; + + rx.Data[0] = (uint8_t) (CAN1->sFIFOMailBox[0].RDLR >> 0) & 0xFF; + rx.Data[1] = (uint8_t) (CAN1->sFIFOMailBox[0].RDLR >> 8) & 0xFF; + rx.Data[2] = (uint8_t) (CAN1->sFIFOMailBox[0].RDLR >> 16) & 0xFF; + rx.Data[3] = (uint8_t) (CAN1->sFIFOMailBox[0].RDLR >> 24) & 0xFF; + rx.Data[4] = (uint8_t) (CAN1->sFIFOMailBox[0].RDHR >> 0) & 0xFF; + rx.Data[5] = (uint8_t) (CAN1->sFIFOMailBox[0].RDHR >> 8) & 0xFF; + rx.Data[6] = (uint8_t) (CAN1->sFIFOMailBox[0].RDHR >> 16) & 0xFF; + rx.Data[7] = (uint8_t) (CAN1->sFIFOMailBox[0].RDHR >> 24) & 0xFF; + + canProcessRxIRQs(&rx); + + CAN1->RF0R |= (CAN_RF0R_RFOM0); + + qSendToBack(&q_rx_can, &rx); // Add to queue (qSendToBack is interrupt safe) + } +} \ No newline at end of file diff --git a/source/torque_vector_fpga/main.h b/source/torque_vector_fpga/main.h new file mode 100644 index 00000000..f499aca7 --- /dev/null +++ b/source/torque_vector_fpga/main.h @@ -0,0 +1,46 @@ +#ifndef MAIN_H_ +#define MAIN_H_ + +#include "common/faults/fault_nodes.h" + +#define FAULT_NODE_NAME NODE_TV + +// QuadSPI CS +#define QUADSPI_CS_FLASH_GPIO_Port (GPIOA) +#define QUADSPI_CS_FLASH_Pin (2) +#define QUADSPI_CS_FPGA_GPIO_Port (GPIOA) +#define QUADSPI_CS_FPGA_Pin (1) +// QuadSPI CLK +#define QUADSPI_CLK_GPIO_Port (GPIOA) +#define QUADSPI_CLK_Pin (3) +// QuadSPI I/O Ports +#define QUADSPI_IO0_GPIO_Port (GPIOB) +#define QUADSPI_IO0_Pin (2) +#define QUADSPI_IO1_GPIO_Port (GPIOB) +#define QUADSPI_IO1_Pin (1) +#define QUADSPI_IO2_GPIO_Port (GPIOA) +#define QUADSPI_IO2_Pin (7) +#define QUADSPI_IO3_GPIO_Port (GPIOA) +#define QUADSPI_IO3_Pin (6) + +// FPGA Configuration Reset +#define FPGA_CFG_RST_GPIO_Port (GPIOA) +#define FPGA_CFG_RST_Pin (4) + +// I2C Bus +#define I2C_SCL_GPIO_Port (GPIOB) +#define I2C_SCL_Pin (6) +#define I2C_SDA_GPIO_Port (GPIOB) +#define I2C_SDA_Pin (7) +#define I2C_WRITE_CONTROL_GPIO_Port (GPIOA) +#define I2C_WRITE_CONTROL_Pin (6) + +// Status LEDs +#define ERROR_LED_GPIO_Port (GPIOA) +#define ERROR_LED_Pin (15) +#define CONN_LED_GPIO_Port (GPIOB) +#define CONN_LED_Pin (4) +#define HEARTBEAT_LED_GPIO_Port (GPIOB) +#define HEARTBEAT_LED_Pin (5) + +#endif \ No newline at end of file