diff --git a/firmware/src/config.h b/firmware/src/config.h index 91644f40..e77f34c4 100644 --- a/firmware/src/config.h +++ b/firmware/src/config.h @@ -49,23 +49,3 @@ #define UART_ENUM UARTB #define UART_REF PAC55XX_UARTB #define UART_BAUD_RATE (115200) - -#define UART_I_SCALING_FACTOR ( 1000.0f ) -#define ONE_OVER_UART_I_SCALING_FACTOR ( 0.001f ) - -#define UART_R_SCALING_FACTOR ( 1000.0f ) -#define ONE_OVER_UART_R_SCALING_FACTOR ( 0.001f ) - -#define UART_L_SCALING_FACTOR ( 1000.0f ) -#define ONE_OVER_UART_L_SCALING_FACTOR ( 0.001f ) - -#define UART_VEL_GAIN_SCALING_FACTOR ( 1000000.0f ) -#define ONE_OVER_UART_VEL_GAIN_SCALING_FACTOR ( 0.000001f ) - -#define UART_VEL_INT_SCALING_FACTOR ( 1000.0f ) -#define ONE_OVER_UART_VEL_INT_SCALING_FACTOR ( 0.001f ) - -#define UART_IQ_LIMIT_SCALING_FACTOR ( 1000.f ) -#define ONE_OVER_UART_IQ_LIMIT_SCALING_FACTOR ( 0.001f ) - -#define UART_V_SCALING_FACTOR ( 1000.0f ) diff --git a/firmware/src/uart/uart_func.c b/firmware/src/uart/uart_func.c index 0be59193..28d851df 100644 --- a/firmware/src/uart/uart_func.c +++ b/firmware/src/uart/uart_func.c @@ -10,106 +10,13 @@ // ONLY BY CERTAIN AUTHORIZED PERSONS. // //============================================================================= - -#include "uart_func.h" - -//============================================================================== -/// @brief -/// UARTA_Tx <--> PA4 -/// UARTA_Rx <--> PA5 -/// -//============================================================================== -void UARTA_IO_Select_PA45(void) -{ - PAC55XX_GPIOA->MODE.P4 = IO_PUSH_PULL_OUTPUT; // TX - PAC55XX_GPIOA->MODE.P5 = IO_HIGH_IMPEDENCE_INPUT; // RX - - PAC55XX_SCC->PAPUEN.P4 = 0; - PAC55XX_SCC->PAPUEN.P5 = 1; - - PAC55XX_SCC->PAMUXSEL.w &= 0xFFF00FFF; // Clear Port Pin selection - PAC55XX_SCC->PAMUXSEL.w |= 0x00011000; // Set Port Pin as UART -} - -//============================================================================== -/// @brief -/// UARTB_Tx <--> PA4 -/// UARTB_Rx <--> PA5 -/// -//============================================================================== -void UARTB_IO_Select_PA45(void) -{ - PAC55XX_GPIOA->MODE.P4 = IO_PUSH_PULL_OUTPUT; // TX - PAC55XX_GPIOA->MODE.P5 = IO_HIGH_IMPEDENCE_INPUT; // RX - - PAC55XX_SCC->PAPUEN.P4 = 0; - PAC55XX_SCC->PAPUEN.P5 = 1; - - PAC55XX_SCC->PAMUXSEL.w &= 0xFFF00FFF; // Clear Port Pin selection - PAC55XX_SCC->PAMUXSEL.w |= 0x00022000; // Set Port Pin as UART -} - -//============================================================================== -/// @brief -/// UARTB_Tx <--> PC0 -/// UARTB_Rx <--> PC1 -/// -//============================================================================== -void UARTB_IO_Select_PC01(void) -{ - PAC55XX_GPIOC->MODE.P0 = IO_PUSH_PULL_OUTPUT; // TX - PAC55XX_GPIOC->MODE.P1 = IO_HIGH_IMPEDENCE_INPUT; // RX - - PAC55XX_SCC->PCPUEN.P0 = 0; - PAC55XX_SCC->PCPUEN.P1 = 1; - - PAC55XX_SCC->PCMUXSEL.w &= 0xFFFFFF00; // Clear Port Pin selection - PAC55XX_SCC->PCMUXSEL.w |= 0x00000044; // Set Port Pin as UART -} - -//============================================================================== -/// @brief -/// UARTB_Tx <--> PC4 -/// UARTB_Rx <--> PC5 -/// -//============================================================================== -void UARTB_IO_Select_PC45(void) -{ - PAC55XX_GPIOC->MODE.P4 = IO_PUSH_PULL_OUTPUT; // TX - PAC55XX_GPIOC->MODE.P5 = IO_HIGH_IMPEDENCE_INPUT; // RX - - PAC55XX_SCC->PCPUEN.P4 = 0; - PAC55XX_SCC->PCPUEN.P5 = 1; - PAC55XX_SCC->PCMUXSEL.w &= 0xFF00FFFF; // Clear Port Pin selection - PAC55XX_SCC->PCMUXSEL.w |= 0x00440000; // Set Port Pin as UART -} - -//============================================================================== -/// @brief -/// UARTB_Tx <--> PE6 -/// UARTB_Rx <--> PE7 -/// -//============================================================================== -void UARTB_IO_Select_PE67(void) -{ - PAC55XX_GPIOE->MODE.P6 = IO_PUSH_PULL_OUTPUT; // TX - PAC55XX_GPIOE->MODE.P7 = IO_HIGH_IMPEDENCE_INPUT; // RX - - PAC55XX_SCC->PEPUEN.P6 = 0; - PAC55XX_SCC->PEPUEN.P7 = 1; - - PAC55XX_SCC->PEMUXSEL.w &= 0x00FFFFFF; // Clear Port Pin selection - PAC55XX_SCC->PEMUXSEL.w |= 0x44000000; // Set Port Pin as UART -} +#include "uart_func.h" //============================================================================== -/// @brief -/// UARTB_Tx <--> PF2 -/// UARTB_Rx <--> PF3 -/// +/// @brief Configure UARTB on PF2 (TX) and PF3 (RX) //============================================================================== -void UARTB_IO_Select_PF23(void) +static void UARTB_IO_Select_PF23(void) { PAC55XX_GPIOF->MODE.P2 = IO_PUSH_PULL_OUTPUT; // TX PAC55XX_GPIOF->MODE.P3 = IO_HIGH_IMPEDENCE_INPUT; // RX @@ -122,551 +29,44 @@ void UARTB_IO_Select_PF23(void) } //============================================================================== -/// @brief -/// UARTC_Tx <--> PC2 -/// UARTC_Rx <--> PC3 -/// -//============================================================================== -void UARTC_IO_Select_PC23(void) -{ - PAC55XX_GPIOC->MODE.P2 = IO_PUSH_PULL_OUTPUT; // TX - PAC55XX_GPIOC->MODE.P3 = IO_HIGH_IMPEDENCE_INPUT; // RX - - PAC55XX_SCC->PCPUEN.P2 = 0; - PAC55XX_SCC->PCPUEN.P3 = 1; - - PAC55XX_SCC->PCMUXSEL.w &= 0xFFFF00FF; // Clear Port Pin selection - PAC55XX_SCC->PCMUXSEL.w |= 0x00005500; // Set Port Pin as UART -} - -//============================================================================== -/// @brief -/// UARTC_Tx <--> PC6 -/// UARTC_Rx <--> PC7 -/// -//============================================================================== -void UARTC_IO_Select_PC67(void) -{ - PAC55XX_GPIOC->MODE.P6 = IO_PUSH_PULL_OUTPUT; // TX - PAC55XX_GPIOC->MODE.P7 = IO_HIGH_IMPEDENCE_INPUT; // RX - - PAC55XX_SCC->PCPUEN.P6 = 0; - PAC55XX_SCC->PCPUEN.P7 = 1; - - PAC55XX_SCC->PCMUXSEL.w &= 0x00FFFFFF; // Clear Port Pin selection - PAC55XX_SCC->PCMUXSEL.w |= 0x55000000; // Set Port Pin as UART -} - -//============================================================================== -/// @brief -/// UARTC_Tx <--> PD2 -/// UARTC_Rx <--> PD3 -/// -//============================================================================== -void UARTC_IO_Select_PD23(void) -{ - PAC55XX_GPIOD->MODE.P2 = IO_PUSH_PULL_OUTPUT; // TX - PAC55XX_GPIOD->MODE.P3 = IO_HIGH_IMPEDENCE_INPUT; // RX - - PAC55XX_SCC->PDPUEN.P2 = 0; - PAC55XX_SCC->PDPUEN.P3 = 1; - - PAC55XX_SCC->PDMUXSEL.w &= 0xFFFF00FF; // Clear Port Pin selection - PAC55XX_SCC->PDMUXSEL.w |= 0x00005500; // Set Port Pin as UART -} - -//============================================================================== -/// @brief -/// UARTC_Tx <--> PE2 -/// UARTC_Rx <--> PE3 -/// -//============================================================================== -void UARTC_IO_Select_PE23(void) -{ - PAC55XX_GPIOE->MODE.P2 = IO_PUSH_PULL_OUTPUT; // TX - PAC55XX_GPIOE->MODE.P3 = IO_HIGH_IMPEDENCE_INPUT; // RX - - PAC55XX_SCC->PEPUEN.P2 = 0; - PAC55XX_SCC->PEPUEN.P3 = 1; - - PAC55XX_SCC->PEMUXSEL.w &= 0xFFFF00FF; // Clear Port Pin selection - PAC55XX_SCC->PEMUXSEL.w |= 0x00005500; // Set Port Pin as UART -} - -//============================================================================== -/// @brief -/// UARTD_Tx <--> PD4 -/// UARTD_Rx <--> PD5 -/// -//============================================================================== -void UARTD_IO_Select_PD45(void) -{ - PAC55XX_GPIOD->MODE.P4 = IO_PUSH_PULL_OUTPUT; // TX - PAC55XX_GPIOD->MODE.P5 = IO_HIGH_IMPEDENCE_INPUT; // RX - - PAC55XX_SCC->PDPUEN.P4 = 0; - PAC55XX_SCC->PDPUEN.P5 = 1; - - PAC55XX_SCC->PDMUXSEL.w &= 0xFF00FFFF; // Clear Port Pin selection - PAC55XX_SCC->PDMUXSEL.w |= 0x00770000; // Set Port Pin as UART -} - -//============================================================================== -/// @brief -/// UARTD_Tx <--> PD6 -/// UARTD_Rx <--> PD7 -/// -//============================================================================== -void UARTD_IO_Select_PD67(void) -{ - PAC55XX_GPIOD->MODE.P6 = IO_PUSH_PULL_OUTPUT; // TX - PAC55XX_GPIOD->MODE.P7 = IO_HIGH_IMPEDENCE_INPUT; // RX - - PAC55XX_SCC->PDPUEN.P6 = 0; - PAC55XX_SCC->PDPUEN.P7 = 1; - - PAC55XX_SCC->PDMUXSEL.w &= 0x00FFFFFF; // Clear Port Pin selection - PAC55XX_SCC->PDMUXSEL.w |= 0x55000000; // Set Port Pin as UART -} - -//============================================================================== -/// @brief -/// UARTD_Tx <--> PE4 -/// UARTD_Rx <--> PE5 -/// -//============================================================================== -void UARTD_IO_Select_PE45(void) -{ - PAC55XX_GPIOE->MODE.P4 = IO_PUSH_PULL_OUTPUT; // TX - PAC55XX_GPIOE->MODE.P5 = IO_HIGH_IMPEDENCE_INPUT; // RX - - PAC55XX_SCC->PEPUEN.P4 = 0; - PAC55XX_SCC->PEPUEN.P5 = 1; - - PAC55XX_SCC->PEMUXSEL.w &= 0xFF00FFFF; // Clear Port Pin selection - PAC55XX_SCC->PEMUXSEL.w |= 0x00550000; // Set Port Pin as UART -} - -//============================================================================== -/// @brief -/// UARTD_Tx <--> PF6 -/// UARTD_Rx <--> PF7 -/// -//============================================================================== -void UARTD_IO_Select_PF67(void) -{ - PAC55XX_GPIOF->MODE.P6 = IO_PUSH_PULL_OUTPUT; // TX - PAC55XX_GPIOF->MODE.P7 = IO_HIGH_IMPEDENCE_INPUT; // RX - - PAC55XX_SCC->PFPUEN.P6 = 0; - PAC55XX_SCC->PFPUEN.P7 = 1; - - PAC55XX_SCC->PFMUXSEL.w &= 0x00FFFFFF; // Clear Port Pin selection - PAC55XX_SCC->PFMUXSEL.w |= 0x55000000; // Set Port Pin as UART -} - -//============================================================================== -/// @brief -/// UARTD_Tx <--> PG2 -/// UARTD_Rx <--> PG3 -/// -//============================================================================== -void UARTD_IO_Select_PG23(void) -{ - PAC55XX_GPIOG->MODE.P2 = IO_PUSH_PULL_OUTPUT; // TX - PAC55XX_GPIOG->MODE.P3 = IO_HIGH_IMPEDENCE_INPUT; // RX - - PAC55XX_SCC->PGPUEN.P2 = 0; - PAC55XX_SCC->PGPUEN.P3 = 1; - - PAC55XX_SCC->PGMUXSEL.w &= 0xFFFF00FF; // Clear Port Pin selection - PAC55XX_SCC->PGMUXSEL.w |= 0x00005500; // Set Port Pin as UART -} - -//============================================================================== -/// @brief -/// UARTD_Tx <--> PG5 -/// UARTD_Rx <--> PG6 -/// -//============================================================================== -void UARTD_IO_Select_PG56(void) -{ - PAC55XX_GPIOG->MODE.P5 = IO_PUSH_PULL_OUTPUT; // TX - PAC55XX_GPIOG->MODE.P6 = IO_HIGH_IMPEDENCE_INPUT; // RX - - PAC55XX_SCC->PGPUEN.P5 = 0; - PAC55XX_SCC->PGPUEN.P6 = 1; - - PAC55XX_SCC->PGMUXSEL.w &= 0xF00FFFFF; // Clear Port Pin selection - PAC55XX_SCC->PGMUXSEL.w |= 0x05500000; // Set Port Pin as UART -} - -//============================================================================== -/// @brief -/// Choose the Uart and Enable the IO you want -/// -///@param[in] -/// UART Type: -/// UARTA -/// UARTB -/// UARTC -/// UARTD -/// +/// @brief Enable UARTB interrupt //============================================================================== -void uart_io_config(UART_TYPE uart) +static void uart_interrupt_enable(void) { - switch (uart) - { - case UARTA: - // Select UART A peripheral choose one! - UARTA_IO_Select_PA45(); - break; - - case UARTB: - // Select UART B peripheral choose one! -// UARTB_IO_Select_PA45(); -// UARTB_IO_Select_PC01(); -// UARTB_IO_Select_PC45(); -// UARTB_IO_Select_PE67(); - UARTB_IO_Select_PF23(); - break; - - case UARTC: - // Select UART C peripheral choose one! -// UARTC_IO_Select_PC23(); -// UARTC_IO_Select_PC67(); -// UARTC_IO_Select_PD23(); - UARTC_IO_Select_PE23(); - break; - - case UARTD: - // Select UART D peripheral choose one! - UARTD_IO_Select_PD45(); -// UARTD_IO_Select_PD67(); -// UARTD_IO_Select_PE45(); -// UARTD_IO_Select_PF67(); -// UARTD_IO_Select_PG23(); -// UARTD_IO_Select_PG56(); - break; - - default: - break; - } + NVIC_ClearPendingIRQ(USARTB_IRQn); + NVIC_SetPriority(USARTB_IRQn, 3); + NVIC_EnableIRQ(USARTB_IRQn); } //============================================================================== -/// @brief -/// enable the interrupt -/// -///@param[in] -/// UART Type: -/// UARTA -/// UARTB -/// UARTC -/// UARTD -/// -//============================================================================== -void uart_interrupt_enable(UART_TYPE uart) -{ - switch (uart) - { - case UARTA: - NVIC_ClearPendingIRQ(USARTA_IRQn); - NVIC_SetPriority(USARTA_IRQn, 3); - NVIC_EnableIRQ(USARTA_IRQn); - break; - - case UARTB: - NVIC_ClearPendingIRQ(USARTB_IRQn); - NVIC_SetPriority(USARTB_IRQn, 3); - NVIC_EnableIRQ(USARTB_IRQn); - break; - - case UARTC: - NVIC_ClearPendingIRQ(USARTC_IRQn); - NVIC_SetPriority(USARTC_IRQn, 3); - NVIC_EnableIRQ(USARTC_IRQn); - break; - - case UARTD: - NVIC_ClearPendingIRQ(USARTD_IRQn); - NVIC_SetPriority(USARTD_IRQn, 3); - NVIC_EnableIRQ(USARTD_IRQn); - break; - - default: - break; - } -} - -//============================================================================== -///@brief -/// the whole uart setting function is here -/// -///@param[in] -/// UART Type: -/// UARTA -/// UARTB -/// UARTC -/// UARTD -/// baudrate: The baudrate you want -/// +/// @brief Initialize UART peripheral +/// @param uart UART type (only UARTB is used, parameter kept for API compatibility) +/// @param baudrate Desired baud rate //============================================================================== void uart_init(UART_TYPE uart, uint32_t baudrate) { - PAC55XX_UART_TYPEDEF *uart_ptr; - - switch (uart) - { - case UARTA: - uart_ptr = PAC55XX_UARTA; - PAC55XX_SCC->CCSCTL.USAMODE = USART_MODE_UART; // UART mode - break; - - case UARTB: - uart_ptr = PAC55XX_UARTB; - PAC55XX_SCC->CCSCTL.USBMODE = USART_MODE_UART; // UART mode - break; + (void)uart; // Only UARTB is used + + PAC55XX_SCC->CCSCTL.USBMODE = USART_MODE_UART; // UART mode - case UARTC: - uart_ptr = PAC55XX_UARTC; - PAC55XX_SCC->CCSCTL.USCMODE = USART_MODE_UART; // UART mode - break; - - case UARTD: - uart_ptr = PAC55XX_UARTD; - PAC55XX_SCC->CCSCTL.USDMODE = USART_MODE_UART; // UART mode - break; - - default: - uart_ptr = PAC55XX_UARTD; - break; - } - - pac5xxx_uart_config_LCR2(uart_ptr, + pac5xxx_uart_config_LCR2(PAC55XX_UARTB, UARTLCR_WL_BPC_8, UART_STOP_BITS_1, UART_PEN_DISABLE, UART_PARITY_FORCE_STICK_1, UART_BRKCTL_DISABLE); - pac5xxx_uart_config_divisor_latch2(uart_ptr, DF_UART_PCLK/16/baudrate); // 115kbps = PCLK / (16///DLR) - - //rx FIFO set - uart_ptr->FCR.FIFOEN = 1; - uart_ptr->FCR.RXFIFORST = 1; - pac5xxx_uart_rx_fifo_threshold2(uart_ptr, UARTFCR_TL_8B); - pac5xxx_uart_tx_fifo_threshold2(uart_ptr, UARTFCR_TL_8B); - - pac5xxx_uart_int_enable_RDAI2(uart_ptr, UART_INT_ENABLE); - // pac5xxx_uart_int_enable_THREI2(uart_ptr, UART_INT_ENABLE); - - uart_io_config(uart); - uart_interrupt_enable(uart); -} - -//============================================================================== -/// @brief -/// read a byte from UART manually -/// -/// @param[in] -/// UART Type: -/// UARTA -/// UARTB -/// UARTC -/// UARTD -/// data: The data read back. -/// -/// @param[out] -/// result: the result of the UART byte read -/// -///@retval -/// 0 All is OK. -/// others Some error occurs. -/// -//============================================================================== -uint32_t uart_read_one(UART_TYPE uart, uint8_t *data) -{ - uint32_t result = PAC5XXX_OK; - uint32_t wait_tick = 0; - - PAC55XX_UART_TYPEDEF *uart_ptr; - - switch (uart) - { - case UARTA: - uart_ptr = PAC55XX_UARTA; - break; - - case UARTB: - uart_ptr = PAC55XX_UARTB; - break; - - case UARTC: - uart_ptr = PAC55XX_UARTC; - break; - - case UARTD: - uart_ptr = PAC55XX_UARTD; - break; - - default: - uart_ptr = PAC55XX_UARTA; - break; - } - - while (uart_ptr->LSR.RDR == 0) - { - wait_tick++; - if (wait_tick > DF_UART_BUSY_TICK) - { - result = PAC5XXX_ERROR; - break; - } - } - - *data = (uint8_t)uart_ptr->RBR.RBR; - - return result; -} - -//============================================================================== -///@brief -/// Write a byte to UART manually -/// -///@param[in] -/// UART Type: -/// UARTA -/// UARTB -/// UARTC -/// UARTD -/// data: The data to write. -/// -///@param[out] -/// result: the result of the UART byte write -/// -///@retval -/// 0 All is OK. -/// others Some error occurs. -/// -//============================================================================== -uint32_t uart_write_one(UART_TYPE uart, uint8_t data) -{ - uint32_t result = PAC5XXX_OK; - uint32_t wait_tick = 0; - - PAC55XX_UART_TYPEDEF *uart_ptr; - - switch (uart) - { - case UARTA: - uart_ptr = PAC55XX_UARTA; - break; - - case UARTB: - uart_ptr = PAC55XX_UARTB; - break; - - case UARTC: - uart_ptr = PAC55XX_UARTC; - break; - - case UARTD: - uart_ptr = PAC55XX_UARTD; - break; - - default: - uart_ptr = PAC55XX_UARTA; - break; - } - - uart_ptr->THR.THR = data; - - while (uart_ptr->LSR.TEMT == 0) - { - wait_tick++; - if (wait_tick > DF_UART_BUSY_TICK) - { - result = PAC5XXX_ERROR; - break; - } - } - - return result; -} - -//============================================================================== -///@brief -/// Write a serial bytes to UART manually -/// -///@param[in] -/// UART Type: -/// UARTA -/// UARTB -/// UARTC -/// UARTD -/// data_p: The data pointer to write. -/// byte_num: the number to write. -/// -///@param[out] -/// result: the result of the UART byte write -/// -///@retval -/// 0 All is OK. -/// others Some error occurs. -/// -//============================================================================== -uint32_t uart_write_multi(UART_TYPE uart, uint8_t *data_p, uint32_t byte_num) -{ - uint8_t *data; - data = data_p; - - uint32_t result = PAC5XXX_OK; - uint32_t wait_tick = 0; - uint32_t byte_num_temp; - - PAC55XX_UART_TYPEDEF *uart_ptr; - - switch (uart) - { - case UARTA: - uart_ptr = PAC55XX_UARTA; - break; - - case UARTB: - uart_ptr = PAC55XX_UARTB; - break; - - case UARTC: - uart_ptr = PAC55XX_UARTC; - break; - - case UARTD: - uart_ptr = PAC55XX_UARTD; - break; - - default: - uart_ptr = PAC55XX_UARTA; - break; - } - - for (byte_num_temp=0; byte_num_tempTHR.THR = *data; - data++; + pac5xxx_uart_config_divisor_latch2(PAC55XX_UARTB, DF_UART_PCLK / 16 / baudrate); - while (uart_ptr->LSR.TEMT == 0) - { - wait_tick++; - if (wait_tick > DF_UART_BUSY_TICK) - { - result = PAC5XXX_ERROR; - break; - } - } + // RX FIFO setup + PAC55XX_UARTB->FCR.FIFOEN = 1; + PAC55XX_UARTB->FCR.RXFIFORST = 1; + pac5xxx_uart_rx_fifo_threshold2(PAC55XX_UARTB, UARTFCR_TL_8B); + pac5xxx_uart_tx_fifo_threshold2(PAC55XX_UARTB, UARTFCR_TL_8B); - wait_tick = 0; - } + // Enable receive data available interrupt + pac5xxx_uart_int_enable_RDAI2(PAC55XX_UARTB, UART_INT_ENABLE); - return result; + UARTB_IO_Select_PF23(); + uart_interrupt_enable(); } diff --git a/firmware/src/uart/uart_func.h b/firmware/src/uart/uart_func.h index 455c857a..4a180f7d 100644 --- a/firmware/src/uart/uart_func.h +++ b/firmware/src/uart/uart_func.h @@ -10,23 +10,19 @@ // ONLY BY CERTAIN AUTHORIZED PERSONS. // //============================================================================= - -#include "src/common.h" #ifndef UART_FUNC_H #define UART_FUNC_H -#define DF_isr_for_UART -#define DF_UART_PCLK HCLK_FREQ_HZ -#define DF_UART_BUSY_TICK (25000u) +#include "src/common.h" -volatile uint8_t uart_read_buf[48]; -volatile uint8_t uart_write_buf[48]; -volatile uint32_t uart_read_count; +#define DF_UART_PCLK HCLK_FREQ_HZ -extern void uart_init(UART_TYPE uart, uint32_t baudrate); -extern uint32_t uart_read_one(UART_TYPE uart, uint8_t *data); -extern uint32_t uart_write_one(UART_TYPE uart, uint8_t data); -extern uint32_t uart_write_multi(UART_TYPE uart, uint8_t *data, uint32_t byte_num); +/** + * @brief Initialize UART peripheral + * @param uart UART type (only UARTB is supported) + * @param baudrate Desired baud rate + */ +void uart_init(UART_TYPE uart, uint32_t baudrate); #endif diff --git a/firmware/src/uart/uart_interface.c b/firmware/src/uart/uart_interface.c index 5166208b..8b88c4ca 100644 --- a/firmware/src/uart/uart_interface.c +++ b/firmware/src/uart/uart_interface.c @@ -1,4 +1,3 @@ - // * This file is part of the Tinymovr-Firmware distribution // * (https://github.com/yconst/tinymovr-firmware). // * Copyright (c) 2020-2023 Ioannis Chatzikonstantinou. @@ -15,239 +14,132 @@ // * You should have received a copy of the GNU General Public License // * along with this program. If not, see . -#include "string.h" -#include "src/system/system.h" -#include "src/motor/motor.h" -#include "src/observer/observer.h" -#include "src/controller/controller.h" -#include "src/controller/trajectory_planner.h" -#include "src/adc/adc.h" -#include "src/nvm/nvm.h" -#include "src/can/can.h" -#include "src/utils/utils.h" -#include "src/uart/uart_lowlevel.h" -#include "src/uart/uart_interface.h" - -void UART_WriteAddr(uint8_t addr, int32_t data) +#include +#include +#include +#include +#include + +// Number of endpoints in the Avlos endpoint array +static const size_t endpoint_count = sizeof(avlos_endpoints) / sizeof(avlos_endpoints[0]); + +/** + * @brief Calculate CRC-16-CCITT using hardware CRC peripheral + * @param data Pointer to data buffer + * @param len Length of data in bytes + * @return 16-bit CRC value + */ +static uint16_t UART_calculate_crc16(const uint8_t *data, uint8_t len) { - switch (addr) + // Configure CRC peripheral for CRC-16-CCITT + PAC55XX_CRC->CTL.POLYSEL = CRC16_CCITT; + PAC55XX_CRC->CTL.DATAWIDTH = CRC_DATA_WIDTH_8BITS; + PAC55XX_CRC->CTL.INREF = 0; + PAC55XX_CRC->CTL.OUTREF = 0; + + // Set seed value + PAC55XX_CRC->SEED.CRCSEED = 0xFFFF; + + // Feed data bytes + for (uint8_t i = 0; i < len; i++) { - case 'P': // pos setpoint - controller_set_Iq_setpoint_user_frame(0); - controller_set_vel_setpoint_user_frame(0); - controller_set_pos_setpoint_user_frame(data); - controller_set_mode(CONTROLLER_MODE_POSITION); - break; - - case 'V': // vel setpoint - controller_set_Iq_setpoint_user_frame(0); - controller_set_vel_setpoint_user_frame(data); - controller_set_mode(CONTROLLER_MODE_VELOCITY); - controller_set_vel_setpoint_user_frame((float)data); - break; - - case 'I': // current setpoint - controller_set_mode(CONTROLLER_MODE_CURRENT); - controller_set_Iq_setpoint_user_frame((float)data * ONE_OVER_UART_I_SCALING_FACTOR); - break; - - case 'G': // velocity integrator gain - controller_set_vel_integral_gain((float)data * ONE_OVER_UART_VEL_INT_SCALING_FACTOR); - break; - - case 'Y': // Position gain - controller_set_pos_gain(data); - break; - - case 'F': // Velocity gain - controller_set_vel_gain(data * ONE_OVER_UART_VEL_GAIN_SCALING_FACTOR); - break; - - case 'H': // phase resistance - motor_set_phase_resistance((float)data * ONE_OVER_UART_R_SCALING_FACTOR); - break; - - case 'L': // phase inductance - motor_set_phase_inductance((float)data * ONE_OVER_UART_L_SCALING_FACTOR); - break; - - case 'M': // Set is motor gimbal? - motor_set_is_gimbal((bool)data); - break; - - case 'W': // Set Iq Limit - controller_set_Iq_limit((float)data * ONE_OVER_UART_IQ_LIMIT_SCALING_FACTOR); - break; - - case 'U': // CAN Baud Rate - CAN_set_kbit_rate((uint16_t)data); - break; - - case 'C': // CAN ID - CAN_set_ID((uint8_t)data); - break; - - case '<': // Max Decel - planner_set_max_decel((float)data); - break; - - case '>': // Max Accel - planner_set_max_accel((float)data); - break; - - case '^': // Max Vel - planner_set_max_vel((float)data); - break; - - case 'T': // Plan trajectory - planner_move_to_vlimit((float)data); - break; - - default: - // No action - break; + PAC55XX_CRC->DATAIN = data[i]; } + + // Return computed CRC + return (uint16_t)PAC55XX_CRC->OUT.CRCOUT; } -int32_t UART_ReadAddr(uint8_t addr) +/** + * @brief Send a binary response frame over UART + * @param data Pointer to response data + * @param len Length of response data in bytes + */ +static void UART_send_response(const uint8_t *data, uint8_t len) { - int32_t ret_val = 0; - switch (addr) - { - case 'b': // vbus value - ret_val = (int32_t)(system_get_Vbus() * UART_V_SCALING_FACTOR); - break; - - case 'e': // controller error - { - // pass - } - break; - - case 'p': // pos estimate - ret_val = user_frame_get_pos_estimate(); - break; - - case 'P': // pos setpoint - ret_val = controller_get_pos_setpoint_user_frame(); - break; - - case 'v': // vel estimate - ret_val = (int32_t)user_frame_get_vel_estimate(); - break; - - case 'V': // vel setpoint - ret_val = (int32_t)controller_get_vel_setpoint_user_frame(); - break; - - case 'i': // current estimate - ret_val = (int32_t)(controller_get_Iq_estimate_user_frame() * UART_I_SCALING_FACTOR); - break; - - case 'I': // current setpoint - ret_val = (int32_t)(controller_get_Iq_setpoint_user_frame() * UART_I_SCALING_FACTOR); - break; - - case 'G': // velocity integrator setpoint - ret_val = (int32_t)(controller_get_vel_integral_gain() * UART_VEL_INT_SCALING_FACTOR); - break; - - case 'H': // phase resistance - ret_val = (int32_t)(motor_get_phase_resistance() * UART_R_SCALING_FACTOR); - break; - - case 'L': // phase inductance - ret_val = (int32_t)(motor_get_phase_inductance() * UART_L_SCALING_FACTOR); - break; - - case 'W': // Get Iq Limit - ret_val = (int32_t)(controller_get_Iq_limit() * UART_IQ_LIMIT_SCALING_FACTOR); - break; - - case 'C': // CAN ID - ret_val = CAN_get_ID(); - break; - - case 'M': // Is motor gimbal? - ret_val = motor_get_is_gimbal(); - break; - - case 'Y': // - ret_val = controller_get_pos_gain(); - break; - - case 'F': // - ret_val = controller_get_vel_gain() * UART_VEL_GAIN_SCALING_FACTOR; - break; - - case 'Q': // calibrate - controller_set_state(CONTROLLER_STATE_CALIBRATE); - break; - - case 'A': // closed loop - controller_set_state(CONTROLLER_STATE_CL_CONTROL); - break; - - case 'Z': // idle - controller_set_state(CONTROLLER_STATE_IDLE); - break; - - case 'R': // reset mcu - system_reset(); - break; - - case 'S': // save config - nvm_save_config(); - break; - - case 'X': // erase config - nvm_erase(); - break; - - default: - // No action - break; - } - return ret_val; + // Build response frame: [STX][LEN][DATA...][CRC16_LO][CRC16_HI] + uart_tx_msg[0] = UART_BINARY_START_BYTE; + uart_tx_msg[1] = len; + memcpy(&uart_tx_msg[2], data, len); + + // Calculate CRC over [LEN][DATA...] + uint16_t crc = UART_calculate_crc16(&uart_tx_msg[1], len + 1); + uart_tx_msg[2 + len] = crc & 0xFF; + uart_tx_msg[3 + len] = (crc >> 8) & 0xFF; + + // Set total frame length: STX + LEN + data + CRC16 + uart_tx_msg_len = 4 + len; + + // Enable transmit interrupt to start sending + pac5xxx_uart_int_enable_THREI2(UART_REF, 1); } +/** + * @brief Process a received UART binary frame + * + * Frame format: [STX][LEN][EP_ID][CMD][DATA...][CRC16_LO][CRC16_HI] + * + * LEN = number of payload bytes (EP_ID + CMD + DATA) + * CRC16 covers: [LEN][EP_ID][CMD][DATA...] + */ void UART_process_message(void) { - int8_t addr = uart_rx_msg[1]; - int8_t len = ((int8_t)uart_rx_msg_len) - 3; - - // Ensure buffer is null-terminated - uart_rx_msg[uart_rx_msg_len] = '\0'; - - if (len > 0) + // Minimum frame: STX + LEN + EP_ID + CMD + CRC16 = 6 bytes + if (uart_rx_msg_len < 6) { - // Write operation - int32_t n = atol(&(uart_rx_msg)[2]); - UART_WriteAddr(addr, n); + return; } - else if (len == 0) + + // Extract frame fields + uint8_t len = uart_rx_msg[1]; // Payload length + uint8_t ep_id = uart_rx_msg[2]; // Endpoint ID + uint8_t cmd = uart_rx_msg[3]; // Command (READ=1, WRITE=0) + + // Verify CRC16 over [LEN][EP_ID][CMD][DATA...] + // CRC is at position: 2 + len (after STX + LEN + payload) + uint16_t rx_crc = uart_rx_msg[2 + len] | ((uint16_t)uart_rx_msg[3 + len] << 8); + uint16_t calc_crc = UART_calculate_crc16(&uart_rx_msg[1], (uint8_t)(len + 1)); + + if (rx_crc != calc_crc) { - // Read operation - int32_t val = UART_ReadAddr(uart_rx_msg[1]); - UART_SendInt32(val); + // CRC mismatch - discard frame silently + return; } - else + + // Validate endpoint ID + if (ep_id >= endpoint_count) { - // Error + return; + } + + // Validate payload length + if (len < 2 || len > UART_MAX_PAYLOAD) + { + return; } -} -void UART_SendInt32(int32_t val) -{ - (void)itoa(val, uart_tx_msg, 10); - for (uint8_t i = 0; i < UART_BYTE_LIMIT; i++) + // Prepare data buffer for endpoint callback + uint8_t buffer[8] = {0}; + uint8_t data_len = len - 2; // Subtract EP_ID and CMD from payload length + + if (data_len > sizeof(buffer)) { - if (uart_tx_msg[i] == '\0') - { - uart_tx_msg[i] = UART_LINEFEED; - break; - } + return; + } + + if (data_len > 0) + { + memcpy(buffer, &uart_rx_msg[4], data_len); + } + + // Call the Avlos endpoint + uint8_t response_len = 0; + uint8_t (*callback)(uint8_t *, uint8_t *, Avlos_Command) = avlos_endpoints[ep_id]; + uint8_t ret = callback(buffer, &response_len, (Avlos_Command)cmd); + + // Send response if this was a read or a function call with return value + if ((ret == AVLOS_RET_READ || ret == AVLOS_RET_CALL) && response_len > 0) + { + UART_send_response(buffer, response_len); } - // Enable transmit interrupt to send reponse to host - pac5xxx_uart_int_enable_THREI2(UART_REF, 1); } diff --git a/firmware/src/uart/uart_interface.h b/firmware/src/uart/uart_interface.h index b1b3d920..1a6e7fe0 100644 --- a/firmware/src/uart/uart_interface.h +++ b/firmware/src/uart/uart_interface.h @@ -1,27 +1,33 @@ - // * This file is part of the Tinymovr-Firmware distribution // * (https://github.com/yconst/tinymovr-firmware). // * Copyright (c) 2020-2023 Ioannis Chatzikonstantinou. -// * -// * This program is free software: you can redistribute it and/or modify -// * it under the terms of the GNU General Public License as published by +// * +// * This program is free software: you can redistribute it and/or modify +// * it under the terms of the GNU General Public License as published by // * the Free Software Foundation, version 3. // * -// * This program is distributed in the hope that it will be useful, but -// * WITHOUT ANY WARRANTY; without even the implied warranty of -// * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// * This program is distributed in the hope that it will be useful, but +// * WITHOUT ANY WARRANTY; without even the implied warranty of +// * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU // * General Public License for more details. // * -// * You should have received a copy of the GNU General Public License +// * You should have received a copy of the GNU General Public License // * along with this program. If not, see . - #ifndef UART_UART_INTERFACE_H_ #define UART_UART_INTERFACE_H_ -#include "src/common.h" - +/** + * @brief Process a received UART binary frame using Avlos endpoints + * + * Frame format: [STX][LEN][EP_ID][CMD][DATA...][CRC16_LO][CRC16_HI] + * - STX: Start byte (0x02) + * - LEN: Payload length (EP_ID + CMD + DATA) + * - EP_ID: Avlos endpoint index + * - CMD: 0=WRITE, 1=READ + * - DATA: 0-8 bytes of payload + * - CRC16: CRC-16-CCITT over LEN through DATA (little-endian) + */ void UART_process_message(void); -void UART_SendInt32(int32_t val); #endif /* UART_UART_INTERFACE_H_ */ diff --git a/firmware/src/uart/uart_lowlevel.c b/firmware/src/uart/uart_lowlevel.c index 6179dd5f..24006f1d 100644 --- a/firmware/src/uart/uart_lowlevel.c +++ b/firmware/src/uart/uart_lowlevel.c @@ -1,4 +1,3 @@ - // * This file is part of the Tinymovr-Firmware distribution // * (https://github.com/yconst/tinymovr-firmware). // * Copyright (c) 2020-2023 Ioannis Chatzikonstantinou. @@ -20,29 +19,33 @@ #include "src/uart/uart_func.h" #include -char uart_rx_buf[96] = {0}; -uint8_t uart_rx_byte_idx = 0; +// Receive buffers and state +static uint8_t uart_rx_buf[UART_BYTE_LIMIT] = {0}; +static uint8_t uart_rx_byte_idx = 0; +static uint8_t uart_rx_expected_len = 0; -typedef enum { - MSG_TYPE_UNKNOWN = 0, - MSG_TYPE_ASCII = 1, - MSG_TYPE_BINARY = 2 -} SerialMessageType; +// Exported receive message buffer +uint8_t uart_rx_msg[UART_BYTE_LIMIT]; +uint8_t uart_rx_msg_len = 0; -SerialMessageType rx_msg_type = MSG_TYPE_UNKNOWN; +// Exported transmit buffer +uint8_t uart_tx_msg[UART_BYTE_LIMIT]; +uint8_t uart_tx_msg_len = 0; +uint8_t uart_tx_byte_idx = 0; -void ResetRxQueue(void) +static void ResetRxQueue(void) { uart_rx_byte_idx = 0; - rx_msg_type = MSG_TYPE_UNKNOWN; + uart_rx_expected_len = 0; } -void ResetTxQueue(void) +static void ResetTxQueue(void) { uart_tx_byte_idx = 0; + uart_tx_msg_len = 0; } -void UART_Init() +void UART_Init(void) { uart_init(UART_ENUM, UART_BAUD_RATE); ResetRxQueue(); @@ -56,53 +59,70 @@ void USARTB_IRQHandler(void) if (int_type == UARTIIR_INTID_TX_HOLD_EMPTY) { + // Transmit path pac5xxx_uart_write2(UART_REF, uart_tx_msg[uart_tx_byte_idx]); uart_tx_byte_idx++; - // Terminate transmission upon newline or transmit overflow - if ((uart_tx_msg[uart_tx_byte_idx - 1u] == UART_LINEFEED) || - (uart_tx_byte_idx > UART_BYTE_LIMIT)) + // Terminate transmission when all bytes sent + if (uart_tx_byte_idx >= uart_tx_msg_len) { // Disable transmit interrupt pac5xxx_uart_int_enable_THREI2(UART_REF, UART_INT_DISABLE); - // Enable receive data interrupt for next incoming message - // pac5xxx_uart_int_enable_RDAI2(UART_REF, UART_INT_ENABLE); ResetTxQueue(); } } else - { - // Check first byte or return - if ((uart_rx_byte_idx == 0u) && (data == UART_ASCII_PROT_START_BYTE)) + { + // Receive path - binary frame format: + // [STX][LEN][EP_ID][CMD][DATA...][CRC16_LO][CRC16_HI] + + if (uart_rx_byte_idx == 0) { - rx_msg_type = MSG_TYPE_ASCII; + // Waiting for start byte + if (data == UART_BINARY_START_BYTE) + { + uart_rx_buf[0] = data; + uart_rx_byte_idx = 1; + } + // Ignore any other bytes when not in a frame } - - if (rx_msg_type != MSG_TYPE_UNKNOWN) + else if (uart_rx_byte_idx == 1) { - uart_rx_buf[uart_rx_byte_idx] = data; - if ((rx_msg_type == MSG_TYPE_ASCII) && - (uart_rx_buf[uart_rx_byte_idx] == UART_LINEFEED)) + // Length byte received + uart_rx_buf[1] = data; + // Total frame length: STX(1) + LEN(1) + payload(data) + CRC16(2) + uart_rx_expected_len = 2 + data + 2; + + // Sanity check: ensure frame fits in buffer + if (uart_rx_expected_len > UART_BYTE_LIMIT) { - uart_rx_msg_len = uart_rx_byte_idx + 1u; - memcpy(&uart_rx_msg, &uart_rx_buf, uart_rx_msg_len); ResetRxQueue(); - UART_ReceiveMessageHandler(); - // Disable receive data interrupt - //pac5xxx_uart_int_enable_RDAI2(UART_REF, UART_INT_DISABLE); - // Reset RX FIFO, to clear RDAI interrupt - pac5xxx_uart_rx_fifo_reset2(UART_REF); - } - else if (uart_rx_byte_idx >= UART_BYTE_LIMIT) + else { - ResetRxQueue(); + uart_rx_byte_idx = 2; } - else + } + else if (uart_rx_byte_idx < uart_rx_expected_len) + { + // Receiving payload and CRC bytes + uart_rx_buf[uart_rx_byte_idx++] = data; + + if (uart_rx_byte_idx == uart_rx_expected_len) { - uart_rx_byte_idx++; + // Frame complete - copy to message buffer and signal handler + uart_rx_msg_len = uart_rx_byte_idx; + memcpy(uart_rx_msg, uart_rx_buf, uart_rx_msg_len); + ResetRxQueue(); + UART_ReceiveMessageHandler(); + // Reset RX FIFO to clear any pending data + pac5xxx_uart_rx_fifo_reset2(UART_REF); } - + } + else + { + // Overflow protection - should not reach here + ResetRxQueue(); } } } diff --git a/firmware/src/uart/uart_lowlevel.h b/firmware/src/uart/uart_lowlevel.h index 9b9552ca..a9fa5b30 100644 --- a/firmware/src/uart/uart_lowlevel.h +++ b/firmware/src/uart/uart_lowlevel.h @@ -1,4 +1,3 @@ - // * This file is part of the Tinymovr-Firmware distribution // * (https://github.com/yconst/tinymovr-firmware). // * Copyright (c) 2020-2023 Ioannis Chatzikonstantinou. @@ -18,22 +17,26 @@ #ifndef UART_UART_LOWLEVEL_H_ #define UART_UART_LOWLEVEL_H_ -#define UART_LINEFEED 0x0A -#define UART_CRETURN 0x0D -#define UART_ASCII_PROT_START_BYTE 0x2E +// Binary protocol frame markers +#define UART_BINARY_START_BYTE 0x02 // STX + +// Buffer limits #define UART_BYTE_LIMIT 32 +#define UART_MAX_PAYLOAD 10 // endpoint_id + cmd + 8 data bytes -char uart_rx_msg[96]; -uint8_t uart_rx_msg_len; +// Receive buffer and state +extern uint8_t uart_rx_msg[UART_BYTE_LIMIT]; +extern uint8_t uart_rx_msg_len; -char uart_tx_msg[96]; -uint8_t uart_tx_byte_idx; +// Transmit buffer and state +extern uint8_t uart_tx_msg[UART_BYTE_LIMIT]; +extern uint8_t uart_tx_msg_len; +extern uint8_t uart_tx_byte_idx; void UART_Init(void); // The following message handler is called when a UART -// message completes being received i.e. with a newline -// character +// binary frame is completely received extern void UART_ReceiveMessageHandler(void); #endif /* UART_UART_LOWLEVEL_H_ */