private void Process(Msg_rc_channels_raw msg)
{
Channel1 = msg.chan1_raw;
Channel2 = msg.chan2_raw;
Channel3 = msg.chan3_raw;
Channel4 = msg.chan4_raw;
Channel5 = msg.chan5_raw;
Channel6 = msg.chan6_raw;
Channel7 = msg.chan7_raw;
Channel8 = msg.chan8_raw;
}
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));
}
}
