Rev 515 | Rev 517 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 515 | Rev 516 | ||
|---|---|---|---|
| Line 102... | Line 102... | ||
| 102 | volatile FC_t FC; |
102 | volatile FC_t FC; |
| 103 | volatile u32 SPIWatchDog = 15000; // stop Navigation if this goes to zero |
103 | volatile u32 SPIWatchDog = 15000; // stop Navigation if this goes to zero |
| 104 | volatile u32 SD_WatchDog = 15000; // stop Logging if this goes to zero |
104 | volatile u32 SD_WatchDog = 15000; // stop Logging if this goes to zero |
| 105 | u32 CountGpsProcessedIn5Sec = 0,CountNewGpsDataIn5Sec = 0, FreqGpsProcessedIn5Sec = 0, FreqNewGpsDataIn5Sec = 0; |
105 | u32 CountGpsProcessedIn5Sec = 0,CountNewGpsDataIn5Sec = 0, FreqGpsProcessedIn5Sec = 0, FreqNewGpsDataIn5Sec = 0; |
| 106 | u8 NewWPL_Name = 0; |
106 | u8 NewWPL_Name = 0; |
| 107 | - | ||
| - | 107 | u32 MaxRadius_in_m = 0; |
|
| 108 | s8 ErrorMSG[25]; |
108 | s8 ErrorMSG[25]; |
| Line 109... | Line 109... | ||
| 109 | 109 | ||
| 110 | //---------------------------------------------------------------------------------------------------- |
110 | //---------------------------------------------------------------------------------------------------- |
| 111 | void SCU_Config(void) |
111 | void SCU_Config(void) |
| Line 141... | Line 141... | ||
| 141 | 141 | ||
| Line 142... | Line 142... | ||
| 142 | //---------------------------------------------------------------------------------------------------- |
142 | //---------------------------------------------------------------------------------------------------- |
| 143 | 143 | ||
| 144 | void CheckErrors(void) |
144 | void CheckErrors(void) |
| 145 | { |
145 | { |
| 146 | static s32 no_error_delay = 0; |
146 | static s32 no_error_delay = 0; |
| Line 147... | Line 147... | ||
| 147 | s32 newErrorCode = 0; |
147 | s32 newErrorCode = 0; |
| 148 | UART_VersionInfo.HardwareError[0] = 0; |
148 | UART_VersionInfo.HardwareError[0] = 0; |
| Line 276... | Line 276... | ||
| 276 | newErrorCode = 19; |
276 | newErrorCode = 19; |
| 277 | } |
277 | } |
| 278 | else if(CheckDelay(UBX_Timeout) && Parameter.GlobalConfig & FC_CFG_GPS_AKTIV) |
278 | else if(CheckDelay(UBX_Timeout) && Parameter.GlobalConfig & FC_CFG_GPS_AKTIV) |
| 279 | { |
279 | { |
| 280 | LED_RED_ON; |
280 | LED_RED_ON; |
| 281 | // if(!(Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)) sprintf(ErrorMSG,"GPS disconnected "); |
281 | // if(!(Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)) sprintf(ErrorMSG,"GPS disconnected "); |
| 282 | // else |
282 | // else |
| 283 | { |
283 | { |
| 284 | sprintf(ErrorMSG,"no GPS communication"); |
284 | sprintf(ErrorMSG,"no GPS communication"); |
| 285 | UART_VersionInfo.HardwareError[0] |= NC_ERROR0_GPS_RX; |
285 | UART_VersionInfo.HardwareError[0] |= NC_ERROR0_GPS_RX; |
| 286 | UART_VersionInfo.Flags &= ~NC_VERSION_FLAG_GPS_PRESENT; |
286 | UART_VersionInfo.Flags &= ~NC_VERSION_FLAG_GPS_PRESENT; |
| Line 346... | Line 346... | ||
| 346 | DebugOut.StatusRed |= AMPEL_BL; |
346 | DebugOut.StatusRed |= AMPEL_BL; |
| 347 | } |
347 | } |
| 348 | else if(BL_MinOfMaxPWM < 30 && !ErrorCode) |
348 | else if(BL_MinOfMaxPWM < 30 && !ErrorCode) |
| 349 | { |
349 | { |
| 350 | u16 i; |
350 | u16 i; |
| 351 | for(i = 0; i < 12; i++) if(Motor[i].MaxPWM == BL_MinOfMaxPWM) break; |
351 | for(i = 0; i < 12; i++) if(Motor[i].MaxPWM == BL_MinOfMaxPWM) break; |
| 352 | LED_RED_ON; |
352 | LED_RED_ON; |
| 353 | sprintf(ErrorMSG,"ERR:BL%2d Test:%2d ",i+1,BL_MinOfMaxPWM); |
353 | sprintf(ErrorMSG,"ERR:BL%2d Test:%2d ",i+1,BL_MinOfMaxPWM); |
| 354 | newErrorCode = 32; |
354 | newErrorCode = 32; |
| 355 | DebugOut.StatusRed |= AMPEL_BL; |
355 | DebugOut.StatusRed |= AMPEL_BL; |
| 356 | } |
356 | } |
| 357 | else if(BL_MinOfMaxPWM < 248 && (FC.StatusFlags & FC_STATUS_FLY) && !ErrorCode) |
357 | else if(BL_MinOfMaxPWM < 248 && (FC.StatusFlags & FC_STATUS_FLY) && !ErrorCode) |
| 358 | { |
358 | { |
| 359 | LED_RED_ON; |
359 | LED_RED_ON; |
| 360 | sprintf(ErrorMSG,"ERR:BL Limitation "); |
360 | sprintf(ErrorMSG,"ERR:BL Limitation "); |
| Line 388... | Line 388... | ||
| 388 | LED_RED_ON; |
388 | LED_RED_ON; |
| 389 | sprintf(ErrorMSG,"ERR:Max Altitude "); |
389 | sprintf(ErrorMSG,"ERR:Max Altitude "); |
| 390 | newErrorCode = 29; |
390 | newErrorCode = 29; |
| 391 | DebugOut.StatusRed |= AMPEL_NC; |
391 | DebugOut.StatusRed |= AMPEL_NC; |
| 392 | } |
392 | } |
| 393 | else if(Parameter.GlobalConfig3 & CFG3_NO_GPSFIX_NO_START && !(NCFlags & NC_FLAG_GPS_OK) && ((FC.StatusFlags & (FC_STATUS_START | FC_STATUS_MOTOR_RUN)) || (FC.StickGas < -50 && FC.StickYaw < -50))) |
393 | else if(Parameter.GlobalConfig3 & CFG3_NO_GPSFIX_NO_START && !(NCFlags & NC_FLAG_GPS_OK) && ((FC.StatusFlags & (FC_STATUS_START | FC_STATUS_MOTOR_RUN)) || (FC.StickGas < -50 && FC.StickYaw < -50))) |
| 394 | { |
394 | { |
| 395 | LED_RED_ON; |
395 | LED_RED_ON; |
| 396 | sprintf(ErrorMSG,"No GPS Fix "); |
396 | sprintf(ErrorMSG,"No GPS Fix "); |
| 397 | newErrorCode = 30; |
397 | newErrorCode = 30; |
| 398 | } |
398 | } |
| Line 406... | Line 406... | ||
| 406 | sprintf(ErrorMSG,"No Error "); |
406 | sprintf(ErrorMSG,"No Error "); |
| 407 | ErrorCode = 0; |
407 | ErrorCode = 0; |
| 408 | } |
408 | } |
| 409 | } |
409 | } |
| Line 410... | Line 410... | ||
| 410 | 410 | ||
| 411 | if(newErrorCode) |
411 | if(newErrorCode) |
| 412 | { |
412 | { |
| 413 | if(FC.StatusFlags & FC_STATUS_MOTOR_RUN) no_error_delay = 8; // delay the errors if the motors are running |
413 | if(FC.StatusFlags & FC_STATUS_MOTOR_RUN) no_error_delay = 8; // delay the errors if the motors are running |
| 414 | ErrorCode = newErrorCode; |
414 | ErrorCode = newErrorCode; |
| 415 | } |
415 | } |
| 416 | FC.Error[0] = 0; |
416 | FC.Error[0] = 0; |
| 417 | FC.Error[1] = 0; |
417 | FC.Error[1] = 0; |
| 418 | FC.Error[2] = 0; |
418 | FC.Error[2] = 0; |
| 419 | FC.Error[3] = 0; |
419 | FC.Error[3] = 0; |
| 420 | FC.Error[4] = 0; |
420 | FC.Error[4] = 0; |
| 421 | ErrorGpsFixLost = 0; |
421 | ErrorGpsFixLost = 0; |
| Line 422... | Line 422... | ||
| 422 | } |
422 | } |
| 423 | 423 | ||
| 424 | 424 | ||
| 425 | 425 | ||
| Line 426... | Line 426... | ||
| 426 | u8 Polling(void) |
426 | u8 Polling(void) |
| 427 | { |
427 | { |
| Line 428... | Line 428... | ||
| 428 | static u8 running = 0, oldFcFlags = 0, count5sec; |
428 | static u8 running = 0, oldFcFlags = 0, count5sec; |
| 429 | static u32 old_ms = 0; |
429 | static u32 old_ms = 0; |
| 430 | 430 | ||
| Line 484... | Line 484... | ||
| 484 | void EXTIT3_IRQHandler(void) // 1ms - Takt |
484 | void EXTIT3_IRQHandler(void) // 1ms - Takt |
| 485 | { |
485 | { |
| 486 | IENABLE; |
486 | IENABLE; |
| 487 | VIC_ITCmd(EXTIT3_ITLine,DISABLE); // disable irq |
487 | VIC_ITCmd(EXTIT3_ITLine,DISABLE); // disable irq |
| Line 488... | Line 488... | ||
| 488 | 488 | ||
| 489 | if(PollingTimeout == 0) |
489 | if(PollingTimeout == 0) |
| 490 | { |
490 | { |
| 491 | PollingTimeout = 5; |
491 | PollingTimeout = 5; |
| 492 | //if(Polling() == 0) DebugOut.Analog[16]++; |
492 | //if(Polling() == 0) DebugOut.Analog[]++; |
| 493 | Polling(); |
493 | Polling(); |
| Line 494... | Line 494... | ||
| 494 | } |
494 | } |
| 495 | 495 | ||
| 496 | VIC_SWITCmd(EXTIT3_ITLine,DISABLE); // clear pending bit |
496 | VIC_SWITCmd(EXTIT3_ITLine,DISABLE); // clear pending bit |
| Line 553... | Line 553... | ||
| 553 | GetNaviCtrlVersion(); |
553 | GetNaviCtrlVersion(); |
| 554 | DebugOut.StatusGreen = AMPEL_NC | AMPEL_COMPASS; // NC and MK3Mag |
554 | DebugOut.StatusGreen = AMPEL_NC | AMPEL_COMPASS; // NC and MK3Mag |
| 555 | DebugOut.StatusRed = 0x00; |
555 | DebugOut.StatusRed = 0x00; |
| 556 | UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++"); |
556 | UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++"); |
| Line 557... | Line 557... | ||
| 557 | 557 | ||
| Line 558... | Line 558... | ||
| 558 | Compass_Init(); |
558 | Compass_Init(); |
| Line 559... | Line 559... | ||
| 559 | 559 | ||
| 560 | GPS_Init(); |
560 | GPS_Init(); |
| Line 573... | Line 573... | ||
| 573 | LED_RED_ON; |
573 | LED_RED_ON; |
| 574 | } |
574 | } |
| 575 | #endif |
575 | #endif |
| 576 | // ---------- Prepare the isr driven |
576 | // ---------- Prepare the isr driven |
| 577 | // set to absolute lowest priority |
577 | // set to absolute lowest priority |
| 578 | VIC_Config(EXTIT3_ITLine, VIC_IRQ, PRIORITY_SW); |
578 | VIC_Config(EXTIT3_ITLine, VIC_IRQ, PRIORITY_SW); |
| 579 | // enable interrupts |
579 | // enable interrupts |
| 580 | VIC_ITCmd(EXTIT3_ITLine, ENABLE); |
580 | VIC_ITCmd(EXTIT3_ITLine, ENABLE); |
| Line 581... | Line 581... | ||
| 581 | 581 | ||
| 582 | Debug_OK("START"); |
582 | Debug_OK("START"); |
| 583 | UART1_PutString("\r\n"); |
583 | UART1_PutString("\r\n"); |
| 584 | fifo_purge(&UART1_rx_fifo); // flush the whole fifo init buffer |
584 | fifo_purge(&UART1_rx_fifo); // flush the whole fifo init buffer |
| 585 | LED_GRN_ON; |
585 | LED_GRN_ON; |
| 586 | LED_RED_OFF; |
586 | LED_RED_OFF; |
| 587 | Settings_GetParamValue(PID_SEND_NMEA, &NMEA_Interval); |
587 | Settings_GetParamValue(PID_SEND_NMEA, &NMEA_Interval); |
| 588 | UART1_PutString("\r\n"); |
588 | UART1_PutString("\r\n"); |
| 589 | CompassValueErrorCount = 0; |
589 | CompassValueErrorCount = 0; |
| 590 | I2CBus(Compass_I2CPort)->Timeout = SetDelay(3000); |
590 | I2CBus(Compass_I2CPort)->Timeout = SetDelay(3000); |
| 591 | for (;;) // the endless main loop |
591 | for (;;) // the endless main loop |
| 592 | { |
592 | { |
| 593 | PollingTimeout = 5; |
593 | PollingTimeout = 5; |
| 594 | Polling(); |
594 | Polling(); |
| 595 | // ++++++++++++++++++++++++++++++++++++++++++++++ |
595 | // ++++++++++++++++++++++++++++++++++++++++++++++ |
| 596 | if(FromFC_LoadWP_List) |
596 | if(FromFC_LoadWP_List) |
| 597 | { |
597 | { |
| 598 | WPL_Store.Index = (FromFC_LoadWP_List & ~0x80); |
598 | WPL_Store.Index = (FromFC_LoadWP_List & ~0x80); |
| 599 | if(WPL_Store.Index <= ToFC_MaxWpListIndex) |
599 | if(WPL_Store.Index <= ToFC_MaxWpListIndex) |
| 600 | { |
600 | { |
| 601 | if(PointList_ReadFromFile(&WPL_Store) == WPL_OK) |
601 | if(PointList_ReadFromFile(&WPL_Store) == WPL_OK) |
| 602 | { |
602 | { |
| 603 | if(FromFC_LoadWP_List & 0x80)// -> load relative |
603 | if(FromFC_LoadWP_List & 0x80)// -> load relative |
| 604 | { |
604 | { |
| 605 | if(NCFlags & NC_FLAG_FREE || NaviData.TargetPositionDeviation.Distance > 7*10) |
605 | if(NCFlags & NC_FLAG_FREE || NaviData.TargetPositionDeviation.Distance > 7*10) |
| 606 | { // take actual position |
606 | { // take actual position |
| 607 | if(!PointList_Move(1,&(GPSData.Position),NaviData.CompassHeading)) PointList_Clear(); // try to move wp-list so that 1st entry matches the current position |
607 | if(!PointList_Move(1,&(GPSData.Position),NaviData.CompassHeading)) PointList_Clear(); // try to move wp-list so that 1st entry matches the current position |
| 608 | } |
608 | } |
| 609 | else |
609 | else |
| 610 | { // take last target position |
610 | { // take last target position |
| 611 | if(!PointList_Move(1, &(NaviData.TargetPosition),NaviData.CompassHeading)) PointList_Clear(); // try to move wp-list so that 1st entry matches the current position |
611 | if(!PointList_Move(1, &(NaviData.TargetPosition),NaviData.CompassHeading)) PointList_Clear(); // try to move wp-list so that 1st entry matches the current position |
| 612 | } |
612 | } |
| 613 | } |
613 | } |
| 614 | if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE); |
614 | if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE); |
| 615 | GPS_pWaypoint = PointList_WPBegin(); // updates POI index |
615 | GPS_pWaypoint = PointList_WPBegin(); // updates POI index |
| Line 620... | Line 620... | ||
| 620 | } |
620 | } |
| 621 | // ++++++++++++++++++++++++++++++++++++++++++++++ |
621 | // ++++++++++++++++++++++++++++++++++++++++++++++ |
| 622 | if(FromFC_Save_SinglePoint) |
622 | if(FromFC_Save_SinglePoint) |
| 623 | { |
623 | { |
| 624 | WPL_Store.Index = FromFC_Save_SinglePoint; |
624 | WPL_Store.Index = FromFC_Save_SinglePoint; |
| 625 | if(PointList_SaveSinglePoint(&WPL_Store) == WPL_OK) BeepTime = 150; |
625 | if(WPL_Store.Index <= ToFC_MaxWpListIndex) if(PointList_SaveSinglePoint(&WPL_Store) == WPL_OK) BeepTime = 150; |
| 626 | FromFC_Save_SinglePoint = 0; |
626 | FromFC_Save_SinglePoint = 0; |
| 627 | } |
627 | } |
| 628 | // ++++++++++++++++++++++++++++++++++++++++++++++ |
628 | // ++++++++++++++++++++++++++++++++++++++++++++++ |
| 629 | if(FromFC_Load_SinglePoint) |
629 | if(FromFC_Load_SinglePoint) |
| 630 | { |
630 | { |
| 631 | WPL_Store.Index = FromFC_Load_SinglePoint; |
631 | WPL_Store.Index = FromFC_Load_SinglePoint; |
| - | 632 | if(WPL_Store.Index <= ToFC_MaxWpListIndex) |
|
| - | 633 | { |
|
| 632 | if(PointList_LoadSinglePoint(&WPL_Store) == WPL_OK) |
634 | if(PointList_LoadSinglePoint(&WPL_Store) == WPL_OK) |
| 633 | { |
635 | { |
| 634 | if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE); |
636 | if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE); |
| 635 | GPS_pWaypoint = PointList_WPBegin(); // updates POI index |
637 | GPS_pWaypoint = PointList_WPBegin(); // updates POI index |
| 636 | BeepTime = 150; |
638 | BeepTime = 150; |
| 637 | } |
639 | } |
| - | 640 | } |
|
| 638 | FromFC_Load_SinglePoint = 0; |
641 | FromFC_Load_SinglePoint = 0; |
| 639 | } |
642 | } |
| 640 | // ++++++++++++++++++++++++++++++++++++++++++++++ |
643 | // ++++++++++++++++++++++++++++++++++++++++++++++ |
| 641 | // ---------------- Logging --------------------------------------- |
644 | // ---------------- Logging --------------------------------------- |
| 642 | if(SD_WatchDog) |
645 | if(SD_WatchDog) |
| 643 | { |
646 | { |
| 644 | SD_WatchDog = 30000; |
647 | SD_WatchDog = 30000; |
| 645 | 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 |
648 | 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 |
| 646 | else |
649 | else |
| 647 | { |
650 | { |
| 648 | ToFC_MaxWpListIndex = 0; |
651 | ToFC_MaxWpListIndex = 0; |
| 649 | if(FC.StatusFlags & FC_STATUS_START) SD_LoggingError = 100; |
652 | if(FC.StatusFlags & FC_STATUS_START) SD_LoggingError = 100; |
| 650 | } |
653 | } |
| 651 | if(!SD_WatchDog) UART1_PutString("\n\rSD-Watchdog - Logging aborted\n\r"); |
654 | if(!SD_WatchDog) UART1_PutString("\n\rSD-Watchdog - Logging aborted\n\r"); |
| 652 | } |
655 | } |
| Line 653... | Line 656... | ||
| 653 | 656 | ||
| Line 698... | Line 701... | ||
| 698 | } |
701 | } |
| 699 | } |
702 | } |
| 700 | */ |
703 | */ |
| 701 | } |
704 | } |
| 702 | } |
705 | } |
| 703 | //DebugOut.Analog[16] |
706 | //DebugOut.Analog[] |