From d4790b526dc92366fb6897fdad9af3d8a4b56059 Mon Sep 17 00:00:00 2001 From: Aditya Anand <90068582+AdityaAsGithub@users.noreply.github.com> Date: Tue, 30 Jan 2024 15:39:21 -0500 Subject: [PATCH] TV F7 Migration + PCB bring up (#85) * TV working * GPS fully updated, Magnetometer removed (no longer needed) * Fixed CircleCI Error * small change --------- Co-authored-by: AdityaAsGithub --- .vscode/launch.json | 6 +- common/phal_F4_F7/gpio/gpio.h | 17 +++ common/phal_F4_F7/usart/usart.c | 175 ++++++++++++++++++++++++++ common/phal_F4_F7/usart/usart.h | 16 +-- source/torque_vector/CMakeLists.txt | 4 +- source/torque_vector/bmi/bmi088.c | 2 +- source/torque_vector/bmi/bmi088.h | 5 +- source/torque_vector/bmm/bmm150.c | 178 --------------------------- source/torque_vector/bmm/bmm150.h | 105 ---------------- source/torque_vector/can/can_parse.h | 2 +- source/torque_vector/gps/gps.c | 41 ++++-- source/torque_vector/gps/gps.h | 21 ++++ source/torque_vector/imu/imu.c | 4 - source/torque_vector/main.c | 169 +++++++++---------------- source/torque_vector/main.h | 3 + 15 files changed, 326 insertions(+), 422 deletions(-) delete mode 100644 source/torque_vector/bmm/bmm150.c delete mode 100644 source/torque_vector/bmm/bmm150.h diff --git a/.vscode/launch.json b/.vscode/launch.json index c4d0dfad..0a99b4b4 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -67,11 +67,11 @@ "request": "launch", "type": "cortex-debug", "servertype": "openocd", - "device:": "STM32L471RE", - "svdFile": "${workspaceRoot}/common/svd/STM32L4x2.svd", + "device:": "STM32F732RE", + "svdFile": "${workspaceRoot}/common/svd/STM32F7x.svd", "configFiles": [ "interface/stlink.cfg", - "target/stm32l4x.cfg" + "target/stm32f7x.cfg" ], "debuggerArgs": [ "-d", diff --git a/common/phal_F4_F7/gpio/gpio.h b/common/phal_F4_F7/gpio/gpio.h index d9a793a6..b7447f6d 100644 --- a/common/phal_F4_F7/gpio/gpio.h +++ b/common/phal_F4_F7/gpio/gpio.h @@ -177,6 +177,7 @@ typedef struct If you find yourself adding the same pin mappings to multiple devices, add a macro below to cut down on duplication. */ +#ifdef STM32F407xx //CAN #define GPIO_INIT_CANRX_PD0 GPIO_INIT_AF(GPIOD, 0, 9, GPIO_OUTPUT_ULTRA_SPEED, GPIO_OUTPUT_OPEN_DRAIN, GPIO_INPUT_OPEN_DRAIN) #define GPIO_INIT_CANTX_PD1 GPIO_INIT_AF(GPIOD, 1, 9, GPIO_OUTPUT_ULTRA_SPEED, GPIO_OUTPUT_PUSH_PULL, GPIO_INPUT_OPEN_DRAIN) @@ -196,6 +197,22 @@ typedef struct #define GPIO_INIT_USART2TX_PA2 GPIO_INIT_AF(GPIOA, 2, 7, GPIO_OUTPUT_ULTRA_SPEED, GPIO_OUTPUT_PUSH_PULL, GPIO_INPUT_OPEN_DRAIN) #define GPIO_INIT_USART2RX_PA3 GPIO_INIT_AF(GPIOA, 3, 7, GPIO_OUTPUT_ULTRA_SPEED, GPIO_OUTPUT_OPEN_DRAIN, GPIO_INPUT_OPEN_DRAIN) +#endif + +#ifdef STM32F732xx +//CAN +#define GPIO_INIT_CANRX_PA11 GPIO_INIT_AF(GPIOA, 11, 9, GPIO_OUTPUT_ULTRA_SPEED, GPIO_OUTPUT_OPEN_DRAIN, GPIO_INPUT_OPEN_DRAIN) +#define GPIO_INIT_CANTX_PA12 GPIO_INIT_AF(GPIOA, 12, 9, GPIO_OUTPUT_ULTRA_SPEED, GPIO_OUTPUT_PUSH_PULL, GPIO_INPUT_OPEN_DRAIN) + + +//SPI + + + +//UART/USART +#define GPIO_INIT_UART4TX_PA0 GPIO_INIT_AF(GPIOA, 0, 8, GPIO_OUTPUT_ULTRA_SPEED, GPIO_OUTPUT_PUSH_PULL, GPIO_INPUT_OPEN_DRAIN) +#define GPIO_INIT_UART4RX_PA1 GPIO_INIT_AF(GPIOA, 1, 8, GPIO_OUTPUT_ULTRA_SPEED, GPIO_OUTPUT_OPEN_DRAIN, GPIO_INPUT_OPEN_DRAIN) +#endif #define GPIO_INIT_USART1TX_PA9 GPIO_INIT_AF(GPIOA, 9, 7, GPIO_OUTPUT_ULTRA_SPEED, GPIO_OUTPUT_PUSH_PULL, GPIO_INPUT_OPEN_DRAIN) #define GPIO_INIT_USART1RX_PA10 GPIO_INIT_AF(GPIOA, 10, 7, GPIO_OUTPUT_ULTRA_SPEED, GPIO_OUTPUT_OPEN_DRAIN, GPIO_INPUT_OPEN_DRAIN) diff --git a/common/phal_F4_F7/usart/usart.c b/common/phal_F4_F7/usart/usart.c index fe921096..dc260400 100644 --- a/common/phal_F4_F7/usart/usart.c +++ b/common/phal_F4_F7/usart/usart.c @@ -49,6 +49,7 @@ bool PHAL_initUSART(usart_init_t* handle, const uint32_t fck) // Enable peripheral clock in RCC // ADD: When adding a new peripheral, be sure to enable the clock here + #ifdef STM32F407xx switch ((ptr_int) handle->periph) { case USART1_BASE: RCC->APB2ENR |= RCC_APB2ENR_USART1EN; @@ -59,6 +60,16 @@ bool PHAL_initUSART(usart_init_t* handle, const uint32_t fck) default: return false; } + #endif + #ifdef STM32F732xx + switch ((ptr_int) handle->periph) { + case UART4_BASE: + RCC->APB1ENR |= RCC_APB1ENR_UART4EN; + break; + default: + return false; + } + #endif // The following FBRG configuration is based off RM0090 p. 978 - 980 over8 = 8 << !handle->ovsample; @@ -131,8 +142,14 @@ bool PHAL_usartTxDma(usart_init_t* handle, uint16_t* data, uint32_t len) { while ((active_uarts[handle->usart_active_num].active_handle->periph->SR & USART_SR_RXNE)) ; #endif + #ifdef STM32F732xx + // Ensure any RX data is not overwritten before continuing with transfer + while ((active_uarts[handle->usart_active_num].active_handle->periph->ISR & USART_ISR_RXNE)) + ; + #endif // Enable All Interrupts needed to complete Tx transaction // ADD: Ensure you enable the TX DMA interrupt for a new UART peripheral + #ifdef STM32F407xx switch ((ptr_int) handle->periph) { case USART2_BASE: NVIC_EnableIRQ(DMA1_Stream6_IRQn); @@ -140,6 +157,16 @@ bool PHAL_usartTxDma(usart_init_t* handle, uint16_t* data, uint32_t len) { default: return false; } + #endif + #ifdef STM32F732xx + switch ((ptr_int) handle->periph) { + case UART4_BASE: + NVIC_EnableIRQ(DMA1_Stream4_IRQn); + break; + default: + return false; + } + #endif PHAL_DMA_setTxferLength(handle->tx_dma_cfg, len); PHAL_DMA_setMemAddress(handle->tx_dma_cfg, (uint32_t) data); @@ -167,6 +194,7 @@ bool PHAL_usartRxDma(usart_init_t* handle, uint16_t* data, uint32_t len, bool co handle->periph->CR1 |= USART_CR1_RE; // Enable All Interrupts needed to complete Tx transaction // ADD: Add cases to this switch statement to enable DMA and USART interrupts for RX USART peripherals + #ifdef STM32F407xx switch ((ptr_int) handle->periph) { case USART2_BASE: NVIC_EnableIRQ(DMA1_Stream5_IRQn); @@ -175,6 +203,17 @@ bool PHAL_usartRxDma(usart_init_t* handle, uint16_t* data, uint32_t len, bool co default: return false; } + #endif + #ifdef STM32F732xx + switch ((ptr_int) handle->periph) { + case UART4_BASE: + NVIC_EnableIRQ(DMA1_Stream2_IRQn); + NVIC_EnableIRQ(UART4_IRQn); + break; + default: + return false; + } + #endif // Configure parts of DMA that will not change each transaction, and enable DMA on USART PHAL_DMA_setMemAddress(handle->rx_dma_cfg, (uint32_t) data); handle->periph->CR3 |= USART_CR3_DMAR; @@ -191,6 +230,7 @@ bool PHAL_disableContinousRxDMA(usart_init_t *handle) handle->periph->CR3 &= ~USART_CR3_DMAR; // Enable All Interrupts needed to complete Tx transaction // ADD: Add cases to this switch statement to disable interrupts for new USART peripherals + #ifdef STM32F407xx switch ((ptr_int) handle->periph) { case USART2_BASE: NVIC_DisableIRQ(DMA1_Stream5_IRQn); @@ -199,6 +239,17 @@ bool PHAL_disableContinousRxDMA(usart_init_t *handle) default: return false; } + #endif + #ifdef STM32F732xx + switch ((ptr_int) handle->periph) { + case USART2_BASE: + NVIC_DisableIRQ(DMA1_Stream2_IRQn); + NVIC_DisableIRQ(UART4_IRQn); + break; + default: + return false; + } + #endif // RX will no longer be busy active_uarts[handle->usart_active_num]._rx_busy = 0; } @@ -289,6 +340,93 @@ static void handleUsartIRQ(USART_TypeDef *handle, uint8_t idx) } //NOTE: According to RM0090, it is not safe to clear the TC bit unless Multibuffer mode is enabled. #endif + + #ifdef STM32F732xx + uint32_t sr = handle->ISR; + static uint32_t trash; + // USART RX Not Empty interrupt flag + if ( sr & USART_ISR_RXNE) + { + // Rx transaction is beginning, so set rx to busy and enable DMA to recieve this message + active_uarts[idx]._rx_busy = 1; + PHAL_DMA_setTxferLength(active_uarts[idx].active_handle->rx_dma_cfg, active_uarts[idx].rxfer_size); + PHAL_reEnable(active_uarts[idx].active_handle->rx_dma_cfg); + // QUESTION: + // active_uarts[idx].active_handle->periph->ICR |= USART_ICR_RXNECF; // Clear RXNE interrupt flag + // trash = active_uarts[idx].active_handle->periph->; // Clear RXNE interrupt flag + // We only need to enable DMA immediately after the reception of the first bit + // We also do not want this interrupt activating for every single bit recieved on the rx buffer + active_uarts[idx].active_handle->periph->CR1 &= ~USART_CR1_RXNEIE; + // Clear any errors that may have been set in the previous Rx + active_uarts[idx].active_handle->rx_errors.framing_error = 0; + active_uarts[idx].active_handle->rx_errors.noise_detected = 0; + active_uarts[idx].active_handle->rx_errors.overrun = 0; + active_uarts[idx].active_handle->rx_errors.parity_error = 0; + } + // Clear to send flag + if (sr & USART_ISR_CTS) + { + active_uarts[idx].active_handle->periph->ICR |= USART_ICR_CMCF; // We currently do not plan on using CTS or LIN mode in the near future, so this is placeholder code for the future + } + // Lin Break Detection flag + if (sr & USART_ISR_LBDF) + { + active_uarts[idx].active_handle->periph->ICR |= USART_ICR_LBDCF; // We currently do not plan on using CTS or LIN mode in the near future, so this is placeholder code for the future + } + // Overrun Error Flag + if (sr & USART_ISR_ORE) + { + // Communicate these errors to the user, so we can clear this bit but leave USART clear for future transactions + active_uarts[idx].active_handle->rx_errors.overrun = 1; + active_uarts[idx].active_handle->periph->ICR |= USART_ICR_ORECF; + } + // Noise Error Flag + if (sr & USART_ISR_NE) + { + // Communicate these errors to the user, so we can clear this bit for future transactions + active_uarts[idx].active_handle->rx_errors.noise_detected = 1; + active_uarts[idx].active_handle->periph->ICR |= USART_ICR_NCF; + } + // Framing Error Flag + if (sr & USART_ISR_FE) + { + // Communicate these errors to the user, so we can clear this bit for future transactions + active_uarts[idx].active_handle->rx_errors.framing_error = 1; + active_uarts[idx].active_handle->periph->ICR |= USART_ICR_FECF; + } + // Parity Error Flag + if (sr & USART_ISR_PE) + { + // Communicate these errors to the user, so we can clear this bit for future transactions + active_uarts[idx].active_handle->rx_errors.parity_error = 1; + active_uarts[idx].active_handle->periph->ICR |= USART_ICR_PECF; + } + + // Idle line is detected upon the completion of a USART reception + // This is the last flag handled, so that important info can be updated before callback function + if (sr & USART_ISR_IDLE) + { + // Stop DMA Transaction + PHAL_stopTxfer(active_uarts[idx].active_handle->rx_dma_cfg); + if (active_uarts[idx].cont_rx) + { + // Re-enable the RX not empty interrupt to accept the next message + active_uarts[idx].active_handle->periph->CR1 |= USART_CR1_RXNEIE; + } + else + { + // No more messages should be recieved, so disable RX mode for the peripheral + active_uarts[idx].active_handle->periph->CR1 &= ~USART_CR1_RE; + } + active_uarts[idx]._rx_busy = 0; + // Clear idle flag + // trash = active_uarts[idx].active_handle->periph->ISR; + // trash = active_uarts[idx].active_handle->periph->DR; + active_uarts[idx].active_handle->periph->ICR |= USART_ICR_IDLECF; + usart_recieve_complete_callback(active_uarts[idx].active_handle); + } + //NOTE: According to 0431, there is no point to clearing the TC bit (we do not handle transmissions here anyway) + #endif } /** @@ -439,6 +577,11 @@ static void handleDMAxComplete(uint8_t idx, uint32_t irq, uint8_t dma_type) while (!(active_uarts[idx].active_handle->periph->SR & USART_SR_TC)) ; #endif + #ifdef STM32F732xx + // Wait for the transfer complete bit to be set, indicating the completion of USART transaction + while (!(active_uarts[idx].active_handle->periph->ISR & USART_ISR_TC)) + ; + #endif // TX is no longer busy, so communicate this and disable TX part of USART active_uarts[idx]._tx_busy = 0; active_uarts[idx].active_handle->periph->CR1 &= ~USART_CR1_TE; @@ -520,6 +663,7 @@ USART TX and RX interrupts - need to modify when adding a usart peripheral //ADD: +#ifdef STM32F407xx // USART1: // USART2: @@ -547,4 +691,35 @@ void USART2_IRQHandler() // USART3: +#endif + +#ifdef STM32F732xx +// USART1: + +// USART2: + +// Add new DMA interrupt handlers here, passing in new active struct index, along with +// the correct DMA IRQ, and corresponding transfer mode (TX or Rx) +void DMA1_Stream4_IRQHandler() //TX +{ + handleDMAxComplete(USART4_ACTIVE_IDX, DMA1_Stream4_IRQn, USART_DMA_TX); +} + +// Add new DMA interrupt handlers here, passing in new active struct index, along with +// the correct DMA IRQ, and corresponding transfer mode (TX or Rx) +void DMA1_Stream2_IRQHandler() //RX +{ + handleDMAxComplete(USART4_ACTIVE_IDX, DMA1_Stream2_IRQn, USART_DMA_RX); +} + +// Add new USART Interrupts as new peripherals are needed, +// feeding in the new USART peripheral, along with active array index +void UART4_IRQHandler() +{ + handleUsartIRQ(UART4, USART4_ACTIVE_IDX); +} + +// USART3: + +#endif //...etc \ No newline at end of file diff --git a/common/phal_F4_F7/usart/usart.h b/common/phal_F4_F7/usart/usart.h index 22156a7e..5bba0822 100644 --- a/common/phal_F4_F7/usart/usart.h +++ b/common/phal_F4_F7/usart/usart.h @@ -228,24 +228,24 @@ extern void usart_recieve_complete_callback(usart_init_t *handle); .periph=DMA1, .stream=DMA1_Stream6 \ } #else - #define USART2_RXDMA_CONT_CONFIG(rx_addr_, priority_) \ + #define USART4_RXDMA_CONT_CONFIG(rx_addr_, priority_) \ { \ - .periph_addr = (uint32_t) & (USART2->TDR), .mem_addr = (uint32_t)(rx_addr_), \ + .periph_addr = (uint32_t) & (UART4->RDR), .mem_addr = (uint32_t)(rx_addr_), \ .tx_size = 1, .increment = false, .circular = false, \ .dir = 0b0, .mem_inc = true, .periph_inc = false, .mem_to_mem = false, \ .priority = (priority_), .mem_size = 0b00, .periph_size = 0b00, \ - .tx_isr_en = false, .dma_chan_request=0b0100, .stream_idx=5, \ - .periph=DMA1, .stream=DMA1_Stream5 \ + .tx_isr_en = true, .dma_chan_request=0b0100, .stream_idx=2, \ + .periph=DMA1, .stream=DMA1_Stream2 \ } - #define USART2_TXDMA_CONT_CONFIG(tx_addr_, priority_) \ + #define USART4_TXDMA_CONT_CONFIG(tx_addr_, priority_) \ { \ - .periph_addr = (uint32_t) & (USART2->RDR), .mem_addr = (uint32_t)(tx_addr_), \ + .periph_addr = (uint32_t) & (UART4->TDR), .mem_addr = (uint32_t)(tx_addr_), \ .tx_size = 1, .increment = false, .circular = false, \ .dir = 0b1, .mem_inc = true, .periph_inc = false, .mem_to_mem = false, \ .priority = (priority_), .mem_size = 0b00, .periph_size = 0b00, \ - .tx_isr_en = true, .dma_chan_request=0b0100, .stream_idx=6, \ - .periph=DMA1, .stream=DMA1_Stream6 \ + .tx_isr_en = true, .dma_chan_request=0b0100, .stream_idx=4, \ + .periph=DMA1, .stream=DMA1_Stream4 \ } #endif #endif \ No newline at end of file diff --git a/source/torque_vector/CMakeLists.txt b/source/torque_vector/CMakeLists.txt index 8d5829fa..741a9d72 100644 --- a/source/torque_vector/CMakeLists.txt +++ b/source/torque_vector/CMakeLists.txt @@ -14,7 +14,7 @@ target_link_libraries(${TARGET_NAME} ${CMAKE_CURRENT_LIST_DIR}/bsxlite/libalgobs set_target_properties(${TARGET_NAME} PROPERTIES COMPONENT_NAME ${COMPONENT_NAME} COMPONENT_DIR ${CMAKE_CURRENT_LIST_DIR} - LINKER_SCRIPT "STM32L496VGTx_FLASH" - COMMON_LIBS "CMSIS_L471;PSCHED_L471;QUEUE;PHAL_L471;libm.a;" + LINKER_SCRIPT "STM32F732RETX_FLASH" + COMMON_LIBS "CMSIS_F732;PSCHED_F732;QUEUE;PHAL_F732;libm.a;" ) COMMON_FIRMWARE_COMPONENT(${TARGET_NAME}) diff --git a/source/torque_vector/bmi/bmi088.c b/source/torque_vector/bmi/bmi088.c index 12039629..adb1510d 100644 --- a/source/torque_vector/bmi/bmi088.c +++ b/source/torque_vector/bmi/bmi088.c @@ -11,7 +11,7 @@ #include "bmi088.h" #include "bsxlite_interface.h" -#include "common/phal_L4/spi/spi.h" +#include "common/phal_F4_F7/spi/spi.h" #include "common_defs.h" #include "main.h" diff --git a/source/torque_vector/bmi/bmi088.h b/source/torque_vector/bmi/bmi088.h index b9c37d66..9fe52c1e 100644 --- a/source/torque_vector/bmi/bmi088.h +++ b/source/torque_vector/bmi/bmi088.h @@ -12,13 +12,12 @@ #ifndef _BMI_H_ #define _BMI_H_ -#include "stm32l4xx.h" #include #include #include #include "bsxlite_interface.h" -#include "common/phal_L4/gpio/gpio.h" -#include "common/phal_L4/spi/spi.h" +#include "common/phal_F4_F7/gpio/gpio.h" +#include "common/phal_F4_F7/spi/spi.h" #define BMI088_GYRO_CHIP_ID (0x0FU) #define BMI088_ACC_CHIP_ID (0x1EU) diff --git a/source/torque_vector/bmm/bmm150.c b/source/torque_vector/bmm/bmm150.c deleted file mode 100644 index 3e800c74..00000000 --- a/source/torque_vector/bmm/bmm150.c +++ /dev/null @@ -1,178 +0,0 @@ -/** - * @file bmm150.c - * @author Christopher McGalliard (cmcgalli@purdue.edu) - * @brief - * @version 0.3 - * @date 2023-02-02 - * - * @copyright Copyright (c) 2023 - * - */ - -#include "bmm150.h" -#include "main.h" -#include "bmi088.h" -#include "common/phal_L4/spi/spi.h" -#include "common_defs.h" -#include "stdbool.h" -#include "can_parse.h" -#include "sfs_pp.h" - -static inline void BMM150_selectMag(BMM150_Handle_t *bmm); -bool BMM150_readID(BMM150_Handle_t *bmm); -void BMM150_powerOnMag(BMM150_Handle_t *bmm); -bool BMM150_init(BMM150_Handle_t *bmm); -bool BMM150_readMag(BMM150_Handle_t *bmm, ExtU *rtU); -void BMM150_setActive(BMM150_Handle_t *bmm); -bool BMM150_selfTest(BMM150_Handle_t *bmm); -bool BMM150_selfTestAdvanced(BMM150_Handle_t *bmm); -int16_t raw_x, raw_y, raw_z; -int16_t new_xa; -int16_t new_xb; -int16_t new_ya; -int16_t new_yb; -int16_t new_za; -int16_t new_zb; -int16_t result_value_x; -int16_t result_value_y; -int16_t result_value_z; -int16_t main_result; -int16_t diff; -extern q_handle_t q_tx_can; - -bool BMM150_readMag(BMM150_Handle_t *bmm, ExtU *rtU) -{ - // SPI rx and tx buffers - static uint8_t spi_rx_buff[16] = {0}; - static uint8_t spi_tx_buff[16] = {0}; - - // Prepare tx buffer with x axis LSB address - spi_tx_buff[0] = (1 << 7) | BMM150_MAG_X_LSB_ADDR; - - // SPI nss for BMM - BMM150_selectMag(bmm); - - // Wait until spi is available - while (PHAL_SPI_busy(bmm->spi)) - ; - - // Complete transfer - PHAL_SPI_transfer_noDMA(bmm->spi, spi_tx_buff, 1, 6, spi_rx_buff); - // while (PHAL_SPI_busy(bmm->spi)) - // ; - - // Parse x axis mag - new_xa = (spi_rx_buff[1] >> 3) & 0x001f; - new_xb = (spi_rx_buff[2] << 8) & 0xff00; - result_value_x = new_xa | (new_xb >> 3); - - // Parse y axis mag - new_ya = (spi_rx_buff[3] >> 3) & 0x001f; - new_yb = (spi_rx_buff[4] << 8) & 0xff00; - result_value_y = new_ya | (new_yb >> 3); - - // Parse z axis mag - new_za = (spi_rx_buff[5] >> 1) & 0x7f; - new_zb = (spi_rx_buff[6] << 8) & 0xff00; - result_value_z = new_za | new_zb; - - // Main result (needs to be sqrt) - main_result = (result_value_x * result_value_x) + (result_value_y * result_value_y) + (result_value_y * result_value_y); - - rtU->mag[0] = CLAMP(((double)result_value_x) * MAG_CALIBRATION, MIN_MAG, MAX_MAG); - rtU->mag[1] = CLAMP(((double)result_value_y) * MAG_CALIBRATION, MIN_MAG, MAX_MAG); - rtU->mag[2] = CLAMP(((double)result_value_z) * MAG_CALIBRATION, MIN_MAG, MAX_MAG); - - SEND_BMM_MAG(q_tx_can, result_value_x, result_value_y, result_value_z); - return true; -} - -bool BMM150_selfTest(BMM150_Handle_t *bmm) -{ - BMM150_selectMag(bmm); - PHAL_SPI_writeByte(bmm->spi, BMM150_OP_MODE_ADDR, 0b00000110); - if (!(PHAL_SPI_readByte(bmm->spi, 0x42, true) && 0b00000001)) - { - return false; - } - if (!(PHAL_SPI_readByte(bmm->spi, 0x44, true) && 0b00000001)) - { - return false; - } - if (!(PHAL_SPI_readByte(bmm->spi, 0x46, true) && 0b00000001)) - { - return false; - } - BMM150_setActive(bmm); - return true; -} - -bool BMM150_selfTestAdvanced(BMM150_Handle_t *bmm) -{ - BMM150_selectMag(bmm); - // 1. Set sleep mode - PHAL_SPI_writeByte(bmm->spi, BMM150_OP_MODE_ADDR, 0b00000110); - // 2. Disable X, Y axis - PHAL_SPI_writeByte(bmm->spi, 0x4E, 0b00011111); - // 3. Set Z repetitions to desired level - PHAL_SPI_writeByte(bmm->spi, 0x52, 0x04); - // 4. Enable positive advanced self test current - PHAL_SPI_writeByte(bmm->spi, BMM150_OP_MODE_ADDR, 0b11000110); - // 5. Set forced mode, readout Z and R channel after measurement is finished - PHAL_SPI_writeByte(bmm->spi, BMM150_OP_MODE_ADDR, 0b11000010); - uint8_t z_byte_1 = PHAL_SPI_readByte(bmm->spi, 0x46, true); - uint8_t z_byte_2 = PHAL_SPI_readByte(bmm->spi, 0x47, true); - int16_t pos_z_res = ((z_byte_1 >> 1) & 0x7f) | ((z_byte_2 << 8) & 0xff00); - // enable no current - PHAL_SPI_writeByte(bmm->spi, BMM150_OP_MODE_ADDR, 0b00000010); - // 6. Enable negative advanced self test current - PHAL_SPI_writeByte(bmm->spi, BMM150_OP_MODE_ADDR, 0b10000010); - // 7. Set forced mode, readout Z and R channel after measurement is finished - PHAL_SPI_writeByte(bmm->spi, BMM150_OP_MODE_ADDR, 0b10000010); - uint8_t z_byte_3 = PHAL_SPI_readByte(bmm->spi, 0x46, true); - uint8_t z_byte_4 = PHAL_SPI_readByte(bmm->spi, 0x47, true); - int16_t neg_z_res = ((z_byte_3 >> 1) & 0x7f) | ((z_byte_4 << 8) & 0xff00); - // 8. Disable advanced self test current(this must be done manually) - PHAL_SPI_writeByte(bmm->spi, BMM150_OP_MODE_ADDR, 0b00000010); - // 9. Calculate difference between the two compensated field values.This difference should be around 200 μT with some margins.10. Perform a soft reset of manually restore desired settings - diff = pos_z_res - neg_z_res; - BMM150_setActive(bmm); - return true; -} - -void BMM150_powerOnMag(BMM150_Handle_t *bmm) -{ - BMM150_selectMag(bmm); - PHAL_SPI_writeByte(bmm->spi, 0x4b, 0b00000001); - BMM150_setActive(bmm); - return; -} - -void BMM150_setActive(BMM150_Handle_t *bmm) -{ - BMM150_selectMag(bmm); - PHAL_SPI_writeByte(bmm->spi, BMM150_OP_MODE_ADDR, 0b00000000); - return; -} -uint8_t testresult = -2; -bool BMM150_readID(BMM150_Handle_t *bmm) -{ - BMM150_selectMag(bmm); - PHAL_SPI_writeByte(bmm->spi, 0x4b, 0b00000001); - PHAL_SPI_writeByte(bmm->spi, BMM150_OP_MODE_ADDR, 0b00111000); - if (PHAL_SPI_readByte(bmm->spi, BMM150_CHIP_ID_ADDR, true) != BMM150_CHIP_ID) - { - // PHAL_writeGPIO(SPI_CS_MAG_GPIO_Port, SPI_CS_MAG_Pin, 1); - return false; - } - // PHAL_writeGPIO(SPI_CS_MAG_GPIO_Port, SPI_CS_MAG_Pin, 1); - return true; -} - -static inline void BMM150_selectMag(BMM150_Handle_t *bmm) -{ - // PHAL_writeGPIO(SPI_CS_ACEL_GPIO_Port, SPI_CS_ACEL_Pin, 1); - // PHAL_writeGPIO(SPI_CS_GYRO_GPIO_Port, SPI_CS_GYRO_Pin, 1); - bmm->spi->nss_gpio_port = bmm->mag_csb_gpio_port; - bmm->spi->nss_gpio_pin = bmm->mag_csb_pin; -} \ No newline at end of file diff --git a/source/torque_vector/bmm/bmm150.h b/source/torque_vector/bmm/bmm150.h deleted file mode 100644 index 6f7a5008..00000000 --- a/source/torque_vector/bmm/bmm150.h +++ /dev/null @@ -1,105 +0,0 @@ -/** - * @file bmm150.h - * @author your name (cmcgalli@purdue.edu) - * @brief - * @version 0.1 - * @date 2023-01-26 - * - * @copyright Copyright (c) 2023 - * - */ - -#ifndef _BMM150_H_ -#define _BMM150_H_ - -#include "stm32l4xx.h" -#include -#include -#include -#include "common/phal_L4/gpio/gpio.h" -#include "common/phal_L4/spi/spi.h" -#include "SFS.h" - -// Suspend mode is the default power mode of BMM150 after the chip is powered -// The device can switch into Active mode from Sleep mode by setting OpMode bits (register 0x4C). - -/* -DATAX_LSB (0x42) contains 5-bit LSB part [4:0] of the 13 bit output data of the X-channel. -DATAX_MSB (0x43) contains 8-bit MSB part [12:5] of the 13 bit output data of the X-channel. -DATAY_LSB (0x44) contains 5-bit LSB part [4:0] of the 13 bit output data of the Y-channel. -DATAY_MSB (0x45) contains 8-bit MSB part [12:5] of the 13 bit output data of the Y-channel. -The width of the Z-axis magnetic field data is 15 bit word stored in two’s complement. -DATAZ_LSB (0x46) contains 7-bit LSB part [6:0] of the 15 bit output data of the Z-channel. -DATAZ_MSB (0x47) contains 8-bit MSB part [14:7] of the 15 bit output data of the Z-channel. -*/ -// All signed register values are in two´s complement representation. - -/* -The “Data ready status” bit (register 0x48 bit0) is set “1” when the data registers have been -updated but the data was not yet read out over digital interface. Data ready is cleared (set “0”) -directly after completed read out of any of the data registers and subsequent stop condition (I²C) -or lifting of CSB (SPI). -*/ - -// Register 0x4C <1:0> -// Bits 1 and 2 -typedef enum -{ - OPMode_NORMAL = 0b00, - OPMode_FORCED = 0b01, - OPMode_SLEEP = 0b11, -} BMM150_OPMode_t; - -// Register 0x4C <2:0> -// Bits 3, 4, and 5 -// output data rate -typedef enum -{ - BMM_ODR_10hz = 0b000, - BMM_ODR_2hz = 0b001, - BMM_ODR_6hz = 0b010, - BMM_ODR_8hz = 0b011, -} BMM150_ODR_t; - -typedef union -{ - struct - { - int16_t value : 13; - int16_t excess : 3; - }; - uint8_t raw_data[2]; -} __attribute__((packed)) parsed_data_t; - -typedef union -{ - struct - { - int16_t value : 15; - int16_t excess : 1; - }; - uint8_t raw_data_1; - uint8_t raw_data_2; -} __attribute__((packed)) parsed_data_z_t; - -typedef struct -{ - SPI_InitConfig_t *spi; - GPIO_TypeDef *mag_csb_gpio_port; - uint32_t mag_csb_pin; -} BMM150_Handle_t; - -#define BMM150_CHIP_ID_ADDR (0x40) -#define BMM150_CHIP_ID (0x32) -#define BMM150_OP_MODE_ADDR (0x4C) -#define BMM150_MAG_X_LSB_ADDR (0x42) -#define BMM150_MAG_X_MSB_ADDR (0x43) - -bool BMM150_readID(BMM150_Handle_t *bmm); -void BMM150_powerOnMag(BMM150_Handle_t *bmm); -bool BMM150_init(BMM150_Handle_t *bmm); -bool BMM150_readMag(BMM150_Handle_t *bmm, ExtU *rtU); -void BMM150_setActive(BMM150_Handle_t *bmm); -bool BMM150_selfTest(BMM150_Handle_t *bmm); -bool BMM150_selfTestAdvanced(BMM150_Handle_t *bmm); -#endif \ 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 097f95fe..a2887538 100644 --- a/source/torque_vector/can/can_parse.h +++ b/source/torque_vector/can/can_parse.h @@ -13,7 +13,7 @@ #include "common/queue/queue.h" #include "common/psched/psched.h" -#include "common/phal_L4/can/can.h" +#include "common/phal_F4_F7/can/can.h" // Make this match the node name within the can_config.json #define NODE_NAME "torque_vector" diff --git a/source/torque_vector/gps/gps.c b/source/torque_vector/gps/gps.c index c9f462e2..cfdf7387 100644 --- a/source/torque_vector/gps/gps.c +++ b/source/torque_vector/gps/gps.c @@ -4,6 +4,23 @@ #include "sfs_pp.h" #include "SFS.h" +/* + Instructions on configuring GPS: + 1. GPS will not send messages without a fix, so find a space where the GPS gets a fix (use UCenter for this) + 2. Pull up configuration menu, go to "msg" configuration + 2a. Select "01-07 NAV-PVT", and enable it on your desired communication port and USB + 2b. Find any messages containing "NMEA", and disable them for USB and your desired communication port + 2c. Send this config change to the GPS + 3. Go to "PRT (Port)" configuration + 3a. This is where you configure your peripheral. Currently, USART1, with baudrate at 115200 is used. Also, ensure that "UBX" is selected ad protocol out. + 3b. Send this config change to the GPS + 4. Go to "RATE" configuration + 4a. Select 40ms as the measurement period + 4b. Send this config change to the GPS + 5. Go to "CFG" setting, and hit the "send" button to once again save changes to the GPS. + 6. At this point, confirm you changes by turning it off and reconnecting it, ensuring the correct message is sent at 40ms +*/ + union i_Long iLong; union i_Short iShort; @@ -41,13 +58,17 @@ GPS_Handle_t gps_handle = {.raw_message = {0}, // Parse velocity from raw message bool parseVelocity(GPS_Handle_t *GPS, ExtU *rtU) { + // For future reference, we use the UBX protocol to communicate with GPS - Specifically UBX-NAV-PVT // Validate the message header, class, and id - if (((GPS->raw_message)[0] == 181) && (GPS->raw_message[1] == 98) && ((GPS->raw_message)[2] == 1) && ((GPS->raw_message)[3] == 7)) + if (((GPS->raw_message)[0] == UBX_NAV_PVT_HEADER_B0) && (GPS->raw_message[1] == UBX_NAV_PVT_HEADER_B1) && + ((GPS->raw_message)[2] == UBX_NAV_PVT_CLASS) && ((GPS->raw_message)[3] == UBX_NAV_PVT_MSG_ID)) { + bool correctFix = false; // Collect fix type GPS->fix_type = GPS->raw_message[26]; - if (GPS->fix_type > 2) + if (GPS->fix_type > GPS_FIX_2D && GPS->fix_type < GPS_FIX_TIME_ONLY) { + correctFix = true; // Collect Ground Speed GPS->g_speed_bytes[0] = GPS->raw_message[66]; GPS->g_speed_bytes[1] = GPS->raw_message[67]; @@ -145,12 +166,14 @@ bool parseVelocity(GPS_Handle_t *GPS, ExtU *rtU) SEND_GPS_POSITION(q_tx_can, 0, 0, 0, GPS->height_rounded); } - // // Collect Magnetic Declination - // GPS->mag_dec_bytes[0] = GPS->raw_message[94]; - // GPS->mag_dec_bytes[1] = GPS->raw_message[95]; - // iShort.bytes[0] = GPS->raw_message[94]; - // iShort.bytes[1] = GPS->raw_message[95]; - // GPS->mag_dec = iShort.iShort; + // Collect Magnetic Declination + GPS->mag_dec_bytes[0] = GPS->raw_message[94]; + GPS->mag_dec_bytes[1] = GPS->raw_message[95]; + iShort.bytes[0] = GPS->raw_message[94]; + iShort.bytes[1] = GPS->raw_message[95]; + GPS->mag_dec = iShort.iShort; + + return correctFix; } - return true; + return false; } \ No newline at end of file diff --git a/source/torque_vector/gps/gps.h b/source/torque_vector/gps/gps.h index 984c1cac..c000ee35 100644 --- a/source/torque_vector/gps/gps.h +++ b/source/torque_vector/gps/gps.h @@ -72,6 +72,27 @@ typedef struct uint8_t fix_type; } GPS_Handle_t; // GPS handle +// GPS Message Attributes +#define UBX_NAV_PVT_HEADER_B0 0xB5 +#define UBX_NAV_PVT_HEADER_B1 0x62 +#define UBX_NAV_PVT_CLASS 0x01 +#define UBX_NAV_PVT_MSG_ID 0x07 + +#define GPS_FIX_NONE 0 +#define GPS_FIX_DEAD_RECKONING 1 +#define GPS_FIX_2D 2 +#define GPS_FIX_3D 3 +#define GPS_FIX_GNSS_DEAD_RECKONING 4 +#define GPS_FIX_TIME_ONLY 5 + +/** + * @brief Function to Parse periodic GPS UBX message + * + * @param GPS Handle for GPS configuration + * @param rtU Handle for SFS + * @return true Parsing successful + * @return false Parsing failed + */ bool parseVelocity(GPS_Handle_t *GPS, ExtU *rtU); #endif //_GPS_H \ No newline at end of file diff --git a/source/torque_vector/imu/imu.c b/source/torque_vector/imu/imu.c index 9b2b0185..139078d6 100644 --- a/source/torque_vector/imu/imu.c +++ b/source/torque_vector/imu/imu.c @@ -55,10 +55,6 @@ void imu_periodic(IMU_Handle_t *imu_h, ExtU *rtU) r = (int16_t)(imu_h->output.orientation.roll * 10.0f / DEG_TO_RAD); y = (int16_t)(imu_h->output.orientation.yaw * 10.0f / DEG_TO_RAD); - // SEND_ANGLE_DATA(q_tx_can, p, r, y); - // SEND_ACCEL_DATA(q_tx_can, (int16_t) (accel_in.x * 100), (int16_t) (accel_in.y * 100), (int16_t) (accel_in.z * 100)); - // SEND_GYRO_DATA(q_tx_can, (int16_t) (gyro_in.x * 100), (int16_t) (gyro_in.y * 100), (int16_t) (gyro_in.z * 100)); - rtU->gyro[0] = CLAMP(gyro_in.x * ACC_CALIBRATION, MIN_GYRO, MAX_GYRO); rtU->gyro[1] = CLAMP(gyro_in.y * ACC_CALIBRATION, MIN_GYRO, MAX_GYRO); rtU->gyro[2] = CLAMP(gyro_in.z * ACC_CALIBRATION, MIN_GYRO, MAX_GYRO); diff --git a/source/torque_vector/main.c b/source/torque_vector/main.c index 0ceda2c5..9d784687 100644 --- a/source/torque_vector/main.c +++ b/source/torque_vector/main.c @@ -1,10 +1,9 @@ /* 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/phal_F4_F7/gpio/gpio.h" +#include "common/phal_F4_F7/rcc/rcc.h" +#include "common/phal_F4_F7/spi/spi.h" #include "common/psched/psched.h" -#include "common/phal_L4/usart/usart.h" +#include "common/phal_F4_F7/usart/usart.h" /* Module Includes */ #include "bmi088.h" @@ -13,7 +12,6 @@ #include "imu.h" #include "main.h" #include "gps.h" -#include "bmm150.h" #include "SFS.h" #include "sfs_pp.h" @@ -36,8 +34,11 @@ GPIOInitConfig_t gpio_config[] = { 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, + GPIO_INIT_UART4RX_PA1, + GPIO_INIT_UART4TX_PA0, + + // GPS Auxillary pins + GPIO_INIT_OUTPUT(GPS_RESET_GPIO_Port, GPS_RESET_Pin, GPIO_OUTPUT_LOW_SPEED), // EEPROM GPIO_INIT_OUTPUT(NAV_EEPROM_CS_GPIO_PORT, NAV_EEPROM_CS_PIN, GPIO_OUTPUT_HIGH_SPEED), @@ -45,30 +46,29 @@ GPIOInitConfig_t gpio_config[] = { // CAN GPIO_INIT_CANRX_PA11, - GPIO_INIT_CANTX_PA12}; + 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 = { +// GPS USART Configuration +dma_init_t usart_gps_tx_dma_config = USART4_TXDMA_CONT_CONFIG(NULL, 1); +dma_init_t usart_gps_rx_dma_config = USART4_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, + .periph = UART4, + .wake_addr = false, + .usart_active_num = USART4_ACTIVE_IDX, + .tx_errors = 0, + .rx_errors = 0, .tx_dma_cfg = &usart_gps_tx_dma_config, - .rx_dma_cfg = &usart_gps_rx_dma_config}; + .rx_dma_cfg = &usart_gps_rx_dma_config +}; #define TargetCoreClockrateHz 16000000 ClockRateConfig_t clock_config = { @@ -92,18 +92,14 @@ 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, + .nss_gpio_port = SPI_CS_ACEL_GPIO_Port, + .nss_gpio_pin = SPI_CS_ACEL_Pin, + .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 = {}; +GPS_Handle_t GPSHandle = {}; vector_3d_t accel_in, gyro_in, mag_in; BMI088_Handle_t bmi_config = { @@ -117,10 +113,7 @@ BMI088_Handle_t bmi_config = { .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, }; @@ -132,7 +125,6 @@ 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; @@ -146,8 +138,6 @@ 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)); @@ -164,31 +154,6 @@ int main(void) 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); @@ -200,8 +165,6 @@ int main(void) taskCreate(sendIMUData, 10); taskCreate(collectGPSData, 40); - taskCreate(collectMagData, 40); - taskCreate(SFS_MAIN, 10); /* No Way Home */ schedStart(); @@ -223,12 +186,30 @@ void preflightChecks(void) NVIC_EnableIRQ(CAN1_RX0_IRQn); break; case 2: - if (!BMM150_readID(&bmm_config)) + /* USART initialization */ + if (!PHAL_initUSART(&huart_gps, APB1ClockRateHz)) { - asm("nop"); + HardFault_Handler(); } - break; + break; + case 3: + // GPS Initialization + PHAL_writeGPIO(GPS_RESET_GPIO_Port, GPS_RESET_Pin, 1); + PHAL_usartRxDma(&huart_gps, (uint16_t *)GPSHandle.raw_message, 100, 1); + break; case 1: + /* 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); + break; + case 4: if (!BMI088_init(&bmi_config)) { HardFault_Handler(); @@ -288,9 +269,9 @@ 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); + 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) @@ -300,41 +281,14 @@ void sendIMUData(void) uint8_t poll_pvt[] = {"0xB5, 0x62, 0x01, 0x07, 0x00, 0x00, 0x08, 0x19"}; -// Test function for usartRxDma +// Periodic function to collect IMU data void collectGPSData(void) { - testGPSHandle.messages_received++; + GPSHandle.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); + GPSHandle.acceleration = accel_in; + GPSHandle.gyroscope = gyro_in; } void canTxUpdate(void) @@ -387,11 +341,10 @@ void CAN1_RX0_IRQHandler() } } -// void main_module_bl_cmd_CALLBACK(CanParsedData_t *msg_data_a) -// { -// if (can_data.main_module_bl_cmd.cmd == BLCMD_RST) -// Bootloader_ResetForFirmwareDownload(); -// } +void usart_recieve_complete_callback(usart_init_t *handle) +{ + parseVelocity(&GPSHandle, &rtU); +} void SFS_MAIN(void) { diff --git a/source/torque_vector/main.h b/source/torque_vector/main.h index 334cdb11..5b9c6cdd 100644 --- a/source/torque_vector/main.h +++ b/source/torque_vector/main.h @@ -44,6 +44,9 @@ #define GPS_TX_GPIO_Port (GPIOC) #define GPS_TX_Pin (4) +#define GPS_RESET_GPIO_Port (GPIOC) +#define GPS_RESET_Pin (9) + // EEPROM #define NAV_EEPROM_CS_GPIO_PORT (GPIOB) #define NAV_EEPROM_CS_PIN (12)