WebSVN - NaviCtrl - Diff - Rev 314 and 315



#include "compass.h"
#include "waypoints.h"
#include "mkprotocol.h"
#include "params.h"
#include "fifo.h"
#include "debug.h"

 
#define FALSE   0

#define TRUE    1

 

UART_TypeDef *DebugUART = UART1;
 
#ifdef FOLLOW_ME
#define FOLLOW_ME_INTERVAL 200 // 5 Hz
u32 UART1_FollowMe_Timer        = 0;

Point_t FollowMe;
#endif

        if(DebugUART != UART1) return;

 
        u8 c;
        // if rx buffer is not locked
        if(UART1_rx_buffer.Locked == FALSE)
        {
                //collect data from primary rx fifo
                while(fifo_get(&UART1_rx_fifo, &c))
                {
                        // break if complete frame is collected
                        if(MKProtocol_CollectSerialFrame(&UART1_rx_buffer, c)) break;
                }
        }

 
                        case 's'://  new target position
                                pPoint = (Point_t*)SerialMsg.pData;
                                if(pPoint->Position.Status == NEWDATA)
                                {
                                        //if(!(FC.StatusFlags & FC_STATUS_FLY)) PointList_Clear(); // flush the list
                                        //pPoint->Index = 1; // must be one after empty list
                                        PointList_SetAt(pPoint);
                                        if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE);
                                        GPS_pWaypoint = PointList_WPBegin(); // updates POI index
                                        if(GPS_pWaypoint != NULL) // if new WP exist
                                        {   // update WP hold time stamp immediately!
/*                                              if(GPS_pWaypoint->Heading > 0 && GPS_pWaypoint->Heading <= 360)

 
                                        if((pPoint->Position.Status == INVALID) && (pPoint->Index == 0))
                                        {
                                                PointList_Clear();
                                                GPS_pWaypoint = PointList_WPBegin();
                                                UART1_Request_WritePoint = 0; // return new point count
                                        }
                                        else
                                        {  // update WP in list at index
                                                UART1_Request_WritePoint = PointList_SetAt(pPoint);
                                                if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE);
                                                if(UART1_Request_WritePoint == pPoint->Index)
                                                {
                                                        BeepTime = 500;
                                                }

                while(UART_GetFlagStatus(UART1, UART_FLAG_TxFIFOFull) != SET)
                {
                        tmp_tx = UART1_tx_buffer.pData[UART1_tx_buffer.Position++]; // read next byte from txd buffer
                        UART_SendData(UART1, tmp_tx); // put character to txd fifo
                        #ifdef FOLLOW_ME
                        if(TransmitAlsoToFC)
                        {
                                UART_SendData(UART2, tmp_tx); // put character to txd fifo
                        }
                        #endif
                        // if terminating character or end of txd buffer reached
                        if((tmp_tx == '\r') || (UART1_tx_buffer.Position == UART1_tx_buffer.DataBytes))

        UART1_Transmit(); // output pending bytes in tx buffer
        if((UART1_tx_buffer.Locked == TRUE)) return;

 
        if(UART1_Request_Parameter && (UART1_tx_buffer.Locked == FALSE))
        {
                s16 ParamValue;
                NCParams_GetValue(UART1_Request_ParameterId, &ParamValue);
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'J', NC_ADDRESS, 2, &UART1_Request_ParameterId, sizeof(UART1_Request_ParameterId), &ParamValue, sizeof(ParamValue)); // answer the param request
                UART1_Request_Parameter = FALSE;
        }

        }
#ifdef FOLLOW_ME
        else if(CheckDelay(UART1_FollowMe_Timer) && (UART1_tx_buffer.Locked == FALSE))
        {
                if((GPSData.Status != INVALID) && (GPSData.SatFix == SATFIX_3D) && (GPSData.Flags & FLAG_GPSFIXOK) && (GPSData.NumOfSats >= 4))
                {
                        TransmitAlsoToFC = 1;
                        // update FollowMe content
                        FollowMe.Position.Longitude     = GPSData.Position.Longitude;
                        FollowMe.Position.Latitude      = GPSData.Position.Latitude;
                        FollowMe.Position.Status = NEWDATA;

        else if(SendDebugOutput && (UART1_tx_buffer.Locked == FALSE))
        {
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer,'0', NC_ADDRESS, 1, (u8 *) &tDebug, sizeof(tDebug));
                SendDebugOutput = 0;
        }
#endif
        UART1_Transmit(); // output pending bytes in tx buffer
}

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	}

Subversion Repositories NaviCtrl

(root)/tags/V0.28o/uart1.c @ 553 - Rev 314 → 315