TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-23 19:25:09 +08:00
Code Issues Packages Projects Releases Wiki Activity

更新了部分文档，提升了遥控器解析的可读性，增加了dji电机守护线程，修复裁判系统未初始化导致任务超时

Browse Source
This commit is contained in:
NeoZng
2023-06-08 15:53:53 +08:00
parent 72884ef96b
commit 12796f8e70
16 changed files with 119 additions and 117 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
modules/motor/DJImotor/dji_motor.c
View File

@@ -1,5 +1,6 @@
#include "dji_motor.h"
#include "general_def.h"
#include "bsp_dwt.h"
#include "bsp_log.h"
static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用
@@ -29,15 +30,12 @@ static CANInstance sender_assignment[6] = {
/**
* @brief 6个用于确认是否有电机注册到sender_assignment中的标志位,防止发送空帧,此变量将在DJIMotorControl()使用
* flag的初始化在 MotorSenderGrouping()中进行
*
*/
static uint8_t sender_enable_flag[6] = {0};
/**
* @brief 根据电调/拨码开关上的ID,根据说明书的默认id分配方式计算发送ID和接收ID,
* 并对电机进行分组以便处理多电机控制命令
*
* @param config
*/
static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *config)
{
@@ -124,7 +122,11 @@ static void DecodeDJIMotor(CANInstance *_instance)
// 这里对can instance的id进行了强制转换,从而获得电机的instance实例地址
// _instance指针指向的id是对应电机instance的地址,通过强制转换为电机instance的指针,再通过->运算符访问电机的成员motor_measure,最后取地址获得指针
uint8_t *rxbuff = _instance->rx_buff;
DJI_Motor_Measure_s *measure = &(((DJIMotorInstance *)_instance->id)->measure); // measure要多次使用,保存指针减小访存开销
DJIMotorInstance *motor = (DJIMotorInstance *)_instance->id;
DJI_Motor_Measure_s *measure = &motor->measure; // measure要多次使用,保存指针减小访存开销
DaemonReload(motor->daemon);
motor->dt = DWT_GetDeltaT(&motor->feed_cnt);
// 解析数据并对电流和速度进行滤波,电机的反馈报文具体格式见电机说明手册
measure->last_ecd = measure->ecd;
@@ -144,6 +146,10 @@ static void DecodeDJIMotor(CANInstance *_instance)
measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
}
static void DJIMotorLostCallback(void *motor_ptr)
{
}
// 电机初始化,返回一个电机实例
DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
{
@@ -172,6 +178,14 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
config->can_init_config.id = instance; // set id,eq to address(it is identity)
instance->motor_can_instance = CANRegister(&config->can_init_config);
// 注册守护线程
Daemon_Init_Config_s daemon_config = {
.callback = DJIMotorLostCallback,
.owner_id = instance,
.reload_count = 1, // 10ms未收到数据则丢失
};
instance->daemon = DaemonRegister(&daemon_config);
DJIMotorEnable(instance);
dji_motor_instance[idx++] = instance;
return instance;

6
modules/motor/DJImotor/dji_motor.h
View File

@@ -19,6 +19,7 @@
#include "controller.h"
#include "motor_def.h"
#include "stdint.h"
#include "daemon.h"
#define DJI_MOTOR_CNT 12
@@ -47,7 +48,6 @@ typedef struct
*/
typedef struct
{
DJI_Motor_Measure_s measure; // 电机测量值
Motor_Control_Setting_s motor_settings; // 电机设置
Motor_Controller_s motor_controller; // 电机控制器
@@ -59,6 +59,10 @@ typedef struct
Motor_Type_e motor_type; // 电机类型
Motor_Working_Type_e stop_flag; // 启停标志
DaemonInstance* daemon;
uint32_t feed_cnt;
float dt;
} DJIMotorInstance;
/**

12
modules/referee/referee_task.c
View File

@@ -13,10 +13,12 @@
#include "rm_referee.h"
#include "referee_UI.h"
#include "string.h"
#include "cmsis_os.h"
static Referee_Interactive_info_t *Interactive_data; // UI绘制需要的机器人状态数据
static referee_info_t *referee_recv_info; // 接收到的裁判系统数据
uint8_t UI_Seq; // 包序号，供整个referee文件使用
uint8_t UI_Seq; // 包序号，供整个referee文件使用
// @todo 不应该使用全局变量
/**
* @brief 判断各种ID，选择客户端ID
@@ -43,6 +45,7 @@ referee_info_t *Referee_Interactive_init(UART_HandleTypeDef *referee_usart_handl
{
referee_recv_info = RefereeInit(referee_usart_handle); // 初始化裁判系统的串口,并返回裁判系统反馈数据指针
Interactive_data = UI_data; // 获取UI绘制需要的机器人状态数据
referee_recv_info->init_flag = 1;
return referee_recv_info;
}
@@ -60,9 +63,12 @@ static uint32_t shoot_line_location[10] = {540, 960, 490, 515, 565};
void My_UI_init()
{
if (!referee_recv_info->init_flag)
vTaskDelete(NULL); // 如果没有初始化裁判系统则直接删除ui任务
while (referee_recv_info->GameRobotState.robot_id == 0)
;
DeterminRobotID();
osDelay(100); // 若还未收到裁判系统数据,等待一段时间后再检查
DeterminRobotID(); // 确定ui要发送到的目标客户端
UIDelete(&referee_recv_info->referee_id, UI_Data_Del_ALL, 0); // 清空UI
// 绘制发射基准线

12
modules/referee/rm_referee.h
View File

@@ -41,6 +41,8 @@ typedef struct
// 自定义交互数据的接收
Communicate_ReceiveData_t ReceiveData;
uint8_t init_flag;
} referee_info_t;
// 模式是否切换标志位，0为未切换，1为切换，static定义默认为0
@@ -67,11 +69,11 @@ typedef struct
Chassis_Power_Data_s Chassis_Power_Data; // 功率控制
// 上一次的模式，用于flag判断
chassis_mode_e chassis_last_mode;
gimbal_mode_e gimbal_last_mode;
shoot_mode_e shoot_last_mode;
friction_mode_e friction_last_mode;
lid_mode_e lid_last_mode;
chassis_mode_e chassis_last_mode;
gimbal_mode_e gimbal_last_mode;
shoot_mode_e shoot_last_mode;
friction_mode_e friction_last_mode;
lid_mode_e lid_last_mode;
Chassis_Power_Data_s Chassis_last_Power_Data;
} Referee_Interactive_info_t;

72
modules/remote/remote_control.c
View File

@@ -5,8 +5,8 @@
#include "stdlib.h"
#include "daemon.h"
#define RC_MOUSE_SMOOTH_COEF 0.9 // 鼠标平滑滤波器的阶数
#define REMOTE_CONTROL_FRAME_SIZE 18u // 遥控器接收的buffer大小
// 遥控器数据
static RC_ctrl_t rc_ctrl[2]; //[0]:当前数据TEMP,[1]:上一次的数据LAST.用于按键持续按下和切换的判断
static uint8_t rc_init_flag = 0; // 遥控器初始化标志位
@@ -14,6 +14,7 @@ static uint8_t rc_init_flag = 0; // 遥控器初始化标志位
// 遥控器拥有的串口实例,因为遥控器是单例,所以这里只有一个,就不封装了
static USARTInstance *rc_usart_instance;
static DaemonInstance *rc_daemon_instance;
/**
* @brief 矫正遥控器摇杆的值,超过660或者小于-660的值都认为是无效值,置0
*
@@ -21,22 +22,17 @@ static DaemonInstance *rc_daemon_instance;
static void RectifyRCjoystick()
{
for (uint8_t i = 0; i < 5; ++i)
{
if (abs(*(&rc_ctrl[TEMP].rc.rocker_l_ + i)) > 660)
*(&rc_ctrl[TEMP].rc.rocker_l_ + i) = 0;
}
}
/**
* @brief remote control protocol resolution
* @param[in] sbus_buf: raw data point
* @param[out] rc_ctrl: remote control data struct point
* @retval none
* @brief 遥控器数据解析
*
* @param sbus_buf 接收buffer
*/
static void sbus_to_rc(const uint8_t *sbus_buf)
{
// 摇杆,直接解算时减去偏置
rc_ctrl[TEMP].rc.rocker_r_ = ((sbus_buf[0] | (sbus_buf[1] << 8)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 0
rc_ctrl[TEMP].rc.rocker_r1 = (((sbus_buf[1] >> 3) | (sbus_buf[2] << 5)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 1
@@ -49,56 +45,45 @@ static void sbus_to_rc(const uint8_t *sbus_buf)
rc_ctrl[TEMP].rc.switch_left = ((sbus_buf[5] >> 4) & 0x000C) >> 2; //!< Switch left
// 鼠标解析
rc_ctrl[TEMP].mouse.x = (float)(sbus_buf[6] | (sbus_buf[7] << 8))*RC_MOUSE_SMOOTH_COEF + (1-RC_MOUSE_SMOOTH_COEF)*(float)(rc_ctrl[LAST].mouse.x); //!< Mouse X axis
rc_ctrl[TEMP].mouse.y = (float)(sbus_buf[8] | (sbus_buf[9] << 8))*RC_MOUSE_SMOOTH_COEF + (1-RC_MOUSE_SMOOTH_COEF)*(float)(rc_ctrl[TEMP].mouse.y); //!< Mouse Y axis
rc_ctrl[TEMP].mouse.x = (sbus_buf[6] | (sbus_buf[7] << 8)); //!< Mouse X axis
rc_ctrl[TEMP].mouse.y = (sbus_buf[8] | (sbus_buf[9] << 8)); //!< Mouse Y axis
rc_ctrl[TEMP].mouse.press_l = sbus_buf[12]; //!< Mouse Left Is Press ?
rc_ctrl[TEMP].mouse.press_r = sbus_buf[13]; //!< Mouse Right Is Press ?
// 位域的按键值解算,直接memcpy即可,注意小端低字节在前,即lsb在第一位,msb在最后. 尚未测试
// 位域的按键值解算,直接memcpy即可,注意小端低字节在前,即lsb在第一位,msb在最后
*(uint16_t *)&rc_ctrl[TEMP].key[KEY_PRESS] = (uint16_t)(sbus_buf[14] | (sbus_buf[15] << 8));
if (rc_ctrl[TEMP].key[KEY_PRESS].ctrl)
if (rc_ctrl[TEMP].key[KEY_PRESS].ctrl) // ctrl键按下
rc_ctrl[TEMP].key[KEY_PRESS_WITH_CTRL] = rc_ctrl[TEMP].key[KEY_PRESS];
else
memset(&rc_ctrl[TEMP].key[KEY_PRESS_WITH_CTRL], 0, sizeof(Key_t));
if (rc_ctrl[TEMP].key[KEY_PRESS].shift)
if (rc_ctrl[TEMP].key[KEY_PRESS].shift) // shift键按下
rc_ctrl[TEMP].key[KEY_PRESS_WITH_SHIFT] = rc_ctrl[TEMP].key[KEY_PRESS];
else
memset(&rc_ctrl[TEMP].key[KEY_PRESS_WITH_SHIFT], 0, sizeof(Key_t));
for (uint32_t i = 0, j = 0x1; i < 16; j <<= 1, i++)
uint16_t key_now = rc_ctrl[TEMP].key[KEY_PRESS].keys, // 当前按键是否按下
key_last = rc_ctrl[LAST].key[KEY_PRESS].keys, // 上一次按键是否按下
key_with_ctrl = rc_ctrl[TEMP].key[KEY_PRESS_WITH_CTRL].keys, // 当前ctrl组合键是否按下
key_with_shift = rc_ctrl[TEMP].key[KEY_PRESS_WITH_SHIFT].keys, // 当前shift组合键是否按下
key_last_with_ctrl = rc_ctrl[LAST].key[KEY_PRESS_WITH_CTRL].keys, // 上一次ctrl组合键是否按下
key_last_with_shift = rc_ctrl[LAST].key[KEY_PRESS_WITH_SHIFT].keys; // 上一次shift组合键是否按下
for (uint16_t i = 0, j = 0x1; i < 16; j <<= 1, i++)
{
if (((*(uint16_t *)&rc_ctrl[TEMP].key[KEY_PRESS] & j) == j) && ((*(uint16_t *)&rc_ctrl[1].key[KEY_PRESS] & j) == 0) && ((*(uint16_t *)&rc_ctrl[TEMP].key[KEY_PRESS_WITH_CTRL] & j) != j) && ((*(uint16_t *)&rc_ctrl[TEMP].key[KEY_PRESS_WITH_SHIFT] & j) != j))
{
if (i == 4 || i == 5) // 4,5位为ctrl和shift,直接跳过
continue;
// 如果当前按键按下,上一次按键没有按下,且ctrl和shift组合键没有按下,则按键按下计数加1(检测到上升沿)
if ((key_now & j) && !(key_last & j) && !(key_with_ctrl & j) && !(key_with_shift & j))
rc_ctrl[TEMP].key_count[KEY_PRESS][i]++;
if (rc_ctrl[TEMP].key_count[KEY_PRESS][i] >= 240)
{
rc_ctrl[TEMP].key_count[KEY_PRESS][i] = 0;
}
}
if (((*(uint16_t *)&rc_ctrl[TEMP].key[KEY_PRESS_WITH_CTRL] & j) == j) && ((*(uint16_t *)&rc_ctrl[1].key[KEY_PRESS_WITH_CTRL] & j) == 0))
{
// 当前ctrl组合键按下,上一次ctrl组合键没有按下,则ctrl组合键按下计数加1(检测到上升沿)
if ((key_with_ctrl & j) && !(key_last_with_ctrl & j))
rc_ctrl[TEMP].key_count[KEY_PRESS_WITH_CTRL][i]++;
if (rc_ctrl[TEMP].key_count[KEY_PRESS_WITH_CTRL][i] >= 240)
{
rc_ctrl[TEMP].key_count[KEY_PRESS_WITH_CTRL][i] = 0;
}
}
if (((*(uint16_t *)&rc_ctrl[TEMP].key[KEY_PRESS_WITH_SHIFT] & j) == j) && ((*(uint16_t *)&rc_ctrl[1].key[KEY_PRESS_WITH_SHIFT] & j) == 0))
{
// 当前shift组合键按下,上一次shift组合键没有按下,则shift组合键按下计数加1(检测到上升沿)
if ((key_with_shift & j) && !(key_last_with_shift & j))
rc_ctrl[TEMP].key_count[KEY_PRESS_WITH_SHIFT][i]++;
if (rc_ctrl[TEMP].key_count[KEY_PRESS_WITH_SHIFT][i] >= 240)
{
rc_ctrl[TEMP].key_count[KEY_PRESS_WITH_SHIFT][i] = 0;
}
}
}
memcpy(&rc_ctrl[1], &rc_ctrl[TEMP], sizeof(RC_ctrl_t)); // 保存上一次的数据,用于按键持续按下和切换的判断
memcpy(&rc_ctrl[LAST], &rc_ctrl[TEMP], sizeof(RC_ctrl_t)); // 保存上一次的数据,用于按键持续按下和切换的判断
}
/**
@@ -117,6 +102,7 @@ static void RemoteControlRxCallback()
*/
static void RCLostCallback(void *id)
{
memset(rc_ctrl, 0, sizeof(rc_ctrl)); // 清空遥控器数据
USARTServiceInit(rc_usart_instance); // 尝试重新启动接收
}
@@ -137,7 +123,7 @@ RC_ctrl_t *RemoteControlInit(UART_HandleTypeDef *rc_usart_handle)
rc_daemon_instance = DaemonRegister(&daemon_conf);
rc_init_flag = 1;
return (RC_ctrl_t *)&rc_ctrl;
return rc_ctrl;
}
uint8_t RemoteControlIsOnline()

51
modules/remote/remote_control.h
View File

@@ -40,11 +40,6 @@
#define switch_is_down(s) (s == RC_SW_DOWN)
#define switch_is_mid(s) (s == RC_SW_MID)
#define switch_is_up(s) (s == RC_SW_UP)
#define LEFT_SW 1 // 左侧开关
#define RIGHT_SW 0 // 右侧开关
//键盘状态的宏
#define key_is_press(s) (s == 1)
#define key_not_press(s) (s == 0)
/* ----------------------- PC Key Definition-------------------------------- */
// 对应key[x][0~16],获取对应的键;例如通过key[KEY_PRESS][Key_W]获取W键是否按下,后续改为位域后删除
@@ -67,26 +62,31 @@
/* ----------------------- Data Struct ------------------------------------- */
// 待测试的位域结构体,可以极大提升解析速度
typedef struct
typedef union
{
uint16_t w : 1;
uint16_t s : 1;
uint16_t d : 1;
uint16_t a : 1;
uint16_t shift : 1;
uint16_t ctrl : 1;
uint16_t q : 1;
uint16_t e : 1;
uint16_t r : 1;
uint16_t f : 1;
uint16_t g : 1;
uint16_t z : 1;
uint16_t x : 1;
uint16_t c : 1;
uint16_t v : 1;
uint16_t b : 1;
struct // 用于访问键盘状态
{
uint16_t w : 1;
uint16_t s : 1;
uint16_t d : 1;
uint16_t a : 1;
uint16_t shift : 1;
uint16_t ctrl : 1;
uint16_t q : 1;
uint16_t e : 1;
uint16_t r : 1;
uint16_t f : 1;
uint16_t g : 1;
uint16_t z : 1;
uint16_t x : 1;
uint16_t c : 1;
uint16_t v : 1;
uint16_t b : 1;
};
uint16_t keys; // 用于memcpy而不需要进行强制类型转换
} Key_t;
// @todo 当前结构体嵌套过深,需要进行优化
typedef struct
{
struct
@@ -104,12 +104,11 @@ typedef struct
{
int16_t x;
int16_t y;
int16_t z;
uint8_t press_l;
uint8_t press_r;
} mouse;
Key_t key[3]; // 改为位域后的键盘索引,空间减少8倍,速度增加16~倍
Key_t key[3]; // 改为位域后的键盘索引,空间减少8倍,速度增加16~倍
uint8_t key_count[3][16];
} RC_ctrl_t;
@@ -126,7 +125,7 @@ RC_ctrl_t *RemoteControlInit(UART_HandleTypeDef *rc_usart_handle);
/**
* @brief 检查遥控器是否在线,若尚未初始化也视为离线
*
*
* @return uint8_t 1:在线 0:离线
*/
uint8_t RemoteControlIsOnline();