/// <summary>
/// Event from bluetooth device
/// </summary>
/// <param name="packet"></param>
private void MavlinkDataReceivedHandlerMap(MavlinkPacket packet)
{
CheckBox chkMagnOn = FindViewById <CheckBox>(Resource.Id.chkMagnetic);
if (packet.Message.GetType() == typeof(MavLink.Msg_mav_game_rotation_vector_euler))
{
Msg_mav_game_rotation_vector_euler game_angle = (MavLink.Msg_mav_game_rotation_vector_euler)packet.Message;
if (chkMagnOn.Checked == false)
{
currVisirDirGeg = game_angle.yaw;
DrawDirection(currVisirDirGeg);
}
}
if (packet.Message.GetType() == typeof(MavLink.Msg_mav_rotation_vector_euler))
{
Msg_mav_rotation_vector_euler rotation_angle = (MavLink.Msg_mav_rotation_vector_euler)packet.Message;
if (chkMagnOn.Checked == true)
{
currVisirDirGeg = rotation_angle.yaw;
DrawDirection(currVisirDirGeg);
}
}
else if (packet.Message.GetType() == typeof(MavLink.Msg_mav_button_event))
{
parse_keys_event((MavLink.Msg_mav_button_event)packet.Message);
}
}
/// <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);
}
private void OnPacketRecieved(object sender, 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;
PressureAbsolute = (int)(Sp.press_abs * 1000f);
Temperature = Sp.temperature;
PressureDifference = (int)(Sp.press_diff * 1000f);
Total++;
if (OnAtmosphericData != null)
{
OnAtmosphericData.Invoke(PressureAbsolute, PressureDifference, Temperature, Total);
}
}
if (x > 0 && OnPacketCountGreaterThanZero != null)
{
OnPacketCountGreaterThanZero.Invoke();
}
}
private void RequestMav()
{
Ds.message_rate = 2;
Ds.on_off = 1;
Ds.stream_id = (byte)MAV_DATA_STREAM.MAV_DATA_STREAM_ALL;
MavlinkPacket p = new MavlinkPacket();
}
private void MavlinkDataReceivedHandlerMap(MavlinkPacket packet)
{
if (packet.Message.GetType() == typeof(MavLink.Msg_mav_game_rotation_vector_euler))
{
Msg_mav_game_rotation_vector_euler game_angle = (MavLink.Msg_mav_game_rotation_vector_euler)packet.Message;
UpdateCurrentPosition(game_angle.pitch, game_angle.yaw);
}
}
public void ProcessRxMavlinkPacket(object sender, MavlinkPacket e)
{
//partical parsing
if (e.Message.GetType() == typeof(MavLink.Msg_mav_button_event))
{
parse_keys_event((MavLink.Msg_mav_button_event)e.Message);
}
MavlinkDataReceived?.Invoke(e);
}
private void Mavlink_PacketReceived(object sender, MavlinkPacket e)
{
var type = e.Message.GetType();
switch (type.Name)
{
case "Msg_attitude":
break;
}
}
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");
}
private void mavlink_PacketReceived(object sender, MavlinkPacket e)
{
if (e.Message == null)
{
return;
}
if (!Disposing && !IsDisposed)
{
BeginInvoke((Action <MavlinkMessage>)ProcessMessage, e.Message);
}
}
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);
}
public void MavlinkDataReceivedHandlerMain(MavlinkPacket packet)
{
if (packet.Message.GetType() == typeof(MavLink.Msg_mav_game_rotation_vector_euler))
{
game_rotation_angles.update_from_mavlink((MavLink.Msg_mav_game_rotation_vector_euler)packet.Message);
UpdateMainScreen();
}
else if (packet.Message.GetType() == typeof(MavLink.Msg_mav_batt_state))
{
MavLink.Msg_mav_batt_state battery_state = (MavLink.Msg_mav_batt_state)packet.Message;
battery_voltage = battery_state.voltage;
}
}
/// <summary>
/// Set mode
/// </summary>
/// <param name="mode">0 - base mode, 1 - target1</param>
void sent_mavlink_switch_mode(byte mode)
{
MavLink.Msg_mav_set_visir_mode mav_msg = new MavLink.Msg_mav_set_visir_mode();
mav_msg.mode_code = mode;
var mvp = new MavlinkPacket
{
ComponentId = 1,
SystemId = 2,//PC
Message = mav_msg
};
send_mavlink_paket(mvp);
}
//set direction
void sent_mavlink_target(float direction_deg)
{
MavLink.Msg_mav_set_target_pos mav_msg = new MavLink.Msg_mav_set_target_pos();
mav_msg.yaw = direction_deg;
var mvp = new MavlinkPacket
{
ComponentId = 1,
SystemId = 2,//PC
Message = mav_msg
};
send_mavlink_paket(mvp);
}
private static void Mavlink_PacketReceived(object sender, MavlinkPacket e)
{
var type = e.Message.GetType();
switch (type.Name)
{
case "Msg_attitude":
var message = (Msg_attitude)e.Message;
FlightData.Attitude.Angle_X = message.roll;
FlightData.Attitude.Angle_Y = message.pitch;
FlightData.Attitude.Angle_Z = message.yaw;
break;
}
}
void sent_mavlink_yaw_correction(byte vect_type, float correction)
{
MavLink.Msg_mav_set_yaw_correction mav_msg = new MavLink.Msg_mav_set_yaw_correction();
mav_msg.vector_type = vect_type;
mav_msg.value = correction;
var mvp = new MavlinkPacket
{
ComponentId = 1,
SystemId = 2,//PC
Message = mav_msg
};
send_mavlink_paket(mvp);
}
public MainForm()
{
InitializeComponent();
StateMachineUpdate(StateMachineStep.None);
mavlink_packet = new MavlinkPacket();
mavlink_packet.ComponentId = (int)MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;
mavlink_packet.SystemId = 0xCE;
mavlink = new Mavlink();
mavlink.PacketReceived += mavlink_PacketReceived;
byte[] ver = BitConverter.GetBytes(FIRMWARE_VERSION);
Text = string.Format("AnySense {0}.{1}.{2} - Konfiguration Manager", ver[2], ver[1], ver[0]);
}
/// <summary>
/// Event from bluetooth device
/// </summary>
/// <param name="packet"></param>
private void MavlinkDataReceivedHandlerMap2(MavlinkPacket packet)
{
if (packet.Message.GetType() == typeof(MavLink.Msg_mav_game_rotation_vector_euler))
{
Msg_mav_game_rotation_vector_euler game_angle = (MavLink.Msg_mav_game_rotation_vector_euler)packet.Message;
UpdateVisirDirection(game_angle.yaw);
}
/*
* if (packet.Message.GetType() == typeof(MavLink.Msg_mav_rotation_vector_euler))
* {
* Msg_mav_rotation_vector_euler rotation_angle = (MavLink.Msg_mav_rotation_vector_euler)packet.Message;
* }
*/
else if (packet.Message.GetType() == typeof(MavLink.Msg_mav_button_event))
{
parse_keys_event((MavLink.Msg_mav_button_event)packet.Message);
}
}
/// <summary>
/// 通过message操作function的状态
/// </summary>
/// <param name="packet"></param>
private static void SetFunction(MavlinkPacket packet)
{
if (packet.Message.GetType().Name == "Msg_setfunctionstatus")
{
var message = packet.Message as Msg_setfunctionstatus;
switch ((int)message.status)
{
case (int)FunctionStatus.Online:
Timer.Elapsed += Functions[Encoding.UTF8.GetString(message.functionname)].Excute;
break;
case (int)FunctionStatus.Offline:
Timer.Elapsed -= Functions[Encoding.UTF8.GetString(message.functionname)].Excute;
break;
case (int)FunctionStatus.Failure:
break;
}
}
}
public void recvMavMsg(object sender, MavlinkPacket e)
{
Console.WriteLine("recvMavMsg: " + Thread.CurrentThread.ManagedThreadId.ToString());
string str = e.Message.ToString();
if (str == "MavLink.Msg_heartbeat")
{
/* convert */
Msg_heartbeat ht = (Msg_heartbeat)e.Message;
/* save the incoming msg */
mode = ht.custom_mode;
/* publish */
commDataEvent(mode);
}
else if (e.Message.ToString() == "Mavlink.Msg_attitude")
{
Msg_attitude at = (Msg_attitude)e.Message;
}
}
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);
}
}
}
/// <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;
}
}
}
});
}
void send_mavlink_paket(MavlinkPacket packet)
{
byte[] data_to_send = mavlink_obj.Send(packet);
serial_worker.send_bin_data_to_serial(data_to_send);
}
public void ProcessRxMavlinkPacket(object sender, MavlinkPacket e)
{
//System.Diagnostics.Debug.WriteLine("number", e.SequenceNumber.ToString());
if (e.Message.GetType() == typeof(MavLink.Msg_mav_rotation_vector_euler))
{
Invoke((MethodInvoker) delegate()
{
rx_cnt++;
rotation_angles.update_from_mavlink((MavLink.Msg_mav_rotation_vector_euler)e.Message);
anglesDisplayRotation.update_angles(rotation_angles);
if (chkUseGyro.Checked == false)
{
mapControl1.draw_direction(rotation_angles.yaw);
}
});
}
if (e.Message.GetType() == typeof(MavLink.Msg_mav_quaternion))
{
Invoke((MethodInvoker) delegate()
{
rx_cnt++;
MavLink.Msg_mav_quaternion m_quaternion = (MavLink.Msg_mav_quaternion)e.Message;
raw_quaternion = new Quaternion(m_quaternion.x, m_quaternion.y, m_quaternion.z, m_quaternion.w);
lblRawQuaternion.Text = $"Raw Quat: X={raw_quaternion.X:0.0000} Y={raw_quaternion.Y:0.0000} Z={raw_quaternion.Z:0.0000} W={raw_quaternion.W:0.0000}";
rotation_angles_t1.update_from_quaternion(raw_quaternion);
//anglesDisplay1.update_angles(rotation_angles_t1);
});
}
if (e.Message.GetType() == typeof(MavLink.Msg_mav_game_rotation_vector_euler))
{
Invoke((MethodInvoker) delegate()
{
rx_cnt++;
game_rotation_angles.update_from_mavlink((MavLink.Msg_mav_game_rotation_vector_euler)e.Message);
anglesDisplay1.update_angles(game_rotation_angles);
if (chkUseGyro.Checked == true)
{
mapControl1.draw_direction(game_rotation_angles.yaw);
}
NMEAPublisherObj.CurrentAngle = game_rotation_angles.yaw;
});
}
if (e.Message.GetType() == typeof(MavLink.Msg_mav_button_event))
{
Invoke((MethodInvoker) delegate()
{
rx_cnt++;
parse_keys_event((MavLink.Msg_mav_button_event)e.Message);
});
}
if (e.Message.GetType() == typeof(MavLink.Msg_mav_batt_state))
{
Invoke((MethodInvoker) delegate()
{
rx_cnt++;
MavLink.Msg_mav_batt_state msg = (MavLink.Msg_mav_batt_state)e.Message;
lblBattery.Text = $"Battery: {msg.voltage:0.00} V";
});
}
statistic_analyse(e.Message.GetType().ToString());
}
public void recvMavMsg(object sender, MavlinkPacket e)
{
Console.WriteLine("mavMsg: " + e.Message);
}
public void SendMessage(MavlinkMessage message)
{
MavlinkPacket packet = new MavlinkPacket(message);
Mavlink.Send(packet);
}
private void Mavlink_PacketReceived(object sender, MavlinkPacket e)
{
RecivePacket?.Invoke(e);
}
