增加robot_cmd的总控制和急停处理,添加遥控器位域处理,待测试
This commit is contained in:
NeoZng
parent
a235aec5df
commit
df356cf675
|
|
@ -36,6 +36,8 @@ static Chassis_Ctrl_Cmd_s chassis_cmd_send; // 发送给底盘应用的信息,
|
|||
static Subscriber_t *chassis_feed_sub;
|
||||
static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等
|
||||
|
||||
static Robot_Status_e robot_state;
|
||||
|
||||
void GimbalCMDInit()
|
||||
{
|
||||
remote_control_data = RemoteControlInit(&huart3); // 修改为对应串口,注意dbus协议串口需加反相器
|
||||
|
|
@ -81,6 +83,8 @@ static void CalcOffsetAngle()
|
|||
*/
|
||||
static void RemoteControlSetMode()
|
||||
{
|
||||
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -91,6 +95,28 @@ static void MouseKeySetMode()
|
|||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 紧急停止,包括遥控器左上侧拨轮打满/重要模块离线/双板通信失效等
|
||||
* '300'待修改成合适的值,或改为开关控制
|
||||
*
|
||||
*/
|
||||
static void EmergencyHandler()
|
||||
{
|
||||
if(remote_control_data[0].joy_stick.ch[4]<-300)//还需添加重要应用和模块离线的判断
|
||||
{
|
||||
robot_state=ROBOT_STOP; // 遥控器左上侧拨轮打满,进入紧急停止模式
|
||||
gimbal_cmd_send.gimbal_mode==GIMBAL_ZERO_FORCE;
|
||||
chassis_cmd_send.chassis_mode==CHASSIS_ZERO_FORCE;
|
||||
shoot_cmd_send.shoot_mode==SHOOT_STOP;
|
||||
return;
|
||||
}
|
||||
// if(remote_control_data[0].joy_stick.ch[4]>300 && 各个模块正常)
|
||||
// {
|
||||
// //恢复运行
|
||||
// //...
|
||||
// }
|
||||
}
|
||||
|
||||
void GimbalCMDTask()
|
||||
{
|
||||
// 从其他应用获取回传数据
|
||||
|
|
@ -105,6 +131,8 @@ void GimbalCMDTask()
|
|||
RemoteControlSetMode();
|
||||
else if (0) // 键盘控制
|
||||
MouseKeySetMode();
|
||||
|
||||
EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况
|
||||
|
||||
// 设置视觉发送数据,work_mode在前一部分设置
|
||||
vision_send_data.bullet_speed = chassis_fetch_data.bullet_speed;
|
||||
|
|
|
|||
|
|
@ -52,6 +52,13 @@
|
|||
* @brief 这些枚举类型和结构体会作为CMD控制数据和各应用的反馈数据的一部分
|
||||
*
|
||||
*/
|
||||
//机器人状态
|
||||
typedef enum
|
||||
{
|
||||
ROBOT_STOP,
|
||||
ROBOT_WORKING,
|
||||
} Robot_Status_e;
|
||||
|
||||
|
||||
// 应用状态
|
||||
typedef enum
|
||||
|
|
|
|||
|
|
@ -36,9 +36,10 @@ static void sbus_to_rc(volatile const uint8_t *sbus_buf)
|
|||
rc_ctrl[0].mouse.press_r = sbus_buf[13]; //!< Mouse Right Is Press ?
|
||||
|
||||
// 按键值,每个键1bit,key_temp共16位;按键顺序在remote_control.h的宏定义中可见
|
||||
// 使用位域后不再需要这一中间操作
|
||||
rc_ctrl[0].key_temp = sbus_buf[14] | (sbus_buf[15] << 8); //!< KeyBoard value
|
||||
|
||||
// @todo 似乎可以直接用位域操作进行,把key_temp通过强制类型转换变成key类型? 方案见remote_control.md
|
||||
// @todo 似乎可以直接用位域操作进行,把key_temp通过强制类型转换变成key类型? 位域方案在下面,尚未测试
|
||||
// 按键值解算,利用宏+循环减少代码长度
|
||||
for (uint16_t i = 0x0001, j = 0; i < 0x8001; i *= 2, j++) // 依次查看每一个键
|
||||
{
|
||||
|
|
@ -53,6 +54,15 @@ static void sbus_to_rc(volatile const uint8_t *sbus_buf)
|
|||
rc_ctrl[0].key[KEY_PRESS_WITH_CTRL][j] = rc_ctrl[0].key_temp & i;
|
||||
}
|
||||
|
||||
// 位域的按键值解算,直接memcpy即可,注意小端低字节在前,即lsb在第一位
|
||||
*(uint16_t*)&rc_ctrl[0].key_test[KEY_PRESS]= (uint16_t)(sbus_buf[14] | (sbus_buf[15] << 8));
|
||||
*(uint16_t*)&rc_ctrl[0].key_test[KEY_STATE]=*(uint16_t*)&rc_ctrl[0].key_test[KEY_PRESS] & ~(*(uint16_t*)&(rc_ctrl[1].key_test[KEY_PRESS]));
|
||||
if(rc_ctrl[0].key_test[KEY_PRESS].ctrl)
|
||||
rc_ctrl[0].key_test[KEY_PRESS_WITH_CTRL]=rc_ctrl[0].key_test[KEY_PRESS];
|
||||
if(rc_ctrl[0].key_test[KEY_PRESS].shift)
|
||||
rc_ctrl[0].key_test[Key_Shift]=rc_ctrl[0].key_test[KEY_PRESS];
|
||||
|
||||
|
||||
// 减去偏置值
|
||||
rc_ctrl[0].joy_stick.ch[0] -= RC_CH_VALUE_OFFSET;
|
||||
rc_ctrl[0].joy_stick.ch[1] -= RC_CH_VALUE_OFFSET;
|
||||
|
|
@ -67,7 +77,7 @@ static void sbus_to_rc(volatile const uint8_t *sbus_buf)
|
|||
* 对sbus_to_rc的简单封装
|
||||
*
|
||||
*/
|
||||
static void RefereeRxCallback()
|
||||
static void RemoteControlRxCallback()
|
||||
{
|
||||
sbus_to_rc(rc_usart_instance->recv_buff);
|
||||
}
|
||||
|
|
@ -75,7 +85,7 @@ static void RefereeRxCallback()
|
|||
RC_ctrl_t *RemoteControlInit(UART_HandleTypeDef *rc_usart_handle)
|
||||
{
|
||||
USART_Init_Config_s conf;
|
||||
conf.module_callback = RefereeRxCallback;
|
||||
conf.module_callback = RemoteControlRxCallback;
|
||||
conf.usart_handle = rc_usart_handle;
|
||||
conf.recv_buff_size = REMOTE_CONTROL_FRAME_SIZE;
|
||||
rc_usart_instance = USARTRegister(&conf);
|
||||
|
|
|
|||
|
|
@ -55,27 +55,6 @@
|
|||
#define Key_B 15
|
||||
|
||||
/* ----------------------- Data Struct ------------------------------------- */
|
||||
typedef struct
|
||||
{
|
||||
struct
|
||||
{
|
||||
int16_t ch[5]; // 右|0 ,右-1 ,左-2 ,左|3 ,拨轮4
|
||||
uint8_t s[2]; //[0]:left [1]:right
|
||||
} joy_stick;
|
||||
struct
|
||||
{
|
||||
int16_t x;
|
||||
int16_t y;
|
||||
int16_t z;
|
||||
uint8_t press_l;
|
||||
uint8_t press_r;
|
||||
} mouse;
|
||||
|
||||
uint16_t key_temp;
|
||||
uint8_t key[4][16];
|
||||
|
||||
} RC_ctrl_t;
|
||||
|
||||
// 待测试的位域结构体,可以极大提升解析速度
|
||||
typedef struct
|
||||
{
|
||||
|
|
@ -97,6 +76,30 @@ typedef struct
|
|||
uint16_t b : 1;
|
||||
} Key_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
struct
|
||||
{
|
||||
int16_t ch[5]; // 右|0 ,右-1 ,左-2 ,左|3 ,拨轮4
|
||||
uint8_t s[2]; //[0]:left [1]:right
|
||||
} joy_stick;
|
||||
struct
|
||||
{
|
||||
int16_t x;
|
||||
int16_t y;
|
||||
int16_t z;
|
||||
uint8_t press_l;
|
||||
uint8_t press_r;
|
||||
} mouse;
|
||||
|
||||
uint16_t key_temp;
|
||||
uint8_t key[4][16]; // 当前使用的键盘索引
|
||||
Key_t key_test[4]; // 改为位域后的键盘索引,空间减少8倍,速度增加16~倍
|
||||
|
||||
} RC_ctrl_t;
|
||||
|
||||
|
||||
|
||||
/* ------------------------- Internal Data ----------------------------------- */
|
||||
|
||||
/**
|
||||
|
|
|
|||
Loading…
Reference in New Issue