void WiFi_forward_to_xcsoar()
{
int bearing = CalcBearing(fo.latitude, fo.longtitude, LATITUDE, LONGTITUDE);
int alt_diff = fo.altitude - ALTITUDE;
char *csum_ptr;
unsigned char cs = 0; //clear any old checksum
Udp.beginPacket(ARGUS_HOSTNAME, XCSOAR_PORT);
snprintf(UDPpacketBuffer, sizeof(UDPpacketBuffer), "$PFLAU,0,1,1,1,%d,2,%d,%u,%X*", \
bearing, alt_diff, (int) fo.distance, fo.addr );
//calculate the checksum
for (unsigned int n = 1; n < strlen(UDPpacketBuffer) - 1; n++) {
cs ^= UDPpacketBuffer[n]; //calculates the checksum
}
csum_ptr = UDPpacketBuffer + strlen(UDPpacketBuffer);
snprintf(csum_ptr, sizeof(UDPpacketBuffer) - strlen(UDPpacketBuffer), "%02X\n", cs);
#ifdef SERIAL_VERBOSE
Serial.println(UDPpacketBuffer);
#endif
Udp.write(UDPpacketBuffer, strlen(UDPpacketBuffer));
Udp.endPacket();
Udp.beginPacket(XCSOAR_HOSTNAME, XCSOAR_PORT);
snprintf(UDPpacketBuffer, sizeof(UDPpacketBuffer), "$PFLAA,2,%d,%d,%d,2,%X,,,,,1*", \
(int) (fo.distance * cos(dtor(bearing))), (int) (fo.distance * sin(dtor(bearing))), \
alt_diff, fo.addr );
cs = 0; //clear any old checksum
for (unsigned int n = 1; n < strlen(UDPpacketBuffer) - 1; n++) {
cs ^= UDPpacketBuffer[n]; //calculates the checksum
}
csum_ptr = UDPpacketBuffer + strlen(UDPpacketBuffer);
snprintf(csum_ptr, sizeof(UDPpacketBuffer) - strlen(UDPpacketBuffer), "%02X\n", cs);
#ifdef SERIAL_VERBOSE
Serial.println(UDPpacketBuffer);
#endif
Udp.write(UDPpacketBuffer, strlen(UDPpacketBuffer));
Udp.endPacket();
}
int CalcAzimuth( double Lat1,
double Lon1,
double Heading1,
double Lat2,
double Lon2,
double Heading2,
double *pAzimuth )
{
int iResult = 0 ;
double Bearing ;
iResult |= CalcBearing( Lat1, Lon1, Lat2, Lon2, &Bearing ) ;
*pAzimuth = ReturnPlusMinus180( Bearing - Heading1 ) ;
return iResult ;
}
