Rev 502 | Rev 504 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 502 | Rev 503 | ||
|---|---|---|---|
| Line 92... | Line 92... | ||
| 92 | u16 BeepTime; |
92 | u16 BeepTime; |
| 93 | u8 NCFlags = 0; |
93 | u8 NCFlags = 0; |
| 94 | s16 GeoMagDec = 0; // local magnetic declination in 0.1 deg |
94 | s16 GeoMagDec = 0; // local magnetic declination in 0.1 deg |
| 95 | u8 ErrorGpsFixLost = 0; |
95 | u8 ErrorGpsFixLost = 0; |
| 96 | u8 FromFC_LoadWP_List = 0; |
96 | u8 FromFC_LoadWP_List = 0; |
| - | 97 | u8 ToFC_MaxWpListIndex = 3; |
|
| 97 | u8 ClearFCStatusFlags = 0; |
98 | u8 ClearFCStatusFlags = 0; |
| 98 | u8 StopNavigation = 0; |
99 | u8 StopNavigation = 0; |
| 99 | volatile u32 PollingTimeout = 10000; |
100 | volatile u32 PollingTimeout = 10000; |
| 100 | Param_t Parameter; |
101 | Param_t Parameter; |
| 101 | volatile FC_t FC; |
102 | volatile FC_t FC; |
| Line 532... | Line 533... | ||
| 532 | // initialize SPI0 to FC |
533 | // initialize SPI0 to FC |
| 533 | SPI0_Init(); |
534 | SPI0_Init(); |
| 534 | // initialize i2c busses (needs Timer 1) |
535 | // initialize i2c busses (needs Timer 1) |
| 535 | I2CBus_Init(I2C0); |
536 | I2CBus_Init(I2C0); |
| 536 | I2CBus_Init(I2C1); |
537 | I2CBus_Init(I2C1); |
| 537 | - | ||
| 538 | // initialize fat16 partition on sd card (needs Timer 1) |
538 | // initialize fat16 partition on sd card (needs Timer 1) |
| 539 | Fat16_Init(); |
539 | Fat16_Init(); |
| 540 | // initialize NC params |
540 | // initialize NC params |
| 541 | NCParams_Init(); |
541 | NCParams_Init(); |
| 542 | // initialize the settings |
542 | // initialize the settings |
| Line 584... | Line 584... | ||
| 584 | LED_GRN_ON; |
584 | LED_GRN_ON; |
| 585 | LED_RED_OFF; |
585 | LED_RED_OFF; |
| 586 | Settings_GetParamValue(PID_SEND_NMEA, &NMEA_Interval); |
586 | Settings_GetParamValue(PID_SEND_NMEA, &NMEA_Interval); |
| 587 | UART1_PutString("\r\n"); |
587 | UART1_PutString("\r\n"); |
| 588 | CompassValueErrorCount = 0; |
588 | CompassValueErrorCount = 0; |
| 589 | - | ||
| - | 589 | I2CBus(Compass_I2CPort)->Timeout = SetDelay(3000); |
|
| 590 | for (;;) // the endless main loop |
590 | for (;;) // the endless main loop |
| 591 | { |
591 | { |
| 592 | PollingTimeout = 5; |
592 | PollingTimeout = 5; |
| 593 | Polling(); |
593 | Polling(); |
| 594 | // ++++++++++++++++++++++++++++++++++++++++++++++ |
594 | // ++++++++++++++++++++++++++++++++++++++++++++++ |
| 595 | if(FromFC_LoadWP_List) |
595 | if(FromFC_LoadWP_List) |
| 596 | { |
596 | { |
| 597 | WPL_Store.Index = FromFC_LoadWP_List; |
597 | WPL_Store.Index = (FromFC_LoadWP_List & ~0x80); |
| - | 598 | if(WPL_Store.Index <= ToFC_MaxWpListIndex) |
|
| - | 599 | { |
|
| 598 | if(PointList_ReadFromFile(&WPL_Store) == WPL_OK) |
600 | if(PointList_ReadFromFile(&WPL_Store) == WPL_OK) |
| 599 | { |
601 | { |
| 600 | if(FromFC_LoadWP_List & 0x80)// -> load relative |
602 | if(FromFC_LoadWP_List & 0x80)// -> load relative |
| 601 | { |
603 | { |
| - | 604 | if(NCFlags & NC_FLAG_FREE || NaviData.TargetPositionDeviation.Distance > 7*10) |
|
| - | 605 | { // take actual position |
|
| 602 | if(!PointList_Move(1, &(GPSData.Position))) // try to move wp-list so that 1st entry matches the current position |
606 | if(!PointList_Move(1,&(GPSData.Position),NaviData.CompassHeading)) PointList_Clear(); // try to move wp-list so that 1st entry matches the current position |
| 603 | { |
607 | } |
| - | 608 | else |
|
| 604 | PointList_Clear(); |
609 | { // take last target position |
| - | 610 | if(!PointList_Move(1, &(NaviData.TargetPosition),NaviData.CompassHeading)) PointList_Clear(); // try to move wp-list so that 1st entry matches the current position |
|
| 605 | } |
611 | } |
| 606 | } |
612 | } |
| 607 | if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE); |
613 | if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE); |
| 608 | GPS_pWaypoint = PointList_WPBegin(); // updates POI index |
614 | GPS_pWaypoint = PointList_WPBegin(); // updates POI index |
| 609 | BeepTime = 150; |
615 | BeepTime = 150; |
| 610 | } |
616 | } |
| 611 | FromFC_LoadWP_List = 0; |
617 | FromFC_LoadWP_List = 0; |
| - | 618 | } |
|
| 612 | } |
619 | } |
| 613 | // ++++++++++++++++++++++++++++++++++++++++++++++ |
620 | // ++++++++++++++++++++++++++++++++++++++++++++++ |
| Line 614... | Line 621... | ||
| 614 | 621 | ||
| 615 | // ---------------- Logging --------------------------------------- |
622 | // ---------------- Logging --------------------------------------- |
| 616 | if(SD_WatchDog) |
623 | if(SD_WatchDog) |
| 617 | { |
624 | { |
| 618 | SD_WatchDog = 30000; |
625 | SD_WatchDog = 30000; |
| - | 626 | if(SDCardInfo.Valid == 1) Logging_Update(); // could be block some time for at max. 2 seconds, therefore move time critical part of the mainloop into the ISR of timer 1 |
|
| - | 627 | else |
|
| - | 628 | { |
|
| 619 | if(SDCardInfo.Valid == 1) Logging_Update(); // could be block some time for at max. 2 seconds, therefore move time critical part of the mainloop into the ISR of timer 1 |
629 | ToFC_MaxWpListIndex = 0; |
| - | 630 | if(FC.StatusFlags & FC_STATUS_START) SD_LoggingError = 100; |
|
| 620 | else if(FC.StatusFlags & FC_STATUS_START) SD_LoggingError = 100; |
631 | } |
| 621 | if(!SD_WatchDog) UART1_PutString("\n\rSD-Watchdog - Logging aborted\n\r"); |
632 | if(!SD_WatchDog) UART1_PutString("\n\rSD-Watchdog - Logging aborted\n\r"); |
| Line 622... | Line -... | ||
| 622 | } |
- | |
| 623 | - | ||
| 624 | 633 | } |
|
| 625 | 634 | ||
| 626 | /* |
635 | /* |
| Line 627... | Line 636... | ||
| 627 | if(CheckDelay(ftimer)) |
636 | if(CheckDelay(ftimer)) |