Compare commits
2 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 5bf4f99113 | |||
| 72a4e5b073 |
@ -71,13 +71,13 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
|
||||
User/component/user_math.c
|
||||
|
||||
# User/device sources
|
||||
User/device/ai.c
|
||||
User/device/bmi088.c
|
||||
User/device/dr16.c
|
||||
User/device/motor.c
|
||||
User/device/motor_dm.c
|
||||
User/device/motor_rm.c
|
||||
User/device/remote_control.c
|
||||
User/device/ai.c
|
||||
|
||||
# User/module sources
|
||||
User/module/chassis.c
|
||||
@ -87,8 +87,6 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
|
||||
User/module/shoot.c
|
||||
|
||||
# User/task sources
|
||||
User/task/Task8.c
|
||||
User/task/ai.c
|
||||
User/task/atti_esti.c
|
||||
User/task/chassis.c
|
||||
User/task/cmd.c
|
||||
@ -96,8 +94,9 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
|
||||
User/task/init.c
|
||||
User/task/rc.c
|
||||
User/task/shoot.c
|
||||
User/task/tempCodeRunnerFile.c
|
||||
User/task/user_task.c
|
||||
User/task/ai.c
|
||||
User/task/Task8.c
|
||||
)
|
||||
|
||||
# Add include paths
|
||||
|
||||
@ -287,7 +287,6 @@ 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 */
|
||||
}
|
||||
|
||||
@ -72,6 +72,4 @@ uart:
|
||||
name: DR16
|
||||
- instance: USART6
|
||||
name: NUC
|
||||
- instance: USART1
|
||||
name: '1'
|
||||
enabled: true
|
||||
|
||||
@ -73,20 +73,14 @@ 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);
|
||||
#endif
|
||||
return BSP_OK;
|
||||
break;
|
||||
case BSP_GPIO_ACCL_INT:
|
||||
#if defined(ACCL_INT_EXTI_IRQn)
|
||||
HAL_NVIC_EnableIRQ(ACCL_INT_EXTI_IRQn);
|
||||
#endif
|
||||
return BSP_OK;
|
||||
break;
|
||||
case BSP_GPIO_GYRO_INT:
|
||||
#if defined(GYRO_INT_EXTI_IRQn)
|
||||
HAL_NVIC_EnableIRQ(GYRO_INT_EXTI_IRQn);
|
||||
#endif
|
||||
return BSP_OK;
|
||||
break;
|
||||
default:
|
||||
return BSP_ERR;
|
||||
}
|
||||
@ -96,20 +90,14 @@ 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);
|
||||
#endif
|
||||
return BSP_OK;
|
||||
break;
|
||||
case BSP_GPIO_ACCL_INT:
|
||||
#if defined(ACCL_INT_EXTI_IRQn)
|
||||
HAL_NVIC_DisableIRQ(ACCL_INT_EXTI_IRQn);
|
||||
#endif
|
||||
return BSP_OK;
|
||||
break;
|
||||
case BSP_GPIO_GYRO_INT:
|
||||
#if defined(GYRO_INT_EXTI_IRQn)
|
||||
HAL_NVIC_DisableIRQ(GYRO_INT_EXTI_IRQn);
|
||||
#endif
|
||||
return BSP_OK;
|
||||
break;
|
||||
default:
|
||||
return BSP_ERR;
|
||||
}
|
||||
|
||||
@ -27,8 +27,6 @@ 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;
|
||||
}
|
||||
@ -121,8 +119,6 @@ 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;
|
||||
}
|
||||
|
||||
@ -29,7 +29,6 @@ extern "C" {
|
||||
typedef enum {
|
||||
BSP_UART_DR16,
|
||||
BSP_UART_NUC,
|
||||
BSP_UART_1,
|
||||
BSP_UART_NUM,
|
||||
BSP_UART_ERR,
|
||||
} BSP_UART_t;
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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 <string.h>
|
||||
// #include <stdbool.h>
|
||||
//#include <string.h>
|
||||
//#include <stdbool.h>
|
||||
|
||||
// /* 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 */
|
||||
|
||||
@ -1,117 +1,117 @@
|
||||
// #pragma once
|
||||
//#pragma once
|
||||
|
||||
// #ifdef __cplusplus
|
||||
// extern "C" {
|
||||
// #endif
|
||||
//#ifdef __cplusplus
|
||||
//extern "C" {
|
||||
//#endif
|
||||
|
||||
// /* Includes ----------------------------------------------------------------- */
|
||||
// #include <cmsis_os2.h>
|
||||
///* Includes ----------------------------------------------------------------- */
|
||||
//#include <cmsis_os2.h>
|
||||
|
||||
// #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
|
||||
|
||||
@ -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 3591.0f / 187.0f;
|
||||
case MOTOR_M3508: return 268.0f/17.0f ; // 通用3508减速比3591.0f / 187.0f;前面的是舵轮减速箱减速比
|
||||
case MOTOR_GM6020: return 1.0f;
|
||||
default: return 1.0f;
|
||||
}
|
||||
|
||||
@ -146,19 +146,19 @@ int8_t CMD_CtrlSet(Chassis_CMD_t *c_cmd, const CMD_RC_t *rc, Gimbal_CMD_t *g_cmd
|
||||
{
|
||||
case CMD_SW_UP:
|
||||
g_cmd->ctrl_mode = GIMBAL_MODE_REMOTE;
|
||||
s_cmd->control_mode = SHOOT_REMOTE;
|
||||
s_cmd->control_mode=SHOOT_REMOTE;
|
||||
break;
|
||||
case CMD_SW_MID:
|
||||
g_cmd->ctrl_mode = GIMBAL_MODE_AI;
|
||||
s_cmd->control_mode = SHOOT_AI;
|
||||
s_cmd->control_mode=SHOOT_AI;
|
||||
break;
|
||||
case CMD_SW_DOWN:
|
||||
g_cmd->ctrl_mode = GIMBAL_MODE_AI;
|
||||
s_cmd->control_mode = SHOOT_AI;
|
||||
s_cmd->control_mode=SHOOT_AI;
|
||||
break;
|
||||
case CMD_SW_ERR:
|
||||
g_cmd->ctrl_mode = GIMBAL_MODE_REMOTE;
|
||||
s_cmd->control_mode = SHOOT_REMOTE;
|
||||
s_cmd->control_mode=SHOOT_REMOTE;
|
||||
break;
|
||||
}
|
||||
|
||||
@ -187,7 +187,7 @@ int8_t CMD_CtrlSet(Chassis_CMD_t *c_cmd, const CMD_RC_t *rc, Gimbal_CMD_t *g_cmd
|
||||
s_cmd->mode = SHOOT_MODE_SINGLE;
|
||||
break;
|
||||
case CMD_SW_DOWN:
|
||||
s_cmd->mode = SHOOT_MODE_CONTINUE;
|
||||
s_cmd->mode = SHOOT_MODE_BURST;
|
||||
break;
|
||||
case CMD_SW_ERR:
|
||||
s_cmd->mode = SHOOT_MODE_SAFE;
|
||||
@ -195,6 +195,7 @@ int8_t CMD_CtrlSet(Chassis_CMD_t *c_cmd, const CMD_RC_t *rc, Gimbal_CMD_t *g_cmd
|
||||
}
|
||||
switch(rc->ET16s.key_D)
|
||||
{
|
||||
|
||||
case CMD_SW_UP:
|
||||
s_cmd->firecmd = false;
|
||||
s_cmd->ready = false;
|
||||
|
||||
@ -47,8 +47,6 @@ typedef struct
|
||||
|
||||
struct
|
||||
{
|
||||
|
||||
uint8_t head;
|
||||
float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */
|
||||
float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */
|
||||
float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */
|
||||
@ -63,8 +61,6 @@ typedef struct
|
||||
CMD_SwitchPos_t key_G;
|
||||
CMD_SwitchPos_t key_H;
|
||||
|
||||
|
||||
uint8_t end;
|
||||
int16_t knob_left; // 左旋钮
|
||||
int16_t knob_right; // 右旋钮
|
||||
} __attribute__((packed)) ET16s;
|
||||
@ -130,15 +126,14 @@ typedef enum {
|
||||
SHOOT_MODE_NUM
|
||||
}Shoot_Mode_t;
|
||||
|
||||
typedef enum {
|
||||
SHOOT_REMOTE = 0,/* 遥控器控制 */
|
||||
SHOOT_AI, /* AI控制 */
|
||||
} Shoot_Control_Mode_t;
|
||||
typedef enum{
|
||||
SHOOT_REMOTE,
|
||||
SHOOT_AI
|
||||
}Shoot_CONTROL_Mode_t;
|
||||
|
||||
typedef struct {
|
||||
Shoot_Mode_t mode;/* 射击模式 */
|
||||
|
||||
Shoot_Control_Mode_t control_mode;/* 控制模式 */
|
||||
Shoot_CONTROL_Mode_t control_mode;
|
||||
bool ready; /* 准备射击 */
|
||||
bool firecmd; /* 射击 */
|
||||
}Shoot_CMD_t;
|
||||
|
||||
@ -145,8 +145,8 @@ static const Config_Param_t config = {
|
||||
},
|
||||
|
||||
.pid.yaw_4310_motor_omega = {
|
||||
.k = 0.5f,
|
||||
.p = 0.5f,
|
||||
.k = 0.0f,
|
||||
.p = 0.3f,
|
||||
.i = 0.0f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 0.0f,
|
||||
|
||||
@ -115,7 +115,6 @@ 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 );
|
||||
@ -247,7 +246,7 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
|
||||
Gimbal_SetMode(g, g_cmd->mode);
|
||||
|
||||
/*处理小yaw轴控制命令*/
|
||||
float delta_yaw_6020 = 10.0f*g_cmd->delta_yaw_6020 * g->dt;
|
||||
float delta_yaw_6020 = 5.0f*g_cmd->delta_yaw_6020 * g->dt;
|
||||
|
||||
if(g->param->travel.yaw_6020 > 0) // 有限位才处理
|
||||
{
|
||||
@ -277,7 +276,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 = 8.0f*g_cmd->delta_pitch_4310*g->dt;
|
||||
float delta_pitch_4310 = 5.0f*g_cmd->delta_pitch_4310*g->dt;
|
||||
if(g->param->travel.pitch_4310 > 0) // 有限位才处理
|
||||
{
|
||||
/* 计算当前电机角度与IMU角度的偏差 */
|
||||
@ -331,15 +330,11 @@ 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,
|
||||
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.motor.yaw_4310_motor_feedback.rotor_speed,0.0f,g->dt);
|
||||
// g->out.yaw_4310 = yaw_omega_set_point; // 直接输出速度环目标值作为电机输出
|
||||
// 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; // 直接输出速度环目标值作为电机输出
|
||||
/*控制云台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);
|
||||
@ -381,9 +376,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 * 2.0f;
|
||||
// output_yaw_4310.kp = 0.2f;
|
||||
output_yaw_4310.kd = 0.1f;
|
||||
output_yaw_4310.torque = g->out.yaw_4310 * 1.0f;
|
||||
output_yaw_4310.kd = 0.2f;
|
||||
// output_yaw_4310.kd=0.1f;
|
||||
|
||||
MOTOR_MIT_Output_t output_pitch_4310 = {0};
|
||||
|
||||
@ -6,6 +6,9 @@
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "task/user_task.h"
|
||||
/* USER INCLUDE BEGIN */
|
||||
#include "bsp/can.h"
|
||||
#include "device/motor_rm.h"
|
||||
#include "component/pid.h"
|
||||
|
||||
/* USER INCLUDE END */
|
||||
|
||||
@ -14,7 +17,57 @@
|
||||
/* Private macro ------------------------------------------------------------ */
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
/* USER STRUCT BEGIN */
|
||||
MOTOR_RM_t motor_6020;
|
||||
MOTOR_RM_t motor_3508;
|
||||
MOTOR_RM_Param_t motor_6020_param = {
|
||||
.can = BSP_CAN_1,
|
||||
.id = 0x205,
|
||||
.module = MOTOR_GM6020,
|
||||
.reverse = false,
|
||||
.gear = false,
|
||||
};
|
||||
MOTOR_RM_Param_t motor_3508_param = {
|
||||
.can = BSP_CAN_1,
|
||||
.id = 0x201,
|
||||
.module = MOTOR_M3508,
|
||||
.reverse = false,
|
||||
.gear = false,
|
||||
};
|
||||
|
||||
KPID_t motor_6020_speed;
|
||||
KPID_t motor_3508_angle;
|
||||
KPID_t motor_3508_omega;
|
||||
|
||||
KPID_Params_t motor_6020_speed_param = {
|
||||
.k = 0.1f,
|
||||
.p = 0.2f,
|
||||
.i = 0.1f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 1.0f,
|
||||
.out_limit = 1.0f,
|
||||
.d_cutoff_freq = 30.0f,
|
||||
};
|
||||
KPID_Params_t motor_3508_angle_param = {
|
||||
.k = 1.0f,
|
||||
.p = 5.0f,
|
||||
.i = 0.1f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 1.0f,
|
||||
.out_limit = 1.0f,
|
||||
.d_cutoff_freq = 30.0f,
|
||||
};
|
||||
KPID_Params_t motor_3508_omega_param = {
|
||||
.k = 1.0f,
|
||||
.p = 5.0f,
|
||||
.i = 0.1f,
|
||||
.d = 0.0f,
|
||||
.i_limit = 1.0f,
|
||||
.out_limit = 1.0f,
|
||||
.d_cutoff_freq = 30.0f,
|
||||
};
|
||||
|
||||
float motor_3508_target_angle = 5*M_2PI;
|
||||
float motor_6020_target_speed = 33.0f;
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
@ -30,17 +83,34 @@ void Task_Task8(void *argument) {
|
||||
|
||||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
||||
/* USER CODE INIT BEGIN */
|
||||
BSP_CAN_Init();
|
||||
MOTOR_RM_Register( &motor_6020_param);
|
||||
MOTOR_RM_Register(&motor_3508_param);
|
||||
|
||||
PID_Init(&motor_6020_speed, KPID_MODE_CALC_D, TASK8_FREQ, &motor_6020_speed_param);
|
||||
PID_Init(&motor_3508_angle, KPID_MODE_NO_D, TASK8_FREQ, &motor_3508_angle_param);
|
||||
PID_Init(&motor_3508_omega, KPID_MODE_NO_D, TASK8_FREQ, &motor_3508_omega_param);
|
||||
/* USER CODE INIT END */
|
||||
|
||||
while (1) {
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
/* USER CODE BEGIN */
|
||||
// osMessageQueueGet(task_runtime.msgq.RC_REMOTE, &remote, NULL, 0);
|
||||
MOTOR_RM_UpdateAll();
|
||||
|
||||
motor_6020.feedback.rotor_speed= (MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&motor_6020_param)->motor)) );
|
||||
motor_3508.feedback.rotor_abs_angle= MOTOR_RM_GetMotor(&motor_3508_param)->gearbox_total_angle;
|
||||
motor_3508.feedback.rotor_speed= (MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&motor_3508_param)->motor)) );
|
||||
|
||||
|
||||
float motor_6020out,motor_3508_ommega,motor_3508_out;
|
||||
|
||||
motor_6020out = PID_Calc(&motor_6020_speed, motor_6020_target_speed, motor_6020.feedback.rotor_speed, 0.0f,0.0f);
|
||||
|
||||
// motor_3508_ommega = PID_Calc(&motor_3508_omega, motor_3508_target_angle, motor_3508.feedback.rotor_abs_angle, 0.0f,0.0f);
|
||||
// motor_3508_out = PID_Calc(&motor_3508_angle, motor_3508_ommega, motor_3508.feedback.rotor_speed, 0.0f,0.0f);
|
||||
|
||||
MOTOR_RM_SetOutput(&motor_6020_param, motor_6020out);
|
||||
MOTOR_RM_Ctrl(&motor_6020_param);
|
||||
/* USER CODE END */
|
||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||||
}
|
||||
|
||||
@ -49,7 +49,9 @@ void Task_ai(void *argument) {
|
||||
AI_Get_NUC(&ai,&ai_cmd);
|
||||
osMessageQueueReset(task_runtime.msgq.gimbal.ai.g_cmd);
|
||||
osMessageQueuePut(task_runtime.msgq.gimbal.ai.g_cmd, &ai_cmd, 0, 0);
|
||||
osMessageQueuePut(task_runtime.msgq.shoot.ai.s_cmd, &ai_cmd, 0, 0);
|
||||
|
||||
osMessageQueueReset(task_runtime.msgq.gimbal.ai.s_cmd);
|
||||
osMessageQueuePut(task_runtime.msgq.gimbal.ai.s_cmd, &ai_cmd, 0, 0);
|
||||
/* USER CODE END */
|
||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||||
}
|
||||
|
||||
@ -38,7 +38,7 @@ void Task_Init(void *argument) {
|
||||
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.Task8 = osThreadNew(Task_Task8, NULL, &attr_Task8);
|
||||
// task_runtime.thread.Task8 = osThreadNew(Task_Task8, NULL, &attr_Task8);
|
||||
|
||||
// 创建消息队列
|
||||
/* USER MESSAGE BEGIN */
|
||||
@ -54,8 +54,7 @@ void Task_Init(void *argument) {
|
||||
task_runtime.msgq.shoot.cmd = osMessageQueueNew(2u, sizeof(Shoot_CMD_t),NULL);
|
||||
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.gimbal.ai.s_cmd = osMessageQueueNew(2u, sizeof(AI_cmd_t),NULL);
|
||||
/* USER MESSAGE END */
|
||||
|
||||
osKernelUnlock(); // 解锁内核
|
||||
|
||||
@ -60,7 +60,7 @@ void Task_rc(void *argument) {
|
||||
osMessageQueueReset(task_runtime.msgq.cmd.raw.rc);
|
||||
osMessageQueuePut(task_runtime.msgq.cmd.raw.rc, &cmd_rc, 0, 0);
|
||||
|
||||
|
||||
|
||||
/* USER CODE END */
|
||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||||
}
|
||||
|
||||
@ -44,28 +44,28 @@ Shoot_SetMode(&shoot,SHOOT_MODE_SINGLE);
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
/* USER CODE BEGIN */
|
||||
osMessageQueueGet(task_runtime.msgq.shoot.cmd, &shoot_cmd, NULL, 0);
|
||||
osMessageQueueGet(task_runtime.msgq.shoot.ai.s_cmd, &shoot_ai_cmd, NULL, 0);
|
||||
osMessageQueueGet(task_runtime.msgq.gimbal.ai.s_cmd, &shoot_ai_cmd, NULL, 0);
|
||||
|
||||
// if(shoot_cmd.control_mode==SHOOT_REMOTE)
|
||||
// {
|
||||
// //do nothing，使用遥控器的指令
|
||||
// }
|
||||
// else if(shoot_cmd.control_mode==SHOOT_AI)
|
||||
// {
|
||||
// shoot_cmd.mode = SHOOT_MODE_SINGLE;
|
||||
// if(shoot_ai_cmd.mode==0 || shoot_ai_cmd.mode==1)
|
||||
// {
|
||||
// shoot_cmd.firecmd=false;
|
||||
// shoot_cmd.ready=true;
|
||||
if(shoot_cmd.control_mode==SHOOT_REMOTE)
|
||||
{
|
||||
//do nothing，使用遥控器的指令
|
||||
}
|
||||
else if(shoot_cmd.control_mode==SHOOT_AI)
|
||||
{
|
||||
shoot_cmd.mode = SHOOT_MODE_SINGLE;
|
||||
if(shoot_ai_cmd.mode==0 || shoot_ai_cmd.mode==1)
|
||||
{
|
||||
shoot_cmd.firecmd=false;
|
||||
shoot_cmd.ready=true;
|
||||
|
||||
// }
|
||||
// else if(shoot_ai_cmd.mode==2)
|
||||
// {
|
||||
}
|
||||
else if(shoot_ai_cmd.mode==2)
|
||||
{
|
||||
|
||||
// shoot_cmd.firecmd=true;
|
||||
// shoot_cmd.ready=true;
|
||||
// }
|
||||
// }
|
||||
shoot_cmd.firecmd=true;
|
||||
shoot_cmd.ready=true;
|
||||
}
|
||||
}
|
||||
|
||||
Shoot_UpdateFeedback(&shoot);
|
||||
Shoot_SetMode(&shoot,shoot_cmd.mode);
|
||||
|
||||
@ -0,0 +1 @@
|
||||
feedback
|
||||
@ -91,21 +91,16 @@ typedef struct {
|
||||
osMessageQueueId_t quat;
|
||||
osMessageQueueId_t feedback;
|
||||
osMessageQueueId_t g_cmd;
|
||||
|
||||
osMessageQueueId_t s_cmd;
|
||||
}ai;
|
||||
/* 新增的 ai 消息队列 主要是给自瞄*/
|
||||
} gimbal;
|
||||
|
||||
struct
|
||||
{
|
||||
osMessageQueueId_t cmd;
|
||||
struct{
|
||||
|
||||
osMessageQueueId_t s_cmd;
|
||||
}ai;
|
||||
osMessageQueueId_t cmd;
|
||||
}shoot;
|
||||
|
||||
|
||||
} msgq;
|
||||
/* USER MESSAGE END */
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user