public void ReadMission(long idx, byte sysid, byte compid, ObservableCollection <Waypoint> waypoints)
{
Application.Current.Dispatcher.BeginInvoke(DispatcherPriority.Normal, new Action(() =>
{
var vehicle = Vehicles.FirstOrDefault(v => v.ID == idx) as GuiQuadVehicle;
if (vehicle != null)
{
//vehicle.Tasks.Clear();
var taskId = ServiceLocator.Current.GetInstance <IMissionSource>().GenerateTaskId();
var mavlinkTaskNav = new MavlinkTaskNav("Task" + taskId, taskId)
{
NavPath = waypoints
};
ServiceLocator.Current.GetInstance <IMissionSource>().AddTask(mavlinkTaskNav);
vehicle.Tasks.Add(mavlinkTaskNav);
vehicle.MissionID = mavlinkTaskNav.TaskID;
var convertPoint = new ObservableCollection <Waypoint>();
foreach (MavlinkWaypoint point in mavlinkTaskNav.NavPath)
{
if (point.Command == (decimal)MMAVLink.MAVLink.MAV_CMD.WAYPOINT)
{
var x = point.Latitude;
var y = point.Longitude;
//Utils.Utils.ConvertMercatorToGeo(ref y, ref x);
convertPoint.Add(new MavlinkWaypoint
{
Latitude = x,
Longitude = y,
Altitude = point.Altitude,
Id = point.Id,
Command = point.Command,
Param1 = point.Param1,
Param2 = point.Param2,
Param3 = point.Param3,
Param4 = point.Param4,
});
}
}
var newShape = new Shape(mavlinkTaskNav.TaskID, convertPoint)
{
Symbol = (LineSymbol)Application.Current.FindResource("LineSymbol")
};
ServiceLocator.Current.GetInstance <IMap>().AddShape(newShape);
}
}));
}
private void MapMouseLeftButtonDownAction(MapMouseLeftButtonDownMessenger obj)
{
int DistanceDefault = 50;
if (VehicleSelected == null)
{
return;
}
if (InsertHoverPoint)
{
//Target
//var a = Utils.Utils.DegreeBearing(VehicleSelected.Latitude, VehicleSelected.Longitude, obj.Latitude,obj.Longitude);
var convertPoint = new ObservableCollection <Waypoint>();
var x = VehicleSelected.Latitude;
var y = VehicleSelected.Longitude;
//Utils.Utils.ConvertMercatorToGeo(ref y, ref x);
/*var bearing = Utils.Utils.DegreeBearing(VehicleSelected.Latitude, VehicleSelected.Longitude, obj.Latitude,obj.Longitude);
* var distance = Utils.Utils.CalculateDistance(VehicleSelected.Latitude, VehicleSelected.Longitude,
* obj.Latitude, obj.Longitude, "K") *1000;
* if (distance > DistanceDefault)
* {
* Utils.Utils.PointRadialDistance(ref x, ref y, bearing, DistanceDefault);
* }*/
convertPoint.Add(new MavlinkWaypoint
{
Latitude = x,
Longitude = y,
Altitude = DefaultAltitude,
Id = 1,
Param4 = (float)VehicleSelected.Heading,
Command = (ushort)MissionMavCmd.WAYPOINT,
});
convertPoint.Add(new MavlinkWaypoint
{
Latitude = obj.Latitude,
Longitude = obj.Longitude,
Altitude = DefaultAltitude,
Id = 2,
Command = (ushort)MissionMavCmd.WAYPOINT,
});
var task = new MavlinkTaskNav("HoverTask", TaskID)
{
NavPath = convertPoint,
TaskType = TaskTypeEnum.Hoverrer
};
MissionSource.AddTask(task);
VehicleSelected.Tasks.Clear();
VehicleSelected.Tasks.Add(task);
var newShape = new Shape(TaskID, convertPoint)
{
Symbol = (LineSymbol)Application.Current.FindResource("LineSymbol")
};
ServiceLocator.Current.GetInstance <IMap>().AddShape(newShape);
//MissionComponent.UploadTask(VehicleSelected.ID,
// VehicleSelected.HomePosition,
// new MavlinkWaypoint
// {
// Latitude = VehicleSelected.Latitude,
// Longitude = VehicleSelected.Longitude,
// Altitude = DefaultAltitude,
// Param4 = (float) VehicleSelected.Heading
// },
// VehicleSelected.Tasks[0]);
VehicleSelected.IsActivateMission = false;
MissionComponent.UploadTask(VehicleSelected.ID, VehicleSelected.HomePosition, null, VehicleSelected.Tasks[0]);
VehicleSelected.MissionID = VehicleSelected.Tasks[0].TaskID;
}
}
private void MapWaypointMessengerAction(MapWaypointMessenger obj)
{
//NavTask.NavPath.Clear();
//ServiceLocator.Current.GetInstance<IMap>().RemoveAllShape();
byte index = 1;
var mission = (MissionPackage.Mission)Mission;
ITask first = null;
foreach (Task task in mission.Tasks)
{
if (task.TaskID == obj.TaskId)
{
first = task;
break;
}
}
var taskNav = first as MavlinkTaskNav;
if (taskNav != null)
{
taskNav.NavPath.Clear();
foreach (var waypoint in obj.Points)
{
double x = waypoint.X, y = waypoint.Y;
//Utils.Utils.ConvertGeoToMercator(ref x, ref y);
taskNav.NavPath.Add(new MavlinkWaypoint
{
Id = index,
Command = (ushort)MissionMavCmd.WAYPOINT,
Latitude = y,
Longitude = x,
Altitude = DefaultAltitude,
Param1 = 0,
Param2 = 0,
Param3 = 0,
Param4 = 0
});
index++;
}
}
else
{
var mavlinkTaskNav = new MavlinkTaskNav("Task_" + (ushort)obj.TaskId, (ushort)obj.TaskId);
foreach (var waypoint in obj.Points)
{
double x = waypoint.X, y = waypoint.Y;
//Utils.Utils.ConvertGeoToMercator(ref x, ref y);
mavlinkTaskNav.NavPath.Add(new MavlinkWaypoint
{
Id = index,
Command = (ushort)MissionMavCmd.WAYPOINT,
Latitude = y,
Longitude = x,
Altitude = DefaultAltitude,
Param1 = 0,
Param2 = 0,
Param3 = 0,
Param4 = 0
});
index++;
}
Mission.AddTask(mavlinkTaskNav);
}
Messenger.Default.Send(new UpdataMissionMessenger {
MissionId = Mission.MissionID
});
}
