/// <summary>
///
/// </summary>
/// <exception cref="Exception">Various IO exceptions</exception>
/// <param name="data"></param>
/// <param name="length"></param>
private void Transmit(MavlinkMessage message)
{
MavlinkPacket packet = new MavlinkPacket();
packet.Message = message;
packet.SequenceNumber = mavMsg++;
packet.SystemId = SYSTEM_ID;
packet.ComponentId = wifiCompIdSettings;
packet.TimeStamp = DateTime.Now;
// TODO: systemid, componentid (?)
byte[] serializedMessageWiFi = mav.Send(packet); // mav.Serialize(message, 0, 0);
packet.ComponentId = serialCompIdSettings;
byte[] serializedMessageSerial = mav.Send(packet);
Parallel.ForEach(sinks, sink => {
lock (sink)
{
if (sink.Enabled)
{
if (sink.GetType() == typeof(SerialSink))
{
sink.Send(serializedMessageSerial);
}
else
{
sink.Send(serializedMessageWiFi);
}
lastTimeSent = DateTime.Now;
}
}
});
}
private void mavlink_send(byte[] buf, bool close)
{
MavlinkPacket p = new MavlinkPacket();
Msg_serial_control sc = new Msg_serial_control();
int left = buf.Length;
int idx = 0;
do
{
int c = Math.Min(70, left);
sc.data = new byte[c]; // System.Text.Encoding.UTF8.GetBytes(cmd);
Array.Copy(buf, idx, sc.data, 0, c);
sc.count = (byte)c;
sc.device = 9;
sc.baudrate = (uint)(close ? 0 : 1);
//Console.WriteLine("mavlink_send() len={0} buf='{1}'",
// sc.count, System.Text.Encoding.UTF8.GetString(buf));
p.Message = sc;
p.SystemId = 201;
p.ComponentId = 0;
byte[] packet = mav.Send(p);
serial_port.Write(packet, 0, packet.Length);
idx += c;
left -= c;
} while (left > 0);
}
public void sendMavMsg(byte cmd, byte addr, float pos)
{
try
{
/* mavlink encode */
_mht.mavlink_version = cmd;
_mht.custom_mode = (uint)pos;
_mht.base_mode = addr;
_mht.system_status = 4;
_mavPack.ComponentId = 1;
_mavPack.SystemId = 1;
_mavPack.SequenceNumber = 0;
_mavPack.TimeStamp = DateTime.Now;
_mavPack.Message = _mht;
byte[] buffer = _mav.Send(_mavPack);
_stream.Write(buffer, 0, buffer.Length);
Console.WriteLine("Write: " + Thread.CurrentThread.ManagedThreadId.ToString());
}
catch (Exception ee)
{
Console.WriteLine(ee.Message);
}
}
private void SendPacket(MavlinkMessage m)
{
MavlinkPacket p = new MavlinkPacket();
p.Message = m;
p.SequenceNumber = (byte)Sequence;
p.SystemId = 255;
p.ComponentId = (byte)MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER;
byte[] b = Mv.Send(p);
Serial.Write(b, 0, b.Length);
}
void send_mavlink_paket(MavlinkPacket packet)
{
byte[] data_to_send = mavlink_obj.Send(packet);
if (BthSocket.IsConnected)
{
var outs = BthSocket.OutputStream;
var base_out_steream = (outs as OutputStreamInvoker).BaseOutputStream;
base_out_steream.Write(data_to_send);
}
}
private void SendPacket(MavlinkMessage m)
{
MavlinkPacket p = new MavlinkPacket();
p.Message = m;
p.SequenceNumber = (byte)seq;
p.SystemId = SystemId;
p.ComponentId = ComponentId;
byte[] by = Mv.Send(p);
// Serial.Write(by, 0, by.Length);
socket.SendTo(by, by.Length, SocketFlags.None, ipEnd);
}
private void Tlogmav_PacketReceived(object sender, MavlinkPacket e)
{
//Console.WriteLine("will send packet from [{0};{1}] type {2}", e.SystemId, e.ComponentId, e.Message);
byte[] b = tlog_mav.Send(e);
if (mav_packet_send(b))
{
tlog_active = false;
}
System.Threading.Thread.Sleep(10);
//Console.WriteLine("sent .tlog packet");
}
public void SendTest()
{
Mavlink mavlink = new Mavlink();
Msg_attitude msg_Attitude = new Msg_attitude();
msg_Attitude.roll = 1;
MavlinkPacket packet = new MavlinkPacket(msg_Attitude);
var res = mavlink.Send(packet);
mavlink.PacketReceived += Mavlink_PacketReceived;
mavlink.ParseBytes(res);
Assert.AreNotEqual(mavlink.PacketsReceived, 0);
}
private void MainForm_FormClosed(object sender, FormClosedEventArgs e)
{
if (serialPort.IsOpen)
{
Msg_configuration_control msg = new Msg_configuration_control();
msg.command = (byte)CONFIG_COMMAND.CONFIG_COMMAND_EXIT;
mavlink_packet.Message = msg;
byte[] bytes = mavlink.Send(mavlink_packet);
try
{
serialPort.Write(bytes, 0, bytes.Length);
serialPort.Close();
deviceChanged.Stop();
}
catch { }
}
}
/// <summary>
///
/// </summary>
/// <exception cref="Exception">Various IO exceptions</exception>
/// <param name="data"></param>
/// <param name="length"></param>
private void Transmit(MavlinkMessage message)
{
MavlinkPacket packet = new MavlinkPacket();
packet.Message = message;
packet.SequenceNumber = mavMsg++;
packet.SystemId = SYSTEM_ID;
packet.ComponentId = COMPONENT_ID;
packet.TimeStamp = DateTime.Now;
// TODO: systemid, componentid (?)
byte[] serializedMessage = mav.Send(packet); // mav.Serialize(message, 0, 0);
// TODO: add all send methods
Parallel.Invoke(() => {
lock (udpLocker)
{
if (udpSink != null)
{
udpSink.Send(serializedMessage);
}
}
}, () => {
lock (tcpClientLocker)
{
if (tcpClientSink != null)
{
tcpClientSink.Send(serializedMessage);
}
}
}, () => {
lock (serialLocker)
{
if (serialSink != null)
{
serialSink.Write(serializedMessage, 0, serializedMessage.Length);
}
}
}, () =>
{
lock (tcpServerLocker)
{
if (tcpServerSink != null)
{
var client = tcpServerClients.First;
while (client != null)
{
var next = client.Next;
try
{
if (!client.Value.Connected)
{
tcpServerClients.Remove(client);
}
else
{
client.Value.GetStream().Write(serializedMessage, 0, serializedMessage.Length);
}
}
catch (Exception ex)
{
log.Error(ex);
}
client = next;
}
}
}
});
}
public void SendMessage(MavlinkMessage message)
{
MavlinkPacket packet = new MavlinkPacket(message);
Mavlink.Send(packet);
}
void send_mavlink_paket(MavlinkPacket packet)
{
byte[] data_to_send = mavlink_obj.Send(packet);
serial_worker.send_bin_data_to_serial(data_to_send);
}