添加了大量调试log，新增了dwt计时宏，增加了USB软件复位防止主控复位后上位机无法连接usb
This commit is contained in:
NeoZng
parent
ea6163a48d
commit
4a45331d31
|
|
@ -52,6 +52,7 @@ void MemManage_Handler(void);
|
|||
void BusFault_Handler(void);
|
||||
void UsageFault_Handler(void);
|
||||
void DebugMon_Handler(void);
|
||||
void FLASH_IRQHandler(void);
|
||||
void EXTI3_IRQHandler(void);
|
||||
void EXTI4_IRQHandler(void);
|
||||
void DMA1_Stream1_IRQHandler(void);
|
||||
|
|
|
|||
|
|
@ -123,9 +123,9 @@ void MX_FREERTOS_Init(void)
|
|||
void StartDefaultTask(void const *argument)
|
||||
{
|
||||
/* init code for USB_DEVICE */
|
||||
MX_USB_DEVICE_Init(); // USB???
|
||||
MX_USB_DEVICE_Init(); // 默认usb启动任务的位置
|
||||
/* USER CODE BEGIN StartDefaultTask */
|
||||
vTaskDelete(NULL); // ??????,????CPU
|
||||
vTaskDelete(NULL); // 避免空置和切换占用cpu
|
||||
/* USER CODE END StartDefaultTask */
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -73,6 +73,11 @@ void HAL_MspInit(void)
|
|||
/* PendSV_IRQn interrupt configuration */
|
||||
HAL_NVIC_SetPriority(PendSV_IRQn, 15, 0);
|
||||
|
||||
/* Peripheral interrupt init */
|
||||
/* FLASH_IRQn interrupt configuration */
|
||||
HAL_NVIC_SetPriority(FLASH_IRQn, 6, 0);
|
||||
HAL_NVIC_EnableIRQ(FLASH_IRQn);
|
||||
|
||||
/* USER CODE BEGIN MspInit 1 */
|
||||
|
||||
/* USER CODE END MspInit 1 */
|
||||
|
|
|
|||
|
|
@ -183,6 +183,20 @@ void DebugMon_Handler(void)
|
|||
/* please refer to the startup file (startup_stm32f4xx.s). */
|
||||
/******************************************************************************/
|
||||
|
||||
/**
|
||||
* @brief This function handles Flash global interrupt.
|
||||
*/
|
||||
void FLASH_IRQHandler(void)
|
||||
{
|
||||
/* USER CODE BEGIN FLASH_IRQn 0 */
|
||||
|
||||
/* USER CODE END FLASH_IRQn 0 */
|
||||
HAL_FLASH_IRQHandler();
|
||||
/* USER CODE BEGIN FLASH_IRQn 1 */
|
||||
|
||||
/* USER CODE END FLASH_IRQn 1 */
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief This function handles EXTI line3 interrupt.
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -27,7 +27,7 @@
|
|||
#include "usbd_cdc.h"
|
||||
|
||||
/* USER CODE BEGIN Includes */
|
||||
|
||||
#include "bsp_dwt.h"
|
||||
/* USER CODE END Includes */
|
||||
|
||||
/* Private typedef -----------------------------------------------------------*/
|
||||
|
|
@ -95,7 +95,11 @@ void HAL_PCD_MspInit(PCD_HandleTypeDef* pcdHandle)
|
|||
HAL_NVIC_SetPriority(OTG_FS_IRQn, 5, 0);
|
||||
HAL_NVIC_EnableIRQ(OTG_FS_IRQn);
|
||||
/* USER CODE BEGIN USB_OTG_FS_MspInit 1 */
|
||||
|
||||
// 上电后重新枚举usb设备
|
||||
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_12, GPIO_PIN_RESET);
|
||||
DWT_Delay(100);
|
||||
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_12, GPIO_PIN_RESET);
|
||||
DWT_Delay(100);
|
||||
/* USER CODE END USB_OTG_FS_MspInit 1 */
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -8,6 +8,9 @@
|
|||
#include "message_center.h"
|
||||
#include "general_def.h"
|
||||
#include "dji_motor.h"
|
||||
// bsp
|
||||
#include "bsp_dwt.h"
|
||||
#include "bsp_log.h"
|
||||
|
||||
// 私有宏,自动将编码器转换成角度值
|
||||
#define YAW_ALIGN_ANGLE (YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF_DJI) // 对齐时的角度,0-360
|
||||
|
|
@ -266,12 +269,14 @@ static void EmergencyHandler()
|
|||
shoot_cmd_send.shoot_mode = SHOOT_OFF;
|
||||
shoot_cmd_send.friction_mode = FRICTION_OFF;
|
||||
shoot_cmd_send.load_mode = LOAD_STOP;
|
||||
LOGERROR("[CMD] emergency stop!");
|
||||
}
|
||||
// 遥控器右侧开关为[上],恢复正常运行
|
||||
if (switch_is_up(rc_data[TEMP].rc.switch_right))
|
||||
{
|
||||
robot_state = ROBOT_READY;
|
||||
shoot_cmd_send.shoot_mode = SHOOT_ON;
|
||||
LOGINFO("[CMD] reinstate, robot ready");
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -13,6 +13,8 @@
|
|||
#include "daemon.h"
|
||||
#include "HT04.h"
|
||||
|
||||
#include "bsp_log.h"
|
||||
|
||||
osThreadId insTaskHandle;
|
||||
osThreadId robotTaskHandle;
|
||||
osThreadId motorTaskHandle;
|
||||
|
|
@ -54,12 +56,15 @@ void StartINSTASK(void const *argument)
|
|||
{
|
||||
static float ins_start, ins_dt;
|
||||
INS_Init(); // 确保BMI088被正确初始化.
|
||||
LOGINFO("[freeRTOS] INS Task Start");
|
||||
while (1)
|
||||
{
|
||||
// 1kHz
|
||||
ins_start = DWT_GetTimeline_ms();
|
||||
INS_Task();
|
||||
ins_dt = DWT_GetTimeline_ms() - ins_start;
|
||||
if (ins_dt > 1)
|
||||
LOGERROR("[freeRTOS] INS Task is being DELAY! dt = [%f]", &ins_dt);
|
||||
VisionSend(); // 解算完成后发送视觉数据,但是当前的实现不太优雅,后续若添加硬件触发需要重新考虑结构的组织
|
||||
osDelay(1);
|
||||
}
|
||||
|
|
@ -68,21 +73,30 @@ void StartINSTASK(void const *argument)
|
|||
void StartMOTORTASK(void const *argument)
|
||||
{
|
||||
static float motor_dt, motor_start;
|
||||
LOGINFO("[freeRTOS] MOTOR Task Start");
|
||||
while (1)
|
||||
{
|
||||
motor_start = DWT_GetTimeline_ms();
|
||||
MotorControlTask();
|
||||
motor_dt = DWT_GetTimeline_ms() - motor_start;
|
||||
if (motor_dt > 1)
|
||||
LOGERROR("[freeRTOS] MOTOR Task is being DELAY! dt = [%f]", &motor_dt);
|
||||
osDelay(1);
|
||||
}
|
||||
}
|
||||
|
||||
void StartDAEMONTASK(void const *argument)
|
||||
{
|
||||
static float daemon_dt, daemon_start;
|
||||
LOGINFO("[freeRTOS] Daemon Task Start");
|
||||
while (1)
|
||||
{
|
||||
// 100Hz
|
||||
daemon_start = DWT_GetTimeline_ms();
|
||||
DaemonTask();
|
||||
daemon_dt = DWT_GetTimeline_ms() - daemon_start;
|
||||
if (daemon_dt > 10)
|
||||
LOGERROR("[freeRTOS] Daemon Task is being DELAY! dt = [%f]", &daemon_dt);
|
||||
osDelay(10);
|
||||
}
|
||||
}
|
||||
|
|
@ -90,19 +104,24 @@ void StartDAEMONTASK(void const *argument)
|
|||
void StartROBOTTASK(void const *argument)
|
||||
{
|
||||
static float robot_dt, robot_start;
|
||||
LOGINFO("[freeRTOS] ROBOT core Task Start");
|
||||
// 200Hz-500Hz,若有额外的控制任务如平衡步兵可能需要提升至1kHz
|
||||
while (1)
|
||||
{
|
||||
robot_start = DWT_GetTimeline_ms();
|
||||
RobotTask();
|
||||
robot_dt = DWT_GetTimeline_ms() - robot_start;
|
||||
if (robot_dt > 5)
|
||||
LOGERROR("[freeRTOS] ROBOT core Task is being DELAY! dt = [%f]", &robot_dt);
|
||||
osDelay(5);
|
||||
}
|
||||
}
|
||||
|
||||
void StartUITASK(void const *argument)
|
||||
{
|
||||
LOGINFO("[freeRTOS] UI Task Start");
|
||||
MyUIInit();
|
||||
LOGINFO("[freeRTOS] UI Init Done, communication with ref has established");
|
||||
while (1)
|
||||
{
|
||||
// 每给裁判系统发送一包数据会挂起一次,详见UITask函数的refereeSend()
|
||||
|
|
|
|||
|
|
@ -308,6 +308,7 @@ NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:fals
|
|||
NVIC.EXTI3_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true\:true
|
||||
NVIC.EXTI4_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
|
||||
NVIC.EXTI9_5_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
|
||||
NVIC.FLASH_IRQn=true\:6\:0\:true\:false\:true\:true\:true\:true\:true
|
||||
NVIC.ForceEnableDMAVector=true
|
||||
NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
|
||||
NVIC.I2C2_ER_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
|
||||
|
|
|
|||
|
|
@ -65,15 +65,20 @@ CANInstance *CANRegister(CAN_Init_Config_s *config)
|
|||
if (!idx)
|
||||
{
|
||||
CANServiceInit(); // 第一次注册,先进行硬件初始化
|
||||
LOGINFO("[bsp_can] CAN Service Init");
|
||||
}
|
||||
if (idx >= CAN_MX_REGISTER_CNT) // 超过最大实例数
|
||||
{
|
||||
while (1)
|
||||
;
|
||||
LOGERROR("[bsp_can] CAN instance exceeded MAX num, consider balance the load of CAN bus");
|
||||
}
|
||||
for (size_t i = 0; i < idx; i++)
|
||||
{ // 重复注册 | id重复
|
||||
if (can_instance[i]->rx_id == config->rx_id && can_instance[i]->can_handle == config->can_handle)
|
||||
{
|
||||
while (1)
|
||||
;
|
||||
LOGERROR("[}bsp_can] CAN id crash ,tx [%d] or rx [%d] already registered", &config->tx_id, &config->rx_id);
|
||||
}
|
||||
}
|
||||
|
||||
CANInstance *instance = (CANInstance *)malloc(sizeof(CANInstance)); // 分配空间
|
||||
|
|
@ -107,7 +112,7 @@ uint8_t CANTransmit(CANInstance *_instance, float timeout)
|
|||
{
|
||||
if (DWT_GetTimeline_ms() - dwt_start > timeout) // 超时
|
||||
{
|
||||
LOGWARNING("CAN BUSY sending! cnt:%d", busy_count);
|
||||
LOGWARNING("[bsp_can] CAN MAILbox full! failed to add msg to mailbox. Cnt [%d]", busy_count);
|
||||
busy_count++;
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -116,7 +121,7 @@ uint8_t CANTransmit(CANInstance *_instance, float timeout)
|
|||
// tx_mailbox会保存实际填入了这一帧消息的邮箱,但是知道是哪个邮箱发的似乎也没啥用
|
||||
if (HAL_CAN_AddTxMessage(_instance->can_handle, &_instance->txconf, _instance->tx_buff, &_instance->tx_mailbox))
|
||||
{
|
||||
LOGWARNING("CAN BUSY bus! cnt:%d", busy_count);
|
||||
LOGWARNING("[bsp_can] CAN bus BUS! cnt:%d", busy_count);
|
||||
busy_count++;
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -125,9 +130,10 @@ uint8_t CANTransmit(CANInstance *_instance, float timeout)
|
|||
|
||||
void CANSetDLC(CANInstance *_instance, uint8_t length)
|
||||
{
|
||||
// 发送长度错误!检查调用参数是否出错,或出现野指针/越界访问
|
||||
if (length > 8 || length == 0) // 安全检查
|
||||
while (1)
|
||||
; // 发送长度错误!检查调用参数是否出错,或出现野指针/越界访问
|
||||
LOGERROR("[bsp_can] CAN DLC error! check your code or wild pointer");
|
||||
_instance->txconf.DLC = length;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -21,8 +21,8 @@ static osMutexId DWT_MUTEX;
|
|||
/**
|
||||
* @brief 私有函数,用于检查DWT CYCCNT寄存器是否溢出,并更新CYCCNT_RountCount
|
||||
* @attention 此函数假设两次调用之间的时间间隔不超过一次溢出
|
||||
*
|
||||
* @todo 更好的方案是为dwt的时间更新单独设置一个任务
|
||||
*
|
||||
* @todo 更好的方案是为dwt的时间更新单独设置一个任务?
|
||||
* 不过,使用dwt的初衷是定时不被中断/任务等因素影响,因此该实现仍然有其存在的意义
|
||||
*
|
||||
*/
|
||||
|
|
@ -31,7 +31,7 @@ static void DWT_CNT_Update(void)
|
|||
if (__get_CONTROL()) // 不在中断中,使用互斥锁;在中断则直接执行即可,本框架将所有中断优先级设置为相同,故不会被其他中断重入
|
||||
if (osOK != osMutexWait(DWT_MUTEX, 0))
|
||||
return;
|
||||
|
||||
|
||||
volatile uint32_t cnt_now = DWT->CYCCNT;
|
||||
if (cnt_now < CYCCNT_LAST)
|
||||
CYCCNT_RountCount++;
|
||||
|
|
@ -131,6 +131,5 @@ void DWT_Delay(float Delay)
|
|||
float wait = Delay;
|
||||
|
||||
while ((DWT->CYCCNT - tickstart) < wait * (float)CPU_FREQ_Hz)
|
||||
{
|
||||
}
|
||||
;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -2,7 +2,8 @@
|
|||
******************************************************************************
|
||||
* @file bsp_dwt.h
|
||||
* @author Wang Hongxi
|
||||
* @version V1.1.0
|
||||
* @author modified by NeoZng
|
||||
* @version V1.2.0
|
||||
* @date 2022/3/8
|
||||
* @brief
|
||||
******************************************************************************
|
||||
|
|
@ -15,6 +16,7 @@
|
|||
|
||||
#include "main.h"
|
||||
#include "stdint.h"
|
||||
#include "bsp_log.h"
|
||||
|
||||
typedef struct
|
||||
{
|
||||
|
|
@ -23,17 +25,30 @@ typedef struct
|
|||
uint16_t us;
|
||||
} DWT_Time_t;
|
||||
|
||||
/**
|
||||
* @brief 该宏用于计算代码段执行时间,单位为秒/s,返回值为float类型
|
||||
* 首先需要创建一个float类型的变量,用于存储时间间隔
|
||||
* 计算得到的时间间隔同时还会通过RTT打印到日志终端,你也可以将你的dt变量添加到查看
|
||||
*/
|
||||
#define TIME_ELAPSE(dt, code) \
|
||||
do \
|
||||
{ \
|
||||
float tstart = DWT_GetTimeline_s(); \
|
||||
code; \
|
||||
dt = DWT_GetTimeline_s() - tstart; \
|
||||
LOGINFO("[DWT] " #dt " = %f s\r\n", dt); \
|
||||
} while (0)
|
||||
|
||||
/**
|
||||
* @brief 初始化DWT,传入参数为CPU频率,单位MHz
|
||||
*
|
||||
*
|
||||
* @param CPU_Freq_mHz c板为168MHz,A板为180MHz
|
||||
*/
|
||||
void DWT_Init(uint32_t CPU_Freq_mHz);
|
||||
|
||||
/**
|
||||
* @brief 获取两次调用之间的时间间隔,单位为秒/s
|
||||
*
|
||||
*
|
||||
* @param cnt_last 上一次调用的时间戳
|
||||
* @return float 时间间隔,单位为秒/s
|
||||
*/
|
||||
|
|
@ -41,7 +56,7 @@ float DWT_GetDeltaT(uint32_t *cnt_last);
|
|||
|
||||
/**
|
||||
* @brief 获取两次调用之间的时间间隔,单位为秒/s,高精度
|
||||
*
|
||||
*
|
||||
* @param cnt_last 上一次调用的时间戳
|
||||
* @return double 时间间隔,单位为秒/s
|
||||
*/
|
||||
|
|
@ -49,22 +64,22 @@ double DWT_GetDeltaT64(uint32_t *cnt_last);
|
|||
|
||||
/**
|
||||
* @brief 获取当前时间,单位为秒/s,即初始化后的时间
|
||||
*
|
||||
*
|
||||
* @return float 时间轴
|
||||
*/
|
||||
float DWT_GetTimeline_s(void);
|
||||
|
||||
/**
|
||||
* @brief 获取当前时间,单位为毫秒/ms,即初始化后的时间
|
||||
*
|
||||
* @return float
|
||||
*
|
||||
* @return float
|
||||
*/
|
||||
float DWT_GetTimeline_ms(void);
|
||||
|
||||
/**
|
||||
* @brief 获取当前时间,单位为微秒/us,即初始化后的时间
|
||||
*
|
||||
* @return uint64_t
|
||||
*
|
||||
* @return uint64_t
|
||||
*/
|
||||
uint64_t DWT_GetTimeline_us(void);
|
||||
|
||||
|
|
@ -72,7 +87,7 @@ uint64_t DWT_GetTimeline_us(void);
|
|||
* @brief DWT延时函数,单位为秒/s
|
||||
* @attention 该函数不受中断是否开启的影响,可以在临界区和关闭中断时使用
|
||||
* @note 禁止在__disable_irq()和__enable_irq()之间使用HAL_Delay()函数,应使用本函数
|
||||
*
|
||||
*
|
||||
* @param Delay 延时时间,单位为秒/s
|
||||
*/
|
||||
void DWT_Delay(float Delay);
|
||||
|
|
@ -83,6 +98,4 @@ void DWT_Delay(float Delay);
|
|||
*/
|
||||
void DWT_SysTimeUpdate(void);
|
||||
|
||||
|
||||
|
||||
#endif /* BSP_DWT_H_ */
|
||||
|
|
|
|||
|
|
@ -2,8 +2,6 @@
|
|||
|
||||
DWT是stm32内部的一个"隐藏资源",他的用途是给下载器提供准确的定时,从而为调试信息加上时间戳.并在固定的时间间隔将调试数据发送到你的xxlink上.
|
||||
|
||||
|
||||
|
||||
## 常用功能
|
||||
|
||||
### 计算两次进入同一个函数的时间间隔
|
||||
|
|
@ -23,8 +21,34 @@ start=DWT_DetTimeline_ms();
|
|||
|
||||
// some proc to go...
|
||||
for(uint8_t i=0;i<10;i++)
|
||||
foo();
|
||||
foo();
|
||||
|
||||
end = DWT_DetTimeline_ms()-start;
|
||||
```
|
||||
|
||||
我们还提供了一个宏用于调试计时:
|
||||
|
||||
```c
|
||||
#define TIME_ELAPSE(dt, code) \
|
||||
do \
|
||||
{ \
|
||||
float tstart = DWT_GetTimeline_s(); \
|
||||
code; \
|
||||
dt = DWT_GetTimeline_s() - tstart; \
|
||||
LOGINFO("[DWT] " #dt " = %f s\r\n", dt); \
|
||||
} while (0)
|
||||
|
||||
```
|
||||
|
||||
传入一个float类型的变量,并将你要执行的代码写入第二个参数:
|
||||
|
||||
```c
|
||||
static float my_func_dt;
|
||||
TIME_ELAPSE(my_func_dt,
|
||||
Function1(vara);
|
||||
Function2(some, var);
|
||||
Function3(your,param);
|
||||
// something more
|
||||
);
|
||||
// my_func_dt can be used for other purpose then;
|
||||
```
|
||||
|
|
|
|||
|
|
@ -36,11 +36,11 @@ void BSPLogInit();
|
|||
|
||||
/* 有颜色格式日志输出,建议使用这些宏来输出日志 */
|
||||
// information level
|
||||
#define LOGINFO(format,...) LOG_PROTO("I", RTT_CTRL_TEXT_BRIGHT_GREEN , format, ##__VA_ARGS__)
|
||||
#define LOGINFO(format,...) LOG_PROTO("I:", RTT_CTRL_TEXT_BRIGHT_GREEN , format, ##__VA_ARGS__)
|
||||
// warning level
|
||||
#define LOGWARNING(format,...) LOG_PROTO("W", RTT_CTRL_TEXT_BRIGHT_YELLOW, format, ##__VA_ARGS__)
|
||||
#define LOGWARNING(format,...) LOG_PROTO("W:", RTT_CTRL_TEXT_BRIGHT_YELLOW, format, ##__VA_ARGS__)
|
||||
// error level
|
||||
#define LOGERROR(format,...) LOG_PROTO("E", RTT_CTRL_TEXT_BRIGHT_RED , format, ##__VA_ARGS__)
|
||||
#define LOGERROR(format,...) LOG_PROTO("E:", RTT_CTRL_TEXT_BRIGHT_RED , format, ##__VA_ARGS__)
|
||||
|
||||
/**
|
||||
* @brief 通过segger RTT打印日志,支持格式化输出,格式化输出的实现参考printf.
|
||||
|
|
|
|||
|
|
@ -5,7 +5,6 @@
|
|||
> TODO:
|
||||
>
|
||||
> 1. 在未接入调试器的时候，将日志写入flash中，并提供接口读取
|
||||
> 2. 增加日志分级，提供info、warning、error三个等级的日志
|
||||
|
||||
## 使用说明
|
||||
|
||||
|
|
|
|||
|
|
@ -9,6 +9,7 @@
|
|||
*
|
||||
*/
|
||||
#include "bsp_usart.h"
|
||||
#include "bsp_log.h"
|
||||
#include "stdlib.h"
|
||||
#include "memory.h"
|
||||
|
||||
|
|
@ -38,7 +39,13 @@ USARTInstance *USARTRegister(USART_Init_Config_s *init_config)
|
|||
{
|
||||
if (idx >= DEVICE_USART_CNT) // 超过最大实例数
|
||||
while (1)
|
||||
;
|
||||
LOGERROR("[bsp_usart] USART exceed max instance count!");
|
||||
|
||||
for (uint8_t i = 0; i < idx; i++) // 检查是否已经注册过
|
||||
if (usart_instance[i]->usart_handle == init_config->usart_handle)
|
||||
while (1)
|
||||
LOGERROR("[bsp_usart] USART instance already registered!");
|
||||
|
||||
USARTInstance *instance = (USARTInstance *)malloc(sizeof(USARTInstance));
|
||||
memset(instance, 0, sizeof(USARTInstance));
|
||||
|
||||
|
|
@ -75,14 +82,10 @@ void USARTSend(USARTInstance *_instance, uint8_t *send_buf, uint16_t send_size,
|
|||
/* 串口发送时,gstate会被设为BUSY_TX */
|
||||
uint8_t USARTIsReady(USARTInstance *_instance)
|
||||
{
|
||||
if(_instance->usart_handle->gState | HAL_UART_STATE_BUSY_TX)
|
||||
{
|
||||
if (_instance->usart_handle->gState | HAL_UART_STATE_BUSY_TX)
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -129,6 +132,7 @@ void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart)
|
|||
{
|
||||
HAL_UARTEx_ReceiveToIdle_DMA(usart_instance[i]->usart_handle, usart_instance[i]->recv_buff, usart_instance[i]->recv_buff_size);
|
||||
__HAL_DMA_DISABLE_IT(usart_instance[i]->usart_handle->hdmarx, DMA_IT_HT);
|
||||
LOGWARNING("[bsp_usart] USART error callback triggered, instance idx [%d]", i);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -11,9 +11,9 @@
|
|||
|
||||
需要在串口实例下设定接收的数据包的长度，实例对应的串口硬件（通过`UART_HandleTypeDef`指定，如`&huart1`），解析接收数据对应的回调函数这三个参数。然后，调用`USARTRegister()`并传入配置好的`usart_instance`指针即可。
|
||||
|
||||
若要发送数据，调用`USARTSend()`。注意buffsize务必小于buff的大小，否则造成指针越界后果未知。
|
||||
若要发送数据，调用`USARTSend()`。注意buffsize务必小于你创建的buff的大小，否则造成指针越界后果未知。
|
||||
|
||||
串口硬件收到数据时，会将其存入`usart_instance.recv_buff[]`中，当收到完整一包数据，会调用设定的回调函数`module_callback`（即你提供的解析函数）。在此函数中，你可以通过`usart_instance.recv_buff[]`访问串口收到的数据。
|
||||
串口硬件收到数据时，会将其存入`usart_instance.recv_buff[]`中，当收到完整一包数据，会调用设定的回调函数`module_callback`（即你注册时提供的解析函数）。在此函数中，你可以通过`usart_instance.recv_buff[]`访问串口收到的数据。
|
||||
|
||||
## 代码结构
|
||||
|
||||
|
|
|
|||
|
|
@ -11,16 +11,17 @@
|
|||
|
||||
#include "bsp_usb.h"
|
||||
#include "bsp_log.h"
|
||||
#include "bsp_dwt.h"
|
||||
|
||||
static uint8_t *bsp_usb_rx_buffer; // 接收到的数据会被放在这里,buffer size为2048
|
||||
// 注意usb单个数据包(Full speed模式下)最大为64byte,超出可能会出现丢包情况
|
||||
|
||||
uint8_t *USBInit(USB_Init_Config_s usb_conf)
|
||||
{
|
||||
// 上电后重新枚举usb设备
|
||||
LOGINFO("USB init success");
|
||||
// usb的软件复位(模拟拔插)在usbd_conf.c中的HAL_PCD_MspInit()中
|
||||
bsp_usb_rx_buffer = CDCInitRxbufferNcallback(usb_conf.tx_cbk, usb_conf.rx_cbk); // 获取接收数据指针
|
||||
// usb的接收回调函数会在这里被设置,并将数据保存在bsp_usb_rx_buffer中
|
||||
LOGINFO("USB init success");
|
||||
return bsp_usb_rx_buffer;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -55,9 +55,7 @@ static void BMI088GyroRead(BMI088Instance *bmi088, uint8_t reg, uint8_t *dataptr
|
|||
*/
|
||||
static void BMI088AccelWriteSingleReg(BMI088Instance *bmi088, uint8_t reg, uint8_t data)
|
||||
{
|
||||
static uint8_t tx[2];
|
||||
tx[0] = reg;
|
||||
tx[1] = data;
|
||||
uint8_t tx[2] = {reg, data};
|
||||
SPITransmit(bmi088->spi_acc, tx, 2);
|
||||
}
|
||||
|
||||
|
|
@ -71,17 +69,15 @@ static void BMI088AccelWriteSingleReg(BMI088Instance *bmi088, uint8_t reg, uint8
|
|||
*/
|
||||
static void BMI088GyroWriteSingleReg(BMI088Instance *bmi088, uint8_t reg, uint8_t data)
|
||||
{
|
||||
static uint8_t tx[2];
|
||||
tx[0] = reg;
|
||||
tx[1] = data;
|
||||
uint8_t tx[2] = {reg, data};
|
||||
SPITransmit(bmi088->spi_gyro, tx, 2);
|
||||
}
|
||||
// -------------------------以上为私有函数,封装了BMI088寄存器读写函数,blocking--------------------------------//
|
||||
|
||||
// -------------------------以下为私有函数,用于初始化BMI088acc和gyro的硬件和配置--------------------------------//
|
||||
#define REG 0
|
||||
#define DATA 1
|
||||
#define ERROR 2
|
||||
#define BMI088REG 0
|
||||
#define BMI088DATA 1
|
||||
#define BMI088ERROR 2
|
||||
// BMI088初始化配置数组for accel,第一列为reg地址,第二列为写入的配置值,第三列为错误码(如果出错)
|
||||
static uint8_t BMI088_Accel_Init_Table[BMI088_WRITE_ACCEL_REG_NUM][3] =
|
||||
{
|
||||
|
|
@ -106,7 +102,7 @@ static uint8_t BMI088_Gyro_Init_Table[BMI088_WRITE_GYRO_REG_NUM][3] =
|
|||
* @brief 初始化BMI088加速度计,提高可读性分拆功能
|
||||
*
|
||||
* @param bmi088 待初始化的BMI088实例
|
||||
* @return uint8_t ERROR CODE if any problems here
|
||||
* @return uint8_t BMI088ERROR CODE if any problems here
|
||||
*/
|
||||
static uint8_t BMI088AccelInit(BMI088Instance *bmi088)
|
||||
{
|
||||
|
|
@ -130,14 +126,14 @@ static uint8_t BMI088AccelInit(BMI088Instance *bmi088)
|
|||
// 使用sizeof而不是magic number,这样如果修改了数组大小,不用修改这里的代码;或者使用宏定义
|
||||
for (uint8_t i = 0; i < sizeof(BMI088_Accel_Init_Table) / sizeof(BMI088_Accel_Init_Table[0]); i++)
|
||||
{
|
||||
reg = BMI088_Accel_Init_Table[i][REG];
|
||||
data = BMI088_Accel_Init_Table[i][DATA];
|
||||
reg = BMI088_Accel_Init_Table[i][BMI088REG];
|
||||
data = BMI088_Accel_Init_Table[i][BMI088DATA];
|
||||
BMI088AccelWriteSingleReg(bmi088, reg, data); // 写入寄存器
|
||||
DWT_Delay(0.01);
|
||||
BMI088AccelRead(bmi088, reg, &data, 1); // 写完之后立刻读回检查
|
||||
DWT_Delay(0.01);
|
||||
if (data != BMI088_Accel_Init_Table[i][DATA])
|
||||
error |= BMI088_Accel_Init_Table[i][ERROR];
|
||||
if (data != BMI088_Accel_Init_Table[i][BMI088DATA])
|
||||
error |= BMI088_Accel_Init_Table[i][BMI088ERROR];
|
||||
//{i--;} 可以设置retry次数,如果retry次数用完了,则返回error
|
||||
}
|
||||
return error;
|
||||
|
|
@ -147,7 +143,7 @@ static uint8_t BMI088AccelInit(BMI088Instance *bmi088)
|
|||
* @brief 初始化BMI088陀螺仪,提高可读性分拆功能
|
||||
*
|
||||
* @param bmi088 待初始化的BMI088实例
|
||||
* @return uint8_t ERROR CODE
|
||||
* @return uint8_t BMI088ERROR CODE
|
||||
*/
|
||||
static uint8_t BMI088GyroInit(BMI088Instance *bmi088)
|
||||
{
|
||||
|
|
@ -169,14 +165,14 @@ static uint8_t BMI088GyroInit(BMI088Instance *bmi088)
|
|||
// 使用sizeof而不是magic number,这样如果修改了数组大小,不用修改这里的代码;或者使用宏定义
|
||||
for (uint8_t i = 0; i < sizeof(BMI088_Gyro_Init_Table) / sizeof(BMI088_Gyro_Init_Table[0]); i++)
|
||||
{
|
||||
reg = BMI088_Gyro_Init_Table[i][REG];
|
||||
data = BMI088_Gyro_Init_Table[i][DATA];
|
||||
reg = BMI088_Gyro_Init_Table[i][BMI088REG];
|
||||
data = BMI088_Gyro_Init_Table[i][BMI088DATA];
|
||||
BMI088GyroWriteSingleReg(bmi088, reg, data); // 写入寄存器
|
||||
DWT_Delay(0.001);
|
||||
BMI088GyroRead(bmi088, reg, &data, 1); // 写完之后立刻读回对应寄存器检查是否写入成功
|
||||
DWT_Delay(0.001);
|
||||
if (data != BMI088_Gyro_Init_Table[i][DATA])
|
||||
error |= BMI088_Gyro_Init_Table[i][ERROR];
|
||||
if (data != BMI088_Gyro_Init_Table[i][BMI088DATA])
|
||||
error |= BMI088_Gyro_Init_Table[i][BMI088ERROR];
|
||||
//{i--;} 可以设置retry次数,尝试重新写入.如果retry次数用完了,则返回error
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -7,24 +7,24 @@
|
|||
```c
|
||||
BMI088_Init_Config_s imu_config = {
|
||||
.spi_acc_config={
|
||||
.GPIOx=GPIOC,
|
||||
.GPIOx=GPIOA,
|
||||
.GPIOx=GPIO_PIN_4,
|
||||
.spi_handle=&hspi1,
|
||||
},
|
||||
.spi_gyro_config={
|
||||
.GPIOx=GPIOC,
|
||||
.GPIOx=GPIO_PIN_4,
|
||||
.GPIOx=GPIOB,
|
||||
.GPIOx=GPIO_PIN_0,
|
||||
.spi_handle=&hspi1,
|
||||
},
|
||||
.acc_int_config={
|
||||
.exti_mode=EXTI_TRIGGER_FALLING,
|
||||
.GPIO_Pin=GPIO_PIN_10,
|
||||
.GPIOx=GPIOA,
|
||||
.GPIO_Pin=GPIO_PIN_4,
|
||||
.GPIOx=GPIOC,
|
||||
},
|
||||
.gyro_int_config={
|
||||
.exti_mode=EXTI_TRIGGER_FALLING,
|
||||
.GPIO_Pin=GPIO_PIN_11,
|
||||
.GPIOx=GPIOA,
|
||||
.GPIO_Pin=GPIO_PIN_5,
|
||||
.GPIOx=GPIOC,
|
||||
},
|
||||
.heat_pid_config={
|
||||
.Kp=0.0f,
|
||||
|
|
|
|||
|
|
@ -3,6 +3,7 @@
|
|||
#include "stdlib.h"
|
||||
#include "crc8.h"
|
||||
#include "bsp_dwt.h"
|
||||
#include "bsp_log.h"
|
||||
|
||||
/**
|
||||
* @brief 重置CAN comm的接收状态和buffer
|
||||
|
|
@ -59,7 +60,7 @@ static void CANCommRxCallback(CANInstance *_instance)
|
|||
if (comm->raw_recvbuf[comm->recv_buf_len - 2] == crc_8(comm->raw_recvbuf + 2, comm->recv_data_len))
|
||||
{ // 数据量大的话考虑使用DMA
|
||||
memcpy(comm->unpacked_recv_data, comm->raw_recvbuf + 2, comm->recv_data_len);
|
||||
comm->update_flag = 1; // 数据更新flag置为1
|
||||
comm->update_flag = 1; // 数据更新flag置为1
|
||||
DaemonReload(comm->comm_daemon); // 重载daemon,避免数据更新后一直不被读取而导致数据更新不及时
|
||||
}
|
||||
}
|
||||
|
|
@ -69,6 +70,13 @@ static void CANCommRxCallback(CANInstance *_instance)
|
|||
}
|
||||
}
|
||||
|
||||
static void CANCommLostCallback(void *cancomm)
|
||||
{
|
||||
CANCommInstance *comm = (CANCommInstance *)cancomm;
|
||||
CANCommResetRx(comm);
|
||||
LOGWARNING("[can_comm] can comm rx[%d] lost, reset rx state.", &comm->can_ins->rx_id);
|
||||
}
|
||||
|
||||
CANCommInstance *CANCommInit(CANComm_Init_Config_s *comm_config)
|
||||
{
|
||||
CANCommInstance *ins = (CANCommInstance *)malloc(sizeof(CANCommInstance));
|
||||
|
|
|
|||
|
|
@ -2,6 +2,7 @@
|
|||
#include "BMI088reg.h"
|
||||
#include "BMI088Middleware.h"
|
||||
#include "bsp_dwt.h"
|
||||
#include "bsp_log.h"
|
||||
#include <math.h>
|
||||
|
||||
#warning this is a legacy support. test the new BMI088 module as soon as possible.
|
||||
|
|
@ -129,7 +130,7 @@ void Calibrate_MPU_Offset(IMU_Data_t *bmi088)
|
|||
startTime = DWT_GetTimeline_s();
|
||||
do
|
||||
{
|
||||
if (DWT_GetTimeline_s() - startTime > 10)
|
||||
if (DWT_GetTimeline_s() - startTime > 12)
|
||||
{
|
||||
// <20><>????
|
||||
bmi088->GyroOffset[0] = GxOFFSET;
|
||||
|
|
@ -137,6 +138,7 @@ void Calibrate_MPU_Offset(IMU_Data_t *bmi088)
|
|||
bmi088->GyroOffset[2] = GzOFFSET;
|
||||
bmi088->gNorm = gNORM;
|
||||
bmi088->TempWhenCali = 40;
|
||||
LOGERROR("[BMI088] Calibrate Failed! Use offline params");
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
@ -206,7 +208,11 @@ void Calibrate_MPU_Offset(IMU_Data_t *bmi088)
|
|||
gyroDiff[0] > 0.15f ||
|
||||
gyroDiff[1] > 0.15f ||
|
||||
gyroDiff[2] > 0.15f)
|
||||
{
|
||||
LOGWARNING("[bmi088] calibration was interrupted\n");
|
||||
break;
|
||||
}
|
||||
|
||||
DWT_Delay(0.0005);
|
||||
}
|
||||
|
||||
|
|
@ -252,7 +258,10 @@ uint8_t bmi088_accel_init(void)
|
|||
|
||||
// check the "who am I"
|
||||
if (res != BMI088_ACC_CHIP_ID_VALUE)
|
||||
{
|
||||
LOGERROR("[bmi088] Can not read bmi088 acc chip id");
|
||||
return BMI088_NO_SENSOR;
|
||||
}
|
||||
|
||||
// set accel sonsor config and check
|
||||
for (write_reg_num = 0; write_reg_num < BMI088_WRITE_ACCEL_REG_NUM; write_reg_num++)
|
||||
|
|
@ -294,7 +303,10 @@ uint8_t bmi088_gyro_init(void)
|
|||
|
||||
// check the "who am I"
|
||||
if (res != BMI088_GYRO_CHIP_ID_VALUE)
|
||||
{
|
||||
LOGERROR("[bmi088] Can not read bmi088 gyro chip id");
|
||||
return BMI088_NO_SENSOR;
|
||||
}
|
||||
|
||||
// set gyro sonsor config and check
|
||||
for (write_reg_num = 0; write_reg_num < BMI088_WRITE_GYRO_REG_NUM; write_reg_num++)
|
||||
|
|
|
|||
|
|
@ -10,11 +10,13 @@
|
|||
*/
|
||||
#include "master_process.h"
|
||||
#include "seasky_protocol.h"
|
||||
#include "daemon.h"
|
||||
#include "bsp_log.h"
|
||||
#include "robot_def.h"
|
||||
|
||||
static Vision_Recv_s recv_data;
|
||||
static Vision_Send_s send_data;
|
||||
static DaemonInstance *vision_daemon_instance;
|
||||
|
||||
void VisionSetFlag(Enemy_Color_e enemy_color, Work_Mode_e work_mode, Bullet_Speed_e bullet_speed)
|
||||
{
|
||||
|
|
@ -30,13 +32,26 @@ void VisionSetAltitude(float yaw, float pitch, float roll)
|
|||
send_data.roll = roll;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 离线回调函数,将在daemon.c中被daemon task调用
|
||||
* @attention 由于HAL库的设计问题,串口开启DMA接收之后同时发送有概率出现__HAL_LOCK()导致的死锁,使得无法
|
||||
* 进入接收中断.通过daemon判断数据更新,重新调用服务启动函数以解决此问题.
|
||||
*
|
||||
* @param id vision_usart_instance的地址,此处没用.
|
||||
*/
|
||||
static void VisionOfflineCallback(void *id)
|
||||
{
|
||||
#ifdef VISION_USE_UART
|
||||
USARTServiceInit(vision_usart_instance);
|
||||
#endif // !VISION_USE_UART
|
||||
LOGWARNING("[vision] vision offline, restart communication.");
|
||||
}
|
||||
|
||||
#ifdef VISION_USE_UART
|
||||
|
||||
#include "bsp_usart.h"
|
||||
#include "daemon.h"
|
||||
|
||||
static USARTInstance *vision_usart_instance;
|
||||
static DaemonInstance *vision_daemon_instance;
|
||||
|
||||
/**
|
||||
* @brief 接收解包回调函数,将在bsp_usart.c中被usart rx callback调用
|
||||
|
|
@ -51,21 +66,8 @@ static void DecodeVision()
|
|||
// TODO: code to resolve flag_register;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 离线回调函数,将在daemon.c中被daemon task调用
|
||||
* @attention 由于HAL库的设计问题,串口开启DMA接收之后同时发送有概率出现__HAL_LOCK()导致的死锁,使得无法
|
||||
* 进入接收中断.通过daemon判断数据更新,重新调用服务启动函数以解决此问题.
|
||||
*
|
||||
* @param id vision_usart_instance的地址,此处没用.
|
||||
*/
|
||||
static void VisionOfflineCallback(void *id)
|
||||
{
|
||||
USARTServiceInit(vision_usart_instance);
|
||||
}
|
||||
|
||||
Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle)
|
||||
{
|
||||
#ifdef VISION_USE_UART
|
||||
USART_Init_Config_s conf;
|
||||
conf.module_callback = DecodeVision;
|
||||
conf.recv_buff_size = VISION_RECV_SIZE;
|
||||
|
|
@ -79,7 +81,6 @@ Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle)
|
|||
.reload_count = 10,
|
||||
};
|
||||
vision_daemon_instance = DaemonRegister(&daemon_conf);
|
||||
#endif // VISION_USE_UART
|
||||
|
||||
return &recv_data;
|
||||
}
|
||||
|
|
@ -125,9 +126,17 @@ static void DecodeVision(uint16_t recv_len)
|
|||
Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle)
|
||||
{
|
||||
UNUSED(_handle); // 仅为了消除警告
|
||||
USB_Init_Config_s conf = {0};
|
||||
conf.rx_cbk = DecodeVision;
|
||||
USB_Init_Config_s conf = {.rx_cbk = DecodeVision};
|
||||
vis_recv_buff = USBInit(conf);
|
||||
|
||||
// 为master process注册daemon,用于判断视觉通信是否离线
|
||||
Daemon_Init_Config_s daemon_conf = {
|
||||
.callback = VisionOfflineCallback, // 离线时调用的回调函数,会重启串口接收
|
||||
.owner_id = NULL,
|
||||
.reload_count = 5, // 50ms
|
||||
};
|
||||
vision_daemon_instance = DaemonRegister(&daemon_conf);
|
||||
|
||||
return &recv_data;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -4,14 +4,15 @@
|
|||
#include "bsp_log.h"
|
||||
|
||||
static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用
|
||||
|
||||
/* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */
|
||||
static DJIMotorInstance *dji_motor_instance[DJI_MOTOR_CNT] = {NULL};
|
||||
static DJIMotorInstance *dji_motor_instance[DJI_MOTOR_CNT] = {NULL}; // 会在control任务中遍历该指针数组进行pid计算
|
||||
|
||||
/**
|
||||
* @brief 由于DJI电机发送以四个一组的形式进行,故对其进行特殊处理,用6个(2can*3group)can_instance专门负责发送
|
||||
* 该变量将在 DJIMotorControl() 中使用,分组在 MotorSenderGrouping()中进行
|
||||
*
|
||||
* @note 因为只用于发送,所以不需要在bsp_can中注册
|
||||
*
|
||||
* C610(m2006)/C620(m3508):0x1ff,0x200;
|
||||
* GM6020:0x1ff,0x2ff
|
||||
* 反馈(rx_id): GM6020: 0x204+id ; C610/C620: 0x200+id
|
||||
|
|
@ -69,9 +70,10 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf
|
|||
{
|
||||
if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id)
|
||||
{
|
||||
LOGERROR("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information."); // 后续可以把id和CAN打印出来
|
||||
while (1)
|
||||
; // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
|
||||
LOGERROR("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
|
||||
uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
|
||||
while (1) // 后续可以把id和CAN打印出来 // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
|
||||
LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
|
@ -98,16 +100,16 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf
|
|||
if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id)
|
||||
{
|
||||
LOGERROR("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
|
||||
while (1)
|
||||
; // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8)
|
||||
uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
|
||||
while (1) // 后续可以把id和CAN打印出来 // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
|
||||
LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
default: // other motors should not be registered here
|
||||
LOGERROR("You must not register other motors using the API of DJI motor.");
|
||||
while (1)
|
||||
; // 其他电机不应该在这里注册
|
||||
LOGERROR("[dji_motor]You must not register other motors using the API of DJI motor."); // 其他电机不应该在这里注册
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -148,6 +150,9 @@ static void DecodeDJIMotor(CANInstance *_instance)
|
|||
|
||||
static void DJIMotorLostCallback(void *motor_ptr)
|
||||
{
|
||||
DJIMotorInstance *motor = (DJIMotorInstance *)motor_ptr;
|
||||
uint16_t can_bus = motor->motor_can_instance->can_handle == &hcan1 ? 1 : 2;
|
||||
LOGWARNING("[dji_motor] Motor lost, can bus [%d] , id [%d]", can_bus, motor->motor_can_instance->tx_id);
|
||||
}
|
||||
|
||||
// 电机初始化,返回一个电机实例
|
||||
|
|
@ -182,7 +187,7 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
|
|||
Daemon_Init_Config_s daemon_config = {
|
||||
.callback = DJIMotorLostCallback,
|
||||
.owner_id = instance,
|
||||
.reload_count = 1, // 10ms未收到数据则丢失
|
||||
.reload_count = 2, // 20ms未收到数据则丢失
|
||||
};
|
||||
instance->daemon = DaemonRegister(&daemon_config);
|
||||
|
||||
|
|
@ -195,17 +200,11 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
|
|||
void DJIMotorChangeFeed(DJIMotorInstance *motor, Closeloop_Type_e loop, Feedback_Source_e type)
|
||||
{
|
||||
if (loop == ANGLE_LOOP)
|
||||
{
|
||||
motor->motor_settings.angle_feedback_source = type;
|
||||
}
|
||||
else if (loop == SPEED_LOOP)
|
||||
{
|
||||
motor->motor_settings.speed_feedback_source = type;
|
||||
}
|
||||
else
|
||||
{
|
||||
LOGERROR("[dji_motor] loop type error, check memory access and func param"); // 检查是否传入了正确的LOOP类型,或发生了指针越界
|
||||
}
|
||||
}
|
||||
|
||||
void DJIMotorStop(DJIMotorInstance *motor)
|
||||
|
|
@ -299,11 +298,9 @@ void DJIMotorControl()
|
|||
sender_assignment[group].tx_buff[2 * num] = (uint8_t)(set >> 8); // 低八位
|
||||
sender_assignment[group].tx_buff[2 * num + 1] = (uint8_t)(set & 0x00ff); // 高八位
|
||||
|
||||
// 电机是否停止运行
|
||||
// 若该电机处于停止状态,直接将buff置零
|
||||
if (motor->stop_flag == MOTOR_STOP)
|
||||
{ // 若该电机处于停止状态,直接将buff置零
|
||||
memset(sender_assignment[group].tx_buff + 2 * num, 0, 16u);
|
||||
}
|
||||
}
|
||||
|
||||
// 遍历flag,检查是否要发送这一帧报文
|
||||
|
|
|
|||
|
|
@ -6,6 +6,7 @@
|
|||
#include "string.h"
|
||||
#include "daemon.h"
|
||||
#include "stdlib.h"
|
||||
#include "bsp_log.h"
|
||||
|
||||
static uint8_t idx;
|
||||
static HTMotorInstance *ht_motor_instance[HT_MOTOR_CNT];
|
||||
|
|
@ -64,17 +65,14 @@ static void HTMotorDecode(CANInstance *motor_can)
|
|||
tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]);
|
||||
measure->real_current = CURRENT_SMOOTH_COEF * uint_to_float(tmp, T_MIN, T_MAX, 12) +
|
||||
(1 - CURRENT_SMOOTH_COEF) * measure->real_current;
|
||||
|
||||
|
||||
}
|
||||
|
||||
static void HTMotorLostCallback(void *motor_ptr)
|
||||
{
|
||||
HTMotorInstance *motor = (HTMotorInstance *)motor_ptr;
|
||||
if (motor->stop_flag == MOTOR_STOP)
|
||||
return;
|
||||
LOGWARNING("[ht_motor] motor %d lost\n", motor->motor_can_instace->tx_id);
|
||||
if (++motor->lost_cnt % 10 != 0)
|
||||
HTMotorSetMode(CMD_MOTOR_MODE, motor); // 若不在停止模式,尝试重新让电机进入控制模式
|
||||
HTMotorSetMode(CMD_MOTOR_MODE, motor); // 尝试重新让电机进入控制模式
|
||||
}
|
||||
|
||||
/* 海泰电机一生黑,什么垃圾协议! */
|
||||
|
|
@ -99,7 +97,7 @@ void HTMotorCalibEncoder(HTMotorInstance *motor)
|
|||
memcpy(zero_buff, buf, 6); // 初始化的时候至少调用一次,故将其他指令为0时发送的报文保存一下,详见ht04电机说明
|
||||
CANTransmit(motor->motor_can_instace, 1);
|
||||
DWT_Delay(0.005);
|
||||
HTMotorSetMode(CMD_ZERO_POSITION, motor);
|
||||
HTMotorSetMode(CMD_ZERO_POSITION, motor); // sb 玩意校准完了编码器也不为0
|
||||
DWT_Delay(0.005);
|
||||
// HTMotorSetMode(CMD_MOTOR_MODE, motor);
|
||||
}
|
||||
|
|
@ -215,7 +213,7 @@ void HTMotorControlInit()
|
|||
char ht_id_buff[2] = {0};
|
||||
__itoa(i, ht_id_buff, 10);
|
||||
strcat(ht_task_name, ht_id_buff); // 似乎没什么吊用,osthreaddef会把第一个变量当作宏字符串传入,作为任务名
|
||||
// todo 还需要一个更优雅的方案来区分不同的电机任务
|
||||
// @todo 还需要一个更优雅的方案来区分不同的电机任务
|
||||
osThreadDef(ht_task_name, HTMotorTask, osPriorityNormal, 0, 128);
|
||||
ht_task_handle[i] = osThreadCreate(osThread(ht_task_name), ht_motor_instance[i]);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -40,3 +40,5 @@ static void HTMotorDecode(CANInstance *motor_can)
|
|||
}
|
||||
}
|
||||
```
|
||||
|
||||
第一次收到数据时默认电机处于限位处,将速度和角度都设置为零,记录当前的编码器数据,之后每次收到都减去该值.
|
||||
|
|
|
|||
|
|
@ -3,6 +3,7 @@
|
|||
#include "general_def.h"
|
||||
#include "daemon.h"
|
||||
#include "bsp_dwt.h"
|
||||
#include "bsp_log.h"
|
||||
|
||||
static uint8_t idx;
|
||||
static LKMotorInstance *lkmotor_instance[LK_MOTOR_MX_CNT] = {NULL};
|
||||
|
|
@ -43,6 +44,12 @@ static void LKMotorDecode(CANInstance *_instance)
|
|||
measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
|
||||
}
|
||||
|
||||
static void LKMotorLostCallback(void *motor_ptr)
|
||||
{
|
||||
LKMotorInstance *motor = (LKMotorInstance *)motor_ptr;
|
||||
LOGWARNING("[LKMotor] motor lost, id: %d", motor->motor_can_ins->tx_id);
|
||||
}
|
||||
|
||||
LKMotorInstance *LKMotorInit(Motor_Init_Config_s *config)
|
||||
{
|
||||
LKMotorInstance *motor = (LKMotorInstance *)malloc(sizeof(LKMotorInstance));
|
||||
|
|
@ -75,7 +82,7 @@ LKMotorInstance *LKMotorInit(Motor_Init_Config_s *config)
|
|||
Daemon_Init_Config_s daemon_config = {
|
||||
.callback = NULL,
|
||||
.owner_id = motor,
|
||||
.reload_count = 5, // 0.05秒
|
||||
.reload_count = 5, // 50ms
|
||||
};
|
||||
motor->daemon = DaemonRegister(&daemon_config);
|
||||
|
||||
|
|
|
|||
|
|
@ -1,115 +0,0 @@
|
|||
#include "ps_handle.h"
|
||||
uint8_t PS2_RawData[9] = {0};
|
||||
PS2_Instance PS2_Data = {0};
|
||||
void PS2_CS(uint8_t Val)
|
||||
{
|
||||
if (Val)
|
||||
HAL_GPIO_WritePin(PS2_CS_GPIOx, PS2_CS_Pin, GPIO_PIN_SET);
|
||||
else
|
||||
HAL_GPIO_WritePin(PS2_CS_GPIOx, PS2_CS_Pin, GPIO_PIN_RESET);
|
||||
}
|
||||
void PS2_CLK(uint8_t Val)
|
||||
{
|
||||
if (Val)
|
||||
HAL_GPIO_WritePin(PS2_CLK_GPIOx, PS2_CLK_Pin, GPIO_PIN_SET);
|
||||
else
|
||||
HAL_GPIO_WritePin(PS2_CLK_GPIOx, PS2_CLK_Pin, GPIO_PIN_RESET);
|
||||
}
|
||||
void PS2_DO(uint8_t Val)
|
||||
{
|
||||
if (Val)
|
||||
HAL_GPIO_WritePin(PS2_DO_GPIOx, PS2_DO_Pin, GPIO_PIN_SET);
|
||||
else
|
||||
HAL_GPIO_WritePin(PS2_DO_GPIOx, PS2_DO_Pin, GPIO_PIN_RESET);
|
||||
}
|
||||
uint8_t PS2_Read_DI()
|
||||
{
|
||||
return HAL_GPIO_ReadPin(PS2_DI_GPIOx, PS2_DI_Pin);
|
||||
}
|
||||
void PS2_Delay()
|
||||
{
|
||||
for (int i = 0; i < 0xBf; i++)
|
||||
__NOP();
|
||||
}
|
||||
uint8_t PS2_ReadWrite_Byte(uint8_t TxData)
|
||||
{
|
||||
uint8_t TX = TxData;
|
||||
uint8_t RX = 0;
|
||||
for (int i = 0; i < 8; i++)
|
||||
{
|
||||
if (TX & 0x01)
|
||||
PS2_DO(1);
|
||||
else
|
||||
PS2_DO(0);
|
||||
TX >>= 1;
|
||||
PS2_CLK(1);
|
||||
PS2_Delay();
|
||||
PS2_CLK(0);
|
||||
RX >>= 1;
|
||||
RX |= (PS2_Read_DI() << 7);
|
||||
PS2_Delay();
|
||||
PS2_CLK(1);
|
||||
PS2_Delay();
|
||||
}
|
||||
return RX;
|
||||
}
|
||||
|
||||
static void PS2_Decode()
|
||||
{
|
||||
if (PS2_RawData[2] == 0x5A)
|
||||
{
|
||||
PS2_Data.Key_Select = (~PS2_RawData[3] >> 0) & 0x01; //选择键
|
||||
PS2_Data.Key_Start = (~PS2_RawData[3] >> 3) & 0x01; //开始键
|
||||
|
||||
//左侧按键
|
||||
PS2_Data.Key_L_Up = (~PS2_RawData[3] >> 4) & 0x01;
|
||||
PS2_Data.Key_L_Right = (~PS2_RawData[3] >> 5) & 0x01;
|
||||
PS2_Data.Key_L_Down = (~PS2_RawData[3] >> 6) & 0x01;
|
||||
PS2_Data.Key_L_Left = (~PS2_RawData[3] >> 7) & 0x01;
|
||||
|
||||
//后侧按键
|
||||
PS2_Data.Key_L2 = (~PS2_RawData[4] >> 0) & 0x01;
|
||||
PS2_Data.Key_R2 = (~PS2_RawData[4] >> 1) & 0x01;
|
||||
PS2_Data.Key_L1 = (~PS2_RawData[4] >> 2) & 0x01;
|
||||
PS2_Data.Key_R1 = (~PS2_RawData[4] >> 3) & 0x01;
|
||||
|
||||
//右侧按键
|
||||
PS2_Data.Key_R_Up = (~PS2_RawData[4] >> 4) & 0x01;
|
||||
PS2_Data.Key_R_Right = (~PS2_RawData[4] >> 5) & 0x01;
|
||||
PS2_Data.Key_R_Down = (~PS2_RawData[4] >> 6) & 0x01;
|
||||
PS2_Data.Key_R_Left = (~PS2_RawData[4] >> 7) & 0x01;
|
||||
|
||||
if (PS2_RawData[1] == 0x41)
|
||||
{ //无灯模式(摇杆值八向)
|
||||
PS2_Data.Rocker_LX = 127 * (PS2_Data.Key_L_Right - PS2_Data.Key_L_Left);
|
||||
PS2_Data.Rocker_LY = 127 * (PS2_Data.Key_L_Up - PS2_Data.Key_L_Down);
|
||||
|
||||
PS2_Data.Rocker_RX = 127 * (PS2_Data.Key_R_Right - PS2_Data.Key_R_Left);
|
||||
PS2_Data.Rocker_RY = 127 * (PS2_Data.Key_R_Up - PS2_Data.Key_R_Down);
|
||||
}
|
||||
else if (PS2_RawData[1] == 0x73)
|
||||
{ //红灯模式(摇杆值模拟)
|
||||
|
||||
//摇杆按键
|
||||
PS2_Data.Key_Rocker_Left = (~PS2_RawData[3] >> 1) & 0x01;
|
||||
PS2_Data.Key_Rocker_Right = (~PS2_RawData[3] >> 2) & 0x01;
|
||||
|
||||
//摇杆值
|
||||
PS2_Data.Rocker_LX = PS2_RawData[7] - 0x80;
|
||||
PS2_Data.Rocker_LY = -1 - (PS2_RawData[8] - 0x80);
|
||||
PS2_Data.Rocker_RX = PS2_RawData[5] - 0x80;
|
||||
PS2_Data.Rocker_RY = -1 - (PS2_RawData[6] - 0x80);
|
||||
}
|
||||
}
|
||||
}
|
||||
void PS2_Read_Data(void)
|
||||
{
|
||||
PS2_CS(0);
|
||||
PS2_RawData[0] = PS2_ReadWrite_Byte(0x01); // 0
|
||||
PS2_RawData[1] = PS2_ReadWrite_Byte(0x42); // 1
|
||||
for (int i = 2; i < 9; i++)
|
||||
PS2_RawData[i] = PS2_ReadWrite_Byte(0xff);
|
||||
PS2_CS(1);
|
||||
PS2_Decode();
|
||||
}
|
||||
|
||||
|
|
@ -1,38 +0,0 @@
|
|||
#ifndef PS_HANDLE_H
|
||||
#define PS_HANDLE_H
|
||||
|
||||
#include "bsp_spi.h"
|
||||
#include "bsp_gpio.h"
|
||||
#include "bsp_dwt.h"
|
||||
|
||||
#define PS2_CS_GPIOx GPIOB
|
||||
#define PS2_CS_Pin GPIO_PIN_12
|
||||
|
||||
#define PS2_CLK_GPIOx GPIOB
|
||||
#define PS2_CLK_Pin GPIO_PIN_13
|
||||
|
||||
#define PS2_DO_GPIOx GPIOB
|
||||
#define PS2_DO_Pin GPIO_PIN_15
|
||||
|
||||
#define PS2_DI_GPIOx GPIOB
|
||||
#define PS2_DI_Pin GPIO_PIN_14
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint8_t A_D; //模拟(红灯)为1 数字(无灯)为0
|
||||
int8_t Rocker_RX, Rocker_RY, Rocker_LX, Rocker_LY; //摇杆值(模拟状态为实际值0-0xFF)(数字态为等效的值0,0x80,0xFF)
|
||||
//按键值0为未触发,1为触发态
|
||||
uint8_t Key_L1, Key_L2, Key_R1, Key_R2; //后侧大按键
|
||||
uint8_t Key_L_Right, Key_L_Left, Key_L_Up, Key_L_Down; //左侧按键
|
||||
uint8_t Key_R_Right, Key_R_Left, Key_R_Up, Key_R_Down; //右侧按键
|
||||
uint8_t Key_Select; //选择键
|
||||
uint8_t Key_Start; //开始键
|
||||
uint8_t Key_Rocker_Left, Key_Rocker_Right; //摇杆按键
|
||||
|
||||
} PS2_Instance;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#endif // !PS_HANDLE_H#define PS_HANDLE_H
|
||||
|
|
@ -1,3 +0,0 @@
|
|||
# ps_handle
|
||||
|
||||
提供ps手柄的支持,其通信协议和spi类似,可以使用bsp_spi来兼容.待编写.
|
||||
|
|
@ -15,8 +15,9 @@
|
|||
#include "bsp_usart.h"
|
||||
#include "task.h"
|
||||
#include "daemon.h"
|
||||
#include "bsp_log.h"
|
||||
|
||||
#define RE_RX_BUFFER_SIZE 200
|
||||
#define RE_RX_BUFFER_SIZE 255u // 裁判系统接收缓冲区大小
|
||||
|
||||
static USARTInstance *referee_usart_instance; // 裁判系统串口实例
|
||||
static DaemonInstance *referee_daemon; // 裁判系统守护进程
|
||||
|
|
@ -114,6 +115,7 @@ static void RefereeRxCallback()
|
|||
static void RefereeLostCallback(void *arg)
|
||||
{
|
||||
USARTServiceInit(referee_usart_instance);
|
||||
LOGWARNING("[rm_ref] lost referee data");
|
||||
}
|
||||
|
||||
/* 裁判系统通信初始化 */
|
||||
|
|
@ -122,7 +124,7 @@ referee_info_t *RefereeInit(UART_HandleTypeDef *referee_usart_handle)
|
|||
USART_Init_Config_s conf;
|
||||
conf.module_callback = RefereeRxCallback;
|
||||
conf.usart_handle = referee_usart_handle;
|
||||
conf.recv_buff_size = RE_RX_BUFFER_SIZE;
|
||||
conf.recv_buff_size = RE_RX_BUFFER_SIZE; // mx 255(u8)
|
||||
referee_usart_instance = USARTRegister(&conf);
|
||||
|
||||
Daemon_Init_Config_s daemon_conf = {
|
||||
|
|
@ -143,5 +145,5 @@ void RefereeSend(uint8_t *send, uint16_t tx_len)
|
|||
{
|
||||
static TickType_t xLastWakeTime;
|
||||
USARTSend(referee_usart_instance, send, tx_len, USART_TRANSFER_DMA);
|
||||
vTaskDelayUntil(&xLastWakeTime, 120);
|
||||
vTaskDelayUntil(&xLastWakeTime, 120); // 裁判系统接收ui数据和多机通信最大支持频率为10Hz
|
||||
}
|
||||
|
|
|
|||
|
|
@ -4,6 +4,7 @@
|
|||
#include "memory.h"
|
||||
#include "stdlib.h"
|
||||
#include "daemon.h"
|
||||
#include "bsp_log.h"
|
||||
|
||||
#define REMOTE_CONTROL_FRAME_SIZE 18u // 遥控器接收的buffer大小
|
||||
|
||||
|
|
@ -104,6 +105,7 @@ static void RCLostCallback(void *id)
|
|||
{
|
||||
memset(rc_ctrl, 0, sizeof(rc_ctrl)); // 清空遥控器数据
|
||||
USARTServiceInit(rc_usart_instance); // 尝试重新启动接收
|
||||
LOGWARNING("[rc] remote control lost");
|
||||
}
|
||||
|
||||
RC_ctrl_t *RemoteControlInit(UART_HandleTypeDef *rc_usart_handle)
|
||||
|
|
|
|||
Loading…
Reference in New Issue