103,6 → 103,12 |
|
UART_TypeDef *DebugUART = UART1; |
|
#ifdef FOLLOW_ME |
#define FOLLOW_ME_INTERVAL 200 // 5 Hz |
u8 UART1_FollowMe_Timer = 0; |
Point_t FollowMe; |
#endif |
|
// the primary rx fifo |
#define UART1_RX_FIFO_LEN 512 |
u8 UART1_rxfifobuffer[UART1_RX_FIFO_LEN]; |
579,6 → 585,9 |
while (UART_GetFlagStatus(UART1, UART_FLAG_TxFIFOFull) != RESET); |
// transmit byte |
UART_SendData(UART1, c); |
#ifdef FOLLOW_ME |
if(TransmitAlsoToFC) UART_SendData(UART2, c); |
#endif |
return (0); |
} |
|
611,10 → 620,19 |
{ |
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)) |
{ |
Buffer_Clear(&UART1_tx_buffer); // clear txd buffer |
#ifdef FOLLOW_ME |
TransmitAlsoToFC = 0; |
#endif |
break; // end while loop |
} |
} |
742,8 → 760,39 |
MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'E', NC_ADDRESS, 1, (u8 *)&ErrorMSG, sizeof(ErrorMSG)); |
UART1_Request_ErrorMessage = 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; |
FollowMe.Position.Altitude = 1; |
// 0 -> no Orientation |
// 1-360 -> CompassCourse Setpoint |
// -1 -> points to WP1 -> itself |
FollowMe.Heading = -1; |
FollowMe.ToleranceRadius = 1; |
FollowMe.HoldTime = 60; |
FollowMe.Event_Flag = 1; |
FollowMe.Index = 1; // 0 = Delete List, 1 place at first entry in the list |
FollowMe.Type = POINT_TYPE_WP; |
FollowMe.WP_EventChannelValue = 100; // set servo value |
FollowMe.AltitudeRate = 0; // do not change height |
FollowMe.reserve[0] = 0; // reserve |
FollowMe.reserve[1] = 0; // reserve |
FollowMe.reserve[2] = 0; // reserve |
FollowMe.reserve[3] = 0; // reserve |
MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 's', NC_ADDRESS, 1, (u8 *)&FollowMe, sizeof(FollowMe)); |
} |
UART1_FollowMe_Timer = SetDelay(FOLLOW_ME_INTERVAL); // set new update time |
} |
#endif |
#ifdef DEBUG // only include functions if DEBUG is defined |
if(SendDebugOutput && (UART1_tx_buffer.Locked == FALSE)) |
else if(SendDebugOutput && (UART1_tx_buffer.Locked == FALSE)) |
{ |
MKProtocol_CreateSerialFrame(&UART1_tx_buffer,'0', NC_ADDRESS, 1, (u8 *) &tDebug, sizeof(tDebug)); |
SendDebugOutput = 0; |