Rev 314 | Rev 321 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 314 | Rev 315 | ||
---|---|---|---|
Line 73... | Line 73... | ||
73 | #include "compass.h" |
73 | #include "compass.h" |
74 | #include "waypoints.h" |
74 | #include "waypoints.h" |
75 | #include "mkprotocol.h" |
75 | #include "mkprotocol.h" |
76 | #include "params.h" |
76 | #include "params.h" |
77 | #include "fifo.h" |
77 | #include "fifo.h" |
78 | #include "debug.h" |
78 | #include "debug.h" |
Line 79... | Line 79... | ||
79 | 79 | ||
80 | #define FALSE 0 |
80 | #define FALSE 0 |
Line 81... | Line 81... | ||
81 | #define TRUE 1 |
81 | #define TRUE 1 |
Line 103... | Line 103... | ||
103 | 103 | ||
Line 104... | Line 104... | ||
104 | UART_TypeDef *DebugUART = UART1; |
104 | UART_TypeDef *DebugUART = UART1; |
105 | 105 | ||
106 | #ifdef FOLLOW_ME |
106 | #ifdef FOLLOW_ME |
107 | #define FOLLOW_ME_INTERVAL 200 // 5 Hz |
107 | #define FOLLOW_ME_INTERVAL 200 // 5 Hz |
108 | u8 UART1_FollowMe_Timer = 0; |
108 | u32 UART1_FollowMe_Timer = 0; |
Line 109... | Line 109... | ||
109 | Point_t FollowMe; |
109 | Point_t FollowMe; |
110 | #endif |
110 | #endif |
Line 359... | Line 359... | ||
359 | if(DebugUART != UART1) return; |
359 | if(DebugUART != UART1) return; |
Line 360... | Line 360... | ||
360 | 360 | ||
361 | u8 c; |
361 | u8 c; |
362 | // if rx buffer is not locked |
362 | // if rx buffer is not locked |
363 | if(UART1_rx_buffer.Locked == FALSE) |
363 | if(UART1_rx_buffer.Locked == FALSE) |
364 | { |
364 | { |
365 | //collect data from primary rx fifo |
365 | //collect data from primary rx fifo |
366 | while(fifo_get(&UART1_rx_fifo, &c)) |
366 | while(fifo_get(&UART1_rx_fifo, &c)) |
367 | { |
367 | { |
368 | // break if complete frame is collected |
368 | // break if complete frame is collected |
369 | if(MKProtocol_CollectSerialFrame(&UART1_rx_buffer, c)) break; |
369 | if(MKProtocol_CollectSerialFrame(&UART1_rx_buffer, c)) break; |
370 | } |
370 | } |
371 | } |
371 | } |
Line 408... | Line 408... | ||
408 | 408 | ||
409 | case 's':// new target position |
409 | case 's':// new target position |
410 | pPoint = (Point_t*)SerialMsg.pData; |
410 | pPoint = (Point_t*)SerialMsg.pData; |
411 | if(pPoint->Position.Status == NEWDATA) |
411 | if(pPoint->Position.Status == NEWDATA) |
412 | { |
412 | { |
413 | //if(!(FC.StatusFlags & FC_STATUS_FLY)) PointList_Clear(); // flush the list |
413 | //if(!(FC.StatusFlags & FC_STATUS_FLY)) PointList_Clear(); // flush the list |
414 | //pPoint->Index = 1; // must be one after empty list |
414 | //pPoint->Index = 1; // must be one after empty list |
415 | PointList_SetAt(pPoint); |
415 | PointList_SetAt(pPoint); |
416 | if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE); |
416 | if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE); |
417 | GPS_pWaypoint = PointList_WPBegin(); // updates POI index |
417 | GPS_pWaypoint = PointList_WPBegin(); // updates POI index |
418 | if(GPS_pWaypoint != NULL) // if new WP exist |
418 | if(GPS_pWaypoint != NULL) // if new WP exist |
419 | { // update WP hold time stamp immediately! |
419 | { // update WP hold time stamp immediately! |
420 | /* if(GPS_pWaypoint->Heading > 0 && GPS_pWaypoint->Heading <= 360) |
420 | /* if(GPS_pWaypoint->Heading > 0 && GPS_pWaypoint->Heading <= 360) |
Line 463... | Line 463... | ||
463 | 463 | ||
464 | if((pPoint->Position.Status == INVALID) && (pPoint->Index == 0)) |
464 | if((pPoint->Position.Status == INVALID) && (pPoint->Index == 0)) |
465 | { |
465 | { |
466 | PointList_Clear(); |
466 | PointList_Clear(); |
467 | GPS_pWaypoint = PointList_WPBegin(); |
467 | GPS_pWaypoint = PointList_WPBegin(); |
468 | UART1_Request_WritePoint = 0; // return new point count |
468 | UART1_Request_WritePoint = 0; // return new point count |
469 | } |
469 | } |
470 | else |
470 | else |
471 | { // update WP in list at index |
471 | { // update WP in list at index |
472 | UART1_Request_WritePoint = PointList_SetAt(pPoint); |
472 | UART1_Request_WritePoint = PointList_SetAt(pPoint); |
473 | if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE); |
473 | if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE); |
474 | if(UART1_Request_WritePoint == pPoint->Index) |
474 | if(UART1_Request_WritePoint == pPoint->Index) |
475 | { |
475 | { |
476 | BeepTime = 500; |
476 | BeepTime = 500; |
477 | } |
477 | } |
Line 619... | Line 619... | ||
619 | while(UART_GetFlagStatus(UART1, UART_FLAG_TxFIFOFull) != SET) |
619 | while(UART_GetFlagStatus(UART1, UART_FLAG_TxFIFOFull) != SET) |
620 | { |
620 | { |
621 | tmp_tx = UART1_tx_buffer.pData[UART1_tx_buffer.Position++]; // read next byte from txd buffer |
621 | tmp_tx = UART1_tx_buffer.pData[UART1_tx_buffer.Position++]; // read next byte from txd buffer |
622 | UART_SendData(UART1, tmp_tx); // put character to txd fifo |
622 | UART_SendData(UART1, tmp_tx); // put character to txd fifo |
623 | #ifdef FOLLOW_ME |
623 | #ifdef FOLLOW_ME |
624 | if(TransmitAlsoToFC) |
624 | if(TransmitAlsoToFC) |
625 | { |
625 | { |
626 | UART_SendData(UART2, tmp_tx); // put character to txd fifo |
626 | UART_SendData(UART2, tmp_tx); // put character to txd fifo |
627 | } |
627 | } |
628 | #endif |
628 | #endif |
629 | // if terminating character or end of txd buffer reached |
629 | // if terminating character or end of txd buffer reached |
630 | if((tmp_tx == '\r') || (UART1_tx_buffer.Position == UART1_tx_buffer.DataBytes)) |
630 | if((tmp_tx == '\r') || (UART1_tx_buffer.Position == UART1_tx_buffer.DataBytes)) |
Line 657... | Line 657... | ||
657 | UART1_Transmit(); // output pending bytes in tx buffer |
657 | UART1_Transmit(); // output pending bytes in tx buffer |
658 | if((UART1_tx_buffer.Locked == TRUE)) return; |
658 | if((UART1_tx_buffer.Locked == TRUE)) return; |
Line 659... | Line 659... | ||
659 | 659 | ||
660 | if(UART1_Request_Parameter && (UART1_tx_buffer.Locked == FALSE)) |
660 | if(UART1_Request_Parameter && (UART1_tx_buffer.Locked == FALSE)) |
661 | { |
661 | { |
662 | s16 ParamValue; |
662 | s16 ParamValue; |
663 | NCParams_GetValue(UART1_Request_ParameterId, &ParamValue); |
663 | NCParams_GetValue(UART1_Request_ParameterId, &ParamValue); |
664 | MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'J', NC_ADDRESS, 2, &UART1_Request_ParameterId, sizeof(UART1_Request_ParameterId), &ParamValue, sizeof(ParamValue)); // answer the param request |
664 | MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'J', NC_ADDRESS, 2, &UART1_Request_ParameterId, sizeof(UART1_Request_ParameterId), &ParamValue, sizeof(ParamValue)); // answer the param request |
665 | UART1_Request_Parameter = FALSE; |
665 | UART1_Request_Parameter = FALSE; |
666 | } |
666 | } |
Line 762... | Line 762... | ||
762 | } |
762 | } |
763 | #ifdef FOLLOW_ME |
763 | #ifdef FOLLOW_ME |
764 | else if(CheckDelay(UART1_FollowMe_Timer) && (UART1_tx_buffer.Locked == FALSE)) |
764 | else if(CheckDelay(UART1_FollowMe_Timer) && (UART1_tx_buffer.Locked == FALSE)) |
765 | { |
765 | { |
766 | if((GPSData.Status != INVALID) && (GPSData.SatFix == SATFIX_3D) && (GPSData.Flags & FLAG_GPSFIXOK) && (GPSData.NumOfSats >= 4)) |
766 | if((GPSData.Status != INVALID) && (GPSData.SatFix == SATFIX_3D) && (GPSData.Flags & FLAG_GPSFIXOK) && (GPSData.NumOfSats >= 4)) |
767 | { |
767 | { |
768 | TransmitAlsoToFC = 1; |
768 | TransmitAlsoToFC = 1; |
769 | // update FollowMe content |
769 | // update FollowMe content |
770 | FollowMe.Position.Longitude = GPSData.Position.Longitude; |
770 | FollowMe.Position.Longitude = GPSData.Position.Longitude; |
771 | FollowMe.Position.Latitude = GPSData.Position.Latitude; |
771 | FollowMe.Position.Latitude = GPSData.Position.Latitude; |
772 | FollowMe.Position.Status = NEWDATA; |
772 | FollowMe.Position.Status = NEWDATA; |
Line 795... | Line 795... | ||
795 | else if(SendDebugOutput && (UART1_tx_buffer.Locked == FALSE)) |
795 | else if(SendDebugOutput && (UART1_tx_buffer.Locked == FALSE)) |
796 | { |
796 | { |
797 | MKProtocol_CreateSerialFrame(&UART1_tx_buffer,'0', NC_ADDRESS, 1, (u8 *) &tDebug, sizeof(tDebug)); |
797 | MKProtocol_CreateSerialFrame(&UART1_tx_buffer,'0', NC_ADDRESS, 1, (u8 *) &tDebug, sizeof(tDebug)); |
798 | SendDebugOutput = 0; |
798 | SendDebugOutput = 0; |
799 | } |
799 | } |
800 | #endif |
800 | #endif |
801 | UART1_Transmit(); // output pending bytes in tx buffer |
801 | UART1_Transmit(); // output pending bytes in tx buffer |
802 | } |
802 | } |