277,6 → 277,8 |
DataTable dtGPX = new DataTable(); |
DataRow drGPX; |
bool _bAirborne = false; |
int _wpIndex = -1, _wpCount = 0; |
|
DispatcherTimer timer = new DispatcherTimer(); |
|
/// <summary> |
702,7 → 704,7 |
for (int k = 0; k < MainMap.Markers.Count;) |
{ |
GMapMarker p = MainMap.Markers[k]; |
if (p.GetType() == markerType | p.Shape.GetType() == markerType) |
if (p.GetType() == markerType | (p.Shape != null && p.Shape.GetType() == markerType)) |
MainMap.Markers.Remove(p); |
else |
k++; |
1692,11 → 1694,28 |
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); |
2125,8 → 2144,13 |
} |
} |
} |
void _sendWPList() |
|
void clearCopterWPList() |
{ |
_clearCopterWPList(); |
} |
bool _clearCopterWPList() |
{ |
int iTimeout = 0; |
if (serialPortCtrl.Port.IsOpen) |
{ |
2138,7 → 2162,7 |
serialStream.Write(bytes, 0, bytes.Length); |
|
_iWPCount = -1; |
while (_iWPCount == -1 & iTimeout < _iWPTimeout * 5) |
while (_iWPCount == -1 & iTimeout < _iWPTimeout) |
{ |
Thread.Sleep(10); |
iTimeout++; |
2145,71 → 2169,26 |
} |
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++) |
{ |
//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]); |
_sendWayPoint(dtWaypoints.Rows[i],-1); |
|
bytes = FlightControllerMessage.CreateMessage('w', 2, bData); |
serialStream.Write(bytes, 0, bytes.Length); |
|
_iWPCount = -1; |
iTimeout = 0; |
while (_iWPCount == -1 & iTimeout < _iWPTimeout * 5) |
2225,14 → 2204,88 |
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> |
3024,6 → 3077,22 |
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 |
3105,6 → 3174,7 |
_clearMapMarkers(typeof(GMapRoute)); |
} |
|
|
void _loadGPXLog() |
{ |
|