| 277,8 → 277,6 |
|---|
| DataTable dtGPX = new DataTable(); |
| DataRow drGPX; |
| bool _bAirborne = false; |
| int _wpIndex = -1, _wpCount = 0; |
| |
| DispatcherTimer timer = new DispatcherTimer(); |
| |
| /// <summary> |
| 704,7 → 702,7 |
|---|
| for (int k = 0; k < MainMap.Markers.Count;) |
| { |
| GMapMarker p = MainMap.Markers[k]; |
| if (p.GetType() == markerType | (p.Shape != null && p.Shape.GetType() == markerType)) |
| if (p.GetType() == markerType | p.Shape.GetType() == markerType) |
| MainMap.Markers.Remove(p); |
| else |
| k++; |
| 1694,28 → 1692,11 |
|---|
| Dispatcher.Invoke((Action)(() => tbHP.Text = ((double)iVal / (double)10).ToString("0.0 m"))); //Distance to HP set by GPS on |
| |
| Dispatcher.Invoke((Action)(() => tbWPIndex.Text = data[48].ToString())); //Waypoint index |
| _wpIndex = data[48]; |
| //if(wpList.Count > 0 && ((data[67] & 2) == 2)) |
| //{ |
| // Dispatcher.Invoke(() => |
| // { |
| // if(data[48] < wpList.Count) |
| // { |
| // DataGridRow row; |
| // if (data[48] - 1 > -1) |
| // { |
| // row = (DataGridRow)dgvWP.ItemContainerGenerator.ContainerFromIndex(data[48] - 1); |
| // row.Background = new SolidColorBrush(Colors.Transparent); |
| // } |
| // row = (DataGridRow)dgvWP.ItemContainerGenerator.ContainerFromIndex(data[48]); |
| // row.Background = new SolidColorBrush(Color.FromArgb(50, 0, 100, 255)); |
| // } |
| // }); |
| //} |
| Dispatcher.Invoke((Action)(() => tbWPCount.Text = data[49].ToString())); //Waypoints count |
| _wpCount = data[49]; |
| |
| Dispatcher.Invoke((Action)(() => tbTopSats.Text = data[50].ToString())); //Satellites |
| |
| |
| //--------------- Capacity used ------------------------ |
| i_16 = data[81]; |
| i_16 = (Int16)(i_16 << 8); |
| 2144,13 → 2125,8 |
|---|
| } |
| } |
| } |
| |
| void clearCopterWPList() |
| void _sendWPList() |
| { |
| _clearCopterWPList(); |
| } |
| bool _clearCopterWPList() |
| { |
| int iTimeout = 0; |
| if (serialPortCtrl.Port.IsOpen) |
| { |
| 2162,7 → 2138,7 |
|---|
| serialStream.Write(bytes, 0, bytes.Length); |
| |
| _iWPCount = -1; |
| while (_iWPCount == -1 & iTimeout < _iWPTimeout) |
| while (_iWPCount == -1 & iTimeout < _iWPTimeout * 5) |
| { |
| Thread.Sleep(10); |
| iTimeout++; |
| 2169,26 → 2145,71 |
|---|
| } |
| Dispatcher.Invoke(() => lblWPCount.Content = _iWPCount.ToString()); |
| if (_iWPCount > -1) |
| return true; |
| else |
| Log(LogMsgType.Error, "Timeout while sending list!"); |
| } |
| else |
| Log(LogMsgType.Error, "NOT CONNECTED!"); |
| { |
| |
| return false; |
| } |
| void _sendWPList() |
| { |
| int iTimeout = 0; |
| if (serialPortCtrl.Port.IsOpen) |
| { |
| if (_clearCopterWPList()) |
| { |
| int iVal; |
| double dVal; |
| NumberFormatInfo nfi = new NumberFormatInfo(); |
| nfi.NumberDecimalSeparator = ","; |
| for (int i = 0; i < dtWaypoints.Rows.Count; i++) |
| { |
| _sendWayPoint(dtWaypoints.Rows[i],-1); |
| //longitude |
| dVal = Convert.ToDouble(dtWaypoints.Rows[i][4], nfi); |
| iVal = (int)(dVal * Math.Pow(10, 7)); |
| bData[0] = (byte)(iVal & 0xff); |
| bData[1] = (byte)((iVal >> 8) & 0xff); |
| bData[2] = (byte)((iVal >> 16) & 0xff); |
| bData[3] = (byte)(iVal >> 24); |
| //latitude |
| dVal = Convert.ToDouble(dtWaypoints.Rows[i][3], nfi); |
| iVal = (int)(dVal * Math.Pow(10, 7)); |
| bData[4] = (byte)(iVal & 0xff); |
| bData[5] = (byte)((iVal >> 8) & 0xff); |
| bData[6] = (byte)((iVal >> 16) & 0xff); |
| bData[7] = (byte)(iVal >> 24); |
| //altitude |
| dVal = Convert.ToDouble(dtWaypoints.Rows[i][5], nfi); |
| iVal = (int)(dVal * 10); |
| bData[8] = (byte)(iVal & 0xff); |
| bData[9] = (byte)((iVal >> 8) & 0xff); |
| bData[10] = (byte)((iVal >> 16) & 0xff); |
| bData[11] = (byte)(iVal >> 24); |
| //Status 'NEWDATA' |
| bData[12] = 1; |
| //heading |
| iVal = Convert.ToInt16(dtWaypoints.Rows[i][6]); |
| bData[13] = (byte)(iVal & 0xff); |
| bData[14] = (byte)((iVal >> 8) & 0xff); |
| //ToleranceRadius |
| bData[15] = Convert.ToByte(dtWaypoints.Rows[i][9]); |
| //HoldTime |
| bData[16] = Convert.ToByte(dtWaypoints.Rows[i][10]); |
| //Event_Flag |
| bData[17] = Convert.ToByte(dtWaypoints.Rows[i][13]); |
| //Index |
| bData[18] = Convert.ToByte((int)dtWaypoints.Rows[i][0]); |
| //Type |
| bData[19] = Convert.ToByte(dtWaypoints.Rows[i][1]); |
| //WP_EventChannelValue |
| bData[20] = Convert.ToByte(dtWaypoints.Rows[i][14]); |
| //AltitudeRate |
| bData[21] = Convert.ToByte(dtWaypoints.Rows[i][8]); |
| //Speed |
| bData[22] = Convert.ToByte(dtWaypoints.Rows[i][7]); |
| //CamAngle |
| bData[23] = (byte)Convert.ToInt16(dtWaypoints.Rows[i][12]); |
| //Name |
| byte[] name = ASCIIEncoding.ASCII.GetBytes((string)dtWaypoints.Rows[i][2]); |
| bData[24] = name.Length > 0 ? name[0] : (byte)0; |
| bData[25] = name.Length > 1 ? name[1] : (byte)0; |
| bData[26] = name.Length > 2 ? name[2] : (byte)0; |
| bData[27] = name.Length > 3 ? name[3] : (byte)0; |
| //Autotrigger |
| bData[28] = Convert.ToByte(dtWaypoints.Rows[i][11]); |
| |
| bytes = FlightControllerMessage.CreateMessage('w', 2, bData); |
| serialStream.Write(bytes, 0, bytes.Length); |
| |
| _iWPCount = -1; |
| iTimeout = 0; |
| while (_iWPCount == -1 & iTimeout < _iWPTimeout * 5) |
| 2204,88 → 2225,14 |
|---|
| Dispatcher.Invoke(() => lblWPCount.Content = _iWPCount.ToString()); |
| } |
| } |
| else |
| Log(LogMsgType.Error, "Timeout while sending list!"); |
| } |
| else |
| Log(LogMsgType.Error, "NOT CONNECTED!"); |
| |
| } |
| bool _sendWayPoint(DataRow dr, int index) |
| { |
| |
| if (serialPortCtrl.Port.IsOpen) |
| { |
| byte[] bData = new byte[30]; |
| for (int i = 0; i < 30; i++) |
| bData[i] = 0; |
| Stream serialStream = serialPortCtrl.Port.BaseStream; |
| byte[] bytes; |
| int iVal; |
| double dVal; |
| NumberFormatInfo nfi = new NumberFormatInfo(); |
| nfi.NumberDecimalSeparator = ","; |
| |
| //longitude |
| dVal = Convert.ToDouble(dr[4], nfi); |
| iVal = (int)(dVal * Math.Pow(10, 7)); |
| bData[0] = (byte)(iVal & 0xff); |
| bData[1] = (byte)((iVal >> 8) & 0xff); |
| bData[2] = (byte)((iVal >> 16) & 0xff); |
| bData[3] = (byte)(iVal >> 24); |
| //latitude |
| dVal = Convert.ToDouble(dr[3], nfi); |
| iVal = (int)(dVal * Math.Pow(10, 7)); |
| bData[4] = (byte)(iVal & 0xff); |
| bData[5] = (byte)((iVal >> 8) & 0xff); |
| bData[6] = (byte)((iVal >> 16) & 0xff); |
| bData[7] = (byte)(iVal >> 24); |
| //altitude |
| dVal = Convert.ToDouble(dr[5], nfi); |
| iVal = (int)(dVal * 10); |
| bData[8] = (byte)(iVal & 0xff); |
| bData[9] = (byte)((iVal >> 8) & 0xff); |
| bData[10] = (byte)((iVal >> 16) & 0xff); |
| bData[11] = (byte)(iVal >> 24); |
| //Status 'NEWDATA' |
| bData[12] = 1; |
| //heading |
| iVal = Convert.ToInt16(dr[6]); |
| bData[13] = (byte)(iVal & 0xff); |
| bData[14] = (byte)((iVal >> 8) & 0xff); |
| //ToleranceRadius |
| bData[15] = Convert.ToByte(dr[9]); |
| //HoldTime |
| bData[16] = Convert.ToByte(dr[10]); |
| //Event_Flag |
| bData[17] = Convert.ToByte(dr[13]); |
| //Index |
| bData[18] = index > 0 ? (byte)index : Convert.ToByte((int)dr[0]); |
| //Type |
| bData[19] = Convert.ToByte(dr[1]); |
| //WP_EventChannelValue |
| bData[20] = Convert.ToByte(dr[14]); |
| //AltitudeRate |
| bData[21] = Convert.ToByte(dr[8]); |
| //Speed |
| bData[22] = Convert.ToByte(dr[7]); |
| //CamAngle |
| bData[23] = (byte)Convert.ToInt16(dr[12]); |
| //Name |
| byte[] name = ASCIIEncoding.ASCII.GetBytes((string)dr[2]); |
| bData[24] = name.Length > 0 ? name[0] : (byte)0; |
| bData[25] = name.Length > 1 ? name[1] : (byte)0; |
| bData[26] = name.Length > 2 ? name[2] : (byte)0; |
| bData[27] = name.Length > 3 ? name[3] : (byte)0; |
| //Autotrigger |
| bData[28] = Convert.ToByte(dr[11]); |
| |
| bytes = FlightControllerMessage.CreateMessage('w', 2, bData); |
| serialStream.Write(bytes, 0, bytes.Length); |
| |
| return true; |
| } |
| |
| return false; |
| } |
| #region OSD-Menue |
| |
| /// <summary> |
| 3077,22 → 3024,6 |
|---|
| Dispatcher.Invoke(() => lblWPCount.Content = 0); |
| Dispatcher.Invoke(() => lblWPRouteDistance.Content = "0 m"); |
| } |
| private void btnClearCopterList_Click(object sender, RoutedEventArgs e) |
| { |
| Thread t = new Thread(new ThreadStart(clearCopterWPList)); |
| t.Start(); |
| } |
| private void dgvWP_MouseUp(object sender, MouseButtonEventArgs e) |
| { |
| if (dgvWP.SelectedIndex > -1 && dgvWP.SelectedIndex < dtWaypoints.Rows.Count) |
| { |
| if(_wpCount > 0) |
| _sendWayPoint(dtWaypoints.Rows[dgvWP.SelectedIndex],-1); |
| else |
| _sendWayPoint(dtWaypoints.Rows[dgvWP.SelectedIndex],1); |
| } |
| } |
| |
| #endregion WP |
| |
| #region GPX |
| 3174,7 → 3105,6 |
|---|
| _clearMapMarkers(typeof(GMapRoute)); |
| } |
| |
| |
| void _loadGPXLog() |
| { |
| |