Rev 577 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 577 | Rev 601 | ||
|---|---|---|---|
| Line 65... | Line 65... | ||
| 65 | #include "spi_slave.h" |
65 | #include "spi_slave.h" |
| Line 66... | Line 66... | ||
| 66 | 66 | ||
| Line 67... | Line -... | ||
| 67 | - | ||
| 68 | WPL_Store_t WPL_Store; |
- | |
| 69 | - | ||
| 70 | // the waypoints list |
67 | |
| 71 | #define MAX_LIST_LEN 101 |
68 | WPL_Store_t WPL_Store; |
| 72 | 69 | ||
| 73 | Point_t PointList[MAX_LIST_LEN]; |
70 | Point_t PointList[MAX_LIST_LEN]; |
| 74 | u8 WPIndex = 0; // list index of GPS point representig the current WP, can be maximal WPCount |
71 | u8 WPIndex = 0; // list index of GPS point representig the current WP, can be maximal WPCount |
| 75 | u8 POIIndex = 0; // list index of GPS Point representing the current POI, can be maximal WPCount |
72 | u8 POIIndex = 0; // list index of GPS Point representing the current POI, can be maximal WPCount |
| - | 73 | u8 WPCount = 0; // number of waypoints |
|
| 76 | u8 WPCount = 0; // number of waypoints |
74 | u8 PointCount = 0; // number of points in the list can be maximal equal to MAX_LIST_LEN |
| 77 | u8 PointCount = 0; // number of points in the list can be maximal equal to MAX_LIST_LEN |
75 | u8 POICount = 0; // number of point of interest in the list |
| Line 78... | Line 76... | ||
| 78 | u8 POICount = 0; // number of point of interest in the list |
76 | u8 FsPointCnt = 0; |
| Line 90... | Line 88... | ||
| 90 | { |
88 | { |
| 91 | u8 i; |
89 | u8 i; |
| 92 | WPIndex = 0; // real list position are 1 ,2, 3 ... |
90 | WPIndex = 0; // real list position are 1 ,2, 3 ... |
| 93 | POIIndex = 0; // real list position are 1 ,2, 3 ... |
91 | POIIndex = 0; // real list position are 1 ,2, 3 ... |
| 94 | WPCount = 0; // no waypoints |
92 | WPCount = 0; // no waypoints |
| - | 93 | FsPointCnt = 0; |
|
| 95 | POICount = 0; |
94 | POICount = 0; |
| 96 | PointCount = 0; // no contents |
95 | PointCount = 0; // no contents |
| 97 | WPActive = FALSE; |
96 | WPActive = FALSE; |
| 98 | NaviData.WaypointNumber = WPCount; |
97 | NaviData.WaypointNumber = WPCount; |
| 99 | NaviData.WaypointIndex = 0; |
98 | NaviData.WaypointIndex = 0; |
| Line 153... | Line 152... | ||
| 153 | 152 | ||
| 154 | case POINT_TYPE_POI: |
153 | case POINT_TYPE_POI: |
| 155 | POICount++; |
154 | POICount++; |
| 156 | PointCount++; |
155 | PointCount++; |
| - | 156 | break; |
|
| - | 157 | case POINT_TYPE_FS: |
|
| - | 158 | FsPointCnt++; |
|
| - | 159 | PointCount++; |
|
| 157 | break; |
160 | break; |
| 158 | } |
161 | } |
| Line 159... | Line 162... | ||
| 159 | break; |
162 | break; |
| 160 | 163 | ||
| Line 168... | Line 171... | ||
| 168 | 171 | ||
| 169 | default: |
172 | default: |
| 170 | case POINT_TYPE_WP: |
173 | case POINT_TYPE_WP: |
| 171 | //nothing to do |
174 | //nothing to do |
| - | 175 | break; |
|
| - | 176 | case POINT_TYPE_FS: |
|
| - | 177 | FsPointCnt++; |
|
| 172 | break; |
178 | WPCount--; |
| 173 | 179 | break; |
|
| 174 | case POINT_TYPE_POI: |
180 | case POINT_TYPE_POI: |
| 175 | POICount++; |
181 | POICount++; |
| 176 | WPCount--; |
182 | WPCount--; |
| 177 | break; |
183 | break; |
| Line 188... | Line 194... | ||
| 188 | 194 | ||
| 189 | case POINT_TYPE_WP: |
195 | case POINT_TYPE_WP: |
| 190 | WPCount++; |
196 | WPCount++; |
| 191 | POICount--; |
197 | POICount--; |
| - | 198 | break; |
|
| - | 199 | ||
| - | 200 | case POINT_TYPE_FS: |
|
| - | 201 | FsPointCnt++; |
|
| - | 202 | POICount--; |
|
| - | 203 | break; |
|
| - | 204 | ||
| - | 205 | case POINT_TYPE_POI: |
|
| - | 206 | default: |
|
| - | 207 | // nothing to do |
|
| - | 208 | break; |
|
| - | 209 | } |
|
| - | 210 | case POINT_TYPE_FS: // was a Failsafe |
|
| - | 211 | switch(pPoint->Type) |
|
| - | 212 | { |
|
| - | 213 | case POINT_TYPE_INVALID: |
|
| - | 214 | FsPointCnt--; |
|
| - | 215 | PointCount--; |
|
| - | 216 | break; |
|
| - | 217 | ||
| - | 218 | case POINT_TYPE_WP: |
|
| - | 219 | WPCount++; |
|
| - | 220 | FsPointCnt--; |
|
| Line 192... | Line 221... | ||
| 192 | break; |
221 | break; |
| - | 222 | ||
| - | 223 | case POINT_TYPE_POI: |
|
| - | 224 | POICount++; |
|
| - | 225 | FsPointCnt--; |
|
| - | 226 | break; |
|
| - | 227 | ||
| 193 | 228 | case POINT_TYPE_FS: |
|
| 194 | case POINT_TYPE_POI: |
229 | break; |
| 195 | default: |
230 | default: |
| 196 | // nothing to do |
231 | // nothing to do |
| 197 | break; |
232 | break; |
| Line 217... | Line 252... | ||
| 217 | if(PointCount > 0) |
252 | if(PointCount > 0) |
| 218 | { |
253 | { |
| 219 | // search for first wp in list |
254 | // search for first wp in list |
| 220 | for(i = 0; i <MAX_LIST_LEN; i++) |
255 | for(i = 0; i <MAX_LIST_LEN; i++) |
| 221 | { |
256 | { |
| 222 | if((PointList[i].Type == POINT_TYPE_WP) && (PointList[i].Position.Status != INVALID)) |
257 | if((PointList[i].Type == POINT_TYPE_WP) && (PointList[i].Position.Status != INVALID)) // jump over POIs and FS-Positions |
| 223 | { |
258 | { |
| 224 | WPIndex = i + 1; |
259 | WPIndex = i + 1; |
| 225 | break; |
260 | break; |
| 226 | } |
261 | } |
| - | 262 | /* |
|
| - | 263 | else |
|
| - | 264 | if((PointList[i].Type == POINT_TYPE_FS) && (PointList[i].Position.Status != INVALID)) |
|
| - | 265 | { |
|
| - | 266 | GPSPos_Copy(&(PointList[i].Position), &GPS_FailsafePosition); |
|
| - | 267 | } |
|
| - | 268 | */ |
|
| 227 | } |
269 | } |
| 228 | if(WPIndex) // found a WP in the list |
270 | if(WPIndex) // found a WP in the list |
| 229 | { |
271 | { |
| 230 | NaviData.WaypointIndex = 1; |
272 | NaviData.WaypointIndex = 1; |
| 231 | NewWaypointsReceived = 1; // activates the Waypoint list as soon as CH is started |
273 | NewWaypointsReceived = 1; // activates the Waypoint list as soon as CH is started |
| Line 316... | Line 358... | ||
| 316 | if(WPIndex < MAX_LIST_LEN) // if there is a next entry in the list |
358 | if(WPIndex < MAX_LIST_LEN) // if there is a next entry in the list |
| 317 | { |
359 | { |
| 318 | u8 i; |
360 | u8 i; |
| 319 | for(i = WPIndex; i < MAX_LIST_LEN; i++) // start search for next at next list entry |
361 | for(i = WPIndex; i < MAX_LIST_LEN; i++) // start search for next at next list entry |
| 320 | { |
362 | { |
| 321 | if((PointList[i].Type == POINT_TYPE_WP) && (PointList[i].Position.Status != INVALID)) // jump over POIs |
363 | if((PointList[i].Type == POINT_TYPE_WP) && (PointList[i].Position.Status != INVALID)) // jump over POIs and FS-Positions |
| 322 | { |
364 | { |
| 323 | wp_found = i+1; |
365 | wp_found = i+1; |
| 324 | break; |
366 | break; |
| 325 | } |
367 | } |
| - | 368 | /* |
|
| - | 369 | else |
|
| - | 370 | if((PointList[i].Type == POINT_TYPE_FS) && (PointList[i].Position.Status != INVALID)) // jump over POIs |
|
| - | 371 | { |
|
| - | 372 | GPSPos_Copy(&(PointList[i].Position), &GPS_FailsafePosition); |
|
| - | 373 | } |
|
| - | 374 | */ |
|
| 326 | } |
375 | } |
| 327 | } |
376 | } |
| 328 | if(wp_found) |
377 | if(wp_found) |
| 329 | { |
378 | { |
| 330 | WPIndex = wp_found; // update list position |
379 | WPIndex = wp_found; // update list position |
| Line 613... | Line 662... | ||
| 613 | case POINT_TYPE_WP: |
662 | case POINT_TYPE_WP: |
| 614 | // this works only if altitude key is set before point type key in WPL file !! |
663 | // this works only if altitude key is set before point type key in WPL file !! |
| 615 | PointList[IsPointSection-1].Position.Altitude /= 10; // dm only for WPs |
664 | PointList[IsPointSection-1].Position.Altitude /= 10; // dm only for WPs |
| 616 | WPCount++; |
665 | WPCount++; |
| 617 | break; |
666 | break; |
| 618 | - | ||
| - | 667 | case POINT_TYPE_FS: |
|
| - | 668 | FsPointCnt++; |
|
| - | 669 | break; |
|
| 619 | case POINT_TYPE_POI: |
670 | case POINT_TYPE_POI: |
| 620 | POICount++; |
671 | POICount++; |
| 621 | break; |
672 | break; |
| 622 | } |
673 | } |
| 623 | } |
674 | } |