Skip to content

Commit 48a847c

Browse files
author
“寧々”
committed
2021-08-25 V1.01
1 parent bf331b3 commit 48a847c

File tree

7 files changed

+210
-12
lines changed

7 files changed

+210
-12
lines changed

‎MiniCarLib/Core/ComDataFactory.cs‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,8 @@ public static QianComData CreateInstance(DataFunctionType fun)
2626
case DataFunctionType.UnregisterRequest:return new UnregisterRequestData();
2727
case DataFunctionType.UnregisterResponse:return new UnregisterResponseData();
2828
case DataFunctionType.CustomData:return new CustomData();
29+
case DataFunctionType.EmergencyStop:return new EmergencyStopData();
30+
case DataFunctionType.ErrorReport:return new ErrorReportData();
2931
default:return null;
3032
}
3133
}

‎MiniCarLib/Core/Util.cs‎

Lines changed: 79 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -12,28 +12,39 @@ namespace MiniCarLib.Core
1212
{
1313
public static class Util
1414
{
15-
public static Tuple<string,IPAddress>[] GetAllNetInterfaceV4()
15+
public static NetWorkInterfaceInfo[] GetAvalibleNetInterfaceV4()
1616
{
17-
List<Tuple<string, IPAddress>> rst = new List<Tuple<string, IPAddress>>();
18-
rst.Add(new Tuple<string,IPAddress>("Any", IPAddress.Any));
17+
List<NetWorkInterfaceInfo> rst = new List<NetWorkInterfaceInfo>();
18+
rst.Add(new NetWorkInterfaceInfo()
19+
{
20+
InterfaceID = "Any",
21+
InterfaceName = "Any",
22+
IP = IPAddress.Any,
23+
Mask = IPAddress.Any
24+
}) ;
1925
var all = NetworkInterface.GetAllNetworkInterfaces();
2026
foreach(var it in all)
2127
{
2228
if (it.OperationalStatus == OperationalStatus.Up)
2329
{
2430
var addrs = it.GetIPProperties().UnicastAddresses;
25-
IPAddress a = IPAddress.Any;
31+
//IPAddress a = IPAddress.Any;
2632
foreach(var addr in addrs)
2733
{
2834
if(addr.Address.AddressFamily == System.Net.Sockets.AddressFamily.InterNetwork)
2935
{
30-
a = addr.Address;
36+
rst.Add(new NetWorkInterfaceInfo()
37+
{
38+
InterfaceID = it.Id,
39+
InterfaceName = it.Name,
40+
IP = addr.Address,
41+
Mask = addr.IPv4Mask
42+
}) ;
3143
}
3244
}
33-
rst.Add(new Tuple<string, IPAddress>(it.Name,a));
45+
3446
}
3547
}
36-
3748
return rst.ToArray();
3849
}
3950

@@ -71,9 +82,69 @@ public static void HalfWordToByte(ushort hword,ref byte hi,ref byte lo)
7182
lo = (byte)(hword & 0xFF);
7283
}
7384

85+
public static IPAddress GetLocalIpInSameSubnet(IPAddress ip)
86+
{
87+
var all = GetAvalibleNetInterfaceV4();
88+
foreach(var a in all)
89+
{
90+
if (ip.IsInSameSubnet(a.IP, a.Mask))
91+
return a.IP;
92+
}
93+
return IPAddress.Any;
94+
}
95+
7496
public static string SourceCodeURL => "https://www.google.com";
7597
public static string AuthorBlogURL => "https://blog.jloli.cc";
7698
}
7799

78-
100+
public static class IPAddressExtensions
101+
{
102+
public static IPAddress GetBroadcastAddress(this IPAddress address, IPAddress subnetMask)
103+
{
104+
byte[] ipAdressBytes = address.GetAddressBytes();
105+
byte[] subnetMaskBytes = subnetMask.GetAddressBytes();
106+
107+
if (ipAdressBytes.Length != subnetMaskBytes.Length)
108+
throw new ArgumentException("Lengths of IP address and subnet mask do not match.");
109+
110+
byte[] broadcastAddress = new byte[ipAdressBytes.Length];
111+
for (int i = 0; i < broadcastAddress.Length; i++)
112+
{
113+
broadcastAddress[i] = (byte)(ipAdressBytes[i] | (subnetMaskBytes[i] ^ 255));
114+
}
115+
return new IPAddress(broadcastAddress);
116+
}
117+
118+
public static IPAddress GetNetworkAddress(this IPAddress address, IPAddress subnetMask)
119+
{
120+
byte[] ipAdressBytes = address.GetAddressBytes();
121+
byte[] subnetMaskBytes = subnetMask.GetAddressBytes();
122+
123+
if (ipAdressBytes.Length != subnetMaskBytes.Length)
124+
throw new ArgumentException("Lengths of IP address and subnet mask do not match.");
125+
126+
byte[] broadcastAddress = new byte[ipAdressBytes.Length];
127+
for (int i = 0; i < broadcastAddress.Length; i++)
128+
{
129+
broadcastAddress[i] = (byte)(ipAdressBytes[i] & (subnetMaskBytes[i]));
130+
}
131+
return new IPAddress(broadcastAddress);
132+
}
133+
134+
public static bool IsInSameSubnet(this IPAddress address2, IPAddress address, IPAddress subnetMask)
135+
{
136+
IPAddress network1 = address.GetNetworkAddress(subnetMask);
137+
IPAddress network2 = address2.GetNetworkAddress(subnetMask);
138+
139+
return network1.Equals(network2);
140+
}
141+
}
142+
143+
public class NetWorkInterfaceInfo
144+
{
145+
public string InterfaceID { get; set; }
146+
public string InterfaceName { get; set; }
147+
public IPAddress IP { get; set; }
148+
public IPAddress Mask { get; set; }
149+
}
79150
}

‎MiniCarLib/QianCarAPI.cs‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11

22
using MiniCarLib.Core;
3+
using System.Net;
34

45
namespace MiniCarLib
56
{
@@ -10,6 +11,7 @@ namespace MiniCarLib
1011
public delegate void OnApplyForLeaveHandler(QianCar car, QianMapPoint point);
1112
public delegate void OnCarLeaveConfirmedHandler(QianCar car);
1213
public delegate void OnCarUnregisterHandler(QianCar car);
14+
public delegate void OnCarErrorReportedHandler(QianCar car, CarErrorState errcode, byte[] errinfo);
1315
public static class QianCarAPI
1416
{
1517
static QianCarController controller;
@@ -26,6 +28,7 @@ public static class QianCarAPI
2628
public static event OnCarLeaveConfirmedHandler OnCarLeft;
2729
public static event OnCarUnregisterHandler OnCarUnregisterRequest;
2830
public static event OnCarUnregisterHandler OnCarUnregisterResponse;
31+
public static event OnCarErrorReportedHandler OnCarErrorReported;
2932

3033
private static void InitEvents()
3134
{
@@ -47,8 +50,15 @@ private static void InitEvents()
4750
controller.OnCarLeaveConfirm += (car, data) => OnCarLeft?.Invoke((QianCar)car);
4851
controller.OnUnregisterRequest += (car, data) => OnCarUnregisterRequest?.Invoke((QianCar)car);
4952
controller.OnUnregisterResponse += (car, data) => OnCarUnregisterResponse?.Invoke((QianCar)car);
53+
controller.OnErrorReport += (car, data) =>
54+
{
55+
var obj = (ErrorReportData)data;
56+
OnCarErrorReported?.Invoke((QianCar)car, obj.ErrorCode, obj.ErrorData);
57+
};
5058
}
5159

60+
61+
5262
public static void Init(ushort serverID,ushort regport,ushort dataport, string mapfile, byte[] password = null)
5363
{
5464
if (!_inited)
@@ -61,6 +71,18 @@ public static void Init(ushort serverID,ushort regport,ushort dataport, string m
6171
InitEvents();
6272
}
6373
}
74+
public static void Init(ushort serverID,IPAddress serverip, ushort regport, ushort dataport, string mapfile, byte[] password = null)
75+
{
76+
if (!_inited)
77+
{
78+
controller = new QianCarController(serverID, serverip, regport, dataport);
79+
controller.SetMap(mapfile);
80+
if (password != null)
81+
controller.PassWord = password;
82+
83+
InitEvents();
84+
}
85+
}
6486
public static void Init(QianCarController ct, string mapfile, byte[] password = null)
6587
{
6688
if (!_inited)
@@ -85,5 +107,6 @@ public static void Init(QianCarController ct, string mapfile, byte[] password =
85107
public static void CallCarLeave(QianCar car, bool allow) => controller.CallCarLeave(car, allow);
86108
public static void UnregisterCar(QianCar car) => controller.UnregisterCar(car);
87109
public static void ConfirmUnregteration(QianCar car, bool allow, bool removecar = true) => controller.ConfirmUnregteration(car, allow, removecar);
110+
public static void EmergencyStopCar(QianCar car) => controller.EmergencyStopCar(car, 0x00);
88111
}
89112
}

‎MiniCarLib/QianCarController.cs‎

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@ public class QianCarController
2323
public Func<CarType,ICar> CreateCarFunc { get; set; } = CarFactory.CreateInstance;
2424
public Func<RegisterRequestData, bool> CheckRegisteration { get; set; }
2525

26+
2627
public event OnCarEventHandler BeforeRegistering;
2728
public event OnCarEventHandler AfterRegistered;
2829
public event OnCarEventHandler BeforeReportState;
@@ -35,6 +36,7 @@ public class QianCarController
3536
public event OnCarEventHandler OnCarLeaveConfirm;
3637
public event OnCarEventHandler OnUnregisterRequest;
3738
public event OnCarEventHandler OnUnregisterResponse;
39+
public event OnCarEventHandler OnErrorReport;
3840
public event OnCustomDataEventHandler OnCustomData;
3941

4042
protected void _init(ushort serverid)
@@ -54,6 +56,15 @@ public QianCarController(ushort serverid,ushort regport,ushort dataport)
5456
_init(serverid);
5557
Server.InitRegServer(IPAddress.Any, regport);
5658
Server.InitDataServer(IPAddress.Any, dataport);
59+
60+
}
61+
62+
public QianCarController(ushort serverid, IPAddress serverip, ushort regport, ushort dataport)
63+
{
64+
_init(serverid);
65+
Server.InitRegServer(serverip, regport);
66+
Server.InitDataServer(serverip, dataport);
67+
5768
}
5869

5970
public void StartServer()
@@ -83,6 +94,11 @@ public virtual void ConfirmRegisteration(QianCar car,bool conf,ushort id)
8394
RegisterResponseData res = (RegisterResponseData)QianComDataFactory.CreateInstance(DataFunctionType.RegisterResponse);
8495
res.AllowRegister = conf;
8596
res.AllocatedID = id;
97+
if (Server.DataServer.ListenIP != IPAddress.Any)
98+
res.ServerIP = Server.DataServer.ListenIP;
99+
else
100+
res.ServerIP = Util.GetLocalIpInSameSubnet(((CarUDPClient)car.ComClient).Client.Address);
101+
res.ServerPort = Server.DataServer.ListenPort;
86102
SendCarDataPack(car, res);
87103
}
88104

@@ -137,6 +153,14 @@ public virtual void ConfirmUnregteration(QianCar car,bool allow,bool removecar=t
137153
SendCarDataPack(car, res);
138154
}
139155

156+
public virtual void EmergencyStopCar(QianCar car,byte code)
157+
{
158+
var data = (EmergencyStopData)QianComDataFactory.CreateInstance(DataFunctionType.EmergencyStop);
159+
data.EmergencyCode = code;
160+
car.State = CarState.EmergencyStop;
161+
SendCarDataPack(car, data);
162+
}
163+
140164
protected virtual void Server_OnCarDataReceived(IComClient client, QianComHeader header, QianComData data)
141165
{
142166
if(header.FuncType == DataFunctionType.RegisterRequest)
@@ -181,12 +205,23 @@ protected virtual void Server_OnCarDataReceived(IComClient client, QianComHeader
181205
OnUnregisterResponse?.Invoke(car, data);
182206
UnregisterResponseHandler(car, (UnregisterResponseData)data);
183207
break;
208+
case DataFunctionType.ErrorReport:
209+
ErrorReportHandler(car, (ErrorReportData)data);
210+
OnErrorReport?.Invoke(car, data);
211+
break;
184212
default:
185213
OnCustomData?.Invoke(car, header, data);
186214
break;
187215
}
188216
}
189217

218+
public virtual void ErrorReportHandler(QianCar car,ErrorReportData data)
219+
{
220+
car.State = CarState.Error;
221+
car.ErrorState = data.ErrorCode;
222+
car.ErrorInfo = data.ErrorData;
223+
}
224+
190225
public virtual void UnregisterResponseHandler(QianCar car, UnregisterResponseData data)
191226
{
192227
if(data.AllowUnregiser)

‎MiniCarLib/QianCarData.cs‎

Lines changed: 67 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@
22
using System.Collections.Generic;
33
using System.IO;
44
using System.Linq;
5+
using System.Net;
56
using System.Runtime.InteropServices;
67
using System.Text;
78
using System.Threading.Tasks;
@@ -20,7 +21,9 @@ public enum CarState : byte
2021
WaitingInbound = 4,
2122
Idle = 5,
2223
Running = 6,
23-
WaitingOutbound = 7
24+
WaitingOutbound = 7,
25+
EmergencyStop = 8,
26+
Error = 0xFF,
2427
}
2528

2629
public enum CarErrorState : byte
@@ -47,6 +50,8 @@ public enum DataFunctionType : byte
4750
CarLeaveConfirm = 0x0B,
4851
UnregisterRequest = 0x0C,
4952
UnregisterResponse = 0x0D,
53+
EmergencyStop = 0x0E,
54+
5055

5156
CustomData = 0xFF,
5257
}
@@ -167,7 +172,10 @@ public class RegisterResponseData : QianComData
167172

168173
public ushort AllocatedID { get; set; }
169174

170-
public override byte DataLen => 3;
175+
public IPAddress ServerIP { get; set; }
176+
public ushort ServerPort { get; set; }
177+
178+
public override byte DataLen => 9;
171179

172180
public override DataFunctionType FuncType => DataFunctionType.RegisterResponse;
173181

@@ -176,13 +184,22 @@ public override void FillByteData(byte[] data, int offset)
176184
ComDataWriter dw = new ComDataWriter(data, offset);
177185
dw.WriteBoolean(AllowRegister);
178186
dw.WriteHalfWord(AllocatedID);
187+
if (ServerIP != null)
188+
dw.WriteByteArray(ServerIP.GetAddressBytes(), 0, 4);
189+
else
190+
dw.SeekRelevant(4);
191+
dw.WriteHalfWord(ServerPort);
179192
}
180193

181194
public override void ParseByteData(byte[] data, int offset)
182195
{
183196
ComDataReader dr = new ComDataReader(data, offset);
184197
AllowRegister = dr.ReadBoolean();
185198
AllocatedID = dr.ReadHalfWord();
199+
byte[] arr = new byte[4];
200+
dr.ReadByteArray(arr, 0, 4);
201+
ServerIP = new IPAddress(arr);
202+
ServerPort = dr.ReadHalfWord();
186203
}
187204
}
188205

@@ -471,4 +488,52 @@ public override void ParseByteData(byte[] data, int offset)
471488
}
472489
}
473490

491+
public class EmergencyStopData : QianComData
492+
{
493+
public byte EmergencyCode { get; set; }
494+
public override byte DataLen => 1;
495+
496+
public override DataFunctionType FuncType => DataFunctionType.EmergencyStop;
497+
498+
public override void FillByteData(byte[] data, int offset)
499+
{
500+
data[offset] = EmergencyCode;
501+
}
502+
503+
public override void ParseByteData(byte[] data, int offset)
504+
{
505+
EmergencyCode = data[offset];
506+
}
507+
}
508+
509+
public class ErrorReportData : QianComData
510+
{
511+
public CarErrorState ErrorCode { get; set; }
512+
public byte ErrorDataLen => (byte)(ErrorData?.Length ?? 0);
513+
514+
public byte[] ErrorData { get; set; }
515+
516+
public override byte DataLen => (byte)(ErrorDataLen + 2);
517+
518+
public override DataFunctionType FuncType => DataFunctionType.ErrorReport;
519+
520+
public override void FillByteData(byte[] data, int offset)
521+
{
522+
ComDataWriter dw = new ComDataWriter(data, offset);
523+
dw.WriteEnum<CarErrorState>(ErrorCode);
524+
dw.WriteByte(ErrorDataLen);
525+
if (ErrorDataLen > 0)
526+
dw.WriteByteArray(ErrorData, 0, ErrorDataLen);
527+
}
528+
529+
public override void ParseByteData(byte[] data, int offset)
530+
{
531+
ComDataReader dr = new ComDataReader(data, offset);
532+
ErrorCode = dr.ReadEnum<CarErrorState>();
533+
byte len = dr.ReadByte();
534+
if (len > 0)
535+
dr.ReadByteArray(ErrorData = new byte[len], 0, len);
536+
}
537+
}
538+
474539
}

‎MiniCarLib/QianCarServer.cs‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,8 @@ public class QianCarServer
1515
UDPDriver udpserver = new UDPDriver();
1616
TCPDriver tcpserver = new TCPDriver();
1717
public ushort ServerID { get; }
18+
public UDPDriver RegServer => udpserver;
19+
public TCPDriver DataServer => tcpserver;
1820

1921
public event OnCarDataRecDlg OnCarDataReceived;
2022

0 commit comments

Comments
 (0)