bool turn_on_robot::Get_Sensor_Data()
{
short transition_16=0, j=0, Header_Pos=0, Tail_Pos=0; //Intermediate variable //中间变量
uint8_t Receive_Data_Pr[RECEIVE_DATA_SIZE]={0}; //Temporary variable to save the data of the lower machine //临时变量,保存下位机数据
Stm32_Serial.read(Receive_Data_Pr,sizeof (Receive_Data_Pr)); //Read the data sent by the lower computer through the serial port //通过串口读取下位机发送过来的数据
/*//View the received raw data directly and debug it for use//直接查看接收到的原始数据,调试使用
ROS_INFO("%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x",
Receive_Data_Pr[0],Receive_Data_Pr[1],Receive_Data_Pr[2],Receive_Data_Pr[3],Receive_Data_Pr[4],Receive_Data_Pr[5],Receive_Data_Pr[6],Receive_Data_Pr[7],
Receive_Data_Pr[8],Receive_Data_Pr[9],Receive_Data_Pr[10],Receive_Data_Pr[11],Receive_Data_Pr[12],Receive_Data_Pr[13],Receive_Data_Pr[14],Receive_Data_Pr[15],
Receive_Data_Pr[16],Receive_Data_Pr[17],Receive_Data_Pr[18],Receive_Data_Pr[19],Receive_Data_Pr[20],Receive_Data_Pr[21],Receive_Data_Pr[22],Receive_Data_Pr[23]);
*/
//Record the position of the head and tail of the frame //记录帧头帧尾位置
for(j=0;j<24;j++)
{
if(Receive_Data_Pr[j]==FRAME_HEADER)
Header_Pos=j;
else if(Receive_Data_Pr[j]==FRAME_TAIL)
Tail_Pos=j;
}
if(Tail_Pos==(Header_Pos+23))
{
//If the end of the frame is the last bit of the packet, copy the packet directly to receive_data.rx
//如果帧尾在数据包最后一位,直接复制数据包到Receive_Data.rx
// ROS_INFO("1----");
memcpy(Receive_Data.rx, Receive_Data_Pr, sizeof(Receive_Data_Pr));
}
else if(Header_Pos==(1+Tail_Pos))
{
//如果帧头在帧尾后面,纠正数据位置后复制数据包到Receive_Data.rx
// If the header is behind the end of the frame, copy the packet to receive_data.rx after correcting the data location
// ROS_INFO("2----");
for(j=0;j<24;j++)
Receive_Data.rx[j]=Receive_Data_Pr[(j+Header_Pos)%24];
}
else
{
//其它情况则认为数据包有错误
// In other cases, the packet is considered to be faulty
// ROS_INFO("3----");
return false;
}
/* //Check receive_data.rx for debugging use //查看Receive_Data.rx,调试使用
ROS_INFO("%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x-%x",
Receive_Data.rx[0],Receive_Data.rx[1],Receive_Data.rx[2],Receive_Data.rx[3],Receive_Data.rx[4],Receive_Data.rx[5],Receive_Data.rx[6],Receive_Data.rx[7],
Receive_Data.rx[8],Receive_Data.rx[9],Receive_Data.rx[10],Receive_Data.rx[11],Receive_Data.rx[12],Receive_Data.rx[13],Receive_Data.rx[14],Receive_Data.rx[15],
Receive_Data.rx[16],Receive_Data.rx[17],Receive_Data.rx[18],Receive_Data.rx[19],Receive_Data.rx[20],Receive_Data.rx[21],Receive_Data.rx[22],Receive_Data.rx[23]);
*/
Receive_Data.Frame_Header= Receive_Data.rx[0]; //The first part of the data is the frame header 0X7B //数据的第一位是帧头0X7B
Receive_Data.Frame_Tail= Receive_Data.rx[23]; //The last bit of data is frame tail 0X7D //数据的最后一位是帧尾0X7D
if (Receive_Data.Frame_Header == FRAME_HEADER ) //Judge the frame header //判断帧头
{
if (Receive_Data.Frame_Tail == FRAME_TAIL) //Judge the end of the frame //判断帧尾
{
if (Receive_Data.rx[22] == Check_Sum(22,READ_DATA_CHECK)||(Header_Pos==(1+Tail_Pos))) //BBC check passes or two packets are interlaced //BBC校验通过或者两组数据包交错
{
Receive_Data.Flag_Stop=Receive_Data.rx[1]; //set aside //预留位
Robot_Vel.X = Odom_Trans(Receive_Data.rx[2],Receive_Data.rx[3]); //Get the speed of the moving chassis in the X direction //获取运动底盘X方向速度
Robot_Vel.Y = Odom_Trans(Receive_Data.rx[4],Receive_Data.rx[5]); //Get the speed of the moving chassis in the Y direction, The Y speed is only valid in the omnidirectional mobile robot chassis
//获取运动底盘Y方向速度,Y速度仅在全向移动机器人底盘有效
Robot_Vel.Z = Odom_Trans(Receive_Data.rx[6],Receive_Data.rx[7]); //Get the speed of the moving chassis in the Z direction //获取运动底盘Z方向速度
//MPU6050 stands for IMU only and does not refer to a specific model. It can be either MPU6050 or MPU9250
//Mpu6050仅代表IMU,不指代特定型号,既可以是MPU6050也可以是MPU9250
Mpu6050_Data.accele_x_data = IMU_Trans(Receive_Data.rx[8],Receive_Data.rx[9]); //Get the X-axis acceleration of the IMU //获取IMU的X轴加速度
Mpu6050_Data.accele_y_data = IMU_Trans(Receive_Data.rx[10],Receive_Data.rx[11]); //Get the Y-axis acceleration of the IMU //获取IMU的Y轴加速度
Mpu6050_Data.accele_z_data = IMU_Trans(Receive_Data.rx[12],Receive_Data.rx[13]); //Get the Z-axis acceleration of the IMU //获取IMU的Z轴加速度
Mpu6050_Data.gyros_x_data = IMU_Trans(Receive_Data.rx[14],Receive_Data.rx[15]); //Get the X-axis angular velocity of the IMU //获取IMU的X轴角速度
Mpu6050_Data.gyros_y_data = IMU_Trans(Receive_Data.rx[16],Receive_Data.rx[17]); //Get the Y-axis angular velocity of the IMU //获取IMU的Y轴角速度
Mpu6050_Data.gyros_z_data = IMU_Trans(Receive_Data.rx[18],Receive_Data.rx[19]); //Get the Z-axis angular velocity of the IMU //获取IMU的Z轴角速度
//Linear acceleration unit conversion is related to the range of IMU initialization of STM32, where the range is ±2g=19.6m/s^2
//线性加速度单位转化,和STM32的IMU初始化的时候的量程有关,这里量程±2g=19.6m/s^2
Mpu6050.linear_acceleration.x = Mpu6050_Data.accele_x_data / ACCEl_RATIO;
Mpu6050.linear_acceleration.y = Mpu6050_Data.accele_y_data / ACCEl_RATIO;
Mpu6050.linear_acceleration.z = Mpu6050_Data.accele_z_data / ACCEl_RATIO;
//The gyroscope unit conversion is related to the range of STM32's IMU when initialized. Here, the range of IMU's gyroscope is ±500°/s
//Because the robot generally has a slow Z-axis speed, reducing the range can improve the accuracy
//陀螺仪单位转化,和STM32的IMU初始化的时候的量程有关,这里IMU的陀螺仪的量程是±500°/s
//因为机器人一般Z轴速度不快,降低量程可以提高精度
Mpu6050.angular_velocity.x = Mpu6050_Data.gyros_x_data * GYROSCOPE_RATIO;
Mpu6050.angular_velocity.y = Mpu6050_Data.gyros_y_data * GYROSCOPE_RATIO;
Mpu6050.angular_velocity.z = Mpu6050_Data.gyros_z_data * GYROSCOPE_RATIO;
//Get the battery voltage
//获取电池电压
transition_16 = 0;
transition_16 |= Receive_Data.rx[20]<<8;
transition_16 |= Receive_Data.rx[21];
Power_voltage = transition_16/1000+(transition_16 % 1000)*0.001; //Unit conversion millivolt(mv)->volt(v) //单位转换毫伏(mv)->伏(v)
return true;
}
}
}
return false;
} |