// __ MavLink events __________________________________________________
private void HandlePacketReceived(object sender, MavlinkPacket e)
{
if (OnPacketReceived != null)
{
OnPacketReceived(sender, e);
}
}
static void NetworkLayerPacketReceived(object sender, MavlinkPacket e)
{
if (e.Message is MAVLink_vfr_hud_message)
ShowMessage((MAVLink_vfr_hud_message)e.Message);
else
Console.WriteLine(DefaultLineFormat, e.SystemId, e.ComponentId, e.Message.GetType().Name);
}
private void SendMavlinkMessage(IPEndPoint ep, MavlinkMessage msg)
{
MavlinkPacket p = GetPacketFromMsg(msg);
byte[] buffer = mMavLink.Send(p);
mUdpClient.Send(buffer, buffer.Length, ep);
}
public override void SendMessage(MavlinkMessage msg)
{
MavlinkPacket packet = CreatePacket(msg);
byte[] bytes = Send(packet);
try { serial.Write(bytes, 0, bytes.Length); }
catch { }
}
void Mv_PacketReceived(object sender, MavLink.MavlinkPacket e)
{
uint x = Mv.PacketsReceived;
MavlinkMessage m = e.Message;
if (m.GetType() == Hb.GetType())
{
Hb = (Msg_heartbeat)e.Message;
}
if (m.GetType() == Ss.GetType())
{
Ss = (Msg_sys_status)e.Message;
}
if (m.GetType() == Ps.GetType())
{
Ps = (Msg_power_status)e.Message;
}
if (m.GetType() == At.GetType())
{
At = (Msg_attitude)e.Message;
}
if (m.GetType() == Gps.GetType())
{
Gps = (Msg_gps_raw_int)e.Message;
}
if (m.GetType() == Vfr.GetType())
{
Vfr = (Msg_vfr_hud)e.Message;
}
if (m.GetType() == Rp.GetType())
{
Rp = (Msg_raw_pressure)e.Message;
}
if (m.GetType() == Sp.GetType())
{
Sp = (Msg_scaled_pressure)e.Message;
Prs = (int)(Sp.press_abs * 1000f);
Tep = Sp.temperature;
Dif = (int)(Sp.press_diff * 1000f);
/*
* if (Sw == null)
* {
* Sw = new StreamWriter("c:\\Baro.csv");
* Sw.Write("\"Item\", \"Pressure\", \"Temp\", \"Difference\"\r\n");
* }
* String S = String.Format("{0}, {1}, {2}, {3}\r\n", Total++, Prs, Tep, Dif);
* Sw.Write(S);*/
}
if (x > 0)
{
if (MavLinkReader.ActiveForm != null)
{
MavLinkReader.ActiveForm.Invalidate();
}
}
}
private MavlinkPacket CreatePacket(MavlinkMessage msg)
{
MavlinkPacket packet = new MavlinkPacket();
packet.ComponentId = (int)MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;
packet.SystemId = 0xCE;
packet.Message = msg;
return(packet);
}
private MavlinkPacket GetPacketFromMsg(MavlinkMessage msg)
{
MavlinkPacket p = new MavlinkPacket();
p.Message = msg;
p.TimeStamp = DateTime.Now;
p.SequenceNumber = 1;
p.SystemId = MavlinkSystemId;
p.ComponentId = MavlinkComponentId;
return(p);
}
// add timestamp to message ( called directly after receive)
protected override void postMessage(MavlinkPacket msg)
{
base.postMessage(msg);
if (_noTimeStamps)
msg.TimeStamp = DateTime.Now;
else
msg.TimeStamp = _timestamp;
if (_planner_format)
_stream.ReadByte(); // trailing newline
_timestamp = msg.TimeStamp;
}
private static void SendParamListRequest(MavlinkPacket mavlinkPacket)
{
if (_mavStream.CanWrite)
{
Console.WriteLine("Sending param list request: " + mavlinkPacket.SystemId);
var req = new MAVLink_param_request_list_message()
{
target_system = (byte) mavlinkPacket.SystemId,
target_component = (byte) mavlinkPacket.ComponentId,
};
_net.Send(new MavlinkPacket { ComponentId = 1, SystemId = 255, Message = req });
}
else
{
Console.WriteLine("Cannot send paramlist request. Scanning for params... ");
}
}
// Process a raw packet in it's entirety in the given byte array
// if deserialization is successful, then the packetdecoded event will be raised
private void ProcessPacketBytes(byte[] packetBytes, byte rxPacketSequence)
{
// System ID 1 - 255
// Component ID 0 - 255
// Message ID 0 - 255
// 6 to (n+6) Data (0 - 255) bytes
var packet = new MavlinkPacket
{
SystemId = packetBytes[0],
ComponentId = packetBytes[1],
SequenceNumber = rxPacketSequence,
Message = this.Deserialize(packetBytes, 2)
};
if (PacketReceived != null)
{
PacketReceived(this, packet);
}
// else do what?
}
/// <summary>
/// 发送数据包，使用此方法时，请先对事件SetMessage赋值
/// 如果消息没有被处理赋值，则会返回null
/// </summary>
/// <param name="obj">message的<see cref="Type"></param>
/// <returns></returns>
public byte[] Send(Type type)
{
var message = Activator.CreateInstance(type) as MavlinkMessage;
MavlinkPacket packet = new MavlinkPacket(message);
packet.SystemId = 1;
packet.ComponentId = 1;
packet.SequenceNumber = (byte)((SequenceNumber >> 24) & 0xFF);
if (SequenceNumber == 255)
{
SequenceNumber = 0;
}
else
{
SequenceNumber++;
}
type.GetField("time_usec").SetValue(message, GetTimeStamp());
if (SetMessage == null || !SetMessage.Invoke(message, type))
{
return(null);
}
return(Send(packet));
}
void Mv_PacketReceived(object sender, MavLink.MavlinkPacket e)
{
uint x = Mv.PacketsReceived;
Systemid = e.SystemId;
Componentid = e.ComponentId;
Sequence = e.SequenceNumber;
MavlinkMessage m = e.Message;
if (m.GetType() == Hb.GetType())
{
Hb = (Msg_heartbeat)e.Message;
}
if (m.GetType() == Ss.GetType())
{
Ss = (Msg_sys_status)e.Message;
}
if (m.GetType() == Ps.GetType())
{
Ps = (Msg_power_status)e.Message;
}
if (m.GetType() == At.GetType())
{
At = (Msg_attitude)e.Message;
}
if (m.GetType() == Gps.GetType())
{
Gps = (Msg_gps_raw_int)e.Message;
}
if (m.GetType() == Vfr.GetType())
{
Vfr = (Msg_vfr_hud)e.Message;
}
if (m.GetType() == Rp.GetType())
{
Rp = (Msg_raw_pressure)e.Message;
}
if (m.GetType() == Sp.GetType())
{
Sp = (Msg_scaled_pressure)e.Message;
Prs = (int)(Sp.press_abs * 1000f);
Tep = Sp.temperature;
Dif = (int)(Sp.press_diff * 1000f);
/*
* Special log of Barometric data to the D drive
* Can be disable if not needed
*/
ut.LogPressure(Sp);
}
if (m.GetType() == Ca.GetType())
{
Ca = (Msg_command_ack)e.Message;
}
if (m.GetType() == St.GetType())
{
St = (Msg_statustext)e.Message;
}
if (m.GetType() == Mc.GetType())
{
Mc = (Msg_mission_count)e.Message;
MI = 0;
GetMissionValues.RunWorkerAsync();
}
if (m.GetType() == Mi[0].GetType())
{
Mi[MI++] = (Msg_mission_item)e.Message;
if (MI >= Mc.count)
{
MI = -1;
}
}
if (x > 0)
{
if (MavLinkReader.ActiveForm != null)
{
MavLinkReader.ActiveForm.Invalidate();
}
}
}
public void Send(MavlinkPacket mavlinkPacket)
{
var bytes = _encoder.Serialize(mavlinkPacket.Message, mavlinkPacket.SystemId, mavlinkPacket.ComponentId);
SendPacketLinkLayer(bytes);
}
private void mavlinkNetwork_PacketReceived(object sender, MavlinkPacket e)
{
if (e.Message is Msg_heartbeat)
{
if (RemoteSystemDetected != null)
RemoteSystemDetected(this, (Msg_heartbeat)e.Message);
}
//if (e.SystemId==_srcSystemId && e.ComponentId==_srcComponentId && PacketReceived!=null)
if (PacketReceived!=null)
{
PacketReceived(this,e.Message);
}
}
/// <summary>
/// Send a mavlink message through this connection
/// </summary>
/// <param name="message">the message to send</param>
public void Send(object message)
{
var mvp = new MavlinkPacket
{
ComponentId = _srcComponentId,
SystemId = _srcSystemId,
Message = message
};
_mavlink.Send(mvp);
}
/// <summary>
/// 发送数据包
/// </summary>
/// <param name="mavlinkPacket"></param>
/// <returns></returns>
public byte[] Send(MavlinkPacket mavlinkPacket)
{
var bytes = this.Serialize(mavlinkPacket.Message, mavlinkPacket.SystemId, mavlinkPacket.ComponentId);
return(SendPacketLinkLayer(bytes));
}
public byte[] Send(MavlinkPacket mavlinkPacket)
{
var bytes = this.Serialize(mavlinkPacket.Message, mavlinkPacket.SystemId, mavlinkPacket.ComponentId);
return SendPacketLinkLayer(bytes);
}
// Process a raw packet in it's entirety in the given byte array
// if deserialization is successful, then the packetdecoded event will be raised
private MavlinkPacket ProcessPacketBytes(byte[] packetBytes, byte rxPacketSequence)
{
// System ID 1 - 255
// Component ID 0 - 255
// Message ID 0 - 255
// 6 to (n+6) Data (0 - 255) bytes
var packet = new MavlinkPacket
{
SystemId = packetBytes[0],
ComponentId = packetBytes[1],
SequenceNumber = rxPacketSequence,
Message = _encoder.Deserialize(packetBytes, 2)
};
if (PacketReceived != null)
{
PacketReceived(this, packet);
}
return packet;
}
private void SendPacket(object msg)
{
var packet = new MavlinkPacket
{
ComponentId = _sourceSystemId,
SystemId = _sourceComponentId,
Message = msg,
};
_mavlink.Send(packet);
}
protected void SetStream(Stream stream)
{
_stream = stream;
_mavlink = new Mavlink(stream);
_mavlink.PacketReceived += (s, e) => { _latestPacket = e; };
}
// Called directly after a receive but before sending packet to client
protected virtual void postMessage(MavlinkPacket packet)
{
//msg._timestamp = time.time()
var type = packet.Message.GetType();
_messages[type] = packet.Message;
// TODO: timestamp somehow
//_timestamp = msg._timestamp;
// Do I want to do this? i.e always aquire target id?
//if type == 'HEARTBEAT':
// self.target_system = msg.get_srcSystem()
// self.target_component = msg.get_srcComponent()
// if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
// self.flightmode = mode_string_v10(msg)
if (type == typeof(Msg_param_value))
{
var m = (Msg_param_value)packet.Message;
_parameters[ByteArrayUtil.ToString(m.param_id)] = m.param_value;
if (m.param_index + 1 == m.param_count)
{
_paramFetchInProgress = false;
_paramFetchComplete = true;
}
}
// what is this all about then?
// else if (type == typeof(Msg_sys_status) && WIRE_PROTOCOL_VERSION == '0.9' )
// {
// _flightmode = mode_string_v09(msg);
// }
// Todo: what's all this about then?
// elif type == 'GPS_RAW':
// if self.messages['HOME'].fix_type < 2:
// self.messages['HOME'] = msg
// Todo: implement message receive 'hook' event
//for hook in self.message_hooks:
// hook(self, msg)
}
// message receive routine
public MavlinkPacket ReceiveMessage()
{
preMessage();
while (true)
{
// Todo: how many bytes to read?
//var n = _mav.bytes_needed();
var n = 1;
var s = new byte[n];
var bytesRead = _stream.Read(s, 0, n);
// todo: get this woriking
//if (bytesRead == 0 && _mav.buf.Length == 0) // Whaaaa?)
// return null;
if (_logfile_raw != null)
_logfile_raw.Write(s, 0, s.Length);
//var packet = _mavnet.ProcessPacketBytes(s, bytesRead);
_latestPacket = null;
_mavlink.ParseBytes(s);
if (_latestPacket != null)
{
postMessage(_latestPacket);
return _latestPacket;
}
}
}
