683 lines
19 KiB
C
Executable File
683 lines
19 KiB
C
Executable File
//Copyright (c) 2013-2015 United States Government as represented by the Administrator of the
|
|
//National Aeronautics and Space Administration. All Rights Reserved.
|
|
//
|
|
//DISCLAIMERS
|
|
// No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF ANY KIND,
|
|
// EITHER EXPRESSED, IMPLIED, OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, ANY WARRANTY THAT
|
|
// THE SUBJECT SOFTWARE WILL CONFORM TO SPECIFICATIONS, ANY IMPLIED WARRANTIES OF
|
|
// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, OR FREEDOM FROM INFRINGEMENT, ANY WARRANTY
|
|
// THAT THE SUBJECT SOFTWARE WILL BE ERROR FREE, OR ANY WARRANTY THAT DOCUMENTATION, IF PROVIDED,
|
|
// WILL CONFORM TO THE SUBJECT SOFTWARE. THIS AGREEMENT DOES NOT, IN ANY MANNER, CONSTITUTE AN
|
|
// ENDORSEMENT BY GOVERNMENT AGENCY OR ANY PRIOR RECIPIENT OF ANY RESULTS, RESULTING DESIGNS,
|
|
// HARDWARE, SOFTWARE PRODUCTS OR ANY OTHER APPLICATIONS RESULTING FROM USE OF THE SUBJECT
|
|
// SOFTWARE. FURTHER, GOVERNMENT AGENCY DISCLAIMS ALL WARRANTIES AND LIABILITIES REGARDING
|
|
// THIRD-PARTY SOFTWARE, IF PRESENT IN THE ORIGINAL SOFTWARE, AND DISTRIBUTES IT "AS IS."
|
|
//
|
|
// Waiver and Indemnity: RECIPIENT AGREES TO WAIVE ANY AND ALL CLAIMS AGAINST THE UNITED STATES
|
|
// GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS, AS WELL AS ANY PRIOR RECIPIENT. IF
|
|
// RECIPIENT'S USE OF THE SUBJECT SOFTWARE RESULTS IN ANY LIABILITIES, DEMANDS, DAMAGES, EXPENSES
|
|
// OR LOSSES ARISING FROM SUCH USE, INCLUDING ANY DAMAGES FROM PRODUCTS BASED ON, OR RESULTING
|
|
// FROM, RECIPIENT'S USE OF THE SUBJECT SOFTWARE, RECIPIENT SHALL INDEMNIFY AND HOLD HARMLESS THE
|
|
// UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS, AS WELL AS ANY PRIOR RECIPIENT,
|
|
// TO THE EXTENT PERMITTED BY LAW. RECIPIENT'S SOLE REMEDY FOR ANY SUCH MATTER SHALL BE THE
|
|
// IMMEDIATE, UNILATERAL TERMINATION OF THIS AGREEMENT.
|
|
|
|
// X-Plane Connect Client
|
|
//
|
|
// DESCRIPTION
|
|
// Communicates with the XPC plugin to facilitate controling and gathering data from X-Plane.
|
|
//
|
|
// INSTRUCTIONS
|
|
// See Readme.md in the root of this repository or the wiki hosted on GitHub at
|
|
// https://github.com/nasa/XPlaneConnect/wiki for requirements, installation instructions,
|
|
// and detailed documentation.
|
|
//
|
|
// CONTACT
|
|
// For questions email Christopher Teubert (christopher.a.teubert@nasa.gov)
|
|
//
|
|
// CONTRIBUTORS
|
|
// CT: Christopher Teubert (christopher.a.teubert@nasa.gov)
|
|
// JW: Jason Watkins (jason.w.watkins@nasa.gov)
|
|
#include "xplaneConnect.h"
|
|
|
|
#include <math.h>
|
|
#include <stdarg.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <sys/types.h>
|
|
#include <time.h>
|
|
|
|
|
|
|
|
|
|
void printError(char *functionName, char *format, ...)
|
|
{
|
|
va_list args;
|
|
va_start(args, format);
|
|
|
|
printf("[%s] ERROR: ", functionName);
|
|
vprintf(format, args);
|
|
printf("\n");
|
|
|
|
va_end(args);
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**** Low Level UDP functions ****/
|
|
/*****************************************************************************/
|
|
XPCSocket openUDP(const char *xpIP)
|
|
{
|
|
return aopenUDP(xpIP, 49009, 0);
|
|
}
|
|
|
|
XPCSocket aopenUDP(const char *xpIP, unsigned short xpPort, unsigned short port)
|
|
{
|
|
XPCSocket sock;
|
|
|
|
// Setup Port
|
|
struct sockaddr_in recvaddr;
|
|
recvaddr.sin_family = AF_INET;
|
|
recvaddr.sin_addr.s_addr = INADDR_ANY;
|
|
recvaddr.sin_port = htons(port);
|
|
|
|
// Set X-Plane Port and IP
|
|
if (strcmp(xpIP, "localhost") == 0)
|
|
{
|
|
xpIP = "127.0.0.1";
|
|
}
|
|
strncpy(sock.xpIP, xpIP, 16);
|
|
sock.xpPort = xpPort == 0 ? 49009 : xpPort;
|
|
|
|
#ifdef _WIN32
|
|
WSADATA wsa;
|
|
if (WSAStartup(MAKEWORD(2, 2), &wsa) != 0)
|
|
{
|
|
printError("OpenUDP", "WSAStartup failed");
|
|
exit(EXIT_FAILURE);
|
|
}
|
|
#endif
|
|
|
|
if ((sock.sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1)
|
|
{
|
|
printError("OpenUDP", "Socket creation failed");
|
|
exit(EXIT_FAILURE);
|
|
}
|
|
if (bind(sock.sock, (struct sockaddr*)&recvaddr, sizeof(recvaddr)) == -1)
|
|
{
|
|
printError("OpenUDP", "Socket bind failed");
|
|
exit(EXIT_FAILURE);
|
|
}
|
|
|
|
// Set timeout to 100ms
|
|
#ifdef _WIN32
|
|
DWORD timeout = 100;
|
|
#else
|
|
struct timeval timeout;
|
|
timeout.tv_sec = 0;
|
|
timeout.tv_usec = 1000;
|
|
#endif
|
|
if (setsockopt(sock.sock, SOL_SOCKET, SO_RCVTIMEO, (char*)&timeout, sizeof(timeout)) < 0)
|
|
{
|
|
printError("OpenUDP", "Failed to set timeout");
|
|
}
|
|
return sock;
|
|
}
|
|
|
|
void closeUDP(XPCSocket sock)
|
|
{
|
|
#ifdef _WIN32
|
|
int result = closesocket(sock.sock);
|
|
#else
|
|
int result = close(sock.sock);
|
|
#endif
|
|
if (result < 0)
|
|
{
|
|
printError("closeUDP", "Failed to close socket");
|
|
exit(EXIT_FAILURE);
|
|
}
|
|
}
|
|
|
|
/// Sends the given data to the X-Plane plugin.
|
|
///
|
|
/// \param sock The socket to use to send the data.
|
|
/// \param buffer A pointer to the data to send.
|
|
/// \param len The number of bytes to send.
|
|
/// \returns If an error occurs, a negative number. Otehrwise, the number of bytes sent.
|
|
int sendUDP(XPCSocket sock, char buffer[], int len)
|
|
{
|
|
// Preconditions
|
|
if (len <= 0)
|
|
{
|
|
printError("sendUDP", "Message length must be positive.");
|
|
return -1;
|
|
}
|
|
|
|
// Set up destination address
|
|
struct sockaddr_in dst;
|
|
dst.sin_family = AF_INET;
|
|
dst.sin_port = htons(sock.xpPort);
|
|
inet_pton(AF_INET, sock.xpIP, &dst.sin_addr.s_addr);
|
|
|
|
int result = sendto(sock.sock, buffer, len, 0, (const struct sockaddr*)&dst, sizeof(dst));
|
|
if (result < 0)
|
|
{
|
|
printError("sendUDP", "Send operation failed.");
|
|
return -2;
|
|
}
|
|
if (result < len)
|
|
{
|
|
printError("sendUDP", "Unexpected number of bytes sent.");
|
|
}
|
|
return result;
|
|
}
|
|
|
|
/// Reads a datagram from the specified socket.
|
|
///
|
|
/// \param sock The socket to read from.
|
|
/// \param buffer A pointer to the location to store the data.
|
|
/// \param len The number of bytes to read.
|
|
/// \returns If an error occurs, a negative number. Otehrwise, the number of bytes read.
|
|
int readUDP(XPCSocket sock, char buffer[], int len)
|
|
{
|
|
#ifdef _WIN32
|
|
// Windows readUDP needs the select command- minimum timeout is 1ms.
|
|
// Without this playback becomes choppy
|
|
|
|
// Definitions
|
|
FD_SET stReadFDS;
|
|
FD_SET stExceptFDS;
|
|
|
|
// Setup for Select
|
|
FD_ZERO(&stReadFDS);
|
|
FD_SET(sock.sock, &stReadFDS);
|
|
FD_ZERO(&stExceptFDS);
|
|
FD_SET(sock.sock, &stExceptFDS);
|
|
|
|
struct timeval tv;
|
|
tv.tv_sec = 0;
|
|
tv.tv_usec = 100000;
|
|
|
|
// Select Command
|
|
int status = select(-1, &stReadFDS, (FD_SET*)0, &stExceptFDS, &tv);
|
|
if (status < 0)
|
|
{
|
|
printError("readUDP", "Select command error");
|
|
return -1;
|
|
}
|
|
if (status == 0)
|
|
{
|
|
// No data
|
|
return 0;
|
|
}
|
|
status = recv(sock.sock, buffer, len, 0);
|
|
#else
|
|
// For apple or linux-just read - will timeout in 0.5 ms
|
|
int status = (int)recv(sock.sock, buffer, len, 0);
|
|
#endif
|
|
if (status < 0)
|
|
{
|
|
printError("readUDP", "Error reading socket");
|
|
}
|
|
return status;
|
|
}
|
|
/*****************************************************************************/
|
|
/**** End Low Level UDP functions ****/
|
|
/*****************************************************************************/
|
|
|
|
/*****************************************************************************/
|
|
/**** Configuration functions ****/
|
|
/*****************************************************************************/
|
|
int setCONN(XPCSocket* sock, unsigned short port)
|
|
{
|
|
// Set up command
|
|
char buffer[32] = "CONN";
|
|
memcpy(&buffer[5], &port, 2);
|
|
|
|
// Send command
|
|
if (sendUDP(*sock, buffer, 7) < 0)
|
|
{
|
|
printError("setCONN", "Failed to send command");
|
|
return -1;
|
|
}
|
|
|
|
// Switch socket
|
|
closeUDP(*sock);
|
|
*sock = aopenUDP(sock->xpIP, sock->xpPort, port);
|
|
|
|
// Read response
|
|
int result = readUDP(*sock, buffer, 32);
|
|
|
|
if (result <= 0)
|
|
{
|
|
printError("setCONN", "Failed to read response");
|
|
return -2;
|
|
}
|
|
if (strncmp(buffer, "CONF", 4) == 0)
|
|
{
|
|
// Response received succesfully.
|
|
return 0;
|
|
}
|
|
// Response incorrect
|
|
return -3;
|
|
}
|
|
|
|
int pauseSim(XPCSocket sock, char pause)
|
|
{
|
|
// Validte input
|
|
if (pause < 0 || pause > 2)
|
|
{
|
|
printError("pauseSim", "Invalid argument: %i", pause);
|
|
return -2;
|
|
}
|
|
|
|
// Setup command
|
|
char buffer[6] = "SIMU";
|
|
buffer[5] = pause;
|
|
|
|
// Send command
|
|
if (sendUDP(sock, buffer, 6) < 0)
|
|
{
|
|
printError("pauseSim", "Failed to send command");
|
|
return -1;
|
|
}
|
|
return 0;
|
|
}
|
|
/*****************************************************************************/
|
|
/**** End Configuration functions ****/
|
|
/*****************************************************************************/
|
|
|
|
/*****************************************************************************/
|
|
/**** X-Plane UDP Data functions ****/
|
|
/*****************************************************************************/
|
|
int sendDATA(XPCSocket sock, float data[][9], int rows)
|
|
{
|
|
// Preconditions
|
|
// There are only 134 DATA rows in X-Plane. Realistically, clients probably
|
|
// shouldn't be trying to set nearly this much data at once anyway.
|
|
if (rows > 134)
|
|
{
|
|
printError("sendDATA", "Too many rows.");
|
|
return -1;
|
|
}
|
|
|
|
// Setup command
|
|
// 5 byte header + 134 rows * 9 values * 4 bytes per value => 4829 byte max length.
|
|
char buffer[4829] = "DATA";
|
|
int len = 5 + rows * 9 * sizeof(float);
|
|
unsigned short step = 9 * sizeof(float);
|
|
for (int i=0;i<rows;i++)
|
|
{
|
|
buffer[5 + i * step] = (char)data[i][0];
|
|
memcpy(&buffer[9 + i*step], &data[i][1], 8 * sizeof(float));
|
|
}
|
|
// Send command
|
|
if (sendUDP(sock, buffer, len ) < 0)
|
|
{
|
|
printError("sendDATA", "Failed to send command");
|
|
return -2;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int readDATA(XPCSocket sock, float data[][9], int rows)
|
|
{
|
|
// Preconditions
|
|
// There are only 134 DATA rows in X-Plane. Realistically, clients probably
|
|
// shouldn't be trying to read nearly this much data at once anyway.
|
|
if (rows > 134)
|
|
{
|
|
printError("sendDATA", "Too many rows.");
|
|
// Read as much as we can anyway
|
|
rows = 134;
|
|
}
|
|
|
|
// Read data
|
|
char buffer[4829] = { 0 };
|
|
int result = readUDP(sock, buffer, 5120);
|
|
if (result <= 0)
|
|
{
|
|
printError("readDATA", "Failed to read from socket.");
|
|
return -1;
|
|
}
|
|
// Validate data
|
|
int readRows = (result - 5) / 36;
|
|
if (readRows > rows)
|
|
{
|
|
printError("readDATA", "Read more rows than will fit in dataRef.");
|
|
// Copy as much data as we can anyway
|
|
rows = readRows;
|
|
}
|
|
|
|
// Parse data
|
|
for (int i = 0; i < rows; ++i)
|
|
{
|
|
data[i][0] = buffer[5 + i * 36];
|
|
memcpy(&data[i][1], &buffer[9 + i * 36], 8 * sizeof(float));
|
|
}
|
|
return rows;
|
|
}
|
|
/*****************************************************************************/
|
|
/**** End X-Plane UDP Data functions ****/
|
|
/*****************************************************************************/
|
|
|
|
/*****************************************************************************/
|
|
/**** DREF functions ****/
|
|
/*****************************************************************************/
|
|
int sendDREF(XPCSocket sock, const char* dref, float value[], int size)
|
|
{
|
|
// Setup command
|
|
// 5 byte header + max 255 char dref name + max 255 values * 4 bytes per value = 1279
|
|
unsigned char buffer[1279] = "DREF";
|
|
int drefLen = strnlen(dref, 256);
|
|
if (drefLen > 255)
|
|
{
|
|
printError("setDREF", "dref length is too long. Must be less than 256 characters.");
|
|
return -1;
|
|
}
|
|
if (size > 255)
|
|
{
|
|
printError("setDREF", "size is too big. Must be less than 256.");
|
|
return -2;
|
|
}
|
|
int len = 7 + drefLen + size * sizeof(float);
|
|
|
|
// Copy dref to buffer
|
|
buffer[5] = (unsigned char)drefLen;
|
|
memcpy(buffer + 6, dref, drefLen);
|
|
|
|
// Copy values to buffer
|
|
buffer[6 + drefLen] = (unsigned char)size;
|
|
memcpy(buffer + 7 + drefLen, value, size * sizeof(float));
|
|
|
|
// Send command
|
|
if (sendUDP(sock, buffer, len) < 0)
|
|
{
|
|
printError("setDREF", "Failed to send command");
|
|
return -3;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int sendDREFRequest(XPCSocket sock, const char* drefs[], unsigned char count)
|
|
{
|
|
// Setup command
|
|
// 6 byte header + potentially 255 drefs, each 256 chars long.
|
|
// Easiest to just round to an even 2^16.
|
|
unsigned char buffer[65536] = "GETD";
|
|
buffer[5] = count;
|
|
int len = 6;
|
|
for (int i = 0; i < count; ++i)
|
|
{
|
|
size_t drefLen = strnlen(drefs[i], 256);
|
|
if (drefLen > 255)
|
|
{
|
|
printError("getDREFs", "dref %d is too long.", i);
|
|
return -1;
|
|
}
|
|
buffer[len++] = (unsigned char)drefLen;
|
|
strncpy(buffer + len, drefs[i], drefLen);
|
|
len += drefLen;
|
|
}
|
|
// Send Command
|
|
if (sendUDP(sock, buffer, len) < 0)
|
|
{
|
|
printError("getDREFs", "Failed to send command");
|
|
return -2;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int getDREFResponse(XPCSocket sock, float* values[], unsigned char count, int sizes[])
|
|
{
|
|
unsigned char buffer[65536];
|
|
int result = readUDP(sock, buffer, 65536);
|
|
|
|
if (result < 0)
|
|
{
|
|
#ifdef _WIN32
|
|
printError("getDREFs", "Read operation failed. (%d)", WSAGetLastError());
|
|
#else
|
|
printError("getDREFs", "Read operation failed.");
|
|
#endif
|
|
return -1;
|
|
}
|
|
|
|
if (result < 6)
|
|
{
|
|
printError("getDREFs", "Response was too short. Expected at least 6 bytes, but only got %d.", result);
|
|
return -2;
|
|
}
|
|
if (buffer[5] != count)
|
|
{
|
|
printError("getDREFs", "Unexpected response size. Expected %d rows, got %d instead.", count, buffer[5]);
|
|
return -3;
|
|
}
|
|
|
|
int cur = 6;
|
|
for (int i = 0; i < count; ++i)
|
|
{
|
|
int l = buffer[cur++];
|
|
if (l > sizes[i])
|
|
{
|
|
printError("getDREFs", "values is too small. Row had %d values, only room for %d.", l, sizes[i]);
|
|
// Copy as many values as we can anyway
|
|
memcpy(values[i], buffer + cur, sizes[i] * sizeof(float));
|
|
}
|
|
else
|
|
{
|
|
memcpy(values[i], buffer + cur, l * sizeof(float));
|
|
sizes[i] = l;
|
|
}
|
|
cur += l * sizeof(float);
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int getDREF(XPCSocket sock, const char* dref, float values[], int* size)
|
|
{
|
|
return getDREFs(sock, &dref, &values, 1, size);
|
|
}
|
|
|
|
int getDREFs(XPCSocket sock, const char* drefs[], float* values[], unsigned char count, int sizes[])
|
|
{
|
|
// Send Command
|
|
int result = sendDREFRequest(sock, drefs, count);
|
|
if (result < 0)
|
|
{
|
|
// A error ocurred while sending.
|
|
// sendDREFRequest will print an error message, so just return.
|
|
return -1;
|
|
}
|
|
|
|
// Read Response
|
|
if (getDREFResponse(sock, values, count, sizes) < 0)
|
|
{
|
|
// A error ocurred while reading the response.
|
|
// getDREFResponse will print an error message, so just return.
|
|
return -2;
|
|
}
|
|
return 0;
|
|
}
|
|
/*****************************************************************************/
|
|
/**** End DREF functions ****/
|
|
/*****************************************************************************/
|
|
|
|
/*****************************************************************************/
|
|
/**** POSI functions ****/
|
|
/*****************************************************************************/
|
|
int sendPOSI(XPCSocket sock, float values[], int size, char ac)
|
|
{
|
|
// Validate input
|
|
if (ac < 0 || ac > 20)
|
|
{
|
|
printError("sendPOSI", "aircraft should be a value between 0 and 20.");
|
|
return -1;
|
|
}
|
|
if (size < 1 || size > 7)
|
|
{
|
|
printError("sendPOSI", "size should be a value between 1 and 7.");
|
|
return -2;
|
|
}
|
|
|
|
// Setup command
|
|
// 5 byte header + up to 7 values * 5 bytes each
|
|
unsigned char buffer[40] = "POSI";
|
|
buffer[5] = ac;
|
|
for (int i = 0; i < 7; i++)
|
|
{
|
|
float val = -998;
|
|
|
|
if (i < size)
|
|
{
|
|
val = values[i];
|
|
}
|
|
*((float*)(buffer + 6 + i * 4)) = val;
|
|
}
|
|
|
|
// Send Command
|
|
if (sendUDP(sock, buffer, 40) < 0)
|
|
{
|
|
printError("sendPOSI", "Failed to send command");
|
|
return -3;
|
|
}
|
|
return 0;
|
|
}
|
|
/*****************************************************************************/
|
|
/**** End POSI functions ****/
|
|
/*****************************************************************************/
|
|
|
|
/*****************************************************************************/
|
|
/**** CTRL functions ****/
|
|
/*****************************************************************************/
|
|
int sendCTRL(XPCSocket sock, float values[], int size, char ac)
|
|
{
|
|
// Validate input
|
|
if (ac < 0 || ac > 20)
|
|
{
|
|
printError("sendCTRL", "aircraft should be a value between 0 and 20.");
|
|
return -1;
|
|
}
|
|
if (size < 1 || size > 7)
|
|
{
|
|
printError("sendCTRL", "size should be a value between 1 and 7.");
|
|
return -2;
|
|
}
|
|
|
|
// Setup Command
|
|
// 5 byte header + 5 float values * 4 + 2 byte values
|
|
unsigned char buffer[31] = "CTRL";
|
|
int cur = 5;
|
|
for (int i = 0; i < 6; i++)
|
|
{
|
|
float val = -998;
|
|
|
|
if (i < size)
|
|
{
|
|
val = values[i];
|
|
}
|
|
if (i == 4)
|
|
{
|
|
buffer[cur++] = val == -998 ? -1 : (unsigned char)val;
|
|
}
|
|
else
|
|
{
|
|
*((float*)(buffer + cur)) = val;
|
|
cur += sizeof(float);
|
|
}
|
|
}
|
|
buffer[26] = ac;
|
|
*((float*)(buffer + 27)) = size == 7 ? values[6]: -998;
|
|
|
|
// Send Command
|
|
if (sendUDP(sock, buffer, 31) < 0)
|
|
{
|
|
printError("sendCTRL", "Failed to send command");
|
|
return -3;
|
|
}
|
|
return 0;
|
|
}
|
|
/*****************************************************************************/
|
|
/**** End CTRL functions ****/
|
|
/*****************************************************************************/
|
|
|
|
/*****************************************************************************/
|
|
/**** Drawing functions ****/
|
|
/*****************************************************************************/
|
|
int sendTEXT(XPCSocket sock, char* msg, int x, int y)
|
|
{
|
|
if (msg == NULL)
|
|
{
|
|
msg = "";
|
|
}
|
|
size_t msgLen = strnlen(msg, 255);
|
|
// Input Validation
|
|
if (x < -1)
|
|
{
|
|
printError("sendTEXT", "x should be positive (or -1 for default).");
|
|
// Technically, this should work, and may print something to the screen.
|
|
}
|
|
if (y < -1)
|
|
{
|
|
printError("sendTEXT", "y should be positive (or -1 for default).");
|
|
// Negative y will never result in text being displayed.
|
|
return -1;
|
|
}
|
|
if (msgLen > 255)
|
|
{
|
|
printError("sendTEXT", "msg must be less than 255 bytes.");
|
|
return -2;
|
|
}
|
|
|
|
// Setup command
|
|
// 5 byte header + 8 byte position + up to 256 byte message
|
|
char buffer[269] = "TEXT";
|
|
size_t len = 14 + msgLen;
|
|
memcpy(buffer + 5, &x, sizeof(int));
|
|
memcpy(buffer + 9, &y, sizeof(int));
|
|
buffer[13] = msgLen;
|
|
strncpy(buffer + 14, msg, msgLen);
|
|
|
|
// Send Command
|
|
if (sendUDP(sock, buffer, len) < 0)
|
|
{
|
|
printError("sendTEXT", "Failed to send command");
|
|
return -3;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int sendWYPT(XPCSocket sock, WYPT_OP op, float points[], int count)
|
|
{
|
|
// Input Validation
|
|
if (op < XPC_WYPT_ADD || op > XPC_WYPT_CLR)
|
|
{
|
|
printError("sendWYPT", "Unrecognized operation.");
|
|
return -1;
|
|
}
|
|
if (count > 255)
|
|
{
|
|
printError("sendWYPT", "Too many points. Must be less than 256.");
|
|
return -2;
|
|
}
|
|
|
|
// Setup Command
|
|
// 7 byte header + 12 bytes * count
|
|
char buffer[3067] = "WYPT";
|
|
buffer[5] = (unsigned char)op;
|
|
buffer[6] = (unsigned char)count;
|
|
size_t ptLen = sizeof(float) * 3 * count;
|
|
memcpy(buffer + 7, points, ptLen);
|
|
|
|
// Send Command
|
|
if (sendUDP(sock, buffer, 40) < 0)
|
|
{
|
|
printError("sendWYPT", "Failed to send command");
|
|
return -2;
|
|
}
|
|
return 0;
|
|
}
|
|
/*****************************************************************************/
|
|
/**** End Drawing functions ****/
|
|
/*****************************************************************************/
|