aleks/hoverboard-sideboard-hack-STM
1
0
Fork 0
mirror of https://github.com/EFeru/hoverboard-sideboard-hack-STM.git synced 2025-08-17 00:56:10 +00:00
Code Issues Packages Projects Releases Wiki Activity

Added Hovercar variant

Browse Source 
- hovercar variant
- iBUS receiver support
- updated baud rate to 115200
This commit is contained in:
EmanuelFeru
2020-12-07 20:30:38 +01:00
parent 786baa4c06
commit 323e30b5bb
95 changed files with 35305 additions and 22056 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
Src/dma.c
View File

@@ -1,8 +1,8 @@
/**
******************************************************************************
* File Name : dma.c
* Description : This file provides code for the configuration
* of all the requested memory to memory DMA transfers.
* @file dma.c
* @brief This file provides code for the configuration
* of all the requested memory to memory DMA transfers.
******************************************************************************
* @attention
*
@@ -16,6 +16,7 @@
*
******************************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include "dma.h"
@@ -31,15 +32,22 @@
/* USER CODE END 1 */
/**
/**
* Enable DMA controller clock
*/
void MX_DMA_Init(void)
void MX_DMA_Init(void)
{
/* DMA controller clock enable */
__HAL_RCC_DMA1_CLK_ENABLE();
/* DMA interrupt init */
/* DMA1_Channel4_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Channel4_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(DMA1_Channel4_IRQn);
/* DMA1_Channel5_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Channel5_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(DMA1_Channel5_IRQn);
/* DMA1_Channel6_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Channel6_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(DMA1_Channel6_IRQn);
@@ -53,12 +61,4 @@ void MX_DMA_Init(void)
/* USER CODE END 2 */
/**
* @}
*/
/**
* @}
*/
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

13
Src/gpio.c
View File

@@ -1,8 +1,8 @@
/**
******************************************************************************
* File Name : gpio.c
* Description : This file provides code for the configuration
* of all used GPIO pins.
* @file gpio.c
* @brief This file provides code for the configuration
* of all used GPIO pins.
******************************************************************************
* @attention
*
@@ -19,6 +19,7 @@
/* Includes ------------------------------------------------------------------*/
#include "gpio.h"
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
@@ -30,9 +31,9 @@
/* USER CODE END 1 */
/** Configure pins as
* Analog
* Input
/** Configure pins as
* Analog
* Input
* Output
* EVENT_OUT
* EXTI

24
Src/i2c.c
View File

@@ -1,8 +1,8 @@
/**
******************************************************************************
* File Name : I2C.c
* Description : This file provides code for the configuration
* of the I2C instances.
* @file i2c.c
* @brief This file provides code for the configuration
* of the I2C instances.
******************************************************************************
* @attention
*
@@ -55,11 +55,11 @@ void HAL_I2C_MspInit(I2C_HandleTypeDef* i2cHandle)
/* USER CODE BEGIN I2C1_MspInit 0 */
/* USER CODE END I2C1_MspInit 0 */
__HAL_RCC_GPIOB_CLK_ENABLE();
/**I2C1 GPIO Configuration
/**I2C1 GPIO Configuration
PB6 ------> I2C1_SCL
PB7 ------> I2C1_SDA
PB7 ------> I2C1_SDA
*/
GPIO_InitStruct.Pin = MPU_SCL_Pin|MPU_SDA_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
@@ -90,12 +90,14 @@ void HAL_I2C_MspDeInit(I2C_HandleTypeDef* i2cHandle)
/* USER CODE END I2C1_MspDeInit 0 */
/* Peripheral clock disable */
__HAL_RCC_I2C1_CLK_DISABLE();
/**I2C1 GPIO Configuration
/**I2C1 GPIO Configuration
PB6 ------> I2C1_SCL
PB7 ------> I2C1_SDA
PB7 ------> I2C1_SDA
*/
HAL_GPIO_DeInit(GPIOB, MPU_SCL_Pin|MPU_SDA_Pin);
HAL_GPIO_DeInit(MPU_SCL_GPIO_Port, MPU_SCL_Pin);
HAL_GPIO_DeInit(MPU_SDA_GPIO_Port, MPU_SDA_Pin);
/* I2C1 interrupt Deinit */
HAL_NVIC_DisableIRQ(I2C1_EV_IRQn);
@@ -104,7 +106,7 @@ void HAL_I2C_MspDeInit(I2C_HandleTypeDef* i2cHandle)
/* USER CODE END I2C1_MspDeInit 1 */
}
}
}
/* USER CODE BEGIN 1 */

127
Src/main.c
View File

@@ -18,7 +18,6 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "dma.h"
@@ -66,25 +65,9 @@ void SystemClock_Config(void);
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
extern UART_HandleTypeDef huart2;
#ifdef SERIAL_CONTROL
extern SerialSideboard Sideboard;
#endif
uint32_t main_loop_counter; // main loop counter to perform task scheduling inside main()
#ifdef SERIAL_FEEDBACK
extern SerialFeedback Feedback;
extern uint16_t timeoutCntSerial; // Timeout counter for Rx Serial command
extern uint8_t timeoutFlagSerial; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data)
#endif
extern MPU_Data mpu; // holds the MPU-6050 data
extern ErrorStatus mpuStatus; // holds the MPU-6050 status: SUCCESS or ERROR
GPIO_PinState sensor1, sensor2; // holds the sensor1 and sensor 2 values
GPIO_PinState sensor1_read, sensor2_read; // holds the instantaneous Read for sensor1 and sensor 2
static uint32_t main_loop_counter; // main loop counter to perform task squeduling inside main()
/* USER CODE END 0 */
/**
@@ -118,108 +101,25 @@ int main(void)
MX_DMA_Init();
MX_USART2_UART_Init();
MX_I2C1_Init();
MX_USART1_UART_Init();
/* USER CODE BEGIN 2 */
input_init(); // Input initialization
input_init(); // Input initialization
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
{
HAL_Delay(DELAY_IN_MAIN_LOOP);
// ==================================== LEDs Handling ====================================
// HAL_GPIO_TogglePin(LED4_GPIO_Port, LED4_Pin); // Toggle BLUE1 LED
#ifdef SERIAL_FEEDBACK
if (!timeoutFlagSerial) {
if (Feedback.cmdLed & LED1_SET) { HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_SET); } else { HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_RESET); }
if (Feedback.cmdLed & LED2_SET) { HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_SET); } else { HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_RESET); }
if (Feedback.cmdLed & LED3_SET) { HAL_GPIO_WritePin(LED3_GPIO_Port, LED3_Pin, GPIO_PIN_SET); } else { HAL_GPIO_WritePin(LED3_GPIO_Port, LED3_Pin, GPIO_PIN_RESET); }
if (Feedback.cmdLed & LED4_SET) { HAL_GPIO_WritePin(LED4_GPIO_Port, LED4_Pin, GPIO_PIN_SET); } else { HAL_GPIO_WritePin(LED4_GPIO_Port, LED4_Pin, GPIO_PIN_RESET); }
if (Feedback.cmdLed & LED5_SET) { HAL_GPIO_WritePin(LED5_GPIO_Port, LED5_Pin, GPIO_PIN_SET); } else { HAL_GPIO_WritePin(LED5_GPIO_Port, LED5_Pin, GPIO_PIN_RESET); }
}
#endif
// ==================================== MPU-6050 Handling ====================================
#if defined(MPU_SENSOR_ENABLE) && defined(SERIAL_DEBUG)
// Get MPU data. Because the MPU-6050 interrupt pin is not wired we have to check DMP data by pooling periodically
if (SUCCESS == mpuStatus) {
mpu_get_data();
} else if (ERROR == mpuStatus && main_loop_counter % 100 == 0) {
HAL_GPIO_TogglePin(LED1_GPIO_Port, LED1_Pin); // Toggle the Red LED every 100 ms
}
// Print MPU data to Console
if (main_loop_counter % 50 == 0) {
mpu_print_to_console();
}
#endif
// ==================================== SENSORS Handling ====================================
sensor1_read = HAL_GPIO_ReadPin(SENSOR1_GPIO_Port, SENSOR1_Pin);
sensor2_read = HAL_GPIO_ReadPin(SENSOR2_GPIO_Port, SENSOR2_Pin);
// SENSOR1
if (sensor1 == GPIO_PIN_RESET && sensor1_read == GPIO_PIN_SET) {
sensor1 = GPIO_PIN_SET;
// Sensor ACTIVE: Do something here (one time task on activation)
HAL_GPIO_WritePin(LED4_GPIO_Port, LED4_Pin, GPIO_PIN_SET);
consoleLog("-- SENSOR 1 Active --\n");
} else if(sensor1 == GPIO_PIN_SET && sensor1_read == GPIO_PIN_RESET) {
// Sensor DEACTIVE: Do something here (one time task on deactivation)
sensor1 = GPIO_PIN_RESET;
HAL_GPIO_WritePin(LED4_GPIO_Port, LED4_Pin, GPIO_PIN_RESET);
consoleLog("-- SENSOR 1 Deactive --\n");
}
// SENSOR2
if (sensor2 == GPIO_PIN_RESET && sensor2_read == GPIO_PIN_SET) {
sensor2 = GPIO_PIN_SET;
// Sensor ACTIVE: Do something here (one time task on activation)
HAL_GPIO_WritePin(LED5_GPIO_Port, LED5_Pin, GPIO_PIN_SET);
consoleLog("-- SENSOR 2 Active --\n");
} else if (sensor2 == GPIO_PIN_SET && sensor2_read == GPIO_PIN_RESET) {
// Sensor DEACTIVE: Do something here (one time task on deactivation)
sensor2 = GPIO_PIN_RESET;
HAL_GPIO_WritePin(LED5_GPIO_Port, LED5_Pin, GPIO_PIN_RESET);
consoleLog("-- SENSOR 2 Deactive --\n");
}
if (sensor1 == GPIO_PIN_SET) {
// Sensor ACTIVE: Do something here (continuous task)
}
if (sensor2 == GPIO_PIN_SET) {
// Sensor ACTIVE: Do something here (continuous task)
}
// ==================================== SERIAL Tx/Rx Handling ====================================
#ifdef SERIAL_CONTROL
if (main_loop_counter % 5 == 0 && __HAL_DMA_GET_COUNTER(huart2.hdmatx) == 0) { // Check if DMA channel counter is 0 (meaning all data has been transferred)
Sideboard.start = (uint16_t)SERIAL_START_FRAME;
Sideboard.roll = (int16_t)mpu.euler.roll;
Sideboard.pitch = (int16_t)mpu.euler.pitch;
Sideboard.yaw = (int16_t)mpu.euler.yaw;
Sideboard.sensors = (uint16_t)(sensor1 | (sensor2 << 1) | (mpuStatus << 2));
Sideboard.checksum = (uint16_t)(Sideboard.start ^ Sideboard.roll ^ Sideboard.pitch ^ Sideboard.yaw ^ Sideboard.sensors);
HAL_UART_Transmit_DMA(&huart2, (uint8_t *)&Sideboard, sizeof(Sideboard));
}
#endif
#ifdef SERIAL_FEEDBACK
if (timeoutCntSerial++ >= SERIAL_TIMEOUT) { // Timeout qualification
timeoutFlagSerial = 1; // Timeout detected
timeoutCntSerial = SERIAL_TIMEOUT; // Limit timout counter value
}
if (timeoutFlagSerial && main_loop_counter % 100 == 0) { // In case of timeout bring the system to a Safe State and indicate error if desired
HAL_GPIO_TogglePin(LED3_GPIO_Port, LED3_Pin); // Toggle the Yellow LED every 100 ms
}
#endif
handle_mpu6050(); // Handle of the MPU-6050 IMU sensor
handle_sensors(); // Handle of the optical sensors
handle_usart(); // Handle of the USART data
handle_leds(); // Handle of the sideboard LEDs
main_loop_counter++;
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
@@ -236,7 +136,8 @@ void SystemClock_Config(void)
RCC_OscInitTypeDef RCC_OscInitStruct = {0};
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
/** Initializes the CPU, AHB and APB busses clocks
/** Initializes the RCC Oscillators according to the specified parameters
* in the RCC_OscInitTypeDef structure.
*/
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
RCC_OscInitStruct.HSIState = RCC_HSI_ON;
@@ -248,7 +149,7 @@ void SystemClock_Config(void)
{
Error_Handler();
}
/** Initializes the CPU, AHB and APB busses clocks
/** Initializes the CPU, AHB and APB buses clocks
*/
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
@@ -288,7 +189,7 @@ void Error_Handler(void)
* @retval None
*/
void assert_failed(uint8_t *file, uint32_t line)
{
{
/* USER CODE BEGIN 6 */
/* User can add his own implementation to report the file name and line number,
tex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */

1290
Src/mpu6050.c
View File

File diff suppressed because it is too large Load Diff

2
Src/stm32f1xx_hal_msp.c
View File

@@ -72,7 +72,7 @@ void HAL_MspInit(void)
/* System interrupt init*/
/** NOJTAG: JTAG-DP Disabled and SW-DP Enabled
/** NOJTAG: JTAG-DP Disabled and SW-DP Enabled
*/
__HAL_AFIO_REMAP_SWJ_NOJTAG();

52
Src/stm32f1xx_it.c
View File

@@ -58,15 +58,18 @@
/* External variables --------------------------------------------------------*/
extern I2C_HandleTypeDef hi2c1;
extern DMA_HandleTypeDef hdma_usart1_rx;
extern DMA_HandleTypeDef hdma_usart1_tx;
extern DMA_HandleTypeDef hdma_usart2_rx;
extern DMA_HandleTypeDef hdma_usart2_tx;
extern UART_HandleTypeDef huart1;
extern UART_HandleTypeDef huart2;
/* USER CODE BEGIN EV */
/* USER CODE END EV */
/******************************************************************************/
/* Cortex-M3 Processor Interruption and Exception Handlers */
/* Cortex-M3 Processor Interruption and Exception Handlers */
/******************************************************************************/
/**
* @brief This function handles Non maskable interrupt.
@@ -201,6 +204,34 @@ void SysTick_Handler(void)
/* please refer to the startup file (startup_stm32f1xx.s). */
/******************************************************************************/
/**
* @brief This function handles DMA1 channel4 global interrupt.
*/
void DMA1_Channel4_IRQHandler(void)
{
/* USER CODE BEGIN DMA1_Channel4_IRQn 0 */
/* USER CODE END DMA1_Channel4_IRQn 0 */
HAL_DMA_IRQHandler(&hdma_usart1_tx);
/* USER CODE BEGIN DMA1_Channel4_IRQn 1 */
/* USER CODE END DMA1_Channel4_IRQn 1 */
}
/**
* @brief This function handles DMA1 channel5 global interrupt.
*/
void DMA1_Channel5_IRQHandler(void)
{
/* USER CODE BEGIN DMA1_Channel5_IRQn 0 */
HAL_NVIC_ClearPendingIRQ(DMA1_Channel5_IRQn);
/* USER CODE END DMA1_Channel5_IRQn 0 */
HAL_DMA_IRQHandler(&hdma_usart1_rx);
/* USER CODE BEGIN DMA1_Channel5_IRQn 1 */
/* USER CODE END DMA1_Channel5_IRQn 1 */
}
/**
* @brief This function handles DMA1 channel6 global interrupt.
*/
@@ -257,6 +288,23 @@ void I2C1_ER_IRQHandler(void)
/* USER CODE END I2C1_ER_IRQn 1 */
}
/**
* @brief This function handles USART1 global interrupt.
*/
void USART1_IRQHandler(void)
{
/* USER CODE BEGIN USART1_IRQn 0 */
/* USER CODE END USART1_IRQn 0 */
HAL_UART_IRQHandler(&huart1);
/* USER CODE BEGIN USART1_IRQn 1 */
if(RESET != __HAL_UART_GET_IT_SOURCE(&huart1, UART_IT_IDLE)) { // Check for IDLE line interrupt
__HAL_UART_CLEAR_IDLEFLAG(&huart1); // Clear IDLE line flag (otherwise it will continue to enter interrupt)
usart2_rx_check(); // Check for data to process
}
/* USER CODE END USART1_IRQn 1 */
}
/**
* @brief This function handles USART2 global interrupt.
*/
@@ -269,7 +317,7 @@ void USART2_IRQHandler(void)
/* USER CODE BEGIN USART2_IRQn 1 */
if(RESET != __HAL_UART_GET_IT_SOURCE(&huart2, UART_IT_IDLE)) { // Check for IDLE line interrupt
__HAL_UART_CLEAR_IDLEFLAG(&huart2); // Clear IDLE line flag (otherwise it will continue to enter interrupt)
usart_rx_check(); // Check for data to process
usart2_rx_check(); // Check for data to process
}
/* USER CODE END USART2_IRQn 1 */
}

133
Src/usart.c
View File

@@ -1,8 +1,8 @@
/**
******************************************************************************
* File Name : USART.c
* Description : This file provides code for the configuration
* of the USART instances.
* @file usart.c
* @brief This file provides code for the configuration
* of the USART instances.
******************************************************************************
* @attention
*
@@ -24,10 +24,32 @@
/* USER CODE END 0 */
UART_HandleTypeDef huart1;
UART_HandleTypeDef huart2;
DMA_HandleTypeDef hdma_usart1_rx;
DMA_HandleTypeDef hdma_usart1_tx;
DMA_HandleTypeDef hdma_usart2_rx;
DMA_HandleTypeDef hdma_usart2_tx;
/* USART1 init function */
void MX_USART1_UART_Init(void)
{
huart1.Instance = USART1;
huart1.Init.BaudRate = USART_AUX_BAUD;
huart1.Init.WordLength = UART_WORDLENGTH_8B;
huart1.Init.StopBits = UART_STOPBITS_1;
huart1.Init.Parity = UART_PARITY_NONE;
huart1.Init.Mode = UART_MODE_TX_RX;
huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;
huart1.Init.OverSampling = UART_OVERSAMPLING_16;
if (HAL_UART_Init(&huart1) != HAL_OK)
{
Error_Handler();
}
}
/* USART2 init function */
void MX_USART2_UART_Init(void)
@@ -52,18 +74,81 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
if(uartHandle->Instance==USART2)
if(uartHandle->Instance==USART1)
{
/* USER CODE BEGIN USART1_MspInit 0 */
/* USER CODE END USART1_MspInit 0 */
/* USART1 clock enable */
__HAL_RCC_USART1_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE();
/**USART1 GPIO Configuration
PA9 ------> USART1_TX
PA10 ------> USART1_RX
*/
GPIO_InitStruct.Pin = GPIO_PIN_9;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
GPIO_InitStruct.Pin = GPIO_PIN_10;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_PULLUP;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
/* USART1 DMA Init */
/* USART1_RX Init */
hdma_usart1_rx.Instance = DMA1_Channel5;
hdma_usart1_rx.Init.Direction = DMA_PERIPH_TO_MEMORY;
hdma_usart1_rx.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_usart1_rx.Init.MemInc = DMA_MINC_ENABLE;
hdma_usart1_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
hdma_usart1_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
hdma_usart1_rx.Init.Mode = DMA_CIRCULAR;
hdma_usart1_rx.Init.Priority = DMA_PRIORITY_LOW;
if (HAL_DMA_Init(&hdma_usart1_rx) != HAL_OK)
{
Error_Handler();
}
__HAL_LINKDMA(uartHandle,hdmarx,hdma_usart1_rx);
/* USART1_TX Init */
hdma_usart1_tx.Instance = DMA1_Channel4;
hdma_usart1_tx.Init.Direction = DMA_MEMORY_TO_PERIPH;
hdma_usart1_tx.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_usart1_tx.Init.MemInc = DMA_MINC_ENABLE;
hdma_usart1_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
hdma_usart1_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
hdma_usart1_tx.Init.Mode = DMA_NORMAL;
hdma_usart1_tx.Init.Priority = DMA_PRIORITY_LOW;
if (HAL_DMA_Init(&hdma_usart1_tx) != HAL_OK)
{
Error_Handler();
}
__HAL_LINKDMA(uartHandle,hdmatx,hdma_usart1_tx);
/* USART1 interrupt Init */
HAL_NVIC_SetPriority(USART1_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(USART1_IRQn);
/* USER CODE BEGIN USART1_MspInit 1 */
__HAL_UART_ENABLE_IT (uartHandle, UART_IT_IDLE); // Enable the USART IDLE line detection interrupt
/* USER CODE END USART1_MspInit 1 */
}
else if(uartHandle->Instance==USART2)
{
/* USER CODE BEGIN USART2_MspInit 0 */
/* USER CODE END USART2_MspInit 0 */
/* USART2 clock enable */
__HAL_RCC_USART2_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE();
/**USART2 GPIO Configuration
/**USART2 GPIO Configuration
PA2 ------> USART2_TX
PA3 ------> USART2_RX
PA3 ------> USART2_RX
*/
GPIO_InitStruct.Pin = GPIO_PIN_2;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
@@ -120,17 +205,41 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle)
{
if(uartHandle->Instance==USART2)
if(uartHandle->Instance==USART1)
{
/* USER CODE BEGIN USART1_MspDeInit 0 */
/* USER CODE END USART1_MspDeInit 0 */
/* Peripheral clock disable */
__HAL_RCC_USART1_CLK_DISABLE();
/**USART1 GPIO Configuration
PA9 ------> USART1_TX
PA10 ------> USART1_RX
*/
HAL_GPIO_DeInit(GPIOA, GPIO_PIN_9|GPIO_PIN_10);
/* USART1 DMA DeInit */
HAL_DMA_DeInit(uartHandle->hdmarx);
HAL_DMA_DeInit(uartHandle->hdmatx);
/* USART1 interrupt Deinit */
HAL_NVIC_DisableIRQ(USART1_IRQn);
/* USER CODE BEGIN USART1_MspDeInit 1 */
/* USER CODE END USART1_MspDeInit 1 */
}
else if(uartHandle->Instance==USART2)
{
/* USER CODE BEGIN USART2_MspDeInit 0 */
/* USER CODE END USART2_MspDeInit 0 */
/* Peripheral clock disable */
__HAL_RCC_USART2_CLK_DISABLE();
/**USART2 GPIO Configuration
/**USART2 GPIO Configuration
PA2 ------> USART2_TX
PA3 ------> USART2_RX
PA3 ------> USART2_RX
*/
HAL_GPIO_DeInit(GPIOA, GPIO_PIN_2|GPIO_PIN_3);
@@ -144,7 +253,7 @@ void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle)
/* USER CODE END USART2_MspDeInit 1 */
}
}
}
/* USER CODE BEGIN 1 */

586
Src/util.c
View File

@@ -28,120 +28,180 @@
#include "util.h"
#include "mpu6050.h"
extern UART_HandleTypeDef huart1;
extern UART_HandleTypeDef huart2;
extern I2C_HandleTypeDef hi2c1;
extern I2C_HandleTypeDef hi2c1;
// USART variables
// USART2 variables
#ifdef SERIAL_CONTROL
SerialSideboard Sideboard;
#endif
#if defined(SERIAL_DEBUG) || defined(SERIAL_FEEDBACK)
static uint8_t rx_buffer[SERIAL_BUFFER_SIZE]; // USART Rx DMA circular buffer
static uint32_t rx_buffer_len = ARRAY_LEN(rx_buffer);
static uint8_t rx2_buffer[SERIAL_BUFFER_SIZE]; // USART Rx DMA circular buffer
static uint32_t rx2_buffer_len = ARRAY_LEN(rx2_buffer);
#endif
#ifdef SERIAL_FEEDBACK
SerialFeedback Feedback;
SerialFeedback FeedbackRaw;
uint16_t timeoutCntSerial = 0; // Timeout counter for Rx Serial command
uint8_t timeoutFlagSerial = 0; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data)
static SerialFeedback Feedback;
static SerialFeedback FeedbackRaw;
static uint16_t timeoutCntSerial2 = 0; // Timeout counter for UART2 Rx Serial
static uint8_t timeoutFlagSerial2 = 0; // Timeout Flag for UART2 Rx Serial: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data)
static uint32_t Feedback_len = sizeof(Feedback);
#endif
// USART1 variables
#ifdef SERIAL_AUX_TX
static SerialAuxTx AuxTx;
#endif
#ifdef SERIAL_AUX_RX
static uint8_t rx1_buffer[SERIAL_BUFFER_SIZE]; // USART Rx DMA circular buffer
static uint32_t rx1_buffer_len = ARRAY_LEN(rx1_buffer);
#endif
#ifdef SERIAL_AUX_RX
static SerialCommand command;
static SerialCommand command_raw;
static uint16_t timeoutCntSerial1 = 0; // Timeout counter for UART1 Rx Serial
static uint8_t timeoutFlagSerial1 = 0; // Timeout Flag for UART1 Rx Serial: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data)
static uint32_t command_len = sizeof(command);
extern uint8_t print_aux;
#ifdef CONTROL_IBUS
static uint16_t ibus_chksum;
static uint16_t ibus_captured_value[IBUS_NUM_CHANNELS];
#endif
#endif
#if (defined(SERIAL_AUX_RX) && defined(CONTROL_IBUS)) || defined(SERIAL_CONTROL)
static int16_t cmd1, cmd2;
static uint16_t cmdSwitch;
#endif
// Optical sensors variables
static GPIO_PinState sensor1, sensor2; // holds the sensor1 and sensor 2 values
static GPIO_PinState sensor1_read, sensor2_read; // holds the instantaneous Read for sensor1 and sensor 2
// MPU variables
ErrorStatus mpuStatus; // holds the MPU-6050 status: SUCCESS or ERROR
extern MPU_Data mpu; // holds the MPU-6050 data
#if defined(MPU_SENSOR_ENABLE) || defined(SERIAL_CONTROL)
static ErrorStatus mpuStatus; // holds the MPU-6050 status: SUCCESS or ERROR
#endif
extern uint32_t main_loop_counter; // main loop counter to perform task scheduling inside main()
/* =========================== General Functions =========================== */
void consoleLog(char *message)
{
#ifdef SERIAL_DEBUG
log_i("%s", message);
#endif
#ifdef SERIAL_DEBUG
log_i("%s", message);
#endif
}
void get_tick_count_ms(unsigned long *count)
{
*count = HAL_GetTick();
*count = HAL_GetTick();
}
/* retarget the C library printf function to the USART */
#ifdef SERIAL_DEBUG
#ifdef __GNUC__
#define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
#else
#define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
#endif
PUTCHAR_PROTOTYPE {
HAL_UART_Transmit(&huart2, (uint8_t *)&ch, 1, 1000);
return ch;
}
#ifdef __GNUC__
int _write(int file, char *data, int len) {
int i;
for (i = 0; i < len; i++) { __io_putchar( *data++ );}
return len;
}
#endif
#ifdef SERIAL_DEBUG
#ifdef __GNUC__
#define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
#else
#define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
#endif
PUTCHAR_PROTOTYPE {
HAL_UART_Transmit(&huart2, (uint8_t *)&ch, 1, 1000);
return ch;
}
#ifdef __GNUC__
int _write(int file, char *data, int len) {
int i;
for (i = 0; i < len; i++) { __io_putchar( *data++ );}
return len;
}
#endif
#endif
void intro_demo_led(uint32_t tDelay)
{
int i;
int i;
for (i = 0; i < 3; i++) {
HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED3_GPIO_Port, LED3_Pin, GPIO_PIN_RESET);
HAL_Delay(tDelay);
HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_RESET);
HAL_Delay(tDelay);
HAL_GPIO_WritePin(LED3_GPIO_Port, LED3_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_RESET);
HAL_Delay(tDelay);
}
for (i = 0; i < 2; i++) {
HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED3_GPIO_Port, LED3_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED4_GPIO_Port, LED4_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED5_GPIO_Port, LED5_Pin, GPIO_PIN_SET);
HAL_Delay(tDelay);
HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(LED3_GPIO_Port, LED3_Pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(LED4_GPIO_Port, LED4_Pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(LED5_GPIO_Port, LED5_Pin, GPIO_PIN_RESET);
}
for (i = 0; i < 3; i++) {
HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED3_GPIO_Port, LED3_Pin, GPIO_PIN_RESET);
HAL_Delay(tDelay);
HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_RESET);
HAL_Delay(tDelay);
HAL_GPIO_WritePin(LED3_GPIO_Port, LED3_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_RESET);
HAL_Delay(tDelay);
}
for (i = 0; i < 2; i++) {
HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED3_GPIO_Port, LED3_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED4_GPIO_Port, LED4_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(LED5_GPIO_Port, LED5_Pin, GPIO_PIN_SET);
HAL_Delay(tDelay);
HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(LED3_GPIO_Port, LED3_Pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(LED4_GPIO_Port, LED4_Pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(LED5_GPIO_Port, LED5_Pin, GPIO_PIN_RESET);
}
}
uint8_t switch_check(uint16_t ch, uint8_t type) {
if (type) { // 3 positions switch
if (ch < 250) return 0; // switch in position 0
else if (ch < 850) return 1; // switch in position 1
else return 2; // switch in position 2
} else { // 2 positions switch
return (ch > 850);
}
}
/* =========================== Input Initialization Function =========================== */
void input_init(void) {
#if defined(SERIAL_DEBUG) || defined(SERIAL_FEEDBACK)
HAL_UART_Receive_DMA(&huart2, (uint8_t *)rx_buffer, sizeof(rx_buffer));
UART_DisableRxErrors(&huart2);
#endif
#if defined(SERIAL_DEBUG) || defined(SERIAL_FEEDBACK)
HAL_UART_Receive_DMA(&huart2, (uint8_t *)rx2_buffer, sizeof(rx2_buffer));
UART_DisableRxErrors(&huart2);
#endif
#ifdef SERIAL_AUX_RX
HAL_UART_Receive_DMA(&huart1, (uint8_t *)rx1_buffer, sizeof(rx1_buffer));
UART_DisableRxErrors(&huart1);
#endif
intro_demo_led(100); // Short LEDs intro demo with 100 ms delay. This also gives some time for the MPU-6050 to power-up.
intro_demo_led(100); // Short LEDs intro demo with 100 ms delay. This also gives some time for the MPU-6050 to power-up.
#ifdef MPU_SENSOR_ENABLE
if(mpu_config()) { // IMU MPU-6050 config
mpuStatus = ERROR;
HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_SET); // Turn on RED LED
}
else {
mpuStatus = SUCCESS;
HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_SET); // Turn on GREEN LED
}
mpu_handle_input('h'); // Print the User Help commands to serial
#else
HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_SET); // Turn on GREEN LED
#endif
#ifdef MPU_SENSOR_ENABLE
if(mpu_config()) { // IMU MPU-6050 config
mpuStatus = ERROR;
HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_SET); // Turn on RED LED - sensor enabled and NOT ok
}
else {
mpuStatus = SUCCESS;
HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_SET); // Turn on GREEN LED - sensor enabled and ok
}
#else
HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_SET); // Turn on GREEN LED - sensor disabled
#endif
#ifdef SERIAL_DEBUG
mpu_handle_input('h'); // Print the User Help commands to serial
#endif
}
/**
@@ -161,60 +221,215 @@ void UART_DisableRxErrors(UART_HandleTypeDef *huart)
}
#endif
/* =========================== USART READ Functions =========================== */
/* =========================== Handle Functions =========================== */
/*
* Handle of the MPU-6050 IMU sensor
*/
void handle_mpu6050(void) {
#ifdef MPU_SENSOR_ENABLE
// Get MPU data. Because the MPU-6050 interrupt pin is not wired we have to check DMP data by pooling periodically
if (SUCCESS == mpuStatus) {
mpu_get_data();
} else if (ERROR == mpuStatus && main_loop_counter % 100 == 0) {
HAL_GPIO_TogglePin(LED1_GPIO_Port, LED1_Pin); // Toggle the Red LED every 100 ms
}
// Print MPU data to Console
#ifdef SERIAL_DEBUG
if (main_loop_counter % 50 == 0) {
mpu_print_to_console();
}
#endif
#endif
}
/*
* Handle of the optical sensors
*/
void handle_sensors(void) {
sensor1_read = HAL_GPIO_ReadPin(SENSOR1_GPIO_Port, SENSOR1_Pin);
sensor2_read = HAL_GPIO_ReadPin(SENSOR2_GPIO_Port, SENSOR2_Pin);
// SENSOR1
if (sensor1 == GPIO_PIN_RESET && sensor1_read == GPIO_PIN_SET) {
sensor1 = GPIO_PIN_SET;
// Sensor ACTIVE: Do something here (one time task on activation)
HAL_GPIO_WritePin(LED4_GPIO_Port, LED4_Pin, GPIO_PIN_SET);
consoleLog("SENSOR 1 ON\r\n");
} else if(sensor1 == GPIO_PIN_SET && sensor1_read == GPIO_PIN_RESET) {
// Sensor DEACTIVE: Do something here (one time task on deactivation)
sensor1 = GPIO_PIN_RESET;
HAL_GPIO_WritePin(LED4_GPIO_Port, LED4_Pin, GPIO_PIN_RESET);
consoleLog("SENSOR 1 OFF\r\n");
}
// SENSOR2
if (sensor2 == GPIO_PIN_RESET && sensor2_read == GPIO_PIN_SET) {
sensor2 = GPIO_PIN_SET;
// Sensor ACTIVE: Do something here (one time task on activation)
HAL_GPIO_WritePin(LED5_GPIO_Port, LED5_Pin, GPIO_PIN_SET);
consoleLog("SENSOR 2 ON\r\n");
} else if (sensor2 == GPIO_PIN_SET && sensor2_read == GPIO_PIN_RESET) {
// Sensor DEACTIVE: Do something here (one time task on deactivation)
sensor2 = GPIO_PIN_RESET;
HAL_GPIO_WritePin(LED5_GPIO_Port, LED5_Pin, GPIO_PIN_RESET);
consoleLog("SENSOR 2 OFF\r\n");
}
if (sensor1 == GPIO_PIN_SET) {
// Sensor ACTIVE: Do something here (continuous task)
}
if (sensor2 == GPIO_PIN_SET) {
// Sensor ACTIVE: Do something here (continuous task)
}
}
/*
* Handle of the USART data
*/
void handle_usart(void) {
// Tx USART MAIN
#ifdef SERIAL_CONTROL
if (main_loop_counter % 5 == 0 && __HAL_DMA_GET_COUNTER(huart2.hdmatx) == 0) { // Check if DMA channel counter is 0 (meaning all data has been transferred)
Sideboard.start = (uint16_t)SERIAL_START_FRAME;
Sideboard.pitch = (int16_t)mpu.euler.pitch;
Sideboard.dPitch = (int16_t)mpu.gyro.y;
Sideboard.cmd1 = (int16_t)cmd1;
Sideboard.cmd2 = (int16_t)cmd2;
Sideboard.sensors = (uint16_t)( (cmdSwitch << 8) | (sensor1 | (sensor2 << 1) | (mpuStatus << 2)) );
Sideboard.checksum = (uint16_t)(Sideboard.start ^ Sideboard.pitch ^ Sideboard.dPitch ^ Sideboard.cmd1 ^ Sideboard.cmd2 ^ Sideboard.sensors);
HAL_UART_Transmit_DMA(&huart2, (uint8_t *)&Sideboard, sizeof(Sideboard));
}
#endif
// Rx USART MAIN
#ifdef SERIAL_FEEDBACK
if (timeoutCntSerial2++ >= SERIAL_TIMEOUT) { // Timeout qualification
timeoutFlagSerial2 = 1; // Timeout detected
timeoutCntSerial2 = SERIAL_TIMEOUT; // Limit timout counter value
}
if (timeoutFlagSerial2 && main_loop_counter % 100 == 0) { // In case of timeout bring the system to a Safe State and indicate error if desired
HAL_GPIO_TogglePin(LED3_GPIO_Port, LED3_Pin); // Toggle the Yellow LED every 100 ms
}
#endif
// Tx USART AUX
#ifdef SERIAL_AUX_TX
if (main_loop_counter % 5 == 0 && __HAL_DMA_GET_COUNTER(huart1.hdmatx) == 0) { // Check if DMA channel counter is 0 (meaning all data has been transferred)
AuxTx.start = (uint16_t)SERIAL_START_FRAME;
AuxTx.signal1 = (int16_t)sensor1;
AuxTx.signal2 = (int16_t)sensor2;
AuxTx.checksum = (uint16_t)(AuxTx.start ^ AuxTx.signal1 ^ AuxTx.signal2);
HAL_UART_Transmit_DMA(&huart1, (uint8_t *)&AuxTx, sizeof(AuxTx));
}
#endif
// Rx USART AUX
#ifdef SERIAL_AUX_RX
#ifdef CONTROL_IBUS
if (!timeoutFlagSerial1) {
for (uint8_t i = 0; i < (IBUS_NUM_CHANNELS * 2); i+=2) {
ibus_captured_value[(i/2)] = CLAMP(command.channels[i] + (command.channels[i+1] << 8) - 1000, 0, 1000); // 1000-2000 -> 0-1000
}
cmd1 = (ibus_captured_value[0] - 500) * 2; // Channel 1
cmd2 = (ibus_captured_value[1] - 500) * 2; // Channel 2
cmdSwitch = (uint16_t)(switch_check(ibus_captured_value[6],0) | // Channel 7
switch_check(ibus_captured_value[7],1) << 1 | // Channel 8
switch_check(ibus_captured_value[8],1) << 3 | // Channel 9
switch_check(ibus_captured_value[9],0) << 5); // Channel 10
}
#endif
if (timeoutCntSerial1++ >= SERIAL_TIMEOUT) { // Timeout qualification
timeoutFlagSerial1 = 1; // Timeout detected
timeoutCntSerial1 = SERIAL_TIMEOUT; // Limit timout counter value
cmd1 = cmd2 = 0; // Set commands to 0
cmdSwitch &= ~(1U << 0); // Clear Bit 0, to switch to default control input
}
// if (timeoutFlagSerial0 && main_loop_counter % 100 == 0) { // In case of timeout bring the system to a Safe State and indicate error if desired
// HAL_GPIO_TogglePin(LED2_GPIO_Port, LED2_Pin); // Toggle the Green LED every 100 ms
// }
#ifdef SERIAL_DEBUG
// Print MPU data to Console
if (main_loop_counter % 50 == 0) {
aux_print_to_console();
}
#endif
#endif
}
/*
* Handle of the sideboard LEDs
*/
void handle_leds(void) {
#ifdef SERIAL_FEEDBACK
if (!timeoutFlagSerial2) {
if (Feedback.cmdLed & LED1_SET) { HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_SET); } else { HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_RESET); }
if (Feedback.cmdLed & LED2_SET) { HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_SET); } else { HAL_GPIO_WritePin(LED2_GPIO_Port, LED2_Pin, GPIO_PIN_RESET); }
if (Feedback.cmdLed & LED3_SET) { HAL_GPIO_WritePin(LED3_GPIO_Port, LED3_Pin, GPIO_PIN_SET); } else { HAL_GPIO_WritePin(LED3_GPIO_Port, LED3_Pin, GPIO_PIN_RESET); }
if (Feedback.cmdLed & LED4_SET) { HAL_GPIO_WritePin(LED4_GPIO_Port, LED4_Pin, GPIO_PIN_SET); } else { HAL_GPIO_WritePin(LED4_GPIO_Port, LED4_Pin, GPIO_PIN_RESET); }
if (Feedback.cmdLed & LED5_SET) { HAL_GPIO_WritePin(LED5_GPIO_Port, LED5_Pin, GPIO_PIN_SET); } else { HAL_GPIO_WritePin(LED5_GPIO_Port, LED5_Pin, GPIO_PIN_RESET); }
}
#endif
}
/* =========================== USART2 READ Functions =========================== */
/*
* Check for new data received on USART with DMA: refactored function from https://github.com/MaJerle/stm32-usart-uart-dma-rx-tx
* - this function is called for every USART IDLE line detection, in the USART interrupt handler
*/
void usart_rx_check(void)
void usart2_rx_check(void)
{
#ifdef SERIAL_DEBUG
static uint32_t old_pos;
uint32_t pos;
#ifdef SERIAL_DEBUG
static uint32_t old_pos;
uint32_t pos;
pos = rx_buffer_len - __HAL_DMA_GET_COUNTER(huart2.hdmarx); // Calculate current position in buffer, Rx: DMA1_Channel6->CNDTR, Tx: DMA1_Channel7
if (pos != old_pos) { // Check change in received data
if (pos > old_pos) { // "Linear" buffer mode: check if current position is over previous one
usart_process_debug(&rx_buffer[old_pos], pos - old_pos); // Process data
} else { // "Overflow" buffer mode
usart_process_debug(&rx_buffer[old_pos], rx_buffer_len - old_pos); // First Process data from the end of buffer
if (pos > 0) { // Check and continue with beginning of buffer
usart_process_debug(&rx_buffer[0], pos); // Process remaining data
}
pos = rx2_buffer_len - __HAL_DMA_GET_COUNTER(huart2.hdmarx); // Calculate current position in buffer, Rx: DMA1_Channel6->CNDTR, Tx: DMA1_Channel7
if (pos != old_pos) { // Check change in received data
if (pos > old_pos) { // "Linear" buffer mode: check if current position is over previous one
usart_process_debug(&rx2_buffer[old_pos], pos - old_pos); // Process data
} else { // "Overflow" buffer mode
usart_process_debug(&rx2_buffer[old_pos], rx2_buffer_len - old_pos);// First Process data from the end of buffer
if (pos > 0) { // Check and continue with beginning of buffer
usart_process_debug(&rx2_buffer[0], pos); // Process remaining data
}
}
}
old_pos = pos; // Updated old position
if (old_pos == rx_buffer_len) { // Check and manually update if we reached end of buffer
old_pos = pos; // Updated old position
if (old_pos == rx2_buffer_len) { // Check and manually update if we reached end of buffer
old_pos = 0;
}
#endif // SERIAL_DEBUG
#endif // SERIAL_DEBUG
#ifdef SERIAL_FEEDBACK
static uint32_t old_pos;
uint32_t pos;
uint8_t *ptr;
pos = rx_buffer_len - __HAL_DMA_GET_COUNTER(huart2.hdmarx); // Calculate current position in buffer, Rx: DMA1_Channel6->CNDTR, Tx: DMA1_Channel7
if (pos != old_pos) { // Check change in received data
ptr = (uint8_t *)&FeedbackRaw; // Initialize the pointer with FeedbackRaw address
if (pos > old_pos && (pos - old_pos) == Feedback_len) { // "Linear" buffer mode: check if current position is over previous one AND data length equals expected length
memcpy(ptr, &rx_buffer[old_pos], Feedback_len); // Copy data. This is possible only if FeedbackRaw is contiguous! (meaning all the structure members have the same size)
usart_process_data(&FeedbackRaw, &Feedback); // Process data
} else if ((rx_buffer_len - old_pos + pos) == Feedback_len) { // "Overflow" buffer mode: check if data length equals expected length
memcpy(ptr, &rx_buffer[old_pos], rx_buffer_len - old_pos); // First copy data from the end of buffer
if (pos > 0) { // Check and continue with beginning of buffer
ptr += rx_buffer_len - old_pos; // Move to correct position in FeedbackRaw
memcpy(ptr, &rx_buffer[0], pos); // Copy remaining data
#ifdef SERIAL_FEEDBACK
static uint32_t old_pos;
uint32_t pos;
uint8_t *ptr;
pos = rx2_buffer_len - __HAL_DMA_GET_COUNTER(huart2.hdmarx); // Calculate current position in buffer, Rx: DMA1_Channel6->CNDTR, Tx: DMA1_Channel7
if (pos != old_pos) { // Check change in received data
ptr = (uint8_t *)&FeedbackRaw; // Initialize the pointer with FeedbackRaw address
if (pos > old_pos && (pos - old_pos) == Feedback_len) { // "Linear" buffer mode: check if current position is over previous one AND data length equals expected length
memcpy(ptr, &rx2_buffer[old_pos], Feedback_len); // Copy data. This is possible only if FeedbackRaw is contiguous! (meaning all the structure members have the same size)
usart_process_data(&FeedbackRaw, &Feedback); // Process data
} else if ((rx2_buffer_len - old_pos + pos) == Feedback_len) { // "Overflow" buffer mode: check if data length equals expected length
memcpy(ptr, &rx2_buffer[old_pos], rx2_buffer_len - old_pos); // First copy data from the end of buffer
if (pos > 0) { // Check and continue with beginning of buffer
ptr += rx2_buffer_len - old_pos; // Move to correct position in FeedbackRaw
memcpy(ptr, &rx2_buffer[0], pos); // Copy remaining data
}
usart_process_data(&FeedbackRaw, &Feedback); // Process data
}
}
old_pos = pos; // Update old position
if (old_pos == rx_buffer_len) { // Check and manually update if we reached end of buffer
usart_process_data(&FeedbackRaw, &Feedback); // Process data
}
}
old_pos = pos; // Update old position
if (old_pos == rx2_buffer_len) { // Check and manually update if we reached end of buffer
old_pos = 0;
}
#endif // SERIAL_FEEDBACK
}
#endif // SERIAL_FEEDBACK
}
/*
@@ -223,13 +438,11 @@ void usart_rx_check(void)
#ifdef SERIAL_DEBUG
void usart_process_debug(uint8_t *userCommand, uint32_t len)
{
for (; len > 0; len--, userCommand++) {
if (*userCommand != '\n' && *userCommand != '\r') { // Do not accept 'new line' and 'carriage return' commands
log_i("Command = %c\n", *userCommand);
#ifdef MPU_SENSOR_ENABLE
mpu_handle_input(*userCommand);
#endif
}
for (; len > 0; len--, userCommand++) {
if (*userCommand != '\n' && *userCommand != '\r') { // Do not accept 'new line' and 'carriage return' commands
log_i("Command = %c\r\n", *userCommand);
mpu_handle_input(*userCommand);
}
}
}
#endif // SERIAL_DEBUG
@@ -240,21 +453,96 @@ void usart_process_debug(uint8_t *userCommand, uint32_t len)
*/
#ifdef SERIAL_FEEDBACK
void usart_process_data(SerialFeedback *Feedback_in, SerialFeedback *Feedback_out)
{
uint16_t checksum;
if (Feedback_in->start == SERIAL_START_FRAME) {
checksum = (uint16_t)(Feedback_in->start ^ Feedback_in->cmd1 ^ Feedback_in->cmd2 ^ Feedback_in->speedR_meas ^ Feedback_in->speedL_meas
^ Feedback_in->batVoltage ^ Feedback_in->boardTemp ^ Feedback_in->cmdLed);
if (Feedback_in->checksum == checksum) {
*Feedback_out = *Feedback_in;
timeoutCntSerial = 0; // Reset timeout counter
timeoutFlagSerial = 0; // Clear timeout flag
}
}
{
uint16_t checksum;
if (Feedback_in->start == SERIAL_START_FRAME) {
checksum = (uint16_t)(Feedback_in->start ^ Feedback_in->cmd1 ^ Feedback_in->cmd2 ^ Feedback_in->speedR_meas ^ Feedback_in->speedL_meas
^ Feedback_in->batVoltage ^ Feedback_in->boardTemp ^ Feedback_in->cmdLed);
if (Feedback_in->checksum == checksum) {
*Feedback_out = *Feedback_in;
timeoutCntSerial2 = 0; // Reset timeout counter
timeoutFlagSerial2 = 0; // Clear timeout flag
}
}
}
#endif // SERIAL_FEEDBACK
/* =========================== USART1 READ Functions =========================== */
/*
* Check for new data received on USART with DMA: refactored function from https://github.com/MaJerle/stm32-usart-uart-dma-rx-tx
* - this function is called for every USART IDLE line detection, in the USART interrupt handler
*/
void usart1_rx_check(void)
{
#ifdef SERIAL_AUX_RX
static uint32_t old_pos;
uint32_t pos;
uint8_t *ptr;
pos = rx1_buffer_len - __HAL_DMA_GET_COUNTER(huart1.hdmarx); // Calculate current position in buffer, Rx: DMA1_Channel5->CNDTR, Tx: DMA1_Channel4
if (pos != old_pos) { // Check change in received data
ptr = (uint8_t *)&command_raw; // Initialize the pointer with structure address
if (pos > old_pos && (pos - old_pos) == command_len) { // "Linear" buffer mode: check if current position is over previous one AND data length equals expected length
memcpy(ptr, &rx1_buffer[old_pos], command_len); // Copy data. This is possible only if structure is contiguous! (meaning all the structure members have the same size)
usart_process_command(&command_raw, &command); // Process data
} else if ((rx1_buffer_len - old_pos + pos) == command_len) { // "Overflow" buffer mode: check if data length equals expected length
memcpy(ptr, &rx1_buffer[old_pos], rx1_buffer_len - old_pos); // First copy data from the end of buffer
if (pos > 0) { // Check and continue with beginning of buffer
ptr += rx1_buffer_len - old_pos; // Update position
memcpy(ptr, &rx1_buffer[0], pos); // Copy remaining data
}
usart_process_command(&command_raw, &command); // Process data
}
}
old_pos = pos; // Updated old position
if (old_pos == rx1_buffer_len) { // Check and manually update if we reached end of buffer
old_pos = 0;
}
#endif // SERIAL_AUX_RX
}
/*
* Process command UART0 Rx data
* - if the command_in data is valid (correct START_FRAME and checksum) copy the command_in to command_out
*/
#ifdef SERIAL_AUX_RX
void usart_process_command(SerialCommand *command_in, SerialCommand *command_out)
{
#ifdef CONTROL_IBUS
if (command_in->start == IBUS_LENGTH && command_in->type == IBUS_COMMAND) {
ibus_chksum = 0xFFFF - IBUS_LENGTH - IBUS_COMMAND;
for (uint8_t i = 0; i < (IBUS_NUM_CHANNELS * 2); i++) {
ibus_chksum -= command_in->channels[i];
}
if (ibus_chksum == (uint16_t)((command_in->checksumh << 8) + command_in->checksuml)) {
*command_out = *command_in;
timeoutCntSerial1 = 0; // Reset timeout counter
timeoutFlagSerial1 = 0; // Clear timeout flag
}
}
#endif
}
#endif
/* =========================== AUX Serial Print data =========================== */
void aux_print_to_console(void)
{
#if defined(SERIAL_DEBUG) && defined(SERIAL_AUX_RX)
#ifdef CONTROL_IBUS
if (print_aux & PRINT_AUX) {
log_i( "Ch1: %d Ch2: %d Sw: %u\r\n", cmd1, cmd2, cmdSwitch);
}
#endif
#endif
}
/* =========================== I2C WRITE Functions =========================== */
/*
@@ -262,12 +550,12 @@ void usart_process_data(SerialFeedback *Feedback_in, SerialFeedback *Feedback_ou
*/
int8_t i2c_writeBytes(uint8_t slaveAddr, uint8_t regAddr, uint8_t length, uint8_t *data)
{
// !! Using the I2C Interrupt will fail writing the DMP.. could be that DMP memory writing requires more time !! So use the I2C without interrupt.
// HAL_I2C_Mem_Write_IT(&hi2c1, slaveAddr << 1, regAddr, 1, data, length);
// while(HAL_I2C_STATE_READY != HAL_I2C_GetState(&hi2c1)); // Wait until all data bytes are sent/received
// return 0;
// !! Using the I2C Interrupt will fail writing the DMP.. could be that DMP memory writing requires more time !! So use the I2C without interrupt.
// HAL_I2C_Mem_Write_IT(&hi2c1, slaveAddr << 1, regAddr, 1, data, length);
// while(HAL_I2C_STATE_READY != HAL_I2C_GetState(&hi2c1)); // Wait until all data bytes are sent/received
// return 0;
return HAL_I2C_Mem_Write(&hi2c1, slaveAddr << 1, regAddr, 1, data, length, 100); // Address is shifted one position to the left. LSB is reserved for the Read/Write bit.
return HAL_I2C_Mem_Write(&hi2c1, slaveAddr << 1, regAddr, 1, data, length, 100); // Address is shifted one position to the left. LSB is reserved for the Read/Write bit.
}
@@ -276,7 +564,7 @@ int8_t i2c_writeBytes(uint8_t slaveAddr, uint8_t regAddr, uint8_t length, uint8_
*/
int8_t i2c_writeByte(uint8_t slaveAddr, uint8_t regAddr, uint8_t data)
{
return i2c_writeBytes(slaveAddr, regAddr, 1, &data);
return i2c_writeBytes(slaveAddr, regAddr, 1, &data);
}
/*
@@ -294,15 +582,15 @@ int8_t i2c_writeBit(uint8_t slaveAddr, uint8_t regAddr, uint8_t bitNum, uint8_t
/*
* read bytes from chip register
*/
int8_t i2c_readBytes(uint8_t slaveAddr, uint8_t regAddr, uint8_t length, uint8_t *data)
{
// !! Using the I2C Interrupt will fail writing the DMP.. could be that DMP memory writing requires more time !! So use the I2C without interrupt.
// HAL_I2C_Mem_Read(&hi2c1, slaveAddr << 1, regAddr, 1, data, length);
// while(HAL_I2C_STATE_READY != HAL_I2C_GetState(&hi2c1)); // Wait until all data bytes are sent/received
// return 0;
int8_t i2c_readBytes(uint8_t slaveAddr, uint8_t regAddr, uint8_t length, uint8_t *data)
{
// !! Using the I2C Interrupt will fail writing the DMP.. could be that DMP memory writing requires more time !! So use the I2C without interrupt.
// HAL_I2C_Mem_Read(&hi2c1, slaveAddr << 1, regAddr, 1, data, length);
// while(HAL_I2C_STATE_READY != HAL_I2C_GetState(&hi2c1)); // Wait until all data bytes are sent/received
// return 0;
return HAL_I2C_Mem_Read(&hi2c1, slaveAddr << 1, regAddr, 1, data, length, 100); // Address is shifted one position to the left. LSB is reserved for the Read/Write bit.
return HAL_I2C_Mem_Read(&hi2c1, slaveAddr << 1, regAddr, 1, data, length, 100); // Address is shifted one position to the left. LSB is reserved for the Read/Write bit.
}
/*
@@ -310,7 +598,7 @@ int8_t i2c_readBytes(uint8_t slaveAddr, uint8_t regAddr, uint8_t length, uint8_t
*/
int8_t i2c_readByte(uint8_t slaveAddr, uint8_t regAddr, uint8_t *data)
{
return i2c_readBytes(slaveAddr, regAddr, 1, data);
return i2c_readBytes(slaveAddr, regAddr, 1, data);
}
/*