Rev 2335 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2335 | Rev 2340 | ||
---|---|---|---|
Line 275... | Line 275... | ||
275 | bool _bShowWPRoute = false; |
275 | bool _bShowWPRoute = false; |
276 | static volatile bool _bGetWPCount = false; |
276 | static volatile bool _bGetWPCount = false; |
277 | DataTable dtGPX = new DataTable(); |
277 | DataTable dtGPX = new DataTable(); |
278 | DataRow drGPX; |
278 | DataRow drGPX; |
279 | bool _bAirborne = false; |
279 | bool _bAirborne = false; |
- | 280 | int _wpIndex = -1, _wpCount = 0; |
|
- | 281 | ||
280 | DispatcherTimer timer = new DispatcherTimer(); |
282 | DispatcherTimer timer = new DispatcherTimer(); |
Line 281... | Line 283... | ||
281 | 283 | ||
282 | /// <summary> |
284 | /// <summary> |
283 | /// stuff for enabeling touch zoom for the map |
285 | /// stuff for enabeling touch zoom for the map |
Line 700... | Line 702... | ||
700 | void _clearMapMarkers(Type markerType) |
702 | void _clearMapMarkers(Type markerType) |
701 | { |
703 | { |
702 | for (int k = 0; k < MainMap.Markers.Count;) |
704 | for (int k = 0; k < MainMap.Markers.Count;) |
703 | { |
705 | { |
704 | GMapMarker p = MainMap.Markers[k]; |
706 | GMapMarker p = MainMap.Markers[k]; |
705 | if (p.GetType() == markerType | p.Shape.GetType() == markerType) |
707 | if (p.GetType() == markerType | (p.Shape != null && p.Shape.GetType() == markerType)) |
706 | MainMap.Markers.Remove(p); |
708 | MainMap.Markers.Remove(p); |
707 | else |
709 | else |
708 | k++; |
710 | k++; |
709 | } |
711 | } |
Line 1690... | Line 1692... | ||
1690 | iVal = data[44] + i_16; |
1692 | iVal = data[44] + i_16; |
1691 | // Dispatcher.Invoke((Action)(() => tbTopDistanceHP.Text = ((double)iVal / (double)10).ToString("0.0 m"))); //Distance to HP set by GPS on |
1693 | // Dispatcher.Invoke((Action)(() => tbTopDistanceHP.Text = ((double)iVal / (double)10).ToString("0.0 m"))); //Distance to HP set by GPS on |
1692 | Dispatcher.Invoke((Action)(() => tbHP.Text = ((double)iVal / (double)10).ToString("0.0 m"))); //Distance to HP set by GPS on |
1694 | Dispatcher.Invoke((Action)(() => tbHP.Text = ((double)iVal / (double)10).ToString("0.0 m"))); //Distance to HP set by GPS on |
Line 1693... | Line 1695... | ||
1693 | 1695 | ||
- | 1696 | Dispatcher.Invoke((Action)(() => tbWPIndex.Text = data[48].ToString())); //Waypoint index |
|
- | 1697 | _wpIndex = data[48]; |
|
- | 1698 | //if(wpList.Count > 0 && ((data[67] & 2) == 2)) |
|
- | 1699 | //{ |
|
- | 1700 | // Dispatcher.Invoke(() => |
|
- | 1701 | // { |
|
- | 1702 | // if(data[48] < wpList.Count) |
|
- | 1703 | // { |
|
- | 1704 | // DataGridRow row; |
|
- | 1705 | // if (data[48] - 1 > -1) |
|
- | 1706 | // { |
|
- | 1707 | // row = (DataGridRow)dgvWP.ItemContainerGenerator.ContainerFromIndex(data[48] - 1); |
|
- | 1708 | // row.Background = new SolidColorBrush(Colors.Transparent); |
|
- | 1709 | // } |
|
- | 1710 | // row = (DataGridRow)dgvWP.ItemContainerGenerator.ContainerFromIndex(data[48]); |
|
- | 1711 | // row.Background = new SolidColorBrush(Color.FromArgb(50, 0, 100, 255)); |
|
- | 1712 | // } |
|
- | 1713 | // }); |
|
1694 | Dispatcher.Invoke((Action)(() => tbWPIndex.Text = data[48].ToString())); //Waypoint index |
1714 | //} |
1695 | Dispatcher.Invoke((Action)(() => tbWPCount.Text = data[49].ToString())); //Waypoints count |
- | |
- | 1715 | Dispatcher.Invoke((Action)(() => tbWPCount.Text = data[49].ToString())); //Waypoints count |
|
1696 | 1716 | _wpCount = data[49]; |
|
Line 1697... | Line -... | ||
1697 | Dispatcher.Invoke((Action)(() => tbTopSats.Text = data[50].ToString())); //Satellites |
- | |
1698 | 1717 | Dispatcher.Invoke((Action)(() => tbTopSats.Text = data[50].ToString())); //Satellites |
|
1699 | 1718 | ||
1700 | //--------------- Capacity used ------------------------ |
1719 | //--------------- Capacity used ------------------------ |
1701 | i_16 = data[81]; |
1720 | i_16 = data[81]; |
1702 | i_16 = (Int16)(i_16 << 8); |
1721 | i_16 = (Int16)(i_16 << 8); |
Line 2123... | Line 2142... | ||
2123 | Thread.Sleep(1); |
2142 | Thread.Sleep(1); |
2124 | iTimeout++; |
2143 | iTimeout++; |
2125 | } |
2144 | } |
2126 | } |
2145 | } |
2127 | } |
2146 | } |
- | 2147 | ||
2128 | void _sendWPList() |
2148 | void clearCopterWPList() |
- | 2149 | { |
|
- | 2150 | _clearCopterWPList(); |
|
- | 2151 | } |
|
- | 2152 | bool _clearCopterWPList() |
|
2129 | { |
2153 | { |
2130 | int iTimeout = 0; |
2154 | int iTimeout = 0; |
2131 | if (serialPortCtrl.Port.IsOpen) |
2155 | if (serialPortCtrl.Port.IsOpen) |
2132 | { |
2156 | { |
2133 | byte[] bData = new byte[30]; |
2157 | byte[] bData = new byte[30]; |
Line 2136... | Line 2160... | ||
2136 | Stream serialStream = serialPortCtrl.Port.BaseStream; |
2160 | Stream serialStream = serialPortCtrl.Port.BaseStream; |
2137 | byte[] bytes = FlightControllerMessage.CreateMessage('w', 2, bData); //delete all WP on Copter by sending 'invalid'(==0) at index 0 |
2161 | byte[] bytes = FlightControllerMessage.CreateMessage('w', 2, bData); //delete all WP on Copter by sending 'invalid'(==0) at index 0 |
2138 | serialStream.Write(bytes, 0, bytes.Length); |
2162 | serialStream.Write(bytes, 0, bytes.Length); |
Line 2139... | Line 2163... | ||
2139 | 2163 | ||
2140 | _iWPCount = -1; |
2164 | _iWPCount = -1; |
2141 | while (_iWPCount == -1 & iTimeout < _iWPTimeout * 5) |
2165 | while (_iWPCount == -1 & iTimeout < _iWPTimeout) |
2142 | { |
2166 | { |
2143 | Thread.Sleep(10); |
2167 | Thread.Sleep(10); |
2144 | iTimeout++; |
2168 | iTimeout++; |
2145 | } |
2169 | } |
2146 | Dispatcher.Invoke(() => lblWPCount.Content = _iWPCount.ToString()); |
2170 | Dispatcher.Invoke(() => lblWPCount.Content = _iWPCount.ToString()); |
- | 2171 | if (_iWPCount > -1) |
|
2147 | if (_iWPCount > -1) |
2172 | return true; |
- | 2173 | else |
|
- | 2174 | Log(LogMsgType.Error, "Timeout while sending list!"); |
|
- | 2175 | } |
|
- | 2176 | else |
|
Line 2148... | Line 2177... | ||
2148 | { |
2177 | Log(LogMsgType.Error, "NOT CONNECTED!"); |
- | 2178 | ||
- | 2179 | return false; |
|
- | 2180 | } |
|
2149 | 2181 | void _sendWPList() |
|
2150 | int iVal; |
2182 | { |
- | 2183 | int iTimeout = 0; |
|
2151 | double dVal; |
2184 | if (serialPortCtrl.Port.IsOpen) |
- | 2185 | { |
|
2152 | NumberFormatInfo nfi = new NumberFormatInfo(); |
2186 | if (_clearCopterWPList()) |
2153 | nfi.NumberDecimalSeparator = ","; |
2187 | { |
2154 | for (int i = 0; i < dtWaypoints.Rows.Count; i++) |
- | |
2155 | { |
- | |
2156 | //longitude |
- | |
2157 | dVal = Convert.ToDouble(dtWaypoints.Rows[i][4], nfi); |
- | |
2158 | iVal = (int)(dVal * Math.Pow(10, 7)); |
- | |
2159 | bData[0] = (byte)(iVal & 0xff); |
- | |
2160 | bData[1] = (byte)((iVal >> 8) & 0xff); |
- | |
2161 | bData[2] = (byte)((iVal >> 16) & 0xff); |
- | |
2162 | bData[3] = (byte)(iVal >> 24); |
- | |
2163 | //latitude |
- | |
2164 | dVal = Convert.ToDouble(dtWaypoints.Rows[i][3], nfi); |
- | |
2165 | iVal = (int)(dVal * Math.Pow(10, 7)); |
- | |
2166 | bData[4] = (byte)(iVal & 0xff); |
- | |
2167 | bData[5] = (byte)((iVal >> 8) & 0xff); |
- | |
2168 | bData[6] = (byte)((iVal >> 16) & 0xff); |
- | |
2169 | bData[7] = (byte)(iVal >> 24); |
- | |
2170 | //altitude |
- | |
2171 | dVal = Convert.ToDouble(dtWaypoints.Rows[i][5], nfi); |
- | |
2172 | iVal = (int)(dVal * 10); |
- | |
2173 | bData[8] = (byte)(iVal & 0xff); |
- | |
2174 | bData[9] = (byte)((iVal >> 8) & 0xff); |
- | |
2175 | bData[10] = (byte)((iVal >> 16) & 0xff); |
- | |
2176 | bData[11] = (byte)(iVal >> 24); |
- | |
2177 | //Status 'NEWDATA' |
- | |
2178 | bData[12] = 1; |
2188 | for (int i = 0; i < dtWaypoints.Rows.Count; i++) |
2179 | //heading |
- | |
2180 | iVal = Convert.ToInt16(dtWaypoints.Rows[i][6]); |
- | |
2181 | bData[13] = (byte)(iVal & 0xff); |
- | |
2182 | bData[14] = (byte)((iVal >> 8) & 0xff); |
- | |
2183 | //ToleranceRadius |
- | |
2184 | bData[15] = Convert.ToByte(dtWaypoints.Rows[i][9]); |
- | |
2185 | //HoldTime |
- | |
2186 | bData[16] = Convert.ToByte(dtWaypoints.Rows[i][10]); |
- | |
2187 | //Event_Flag |
- | |
2188 | bData[17] = Convert.ToByte(dtWaypoints.Rows[i][13]); |
- | |
2189 | //Index |
- | |
2190 | bData[18] = Convert.ToByte((int)dtWaypoints.Rows[i][0]); |
- | |
2191 | //Type |
- | |
2192 | bData[19] = Convert.ToByte(dtWaypoints.Rows[i][1]); |
- | |
2193 | //WP_EventChannelValue |
- | |
2194 | bData[20] = Convert.ToByte(dtWaypoints.Rows[i][14]); |
- | |
2195 | //AltitudeRate |
- | |
2196 | bData[21] = Convert.ToByte(dtWaypoints.Rows[i][8]); |
- | |
2197 | //Speed |
- | |
2198 | bData[22] = Convert.ToByte(dtWaypoints.Rows[i][7]); |
- | |
2199 | //CamAngle |
- | |
2200 | bData[23] = (byte)Convert.ToInt16(dtWaypoints.Rows[i][12]); |
- | |
2201 | //Name |
- | |
2202 | byte[] name = ASCIIEncoding.ASCII.GetBytes((string)dtWaypoints.Rows[i][2]); |
- | |
2203 | bData[24] = name.Length > 0 ? name[0] : (byte)0; |
- | |
2204 | bData[25] = name.Length > 1 ? name[1] : (byte)0; |
- | |
2205 | bData[26] = name.Length > 2 ? name[2] : (byte)0; |
- | |
2206 | bData[27] = name.Length > 3 ? name[3] : (byte)0; |
- | |
2207 | //Autotrigger |
- | |
2208 | bData[28] = Convert.ToByte(dtWaypoints.Rows[i][11]); |
- | |
2209 | - | ||
Line 2210... | Line 2189... | ||
2210 | bytes = FlightControllerMessage.CreateMessage('w', 2, bData); |
2189 | { |
2211 | serialStream.Write(bytes, 0, bytes.Length); |
2190 | _sendWayPoint(dtWaypoints.Rows[i],-1); |
2212 | 2191 | ||
2213 | _iWPCount = -1; |
2192 | _iWPCount = -1; |
Line 2223... | Line 2202... | ||
2223 | break; |
2202 | break; |
2224 | } |
2203 | } |
2225 | Dispatcher.Invoke(() => lblWPCount.Content = _iWPCount.ToString()); |
2204 | Dispatcher.Invoke(() => lblWPCount.Content = _iWPCount.ToString()); |
2226 | } |
2205 | } |
2227 | } |
2206 | } |
2228 | else |
- | |
2229 | Log(LogMsgType.Error, "Timeout while sending list!"); |
- | |
2230 | } |
2207 | } |
2231 | else |
2208 | else |
2232 | Log(LogMsgType.Error, "NOT CONNECTED!"); |
2209 | Log(LogMsgType.Error, "NOT CONNECTED!"); |
Line 2233... | Line 2210... | ||
2233 | 2210 | ||
- | 2211 | } |
|
- | 2212 | bool _sendWayPoint(DataRow dr, int index) |
|
- | 2213 | { |
|
- | 2214 | ||
- | 2215 | if (serialPortCtrl.Port.IsOpen) |
|
- | 2216 | { |
|
- | 2217 | byte[] bData = new byte[30]; |
|
- | 2218 | for (int i = 0; i < 30; i++) |
|
- | 2219 | bData[i] = 0; |
|
- | 2220 | Stream serialStream = serialPortCtrl.Port.BaseStream; |
|
- | 2221 | byte[] bytes; |
|
- | 2222 | int iVal; |
|
- | 2223 | double dVal; |
|
- | 2224 | NumberFormatInfo nfi = new NumberFormatInfo(); |
|
- | 2225 | nfi.NumberDecimalSeparator = ","; |
|
- | 2226 | ||
- | 2227 | //longitude |
|
- | 2228 | dVal = Convert.ToDouble(dr[4], nfi); |
|
- | 2229 | iVal = (int)(dVal * Math.Pow(10, 7)); |
|
- | 2230 | bData[0] = (byte)(iVal & 0xff); |
|
- | 2231 | bData[1] = (byte)((iVal >> 8) & 0xff); |
|
- | 2232 | bData[2] = (byte)((iVal >> 16) & 0xff); |
|
- | 2233 | bData[3] = (byte)(iVal >> 24); |
|
- | 2234 | //latitude |
|
- | 2235 | dVal = Convert.ToDouble(dr[3], nfi); |
|
- | 2236 | iVal = (int)(dVal * Math.Pow(10, 7)); |
|
- | 2237 | bData[4] = (byte)(iVal & 0xff); |
|
- | 2238 | bData[5] = (byte)((iVal >> 8) & 0xff); |
|
- | 2239 | bData[6] = (byte)((iVal >> 16) & 0xff); |
|
- | 2240 | bData[7] = (byte)(iVal >> 24); |
|
- | 2241 | //altitude |
|
- | 2242 | dVal = Convert.ToDouble(dr[5], nfi); |
|
- | 2243 | iVal = (int)(dVal * 10); |
|
- | 2244 | bData[8] = (byte)(iVal & 0xff); |
|
- | 2245 | bData[9] = (byte)((iVal >> 8) & 0xff); |
|
- | 2246 | bData[10] = (byte)((iVal >> 16) & 0xff); |
|
- | 2247 | bData[11] = (byte)(iVal >> 24); |
|
- | 2248 | //Status 'NEWDATA' |
|
- | 2249 | bData[12] = 1; |
|
- | 2250 | //heading |
|
- | 2251 | iVal = Convert.ToInt16(dr[6]); |
|
- | 2252 | bData[13] = (byte)(iVal & 0xff); |
|
- | 2253 | bData[14] = (byte)((iVal >> 8) & 0xff); |
|
- | 2254 | //ToleranceRadius |
|
- | 2255 | bData[15] = Convert.ToByte(dr[9]); |
|
- | 2256 | //HoldTime |
|
- | 2257 | bData[16] = Convert.ToByte(dr[10]); |
|
- | 2258 | //Event_Flag |
|
- | 2259 | bData[17] = Convert.ToByte(dr[13]); |
|
- | 2260 | //Index |
|
- | 2261 | bData[18] = index > 0 ? (byte)index : Convert.ToByte((int)dr[0]); |
|
- | 2262 | //Type |
|
- | 2263 | bData[19] = Convert.ToByte(dr[1]); |
|
- | 2264 | //WP_EventChannelValue |
|
- | 2265 | bData[20] = Convert.ToByte(dr[14]); |
|
- | 2266 | //AltitudeRate |
|
- | 2267 | bData[21] = Convert.ToByte(dr[8]); |
|
- | 2268 | //Speed |
|
- | 2269 | bData[22] = Convert.ToByte(dr[7]); |
|
- | 2270 | //CamAngle |
|
- | 2271 | bData[23] = (byte)Convert.ToInt16(dr[12]); |
|
- | 2272 | //Name |
|
- | 2273 | byte[] name = ASCIIEncoding.ASCII.GetBytes((string)dr[2]); |
|
- | 2274 | bData[24] = name.Length > 0 ? name[0] : (byte)0; |
|
- | 2275 | bData[25] = name.Length > 1 ? name[1] : (byte)0; |
|
- | 2276 | bData[26] = name.Length > 2 ? name[2] : (byte)0; |
|
- | 2277 | bData[27] = name.Length > 3 ? name[3] : (byte)0; |
|
- | 2278 | //Autotrigger |
|
- | 2279 | bData[28] = Convert.ToByte(dr[11]); |
|
- | 2280 | ||
- | 2281 | bytes = FlightControllerMessage.CreateMessage('w', 2, bData); |
|
- | 2282 | serialStream.Write(bytes, 0, bytes.Length); |
|
- | 2283 | ||
- | 2284 | return true; |
|
Line -... | Line 2285... | ||
- | 2285 | } |
|
- | 2286 | ||
2234 | } |
2287 | return false; |
Line 2235... | Line 2288... | ||
2235 | 2288 | } |
|
2236 | #region OSD-Menue |
2289 | #region OSD-Menue |
2237 | 2290 | ||
Line 3022... | Line 3075... | ||
3022 | Dispatcher.Invoke(() => dgvWP.Items.Refresh()); |
3075 | Dispatcher.Invoke(() => dgvWP.Items.Refresh()); |
3023 | Dispatcher.Invoke(() => lblWPIndex.Content = 0); |
3076 | Dispatcher.Invoke(() => lblWPIndex.Content = 0); |
3024 | Dispatcher.Invoke(() => lblWPCount.Content = 0); |
3077 | Dispatcher.Invoke(() => lblWPCount.Content = 0); |
3025 | Dispatcher.Invoke(() => lblWPRouteDistance.Content = "0 m"); |
3078 | Dispatcher.Invoke(() => lblWPRouteDistance.Content = "0 m"); |
3026 | } |
3079 | } |
- | 3080 | private void btnClearCopterList_Click(object sender, RoutedEventArgs e) |
|
- | 3081 | { |
|
- | 3082 | Thread t = new Thread(new ThreadStart(clearCopterWPList)); |
|
- | 3083 | t.Start(); |
|
- | 3084 | } |
|
- | 3085 | private void dgvWP_MouseUp(object sender, MouseButtonEventArgs e) |
|
- | 3086 | { |
|
- | 3087 | if (dgvWP.SelectedIndex > -1 && dgvWP.SelectedIndex < dtWaypoints.Rows.Count) |
|
- | 3088 | { |
|
- | 3089 | if(_wpCount > 0) |
|
- | 3090 | _sendWayPoint(dtWaypoints.Rows[dgvWP.SelectedIndex],-1); |
|
- | 3091 | else |
|
- | 3092 | _sendWayPoint(dtWaypoints.Rows[dgvWP.SelectedIndex],1); |
|
- | 3093 | } |
|
- | 3094 | } |
|
- | 3095 | ||
3027 | #endregion WP |
3096 | #endregion WP |
Line 3028... | Line 3097... | ||
3028 | 3097 | ||
3029 | #region GPX |
3098 | #region GPX |
3030 | private void checkBoxGPXLog_Click(object sender, RoutedEventArgs e) |
3099 | private void checkBoxGPXLog_Click(object sender, RoutedEventArgs e) |
Line 3103... | Line 3172... | ||
3103 | private void btnClearRoute_Click(object sender, RoutedEventArgs e) |
3172 | private void btnClearRoute_Click(object sender, RoutedEventArgs e) |
3104 | { |
3173 | { |
3105 | _clearMapMarkers(typeof(GMapRoute)); |
3174 | _clearMapMarkers(typeof(GMapRoute)); |
3106 | } |
3175 | } |
Line -... | Line 3176... | ||
- | 3176 | ||
3107 | 3177 | ||
3108 | void _loadGPXLog() |
3178 | void _loadGPXLog() |
Line 3109... | Line 3179... | ||
3109 | { |
3179 | { |
3110 | 3180 |