private void UpdateDeviceCatalogue(object stateInfo)
{
CANFrame frame = (CANFrame)stateInfo;
DeviceCatalogueInfo dci = frame.ToDeviceCatalogueInfo();
if (dci == null)
{
return;
}
CNXLog.InfoFormat("DeviceCataloge {0}", dci.ToString());
DeviceCatalogue.CatalogueChangeType catalogeUpdated = mDeviceCatalogue.UpdateDeviceCatalogue(dci);
if (catalogeUpdated != DeviceCatalogue.CatalogueChangeType.NONE)
{
CNXLog.InfoFormat("DeviceCataloge Updating calalogue.");
if (catalogeUpdated == DeviceCatalogue.CatalogueChangeType.EQUIPMENT)
{
// persist the catalogue
PersistCatalogue();
}
EquipmentChangedEventArgs a = new EquipmentChangedEventArgs(mDeviceCatalogue.EquipmentMask, mDeviceCatalogue.EquipmentStatus);
OnRaiseEquipmentChangedEvent(a);
}
}
private void SendBlockRequest()
{
CANFrame frame = new CANFrame();
frame.MailboxId = (uint)CNXMsgIds.BlockQuery + (uint)mBlockId;
mClient.Send(frame);
}
private void Can_MessageReceived(CANFrame frame, object msg, byte transferID)
{
if (msg.GetType().Name == targettype)
{
this.BeginInvokeIfRequired(() => {
var item = msg.ToJSON(Newtonsoft.Json.Formatting.Indented);
var lines = item.Split(new char[] { '\n', '\r' }, System.StringSplitOptions.RemoveEmptyEntries);
var newlines = txt_packet.Lines.OfType <string>().ToList();
newlines.AddRange(lines);
if (newlines.Count > num_lines.Value)
{
txt_packet.Lines = newlines.Skip(txt_packet.Lines.Length - (int)num_lines.Value).ToArray();
}
else
{
txt_packet.Lines = newlines.ToArray();
}
});
}
if (!msgtypes.Contains(msg.GetType().Name))
{
msgtypes.Add(msg.GetType().Name);
}
}
private void OnTimedEvent(object sourcea)
{
CANFrame frame = new CANFrame();
try
{
// now config Block Requests
frame.MailboxId = (uint)CNXMsgIds.BlockQuery + (uint)Block.RouteConfig;
mCANClient.Send(frame);
frame.MailboxId = (uint)CNXMsgIds.BlockQuery + (uint)Block.VehicleConfig;
mCANClient.Send(frame);
frame.MailboxId = (uint)CNXMsgIds.BlockQuery + (uint)Block.ServiceAlert;
mCANClient.Send(frame);
frame.MailboxId = (uint)CNXMsgIds.BlockQuery + (uint)Block.DriverConfig;
mCANClient.Send(frame);
// test the status every few cycles
if (++mTestCatalogueCount > TestCatalogueCycles)
{
// age the device catalogue states
if (mDeviceCatalogue.AgeCatalogue() == CANLib.DeviceCatalogue.CatalogueChangeType.STATUS)
{
EquipmentChangedEventArgs args = new EquipmentChangedEventArgs(mDeviceCatalogue.EquipmentMask, mDeviceCatalogue.EquipmentStatus);
OnRaiseEquipmentChangedEvent(args);
}
mTestCatalogueCount = 0;
}
}
catch (Exception e)
{
CNXLog.Error(e.ToString());
}
}
public void TC_CycleTimeCheck(int nTime)
{
// レポート出力
Report.TestCaseTitle("Cycle Time Check");
Report.TestCaseComment("Test pattern begin");
// 監視開始
CANFrame frame1 = new CANFrame(0x123, 8);
CANFrame frame2 = new CANFrame(0x321, 8);
ICheck check1 = new AbsoluteCycleTimeCheck <CANFrame>(frame1, CheckType.Condition, 90, 110);
ICheck check2 = new AbsoluteCycleTimeCheck <CANFrame>(frame2, CheckType.Condition, 190, 210);
check1.Activate();
check2.Activate();
// 指定時間待ち
Execution.Wait(nTime);
// 監視完了
check1.Deactivate();
check2.Deactivate();
check1.Dispose();
check2.Dispose();
// レポート出力
Report.TestCaseComment("Test pattern end");
}
/// <summary> Reads a CAN frame asynchronously, returning the payload and the ID of the received CAN frame. </summary>
/// <returns> A task that will return a tuple, with the first element being the ID of the received CAN frame and the second being the payload </returns>
public Task <Tuple <uint, byte[]> > ReadAsync()
{
return(Task.Run(() =>
{
if (CANFD)
{
CANFDFrame Frame = new CANFDFrame();
read(Socket, ref Frame, Marshal.SizeOf(Frame));
byte[] Payload = new byte[Frame.DataLength];
unsafe
{
for (int i = 0; i < Frame.DataLength; i++)
{
Payload[i] = Frame.Data[i];
}
}
return new Tuple <uint, byte[]>(Frame.CANID, Payload);
}
else
{
CANFrame Frame = new CANFrame();
read(Socket, ref Frame, Marshal.SizeOf(Frame));
byte[] Payload = new byte[Frame.DataLength];
unsafe
{
for (int i = 0; i < Frame.DataLength; i++)
{
Payload[i] = Frame.Data[i];
}
}
return new Tuple <uint, byte[]>(Frame.CANID, Payload);
}
}));
}
private void GPSPositionChangedEventHandler(object sender, EventArgs e)
{
try
{
NMEAGPSClient.NMEAGPSClient.GPSPositionEvent gpsPosition = (NMEAGPSClient.NMEAGPSClient.GPSPositionEvent)e;
GPSPosition position = gpsPosition.Position;
NMEAGPSClient.NMEAGPSClient nmeaClient = (NMEAGPSClient.NMEAGPSClient)sender;
// make sure we have valid data
byte gpsState = (byte)position.NMEAMode;
if (position.HorizontalErrorEstimate > TrackingService.DefaultAcceptableError)
{
gpsState &= 0xfe;
}
// publish the new position to CAN clients.
CANFrame frame = CNXCANMsgHelper.PackGPSFrame(gpsState, position.CurrentPosition.Latitude, position.CurrentPosition.Longitude, position.SpeedOverGround);
int count = mCANClient.Send(frame);
if ((gpsState == (byte)CANGPSState.GoodFix) || (gpsState == (byte)CANGPSState.PoorFix))
{
// publish the new time.
frame = CNXCANMsgHelper.PackDateTime(position.TimeStamp);
count = mCANClient.Send(frame);
}
CNXLog.Debug("GPSPositionChangedEventHandler");
}
catch (Exception ex)
{
CNXLog.Error("CAN GPSPositionChangedEventHandler", ex);
}
}
/////
// CANframeの内容を出力する (デバッグ用)
// e.g. 16.1702 1 123 Rx d 8 01 02 03 04 05 06 07 08
private void printCANFrame(CANFrame frame)
{
Vector.Tools.Output.WriteLine(
((double)frame.TimeNS / 1000000000.0).ToString("F4") + " "
+ frame.Channel.ToString() + " "
+ frame.ID.ToString("X3") + " " // [ref3]
+ "Rx d " + frame.DLC.ToString() + " "
+ string.Join(" ", frame.Bytes.Select(b => b.ToString("X2")).ToArray()) //[ref1],[ref2]
);
}
private void SendBlock(Object stateInfo)
{
bool sendComplete = false;
try
{
// build a chunk 1 message
CANFrame chunk1 = BuildChunk1(mBlockId, mBlockCRC, (uint)mBlockData.Length, mBlockVersion, 0);
// send chunk 1
mClient.Send(chunk1);
// transfer the block
CANFrame frame = new CANFrame();
frame.MailboxId = (uint)CNXMsgIds.BlockChunkN + (uint)mBlockId;
byte[] data = new byte[8];
for (uint offset = 0; offset < mBlockData.Length; offset = mLastOffset > offset ? mLastOffset : offset + 4)
//for (uint offset = 0; offset < mBlockData.Length; offset += 4)
{
// set the offset
BitConverter.GetBytes(offset).CopyTo(data, 0);
if (!BitConverter.IsLittleEndian)
{
Array.Reverse(data, 0, 4);
}
uint remainder = (uint)(mBlockData.LongLength - offset);
int count = (int)((remainder > 4) ? 4 : remainder);
Array.Copy(mBlockData, offset, data, 4, count);
frame.DataFromArray(data, 0, 4 + count);
int sent = mClient.Send(frame);
// pause to give CPC's time to commit to flash
// 40mS every 10 frames.
if (offset % 10 == 0)
{
Thread.Sleep(100);
}
Thread.Sleep(1);
for (int retry = 1; sent < 1 && retry < 10; ++retry)
{
Thread.Sleep(retry * 5);
sent = mClient.Send(frame);
}
}
// send chunk 1 for good luck
Thread.Sleep(40);
mClient.Send(chunk1);
sendComplete = true;
Thread.Sleep(40);
}
catch (Exception e)
{
CNXLog.ErrorFormat("SendBlock error - {0}", e.ToString());
}
BlockStateMachine(sendComplete ? BlockEvent.SendComplete : BlockEvent.SendFailed);
}
[OnCANFrame(1, 0x123)] // [OnCANFrame(byte channnel, int32 id)] @ OnCANFrameAttribute Constructor (Byte, Int32)
public void CANFrameReceived(CANFrame frame)
{
// printCANFrame(frame);
if (frame.Bytes[0] == 0x0)
{
power_off_delay.Stop();
power_off_delay.Start();
}
else
{
power_off_delay.Stop();
Texio.RequestVoltage.Value = 13.0;
}
}
private void GPSStatusChangedEventHandler(object sender, EventArgs e)
{
NMEAGPSClient.NMEAGPSClient.GPSStatusEvent gpsStatus = (NMEAGPSClient.NMEAGPSClient.GPSStatusEvent)e;
mGPSStatus = gpsStatus.Status;
if (mGPSStatus < NMEAGPSClient.NMEAGPSClient.GPS_STATUS.Fix)
{
GPSPosition.Position position = ((NMEAGPSClient.NMEAGPSClient)sender).Position;
// publish the new position to CAN clients.
CANFrame frame = CNXCANMsgHelper.PackGPSFrame((byte)mGPSStatus, position.Latitude, position.Longitude, 0);
int count = mCANClient.Send(frame);
}
CNXLog.Debug("GPSStatusChangedEventHandler");
}
/// <summary> Write a payload with specified ID </summary>
/// <param name="ID"> ID of CAN Frame </param>
/// <param name="Data"> Payload of CAN Frame. Must be at most 8 bytes. </param>
public void Write(uint ID, byte[] Data)
{
if (!CANFD && Data.Length > 8)
{
throw new Exception("CAN Data Length must be no more than 8 bytes for non-Extended frames");
}
else if (Data.Length > 64)
{
throw new Exception("CAN Data Length must be no more than 64 bytes for Extended frames.");
}
int BytesWritten;
unsafe
{
if (CANFD)
{
CANFDFrame Frame = new CANFDFrame();
Frame.CANID = ID | CAN_EFF_FLAG;
Frame.DataLength = DLCToLength(LengthToDLC((byte)Data.Length));
for (int i = 0; i < Data.Length; i++)
{
Frame.Data[i] = Data[i];
}
BytesWritten = write(Socket, ref Frame, Marshal.SizeOf(Frame));
}
else
{
CANFrame Frame = new CANFrame();
Frame.CANID = ID;
Frame.DataLength = DLCToLength(LengthToDLC((byte)Data.Length));
for (int i = 0; i < Data.Length; i++)
{
Frame.Data[i] = Data[i];
}
BytesWritten = write(Socket, ref Frame, Marshal.SizeOf(Frame));
}
}
if (BytesWritten < 0)
{
throw new Exception("Failed to write CAN frame. Error code: " + Marshal.GetLastWin32Error());
}
}
/// <summary>
/// Builds a Chunck 1 frame based on supplied data.
/// </summary>
/// <param name="blockId">Id of the block.</param>
/// <param name="crc">CRC of the complete block data.</param>
/// <param name="size">Size of the block if known.</param>
/// <param name="version">The version of the block or -1 if unknown.</param>
/// <param name="flags">Any flags that should be included.</param>
/// <returns>A chunk1 frame redy for transmission.</returns>
/// <remarks>
/// The chunck1 will be formatted based on the suppiled data. No size will be included if size is set to zeor.
/// No version will be included unless the size is valid and the verion is +ve.
/// </remarks>
public static CANFrame BuildChunk1(byte blockId, ushort crc, uint size, int version, byte flags)
{
CANFrame frame = new CANFrame();
frame.MailboxId = (uint)CNXMsgIds.BlockChunk1 + (uint)blockId;
bool verIncluded = (version != -1);
byte[] data = new byte[verIncluded ? 8 : 7];
// set the flags
if (verIncluded)
{
flags |= (byte)CANLib.BlockFlags.VersionPresent;
}
data[2] = flags;
// set the CRC
BitConverter.GetBytes(crc).CopyTo(data, 0);
if (!BitConverter.IsLittleEndian)
{
Array.Reverse(data, 0, 2);
}
// set the size
BitConverter.GetBytes(size).CopyTo(data, 3);
if (!BitConverter.IsLittleEndian)
{
Array.Reverse(data, 3, 4);
}
// set the version
if (verIncluded)
{
data[7] = (byte)(version & 0xff);
}
frame.Data = data;
return(frame);
}
private static extern int write(int FileDescriptor, ref CANFrame Frame, int Size);
private void StartMavlinkCAN(byte bus = 1)
{
BusInUse = bus;
but_slcanmode1.Enabled = false;
but_mavlinkcanmode2.Enabled = true;
but_mavlinkcanmode2_2.Enabled = true;
but_filter.Enabled = true;
Task.Run(() =>
{
// allows old instance to exit
Thread.Sleep(1000);
mavlinkCANRun = true;
// send every second, timeout is in 5 seconds
while (mavlinkCANRun)
{
try
{
// setup forwarding on can port 1
var ans = MainV2.comPort.doCommand((byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent, MAVLink.MAV_CMD.CAN_FORWARD, bus, 0, 0, 0, 0, 0, 0, false);
if (ans == false) // MAVLink.MAV_RESULT.UNSUPPORTED)
{
//return;
}
}
catch (Exception ex) { }
if (mavlinkCANRun)
{
Thread.Sleep(1000);
}
}
});
var port = new CommsInjection();
var can = new DroneCAN.DroneCAN();
can.FrameReceived += (frame, payload) =>
{
//https://github.com/dronecan/pydronecan/blob/master/dronecan/driver/mavcan.py#L114
//if frame.extended:
// message_id |= 1 << 31
if (payload.packet_data.Length > 8)
{
MainV2.comPort.sendPacket(new MAVLink.mavlink_canfd_frame_t(BitConverter.ToUInt32(frame.packet_data, 0) + (frame.Extended ? 0x80000000: 0),
(byte)MainV2.comPort.sysidcurrent,
(byte)MainV2.comPort.compidcurrent, (byte)(bus - 1), (byte)DroneCAN.DroneCAN.dataLengthToDlc(payload.packet_data.Length),
payload.packet_data),
(byte)MainV2.comPort.sysidcurrent,
(byte)MainV2.comPort.compidcurrent);
}
else
{
var frame2 = new MAVLink.mavlink_can_frame_t(BitConverter.ToUInt32(frame.packet_data, 0) + (frame.Extended ? 0x80000000 : 0),
(byte)MainV2.comPort.sysidcurrent,
(byte)MainV2.comPort.compidcurrent, (byte)(bus - 1), (byte)DroneCAN.DroneCAN.dataLengthToDlc(payload.packet_data.Length),
payload.packet_data);
MainV2.comPort.sendPacket(frame2,
(byte)MainV2.comPort.sysidcurrent,
(byte)MainV2.comPort.compidcurrent);
}
};
port.ReadBufferUpdate += (o, i) => { };
port.WriteCallback += (o, bytes) =>
{
var lines = ASCIIEncoding.ASCII.GetString(bytes.ToArray())
.Split(new[] { '\r' }, StringSplitOptions.RemoveEmptyEntries);
foreach (var line in lines)
{
can.ReadMessageSLCAN(line);
}
};
// mavlink to slcan
MainV2.comPort.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.CAN_FRAME, (m) =>
{
if (m.msgid == (uint)MAVLink.MAVLINK_MSG_ID.CAN_FRAME)
{
var canfd = false;
var pkt = (MAVLink.mavlink_can_frame_t)m.data;
var cf = new CANFrame(BitConverter.GetBytes(pkt.id));
var length = pkt.len;
var payload = new CANPayload(pkt.data);
var ans2 = String.Format("{0}{1}{2}{3}\r", canfd ? 'B' : 'T', cf.ToHex(), length.ToString("X")
, payload.ToHex(DroneCAN.DroneCAN.dlcToDataLength(length)));
port.AppendBuffer(ASCIIEncoding.ASCII.GetBytes(ans2));
}
else if (m.msgid == (uint)MAVLink.MAVLINK_MSG_ID.CANFD_FRAME)
{
var canfd = true;
var pkt = (MAVLink.mavlink_canfd_frame_t)m.data;
var cf = new CANFrame(BitConverter.GetBytes(pkt.id));
var length = pkt.len;
var payload = new CANPayload(pkt.data);
var ans2 = String.Format("{0}{1}{2}{3}\r", canfd ? 'B' : 'T', cf.ToHex(), length.ToString("X")
, payload.ToHex(DroneCAN.DroneCAN.dlcToDataLength(length)));
port.AppendBuffer(ASCIIEncoding.ASCII.GetBytes(ans2));
}
return(true);
}, (byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent, true);
SetupSLCanPort(port);
}
private void but_slcanmavlink_Click(object sender, EventArgs e)
{
byte bus = 1;
Task.Run(() =>
{
mavlinkCANRun = true;
// send every second, timeout is in 5 seconds
while (mavlinkCANRun)
{
// setup forwarding on can port 1
var ans = MainV2.comPort.doCommand((byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent, MAVLink.MAV_CMD.CAN_FORWARD, bus, 0, 0, 0, 0, 0, 0);
if (ans == false) // MAVLink.MAV_RESULT.UNSUPPORTED)
{
return;
}
Thread.Sleep(1000);
}
});
var port = new CommsInjection();
var can = new DroneCAN.DroneCAN();
can.FrameReceived += (frame, payload) =>
{
if (payload.packet_data.Length > 8)
{
MainV2.comPort.sendPacket(new MAVLink.mavlink_canfd_frame_t(BitConverter.ToUInt32(frame.packet_data, 0),
(byte)MainV2.comPort.sysidcurrent,
(byte)MainV2.comPort.compidcurrent, (byte)(bus), (byte)payload.packet_data.Length,
payload.packet_data),
(byte)MainV2.comPort.sysidcurrent,
(byte)MainV2.comPort.compidcurrent);
}
else
{
MainV2.comPort.sendPacket(new MAVLink.mavlink_can_frame_t(BitConverter.ToUInt32(frame.packet_data, 0),
(byte)MainV2.comPort.sysidcurrent,
(byte)MainV2.comPort.compidcurrent, (byte)(bus), (byte)payload.packet_data.Length,
payload.packet_data),
(byte)MainV2.comPort.sysidcurrent,
(byte)MainV2.comPort.compidcurrent);
}
};
port.ReadBufferUpdate += (o, i) => { };
port.WriteCallback += (o, bytes) =>
{
var lines = ASCIIEncoding.ASCII.GetString(bytes.ToArray())
.Split(new[] { '\r' }, StringSplitOptions.RemoveEmptyEntries);
foreach (var line in lines)
{
can.ReadMessageSLCAN(line);
}
};
// mavlink to slcan
MainV2.comPort.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.CAN_FRAME, (m) =>
{
if (m.msgid == (uint)MAVLink.MAVLINK_MSG_ID.CAN_FRAME)
{
var canfd = false;
var pkt = (MAVLink.mavlink_can_frame_t)m.data;
var cf = new CANFrame(BitConverter.GetBytes(pkt.id));
var length = pkt.len;
var payload = new CANPayload(pkt.data);
var ans2 = String.Format("{0}{1}{2}{3}\r", canfd ? 'B' : 'T', cf.ToHex(), length.ToString("X")
, payload.ToHex(DroneCAN.DroneCAN.dlcToDataLength(length)));
port.AppendBuffer(ASCIIEncoding.ASCII.GetBytes(ans2));
}
else if (m.msgid == (uint)MAVLink.MAVLINK_MSG_ID.CANFD_FRAME)
{
var canfd = true;
var pkt = (MAVLink.mavlink_canfd_frame_t)m.data;
var cf = new CANFrame(BitConverter.GetBytes(pkt.id));
var length = pkt.len;
var payload = new CANPayload(pkt.data);
var ans2 = String.Format("{0}{1}{2}{3}\r", canfd ? 'B' : 'T', cf.ToHex(), length.ToString("X")
, payload.ToHex(DroneCAN.DroneCAN.dlcToDataLength(length)));
port.AppendBuffer(ASCIIEncoding.ASCII.GetBytes(ans2));
}
return(true);
}, true);
SetupSLCanPort(port);
}
public static void SendBlock(object stateInfo)
{
if (stateInfo == null)
{
CNXLog.ErrorFormat("SendBlock - stateInfo NULL");
return;
}
BlockInfo blockInfo = (BlockInfo)stateInfo;
//CNXLog.InfoFormat("Running transfere for block {0}, CRC {1} {2}", blockInfo.mBlockId, blockInfo.mCrc, blockInfo.ToString());
try
{
// build a chunk 1 message
CANFrame chunk1 = BlockTransferManager.BuildChunk1(blockInfo.mBlockId, blockInfo.mCrc, (uint)blockInfo.mBlockData.Length, blockInfo.mVersion, 0);
// send chunk 1
//CNXLog.InfoFormat("Sending Block {0} Chunck 1 {1}.", blockInfo.mBlockId, chunk1.ToString());
blockInfo.mClient.Send(chunk1);
// transfer the block
CANFrame frame = new CANFrame();
frame.MailboxId = (uint)CNXMsgIds.BlockChunkN + (uint)blockInfo.mBlockId;
byte[] data = new byte[8];
for (uint offset = 0; offset < blockInfo.mBlockData.Length; offset += 4)
{
// set the offset
BitConverter.GetBytes(offset).CopyTo(data, 0);
if (!BitConverter.IsLittleEndian)
{
Array.Reverse(data, 0, 4);
}
uint remainder = (uint)(blockInfo.mBlockData.LongLength - offset);
int count = (int)((remainder > 4) ? 4 : remainder);
Array.Copy(blockInfo.mBlockData, offset, data, 4, count);
frame.DataFromArray(data, 0, 4 + count);
int sent = blockInfo.mClient.Send(frame);
//CNXLog.InfoFormat("Sending Block {0} Chunck ({1}) {2}.", blockInfo.mBlockId, offset, frame.ToString());
Thread.Yield();
Thread.Sleep(10);
if (offset % 16 == 0)
{
//Console.WriteLine("Block pause.");
Thread.Yield();
Thread.Sleep(10);
}
}
// allow time for the block to be consumed
Thread.Sleep(41);
if (blockInfo.mRepeat != 0)
{
frame.MailboxId = (uint)CNXMsgIds.BlockQuery + (uint)blockInfo.mBlockId;
frame.ClearData();
blockInfo.mClient.Send(frame);
}
}
catch (ThreadAbortException)
{
// normal behaviour if the transfere is canceled
CNXLog.WarnFormat("Block {0} transfere cancled.", (Block)blockInfo.mBlockId);
}
catch (Exception e)
{
CNXLog.ErrorFormat("SendBlock {0} {1}", (Block)blockInfo.mBlockId, e.ToString());
}
}
protected void OnDatagramReceivedEventHandler(object sender, DatagramReceivedEventArgs a)
{
CANFrame frame = new CANFrame();
if (a.Datagram.Length < 4)
{
ServerMsgLog.WarnFormat("UDP out-of-range, length {0}", a.Datagram.Length);
return;
}
uint msgId = 0;
try
{
// work out what it all meens
// first assume that the datgram is a CNX CAN message
if (BitConverter.IsLittleEndian)
{
Array.Reverse(a.Datagram, 0, 4);
}
msgId = BitConverter.ToUInt32(a.Datagram, 0);
switch (msgId)
{
case (uint)CNXMsgIds.TripProgress:
frame.MailboxId = msgId;
frame.DataFromArray(a.Datagram, 4, a.Datagram.Length - 4);
TripProgressType msgType;
int pathId;
int position;
int?tripNo;
CNXCANMsgHelper.UnpackTripProgress(frame, out msgType, out pathId, out position, out tripNo);
object[] args = { msgType, pathId, position, tripNo };
ServerMsgLog.WarnFormat("UDP Trip Progress {0}, RouteTag {1}, Progress {2}, tripNo {3}.", args);
mCANClient.Send(frame);
break;
case (uint)CNXMsgIds.DeviceCatalogue:
ServerMsgLog.WarnFormat("UDP Re-Cataloge message (0x101).");
// clear our catalogue
mCANServer.ClearDeviceCatalogue();
// put our self into the new catalogue
AddSelfToCatalogue();
// reset catalogues on all equipment.
frame.ClearData();
frame.MailboxId = (uint)CANLib.CNXMsgIds.DeviceCatalogue;
mCANClient.Send(frame);
mCANClient.Send(CNXCANMsgHelper.PackIdentifiers(mCommsServer.CommsAddress, mCommsServer.CompanyTag));
break;
case (uint)CNXMsgIds.TripNone:
case (uint)CNXMsgIds.TripOffRoute:
frame.MailboxId = msgId;
mCANClient.Send(frame);
ServerMsgLog.WarnFormat("UDP {0}.", (CNXMsgIds)msgId);
break;
case (uint)CNXMsgIds.TripOnRoute:
frame.MailboxId = msgId;
frame.DataFromArray(a.Datagram, 4, a.Datagram.Length - 4);
ServerMsgLog.WarnFormat("UDP {0}.", frame.ToString());
mCANClient.Send(frame);
break;
case (uint)RTTMesg.RTTInMsgIds.FirmwareBuildNos:
ServerMsgLog.WarnFormat("UDP firmware version (0x1001) length {0}\n{1}.", a.Datagram.Length, CommsServer.HexDump(a.Datagram, a.Datagram.Length));
if (a.Datagram.Length > 5)
{
StartFirmwareManagement(a.Datagram, 4);
}
break;
case (uint)RTTMesg.RTTInMsgIds.ResourceBuildNos:
ServerMsgLog.WarnFormat("UDP configuration message (0x1002) length {0}\n{1}.", a.Datagram.Length, CommsServer.HexDump(a.Datagram, a.Datagram.Length));
if (a.Datagram.Length > 5)
{
byte[] versions = new byte[a.Datagram.Length - 4];
Array.Copy(a.Datagram, 4, versions, 0, versions.Length);
StartDeviceConfigurationManagement(versions);
}
break;
case (uint)RTTMesg.RTTInMsgIds.CompanyTag:
mCommsServer.CompanyTag = a.Datagram[4];
ServerMsgLog.WarnFormat("UDP CompanyTag {0}", mCommsServer.CompanyTag);
mCANClient.Send(CNXCANMsgHelper.PackIdentifiers(mCommsServer.CommsAddress, mCommsServer.CompanyTag));
break;
case (uint)RTTMesg.RTTInMsgIds.DriverLogonOK:
ServerMsgLog.WarnFormat("UDP {0}.", (RTTMesg.RTTInMsgIds)msgId);
frame.MailboxId = (uint)CNXMsgIds.LogonResult;
frame.Data = new byte[1];
frame.Data[0] = (byte)LogonState.LogonOK;
mCANClient.Send(frame);
break;
case (uint)RTTMesg.RTTInMsgIds.DriverLogonFail:
ServerMsgLog.WarnFormat("UDP {0}.", (RTTMesg.RTTInMsgIds)msgId);
frame.MailboxId = (uint)CNXMsgIds.LogonResult;
frame.Data = new byte[1];
frame.Data[0] = (byte)LogonState.LogonFailed;
mCANClient.Send(frame);
break;
case (uint)RTTMesg.RTTInMsgIds.DriverLogoff:
ServerMsgLog.WarnFormat("UDP {0}.", (RTTMesg.RTTInMsgIds)msgId);
BeginSendTransientBlock(Block.DriverLogon, new byte[] { 0 });
break;
case (uint)RTTMesg.RTTInMsgIds.DriverMesg2:
ServerMsgLog.WarnFormat("UDP {0}\n{1}.", (RTTMesg.RTTInMsgIds)msgId, CommsServer.HexDump(a.Datagram, a.Datagram.Length));
//TransientBlock block = new TransientBlock(mCANClient, (byte)Block.MessageToDriver);
byte[] blockData = new byte[a.Datagram.Length - 4];
Array.Copy(a.Datagram, 4, blockData, 0, blockData.Length);
//block.Send(blockData, false);
BeginSendTransientBlock(Block.MessageToDriver, blockData);
break;
case (uint)RTTMesg.RTTInMsgIds.VehicleInfo:
mCommsServer.CompanyTag = a.Datagram[4];
mCommsServer.VehicleId = Encoding.ASCII.GetString(a.Datagram, 5, a.Datagram.Length - 5);
ServerMsgLog.WarnFormat("UDP VehicleInfo {0} {1}", mCommsServer.CompanyTag, mCommsServer.VehicleId);
mCANClient.Send(CNXCANMsgHelper.PackIdentifiers(mCommsServer.CommsAddress, mCommsServer.CompanyTag));
break;
case (uint)RTTMesg.RTTInMsgIds.FlushData:
ServerMsgLog.WarnFormat("UDP Flush APC");
frame.MailboxId = (uint)CNXMsgIds.ResetLoading;
frame.DataFromArray(a.Datagram, 4, a.Datagram.Length - 4);
mApc.FrameReceivedEventHandler(null, new FrameReceivedEventArgs(frame));
mCANClient.Send(frame);
// force daily restart as requested by Iris
//if (mTracker.mIris != null)
// mTracker.mIris.Restart();
break;
default:
ServerMsgLog.WarnFormat("UDP Unknown id {0} length {1}\n{2}.", msgId, a.Datagram.Length, CommsServer.HexDump(a.Datagram, a.Datagram.Length));
break;
}
}
catch (Exception e)
{
ServerMsgLog.ErrorFormat("Datagram {0} Handler Error : {1}", msgId, e.ToString());
}
}
