电控-视觉通信
新通信协议：
具体修改在AimbotV2_Init函数的Visual_Com_Init中
Aimbot_UART_Init->Aimbot_RX_Thread->Aimbot_Read_UART_Data
读取主要修改了寻找帧头的算法，转移数据算法和解包算法
/**
* @brief 将收到的数据解包成原始通信协议数据
* @author yych
* @param rx_msg 收到的原始数据
* @param unpacked 串口包解开以后的数据存放的位置
* @param size 检查数组的大小
* @return int 本次被处理为正常数据的字节数
*/
static int Aimbot_Read_UART_Data(const rt_uint8_t rx_msg[], rt_uint8_t unpacked[], const uint8_t size)
{
int fori = 0; // 用于记录收到的数据中实际数据的起始位置(独立 0xFC 所在的位置)
int last_frame_end = 0; // 用于记录上一帧数据结束的下标位置
rt_err_t err = RT_ERROR; // 用于记录本次解包是否成功
//找到第一个帧头的位置
while (-1 != (fori = Find_First_Frameheader(rx_msg, fori, size, Framehead, 4)))
{
err = RT_ERROR;
// 将数据写到缓存区
Remove_Data(
unpacked, rx_msg + fori + 5, 0, utils_min_2_int((rx_msg[fori + 4] & 0x7F), AIMBOT_RX_REAL_DATA_SIZEMAX));
err = Aimbot_UART_Msg.flag_get(unpacked);
// 判断本次解包是否成功
if (RT_EOK == err)
{
// 继续检查剩余的数据内是否存在新的报文
fori += ((rx_msg[fori + 4] & 0x7F) + 4);
// 更新一帧数据结束的位置
last_frame_end = fori;
}
else
// 没有成功解包就直接从下一字节开始继续寻找
++fori;
if (fori >= size)
break;
}
return last_frame_end;
}
寻找帧头
/**
* @brief 在数组中寻找第一个被指定数据
* @author yych
* @param data 待查找数据的数组
* @param start 待查找目标在数组中的起始位置(包含)
* @param end 待查找目标在数组中的结束位置(不包含)
* @param target 待查找的目标数组
* @param len 待查找的目标数组长度
* @return int 返回值为非负数代表找到的目标数据下标, 返回 -1 代表未找到
*/
static int Find_First_Frameheader(const uint8_t data[], const uint8_t start, const uint8_t end, const uint8_t target[], const uint8_t len)
{
int index1 = start, index2 = 0;
uint8_t flagfind = 0;
// 开始遍历查找
for (; index1 < end - len + 1 && flagfind == 0; ++index1)
{
for(index2 = 0; index2 < len && data[index1 + index2] == target[index2]; index2 ++)
{
if(index2 == len - 1) flagfind = 1;
}
if(flagfind == 1) break;
}
// 返回结果
if (flagfind == 1)
return index1;
else
return -1;
}
转移数据
/**
* @brief 将数组中连续重复出现的数字删掉放到另一个数组中
* @author yych
* @param dst 目标位置
* @param src 源数据循环队列数组(不会被修改)
* @param start 检查数组的起始位置
* @param end 检查数组的结束位置
*/
static void Remove_Data(uint8_t dst[], const uint8_t src[], const uint8_t start, const uint8_t end)
{
// 定义当前操作到哪个位置了
int dst_i = 0, src_i = start;
// 遍历写入数据
for (; src_i < end; ++src_i)
{
dst[dst_i++] = src[src_i];
}
}
数据解包
// 接收：标志位报文
rt_err_t VisualCom_Receive_Flag(rt_uint8_t rxmsg[])
{
int fori;
char sum;
char FlagsTemp;
union Float_Char Temp;
// 校验和检查
sum = 0;
for (fori = 0; fori < 25; fori++)
sum += rxmsg[fori];
if (sum != rxmsg[fori])
{
error_time ++;
// 和校验未通过
return RT_ERROR;
}
Visual_LastFresh_Tick = rt_tick_get();
// 数据保存
VisualMode_FB = rxmsg[1]; // 视觉当前自瞄模式
Temp.c[0] = rxmsg[2];Temp.c[1] = rxmsg[3];Temp.c[2] = rxmsg[4];Temp.c[3] = rxmsg[5];
GimbalTolerance_Pitch = Temp.f; // 云台Pitch轴当前允许控制误差 (和视觉的通信规则存疑)
Temp.c[0] = rxmsg[6];Temp.c[1] = rxmsg[7];Temp.c[2] = rxmsg[8];Temp.c[3] = rxmsg[9];
GimbalTolerance_Yaw = Temp.f; // 云台Yaw轴当前允许控制误差 (和视觉的通信规则存疑)
FlagsTemp = rxmsg[0];
VisualFlag_TargetFound = utils_read_bit(FlagsTemp, 0); // 视觉是否搜索到目标
VisualFlag_Fire = utils_read_bit(FlagsTemp, 1); // 视觉是否允许开火
VisualFlag_BurstShoot = utils_read_bit(FlagsTemp, 2); // 视觉是否允许进行爆发开火
VisualFlag_ExitRune = utils_read_bit(FlagsTemp, 3); // 是否退出能量机关模式
VisualFlag_RuneFire = utils_read_bit(FlagsTemp, 4); // 能量机关模式下视觉开火翻转标志位
VisualFlag_RuneBurstShoot = utils_read_bit(FlagsTemp, 5); // 能量机关模式下爆发开火标志位（5连发）
VisualFlag_WorkingCorrect = utils_read_bit(FlagsTemp, 6); // 视觉当前是否工作正常
if (VisualFlag_TargetFound == 0)
{
// 视觉丢失目标了，此时需要直接将已有设定值数据设定为 失效
GimbalSet_Receive[0].State = RT_ERROR;
GimbalSet_Receive[1].State = RT_ERROR;
}
// 数据保存
VisualAttiSet_TickOri = rxmsg[10];
Temp.c[0] = rxmsg[11];Temp.c[1] = rxmsg[12];Temp.c[2] = rxmsg[13];Temp.c[3] = rxmsg[14];
Fire_Time = Temp.f;
Temp.c[0] = rxmsg[15];Temp.c[1] = rxmsg[16];Temp.c[2] = rxmsg[17];Temp.c[3] = rxmsg[18];
VisualAttiSetOri_Pitch = Temp.f;
Temp.c[0] = rxmsg[19];Temp.c[1] = rxmsg[20];Temp.c[2] = rxmsg[21];Temp.c[3] = rxmsg[22];
VisualAttiSetOri_Yaw = Temp.f;
VisualAttiSetOri_PitchSpe = (rt_int8_t)rxmsg[23];
VisualAttiSetOri_YawSpe = (rt_int8_t)rxmsg[24];
/*数据处理*/
// 先计算Tick的数值
GimbalSet_Receive[!Gimbal_Set_Cal_READ_Valid].PredictedTime = VisualCom_Tick_Fill(VisualAttiSet_TickOri);
GimbalSet_Receive[!Gimbal_Set_Cal_READ_Valid].GimbalSet_Angle.Pitch = VisualAttiSetOri_Pitch + PitchFix;
GimbalSet_Receive[!Gimbal_Set_Cal_READ_Valid].GimbalSet_Angle.Yaw = VisualAttiSetOri_Yaw + YawFix;
GimbalSet_Receive[!Gimbal_Set_Cal_READ_Valid].GimbalSet_Speed.Pitch = VisualAttiSetOri_PitchSpe;
GimbalSet_Receive[!Gimbal_Set_Cal_READ_Valid].GimbalSet_Speed.Yaw = VisualAttiSetOri_YawSpe;
if (Visual_Mode_Set == VisualMode_FB)
{
// 当前视觉工作模式正确
GimbalSet_Receive[!Gimbal_Set_Cal_READ_Valid].State = RT_EOK;
Gimbal_Set_Cal_READ_Valid = !Gimbal_Set_Cal_READ_Valid; // 切换可读取
}
else
{
// 视觉工作模式错误 则所有数据均标记为失效
GimbalSet_Receive[!Gimbal_Set_Cal_READ_Valid].State = RT_ERROR;
GimbalSet_Receive[Gimbal_Set_Cal_READ_Valid].State = RT_ERROR;
}
#ifdef AIMBOT_COM_COLLECT_PACKET_LOSS_RATE
++Package_SumCheck_Passed_Num;
#endif /* AIMBOT_COM_COLLECT_PACKET_LOSS_RATE */
return RT_EOK;
}
发送在Visual_Send_Thread中
着重修改了对时方式，现在是2ms发一次，但是对时精度为1ms，所以注意Count_2ms
void Visual_Send_Thread(void *Para)
{
VisualTiming_Sendmsg.id = ID_VISUAL_TIMING_SEND; // 设置ID
VisualTiming_Sendmsg.ide = RT_CAN_STDID; // 标准帧
VisualTiming_Sendmsg.rtr = RT_CAN_DTR; // 数据帧
VisualTiming_Sendmsg.priv = 0; // 报文优先级最高
VisualTiming_Sendmsg.len = 7; // 长度7
GimbalAtti_Sendmsg.id = ID_VISUAL_ATTI_SEND; // 设置ID
GimbalAtti_Sendmsg.ide = RT_CAN_STDID; // 标准帧
GimbalAtti_Sendmsg.rtr = RT_CAN_DTR; // 数据帧
GimbalAtti_Sendmsg.priv = 0; // 报文优先级最高
GimbalAtti_Sendmsg.len = 8; // 长度8
/* 延时，使第一次发送的时刻为100ms的整数倍 */
// 读当前Tick
Timing_Tick_Get = rt_tick_get();
// 取当前Tick的百位及以上 再加2 得到需要延时到的时刻
Timing_Tick_NextSend = (Timing_Tick_Get / 100 + 2) * 100;
// 等到Timing_Tick_NextSend
rt_thread_delay_to_tick(Timing_Tick_NextSend, &Timing_Tick_Get);
CREAT_ID(id);
ADDTOMONITOR_ID("Visual_Send_Thread", 5000, MONITOR_DEHANDLER, ALARM_RED, 1, id);
SWDG_START(id);
while (1)
{
Count_2ms = Timing_Tick_Get / 1; // 取当前Tick的整1ms的个数
Timing_Tick_NextSend = (Count_2ms) * 1 + 2; // 计算出下一次发送的时间
if (Timing_Tick_Get % 100 == 0)
{
// 当前为整100ms，需要先发送对时
//Count_100ms = Timing_Tick_Get / 100;
Count_100ms = Timing_Tick_Get;
VisualCom_TimingSend(); // 发送对时信息
#ifdef AIMBOT_WATCH_CLICK_DATA
click = VisualSend_Flags & 0x01;
#endif /* AIMBOT_WATCH_CLICK_DATA */
}
VisualCom_AttiSend(); // 发送云台姿态信息
//rt_thread_delay(200);
rt_thread_delay_to_tick(Timing_Tick_NextSend, &Timing_Tick_Get); // 准备下一次发送
SWDG_FEED(id);
}
}
对时函数如下
// 发送对时信息
static void VisualCom_TimingSend(void)
{
rt_int16_t MuzzleV_Temp; // 用于适应发送协议的中转变量
int fori;
char sum;
MuzzleV_Temp = (rt_int16_t)(Muzzle_V_REM); // 先存入变量，避免读写访问冲突导致出错 单位0.01
if (MuzzleV_Temp > 4095 || MuzzleV_Temp <= 0)
// 通信不能溢出
MuzzleV_Temp = 4095;
//电控时间32位
VisualTiming_Sendmsg.data[0] = (Count_100ms >> 0) & 0xFF;
VisualTiming_Sendmsg.data[1] = (Count_100ms >> 8) & 0xFF;
VisualTiming_Sendmsg.data[2] = (Count_100ms >> 16) & 0xFF;
VisualTiming_Sendmsg.data[3] = (Count_100ms >> 24) & 0xFF;
//模式+弹速
VisualTiming_Sendmsg.data[4] = (Visual_Mode_Set & 0x0F) | ((MuzzleV_Temp >> 4) & 0xF0);
VisualTiming_Sendmsg.data[5] = MuzzleV_Temp & 0xFF;
utils_write_bit(&VisualSend_Flags, AimFlag_MyColor, Color_Myself); // 更新当前自瞄颜色
//标志位包，12位为空
VisualTiming_Sendmsg.data[6] = (VisualSend_Flags & 0xC3);
VisualTiming_Sendmsg.data[7] = 0x00;
sum = 0;
for (fori = 0; fori < 8; fori++)
sum += VisualTiming_Sendmsg.data[fori];
VisualTiming_Sendmsg.data[8] = sum;
#ifdef AIMBOT_COM_COLLECT_PACKET_LOSS_RATE
++send_Package_Num;
#endif /* AIMBOT_COM_COLLECT_PACKET_LOSS_RATE */
/* 发送 */
#ifndef AIMBOT_CIMMUNICATION_USING_CAN
Aimbot_Write_UART_Data(&aimbot_device, 0, VisualTiming_Sendmsg.data, 9);
#else
rt_device_write(aimbot_device, 0, &VisualTiming_Sendmsg, sizeof(GimbalAtti_Sendmsg));
#endif
}
云台姿态信息如下
// 发送云台姿态信息
static void VisualCom_AttiSend(void)
{
int fori;
char sum;
union Float_Char IntToChar_Temp;
// 获取当前陀螺仪姿态
Gimbal_Atti_Send.Pitch = gimbal_atti.pitch;
Gimbal_Atti_Send.Yaw = gimbal_atti.yaw;
Gimbal_Atti_Send.Roll = gimbal_atti.roll;
// 矫正摄像头安装角度误差
Gimbal_Atti_Send.Pitch += CAMERA_PITCH_FIX;
Gimbal_Atti_Send.Yaw += CAMERA_YAW_FIX;
utils_norm_circle_number(&Gimbal_Atti_Send.Yaw, -180.f, 360.f);
IntToChar_Temp.f = Gimbal_Atti_Send.Pitch;
GimbalAtti_Sendmsg.data[0] = IntToChar_Temp.c[0];
GimbalAtti_Sendmsg.data[1] = IntToChar_Temp.c[1];
GimbalAtti_Sendmsg.data[2] = IntToChar_Temp.c[2];
GimbalAtti_Sendmsg.data[3] = IntToChar_Temp.c[3];
IntToChar_Temp.f = Gimbal_Atti_Send.Yaw;
GimbalAtti_Sendmsg.data[4] = IntToChar_Temp.c[0];
GimbalAtti_Sendmsg.data[5] = IntToChar_Temp.c[1];
GimbalAtti_Sendmsg.data[6] = IntToChar_Temp.c[2];
GimbalAtti_Sendmsg.data[7] = IntToChar_Temp.c[3];
IntToChar_Temp.f = Gimbal_Atti_Send.Roll;
GimbalAtti_Sendmsg.data[8] = IntToChar_Temp.c[0];
GimbalAtti_Sendmsg.data[9] = IntToChar_Temp.c[1];
GimbalAtti_Sendmsg.data[10] = IntToChar_Temp.c[2];
GimbalAtti_Sendmsg.data[11] = IntToChar_Temp.c[3];
//1ms对时
AttiSend_2ms_Count = Count_2ms % 1000;
if (AttiSend_2ms_Count == 0)
SendCount = 0;
else
SendCount++;
GimbalAtti_Sendmsg.data[12] = (AttiSend_2ms_Count >> 0) & 0xFF;
GimbalAtti_Sendmsg.data[13] = (AttiSend_2ms_Count >> 8) & 0xFF;
sum = 0;
for (fori = 0; fori < 14; fori++)
sum += GimbalAtti_Sendmsg.data[fori];
GimbalAtti_Sendmsg.data[14] = sum;
#ifdef AIMBOT_COM_COLLECT_PACKET_LOSS_RATE
++send_Package_Num;
#endif /* AIMBOT_COM_COLLECT_PACKET_LOSS_RATE */
/* 发送 */
#ifndef AIMBOT_CIMMUNICATION_USING_CAN
Aimbot_Write_UART_Data(&aimbot_device, 1, GimbalAtti_Sendmsg.data, 15);
#else
rt_device_write(aimbot_device, 0, &GimbalAtti_Sendmsg, sizeof(GimbalAtti_Sendmsg));
#endif
}
这里面所有发送的float都用了联合体和视觉进行通信
10.28更新：
调试时出现jlink找不到的现象，要先排除硬件原因，更换jlink和数据线，不然会很痛苦
不要更改rtt库！！！之前把can.h中data从8位改成了32位会导致失去通用性
gitee推送
更新了从视觉接收的时间，由一字节变成四字节整型
10.31更新
之前会有当视觉重启时发生两边通信错误的情况，经排查是视觉重启会造成串口数据堵住。老代码不会出现可能是因为使用的can通信，而且之前的数据位数比较少。
于是增大了串口接收缓冲区至128，新增了两次接收数据间超时则清空缓冲区的操作，修复了可能出现的只有头帧没有数据长度的情况。
浙公网安备 33010602011771号