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