public MapaInterpolado()
{
lista = new ListaMapas();
//lista.BuscarFiles(@"D:\Mapas José Antonio", "*.map", true);
center = new WayPoint("", -6.126663f, 37.35648f);
InitializeComponent();
}
public WayPoint getWpt()
{
WayPoint wpt = new WayPoint();
wpt.Longitude = this.getLongitude();
wpt.Latitude = this.getLatitude();
return wpt;
}
public WayPoint(WayPoint wpt)
{
name = wpt.name;
this.Longitude = wpt.Longitude;
this.Latitude = wpt.Latitude;
this.Altitude = wpt.Altitude;
this.icon = wpt.icon;
}
public void TeamRefresh(WayPoint wpt)
{
try
{
mapControl1.team_pos[wpt.name] = wpt;
mapControl1.Invalidate();
}
catch (Exception) { }
}
public OziexplorerMapBackgrnd()
{
lista = new ListaMapas();
lista.BuscarFiles(me.CacheMapsPath, "*.map", true);
//lista.BuscarFiles(@"D:\Mapas José Antonio", "*.map", true);
center = new WayPoint("", -6.1939f, 37.2630f);
}
public IkarusMapControl()
{
home = new WayPoint("HOME", singleton.HomeLon, singleton.HomeLat);
web_bg = new WebMapBackgrnd(this);
//ozi_bg = new OziexplorerMapBackgrnd();
map = web_bg;
mensaje = null;
SetCenter(home);
InitializeComponent();
this.DoubleBuffered = true;
}
public WayPoint getNextWpt(WayPoint pos)
{
float dist=pos.getDistance(ruta[currWptID]);
if (dist <= lastdistance)
lastdistance = dist;
else if (dist > lastdistance && lastdistance < WptRange)
{
lastdistance = float.PositiveInfinity;
if (currWptID < ruta.Count - 1)
currWptID++;
else
currWptID = 0;
}
return ruta[currWptID];
}
public void LoadFromFile(string filename)
{
StreamReader fin = File.OpenText(filename);
int numwpts = 0;
if (int.TryParse(fin.ReadLine(), out numwpts))
{
ruta.Clear();
for (int i = 0; i < numwpts; i++)
{
WayPoint wpt = new WayPoint();
wpt.name = fin.ReadLine();
wpt.Longitude = float.Parse(fin.ReadLine());
wpt.Latitude = float.Parse(fin.ReadLine());
wpt.Altitude = float.Parse(fin.ReadLine());
ruta.Add(wpt);
}
}
}
private void listBox1_SelectedIndexChanged(object sender, EventArgs e)
{
if (listBoxWpts.SelectedIndex >= 0)
{
wpt = ruta[listBoxWpts.SelectedIndex];
mapControl1.Invalidate();
formUpdate();
// mapControl1.listeners += Listener_ModificarWaypoint;
}
}
void Listener_FijarRuta(WayPoint waypoint, MouseButtons btn)
{
if (btn == MouseButtons.Left)
{
waypoint.name = "WPT " + ruta.Count;
ruta.Add(waypoint);
listBoxUpdate();
mapControl1.Invalidate();
}
else if (btn == MouseButtons.None && ruta.Count>0)
{
WayPoint wpt = ruta[ruta.Count-1];
float dist=wpt.getDistance(waypoint);
mapControl1.mensaje = "Dist "+dist.ToString(".000");
}
}
public WayPoint ReadGPS()
{
IkarusGPSInfo gps = new IkarusGPSInfo();
byte[] tmp = Read(Comandos.gpsinfo, 0, 0, gps.size_bytes());
gps.FromByteArray(tmp);
WayPoint wpt=new WayPoint("GPS");
wpt.Altitude = gps.alt;
wpt.Longitude = gps.lon;
wpt.Latitude = gps.lat;
if (gps.fix != 0) //gpsinfo.valid
return wpt;
else
return null;
}
public WayPoint ReadWpt(int id)
{
WayPoint wpt = null;
wpt = new WayPoint();
int i = 0;
byte[] buffer = Read(Comandos.Ruta, (byte)id, 0, 32);
wpt.name = USBXpress.tostring(buffer, ref i, 20);
wpt.Longitude = USBXpress.tofloat(buffer, ref i);
wpt.Latitude = USBXpress.tofloat(buffer, ref i);
wpt.Altitude = USBXpress.tofloat(buffer, ref i);
return wpt;
}
public void SetCenter(WayPoint wpt)
{
map.SetCenter(wpt);
}
public void Send(PlaneState planeState, WayPoint home)
{
if (planeState != null)
{
this.planeState = planeState;
this.home = home;
this.planeStateUpdated = true;
}
}
private void button7_Click(object sender, EventArgs e)
{
OpenFileDialog dlg = new OpenFileDialog();
dlg.DefaultExt = ".ikarus";
dlg.AddExtension = true;
dlg.Filter = "Rutas (*.ikarus)|*.ikarus";
dlg.ShowDialog(this);
if (!dlg.FileName.Equals(""))
{
StreamReader fin = File.OpenText(dlg.FileName);
int numwpts = 0;
if (int.TryParse(fin.ReadLine(), out numwpts))
{
ruta.Clear();
for (int i = 0; i < numwpts; i++)
{
WayPoint wpt = new WayPoint();
wpt.name = fin.ReadLine();
// Poner que soporte tambien en cultura nativa
wpt.Longitude = float.Parse(fin.ReadLine().Replace(',', '.'), CultureInfo.InvariantCulture);
wpt.Latitude = float.Parse(fin.ReadLine().Replace(',', '.'), CultureInfo.InvariantCulture);
wpt.Altitude = float.Parse(fin.ReadLine().Replace(',', '.'), CultureInfo.InvariantCulture);
ruta.Add(wpt);
}
}
listBoxUpdate();
formUpdate();
mapControl1.Invalidate();
}
}
private void timer1_Tick(object sender, EventArgs e)
{
if (button7.Text == "FIXED")
{
if (comboBoxTeam.SelectedItem != null)
{
string nombre = comboBoxTeam.SelectedItem.ToString();
if (jthread != null && nombre != null)
{
WayPoint wpt;
if (mapControl1.team_pos.TryGetValue(nombre, out wpt))
{
jthread.modem_lon = (float)wpt.Longitude;
jthread.modem_lat = (float)wpt.Latitude;
jthread.modem_alt = (float)wpt.Altitude;
}
else
{
button7.Text = "Fail";
}
}
}
else
{
button7.Text = "Track";
}
}
if (button2.Text == "STOP")
{
mapControl1.SetCenter(mapControl1.plane);
mapControl1.Invalidate();
}
else if (button3.Text == "STOP")
{
int texel = 256;
GeoPos geo = new GeoPos(mapControl1.plane,mapControl1.GetZoom());
double x = Math.Abs(geo.getdX(mapControl1.home))* texel;
double y = Math.Abs(geo.getdY(mapControl1.home)) * texel;
int zoom;
// zona de seguridad
x = x * 1.2f;
y = y * 1.2f;
while ((x > mapControl1.Width || y > mapControl1.Height) && (geo.getZoom() > 0))
{
geo.setZoom(geo.getZoom() - 1);
x = Math.Abs(geo.getdX(mapControl1.home)) * texel;
y = Math.Abs(geo.getdY(mapControl1.home)) * texel;
}
while ((x < mapControl1.Width / 2) && (y < mapControl1.Height / 2) && (geo.getZoom() < 17))
{
zoom = geo.getZoom();
geo.setZoom(geo.getZoom() + 1);
x = Math.Abs(geo.getdX(mapControl1.home)) * texel;
y = Math.Abs(geo.getdY(mapControl1.home)) * texel;
}
WayPoint medio=new WayPoint("",(mapControl1.plane.Longitude+mapControl1.home.Longitude)/2,
(mapControl1.plane.Latitude+mapControl1.home.Latitude)/2);
mapControl1.SetCenter(medio);
mapControl1.SetZoom(geo.getZoom());
mapControl1.Invalidate();
}/* */
RefreshInstruments();
UpdateTeams();
if (antTracker != null)
{
antTracker.Send(knob_anttracker.Manual, knob_anttracker.Valor, 10);
}
}
public float calcBearing(WayPoint b)
{
return calcBearing(this, b);
}
public GeoPos(WayPoint wpt, int zoom)
{
this.Zoom = zoom;
this.Longitude = wpt.Longitude;
this.Latitude = wpt.Latitude;
}
public double getdY(WayPoint wpt)
{
GeoPos geo = new GeoPos(wpt, this.getZoom());
return geo.getY() - this.getY();
}
bool CheckPlaneState(PlaneState ps)
{
if (float.IsNaN(ps.Lat) || float.IsNaN(ps.Lon) || float.IsNaN(ps.homeLat) || float.IsNaN(ps.homeLon))
return false;
if (ps.Lat > 90.0f || ps.Lat < -90.0f)
return false;
if (ps.Lon > 180.0f || ps.Lon < -180.0f)
return false;
if (ps.homeLat > 90.0f || ps.homeLat < -90.0f)
return false;
if (ps.homeLon > 180.0f || ps.homeLon < -180.0f)
return false;
if (ps.Lat < 1 && ps.Lat > -1 && ps.Lon < 1 && ps.Lon > -1)
return false;
if (lastWpt == null)
lastWpt = new WayPoint("", ps.Lon, ps.Lat);
WayPoint here = new WayPoint("", ps.Lon, ps.Lat);
if ((check_counter < 20) && (lastWpt.getDistance(here) > 1))
{
check_counter++;
return false;
}
else
{
check_counter = 0;
lastWpt = here;
}
return true;
}
void Listener_ModificarWaypoint(WayPoint waypoint, MouseButtons btn)
{
if (btn == MouseButtons.Left)
{
wpt.Latitude = waypoint.Latitude;
wpt.Longitude = waypoint.Longitude;
mapControl1.listeners -= Listener_ModificarWaypoint;
mapControl1.Invalidate();
formUpdate();
}
}
public static float getDistance(WayPoint a, WayPoint b)
{
double lon1 = a.Longitude;
double lat1 = a.Latitude;
double lon2 = b.Longitude;
double lat2 = b.Latitude;
double sindlat = Math.Sin(toRadians(lat1 - lat2) / 2.0);
double sindlon = Math.Sin(toRadians(lon1 - lon2) / 2.0);
double d = 2.0 * Math.Asin(Math.Sqrt(sindlat * sindlat + sindlon * sindlon * Math.Cos(toRadians(lat1)) * Math.Cos(toRadians(lat2))));
return (float)d * EARTH;
}
void Listener_InsertarWaypoint(WayPoint waypoint, MouseButtons btn)
{
if (btn == MouseButtons.Left)
{
int sel = listBoxWpts.SelectedIndex;
wpt = new WayPoint("new");
wpt.Longitude = waypoint.Longitude;
wpt.Latitude = waypoint.Latitude;
if (sel >= 0)
ruta.Insert(sel, wpt);
else
ruta.Add(wpt);
listBoxUpdate();
listBoxWpts.SelectedIndex = sel;
mapControl1.listeners -= Listener_InsertarWaypoint;
mapControl1.Invalidate();
formUpdate();
}
}
public void SetCenter(WayPoint wpt)
{
this.tmpCenter = wpt;
mapa = lista.GetBest(new PuntoF((float)tmpCenter.Longitude, (float)tmpCenter.Latitude));
}
void Listener_FijarCasa(WayPoint waypoint, MouseButtons btn)
{
if (btn == MouseButtons.Left)
{
mapControl1.home.Latitude = waypoint.Latitude;
mapControl1.home.Longitude = waypoint.Longitude;
Singleton.GetInstance().HomeLat = (float)waypoint.Latitude;
Singleton.GetInstance().HomeLon = (float)waypoint.Longitude;
Singleton.GetInstance().HomeAlt = (float)waypoint.Altitude;
mapControl1.listeners -= Listener_FijarCasa;
mapControl1.Invalidate();
}
}
public static float calcBearing(WayPoint a, WayPoint b)
{
double lon1 = a.Longitude;
double lat1 = a.Latitude;
double lon2 = b.Longitude;
double lat2 = b.Latitude;
double y = Math.Sin(toRadians(lon2-lon1)) * Math.Cos(toRadians(lat2));
double x = Math.Cos(toRadians(lat1)) * Math.Sin(toRadians(lat2)) - Math.Sin(toRadians(lat1)) * Math.Cos(toRadians(lat2)) * Math.Cos(toRadians(lon2 - lon1));
if(y==0&&x==0)
return 00.0f;
else
return (float)toDegrees(Math.Atan2(y,x));
}
public void SetCenter(WayPoint wpt)
{
this.center = wpt;
this.Invalidate();
}
public void setWpt(WayPoint wpt)
{
setLatitude(wpt.Latitude);
setLongitude(wpt.Longitude);
}
void home_soft(WayPoint waypoint, MouseButtons btn)
{
if (btn == MouseButtons.Left)
{
if (jthread != null)
{
jthread.modem_lon = (float)waypoint.Longitude;
jthread.modem_lat = (float)waypoint.Latitude;
}
mapControl1.soft_wpt = waypoint;
mapControl1.listeners -= home_soft;
mapControl1.Invalidate();
}
}
public void tarea(Object obj)
{
Dictionary<string, WayPoint> team = (Dictionary<string, WayPoint>)obj;
Socket sock = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp);
sock.EnableBroadcast = true;
sock.ReceiveTimeout = 100; // 100ms
IPEndPoint iep = new IPEndPoint(IPAddress.Any, this.port);
sock.Bind(iep);
EndPoint ep = (EndPoint)iep;
Console.WriteLine("Ready to receive...");
byte[] data = new byte[1024];
while (running)
{
int recv;
try
{
recv = sock.ReceiveFrom(data, ref ep);
}
catch (Exception) { recv = 0; }
if (recv != 0) // paquete vacio
{
string stringData = Encoding.ASCII.GetString(data, 0, recv);
string[] values = stringData.Split(new char[1] { ';' });
if (values[0] == "IKARUS")
{
try
{
WayPoint wpt = new WayPoint();
if (values[1].CompareTo(me.NombrePiloto) != 0)
{
wpt.name = values[1];
wpt.Latitude = double.Parse(values[2], System.Globalization.CultureInfo.InvariantCulture);
wpt.Longitude = double.Parse(values[3], System.Globalization.CultureInfo.InvariantCulture);
wpt.Altitude = float.Parse(values[4], System.Globalization.CultureInfo.InvariantCulture);
wpt.heading = float.Parse(values[5], System.Globalization.CultureInfo.InvariantCulture);
if (me.planeState != null && wpt.Altitude < me.planeState.Alt)
wpt.icon = UAVConsole.Properties.Resources.plane1;
else
wpt.icon = UAVConsole.Properties.Resources.plane2;
wpt.icon.MakeTransparent(Color.White);
if (listeners != null)
listeners.Invoke(wpt);
}
}
catch (Exception) { }
}
}
}
sock.Close();
}
public FormIkarusMain()
{
InitializeComponent();
if (me.SistemaMetrico == (int)Singleton.SistemasMetricos.Metrico)
instrumento_Altimeter1.Calibration = me.HomeAlt;
else
instrumento_Altimeter1.Calibration = me.HomeAlt * 3.28f;
this.SetStyle(ControlStyles.AllPaintingInWmPaint | ControlStyles.UserPaint | ControlStyles.DoubleBuffer, true);
debug_values = new float[5, 5]{{me.uplink_pid_ele_P, me.uplink_pid_ele_I, me.uplink_pid_ele_D, me.uplink_pid_ele_IL, me.uplink_pid_ele_DL},
{me.uplink_pid_ail_P , me.uplink_pid_ail_I, me.uplink_pid_ail_D, me.uplink_pid_ail_IL, me.uplink_pid_ail_DL},
{me.uplink_pid_tail_P, me.uplink_pid_tail_I, me.uplink_pid_tail_D, me.uplink_pid_tail_IL, me.uplink_pid_tail_DL},
{me.uplink_pid_thr_P, me.uplink_pid_thr_I, me.uplink_pid_thr_D, me.uplink_pid_thr_IL, me.uplink_pid_thr_DL },
{me.uplink_IR_offX, me.uplink_IR_offY, me.uplink_IR_gain, me.uplink_rumbo_ail, me.uplink_altura_ele}};
comboBox3.SelectedIndex = 0;
comboBox2.SelectedIndex = 0;
comboBox1.SelectedIndex = 0;
this.planeWpt = new WayPoint("Avion", me.HomeLon, me.HomeLat, global::UAVConsole.Properties.Resources.plane3);
this.planeWpt.icon.MakeTransparent(Color.White);
this.planeWpt.heading = 0;
this.targetWpt = new WayPoint("Avion", me.HomeLon, me.HomeLat);
this.homeWpt = new WayPoint("Home", me.HomeLon, me.HomeLat);
mapControl1.plane = this.planeWpt;
mapControl1.target = this.targetWpt;
mapControl1.ruta = me.Ruta;
medidorBaterias1.num_cells = me.cells1;
medidorBaterias2.num_cells = me.cells2;
try
{
filters = new Filters();
}
catch (Exception)
{
}
if (filters != null && filters.VideoInputDevices.Count > 0)
{
foreach (Filter f in filters.VideoInputDevices)
{
if (f.Name.Equals(me.videoCaptureStr))
{
capture = new Capture(f, null, false);
break;
}
}
if (capture != null)
{
foreach (Source vs in capture.VideoSources)
{
if (vs.ToString().Contains("omposi")) // Video Composite
{
capture.VideoSource = vs;
break;
}
}
if (me.videosystem == Singleton.VideoSystem.PAL)
{
capture.dxUtils.VideoStandard = DShowNET.AnalogVideoStandard.PAL_B;
try
{
capture.FrameRate = 25;
capture.FrameSize = new Size(720, 576);
capture.PreviewFrameSize = new Size(720, 576);
}
catch (Exception) { }
}
else
{
capture.dxUtils.VideoStandard = DShowNET.AnalogVideoStandard.NTSC_M;
try
{
capture.FrameRate = 30;
capture.FrameSize = new Size(720, 480);
}
catch (Exception) { }
}
capture.AllowSampleGrabber = true;
capture.PreviewWindow = panel1;
capture.FrameEvent2 += new Capture.HeFrame(CaptureDone);
capture.GrapImg();
//capture.ShowPropertyPage(1, this);
}
}
if (me.enableAntTrack)
{
antTracker = new AntTracker();
medidorBaterias3.Enabled = true;
medidorBaterias3.AutoCalculate = false;
label6.Enabled = true;
knob_anttracker.Visible = true;
knob_anttracker.Manual = false;
medidorRSSI.Height = 97;
}
else
{
knob_anttracker.Visible = false;
medidorRSSI.Height = 137;
}
if (me.telemetria == Singleton.Telemetria.Video)
{
modem = new ModemVideo();
}
else if (me.telemetria == Singleton.Telemetria.XBEE)
modem = new ModemXbee(me.commPort, me.commBps);
else if (me.telemetria == Singleton.Telemetria.AntTracker)
modem = new ModemAntTracker(antTracker);
if (me.telemetria != Singleton.Telemetria.None)
modem.listeners += RefreshInstruments;
if (me.moduloTX == Singleton.ModuloControl.Uplink)
{
if (jthread == null)
jthread = new JoystickThread(this);
decimal altura = (decimal)(me.HomeAlt + 100);
if (altura < numericUpDown1.Minimum)
altura = numericUpDown1.Minimum;
else if (altura > numericUpDown1.Maximum)
altura = numericUpDown1.Maximum;
numericUpDown1.Value = altura;
label7.Enabled = true;
medidorBaterias4.Enabled = true;
medidorBaterias4.volts_max = me.uplinkVmax;
medidorBaterias4.volts_min = me.uplinkVmin;
}
else
{
splitContainer1.SplitterDistance+=panel4.Height;
panel5.Location = panel4.Location;
panel6.Location = new Point(panel6.Location.X, panel5.Location.Y);
panel4.Visible = false;
}
if (me.enableUDPinout)
{
if (this.sender == null)
this.sender = new SenderUDP(me.portUDPinout);
this.sender.listeners += TeamRefresh;
}
else
{
comboBoxTeam.Visible = false;
button6.Visible = false;
button7.Visible = false;
//panel8.Width -= 56;
}
if (me.enableWebServer)
{
this.KmlWebServer = new MyWebServer(me.portWebServer);
}
timer1.Enabled = true;
}
public void SetCenter(WayPoint wpt)
{
map.SetCenter(wpt);
}
