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)) |