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();
}
}
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();
}
}
}
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);
}
// Use this for initialization
void Start()
{
SystemId = (byte)MAV_MODE.MAV_MODE_MANUAL_ARMED;
ComponentId = (byte)MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1;
Application.runInBackground = true;
mlpn = new Msg_local_position_ned();
mgpi = new Msg_global_position_int();
Mheratbeat = new Msg_heartbeat();
Msts = new Msg_sys_status();
Matt = new Msg_attitude();
Mdatastream = new Msg_request_data_stream();
Mgpsr = new Msg_gps_raw_int();
Mattq = new Msg_attitude_quaternion();
Mvhud = new Msg_vfr_hud();
MattTar = new Msg_attitude_target();
MHome = new Msg_home_position();
setMHome = new Msg_set_home_position();
setMode = new Msg_set_mode();
EstimatorStatus = new Msg_estimator_status();
Mvirbration = new Msg_vibration();
ExtendState = new Msg_extended_sys_state();
latlonalt = new LatLonAlt();
Malt = new Msg_altitude();
positionL = new Msg_position_target_local_ned();
setPositionL = new Msg_set_position_target_local_ned();
setPositionG = new Msg_set_position_target_global_int();
PositionG = new Msg_position_target_global_int();
MhiIMU = new Msg_highres_imu();
BatteryStatus = new Msg_battery_status();
player = GameObject.FindGameObjectWithTag("Player");
DM = player.GetComponent <DroneMove> ();
InitSocket();
setHomePosition();
setPositionTargetLocal();
setPositionTargetGlobal();
}
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;
}
}
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();
}
}
}
private void Process(Msg_attitude msg)
{
Roll = msg.roll * 180.0 / Math.PI;
Pitch = msg.pitch * 180.0 / Math.PI;
Heading = msg.yaw * 180.0 / Math.PI;
}
private void ProcessMessage(MavlinkMessage msg)
{
if (msg == null)
{
return;
}
Type t = msg.GetType();
if (currentStep != StateMachineStep.Connected && t != typeof(Msg_configuration_version) && t != typeof(Msg_configuration_version2))
{
return;
}
if (t == typeof(Msg_configuration_control))
{
Msg_configuration_control msg_control = (msg as Msg_configuration_control);
switch ((CONFIG_COMMAND)msg_control.command)
{
case CONFIG_COMMAND.CONFIG_COMMAND_ACK:
if (msg_control.param1 == (byte)CONFIG_PORT.CONFIG_PORT_1)
{
lblPort1.Text = ((ProtocolType)msg_control.param2).GetDescription();
}
else if (msg_control.param1 == (byte)CONFIG_PORT.CONFIG_PORT_2)
{
lblPort2.Text = ((ProtocolType)msg_control.param2).GetDescription();
}
tsLoading.Visible = false;
btnPort1Configure.Enabled = true;
btnPort2Configure.Enabled = true;
break;
case CONFIG_COMMAND.CONFIG_COMMAND_NACK:
tsLoading.Visible = false;
btnPort1Configure.Enabled = true;
btnPort2Configure.Enabled = true;
MessageBox.Show(this, string.Format("Error while saving Configuration for Port {0}", msg_control.param1), "Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
break;
case CONFIG_COMMAND.CONFIG_COMMAND_BOOTLOADER:
serialPort.Close();
StateMachineUpdate(StateMachineStep.None);
break;
default:
break;
}
}
else if (t == typeof(Msg_configuration_port))
{
Msg_configuration_port msg_config = (msg as Msg_configuration_port);
try
{
PortConfiguration config = new PortConfiguration();
config.Port = msg_config.port;
config.Protocol = (ProtocolType)msg_config.protocol;
if (config.Configuration != null)
{
config.Configuration.DeSerialize(msg_config.data);
}
tsLoading.Visible = false;
if (PortConfigurationForm.ShowDialog(this, config) == DialogResult.OK)
{
try
{
Msg_configuration_port msg_send = new Msg_configuration_port();
msg_send.port = msg_config.port;
msg_send.protocol = (byte)config.Protocol;
if (config.Configuration != null)
{
msg_send.data = config.Configuration.Serialize();
}
else
{
msg_send.data = new byte[128];
}
mavlink_packet.Message = msg_send;
byte[] bytes = mavlink.Send(mavlink_packet);
serialPort.Write(bytes, 0, bytes.Length);
tsLoading.Visible = true;
btnPort1Configure.Enabled = false;
btnPort2Configure.Enabled = false;
Task.Factory.StartNew(() =>
{
Extensions.TaskExtensions.Delay(5000).Wait();
if (!Disposing && !IsDisposed)
{
BeginInvoke(((Action)(() =>
{
if (!Disposing && !IsDisposed)
{
if (tsLoading.Visible)
{
tsLoading.Visible = false;
btnPort1Configure.Enabled = true;
btnPort2Configure.Enabled = true;
MessageBox.Show(this, "Could not load Configuration. A connection timeout occurred.", "Timeout", MessageBoxButtons.OK, MessageBoxIcon.Error);
}
}
})));
}
});
}
catch (Exception ex)
{
StateMachineUpdate(StateMachineStep.None);
MessageBox.Show(this, "Error while communicating:\n" + ex.Message, "Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
}
}
else
{
btnPort1Configure.Enabled = true;
btnPort2Configure.Enabled = true;
}
}
catch (Exception ex)
{
tsLoading.Visible = false;
btnPort1Configure.Enabled = true;
btnPort2Configure.Enabled = true;
MessageBox.Show(this, string.Format("Error loading Configuration for Port {0}\n{1}", msg_config.port, ex.Message), "Error",
MessageBoxButtons.OK, MessageBoxIcon.Error);
}
}
else if (t == typeof(Msg_configuration_version) || t == typeof(Msg_configuration_version2))
{
StateMachineUpdate(StateMachineStep.Connected);
uint version = 0;
if (t == typeof(Msg_configuration_version))
{
//0x0009
Msg_configuration_version msg_version = (msg as Msg_configuration_version);
lblPort1.Text = ((ProtocolType)msg_version.port1).GetDescription();
lblPort2.Text = ((ProtocolType)msg_version.port2).GetDescription();
version = msg_version.fw_version;
version = version << 8;
}
else
{
//0x00000901
Msg_configuration_version2 msg_version = (msg as Msg_configuration_version2);
version = msg_version.fw_version;
lblPort1.Text = ((ProtocolType)msg_version.port1).GetDescription();
lblPort2.Text = ((ProtocolType)msg_version.port2).GetDescription();
}
tsFWVersion.Text = string.Format("{0}.{1}.{2}", (byte)(version >> 16), (byte)(version >> 8), (byte)version);
btnUpdate.Visible = FIRMWARE_VERSION > version;
lblUpdateHint.Visible = FIRMWARE_VERSION != version;
lblUpdateHint.Text = (FIRMWARE_VERSION > version ? "Your Firmware ist outdated.\n Please click Update to flash your AnySense with latest Firmware Version." :
"Your Konfiguration Manager is outdated.\nPlease visit our support page for more information.");
btnPort1Configure.Enabled = FIRMWARE_VERSION == version;
btnPort2Configure.Enabled = FIRMWARE_VERSION == version;
}
else if (t == typeof(Msg_heartbeat))
{
Msg_heartbeat heartbeat = (msg as Msg_heartbeat);
lblFlightMode.Text = ((FlightMode)heartbeat.custom_mode).GetDescription();
tsUniAdapterHeartbeat.BackgroundImage = KonfigurationTool.Properties.Resources.led_g_an;
if (UAHeartbeat == null || UAHeartbeat.Status == TaskStatus.RanToCompletion)
{
UAHeartbeat = Task.Factory.StartNew(() =>
{
Extensions.TaskExtensions.Delay(300).Wait();
if (!Disposing && !IsDisposed)
{
BeginInvoke(((Action)(() =>
{
if (!Disposing && !IsDisposed)
{
tsUniAdapterHeartbeat.BackgroundImage = KonfigurationTool.Properties.Resources.led_aus;
}
})));
}
});
}
retry = 0;
}
else if (t == typeof(Msg_configuration_naza_heartbeat))
{
tsFCHearbeat.BackgroundImage = KonfigurationTool.Properties.Resources.led_b_an;
if (FCHeartbeat == null || FCHeartbeat.Status == TaskStatus.RanToCompletion)
{
FCHeartbeat = Task.Factory.StartNew(() =>
{
Extensions.TaskExtensions.Delay(300).Wait();
if (!Disposing && !IsDisposed)
{
BeginInvoke(((Action)(() =>
{
if (!Disposing && !IsDisposed)
{
tsFCHearbeat.BackgroundImage = KonfigurationTool.Properties.Resources.led_aus;
}
})));
}
});
}
}
else if (t == typeof(Msg_sys_status))
{
Msg_sys_status sys = (msg as Msg_sys_status);
lblBattery.Text = (sys.voltage_battery / 1000f).ToString("0.00 V");
lblCurrent.Text = (sys.current_battery / 100f).ToString("0.00 A");
}
else if (t == typeof(Msg_gps_raw_int))
{
Msg_gps_raw_int gps = (msg as Msg_gps_raw_int);
lblCOG.Text = (gps.cog / 100f).ToString("0.00 °");
lblAltitude.Text = (gps.alt / 1000f).ToString("0.00 m");
lblGPSFix.Text = ((FixType)gps.fix_type).GetDescription();
lblHDOP.Text = (gps.eph / 100f).ToString("0.00");
lblVDOP.Text = (gps.epv / 100f).ToString("0.00");
lblLatitude.Text = (gps.lat / 10000000.0f).ToString("0.000000");
lblLongitude.Text = (gps.lon / 10000000.0f).ToString("0.000000");
lblSpeed.Text = (gps.vel / 100f).ToString("0.00 m/s");
lblNumSat.Text = gps.satellites_visible.ToString();
}
else if (t == typeof(Msg_vfr_hud))
{
Msg_vfr_hud vfr = (msg as Msg_vfr_hud);
lblClimb.Text = vfr.climb.ToString("0.00 m/s");
lblSpeed.Text = vfr.groundspeed.ToString("0.00 m/s");
lblHeading.Text = vfr.heading.ToString("0 °");
lblThrottle.Text = vfr.throttle.ToString() + " %";
}
else if (t == typeof(Msg_attitude))
{
Msg_attitude att = (msg as Msg_attitude);
lblRoll.Text = (att.roll * 180.0 / Math.PI).ToString("0.00 °");
lblPitch.Text = (att.pitch * 180.0 / Math.PI).ToString("0.00 °");
}
else if (t == typeof(Msg_rc_channels))
{
Msg_rc_channels rc = (msg as Msg_rc_channels);
lblRC1.Text = rc.chan2_raw.ToString();
lblRC2.Text = rc.chan3_raw.ToString();
lblRC3.Text = rc.chan6_raw.ToString();
lblRC4.Text = rc.chan4_raw.ToString();
lblRC5.Text = rc.chan8_raw.ToString();
lblRC6.Text = rc.chan9_raw.ToString();
lblRC7.Text = rc.chan5_raw.ToString();
lblRC8.Text = rc.chan10_raw.ToString();
}
else if (t == typeof(Msg_rc_channels_raw))
{
Msg_rc_channels_raw rc = (msg as Msg_rc_channels_raw);
lblRC1.Text = rc.chan1_raw.ToString();
lblRC2.Text = rc.chan2_raw.ToString();
lblRC3.Text = rc.chan3_raw.ToString();
lblRC4.Text = rc.chan4_raw.ToString();
lblRC5.Text = rc.chan5_raw.ToString();
lblRC6.Text = rc.chan6_raw.ToString();
lblRC7.Text = rc.chan7_raw.ToString();
lblRC8.Text = rc.chan8_raw.ToString();
}
else if (t == typeof(Msg_battery_status))
{
Msg_battery_status batt = (msg as Msg_battery_status);
lbCell1.Text = string.Format("{0:00}. {1:0.00} V", 1, ((float)batt.voltages[0] / 1000f));
lbCell2.Text = string.Format("{0:00}. {1:0.00} V", 2, ((float)batt.voltages[1] / 1000f));
lbCell3.Text = string.Format("{0:00}. {1:0.00} V", 3, ((float)batt.voltages[2] / 1000f));
lbCell4.Text = string.Format("{0:00}. {1:0.00} V", 4, ((float)batt.voltages[3] / 1000f));
lbCell5.Text = string.Format("{0:00}. {1:0.00} V", 5, ((float)batt.voltages[4] / 1000f));
lbCell6.Text = string.Format("{0:00}. {1:0.00} V", 6, ((float)batt.voltages[5] / 1000f));
lbCell7.Text = string.Format("{0:00}. {1:0.00} V", 7, ((float)batt.voltages[6] / 1000f));
lbCell8.Text = string.Format("{0:00}. {1:0.00} V", 8, ((float)batt.voltages[7] / 1000f));
lbCell9.Text = string.Format("{0:00}. {1:0.00} V", 9, ((float)batt.voltages[8] / 1000f));
lbCell10.Text = string.Format("{0:00}. {1:0.00} V", 10, ((float)batt.voltages[9] / 1000f));
}
}