private static void RequestCallback(IAsyncResult ar)
{
TcpClient client = (TcpClient)ar.AsyncState;
lock (locker)
{
byte localsysid = newsysid++;
if (client.Connected)
{
MAVLinkInterface mav = new MAVLinkInterface();
mav.BaseStream = new TcpSerial()
{
client = client
};
try
{
mav.GetParam("SYSID_THISMAV");
}
catch
{
}
try
{
mav.GetParam("SYSID_THISMAV");
}
catch
{
}
try
{
mav.GetParam("SYSID_THISMAV");
}
catch
{
}
try
{
var ans = mav.setParam("SYSID_THISMAV", localsysid);
Console.WriteLine("this mav set " + ans);
}
catch
{
}
mav = null;
clients.Add(client);
}
}
}
private static void RequestCallback(IAsyncResult ar)
{
TcpClient client = (TcpClient)ar.AsyncState;
byte localsysid = 0;
lock (locker)
{
localsysid = newsysid++;
}
if (client.Connected)
{
MAVLinkInterface mav = new MAVLinkInterface();
mav.BaseStream = new TcpSerial()
{
client = client
};
try
{
mav.GetParam((byte)mav.sysidcurrent, (byte)mav.compidcurrent, "SYSID_THISMAV");
}
catch
{
}
try
{
mav.GetParam((byte)mav.sysidcurrent, (byte)mav.compidcurrent, "SYSID_THISMAV");
}
catch
{
}
try
{
mav.GetParam((byte)mav.sysidcurrent, (byte)mav.compidcurrent, "SYSID_THISMAV");
}
catch
{
}
try
{
var ans = mav.setParam("SYSID_THISMAV", localsysid);
Console.WriteLine("this mav set " + ans);
}
catch
{
}
Connect?.Invoke(mav, localsysid.ToString());
}
}
protected override bool ProcessCmdKey(ref Message msg, Keys keyData)
{
if (keyData == (Keys.Control | Keys.S))
{
var exepath = CheckandGetSITLImage("ArduCopter.elf");
var model = "+";
var config = GetDefaultConfig(model);
var max = 10;
for (int a = max; a >= 0; a--)
{
var extra = "";
if (!string.IsNullOrEmpty(config))
{
extra += @" --defaults """ + config + @""" -P SERIAL0_PROTOCOL=2 -P SERIAL1_PROTOCOL=2 ";
}
var home = new PointLatLngAlt(markeroverlay.Markers[0].Position).newpos((double)NUM_heading.Value, a * 4);
if (max == a)
{
extra += String.Format(
" -M{4} -s1 --home {3} --instance {0} --uartA tcp:0 {1} -P SYSID_THISMAV={2} ",
a, "", a + 1, BuildHomeLocation(home, (int)NUM_heading.Value), model);
}
else
{
extra += String.Format(
" -M{4} -s1 --home {3} --instance {0} --uartA tcp:0 {1} -P SYSID_THISMAV={2} ",
a, "--uartD tcpclient:127.0.0.1:" + (5770 + 10 * a), a + 1, BuildHomeLocation(home, (int)NUM_heading.Value), model);
}
string simdir = sitldirectory + model + (a + 1) + Path.DirectorySeparatorChar;
Directory.CreateDirectory(simdir);
string path = Environment.GetEnvironmentVariable("PATH");
Environment.SetEnvironmentVariable("PATH", sitldirectory + ";" + simdir + ";" + path, EnvironmentVariableTarget.Process);
Environment.SetEnvironmentVariable("HOME", simdir, EnvironmentVariableTarget.Process);
ProcessStartInfo exestart = new ProcessStartInfo();
exestart.FileName = exepath;
exestart.Arguments = extra;
exestart.WorkingDirectory = simdir;
exestart.WindowStyle = ProcessWindowStyle.Minimized;
exestart.UseShellExecute = true;
Process.Start(exestart);
}
try
{
var client = new Comms.TcpSerial();
client.client = new TcpClient("127.0.0.1", 5760);
MainV2.comPort.BaseStream = client;
SITLSEND = new UdpClient("127.0.0.1", 5501);
Thread.Sleep(200);
MainV2.instance.doConnect(MainV2.comPort, "preset", "5760");
return(true);
}
catch
{
CustomMessageBox.Show(Strings.Failed_to_connect_to_SITL_instance, Strings.ERROR);
return(true);
}
}
if (keyData == (Keys.Control | Keys.D))
{
var exepath = CheckandGetSITLImage("ArduCopter.elf");
var model = "+";
var config = GetDefaultConfig(model);
var max = 10;
for (int a = max; a >= 0; a--)
{
var extra = "";
if (!string.IsNullOrEmpty(config))
{
extra += @" --defaults """ + config + @""" -P SERIAL0_PROTOCOL=2 -P SERIAL1_PROTOCOL=2 ";
}
var home = new PointLatLngAlt(markeroverlay.Markers[0].Position).newpos((double)NUM_heading.Value, a * 4);
if (max == a)
{
extra += String.Format(
" -M{4} -s1 --home {3} --instance {0} --uartA tcp:0 {1} -P SYSID_THISMAV={2} ",
a, "", a + 1, BuildHomeLocation(home, (int)NUM_heading.Value), model);
}
else
{
extra += String.Format(
" -M{4} -s1 --home {3} --instance {0} --uartA tcp:0 {1} -P SYSID_THISMAV={2} ",
a, "" /*"--uartD tcpclient:127.0.0.1:" + (5770 + 10 * a)*/, a + 1, BuildHomeLocation(home, (int)NUM_heading.Value), model);
}
string simdir = sitldirectory + model + (a + 1) + Path.DirectorySeparatorChar;
Directory.CreateDirectory(simdir);
string path = Environment.GetEnvironmentVariable("PATH");
Environment.SetEnvironmentVariable("PATH", sitldirectory + ";" + simdir + ";" + path, EnvironmentVariableTarget.Process);
Environment.SetEnvironmentVariable("HOME", simdir, EnvironmentVariableTarget.Process);
ProcessStartInfo exestart = new ProcessStartInfo();
exestart.FileName = exepath;
exestart.Arguments = extra;
exestart.WorkingDirectory = simdir;
exestart.WindowStyle = ProcessWindowStyle.Minimized;
exestart.UseShellExecute = true;
Process.Start(exestart);
}
try
{
for (int a = max; a >= 0; a--)
{
var mav = new MAVLinkInterface();
var client = new Comms.TcpSerial();
client.client = new TcpClient("127.0.0.1", 5760 + (10 * (a)));
mav.BaseStream = client;
//SITLSEND = new UdpClient("127.0.0.1", 5501);
Thread.Sleep(200);
MainV2.instance.doConnect(mav, "preset", "5760");
try
{
mav.GetParam("SYSID_THISMAV");
mav.setParam("SYSID_THISMAV", a + 1, true);
mav.GetParam("FRAME_CLASS");
mav.setParam("FRAME_CLASS", 1, true);
} catch { }
MainV2.Comports.Add(mav);
}
return(true);
}
catch
{
CustomMessageBox.Show(Strings.Failed_to_connect_to_SITL_instance, Strings.ERROR);
return(true);
}
}
return(base.ProcessCmdKey(ref msg, keyData));
}
static void Main(string[] args)
{
Console.CancelKeyPress += Console_CancelKeyPress;
if (args.Length < 2)
{
Console.WriteLine("usage: UAVCANFlasher com1 flow-crc.bin");
Console.WriteLine("this program will flash the firmware to every connected uavcan node. DONT connect devices that its not intended for");
Console.ReadKey();
return;
}
var portname = args[0];
var firmware_name = args[1];
var port = new MissionPlanner.Comms.SerialPort(args[0], 115200);
port.Open();
if (!File.Exists(firmware_name))
{
Console.WriteLine("firmware file not found");
Console.ReadKey();
return;
}
MAVLinkInterface mav = new MAVLinkInterface();
mav.BaseStream = port;
mav.getHeartBeat();
try
{
mav.GetParam((byte)mav.sysidcurrent, (byte)mav.compidcurrent, "CAN_SLCAN_TIMOUT");
mav.GetParam((byte)mav.sysidcurrent, (byte)mav.compidcurrent, "CAN_SLCAN_SERNUM");
mav.setParam("CAN_SLCAN_TIMOUT", 2, true);
mav.setParam("CAN_SLCAN_SERNUM", 0, true); // usb
}
catch { }
DroneCAN.DroneCAN can = new DroneCAN.DroneCAN();
can.SourceNode = 127;
//can.Update();
// updater
var firmware_namebytes = ASCIIEncoding.ASCII.GetBytes(Path.GetFileName(firmware_name.ToLower()));
ulong firmware_crc = ulong.MaxValue;
Exception exception = null;
Dictionary <int, DateTime> lastseenDateTimes = new Dictionary <int, DateTime>();
DroneCAN.DroneCAN.MessageRecievedDel updatedelegate = (frame, msg, transferID) =>
{
if (frame.IsServiceMsg && frame.SvcDestinationNode != can.SourceNode)
{
return;
}
if (frame.SourceNode == 0)
{
return;
}
lastseenDateTimes[frame.SourceNode] = DateTime.Now;
if (msg.GetType() == typeof(DroneCAN.DroneCAN.uavcan_protocol_file_BeginFirmwareUpdate_res))
{
var bfures = msg as DroneCAN.DroneCAN.uavcan_protocol_file_BeginFirmwareUpdate_res;
if (bfures.error != 0)
{
exception = new Exception(frame.SourceNode + " Begin Firmware Update returned an error");
}
}
else if (msg.GetType() == typeof(DroneCAN.DroneCAN.uavcan_protocol_GetNodeInfo_res))
{
var gnires = msg as DroneCAN.DroneCAN.uavcan_protocol_GetNodeInfo_res;
Console.WriteLine("GetNodeInfo: seen '{0}' from {1}",
ASCIIEncoding.ASCII.GetString(gnires.name).TrimEnd('\0'), frame.SourceNode);
if (firmware_crc != gnires.software_version.image_crc || firmware_crc == ulong.MaxValue)
{
if (gnires.status.mode != DroneCAN.DroneCAN.uavcan_protocol_NodeStatus.UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE)
{
Console.WriteLine("Update node " + frame.SourceNode);
var req_msg =
new DroneCAN.DroneCAN.uavcan_protocol_file_BeginFirmwareUpdate_req()
{
image_file_remote_path = new DroneCAN.DroneCAN.uavcan_protocol_file_Path()
{
path = firmware_namebytes
},
source_node_id = can.SourceNode
};
req_msg.image_file_remote_path.path_len = (byte)firmware_namebytes.Length;
var slcan = can.PackageMessageSLCAN(frame.SourceNode, frame.Priority, transferID, req_msg);
can.WriteToStreamSLCAN(slcan);
}
else
{
//exception = new Exception("already in update mode");
return;
}
}
else
{
Console.WriteLine(frame.SourceNode + " CRC matchs");
}
}
};
can.MessageReceived += updatedelegate;
// getfile crc
using (var stream = File.OpenRead(firmware_name))
{
string app_descriptor_fmt = "<8cQI";
var SHARED_APP_DESCRIPTOR_SIGNATURES = new byte[][] {
new byte[] { 0xd7, 0xe4, 0xf7, 0xba, 0xd0, 0x0f, 0x9b, 0xee },
new byte[] { 0x40, 0xa2, 0xe4, 0xf1, 0x64, 0x68, 0x91, 0x06 }
};
var app_descriptor_len = 8 * 1 + 8 + 4;
var location = GetPatternPositions(stream, SHARED_APP_DESCRIPTOR_SIGNATURES[0]);
stream.Seek(0, SeekOrigin.Begin);
var location2 = GetPatternPositions(stream, SHARED_APP_DESCRIPTOR_SIGNATURES[1]);
if (location.Count > 0 || location2.Count > 0)
{
var offset = location.Count > 0 ? location[0] : location2[0];
stream.Seek(offset, SeekOrigin.Begin);
byte[] buf = new byte[app_descriptor_len];
stream.Read(buf, 0, app_descriptor_len);
firmware_crc = BitConverter.ToUInt64(buf, 8);
}
}
can.NodeAdded += (id, status) =>
{
Console.WriteLine(id + " Node status seen");
// get node info
DroneCAN.DroneCAN.uavcan_protocol_GetNodeInfo_req gnireq = new DroneCAN.DroneCAN.uavcan_protocol_GetNodeInfo_req()
{
};
var slcan = can.PackageMessageSLCAN((byte)id, 30, 0, gnireq);
can.WriteToStreamSLCAN(slcan);
};
DroneCAN.DroneCAN.uavcan_protocol_NodeStatus node;
can.FileSendComplete += (id, file) =>
{
Console.WriteLine(id + " File send complete " + file);
can.NodeList.TryRemove(id, out node);
lastseenDateTimes.Remove(id);
};
can.StartSLCAN(port.BaseStream);
can.ServeFile(args[1]);
can.SetupFileServer();
can.SetupDynamicNodeAllocator();
Console.WriteLine("Ready.. " + portname + " -> " + firmware_name);
run = true;
while (run)
{
Thread.Sleep(100);
foreach (var lastseenDateTime in lastseenDateTimes.ToArray())
{
if (lastseenDateTime.Value.AddSeconds(3) < DateTime.Now)
{
can.NodeList.TryRemove(lastseenDateTime.Key, out node);
lastseenDateTimes.Remove(lastseenDateTime.Key);
Console.WriteLine(lastseenDateTime.Key + " Remove old node - 3 seconds of no data");
}
}
if (exception != null)
{
Console.WriteLine(exception.ToString());
exception = null;
}
}
}
public async Task StartSwarmSeperate()
{
var max = 10.0;
if (InputBox.Show("how many?", "how many?", ref max) != DialogResult.OK)
{
return;
}
// kill old session
try
{
simulator.ForEach(a =>
{
try
{
a.Kill();
}
catch { }
});
}
catch
{
}
var exepath = CheckandGetSITLImage("ArduCopter.elf");
var model = "+";
var config = await GetDefaultConfig(model);
max--;
for (int a = (int)max; a >= 0; a--)
{
var extra = " --disable-fgview -r50 ";
if (!string.IsNullOrEmpty(config))
{
extra += @" --defaults """ + config + @",identity.parm"" -P SERIAL0_PROTOCOL=2 -P SERIAL1_PROTOCOL=2 ";
}
var home = new PointLatLngAlt(markeroverlay.Markers[0].Position).newpos((double)NUM_heading.Value, a * 4);
if (max == a)
{
extra += String.Format(
" -M{4} -s1 --home {3} --instance {0} --uartA tcp:0 {1} -P SYSID_THISMAV={2} ",
a, "", a + 1, BuildHomeLocation(home, (int)NUM_heading.Value), model);
}
else
{
extra += String.Format(
" -M{4} -s1 --home {3} --instance {0} --uartA tcp:0 {1} -P SYSID_THISMAV={2} ",
a, "" /*"--uartD tcpclient:127.0.0.1:" + (5770 + 10 * a)*/, a + 1,
BuildHomeLocation(home, (int)NUM_heading.Value), model);
}
string simdir = sitldirectory + model + (a + 1) + Path.DirectorySeparatorChar;
Directory.CreateDirectory(simdir);
File.WriteAllText(simdir + "identity.parm", String.Format(@"SERIAL0_PROTOCOL=2
SERIAL1_PROTOCOL=2
SYSID_THISMAV={0}
SIM_TERRAIN=0
TERRAIN_ENABLE=0
SCHED_LOOP_RATE=50
SIM_DRIFT_SPEED=0
SIM_DRIFT_TIME=0
", a + 1));
string path = Environment.GetEnvironmentVariable("PATH");
Environment.SetEnvironmentVariable("PATH", sitldirectory + ";" + simdir + ";" + path,
EnvironmentVariableTarget.Process);
Environment.SetEnvironmentVariable("HOME", simdir, EnvironmentVariableTarget.Process);
ProcessStartInfo exestart = new ProcessStartInfo();
exestart.FileName = await exepath;
exestart.Arguments = extra;
exestart.WorkingDirectory = simdir;
exestart.WindowStyle = ProcessWindowStyle.Minimized;
exestart.UseShellExecute = true;
simulator.Add(System.Diagnostics.Process.Start(exestart));
}
System.Threading.Thread.Sleep(2000);
MainV2.View.ShowScreen(MainV2.View.screens[0].Name);
try
{
for (int a = (int)max; a >= 0; a--)
{
var mav = new MAVLinkInterface();
var client = new Comms.TcpSerial();
client.client = new TcpClient("127.0.0.1", 5760 + (10 * (a)));
mav.BaseStream = client;
//SITLSEND = new UdpClient("127.0.0.1", 5501);
Thread.Sleep(200);
MainV2.instance.doConnect(mav, "preset", "5760");
try
{
mav.GetParam((byte)mav.sysidcurrent, (byte)mav.compidcurrent, "SYSID_THISMAV");
mav.setParam("SYSID_THISMAV", a + 1, true);
mav.GetParam((byte)mav.sysidcurrent, (byte)mav.compidcurrent, "FRAME_CLASS");
mav.setParam("FRAME_CLASS", 1, true);
}
catch
{
}
MainV2.Comports.Add(mav);
}
return;
}
catch
{
CustomMessageBox.Show(Strings.Failed_to_connect_to_SITL_instance, Strings.ERROR);
return;
}
}
private static void RequestCallback(IAsyncResult ar)
{
TcpClient client = (TcpClient)ar.AsyncState;
byte localsysid = newsysid++;
if (client.Connected)
{
MAVLinkInterface mav = new MAVLinkInterface();
mav.BaseStream = new TcpSerial() { client = client };
try
{
mav.GetParam("SYSID_THISMAV");
}
catch { }
var ans = mav.setParam("SYSID_THISMAV", localsysid);
Console.WriteLine("this mav set " + ans);
mav = null;
clients.Add(client);
}
}
private static void RequestCallback(IAsyncResult ar)
{
TcpClient client = (TcpClient)ar.AsyncState;
lock (locker)
{
byte localsysid = newsysid++;
if (client.Connected)
{
MAVLinkInterface mav = new MAVLinkInterface();
mav.BaseStream = new TcpSerial()
{
client = client
};
try
{
mav.GetParam("SYSID_THISMAV");
}
catch
{
}
try
{
mav.GetParam("SYSID_THISMAV");
}
catch
{
}
try
{
mav.GetParam("SYSID_THISMAV");
}
catch
{
}
try
{
var ans = mav.setParam("SYSID_THISMAV", localsysid);
Console.WriteLine("this mav set " + ans);
}
catch
{
}
//mav = null;
MainV2.instance.Invoke((Action) delegate
{
MainV2.instance.doConnect(mav, "preset",
localsysid.ToString());
MainV2.Comports.Add(mav);
});
//clients.Add(client);
}
}
}
