Rev 838 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 838 | Rev 966 | ||
|---|---|---|---|
| Line 10... | Line 10... | ||
| 10 | Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that |
10 | Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that |
| 11 | are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de |
11 | are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de |
| 12 | unless it is stated otherwise. |
12 | unless it is stated otherwise. |
| 13 | */ |
13 | */ |
| Line -... | Line 14... | ||
| - | 14 | ||
| - | 15 | /***************************************************************************** |
|
| - | 16 | INCLUDES |
|
| 14 | 17 | **************************************************************************** */ |
|
| 15 | #include "main.h" |
18 | #include "main.h" |
| 16 | #include "kafi.h" |
- | |
| 17 | 19 | #include "kafi.h" |
|
| 18 | #include "mymath.h" |
20 | #include "mymath.h" |
| Line -... | Line 21... | ||
| - | 21 | #include <math.h> |
|
| - | 22 | ||
| - | 23 | /***************************************************************************** |
|
| - | 24 | (SYMBOLIC) CONSTANTS |
|
| - | 25 | *****************************************************************************/ |
|
| - | 26 | ||
| - | 27 | /***************************************************************************** |
|
| - | 28 | VARIABLES |
|
| 19 | #include <math.h> |
29 | *****************************************************************************/ |
| 20 | 30 | ||
| 21 | int GPSTracking = 0; |
31 | int GPSTracking = 0; |
| 22 | int targetPosValid = 0; |
- | |
| 23 | int homePosValid = 0; |
32 | int targetPosValid = 0; |
| 24 | 33 | int homePosValid = 0; |
|
| 25 | uint8_t gpsState; |
34 | int holdPosValid = 0; |
| 26 | 35 | ||
| 27 | gpsInfo_t actualPos; // measured position (last gps record) |
36 | volatile gpsInfo_t actualPos;// measured position (last gps record) |
| - | 37 | volatile gpsInfo_t targetPos;// measured position (last gps record) |
|
| 28 | gpsInfo_t targetPos; // measured position (last gps record) |
38 | volatile gpsInfo_t homePos;// measured position (last gps record) |
| 29 | gpsInfo_t homePos; // measured position (last gps record) |
39 | volatile gpsInfo_t holdPos; // measured position (last gps record) |
| 30 | 40 | ||
| 31 | NAV_STATUS_t navStatus; |
41 | NAV_STATUS_t navStatus; |
| 32 | NAV_POSLLH_t navPosLlh; |
42 | NAV_POSLLH_t navPosLlh; |
| - | 43 | NAV_POSUTM_t navPosUtm; |
|
| - | 44 | NAV_VELNED_t navVelNed; |
|
| 33 | NAV_POSUTM_t navPosUtm; |
45 | |
| 34 | NAV_VELNED_t navVelNed; |
46 | uint8_t gpsState; |
| 35 | 47 | ||
| 36 | signed int GPS_Nick = 0; |
48 | signed int GPS_Nick = 0; |
| 37 | signed int GPS_Roll = 0; |
49 | signed int GPS_Roll = 0; |
| 38 | 50 | ||
| 39 | long distanceNS = 0; |
51 | long distanceNS = 0; |
| 40 | long distanceEW = 0; |
52 | long distanceEW = 0; |
| 41 | long velocityNS = 0; |
53 | long velocityNS = 0; |
| Line 42... | Line 54... | ||
| 42 | long velocityEW = 0; |
54 | long velocityEW = 0; |
| 43 | unsigned long maxDistance = 0; |
55 | unsigned long maxDistance = 0; |
| 44 | 56 | ||
| 45 | int roll_gain = 0; |
- | |
| 46 | int nick_gain = 0; |
57 | int roll_gain = 0; |
| 47 | int nick_gain_p = 0; |
- | |
| 48 | int nick_gain_d = 0; |
58 | int nick_gain = 0; |
| 49 | int roll_gain_p = 0; |
59 | int nick_gain_p, nick_gain_d; |
| 50 | int roll_gain_d = 0; |
60 | int roll_gain_p, roll_gain_d; |
| 51 | int Override = 0; |
61 | int Override = 0; |
| - | 62 | int TargetGier = 0; |
|
| 52 | int TargetGier = 0; |
63 | |
| 53 | 64 | extern int sollGier; |
|
| 54 | extern int sollGier; |
65 | extern int RemoteLinkLost; |
| 55 | 66 | ||
| 56 | char *ubxP, *ubxEp, *ubxSp; // pointers to packet currently transfered |
67 | volatile char*ubxP, *ubxEp, *ubxSp;// pointers to packet currently transfered |
| 57 | uint8_t CK_A, CK_B; // UBX checksum bytes |
68 | volatile uint8_t CK_A, CK_B;// UBX checksum bytes |
| Line 58... | Line 69... | ||
| 58 | uint8_t ignorePacket; // true when previous packet was not processed |
69 | volatile uint8_t ignorePacket;// true when previous packet was not processed |
| 59 | unsigned short msgLen; |
70 | volatile unsigned short msgLen; |
| 60 | uint8_t msgID; |
- | |
| - | 71 | volatile uint8_t msgID; |
|
| Line 61... | Line 72... | ||
| 61 | 72 | ||
| 62 | void GPSscanData (void); |
73 | void GPSscanData (void); |
| 63 | void GPSupdate(void); |
74 | void GPSupdate(void); |
| Line -... | Line 75... | ||
| - | 75 | void SetNewHeading(unsigned long maxDistance); |
|
| - | 76 | ||
| 64 | 77 | /* **************************************************************************** |
|
| 65 | 78 | Functionname: GPSupdate */ /*! |
|
| 66 | /* **************************************************************************** |
79 | Description: |
| 67 | Functionname: GPSupdate */ /*! |
80 | |
| 68 | Description: |
81 | @param[in] |
| 69 | 82 | ||
| 70 | @return void |
83 | @return void |
| Line 135... | Line -... | ||
| 135 | beeptime = 5000; |
- | |
| 136 | BeepMuster = 0x0c00; |
- | |
| 137 | } |
- | |
| 138 | - | ||
| Line 139... | Line -... | ||
| 139 | max_p = (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 170)) / 20); |
- | |
| 140 | max_d = (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 170)) / 40); |
- | |
| 141 | - | ||
| 142 | #if 0 |
- | |
| 143 | DebugOut.Analog[11] = max_p; |
- | |
| 144 | DebugOut.Analog[12] = max_d; |
- | |
| 145 | #endif |
- | |
| 146 | - | ||
| 147 | static int GPSTrackingCycles = 0; |
- | |
| 148 | long maxGainPos = 140; |
- | |
| 149 | long maxGainVel = 140; |
- | |
| 150 | - | ||
| 151 | /* Slowly ramp up gain */ |
- | |
| 152 | if (GPSTrackingCycles < 1000) |
- | |
| 153 | { |
- | |
| 154 | GPSTrackingCycles++; |
- | |
| 155 | } |
- | |
| 156 | maxGainPos = (maxGainPos * GPSTrackingCycles) / 1000; |
- | |
| 157 | maxGainVel = (maxGainVel * GPSTrackingCycles) / 1000; |
- | |
| 158 | - | ||
| 159 | if (actualPos.state > 2 ) |
- | |
| 160 | { |
- | |
| 161 | distanceNS = actualPos.north - targetPos.north; // in 0.1m |
- | |
| 162 | distanceEW = actualPos.east - targetPos.east; // in 0.1m |
- | |
| 163 | velocityNS = actualPos.velNorth; // in 0.1m/s |
- | |
| 164 | velocityEW = actualPos.velEast; // in 0.1m/s |
- | |
| 165 | maxDistance = sqrt(distanceNS * distanceNS + distanceEW * distanceEW); |
- | |
| 166 | - | ||
| 167 | // Limit GPS_Nick to 25 |
- | |
| 168 | nick_gain_p = (long) MAX(-maxGainPos, MIN(maxGainPos, ((distanceNS* max_p)/50))); //m |
- | |
| 169 | nick_gain_d = (long) MAX(-maxGainVel, MIN(maxGainVel, ((actualPos.velNorth * max_d)/50))); //m/s |
- | |
| 170 | roll_gain_p = (long) MAX(-maxGainPos, MIN(maxGainPos, ((distanceEW * max_p)/50))); //m |
- | |
| 171 | roll_gain_d = (long) MAX(-maxGainVel, MIN(maxGainVel, ((actualPos.velEast * max_d)/50))); //m/s |
- | |
| 172 | - | ||
| 173 | TargetGier = c_atan2(-distanceEW / 8, -distanceNS / 8) * 10; |
- | |
| 174 | } |
- | |
| 175 | - | ||
| 176 | if ((GPSTracking == 1) && |
- | |
| 177 | (actualPos.state > 2) && |
- | |
| 178 | (Override == 0)) |
- | |
| 179 | { |
- | |
| 180 | /* Calculate Distance to Target */ |
- | |
| 181 | SIN_H = (float) sin(status.Psi); |
- | |
| 182 | COS_H =(float) cos(status.Psi); |
- | |
| 183 | - | ||
| 184 | /* PD-Regler */ |
- | |
| 185 | nick_gain = nick_gain_p + nick_gain_d; |
- | |
| 186 | roll_gain = -(roll_gain_p + roll_gain_d); |
- | |
| 187 | - | ||
| 188 | GPS_Nick = (int) (COS_H * (float) nick_gain - SIN_H * (float) roll_gain); |
- | |
| 189 | GPS_Roll = (int) (SIN_H * (float) nick_gain + COS_H * (float) roll_gain); |
- | |
| 190 | - | ||
| 191 | GPS_Nick = MAX(-maxGainPos, MIN(maxGainPos, GPS_Nick)); |
- | |
| 192 | GPS_Roll = MAX(-maxGainPos, MIN(maxGainPos, GPS_Roll)); |
- | |
| 193 | - | ||
| 194 | /* Turn the mikrokopter slowly towards the target position */ |
- | |
| 195 | { |
- | |
| 196 | int OffsetGier; |
- | |
| 197 | if (maxDistance / 10 > 2) |
- | |
| 198 | { |
- | |
| 199 | OffsetGier = 2; |
- | |
| 200 | } |
- | |
| 201 | else |
- | |
| 202 | { |
- | |
| 203 | OffsetGier = 0; |
- | |
| 204 | } |
- | |
| 205 | - | ||
| 206 | if (TargetGier < 0) |
- | |
| 207 | { |
- | |
| 208 | TargetGier += 3600; |
- | |
| 209 | } |
- | |
| 210 | if (TargetGier > sollGier) |
- | |
| 211 | { |
- | |
| 212 | if (TargetGier - sollGier < 1800) |
- | |
| 213 | { |
- | |
| 214 | sollGier += OffsetGier; |
- | |
| 215 | } |
- | |
| 216 | else |
- | |
| 217 | { |
- | |
| 218 | sollGier -= OffsetGier; |
- | |
| 219 | } |
- | |
| 220 | } |
- | |
| 221 | else |
- | |
| 222 | { |
- | |
| 223 | if (sollGier - TargetGier < 1800) |
- | |
| 224 | { |
- | |
| 225 | sollGier -= OffsetGier; |
272 | { |
| 226 | } |
- | |
| 227 | else |
273 | GPS_Nick = 0; |
| 228 | { |
274 | GPS_Roll = 0; |
| 229 | sollGier += OffsetGier; |
275 | maxDistance = 0.0F; |
| 230 | } |
276 | } |
| 231 | } |
277 | } |
| 232 | } |
278 | |
| 233 | #if 0 |
279 | |
| 234 | /* Go round in a square */ |
280 | #if 0 |
| 235 | if (maxDistance / 10 < 10) |
281 | if (maxDistance / 10 < 12) |
| 236 | { |
282 | { |
| 237 | static int iState = 0; |
283 | static int iState = 0; |
| 238 | switch (iState) |
284 | switch (iState) |
| 239 | { |
285 | { |
| 240 | case 0: |
286 | case 0: |
| 241 | targetPos.north += 400; |
287 | targetPos.north += 400; |
| 242 | targetPos.east += 0; |
288 | targetPos.east += 0; |
| 243 | GPSTrackingCycles = 0; |
289 | GPSTrackingCycles = 0; |
| 244 | iState++; |
290 | iState++; |
| 245 | break; |
291 | break; |
| 246 | case 1: |
292 | case 1: |
| 247 | targetPos.north += 0; |
293 | targetPos.north += 0; |
| 248 | targetPos.east += 400; |
294 | targetPos.east += 400; |
| 249 | GPSTrackingCycles = 0; |
295 | GPSTrackingCycles = 0; |
| 250 | iState++; |
296 | iState++; |
| 251 | break; |
297 | break; |
| 252 | case 2: |
298 | case 2: |
| 253 | targetPos.north -= 400; |
299 | targetPos.north -= 400; |
| 254 | targetPos.east += 0; |
300 | targetPos.east += 0; |
| 255 | GPSTrackingCycles = 0; |
301 | GPSTrackingCycles = 0; |
| 256 | iState++; |
302 | iState++; |
| 257 | break; |
303 | break; |
| 258 | case 3: |
304 | case 3: |
| 259 | targetPos.north += 0; |
305 | targetPos.north += 0; |
| 260 | targetPos.east -= 400; |
306 | targetPos.east -= 400; |
| 261 | GPSTrackingCycles = 0; |
307 | GPSTrackingCycles = 0; |
| 262 | iState=0; |
308 | iState=0; |
| 263 | break; |
309 | break; |
| - | 310 | default: |
|
| - | 311 | iState=0; |
|
| - | 312 | break; |
|
| - | 313 | } |
|
| - | 314 | beeptime = 5000; |
|
| - | 315 | BeepMuster = 0x0c00; |
|
| - | 316 | } |
|
| - | 317 | #endif |
|
| - | 318 | ||
| - | 319 | ||
| - | 320 | void SetNewHeading(unsigned long maxDistance) |
|
| - | 321 | /* Set New Heading */ |
|
| - | 322 | { |
|
| - | 323 | int OffsetGier = 0; |
|
| 264 | default: |
324 | if (maxDistance / 10 > 8) |
| - | 325 | { |
|
| - | 326 | OffsetGier = 4; |
|
| - | 327 | } |
|
| - | 328 | ||
| - | 329 | if (TargetGier < 0) |
|
| - | 330 | { |
|
| - | 331 | TargetGier += 3600; |
|
| - | 332 | } |
|
| - | 333 | if (TargetGier > sollGier) |
|
| - | 334 | { |
|
| - | 335 | if (TargetGier - sollGier < 1800) |
|
| 265 | iState=0; |
336 | { |
| 266 | break; |
337 | sollGier += OffsetGier; |
| - | 338 | } |
|
| 267 | } |
339 | else |
| 268 | beeptime = 5000; |
340 | { |
| - | 341 | sollGier -= OffsetGier; |
|
| - | 342 | } |
|
| - | 343 | } |
|
| 269 | BeepMuster = 0x0c00; |
344 | else |
| - | 345 | { |
|
| 270 | } |
346 | if (sollGier - TargetGier < 1800) |
| 271 | #endif |
347 | { |
| Line 272... | Line 348... | ||
| 272 | } |
348 | sollGier -= OffsetGier; |
| 273 | else |
349 | } |
| 274 | { |
350 | else |
| Line 287... | Line 363... | ||
| 287 | @post - |
363 | @post - |
| 288 | @author |
364 | @author |
| 289 | **************************************************************************** */ |
365 | **************************************************************************** */ |
| 290 | void InitGPS(void) |
366 | void InitGPS(void) |
| 291 | { |
367 | { |
| 292 | // USART1 Control and Status Register A, B, C and baud rate register |
368 | // USART1 Control and Status Register A, B, C and baud rate register |
| 293 | uint8_t sreg = SREG; |
369 | uint8_t sreg = SREG; |
| 294 | //uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART1_BAUD) - 1); |
370 | //uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART1_BAUD) - 1); |
| 295 | 371 | ||
| 296 | // disable all interrupts before reconfiguration |
372 | // disable all interrupts before reconfiguration |
| 297 | cli(); |
373 | cli(); |
| 298 | 374 | ||
| 299 | // disable RX-Interrupt |
375 | // disable RX-Interrupt |
| 300 | UCSR1B &= ~(1 << RXCIE1); |
376 | UCSR1B &= ~(1 << RXCIE1); |
| 301 | // disable TX-Interrupt |
377 | // disable TX-Interrupt |
| 302 | UCSR1B &= ~(1 << TXCIE1); |
378 | UCSR1B &= ~(1 << TXCIE1); |
| 303 | // disable DRE-Interrupt |
379 | // disable DRE-Interrupt |
| 304 | UCSR1B &= ~(1 << UDRIE1); |
380 | UCSR1B &= ~(1 << UDRIE1); |
| 305 | 381 | ||
| 306 | // set direction of RXD1 and TXD1 pins |
382 | // set direction of RXD1 and TXD1 pins |
| 307 | // set RXD1 (PD2) as an input pin |
383 | // set RXD1 (PD2) as an input pin |
| 308 | PORTD |= (1 << PORTD2); |
384 | PORTD |= (1 << PORTD2); |
| 309 | DDRD &= ~(1 << DDD2); |
385 | DDRD &= ~(1 << DDD2); |
| 310 | 386 | ||
| 311 | // set TXD1 (PD3) as an output pin |
387 | // set TXD1 (PD3) as an output pin |
| 312 | PORTD |= (1 << PORTD3); |
388 | PORTD |= (1 << PORTD3); |
| 313 | DDRD |= (1 << DDD3); |
389 | DDRD |= (1 << DDD3); |
| 314 | 390 | ||
| 315 | // USART0 Baud Rate Register |
391 | // USART0 Baud Rate Register |
| 316 | // set clock divider |
392 | // set clock divider |
| 317 | UBRR1H = 0; |
393 | UBRR1H = 0; |
| 318 | UBRR1L = BAUDRATE; |
394 | UBRR1L = BAUDRATE; |
| 319 | 395 | ||
| 320 | // enable double speed operation |
396 | // enable double speed operation |
| 321 | //UCSR1A |= (1 << U2X1); |
397 | //UCSR1A |= (1 << U2X1); |
| 322 | UCSR1A = _B0(U2X1); |
398 | UCSR1A = _B0(U2X1); |
| 323 | 399 | ||
| 324 | // enable receiver and transmitter |
400 | // enable receiver and transmitter |
| 325 | UCSR1B = (1 << TXEN1) | (1 << RXEN1); |
401 | UCSR1B = (1 << TXEN1) | (1 << RXEN1); |
| 326 | // set asynchronous mode |
402 | // set asynchronous mode |
| 327 | UCSR1C &= ~(1 << UMSEL11); |
403 | UCSR1C &= ~(1 << UMSEL11); |
| 328 | UCSR1C &= ~(1 << UMSEL10); |
404 | UCSR1C &= ~(1 << UMSEL10); |
| 329 | // no parity |
405 | // no parity |
| 330 | UCSR1C &= ~(1 << UPM11); |
406 | UCSR1C &= ~(1 << UPM11); |
| 331 | UCSR1C &= ~(1 << UPM10); |
407 | UCSR1C &= ~(1 << UPM10); |
| 332 | // 1 stop bit |
408 | // 1 stop bit |
| 333 | UCSR1C &= ~(1 << USBS1); |
409 | UCSR1C &= ~(1 << USBS1); |
| 334 | // 8-bit |
410 | // 8-bit |
| 335 | UCSR1B &= ~(1 << UCSZ12); |
411 | UCSR1B &= ~(1 << UCSZ12); |
| 336 | UCSR1C |= (1 << UCSZ11); |
412 | UCSR1C |= (1 << UCSZ11); |
| 337 | UCSR1C |= (1 << UCSZ10); |
413 | UCSR1C |= (1 << UCSZ10); |
| 338 | 414 | ||
| 339 | // flush receive buffer explicit |
415 | // flush receive buffer explicit |
| 340 | while ( UCSR1A & (1<<RXC1) ) UDR1; |
416 | while ( UCSR1A & (1<<RXC1) ) UDR1; |
| 341 | 417 | ||
| 342 | // enable interrupts at the end |
418 | // enable interrupts at the end |
| 343 | // enable RX-Interrupt |
419 | // enable RX-Interrupt |
| 344 | UCSR1B |= (1 << RXCIE1); |
420 | UCSR1B |= (1 << RXCIE1); |
| 345 | // enable TX-Interrupt |
421 | // enable TX-Interrupt |
| 346 | UCSR1B |= (1 << TXCIE1); |
422 | UCSR1B |= (1 << TXCIE1); |
| 347 | // enable DRE interrupt |
423 | // enable DRE interrupt |
| 348 | //UCSR1B |= (1 << UDRIE1); |
424 | //UCSR1B |= (1 << UDRIE1); |
| 349 | 425 | ||
| 350 | // restore global interrupt flags |
426 | // restore global interrupt flags |
| 351 | SREG = sreg; |
427 | SREG = sreg; |
| 352 | 428 | ||
| 353 | gpsState = GPS_EMPTY; |
429 | gpsState = GPS_EMPTY; |
| 354 | } |
430 | } |
| Line 355... | Line 431... | ||
| 355 | 431 | ||
| 356 | 432 | ||
| Line 363... | Line 439... | ||
| 363 | @post - |
439 | @post - |
| 364 | @author |
440 | @author |
| 365 | **************************************************************************** */ |
441 | **************************************************************************** */ |
| 366 | void GPSscanData (void) |
442 | void GPSscanData (void) |
| 367 | { |
443 | { |
| 368 | if (navStatus.packetStatus == 1) // valid packet |
444 | if (navStatus.packetStatus == 1)// valid packet |
| 369 | { |
445 | { |
| 370 | actualPos.state = navStatus.GPSfix; |
446 | actualPos.state = navStatus.GPSfix; |
| 371 | if ((actualPos.state > 1) && (GPSTracking == 0)) |
447 | if ((actualPos.state > 1) && (GPSTracking == 0)) |
| 372 | { |
448 | { |
| 373 | GRN_FLASH; |
449 | GRN_FLASH; |
| 374 | } |
450 | } |
| 375 | navStatus.packetStatus = 0; |
451 | navStatus.packetStatus = 0; |
| 376 | } |
452 | } |
| 377 | 453 | ||
| 378 | if (navPosUtm.packetStatus == 1) // valid packet |
454 | if (navPosUtm.packetStatus == 1)// valid packet |
| 379 | { |
455 | { |
| 380 | actualPos.north = navPosUtm.NORTH/10L; |
456 | actualPos.north = navPosUtm.NORTH/10L; |
| 381 | actualPos.east = navPosUtm.EAST/10L; |
457 | actualPos.east = navPosUtm.EAST/10L; |
| 382 | actualPos.altitude = navPosUtm.ALT/100; |
458 | actualPos.altitude = navPosUtm.ALT/100; |
| 383 | actualPos.ITOW = navPosUtm.ITOW; |
459 | actualPos.ITOW = navPosUtm.ITOW; |
| 384 | navPosUtm.packetStatus = 0; |
460 | navPosUtm.packetStatus = 0; |
| 385 | } |
461 | } |
| 386 | /* |
462 | /* |
| 387 | if (navPosLlh.packetStatus == 1) |
463 | if (navPosLlh.packetStatus == 1) |
| 388 | { |
464 | { |
| 389 | actualPos.longi = navPosLlh.LON; |
465 | actualPos.longi = navPosLlh.LON; |
| 390 | actualPos.lati = navPosLlh.LAT; |
466 | actualPos.lati = navPosLlh.LAT; |
| 391 | actualPos.height = navPosLlh.HEIGHT; |
467 | actualPos.height = navPosLlh.HEIGHT; |
| 392 | navPosLlh.packetStatus = 0; |
468 | navPosLlh.packetStatus = 0; |
| 393 | } |
469 | } |
| 394 | */ |
470 | */ |
| 395 | if (navVelNed.packetStatus == 1) |
471 | if (navVelNed.packetStatus == 1) |
| 396 | { |
472 | { |
| 397 | actualPos.velNorth = navVelNed.VEL_N; |
473 | actualPos.velNorth = navVelNed.VEL_N; |
| 398 | actualPos.velEast = navVelNed.VEL_E; |
474 | actualPos.velEast = navVelNed.VEL_E; |
| 399 | actualPos.velDown = navVelNed.VEL_D; |
475 | actualPos.velDown = navVelNed.VEL_D; |
| 400 | actualPos.groundSpeed = navVelNed.GSpeed; |
476 | actualPos.groundSpeed = navVelNed.GSpeed; |
| 401 | navVelNed.packetStatus = 0; |
477 | navVelNed.packetStatus = 0; |
| 402 | } |
478 | } |
| 403 | } |
479 | } |
| Line 404... | Line 480... | ||
| 404 | 480 | ||
| 405 | /* **************************************************************************** |
481 | /* **************************************************************************** |
| 406 | Functionname: SIGNAL */ /*! |
482 | Functionname: SIGNAL */ /*! |
| Line 411... | Line 487... | ||
| 411 | @post - |
487 | @post - |
| 412 | @author |
488 | @author |
| 413 | **************************************************************************** */ |
489 | **************************************************************************** */ |
| 414 | SIGNAL (SIG_USART1_RECV) |
490 | SIGNAL (SIG_USART1_RECV) |
| 415 | { |
491 | { |
| 543 | } |
619 | } |