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);
}
private void Mav_PacketReceived(object sender, MavlinkPacket e)
{
//Console.WriteLine("-- incomming packet type {0} --", e.Message.GetType());
MavlinkMessage m = e.Message;
if (e.Message.GetType() == typeof(MavLink.Msg_heartbeat))
{
Msg_heartbeat hb = (m as Msg_heartbeat);
Console.WriteLine("heartbeat from [{3};{4}] type={0} mav_version={1} autopilot={2}",
hb.type, hb.mavlink_version, hb.autopilot, e.SystemId, e.ComponentId);
}
else if (e.Message.GetType() == typeof(MavLink.Msg_serial_control))
{
Msg_serial_control sc = (m as Msg_serial_control);
if (sc.device == 9)
{
byte[] inbuf = new byte[sc.count];
Array.Copy(sc.data, inbuf, sc.count);
AppendToBuffer(inbuf);
}
}
}
