From a05a69cba58048121f43b1bf8027d43ec42a0706 Mon Sep 17 00:00:00 2001 From: Xiaocheng <2544262366@qq.com> Date: Sat, 24 Jan 2026 01:37:21 +0800 Subject: [PATCH] 111 --- Core/Src/stm32f4xx_it.c | 1 + User/bsp/bsp_config.yaml | 2 + User/bsp/gpio.c | 24 +++- User/bsp/uart.c | 4 + User/bsp/uart.h | 1 + User/component/user_math.c | 16 +-- User/device/dr16.c | 258 ++++++++++++++++++------------------- User/device/dr16.h | 192 +++++++++++++-------------- User/device/motor_rm.c | 6 +- User/module/cmd.h | 2 + User/module/config.c | 2 +- User/module/gimbal.c | 36 ++++-- User/task/Task8.c | 10 +- User/task/init.c | 4 +- User/task/rc.c | 2 + User/task/user_task.h | 1 + 16 files changed, 306 insertions(+), 255 deletions(-) diff --git a/Core/Src/stm32f4xx_it.c b/Core/Src/stm32f4xx_it.c index 5e51a6d..ae4298e 100644 --- a/Core/Src/stm32f4xx_it.c +++ b/Core/Src/stm32f4xx_it.c @@ -287,6 +287,7 @@ void USART1_IRQHandler(void) /* USER CODE END USART1_IRQn 0 */ HAL_UART_IRQHandler(&huart1); /* USER CODE BEGIN USART1_IRQn 1 */ + BSP_UART_IRQHandler(&huart1); /* USER CODE END USART1_IRQn 1 */ } diff --git a/User/bsp/bsp_config.yaml b/User/bsp/bsp_config.yaml index 11ce6e6..6bb31db 100644 --- a/User/bsp/bsp_config.yaml +++ b/User/bsp/bsp_config.yaml @@ -72,4 +72,6 @@ uart: name: DR16 - instance: USART6 name: NUC + - instance: USART1 + name: '1' enabled: true diff --git a/User/bsp/gpio.c b/User/bsp/gpio.c index 2e26478..b8fb1ad 100644 --- a/User/bsp/gpio.c +++ b/User/bsp/gpio.c @@ -73,14 +73,20 @@ int8_t BSP_GPIO_RegisterCallback(BSP_GPIO_t gpio, void (*callback)(void)) { int8_t BSP_GPIO_EnableIRQ(BSP_GPIO_t gpio) { switch (gpio) { case BSP_GPIO_KEY: +#if defined(KEY_EXTI_IRQn) HAL_NVIC_EnableIRQ(KEY_EXTI_IRQn); - break; +#endif + return BSP_OK; case BSP_GPIO_ACCL_INT: +#if defined(ACCL_INT_EXTI_IRQn) HAL_NVIC_EnableIRQ(ACCL_INT_EXTI_IRQn); - break; +#endif + return BSP_OK; case BSP_GPIO_GYRO_INT: +#if defined(GYRO_INT_EXTI_IRQn) HAL_NVIC_EnableIRQ(GYRO_INT_EXTI_IRQn); - break; +#endif + return BSP_OK; default: return BSP_ERR; } @@ -90,14 +96,20 @@ int8_t BSP_GPIO_EnableIRQ(BSP_GPIO_t gpio) { int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio) { switch (gpio) { case BSP_GPIO_KEY: +#if defined(KEY_EXTI_IRQn) HAL_NVIC_DisableIRQ(KEY_EXTI_IRQn); - break; +#endif + return BSP_OK; case BSP_GPIO_ACCL_INT: +#if defined(ACCL_INT_EXTI_IRQn) HAL_NVIC_DisableIRQ(ACCL_INT_EXTI_IRQn); - break; +#endif + return BSP_OK; case BSP_GPIO_GYRO_INT: +#if defined(GYRO_INT_EXTI_IRQn) HAL_NVIC_DisableIRQ(GYRO_INT_EXTI_IRQn); - break; +#endif + return BSP_OK; default: return BSP_ERR; } diff --git a/User/bsp/uart.c b/User/bsp/uart.c index 205d9df..4e62a04 100644 --- a/User/bsp/uart.c +++ b/User/bsp/uart.c @@ -27,6 +27,8 @@ static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) { return BSP_UART_DR16; else if (huart->Instance == USART6) return BSP_UART_NUC; + else if (huart->Instance == USART1) + return BSP_UART_1; else return BSP_UART_ERR; } @@ -119,6 +121,8 @@ UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) { return &huart3; case BSP_UART_NUC: return &huart6; + case BSP_UART_1: + return &huart1; default: return NULL; } diff --git a/User/bsp/uart.h b/User/bsp/uart.h index f5fabfc..1c880ba 100644 --- a/User/bsp/uart.h +++ b/User/bsp/uart.h @@ -29,6 +29,7 @@ extern "C" { typedef enum { BSP_UART_DR16, BSP_UART_NUC, + BSP_UART_1, BSP_UART_NUM, BSP_UART_ERR, } BSP_UART_t; diff --git a/User/component/user_math.c b/User/component/user_math.c index 5237262..34e1581 100644 --- a/User/component/user_math.c +++ b/User/component/user_math.c @@ -12,14 +12,14 @@ inline float InvSqrt(float x) { //#if 0 /* Fast inverse square-root */ /* See: http://en.wikipedia.org/wiki/Fast_inverse_square_root */ - float halfx = 0.5f * x; - float y = x; - long i = *(long*)&y; - i = 0x5f3759df - (i>>1); - y = *(float*)&i; - y = y * (1.5f - (halfx * y * y)); - y = y * (1.5f - (halfx * y * y)); - return y; + float halfx = 0.5f * x; + float y = x; + long i = *(long*)&y; + i = 0x5f3759df - (i>>1); + y = *(float*)&i; + y = y * (1.5f - (halfx * y * y)); + y = y * (1.5f - (halfx * y * y)); + return y; //#else // return 1.0f / sqrtf(x); //#endif diff --git a/User/device/dr16.c b/User/device/dr16.c index bc0567e..b0bf495 100644 --- a/User/device/dr16.c +++ b/User/device/dr16.c @@ -1,169 +1,169 @@ -///* -// DR16接收机 -// Example： +// /* +// DR16接收机 +// Example： -// DR16_Init(&dr16); -// -// while (1) { -// DR16_StartDmaRecv(&dr16); -// if (DR16_WaitDmaCplt(20)) { -// DR16_ParseData(&dr16); -// } else { -// DR16_Offline(&dr16); -// } -//} -//*/ +// DR16_Init(&dr16); + +// while (1) { +// DR16_StartDmaRecv(&dr16); +// if (DR16_WaitDmaCplt(20)) { +// DR16_ParseData(&dr16); +// } else { +// DR16_Offline(&dr16); +// } +// } +// */ -///* Includes ----------------------------------------------------------------- */ -//#include "dr16.h" -//#include "bsp/uart.h" -//#include "bsp/time.h" -//#include "device.h" +// /* Includes ----------------------------------------------------------------- */ +// #include "dr16.h" +// #include "bsp/uart.h" +// #include "bsp/time.h" +// #include "device.h" -//#include -//#include +// #include +// #include -///* USER INCLUDE BEGIN */ +// /* USER INCLUDE BEGIN */ -///* USER INCLUDE END */ -///* Private define ----------------------------------------------------------- */ -//#define DR16_CH_VALUE_MIN (364u) -//#define DR16_CH_VALUE_MID (1024u) -//#define DR16_CH_VALUE_MAX (1684u) +// /* USER INCLUDE END */ +// /* Private define ----------------------------------------------------------- */ +// #define DR16_CH_VALUE_MIN (364u) +// #define DR16_CH_VALUE_MID (1024u) +// #define DR16_CH_VALUE_MAX (1684u) -///* USER DEFINE BEGIN */ +// /* USER DEFINE BEGIN */ -///* USER DEFINE END */ +// /* USER DEFINE END */ -///* Private macro ------------------------------------------------------------ */ -///* Private typedef ---------------------------------------------------------- */ -///* Private variables -------------------------------------------------------- */ +// /* Private macro ------------------------------------------------------------ */ +// /* Private typedef ---------------------------------------------------------- */ +// /* Private variables -------------------------------------------------------- */ -//static osThreadId_t thread_alert; -//static bool inited = false; +// static osThreadId_t thread_alert; +// static bool inited = false; -///* Private function -------------------------------------------------------- */ -//static void DR16_RxCpltCallback(void) { -// osThreadFlagsSet(thread_alert, SIGNAL_DR16_RAW_REDY); -//} +// /* Private function -------------------------------------------------------- */ +// static void DR16_RxCpltCallback(void) { +// osThreadFlagsSet(thread_alert, SIGNAL_DR16_RAW_REDY); +// } -//static bool DR16_DataCorrupted(const DR16_t *dr16) { -// if (dr16 == NULL) return DEVICE_ERR_NULL; +// static bool DR16_DataCorrupted(const DR16_t *dr16) { +// if (dr16 == NULL) return DEVICE_ERR_NULL; -// if ((dr16->raw_data.ch_r_x < DR16_CH_VALUE_MIN) || -// (dr16->raw_data.ch_r_x > DR16_CH_VALUE_MAX)) -// return DEVICE_ERR; +// if ((dr16->raw_data.ch_r_x < DR16_CH_VALUE_MIN) || +// (dr16->raw_data.ch_r_x > DR16_CH_VALUE_MAX)) +// return DEVICE_ERR; -// if ((dr16->raw_data.ch_r_y < DR16_CH_VALUE_MIN) || -// (dr16->raw_data.ch_r_y > DR16_CH_VALUE_MAX)) -// return DEVICE_ERR; +// if ((dr16->raw_data.ch_r_y < DR16_CH_VALUE_MIN) || +// (dr16->raw_data.ch_r_y > DR16_CH_VALUE_MAX)) +// return DEVICE_ERR; -// if ((dr16->raw_data.ch_l_x < DR16_CH_VALUE_MIN) || -// (dr16->raw_data.ch_l_x > DR16_CH_VALUE_MAX)) -// return DEVICE_ERR; +// if ((dr16->raw_data.ch_l_x < DR16_CH_VALUE_MIN) || +// (dr16->raw_data.ch_l_x > DR16_CH_VALUE_MAX)) +// return DEVICE_ERR; -// if ((dr16->raw_data.ch_l_y < DR16_CH_VALUE_MIN) || -// (dr16->raw_data.ch_l_y > DR16_CH_VALUE_MAX)) -// return DEVICE_ERR; +// if ((dr16->raw_data.ch_l_y < DR16_CH_VALUE_MIN) || +// (dr16->raw_data.ch_l_y > DR16_CH_VALUE_MAX)) +// return DEVICE_ERR; -// if (dr16->raw_data.sw_l == 0) return DEVICE_ERR; +// if (dr16->raw_data.sw_l == 0) return DEVICE_ERR; -// if (dr16->raw_data.sw_r == 0) return DEVICE_ERR; +// if (dr16->raw_data.sw_r == 0) return DEVICE_ERR; -// return DEVICE_OK; -//} +// return DEVICE_OK; +// } -///* Exported functions ------------------------------------------------------- */ -//int8_t DR16_Init(DR16_t *dr16) { -// if (dr16 == NULL) return DEVICE_ERR_NULL; -// if (inited) return DEVICE_ERR_INITED; -// if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL; +// /* Exported functions ------------------------------------------------------- */ +// int8_t DR16_Init(DR16_t *dr16) { +// if (dr16 == NULL) return DEVICE_ERR_NULL; +// if (inited) return DEVICE_ERR_INITED; +// if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL; -// BSP_UART_RegisterCallback(BSP_UART_DR16, BSP_UART_RX_CPLT_CB, -// DR16_RxCpltCallback); +// BSP_UART_RegisterCallback(BSP_UART_DR16, BSP_UART_RX_CPLT_CB, +// DR16_RxCpltCallback); -// inited = true; -// return DEVICE_OK; -//} +// inited = true; +// return DEVICE_OK; +// } -//int8_t DR16_Restart(void) { -// __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_DR16)); -// __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_DR16)); -// return DEVICE_OK; -//} +// int8_t DR16_Restart(void) { +// __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_DR16)); +// __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_DR16)); +// return DEVICE_OK; +// } -//int8_t DR16_StartDmaRecv(DR16_t *dr16) { -// if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_DR16), -// (uint8_t *)&(dr16->raw_data), -// sizeof(dr16->raw_data)) == HAL_OK) -// return DEVICE_OK; -// return DEVICE_ERR; -//} +// int8_t DR16_StartDmaRecv(DR16_t *dr16) { +// if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_DR16), +// (uint8_t *)&(dr16->raw_data), +// sizeof(dr16->raw_data)) == HAL_OK) +// return DEVICE_OK; +// return DEVICE_ERR; +// } -//bool DR16_WaitDmaCplt(uint32_t timeout) { -// return (osThreadFlagsWait(SIGNAL_DR16_RAW_REDY, osFlagsWaitAll, timeout) == -// SIGNAL_DR16_RAW_REDY); -//} +// bool DR16_WaitDmaCplt(uint32_t timeout) { +// return (osThreadFlagsWait(SIGNAL_DR16_RAW_REDY, osFlagsWaitAll, timeout) == +// SIGNAL_DR16_RAW_REDY); +// } -//int8_t DR16_ParseData(DR16_t *dr16){ -// if (dr16 == NULL) return DEVICE_ERR_NULL; +// int8_t DR16_ParseData(DR16_t *dr16){ +// if (dr16 == NULL) return DEVICE_ERR_NULL; -// if (DR16_DataCorrupted(dr16)) { -// return DEVICE_ERR; -// } +// if (DR16_DataCorrupted(dr16)) { +// return DEVICE_ERR; +// } -// dr16->header.online = true; -// dr16->header.last_online_time = BSP_TIME_Get_us(); -// -// memset(&(dr16->data), 0, sizeof(dr16->data)); +// dr16->header.online = true; +// dr16->header.last_online_time = BSP_TIME_Get_us(); + +// memset(&(dr16->data), 0, sizeof(dr16->data)); -// float full_range = (float)(DR16_CH_VALUE_MAX - DR16_CH_VALUE_MIN); +// float full_range = (float)(DR16_CH_VALUE_MAX - DR16_CH_VALUE_MIN); -// // 解析摇杆数据 -// dr16->data.ch_r_x = 2.0f * ((float)dr16->raw_data.ch_r_x - DR16_CH_VALUE_MID) / full_range; -// dr16->data.ch_r_y = 2.0f * ((float)dr16->raw_data.ch_r_y - DR16_CH_VALUE_MID) / full_range; -// dr16->data.ch_l_x = 2.0f * ((float)dr16->raw_data.ch_l_x - DR16_CH_VALUE_MID) / full_range; -// dr16->data.ch_l_y = 2.0f * ((float)dr16->raw_data.ch_l_y - DR16_CH_VALUE_MID) / full_range; +// // 解析摇杆数据 +// dr16->data.ch_r_x = 2.0f * ((float)dr16->raw_data.ch_r_x - DR16_CH_VALUE_MID) / full_range; +// dr16->data.ch_r_y = 2.0f * ((float)dr16->raw_data.ch_r_y - DR16_CH_VALUE_MID) / full_range; +// dr16->data.ch_l_x = 2.0f * ((float)dr16->raw_data.ch_l_x - DR16_CH_VALUE_MID) / full_range; +// dr16->data.ch_l_y = 2.0f * ((float)dr16->raw_data.ch_l_y - DR16_CH_VALUE_MID) / full_range; -// // 解析拨杆位置 -// dr16->data.sw_l = (DR16_SwitchPos_t)dr16->raw_data.sw_l; -// dr16->data.sw_r = (DR16_SwitchPos_t)dr16->raw_data.sw_r; +// // 解析拨杆位置 +// dr16->data.sw_l = (DR16_SwitchPos_t)dr16->raw_data.sw_l; +// dr16->data.sw_r = (DR16_SwitchPos_t)dr16->raw_data.sw_r; -// // 解析鼠标数据 -// dr16->data.mouse.x = dr16->raw_data.x; -// dr16->data.mouse.y = dr16->raw_data.y; -// dr16->data.mouse.z = dr16->raw_data.z; +// // 解析鼠标数据 +// dr16->data.mouse.x = dr16->raw_data.x; +// dr16->data.mouse.y = dr16->raw_data.y; +// dr16->data.mouse.z = dr16->raw_data.z; -// dr16->data.mouse.l_click = dr16->raw_data.press_l; -// dr16->data.mouse.r_click = dr16->raw_data.press_r; +// dr16->data.mouse.l_click = dr16->raw_data.press_l; +// dr16->data.mouse.r_click = dr16->raw_data.press_r; -// // 解析键盘按键 - 使用union简化代码 -// uint16_t key_value = dr16->raw_data.key; -// -// // 解析键盘位映射（W-B键，位0-15） -// for (int i = DR16_KEY_W; i <= DR16_KEY_B; i++) { -// dr16->data.keyboard.key[i] = (key_value & (1 << i)) != 0; -// } +// // 解析键盘按键 - 使用union简化代码 +// uint16_t key_value = dr16->raw_data.key; + +// // 解析键盘位映射（W-B键，位0-15） +// for (int i = DR16_KEY_W; i <= DR16_KEY_B; i++) { +// dr16->data.keyboard.key[i] = (key_value & (1 << i)) != 0; +// } -// // 解析鼠标点击 -// dr16->data.keyboard.key[DR16_L_CLICK] = dr16->data.mouse.l_click; -// dr16->data.keyboard.key[DR16_R_CLICK] = dr16->data.mouse.r_click; +// // 解析鼠标点击 +// dr16->data.keyboard.key[DR16_L_CLICK] = dr16->data.mouse.l_click; +// dr16->data.keyboard.key[DR16_R_CLICK] = dr16->data.mouse.r_click; -// // 解析第五通道 -// dr16->data.ch_res = 2.0f * ((float)dr16->raw_data.res - DR16_CH_VALUE_MID) / full_range; -// -// return DEVICE_OK; -//} +// // 解析第五通道 +// dr16->data.ch_res = 2.0f * ((float)dr16->raw_data.res - DR16_CH_VALUE_MID) / full_range; + +// return DEVICE_OK; +// } -//int8_t DR16_Offline(DR16_t *dr16){ -// if (dr16 == NULL) return DEVICE_ERR_NULL; +// int8_t DR16_Offline(DR16_t *dr16){ +// if (dr16 == NULL) return DEVICE_ERR_NULL; -// dr16->header.online = false; -// memset(&(dr16->data), 0, sizeof(dr16->data)); +// dr16->header.online = false; +// memset(&(dr16->data), 0, sizeof(dr16->data)); -// return DEVICE_OK; -//} +// return DEVICE_OK; +// } -///* USER FUNCTION BEGIN */ +// /* USER FUNCTION BEGIN */ -///* USER FUNCTION END */ +// /* USER FUNCTION END */ diff --git a/User/device/dr16.h b/User/device/dr16.h index e19497c..0148f43 100644 --- a/User/device/dr16.h +++ b/User/device/dr16.h @@ -1,117 +1,117 @@ -//#pragma once +// #pragma once -//#ifdef __cplusplus -//extern "C" { -//#endif +// #ifdef __cplusplus +// extern "C" { +// #endif -///* Includes ----------------------------------------------------------------- */ -//#include +// /* Includes ----------------------------------------------------------------- */ +// #include -//#include "component/user_math.h" -//#include "device.h" +// #include "component/user_math.h" +// #include "device.h" -///* USER INCLUDE BEGIN */ +// /* USER INCLUDE BEGIN */ -///* USER INCLUDE END */ +// ///* USER INCLUDE END */ -///* USER DEFINE BEGIN */ +// /* USER DEFINE BEGIN */ -///* USER DEFINE END */ +// ///* USER DEFINE END */ -///* Exported constants ------------------------------------------------------- */ -///* Exported macro ----------------------------------------------------------- */ -///* Exported types ----------------------------------------------------------- */ -//typedef struct __packed { -// uint16_t ch_r_x : 11; -// uint16_t ch_r_y : 11; -// uint16_t ch_l_x : 11; -// uint16_t ch_l_y : 11; -// uint8_t sw_r : 2; -// uint8_t sw_l : 2; -// int16_t x; -// int16_t y; -// int16_t z; -// uint8_t press_l; -// uint8_t press_r; -// uint16_t key; -// uint16_t res; -//} DR16_RawData_t; +// /* Exported constants ------------------------------------------------------- */ +// /* Exported macro ----------------------------------------------------------- */ +// /* Exported types ----------------------------------------------------------- */ +// typedef struct __packed { +// uint16_t ch_r_x : 11; +// uint16_t ch_r_y : 11; +// uint16_t ch_l_x : 11; +// uint16_t ch_l_y : 11; +// uint8_t sw_r : 2; +// uint8_t sw_l : 2; +// int16_t x; +// int16_t y; +// int16_t z; +// uint8_t press_l; +// uint8_t press_r; +// uint16_t key; +// uint16_t res; +// } DR16_RawData_t; -//typedef enum { -// DR16_SW_ERR = 0, -// DR16_SW_UP = 1, -// DR16_SW_MID = 3, -// DR16_SW_DOWN = 2, -//} DR16_SwitchPos_t; +// typedef enum { +// DR16_SW_ERR = 0, +// DR16_SW_UP = 1, +// DR16_SW_MID = 3, +// DR16_SW_DOWN = 2, +// } DR16_SwitchPos_t; -///* 键盘按键值 */ -//typedef enum { -// DR16_KEY_W = 0, -// DR16_KEY_S, -// DR16_KEY_A, -// DR16_KEY_D, -// DR16_KEY_SHIFT, -// DR16_KEY_CTRL, -// DR16_KEY_Q, -// DR16_KEY_E, -// DR16_KEY_R, -// DR16_KEY_F, -// DR16_KEY_G, -// DR16_KEY_Z, -// DR16_KEY_X, -// DR16_KEY_C, -// DR16_KEY_V, -// DR16_KEY_B, -// DR16_L_CLICK, -// DR16_R_CLICK, -// DR16_KEY_NUM, -//} DR16_Key_t; +// /* 键盘按键值 */ +// typedef enum { +// DR16_KEY_W = 0, +// DR16_KEY_S, +// DR16_KEY_A, +// DR16_KEY_D, +// DR16_KEY_SHIFT, +// DR16_KEY_CTRL, +// DR16_KEY_Q, +// DR16_KEY_E, +// DR16_KEY_R, +// DR16_KEY_F, +// DR16_KEY_G, +// DR16_KEY_Z, +// DR16_KEY_X, +// DR16_KEY_C, +// DR16_KEY_V, +// DR16_KEY_B, +// DR16_L_CLICK, +// DR16_R_CLICK, +// DR16_KEY_NUM, +// } DR16_Key_t; -//typedef struct { -// float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */ -// float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */ -// float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */ -// float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */ +// typedef struct { +// float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */ +// float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */ +// float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */ +// float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */ -// float ch_res; /* 第五通道值 */ +// float ch_res; /* 第五通道值 */ -// DR16_SwitchPos_t sw_r; /* 右侧拨杆位置 */ -// DR16_SwitchPos_t sw_l; /* 左侧拨杆位置 */ +// DR16_SwitchPos_t sw_r; /* 右侧拨杆位置 */ +// DR16_SwitchPos_t sw_l; /* 左侧拨杆位置 */ -// struct { -// int16_t x; -// int16_t y; -// int16_t z; -// bool l_click; /* 左键 */ -// bool r_click; /* 右键 */ -// } mouse; /* 鼠标值 */ +// struct { +// int16_t x; +// int16_t y; +// int16_t z; +// bool l_click; /* 左键 */ +// bool r_click; /* 右键 */ +// } mouse; /* 鼠标值 */ -// union { -// bool key[DR16_KEY_NUM]; /* 键盘按键值 */ -// uint16_t value; /* 键盘按键值的位映射 */ -// } keyboard; +// union { +// bool key[DR16_KEY_NUM]; /* 键盘按键值 */ +// uint16_t value; /* 键盘按键值的位映射 */ +// } keyboard; -// uint16_t res; /* 保留，未启用 */ -//} DR16_Data_t; +// uint16_t res; /* 保留，未启用 */ +// } DR16_Data_t; -//typedef struct { -// DEVICE_Header_t header; -// DR16_RawData_t raw_data; -// DR16_Data_t data; -//} DR16_t; +// typedef struct { +// DEVICE_Header_t header; +// DR16_RawData_t raw_data; +// DR16_Data_t data; +// } DR16_t; -///* Exported functions prototypes -------------------------------------------- */ -//int8_t DR16_Init(DR16_t *dr16); -//int8_t DR16_Restart(void); -//int8_t DR16_StartDmaRecv(DR16_t *dr16); -//bool DR16_WaitDmaCplt(uint32_t timeout); -//int8_t DR16_ParseData(DR16_t *dr16); -//int8_t DR16_Offline(DR16_t *dr16); +// /* Exported functions prototypes -------------------------------------------- */ +// int8_t DR16_Init(DR16_t *dr16); +// int8_t DR16_Restart(void); +// int8_t DR16_StartDmaRecv(DR16_t *dr16); +// bool DR16_WaitDmaCplt(uint32_t timeout); +// int8_t DR16_ParseData(DR16_t *dr16); +// int8_t DR16_Offline(DR16_t *dr16); -///* USER FUNCTION BEGIN */ +// /* USER FUNCTION BEGIN */ -///* USER FUNCTION END */ +// ///* USER FUNCTION END */ -//#ifdef __cplusplus -//} -//#endif +// #ifdef __cplusplus +// } +// #endif diff --git a/User/device/motor_rm.c b/User/device/motor_rm.c index 4bf3263..88c1ef4 100644 --- a/User/device/motor_rm.c +++ b/User/device/motor_rm.c @@ -56,12 +56,12 @@ static int8_t MOTOR_RM_GetLogicalIndex(uint16_t can_id, MOTOR_RM_Module_t module switch (module) { case MOTOR_M2006: case MOTOR_M3508: - if (can_id >= M3508_M2006_FB_ID_BASE && can_id < M3508_M2006_FB_ID_BASE + 7) { + if (can_id >= M3508_M2006_FB_ID_BASE && can_id <= M3508_M2006_FB_ID_BASE + 7) { return can_id - M3508_M2006_FB_ID_BASE; } break; case MOTOR_GM6020: - if (can_id >= GM6020_FB_ID_BASE && can_id < GM6020_FB_ID_BASE + 6) { + if (can_id >= GM6020_FB_ID_BASE && can_id <= GM6020_FB_ID_BASE + 6) { return can_id - GM6020_FB_ID_BASE + 4; } break; @@ -74,7 +74,7 @@ static int8_t MOTOR_RM_GetLogicalIndex(uint16_t can_id, MOTOR_RM_Module_t module static float MOTOR_RM_GetRatio(MOTOR_RM_Module_t module) { switch (module) { case MOTOR_M2006: return 36.0f; - case MOTOR_M3508: return 268.0f/17.0f ; // 通用3508减速比3591.0f / 187.0f;前面的是舵轮减速箱减速比 + case MOTOR_M3508: return 3591.0f / 187.0f; case MOTOR_GM6020: return 1.0f; default: return 1.0f; } diff --git a/User/module/cmd.h b/User/module/cmd.h index cd9090e..37ef958 100644 --- a/User/module/cmd.h +++ b/User/module/cmd.h @@ -47,6 +47,8 @@ typedef struct struct { + + float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */ float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */ float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */ diff --git a/User/module/config.c b/User/module/config.c index 3ccd029..0d5c720 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -145,7 +145,7 @@ static const Config_Param_t config = { }, .pid.yaw_4310_motor_omega = { - .k = 0.0f, + .k = 0.1f, .p = 0.3f, .i = 0.0f, .d = 0.0f, diff --git a/User/module/gimbal.c b/User/module/gimbal.c index 230f9e9..47a198f 100644 --- a/User/module/gimbal.c +++ b/User/module/gimbal.c @@ -115,6 +115,7 @@ int8_t Gimbal_Init(Gimbal_t *g, Gimbal_Param_t *param,float target_freq) PID_Init(&g->pid.yaw_6020_omega,KPID_MODE_CALC_D, target_freq,&g->param->pid.yaw_6020_motor_omega ); /*4310大yaw pid初始化 大yaw单环控*/ PID_Init(&g->pid.yaw_4310_angle,KPID_MODE_SET_D, target_freq,&g->param->pid.yaw_4310_motor_angle ); + PID_Init(&g->pid.yaw_4310_omega,KPID_MODE_CALC_D, target_freq,&g->param->pid.yaw_4310_motor_omega ); /*4310 pitch pid初始化 单环*/ PID_Init(&g->pid.pitch_4310_angle,KPID_MODE_NO_D, target_freq,&g->param->pid.pitch_4310_motor_angle ); PID_Init(&g->pid.pitch_4310_omega,KPID_MODE_CALC_D, target_freq,&g->param->pid.pitch_4310_motor_omega ); @@ -246,7 +247,7 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){ Gimbal_SetMode(g, g_cmd->mode); /*处理小yaw轴控制命令*/ -float delta_yaw_6020 = 5.0f*g_cmd->delta_yaw_6020 * g->dt; +float delta_yaw_6020 = 10.0f*g_cmd->delta_yaw_6020 * g->dt; if(g->param->travel.yaw_6020 > 0) // 有限位才处理 { @@ -276,7 +277,7 @@ const float delta_min_6020 = CircleError(g->limit.yaw_6020.min, g->setpoint.yaw_4310=small_yaw_center_offset; /*处理大pitch控制命令*/ -float delta_pitch_4310 = 5.0f*g_cmd->delta_pitch_4310*g->dt; +float delta_pitch_4310 = 8.0f*g_cmd->delta_pitch_4310*g->dt; if(g->param->travel.pitch_4310 > 0) // 有限位才处理 { /* 计算当前电机角度与IMU角度的偏差 */ @@ -330,11 +331,29 @@ g->setpoint.eulr.pit = g_cmd->set_pitch; /*4310大YAW控制 这里是单环控制的，有需要加双环 */ - yaw_omega_set_point =PID_Calc(&g->pid.yaw_4310_angle,g->setpoint.yaw_4310, - g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle,g->feedback.motor.yaw_6020_motor_feedback.rotor_speed,g->dt); - // g->out.yaw_4310 = PID_Calc(&g->pid.yaw_4310_omega,yaw_omega_set_point, - // g->feedback.imu.gyro.z,0.0f,g->dt); - g->out.yaw_4310 = yaw_omega_set_point; // 直接输出速度环目标值作为电机输出 + // /* 大YAW角度环：使用环形误差，避免过零点抽搐 */ + // float yaw4310_err = CircleError( + // g->setpoint.yaw_4310, + // g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle, + // M_2PI + // ); + // /* 构造等效的包角设定值，使 PID 内部误差=set-ref=环形误差 */ + // float yaw4310_set_wrapped = g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle + yaw4310_err; + // yaw_omega_set_point = PID_Calc( + // &g->pid.yaw_4310_angle, + // yaw4310_set_wrapped, + // g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle, + // g->feedback.motor.yaw_6020_motor_feedback.rotor_speed, + // g->dt + // ); + +yaw_omega_set_point =PID_Calc(&g->pid.yaw_4310_angle,3.90f, + g->feedback.motor.yaw_4310_motor_feedback.rotor_abs_angle,g->feedback.motor.yaw_6020_motor_feedback.rotor_speed,g->dt); + + + g->out.yaw_4310 = PID_Calc(&g->pid.yaw_4310_omega,yaw_omega_set_point, + g->feedback.motor.yaw_4310_motor_feedback.rotor_speed,0.0f,g->dt); + // g->out.yaw_4310 = yaw_omega_set_point; // 直接输出速度环目标值作为电机输出 /*控制云台4310电机 也是单环，但是加了重力补偿函数，可以根据不一样的情况去拟合函数*/ pitch_omega_set_point =PID_Calc(&g->pid.pitch_4310_angle,g->setpoint.eulr.pit, g->feedback.imu.eulr.rol,0.0f,g->dt); @@ -376,7 +395,8 @@ void Gimbal_Output(Gimbal_t *g) MOTOR_RM_SetOutput(&g->param->yaw_6020_motor, g->out.yaw_6020); MOTOR_MIT_Output_t output_yaw_4310 = {0}; - output_yaw_4310.torque = g->out.yaw_4310 * 1.0f; + output_yaw_4310.torque = g->out.yaw_4310 * 2.0f; + // output_yaw_4310.kp = 0.2f; output_yaw_4310.kd = 0.1f; // output_yaw_4310.kd=0.1f; diff --git a/User/task/Task8.c b/User/task/Task8.c index 17446e5..ea6f784 100644 --- a/User/task/Task8.c +++ b/User/task/Task8.c @@ -6,7 +6,8 @@ /* Includes ----------------------------------------------------------------- */ #include "task/user_task.h" /* USER INCLUDE BEGIN */ - +#include "module/cmd.h" +#include "bsp/uart.h" /* USER INCLUDE END */ /* Private typedef ---------------------------------------------------------- */ @@ -14,7 +15,7 @@ /* Private macro ------------------------------------------------------------ */ /* Private variables -------------------------------------------------------- */ /* USER STRUCT BEGIN */ - +CMD_RC_t remote; /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ @@ -36,6 +37,11 @@ void Task_Task8(void *argument) { while (1) { tick += delay_tick; /* 计算下一个唤醒时刻 */ /* USER CODE BEGIN */ + osMessageQueueGet(task_runtime.msgq.RC_REMOTE, &remote, NULL, 0); + + + BSP_UART_Transmit(BSP_UART_1, (uint8_t *)&remote, sizeof(CMD_RC_t), false); + /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ diff --git a/User/task/init.c b/User/task/init.c index d1d8b48..634efb0 100644 --- a/User/task/init.c +++ b/User/task/init.c @@ -37,7 +37,7 @@ void Task_Init(void *argument) { task_runtime.thread.cmd = osThreadNew(Task_cmd, NULL, &attr_cmd); task_runtime.thread.gimbal = osThreadNew(Task_gimbal, NULL, &attr_gimbal); task_runtime.thread.shoot = osThreadNew(Task_shoot, NULL, &attr_shoot); - task_runtime.thread.ai = osThreadNew(Task_ai, NULL, &attr_ai); + // task_runtime.thread.ai = osThreadNew(Task_ai, NULL, &attr_ai); task_runtime.thread.Task8 = osThreadNew(Task_Task8, NULL, &attr_Task8); // 创建消息队列 @@ -55,7 +55,7 @@ void Task_Init(void *argument) { task_runtime.msgq.gimbal.ai.feedback = osMessageQueueNew(2u, sizeof(Gimbal_feedback_t),NULL); task_runtime.msgq.gimbal.ai.g_cmd = osMessageQueueNew(2u, sizeof(AI_cmd_t),NULL); task_runtime.msgq.shoot.ai.s_cmd = osMessageQueueNew(2u, sizeof(AI_cmd_t),NULL); - + task_runtime.msgq.RC_REMOTE = osMessageQueueNew(2u, sizeof(CMD_RC_t), NULL); /* USER MESSAGE END */ osKernelUnlock(); // 解锁内核 diff --git a/User/task/rc.c b/User/task/rc.c index f555b9d..30d1dac 100644 --- a/User/task/rc.c +++ b/User/task/rc.c @@ -60,6 +60,8 @@ void Task_rc(void *argument) { osMessageQueueReset(task_runtime.msgq.cmd.raw.rc); osMessageQueuePut(task_runtime.msgq.cmd.raw.rc, &cmd_rc, 0, 0); + osMessageQueueReset(task_runtime.msgq.RC_REMOTE); + osMessageQueuePut(task_runtime.msgq.RC_REMOTE, &cmd_rc, 0, 0); /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ diff --git a/User/task/user_task.h b/User/task/user_task.h index f406673..7773c2d 100644 --- a/User/task/user_task.h +++ b/User/task/user_task.h @@ -105,6 +105,7 @@ typedef struct { }ai; }shoot; + osMessageQueueId_t RC_REMOTE; } msgq; /* USER MESSAGE END */