public IEnumerable <LogEntry> GetRows(string typeName, DateTime startTime, TimeSpan duration) { foreach (var msg in GetMessages(startTime, duration)) { object typedValue = msg.TypedValue; if (typedValue != null) { if (typeName == "GPS") { // do auto-conversion from MAVLink.mavlink_global_position_int_t if (typedValue is MAVLink.mavlink_global_position_int_t) { MAVLink.mavlink_global_position_int_t gps = (MAVLink.mavlink_global_position_int_t)typedValue; LogEntry e = new LogEntry(); e.SetField("GPSTime", (ulong)gps.time_boot_ms * 1000); // must be in microseconds. e.SetField("Lat", (double)gps.lat / 1E7); e.SetField("Lon", (double)gps.lon / 1E7); e.SetField("Alt", (float)((double)gps.alt / 1000)); e.SetField("nSat", 9); // fake values so the map works e.SetField("EPH", 1); yield return(e); } } } } }
static PointF GetPixel(RectLatLng area, MAVLink.mavlink_global_position_int_t loc, Size size) { double lon = loc.lon / 10000000.0f; double lat = loc.lat / 10000000.0f; double lonscale = (lon - area.Left) * (size.Width - 0) / (area.Right - area.Left) + 0; double latscale = (lat - area.Top) * (size.Height - 0) / (area.Bottom - area.Top) + 0; return(new PointF((float)lonscale, (float)latscale)); }
private void DowdingPlugin_UpdateOutput(object sender, PointLatLngAlt e) { var gpi = new MAVLink.mavlink_global_position_int_t((uint)(DateTime.Now - starttime).TotalMilliseconds, (int)(e.Lat * 1e7), (int)(e.Lng * 1e7), (int)(e.Alt * 1e2), 0, 0, 0, 0, 0); var packet = mavlink.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.GLOBAL_POSITION_INT, gpi, 255, 190, sequence++); ATStream.Write(packet, 0, packet.Length); if (sequence % 10 == 0) { SendHome(); } }
public position(MAVLink.mavlink_global_position_int_t globalposition) { mission_position.alt = globalposition.alt; mission_position.hdg = globalposition.hdg; mission_position.lat = globalposition.lat; mission_position.lon = globalposition.lon; mission_position.relative_alt = globalposition.relative_alt; mission_position.time_boot_ms = globalposition.time_boot_ms; mission_position.vx = globalposition.vx; mission_position.vy = globalposition.vy; mission_position.vz = globalposition.vz; time = DateTime.Now; }
public GlobalPositionInt(MavLinkMessage message) : base(null) { if (message.messid.Equals(this.MessageID)) { MAVLink.mavlink_global_position_int_t data = (MAVLink.mavlink_global_position_int_t)message.data_struct; this.alt = data.alt; this.hdg = data.hdg; this.lat = data.lat; this.lon = data.lon; this.relative_alt = data.relative_alt; this.time_boot_ms = data.time_boot_ms; this.vx = data.vx; this.vy = data.vy; this.vz = data.vz; } }
private void DowdingPlugin_UpdateOutput(object sender, PointLatLngAlt e) { if (e == lastplla) { return; } lastplla = e; if (!mavlink.BaseStream.IsOpen) { return; } var gpi = new MAVLink.mavlink_global_position_int_t((uint)(DateTime.Now - starttime).TotalMilliseconds, (int)(e.Lat * 1e7), (int)(e.Lng * 1e7), (int)(e.Alt * 1e2), 0, 0, 0, 0, 0); mavlink.generatePacket((int)MAVLink.MAVLINK_MSG_ID.GLOBAL_POSITION_INT, gpi, 2, 1); }
public void CheckGlobalPostionIntObj() { MAVLink.mavlink_global_position_int_t data = new MAVLink.mavlink_global_position_int_t(); data.alt = 1; data.hdg = 2; data.lat = 3; data.lon = 4; data.relative_alt = 5; data.time_boot_ms = 6; data.vx = 7; data.vy = 8; data.vz = 9; MavLinkMessage message = createSampleMessage(MAVLink.MAVLINK_MSG_ID.GLOBAL_POSITION_INT, data); GlobalPositionInt obj = new GlobalPositionInt(message); Assert.AreEqual(data.alt, obj.alt); Assert.AreEqual(data.hdg, obj.hdg); Assert.AreEqual(data.lat, obj.lat); Assert.AreEqual(data.lon, obj.lon); Assert.AreEqual(data.relative_alt, obj.relative_alt); Assert.AreEqual(data.time_boot_ms, obj.time_boot_ms); Assert.AreEqual(data.vx, obj.vx); Assert.AreEqual(data.vy, obj.vy); Assert.AreEqual(data.vz, obj.vz); GlobalPositionIntDTO dto = DTOFactory.createGlobalPositionIntDTO(obj); Assert.AreEqual(dto.alt, obj.alt); Assert.AreEqual(dto.hdg, obj.hdg); Assert.AreEqual(dto.lat, obj.lat); Assert.AreEqual(dto.lon, obj.lon); Assert.AreEqual(dto.relative_alt, obj.relative_alt); Assert.AreEqual(dto.time_boot_ms, obj.time_boot_ms); Assert.AreEqual(dto.vx, obj.vx); Assert.AreEqual(dto.vy, obj.vy); Assert.AreEqual(dto.vz, obj.vz); }
private void LogPacket(object packet, bool ingoing, int sysId, int compId) { if (threadDone) { return; } if (listViewMessages.InvokeRequired) { try { listViewMessages.Invoke(new Action <object, bool, int, int>(LogPacket), packet, ingoing, sysId, compId); } catch { }; return; } List <string> fields = new List <string>(); fields.Add(sysId.ToString()); fields.Add(compId.ToString()); if ((ingoing && !checkBoxIngoing.Checked) || (!ingoing && !checkBoxOutgoing.Checked)) { return; } string messageName = packet.ToString().Replace("MAVLink+mavlink_", ""); if (IsMessageFilteredOut(messageName)) { return; } if (listViewMessages.IsDisposed) { return; } if (packet.GetType() == typeof(MAVLink.mavlink_gps_raw_int_t)) { MAVLink.mavlink_gps_raw_int_t gps = (MAVLink.mavlink_gps_raw_int_t)packet; fields.Add("GPS Raw Int"); fields.Add(((double)gps.lat / 10000000).ToString()); fields.Add(((double)gps.lon / 10000000).ToString()); fields.Add(gps.alt.ToString()); fields.Add(gps.vel.ToString()); fields.Add(gps.satellites_visible.ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_global_position_int_t)) { MAVLink.mavlink_global_position_int_t gps = (MAVLink.mavlink_global_position_int_t)packet; fields.Add("GPS Global Position Int"); fields.Add(((double)gps.lat / 10000000).ToString()); fields.Add(((double)gps.lon / 10000000).ToString()); fields.Add(gps.alt.ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_attitude_t)) { MAVLink.mavlink_attitude_t att = (MAVLink.mavlink_attitude_t)packet; fields.Add("Attitude"); fields.Add((att.pitch * 180.0 / Math.PI).ToString()); fields.Add((att.roll * 180.0 / Math.PI).ToString()); fields.Add((att.yaw * 180.0 / Math.PI).ToString()); fields.Add((att.pitchspeed * 180.0 / Math.PI).ToString()); fields.Add((att.rollspeed * 180.0 / Math.PI).ToString()); fields.Add((att.yawspeed * 180.0 / Math.PI).ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_scaled_imu_t)) { MAVLink.mavlink_scaled_imu_t imu = (MAVLink.mavlink_scaled_imu_t)packet; fields.Add("Scaled IMU"); fields.Add(imu.xmag.ToString()); fields.Add(imu.ymag.ToString()); fields.Add(imu.zmag.ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_scaled_imu3_t)) { MAVLink.mavlink_scaled_imu3_t imu = (MAVLink.mavlink_scaled_imu3_t)packet; fields.Add("Scaled IMU3"); fields.Add(imu.xmag.ToString()); fields.Add(imu.ymag.ToString()); fields.Add(imu.zmag.ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_scaled_imu2_t)) { MAVLink.mavlink_scaled_imu2_t imu = (MAVLink.mavlink_scaled_imu2_t)packet; fields.Add("Scaled IMU2"); fields.Add(imu.xmag.ToString()); fields.Add(imu.ymag.ToString()); fields.Add(imu.zmag.ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_sys_status_t)) { MAVLink.mavlink_sys_status_t status = (MAVLink.mavlink_sys_status_t)packet; fields.Add("System Status"); fields.Add(status.voltage_battery.ToString()); } // else if (packet.GetType() == typeof(MAVLink.mavlink_autopilot_version_t)) // { // MAVLink.mavlink_autopilot_version_t ver = (MAVLink.mavlink_autopilot_version_t)packet; // listViewMessages.Items.Add(new ListViewItem(new string[] { // "Autopilot Version", // ver.version.ToString(), // ver.custom_version.ToString(), // ver.capabilities.ToString()})); // } else if (packet.GetType() == typeof(MAVLink.mavlink_heartbeat_t)) { MAVLink.mavlink_heartbeat_t hb = (MAVLink.mavlink_heartbeat_t)packet; fields.Add("Heartbeat"); fields.Add(hb.autopilot.ToString()); fields.Add(hb.system_status.ToString()); fields.Add(hb.mavlink_version.ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_statustext_t)) { MAVLink.mavlink_statustext_t status = (MAVLink.mavlink_statustext_t)packet; fields.Add("Status Text"); fields.Add(ASCIIEncoding.ASCII.GetString(status.text)); fields.Add(status.severity.ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_param_value_t)) { MAVLink.mavlink_param_value_t paramValue = (MAVLink.mavlink_param_value_t)packet; fields.Add("Param Value"); fields.Add(ASCIIEncoding.ASCII.GetString(paramValue.param_id)); fields.Add(paramValue.param_value.ToString()); fields.Add(paramValue.param_count.ToString()); fields.Add(paramValue.param_index.ToString()); fields.Add(paramValue.param_type.ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_param_request_read_t)) { MAVLink.mavlink_param_request_read_t paramReq = (MAVLink.mavlink_param_request_read_t)packet; fields.Add("Param Request Read"); fields.Add(ASCIIEncoding.ASCII.GetString(paramReq.param_id)); } else if (packet.GetType() == typeof(MAVLink.mavlink_param_set_t)) { MAVLink.mavlink_param_set_t paramSet = (MAVLink.mavlink_param_set_t)packet; fields.Add("Param Set"); fields.Add(ASCIIEncoding.ASCII.GetString(paramSet.param_id)); fields.Add(paramSet.param_value.ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_mission_count_t)) { MAVLink.mavlink_mission_count_t paramValue = (MAVLink.mavlink_mission_count_t)packet; fields.Add("Mission Count"); fields.Add(paramValue.count.ToString()); fields.Add(paramValue.target_component.ToString()); fields.Add(paramValue.target_system.ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_mission_item_t)) { MAVLink.mavlink_mission_item_t item = (MAVLink.mavlink_mission_item_t)packet; fields.Add("Mission Item"); fields.Add(item.seq.ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_mission_request_t)) { MAVLink.mavlink_mission_request_t item = (MAVLink.mavlink_mission_request_t)packet; fields.Add("Mission Request Item"); fields.Add(item.seq.ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_command_ack_t)) { MAVLink.mavlink_command_ack_t paramValue = (MAVLink.mavlink_command_ack_t)packet; fields.Add("Ack"); fields.Add(((MAVLink.MAV_CMD)paramValue.command).ToString()); fields.Add(((MAVLink.MAV_RESULT)paramValue.result).ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_mission_ack_t)) { MAVLink.mavlink_mission_ack_t paramValue = (MAVLink.mavlink_mission_ack_t)packet; fields.Add("Mission Ack"); fields.Add(paramValue.type.ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_radio_status_t)) { MAVLink.mavlink_radio_status_t radio = (MAVLink.mavlink_radio_status_t)packet; fields.Add("Radio Status"); fields.Add(radio.rssi.ToString()); fields.Add(radio.remrssi.ToString()); } else if (packet.GetType() == typeof(MAVLink.mavlink_ekf_status_report_t)) { MAVLink.mavlink_ekf_status_report_t ekf = (MAVLink.mavlink_ekf_status_report_t)packet; fields.Add("EKF Status"); fields.Add(ekf.flags.ToString()); fields.Add(ekf.velocity_variance.ToString()); fields.Add(ekf.pos_horiz_variance.ToString()); fields.Add(ekf.pos_vert_variance.ToString()); fields.Add(ekf.compass_variance.ToString()); fields.Add(ekf.terrain_alt_variance.ToString()); } else { fields.Add(messageName); //Log(packet.ToString()); } if (ingoing) { listViewMessages.Items.Add(INGOING(new ListViewItem(fields.ToArray()))); } else { listViewMessages.Items.Add(OUTGOING(new ListViewItem(fields.ToArray()))); } }
public void SetAttitude(MAVLink.mavlink_global_position_int_t globalposition) { mission_position = globalposition; time = DateTime.Now; }
private int updatedata(int timeout = 5000) { if (ReadHeartBeat()) { DateTime deadline = DateTime.Now.AddMilliseconds(timeout); while (DateTime.Now < deadline) { try { var data = _mavlinkparse.ReadPacket(_datastream); if (data == null) { continue; } switch (data.GetType().ToString()) { case "MAVLink+mavlink_attitude_t": MAVLink.mavlink_attitude_t pack0 = (MAVLink.mavlink_attitude_t)data; Vehicle.Roll = pack0.roll; Vehicle.RollSpeed = pack0.rollspeed; Vehicle.Pitch = pack0.pitch; Vehicle.PitchSpeed = pack0.pitchspeed; Vehicle.Yaw = pack0.yaw; Vehicle.YawSpeed = pack0.yawspeed; Vehicle.BootTime = pack0.time_boot_ms; break; case "MAVLink+_global_position_int_t": MAVLink.mavlink_global_position_int_t pack1 = (MAVLink.mavlink_global_position_int_t)data; Vehicle.Attitude = pack1.alt; Vehicle.Latitude = pack1.lat; Vehicle.Longtitude = pack1.lon; break; case "MAVLink+mavlink_sys_status_t": MAVLink.mavlink_sys_status_t pack2 = (MAVLink.mavlink_sys_status_t)data; Vehicle.BatteryVoltage = pack2.voltage_battery; Vehicle.BatteryCurrent = pack2.current_battery; Vehicle.BatteryRemaining = pack2.battery_remaining; break; case "MAVLink+mavlink_vfr_hud_t": MAVLink.mavlink_vfr_hud_t pack3 = (MAVLink.mavlink_vfr_hud_t)data; Vehicle.AirSpeed = pack3.airspeed; Vehicle.GroungSpeed = pack3.groundspeed; break; case "MAVLink+mavlink_heartbeat_t": ReadHeartBeat(); Vehicle.HeartBeating = true; deadline = DateTime.Now.AddMilliseconds(timeout); break; default: break; } } catch { Vehicle.Connected = false; } } } return(0); }
private static void HandlePacket(object packet) { if (packet.GetType() == typeof(MAVLink.mavlink_heartbeat_t)) { MAVLink.mavlink_heartbeat_t parsed = (MAVLink.mavlink_heartbeat_t)packet; droneState = (MAVLink.MAV_STATE)parsed.system_status; //MAVLink.MAV_MODE mode = (APMModes.Quadrotor)parsed.custom_mode; } else if (packet.GetType() == typeof(MAVLink.mavlink_global_position_int_t)) { MAVLink.mavlink_global_position_int_t gps = (MAVLink.mavlink_global_position_int_t)packet; // dronePosition.lat = (double)gps.lat / 10000000; // dronePosition.lng = (double)gps.lon / 10000000; // dronePosition.alt = (double)gps.alt / 1000; dronePosition.altFromGround = (double)gps.relative_alt / 1000; dronePosition.heading = (double)gps.hdg / 100; } else if (packet.GetType() == typeof(MAVLink.mavlink_gps_raw_int_t)) { MAVLink.mavlink_gps_raw_int_t gps = (MAVLink.mavlink_gps_raw_int_t)packet; GPSHdop = (double)gps.eph / 100; dronePosition.lat = (double)gps.lat / 10000000; dronePosition.lng = (double)gps.lon / 10000000; dronePosition.alt = (double)gps.alt / 1000; satCount = gps.satellites_visible; IsDroneReady(); } else if (packet.GetType() == typeof(MAVLink.mavlink_mission_count_t)) { MAVLink.mavlink_mission_count_t paramValue = (MAVLink.mavlink_mission_count_t)packet; missionWaypointsCount = paramValue.count; missionItems = new SMissionItem[missionWaypointsCount]; } else if (packet.GetType() == typeof(MAVLink.mavlink_mission_item_t)) { MAVLink.mavlink_mission_item_t paramValue = (MAVLink.mavlink_mission_item_t)packet; SMissionItem missionItem = new SMissionItem(); missionItem.command = (MAVLink.MAV_CMD)paramValue.command; // if (missionItem.command == MAVLink.MAV_CMD.TAKEOFF) // // || (missionItem.command == MAVLink.MAV_CMD.RETURN_TO_LAUNCH)) // { // missionItem.lat = dronePosition.lat; // missionItem.lng = dronePosition.lng; // } // else { missionItem.lat = paramValue.x; missionItem.lng = paramValue.y; } missionItem.p1 = paramValue.param1; missionItem.p2 = paramValue.param2; missionItem.p3 = paramValue.param3; missionItem.p4 = paramValue.param4; missionItem.alt = paramValue.z; // Is this the home? missionItem.IsHome = (paramValue.seq == 0); missionItems[paramValue.seq] = missionItem; } else if (packet.GetType() == typeof(MAVLink.mavlink_mission_request_t)) { MAVLink.mavlink_mission_request_t missionReq = (MAVLink.mavlink_mission_request_t)packet; SetMissionItem(missionReq.seq); IsDroneReady(); } else if (packet.GetType() == typeof(MAVLink.mavlink_param_value_t)) { MAVLink.mavlink_param_value_t paramValue = (MAVLink.mavlink_param_value_t)packet; string name = ASCIIEncoding.ASCII.GetString(paramValue.param_id); name = name.Trim().Replace("\0", ""); double val = paramValue.param_value; if (OnParamUpdate != null) { OnParamUpdate(name, val, paramValue.param_index); } } else if (packet.GetType() == typeof(MAVLink.mavlink_ekf_status_report_t)) { MAVLink.mavlink_ekf_status_report_t ekf = (MAVLink.mavlink_ekf_status_report_t)packet; // Check that all estimations are good EKFStatus = (float)Math.Max(ekf.pos_vert_variance, Math.Max(ekf.compass_variance, Math.Max(ekf.pos_horiz_variance, Math.Max(ekf.pos_vert_variance, ekf.terrain_alt_variance)))); EKFFlags = ekf.flags; IsDroneReady(); } }