/// <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); }