效果展示
话不多说,先上激光雷达数据显示效果图:
激光雷达用的是雷达大叔的二手货,基本上长这个样子:
操作步骤
要生成激光雷达图像,首先就是获取串口数据了,所以第一步就是读取串口数据:
由于是帮朋友调试,开始朋友给了错误的SDK,导致调试半天没有任何结果,索性用串口助手查看一下这货返回些什么数据:
眼神要好,有一列FA,没错,就是250,看样子是帧头,查询一下这款雷达的数据格式:
<start> <index> <speed_l> <speed_h> [Data 0] [Data 1] [Data 2] [Data 3] <checksum_l> <checksum_h>
<start>:0xFA是固定格式表明数据包开始,可用来从数据流中分割数据包
<index>:数据包的索引号,范围从0xA0 到 0xF9 (每个包4个数据)。
<speed>:有两个speedL和speedH ,它们各一个字节,共同组成转速信息。
<data>:[ Data 0] 到 [Data 3] 是四组测量数据,其中每组测量数据分别由4个字节组成,如下:
byte 0 : <distance 7:0=""> # 距离信息的0-7位
byte 1 : <“invalid data” flag> <“strength warning” flag> <distance 13:8=""> # 错误信息标志位 , 警告位, 距离信息13-8位
byte 2 : <signal strength="" 7:0=""> # 信号强度 0-7位
byte 3 : <signal strength="" 15:8=""> # 讯号强度 8-15位
距离信息的单位是mm ,整个激光雷达的测量范围大概是15cm 到6m, 只需要把距离信息的两个字节组装成一个整数即可(注意判断无效数据位为0,如果为1意味是无效的数据,需丢弃)。
<checksum>:由两个字节组成的校验码,用来校验整个数据包是否正确。
一、读取串口数据
有了上述信息之后,首先编写基本串口类,用来打开串口、读取数据:
有以下2点值得注意:
1. 使用DataReceived事件来读取数据;
2. 使用isReading和isClosing标识来保证正常关闭激光雷达(防止出现读取已关闭串口数据的情况);
还有就是,可以定义一个委托,用来读取完数据后,执行处理操作;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.IO;
using System.IO.Ports;
namespace Laser {
public class MPort {
///////////////////////////////////////////////////////////////////////////////////////////
/// Attribute(属性域)
///////////////////////////////////////////////////////////////////////////////////////////
// 串口实例
private SerialPort port;
// 串口名和串口波特率
private String portName = "COM3";
private int baudRate = 115200;
// 正在关闭、正在读取串口状态
private bool isClosing;
private bool isReading;
// 串口返回数据
private byte[] receData = new byte[2048];
private Object receLock = new object();
// 定义一个委托
public delegate void processData(byte[] receData);
public processData processDel;
///////////////////////////////////////////////////////////////////////////////////////////
/// Attribute Modify(属性域修改)
///////////////////////////////////////////////////////////////////////////////////////////
/// <summary>
/// 串口名
/// </summary>
public String PortName{
get{ return portName;}
set{ portName = value;}
}
/// <summary>
/// 波特率
/// </summary>
public int BaudRate{
get{ return baudRate;}
set{ baudRate = value;}
}
/// <summary>
/// 读取数据
/// </summary>
public byte[] ReceData {
get {
lock (receLock) {
return receData;
}
}
set {
lock (receLock) {
receData = value;
}
}
}
/// <summary>
/// 串口打开状态
/// </summary>
public bool IsOpen {
get {
return port.IsOpen;
}
}
///////////////////////////////////////////////////////////////////////////////////////////
/// Method(方法域)
///////////////////////////////////////////////////////////////////////////////////////////
/// <summary>
/// 串口实例构造方法
/// </summary>
public MPort(String portName, int baudRate = 115200) {
try {
// 尝试关闭已使用的串口
if (port != null) {
this.closePort();
}
// 尝试打开新串口
this.PortName = portName;
this.BaudRate = baudRate;
port = new SerialPort(this.PortName, this.BaudRate);
}
catch {
// "Check your port!"
}
}
/// <summary>
/// 打开串口方法
/// </summary>
/// <returns>打开是否成功</returns>
public bool openPort() {
// 注册读取串口事件
port.DataReceived -= port_DataReceived;
port.DataReceived += port_DataReceived;
// 串口未打开时尝试打开串口
if (!port.IsOpen) {
isClosing = false;
try {
port.Open();
}
catch (Exception ex) {
Console.WriteLine(ex.Message);
return false;
}
}
return true;
}
/// <summary>
/// 关闭串口方法
/// </summary>
/// <returns>关闭串口是否成功</returns>
public bool closePort() {
if (!port.IsOpen) {
return true;
}
isClosing = true;
// 等待读取事件完毕
while (isReading) {
System.Windows.Forms.Application.DoEvents();
}
// 尝试关闭串口
try {
port.Close();
}
catch {
return false;
}
return true;
}
/// <summary>
/// 读取串口事件
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
void port_DataReceived(object sender, SerialDataReceivedEventArgs e) {
// 正在关闭则返回
if (isClosing) {
return;
}
// 设置正在读取状态
isReading = true;
try {
int count = Math.Min(port.BytesToRead, ReceData.Length);
port.Read(ReceData, 0, count);
// 委托方法不为空,调用委托方法
if (processDel != null) {
processDel(ReceData);
}
}
finally {
isReading = false;
}
}
/// <summary>
/// 向串口写入数据
/// </summary>
/// <param name="buff">写入数据的内容</param>
/// <returns>是否写入成功的返回值</returns>
public bool writeData(byte[] buff) {
// 尝试向串口写入数据
try {
port.Write(buff, 0, buff.Length);
}
catch (Exception ex) {
Console.WriteLine(ex.Message);
return false;
}
return true;
}
}
}
上述代码完成后,基本的串口操作就可以通过这个类完成,如果要对激光雷达做处理,则在次类基础上继承一个激光雷达类即可:
二、处理激光雷达数据
1. 继承MPort类,生成激光雷达类;
2. 考虑到当前只有到一个激光雷达,采用单例模式设计;
3. 绑定ProcessData方法,以便读取完数据后进行处理;
4. 数据处理是根据数据格式定义来编写的;
5. 预留了一个处理后的委托;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
namespace Laser {
public class Radar: MPort {
///////////////////////////////////////////////////////////////////////////////////////////
/// Attribute(属性域)
///////////////////////////////////////////////////////////////////////////////////////////
// 距离信息
private int[] dist = new int[360];
// 强度信息
private int[] back = new int[360];
// 旋转速度
private int speed;
// 读写锁
private Object readLock = new object();
///////////////////////////////////////////////////////////////////////////////////////////
/// Attribute Modify(属性域修改)
///////////////////////////////////////////////////////////////////////////////////////////
/// <summary>
/// 距离信息
/// </summary>
public int[] Dist {
get {
lock (readLock) {
return dist;
}
}
set {
lock (readLock) {
dist = value;
}
}
}
/// <summary>
/// 强度信息
/// </summary>
public int[] Back {
get {
lock (readLock) {
return back;
}
}
set {
lock (readLock) {
back = value;
}
}
}
/// <summary>
/// 速度信息
/// </summary>
public int Speed {
get {
lock (readLock){
return speed;
}
}
set {
lock (readLock) {
speed = value;
}
}
}
///////////////////////////////////////////////////////////////////////////////////////////
/// Method(方法域)
///////////////////////////////////////////////////////////////////////////////////////////
private Radar(String portName, int baudRate = 115200):base(
portName, baudRate) {
this.processDel -= new processData(this.processData);
this.processDel += new processData(this.processData);
}
private static Radar radar;
/// <summary>
/// 单例模式,创建激光雷达类
/// </summary>
/// <param name="portName">串口号</param>
/// <param name="baudRate">波特率</param>
/// <returns>激光雷达实例</returns>
public static Radar getInstance(String portName, int baudRate = 115200) {
if (radar == null || !portName.Equals(radar.PortName)) {
radar = new Radar(portName, baudRate);
}
return radar;
}
/// <summary>
/// 数据头
/// </summary>
public static byte dHead = (byte)0xFA;
/// <summary>
/// 数据帧长度
/// </summary>
public static int dLen = 22;
/// <summary>
/// 处理激光雷达获取的数据信息
/// </summary>
/// <param name="data">获取的数据</param>
public void processData(byte[] data) {
// 该数据完整则进行处理
for (int i = 0; i < data.Length && (i + dLen - 1 < data.Length); ) {
// 寻找帧头
if (data[i] != dHead) {
++i;
continue;
}
// 索引号
int index = data[i + 1];
Speed = data[i + 2] + 256 * (data[i + 3]);
// 计算距离和强度信息
for (int j = 0; j < 4; ++j) {
int tag = (index - 160) * 4 + j;
if (tag < 0 || tag >= 360) {
break;
}
Dist[tag] = data[i + 3 + j * 4 + 1] + 256 * (data[i + 3 + j * 4 + 2] & 0x3F);
Back[tag] = data[i + 3 + j * 4 + 3] + 256 * data[i + 3 + j * 4 + 4];
}
i += dLen;
}
// 执行数据处理完成后的方法
if (afterProcessDel != null) {
afterProcessDel();
}
}
/// <summary>
/// 定义数据处理后执行方法
/// </summary>
public delegate void afterProcess();
public static afterProcess afterProcessDel;
}
}
三、绘图显示
为了进行显示,需要把从串口读到的激光雷达距离信息和强度信息画在图上,其中,画图使用winForm的Graphic,
1. 创建一张Bitmap;
2. 根据角度距离信息,计算当前测量点的坐标;
3. 在坐标处绘制一个点,其实就是实心圆,使用FillEclipse方法;
4. 此处画图的坐标比较神奇,是因为坐标原点在左上角(0,0),向下为Y轴增大方向,向右为X轴增大方向;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
namespace Laser {
public class DrawRadarImg {
///////////////////////////////////////////////////////////////////////////////////////////
/// Attribute(属性域)
///////////////////////////////////////////////////////////////////////////////////////////
// 图像宽度
private int width;
// 图像高度
private int height;
// 显示比例
private double rate = 0.05;
// 像素点大小
private static int ps = 4;
///////////////////////////////////////////////////////////////////////////////////////////
/// Attribute Modify(属性域修改)
///////////////////////////////////////////////////////////////////////////////////////////
/// <summary>
/// 图像宽度
/// </summary>
public int Width {
get { return width; }
set { width = value; }
}
/// <summary>
/// 图像高度
/// </summary>
public int Height {
get { return height; }
set { height = value; }
}
/// <summary>
/// 长度比例
/// </summary>
public double Rate {
get { return rate; }
set { rate = value; }
}
/// <summary>
/// 一个测量点大小
/// </summary>
public int PS {
get { return ps; }
set { ps = value; }
}
///////////////////////////////////////////////////////////////////////////////////////////
/// Method(方法域)
///////////////////////////////////////////////////////////////////////////////////////////
/// <summary>
/// 根据距离和强度信息画图
/// </summary>
/// <param name="dist">距离信息</param>
/// <param name="back">强度信息</param>
/// <returns></returns>
public Image drawImg(int[] dist, int[] back) {
Bitmap img = new Bitmap(400, 400);
Graphics g = Graphics.FromImage(img);
for (int i = 0; i < 360; ++i) {
// 计算当前角度、X、Y坐标(偏差90度,与设定相关)
double ang = ((i + 90) / 180.0) * Math.PI;
double x = Math.Cos(ang) * dist[i] * rate;
double y = Math.Sin(ang) * dist[i] * rate;
// 调整强度显示的颜色
Brush brush = (back[i] > 300) ? (Brushes.Red) :
(back[i] > 200) ? (Brushes.Green) :
(back[i] > 100) ? (Brushes.Blue) : Brushes.Purple;
// 画点
g.FillEllipse(brush, (int)(x + 200 - ps / 2), (int)(200 - (y - ps / 2)), ps, ps);
}
return img;
}
}
}
最后,将这些类在Form1中进行调用,代码如下:
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
namespace Laser {
public partial class Form1 : Form {
// 激光雷达对象
private Radar radar;
private DrawRadarImg dImg = new DrawRadarImg();
public Form1() {
InitializeComponent();
// 绑定更新界面方法
updateBoardDel += new updateBoard(this.updateBoardMethod);
}
/// <summary>
/// 打开关闭串口方法
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void button2_Click(object sender, EventArgs e) {
radar = Radar.getInstance(this.textBox1.Text.Trim());
Radar.afterProcessDel = new Radar.afterProcess(this.processData);
if (!radar.IsOpen) {
radar.openPort();
this.button2.Text = "closePort";
this.textBox1.Enabled = false;
}
else {
radar.closePort();
this.button2.Text = "openPort";
}
}
/// <summary>
/// 处理完数据执行画图方法
/// </summary>
public void processData() {
Image img = dImg.drawImg(radar.Dist, radar.Back);
if (updateBoardDel != null) {
this.Invoke(updateBoardDel,img);
}
}
/// <summary>
/// 更新窗口委托
/// </summary>
/// <param name="img"></param>
public delegate void updateBoard(Image img);
public updateBoard updateBoardDel;
public void updateBoardMethod(Image img) {
this.pictureBox1.Image = img;
this.textSpeed.Text = radar.Speed.ToString();
}
}
}