public bool SaveConfig(byte node)
{
//TX 15:23:27.460269 1E0AF9FE 00 00 00 00 00 00 00 C1........ 126 121 uavcan.protocol.param.ExecuteOpcode
//RX 15:23:27.471806 1E0A7EF9 00 00 00 00 00 00 80 C1 ........ 121 126 uavcan.protocol.param.ExecuteOpcode
bool?ok = null;
MessageRecievedDel configdelgate = (frame, msg, transferID) =>
{
if (frame.IsServiceMsg && frame.SvcDestinationNode != SourceNode)
{
return;
}
if (msg.GetType() == typeof(uavcan.uavcan_protocol_param_ExecuteOpcode_res))
{
var exopres = msg as uavcan.uavcan_protocol_param_ExecuteOpcode_res;
if (frame.SourceNode != node)
{
return;
}
ok = exopres.ok;
}
};
MessageReceived += configdelgate;
var req = new uavcan.uavcan_protocol_param_ExecuteOpcode_req()
{
opcode = (byte)uavcan.UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_REQ_OPCODE_SAVE
};
var trys = 0;
DateTime nextsend = DateTime.MinValue;
while (!ok.HasValue)
{
if (trys > 3)
{
return(false);
}
if (nextsend < DateTime.Now)
{
var slcan = PackageMessage(node, 30, transferID++, req);
WriteToStream(slcan);
nextsend = DateTime.Now.AddSeconds(1);
trys++;
}
Thread.Sleep(20);
}
MessageReceived -= configdelgate;
return(ok.Value);
}
public async void Update(string devicename, double hwversion, string firmware_name)
{
ServeFile(firmware_name);
var firmware_namebytes = ASCIIEncoding.ASCII.GetBytes(Path.GetFileName(firmware_name.ToLower()));
ulong firmware_crc = ulong.MaxValue;
MessageRecievedDel updatedelegate = (frame, msg, transferID) =>
{
if (frame.IsServiceMsg && frame.SvcDestinationNode != SourceNode)
{
return;
}
if (msg.GetType() == typeof(uavcan.uavcan_protocol_file_BeginFirmwareUpdate_res))
{
var bfures = msg as uavcan.uavcan_protocol_file_BeginFirmwareUpdate_res;
if (bfures.error != 0)
{
throw new Exception("Begin Firmware Update returned an error");
}
}
else if (msg.GetType() == typeof(uavcan.uavcan_protocol_GetNodeInfo_res))
{
var gnires = msg as uavcan.uavcan_protocol_GetNodeInfo_res;
Console.WriteLine("GetNodeInfo: seen '{0}' from {1}", ASCIIEncoding.ASCII.GetString(gnires.name).TrimEnd('\0'), frame.SourceNode);
if (devicename == ASCIIEncoding.ASCII.GetString(gnires.name).TrimEnd('\0') || devicename == ASCIIEncoding.ASCII.GetString(gnires.name).TrimEnd('\0') + "-BL")
{
if (firmware_crc != gnires.software_version.image_crc)
{
if (hwversion == double.Parse(gnires.hardware_version.major + "." + gnires.hardware_version.minor, CultureInfo.InvariantCulture) || hwversion == 0)
{
if (gnires.status.mode != uavcan.UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE)
{
var req_msg =
new uavcan.uavcan_protocol_file_BeginFirmwareUpdate_req()
{
image_file_remote_path = new uavcan.uavcan_protocol_file_Path()
{
path = firmware_namebytes
},
source_node_id = SourceNode
};
req_msg.image_file_remote_path.path_len = (byte)firmware_namebytes.Length;
var slcan = PackageMessage(frame.SourceNode, frame.Priority, transferID, req_msg);
lock (sr_lock)
WriteToStream(slcan);
}
else
{
Console.WriteLine("already in update mode");
}
}
else
{
Console.WriteLine("hwversion does not match");
}
}
else
{
throw new Exception(String.Format("{0} - No need to upload, crc matchs", frame.SourceNode));
}
}
else
{
Console.WriteLine("device name does not match");
}
}
};
MessageReceived += updatedelegate;
// getfile crc
using (var stream = File.OpenRead(fileServerList[Path.GetFileName(firmware_name.ToLower())]))
{
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);
}
}
NodeAdded += (NodeID, msg) =>
{
var statetracking = new statetracking();
// get node info
uavcan.uavcan_protocol_GetNodeInfo_req gnireq = new uavcan.uavcan_protocol_GetNodeInfo_req()
{
};
gnireq.encode(uavcan_transmit_chunk_handler, statetracking);
var slcan = PackageMessage(NodeID, 30, transferID++, gnireq);
lock (sr_lock)
WriteToStream(slcan);
};
foreach (var i in nodeList.Keys.ToArray())
{
var statetracking = new statetracking();
// get node info
uavcan.uavcan_protocol_GetNodeInfo_req gnireq = new uavcan.uavcan_protocol_GetNodeInfo_req()
{
};
gnireq.encode(uavcan_transmit_chunk_handler, statetracking);
var slcan = PackageMessage((byte)i, 30, transferID++, gnireq);
lock (sr_lock)
WriteToStream(slcan);
}
int b = 0;
while (true)
{
await Task.Delay(1000);
if (nodeList.Values.Any(a => a.mode == uavcan.UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE))
{
}
else
{
b++;
if (b > 100)
{
break;
}
}
}
MessageReceived -= updatedelegate;
}
public List <uavcan.uavcan_protocol_param_GetSet_res> GetParameters(byte node)
{
List <uavcan.uavcan_protocol_param_GetSet_res> paramlist = new List <uavcan.uavcan_protocol_param_GetSet_res>();
ushort index = 0;
var timeout = DateTime.Now.AddSeconds(2);
MessageRecievedDel paramdelegate = (frame, msg, transferID) =>
{
if (frame.IsServiceMsg && frame.SvcDestinationNode != SourceNode)
{
return;
}
if (msg.GetType() == typeof(uavcan.uavcan_protocol_param_GetSet_res))
{
var getsetreq = msg as uavcan.uavcan_protocol_param_GetSet_res;
if (getsetreq.name_len == 0)
{
timeout = DateTime.MinValue;
return;
}
paramlist.Add(getsetreq);
var value = getsetreq.value;
var name = ASCIIEncoding.ASCII.GetString(getsetreq.name, 0, getsetreq.name_len);
Console.WriteLine("{0}: {1}", name, value);
timeout = DateTime.Now.AddSeconds(2);
index++;
}
};
MessageReceived += paramdelegate;
while (true)
{
if (DateTime.Now > timeout)
{
break;
}
uavcan.uavcan_protocol_param_GetSet_req req = new uavcan.uavcan_protocol_param_GetSet_req()
{
index = index
};
var slcan = PackageMessage(node, 30, transferID++, req);
lock (sr_lock)
{
WriteToStream(slcan);
}
Thread.Sleep(333);
}
MessageReceived -= paramdelegate;
_paramlistcache = paramlist;
return(paramlist);
}
public void RequestFile(byte nodeID, string filename)
{
bool reading = true;
int gotbytes = 0;
var req = new uavcan.uavcan_protocol_file_Read_req()
{
offset = 0,
path = new uavcan.uavcan_protocol_file_Path()
{
path = ASCIIEncoding.ASCII.GetBytes(filename),
path_len = (byte)filename.Length
}
};
MessageRecievedDel reqfile = (frame, msg, transferID) =>
{
if (frame.IsServiceMsg && frame.SvcDestinationNode != SourceNode)
{
return;
}
if (msg.GetType() == typeof(uavcan.uavcan_protocol_file_Read_res))
{
var readres = msg as uavcan.uavcan_protocol_file_Read_res;
//readres.data
req.offset += readres.data_len;
var slcan2 = PackageMessage(nodeID, 20, transferID, req);
lock (sr_lock)
WriteToStream(slcan2);
}
else if (msg.GetType() == typeof(uavcan.uavcan_protocol_debug_LogMessage))
{
var debug = msg as uavcan.uavcan_protocol_debug_LogMessage;
Console.WriteLine(ASCIIEncoding.ASCII.GetString(debug.text, 0, debug.text_len));
}
};
MessageReceived += reqfile;
var slcan = PackageMessage(nodeID, 20, transferID, req);
lock (sr_lock)
WriteToStream(slcan);
while (reading)
{
Thread.Sleep(3000);
if (gotbytes == 0)
{
lock (sr_lock)
WriteToStream(slcan);
}
}
MessageReceived -= reqfile;
}
