/// <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 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();
}
}
}
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);
}
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);
}
protected void ProcessMessage(MavlinkMessage msg)
{
if (msg.GetType() == typeof(Msg_heartbeat))
{
Process(msg as Msg_heartbeat);
}
else if (msg.GetType() == typeof(Msg_sys_status))
{
Process(msg as Msg_sys_status);
}
else if (msg.GetType() == typeof(Msg_gps_raw_int))
{
Process(msg as Msg_gps_raw_int);
}
else if (msg.GetType() == typeof(Msg_vfr_hud))
{
Process(msg as Msg_vfr_hud);
}
else if (msg.GetType() == typeof(Msg_attitude))
{
Process(msg as Msg_attitude);
}
else if (msg.GetType() == typeof(Msg_rc_channels_raw))
{
Process(msg as Msg_rc_channels_raw);
}
else if (msg.GetType() == typeof(Msg_battery_status))
{
Process(msg as Msg_battery_status);
}
else if (msg.GetType() == typeof(Msg_log_data))
{
Process(msg as Msg_log_data);
}
else
{
}
}
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);
}
}
}
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();
}
}
}
/// <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;
}
}
}
});
}
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));
}
}
private bool Mavlink_SetMessage(MavlinkMessage message, Type type)
{
switch (type.Name)
{
case "Msg_controlmode":
type.GetField("contrlmode").SetValue(message, (byte)DataBus.FlightStatus.ContrlMode);
break;
case "Msg_speedmode":
type.GetField("speedmode").SetValue(message, (byte)DataBus.FlightStatus.SpeedMode);
break;
case "Msg_gps_status":
type.GetField("satellites_visible").SetValue(message, (byte)DataBus.FlightData.GPSData.SatellitesCount);
break;
case "Msg_attitude":
type.GetField("roll").SetValue(message, DataBus.FlightData.Attitude.Angle_X);
type.GetField("pitch").SetValue(message, DataBus.FlightData.Attitude.Angle_Y);
type.GetField("yaw").SetValue(message, DataBus.FlightData.Attitude.Angle_Z);
type.GetField("rollspeed").SetValue(message, DataBus.FlightData.Attitude.Palstance_X);
type.GetField("pitchspeed").SetValue(message, DataBus.FlightData.Attitude.Palstance_Y);
type.GetField("yawspeed").SetValue(message, DataBus.FlightData.Attitude.Palstance_Z);
break;
case "Msg_attitude_ext":
type.GetField("Aacceleration_X").SetValue(message, DataBus.FlightData.Attitude.Aacceleration_X);
type.GetField("Aacceleration_Y").SetValue(message, DataBus.FlightData.Attitude.Aacceleration_Y);
type.GetField("Aacceleration_Z").SetValue(message, DataBus.FlightData.Attitude.Aacceleration_Z);
type.GetField("BarometricAltitude").SetValue(message, DataBus.FlightData.Attitude.BarometricAltitude);
type.GetField("Pressure").SetValue(message, DataBus.FlightData.Attitude.Pressure);
type.GetField("MicroAltitude").SetValue(message, DataBus.FlightData.Attitude.MicroAltitude);
break;
case "Msg_global_position_int":
type.GetField("lat").SetValue(message, DataBus.FlightData.GPSData.Latitude);
type.GetField("lon").SetValue(message, DataBus.FlightData.GPSData.Longitude);
type.GetField("alt").SetValue(message, DataBus.FlightData.GPSData.GPSAltitude);
type.GetField("relative_alt").SetValue(message, DataBus.FlightData.GPSData.GPSAltitude - DataBus.FlightData.GPSData.GroundAlt);
type.GetField("hdg").SetValue(message, DataBus.FlightData.GPSData.GPSHeading);
break;
case "Msg_global_position_int_ext":
type.GetField("GPSSpeed").SetValue(message, DataBus.FlightData.GPSData.GPSSpeed);
type.GetField("GPSYaw").SetValue(message, DataBus.FlightData.GPSData.GPSYaw);
type.GetField("SatellitesCount").SetValue(message, DataBus.FlightData.GPSData.SatellitesCount);
type.GetField("PositionalAccuracy").SetValue(message, DataBus.FlightData.GPSData.PositionalAccuracy);
type.GetField("HorizontalAccuracy").SetValue(message, DataBus.FlightData.GPSData.HorizontalAccuracy);
type.GetField("VerticalAccuracy").SetValue(message, DataBus.FlightData.GPSData.VerticalAccuracy);
break;
case "Msg_functionstatus":
break;
case "Msg_setfunctionstatus":
break;
case "Msg_log":
break;
case "Msg_home_position":
break;
default: return(false);
}
return(true);
}
public void SendMessage(MavlinkMessage message)
{
MavlinkPacket packet = new MavlinkPacket(message);
Mavlink.Send(packet);
}
