diff --git a/src/libsrc/XPlaneBeaconListener.cpp b/src/libsrc/XPlaneBeaconListener.cpp index d5d6229..efb6fc7 100644 --- a/src/libsrc/XPlaneBeaconListener.cpp +++ b/src/libsrc/XPlaneBeaconListener.cpp @@ -21,20 +21,44 @@ * */ +#define _DEFAULT_SOURCE + #include #include + +#ifdef _WIN32 +#include +#include +#include +#define ERRNO WSAGetLastError() +#else #include -#include -#include #include #include -#include +//typedef int socklen_t; +#define ERRNO errno +#define WSAETIMEDOUT 10060 +#endif +#include +#include #include #include #include #include #include +#ifdef __linux__ #include +#else +inline void syslog(int /*prio*/, const char */*fmt*/, ...) {} // TODO Windows Syslog not supported +#define LOG_EMERG 0 /* system is unusable */ +#define LOG_ALERT 1 /* action must be taken immediately */ +#define LOG_CRIT 2 /* critical conditions */ +#define LOG_ERR 3 /* error conditions */ +#define LOG_WARNING 4 /* warning conditions */ +#define LOG_NOTICE 5 /* normal but significant condition */ +#define LOG_INFO 6 /* informational */ +#define LOG_DEBUG 7 /* debug-level messages */ +#endif #include "XPlaneBeaconListener.h" @@ -44,235 +68,330 @@ using namespace std; XPlaneBeaconListener * XPlaneBeaconListener::instance = NULL; XPlaneBeaconListener::XPlaneBeaconListener() { - - debug = 0; - quitFlag = false; - isRunning = false; - std::thread t(&XPlaneBeaconListener::runListener, this); - t.detach(); - + cout << __PRETTY_FUNCTION__ << endl; + fflush(stdout); + debug = 0; + quitFlag = false; + isRunning = false; + worker_thread = new std::thread(&XPlaneBeaconListener::runListener, this); } -XPlaneBeaconListener::~XPlaneBeaconListener() { - - quitFlag = true; - time_t nowTime = time(NULL); - - while (isRunning && nowTime < time(NULL) - 5) { - if (debug) { - cerr << "waiting for XPlaneBeaconListener to stop" << endl; - } - } - - if (isRunning) { - cerr << "... XPlaneBeaconListener failed to stop within 5 seconds." - << endl; - } +XPlaneBeaconListener::~XPlaneBeaconListener() +{ + setDebug(true); + quitFlag = true; + time_t nowTime = time(NULL); + cerr << "Waiting for XPlaneBeaconListener to stop: " << endl; + fflush(stderr); + if(worker_thread!=nullptr && worker_thread->joinable()) + worker_thread->join(); } void XPlaneBeaconListener::runListener() { - // https://web.cs.wpi.edu/~claypool/courses/4514-B99/samples/multicast.c - // http://ntrg.cs.tcd.ie/undergrad/4ba2/multicast/antony/example.html - - struct sockaddr_in addr; - socklen_t addrlen; - int sock, recv_len; - struct ip_mreq mreq; - char message[1024]; - - /* set up socket */ - sock = socket(AF_INET, SOCK_DGRAM, 0); - if (sock < 0) { - perror("socket"); - exit(1); - } - bzero((char *) &addr, sizeof(addr)); - addr.sin_family = AF_INET; - addr.sin_addr.s_addr = htonl(INADDR_ANY); - addr.sin_port = htons(49707); - addrlen = sizeof(addr); - - /* allow multiple sockets to use the same ADDR */ - u_int yes = 1; - if (setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &yes, sizeof(yes)) < 0) { - ostringstream buf; - buf << "XPlaneBeaconListener: Reusing ADDR failed: " << strerror(errno); - throw runtime_error(buf.str()); - } - - /* allow multiple sockets to use the same ADDR */ - if (setsockopt(sock, SOL_SOCKET, SO_REUSEPORT, &yes, sizeof(yes)) < 0) { - ostringstream buf; - buf << "XPlaneBeaconListener: Reusing PORT failed: " << strerror(errno); - throw runtime_error(buf.str()); - } - - /* timeout after 1 second if no data received */ - struct timeval tv; - tv.tv_sec = 1; - tv.tv_usec = 0; - if (setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, (struct timeval *) &tv, - sizeof(struct timeval))) { - ostringstream buf; - buf << "XPlaneBeaconListener: set SO_RCVTIMEO failed: " - << strerror(errno); - throw runtime_error(buf.str()); - } - - /* receive */ - if (bind(sock, (struct sockaddr *) &addr, sizeof(addr)) < 0) { - ostringstream buf; - buf << "XPlaneBeaconListener: bind failed: " << strerror(errno); - throw runtime_error(buf.str()); - } - - /* subscribe multicast - * - * Under systemd on raspbian multicast is sometimes is not available - * even though "network-online.target" has been satisfied. This - * retries for up to 5 seconds before giving up. - */ - - mreq.imr_multiaddr.s_addr = inet_addr("239.255.1.1"); - mreq.imr_interface.s_addr = htonl(INADDR_ANY); - int result; - int retry = 0; - - do { - result = setsockopt(sock, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)); - if (result < 0) { - retry++; - syslog (LOG_INFO, "Retrying subscribe to multicast (attempt %d)", retry); - sleep (1); - } - } while (result < 0 && retry < 5); - if (retry < 0) { - ostringstream buf; - buf << "XPlaneBeaconListner: setsockopt IP_ADD_MEMBERSHIP failed: " - << strerror(errno); - throw runtime_error(buf.str()); - } - - time_t lastExpiredCheck = time(NULL); - - isRunning = true; - while (!quitFlag) { - - recv_len = recvfrom(sock, message, sizeof(message), 0, - (struct sockaddr *) &addr, &addrlen); - time_t nowTime = time(NULL); - - if (recv_len < 0) { - - if (errno != EWOULDBLOCK) { - ostringstream buf; - buf << "recvfrom returned " << recv_len << " errno is " << errno - << endl; - throw runtime_error(buf.str()); - }; - - } else if (recv_len == 0) { - break; - } - - - - else if (recv_len > 5 && memcmp(message, "BECN", 5) == 0) { - - // parse the message - - XPlaneServer s(nowTime, message, inet_ntoa(addr.sin_addr)); - - cachedServersMutex.lock(); - ostringstream key; - key << s.host << ":" << s.name << ":" << s.receivePort; - - if (cachedServers.find(key.str()) == cachedServers.end()) { - for (auto callback : callbacks) { - callback(s, true); - } - } - cachedServers[key.str()] = s; - cachedServersMutex.unlock(); - - }; - - // Check the list of servers we have for expired servers. - if (lastExpiredCheck < nowTime) { - checkForExpiredServers(); - lastExpiredCheck = nowTime; - } - } - - // exit requested. - close(sock); - isRunning = false; + // https://web.cs.wpi.edu/~claypool/courses/4514-B99/samples/multicast.c + // http://ntrg.cs.tcd.ie/undergrad/4ba2/multicast/antony/example.html + cerr << "Start " << __PRETTY_FUNCTION__ << endl; + fflush(stderr); + struct sockaddr_in addr; + socklen_t addrlen; + int sock, recv_len; + struct ip_mreq mreq; + char message[1024]; + + /* set up socket */ + sock = socket(AF_INET, SOCK_DGRAM, 0); + if (sock < 0) { + perror("socket"); + exit(1); + } + memset((char *) &addr, 0, sizeof(addr)); + addr.sin_family = AF_INET; + addr.sin_addr.s_addr = htonl(INADDR_ANY); + addr.sin_port = htons(49707); + addrlen = sizeof(addr); + + /* allow multiple sockets to use the same ADDR */ +#ifdef _WIN32 + char yes = 1; +#else + u_int yes = 1; +#endif + + if(debug) + { + cerr << "1: " << __PRETTY_FUNCTION__ << endl; + fflush(stderr); + } + + if (setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &yes, sizeof(yes)) < 0) { + ostringstream buf; + buf << "XPlaneBeaconListener: Reusing ADDR failed: " << strerror(ERRNO); + throw (buf.str()); + } + + if(debug) + { + cerr << "2: " << __PRETTY_FUNCTION__ << endl; + fflush(stderr); + } + // SO_REUSEPORT not needed on WIN32 +#ifndef _WIN32 + /* allow multiple sockets to use the same ADDR */ + if (setsockopt(sock, SOL_SOCKET, SO_REUSEPORT, &yes, sizeof(yes)) < 0) { + ostringstream buf; + buf << "XPlaneBeaconListener: Reusing PORT failed: " << strerror(ERRNO); + throw runtime_error(buf.str()); + } +#endif + + /* timeout after 1 second if no data received */ +#ifdef _WIN32 + DWORD tv; + tv = 10000; + cerr << "timeout : " << tv << endl; + fflush(stderr); + if (setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO,(char *) &tv, + sizeof(tv))) { + ostringstream buf; + buf << "XPlaneBeaconListener: set SO_RCVTIMEO failed: " + << strerror(ERRNO); + throw runtime_error(buf.str()); + } +#else + struct timeval tv; + tv.tv_sec = 1; + tv.tv_usec = 0; + if (setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, (struct timeval *) &tv, + sizeof(struct timeval))) { + ostringstream buf; + buf << "XPlaneBeaconListener: set SO_RCVTIMEO failed: " + << strerror(ERRNO); + throw runtime_error(buf.str()); + } +#endif + cerr << "3: " << __PRETTY_FUNCTION__ << endl; + fflush(stderr); + + /* receive */ + if (bind(sock, (struct sockaddr *) &addr, sizeof(addr)) < 0) { + ostringstream buf; + buf << "XPlaneBeaconListener: bind failed. ERRNO: " << ERRNO << " : " << strerror(ERRNO); + throw runtime_error(buf.str()); + } + + /* subscribe multicast + * + * Under systemd on raspbian multicast is sometimes is not available + * even though "network-online.target" has been satisfied. This + * retries for up to 5 seconds before giving up. + */ + + mreq.imr_multiaddr.s_addr = inet_addr("239.255.1.1"); + mreq.imr_interface.s_addr = htonl(INADDR_ANY); + int result; + int retry = 0; + + do { + result = setsockopt(sock, IPPROTO_IP, IP_ADD_MEMBERSHIP, (const char*) &mreq, sizeof(mreq)); + if (result < 0) { + retry++; + syslog (LOG_INFO, "Retrying subscribe to multicast (attempt %d)", retry); + cerr << "Retrying subscribe to multicast (attempt " << retry << ")" << endl; + fflush(stderr); + sleep (1); + } + } while (result < 0 && retry < 5); + + if (retry < 0) { + ostringstream buf; + buf << "XPlaneBeaconListner: setsockopt IP_ADD_MEMBERSHIP failed: " + << strerror(ERRNO); + cerr << "XPlaneBeaconListner: setsockopt IP_ADD_MEMBERSHIP failed: " + << strerror(ERRNO) << " retry: " << retry << endl; + fflush(stderr); + throw runtime_error(buf.str()); + } + + + time_t lastExpiredCheck = time(NULL); + + isRunning = true; + while (!quitFlag) + { + if(debug) + { + cerr << "loop " << __PRETTY_FUNCTION__ << endl; + fflush(stderr); + } + recv_len = recvfrom(sock, message, sizeof(message), 0, + (struct sockaddr *) &addr, &addrlen); + time_t nowTime = time(NULL); + + if(debug) + { + cerr << "loop2 " << __PRETTY_FUNCTION__ << endl; + fflush(stderr); + } + + if (recv_len < 0) + { + if(debug) + { + cerr << "loop3 " << __PRETTY_FUNCTION__ << endl; + fflush(stderr); + } + if(ERRNO == EINTR) + continue; //see http://250bpm.com/blog:12 + if (ERRNO != EWOULDBLOCK && ERRNO != EAGAIN && ERRNO != WSAETIMEDOUT) + { + if(debug) + { + cerr << "loop3.1 " << ERRNO << __PRETTY_FUNCTION__ << endl; + cerr << "recvfrom returned " << recv_len << " ERRNO is " << ERRNO << endl; + } + + fflush(stderr); + ostringstream buf; + buf << "recvfrom returned " << recv_len << " errno is " << ERRNO + << endl; + throw runtime_error(buf.str()); + } + } + else if (recv_len == 0) + { + break; + } + else if (recv_len > 5 && memcmp(message, "BECN", 5) == 0) + { + if(debug) + { + cerr << "loop4 " << __PRETTY_FUNCTION__ << endl; + fflush(stderr); + } + // parse the message + + XPlaneServer s(nowTime, message, inet_ntoa(addr.sin_addr)); + + if(debug) + { + cerr << "loop5 " << __PRETTY_FUNCTION__ << endl; + fflush(stderr); + } + cachedServersMutex.lock(); + ostringstream key; + key << s.host << ":" << s.name << ":" << s.receivePort; + + if (cachedServers.find(key.str()) == cachedServers.end()) { + for (auto callback : callbacks) { + callback(s, true); + } + } + cachedServers[key.str()] = s; + cachedServersMutex.unlock(); + if(debug) + { + cerr << "loop6 " << __PRETTY_FUNCTION__ << endl; + fflush(stderr); + } + } + + // Check the list of servers we have for expired servers. + if (lastExpiredCheck < nowTime) + { + if(debug) + { + cerr << "loop7 " << __PRETTY_FUNCTION__ << endl; + fflush(stderr); + } + checkForExpiredServers(); + lastExpiredCheck = nowTime; + } + } + + // exit requested. + if(debug) + { + cerr << "loop8 " << __PRETTY_FUNCTION__ << endl; + fflush(stderr); + } + if(sock > 0) + { + cerr << "Shutdown sock: " << sock << endl; + fflush(stderr); + shutdown(sock,2); + } + isRunning = false; + cerr << "End " << __PRETTY_FUNCTION__ << endl; + fflush(stderr); } XPlaneBeaconListener::XPlaneServer::XPlaneServer(time_t time, char * msg, - char * _host) { + char * _host) { - prologue = msg; + prologue = msg; - received = time; - memcpy(&beaconMajorVersion, msg + 5, 1); - memcpy(&beaconMinorVersion, msg + 6, 1); - memcpy(&applicationHostId, msg + 7, 4); - memcpy(&versionNumber, msg + 11, 4); - memcpy(&role, msg + 15, 4); - memcpy(&receivePort, msg + 19, 2); - name = (char *) msg + 21; - host = _host; + received = time; + memcpy(&beaconMajorVersion, msg + 5, 1); + memcpy(&beaconMinorVersion, msg + 6, 1); + memcpy(&applicationHostId, msg + 7, 4); + memcpy(&versionNumber, msg + 11, 4); + memcpy(&role, msg + 15, 4); + memcpy(&receivePort, msg + 19, 2); + name = (char *) msg + 21; + host = _host; } std::string XPlaneBeaconListener::XPlaneServer::toString() { - ostringstream s; - s << "XPlaneServer [ prologue: " << prologue << " beaconMajorVersion:" - << (int) beaconMajorVersion << " beaconMinorVersion:" - << (int) beaconMinorVersion << " applicationHostID:" - << applicationHostId << " versionNumber:" << versionNumber - << " role:" << role << " receivePort: " << receivePort << " name:" - << name << " host:" << host << "]" << endl; - return s.str(); + ostringstream s; + s << "XPlaneServer [ prologue: " << prologue << " beaconMajorVersion:" + << (int) beaconMajorVersion << " beaconMinorVersion:" + << (int) beaconMinorVersion << " applicationHostID:" + << applicationHostId << " versionNumber:" << versionNumber + << " role:" << role << " receivePort: " << receivePort << " name:" + << name << " host:" << host << "]" << endl; + return s.str(); } void XPlaneBeaconListener::get( - std::list & ret) { + std::list & ret) { - cachedServersMutex.lock(); - ret.clear(); - for (auto server : cachedServers) { - ret.push_back(server.second); - } + cachedServersMutex.lock(); + ret.clear(); + for (auto server : cachedServers) { + ret.push_back(server.second); + } - cachedServersMutex.unlock(); + cachedServersMutex.unlock(); } void XPlaneBeaconListener::registerNotificationCallback( - std::function callback) { - callbacks.push_back(callback); + std::function callback) { + callbacks.push_back(callback); } -void XPlaneBeaconListener::checkForExpiredServers() { - - cachedServersMutex.lock(); - time_t nowTime = time(NULL); - for (auto it = cachedServers.cbegin(); it != cachedServers.cend();) { - // see if it has expired, i.e. we haven't received any beacons in last 30 seconds. - if (it->second.received < nowTime - 30) { - for (auto callback : callbacks) { - callback(it->second, false); - } - cachedServers.erase(it++); // or "it = m.erase(it)" since C++11 - } else { - ++it; - } - } - cachedServersMutex.unlock(); - +void XPlaneBeaconListener::checkForExpiredServers() +{ + if(quitFlag) + { + return; + } + cachedServersMutex.lock(); + time_t nowTime = time(NULL); + for (auto it = cachedServers.cbegin(); it != cachedServers.cend();) { + // see if it has expired, i.e. we haven't received any beacons in last 30 seconds. + if (it->second.received < nowTime - 30) { + for (auto callback : callbacks) { + callback(it->second, false); + } + cachedServers.erase(it++); // or "it = m.erase(it)" since C++11 + } else { + ++it; + } + } + cachedServersMutex.unlock(); } diff --git a/src/libsrc/XPlaneBeaconListener.h b/src/libsrc/XPlaneBeaconListener.h index ed57dd7..25728fd 100644 --- a/src/libsrc/XPlaneBeaconListener.h +++ b/src/libsrc/XPlaneBeaconListener.h @@ -31,6 +31,7 @@ #include #include #include +#include /** All running X-Plane instances broadcast their presence on the LANs they @@ -65,6 +66,7 @@ class XPlaneBeaconListener { XPlaneBeaconListener(); // private so it cannot be called XPlaneBeaconListener(XPlaneBeaconListener const &) {}; // private so it cannot be called XPlaneBeaconListener & operator= (XPlaneBeaconListener const &) { abort();}; + std::thread * worker_thread; public: @@ -97,9 +99,7 @@ class XPlaneBeaconListener { } virtual ~XPlaneBeaconListener(); - void get (std::list & ret); - void registerNotificationCallback (std::function callback); protected: diff --git a/src/libsrc/XPlaneUDPClient.cpp b/src/libsrc/XPlaneUDPClient.cpp index da6e3cb..32a23df 100644 --- a/src/libsrc/XPlaneUDPClient.cpp +++ b/src/libsrc/XPlaneUDPClient.cpp @@ -24,11 +24,22 @@ #include #include #include + +#ifdef _WIN32 +#include +#include +#include +#define ERRNO WSAGetLastError() +#else #include #include #include #include #include +#define ERRNO errno +#define WSAETIMEDOUT 10060 +#endif + #include #include #include @@ -42,575 +53,714 @@ using namespace std; XPlaneUDPClient::SubscribedDataRef::SubscribedDataRef(std::string _dataRef, - uint32_t _en, int _minFreq, bool _emitable, - SubscribedCharArray * _sca) { + uint32_t _en, int _minFreq, bool _emitable, + SubscribedCharArray * _sca) { - dataRef = _dataRef; - en = _en; - minFreq = _minFreq; + cerr << __PRETTY_FUNCTION__ << endl; - // defaults - emitable = _emitable; - partOfCharArray = _sca; - first = true; - value = -12345; + dataRef = _dataRef; + en = _en; + minFreq = _minFreq; - // if part of a char array, add the en to the list - if (_sca != NULL) { - _sca->addToEnList(_en); - } + // defaults + emitable = _emitable; + partOfCharArray = _sca; + first = true; + value = -12345; - ts = 0; - value = 0; + // if part of a char array, add the en to the list + if (_sca != NULL) { + _sca->addToEnList(_en); + } - // extract array index out of dataRef if there is one. - regex r("^.*\\[(\\d+)\\]$"); - smatch matches; + ts = 0; + value = 0; - if (regex_match(_dataRef, matches, r)) { - arrIndex = stoi(matches[1]); - } + // extract array index out of dataRef if there is one. + regex r("^.*\\[(\\d+)\\]$"); + smatch matches; + if (regex_match(_dataRef, matches, r)) { + arrIndex = stoi(matches[1]); + } } std::string XPlaneUDPClient::SubscribedDataRef::getDataRefName() { - return dataRef; + return dataRef; } uint32_t XPlaneUDPClient::SubscribedDataRef::getEn() { - return en; + return en; } uint32_t XPlaneUDPClient::SubscribedDataRef::getFreq() { - return minFreq; + return minFreq; } void XPlaneUDPClient::SubscribedDataRef::setFreq(uint32_t _minFreq) { - minFreq = _minFreq; + minFreq = _minFreq; } // ============================================================================= XPlaneUDPClient::XPlaneUDPClient(std::string _server, uint16_t _port, - std::function _receiverCallbackFloat, - std::function _receiverCallbackString) { - - server = _server; - port = _port; - receiverCallbackFloat = _receiverCallbackFloat; - receiverCallbackString = _receiverCallbackString; - lastUnsubscribeCheck = 0L; - quitFlag = false; - isRunning = false; - debug = 0; - - lastEn = 0; - - // setup socket - sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); - if (sock == -1) { - throw runtime_error("Unable to create UDP socket"); - } - - struct sockaddr_in srcaddr; - memset((char *) &srcaddr, 0, sizeof(srcaddr)); - srcaddr.sin_family = AF_INET; - srcaddr.sin_addr.s_addr = htonl(INADDR_ANY); - srcaddr.sin_port = htons(0); - - struct timeval tv; - tv.tv_sec = 1; - tv.tv_usec = 0; - if (setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, (struct timeval *) &tv, - sizeof(struct timeval))) { - perror("setsockopt: rcvtimeo"); - exit(1); - } - - // bind - if (bind(sock, (struct sockaddr *) &srcaddr, sizeof(srcaddr)) < 0) { - throw runtime_error("Unable to bind socket"); - } - - // setup dest address - memset((void *) &serverAddr, 0, sizeof(serverAddr)); - serverAddr.sin_family = AF_INET; - serverAddr.sin_addr.s_addr = inet_addr(server.c_str()); - serverAddr.sin_port = htons(port); - slen = sizeof(serverAddr); - - // launch listener thread - std::thread t(&XPlaneUDPClient::listenerThread, this); - t.detach(); + std::function _receiverCallbackFloat, + std::function _receiverCallbackString) { + + server = _server; + port = _port; + receiverCallbackFloat = _receiverCallbackFloat; + receiverCallbackString = _receiverCallbackString; + lastUnsubscribeCheck = 0L; + quitFlag = false; + isRunning = false; + + debug = 0; + + lastEn = 0; + + // setup socket + sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); + if (sock == -1) { + throw runtime_error("Unable to create UDP socket"); + } + + struct sockaddr_in srcaddr; + memset((char *) &srcaddr, 0, sizeof(srcaddr)); + srcaddr.sin_family = AF_INET; + srcaddr.sin_addr.s_addr = htonl(INADDR_ANY); + srcaddr.sin_port = htons(0); + +#ifdef _WIN32 + DWORD tv; + tv = 1000; + if (setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, (const char *) &tv, + sizeof(tv))) { + perror("setsockopt: rcvtimeo"); + //exit(1); + } +#else + struct timeval tv; + tv.tv_sec = 1; + tv.tv_usec = 0; + if (setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, (struct timeval *) &tv, + sizeof(struct timeval))) { + perror("setsockopt: rcvtimeo"); + exit(1); + } +#endif + // bind + if (bind(sock, (struct sockaddr *) &srcaddr, sizeof(srcaddr)) < 0) { + throw runtime_error("Unable to bind socket"); + } + + // setup dest address + memset((void *) &serverAddr, 0, sizeof(serverAddr)); + serverAddr.sin_family = AF_INET; + serverAddr.sin_addr.s_addr = inet_addr(server.c_str()); + serverAddr.sin_port = htons(port); + slen = sizeof(serverAddr); + + // launch listener thread + std::thread t(&XPlaneUDPClient::listenerThread, this); + t.detach(); } XPlaneUDPClient::~XPlaneUDPClient() { - - quitFlag = true; - - time_t nowTime = time(NULL); - - while (isRunning && time(NULL) < nowTime + 5) { - if (debug) { - cerr << "waiting for XPlaneUDPClient to stop" << endl; - } - usleep (100000); // 10ms - } - - if (isRunning) { - cerr << "... XPlaneUDPClient failed to stop within 5 seconds." << endl; - } else { - - // unsubscribe from everything - dataRefMutex.lock(); - for (auto dataRef : dataRefs) { - char buf[413]; - memset(buf, 0, sizeof(buf)); - - uint32_t zero = 0; - uint32_t en = dataRef->getEn(); - strcpy(buf, "RREF"); - memcpy(buf + 5, &zero, 4); - memcpy(buf + 9, &en, 4); - - sendto(sock, (void *) buf, sizeof(buf), 0, - (struct sockaddr *) &serverAddr, slen); - } - - dataRefs.clear(); - dataRefByNameIndex.clear(); - dataRefByEnIndex.clear(); - dataRefMutex.unlock(); - } - - close(sock); + cerr << __PRETTY_FUNCTION__ << " Begin!\n"; + + quitFlag = true; + + time_t nowTime = time(NULL); + + while (isRunning && time(NULL) < nowTime + 5) { + if (debug) { + cerr << "waiting for XPlaneUDPClient to stop" << endl; + } + usleep (100000); // 10ms + } + + if (isRunning) { + cerr << "... XPlaneUDPClient failed to stop within 5 seconds." << endl; + } else { + + if (true || debug) { + cerr << "Unsubscribe from "; // << endl; + } + // unsubscribe from everything + dataRefMutex.lock(); + for (auto dataRef : dataRefs) { + if (true || debug) + cout << dataRef->getDataRefName() << " "; + char buf[413]; + memset(buf, 0, sizeof(buf)); + + uint32_t zero = 0; + uint32_t en = dataRef->getEn(); + strcpy(buf, "RREF"); + memcpy(buf + 5, &zero, 4); + memcpy(buf + 9, &en, 4); + + sendto(sock, (void *) buf, sizeof(buf), 0, + (struct sockaddr *) &serverAddr, slen); + } + if (debug) + cout << " DataRef." << endl; + try { + if (debug) + cerr << "Trying dataRefs.clear()\n"; + dataRefs.clear(); + if (debug) + cerr << "Trying dataRefByNameIndex.clear()\n"; + dataRefByNameIndex.clear(); + if (debug) + cerr << "Trying dataRefByEnIndex.clear()\n"; + dataRefByEnIndex.clear(); + } catch (...) { + cerr << "Clear unsuccessful!\n"; + } + dataRefMutex.unlock(); + } + + if(sock > 0) + { + if (debug) + cerr << "Now close socket: " << sock << endl; + close(sock); + } + cerr << __PRETTY_FUNCTION__ << " end!\n"; } void XPlaneUDPClient::listenerThread() { - if (debug) { - cerr << "In XPlaneUDPClient listenerThread" << endl; - } - - uint8_t buf[1024]; - struct sockaddr remoteAddr; - socklen_t slen = sizeof(remoteAddr); - int recv_len; - isRunning = true; - while (!quitFlag) { - - // this will timeout after 1 second if nothing is received, due to - // the socket option set earlier. + if (debug) { + cerr << "In XPlaneUDPClient listenerThread" << endl; + } - if ((recv_len = recvfrom(sock, buf, sizeof(buf), 0, - (struct sockaddr *) &remoteAddr, &slen)) < 0) { + uint8_t buf[1024]; + struct sockaddr remoteAddr; + socklen_t slen = sizeof(remoteAddr); + int recv_len; + isRunning = true; + while (!quitFlag) { - if (errno != EWOULDBLOCK) { - ostringstream buf; - buf << "recvfrom returned " << recv_len << " errno is " << errno - << endl; - throw runtime_error(buf.str()); - }; + // this will timeout after 1 second if nothing is received, due to + // the socket option set earlier. - }; + if ((recv_len = recvfrom(sock, buf, sizeof(buf), 0, + (struct sockaddr *) &remoteAddr, &slen)) < 0) { - time_t nowTime = time(NULL); - - // check for RREF - - if (recv_len >= 4 && buf[0] == 'R' && buf[1] == 'R' && buf[2] == 'E' - && buf[3] == 'F') { - - // 9, 10, 11, 12 = value - // 13, 14, 15, 16 = en - - // cerr << nowTime << " Received RREF of " << recv_len << " bytes" - // << endl; - - for (int idx = 5; idx < recv_len; idx += 8) { - - // get value - float value = xflt2float(&(buf[idx + 4])); - - // get en - uint32_t en = xint2uint32(&(buf[idx])); - - // cerr << " idx is " << idx << " en is " << en << " value is " - // << value << endl; - - // is it something we subscribed to? - auto sdri = dataRefByEnIndex.find(en); - if (sdri == dataRefByEnIndex.end()) { - - // not subscribed ... let's get rid of it. - char buf[413]; - memset(buf, 0, sizeof(buf)); - - uint32_t zero = 0; - strcpy(buf, "RREF"); - memcpy(buf + 5, &zero, 4); - memcpy(buf + 9, &en, 4); - - sendto(sock, (void *) buf, sizeof(buf), 0, - (struct sockaddr *) &serverAddr, slen); - - if (debug) { - cerr << "Unsubscribing from non-subscribed dataref en=" - << en << endl; - } - - } else { - // subscribed - SubscribedDataRef * sdr = (*sdri).second; - - // is it emitable? - if (sdr->getEmitable()) { - - // is it part of a char array? - SubscribedCharArray * sca = - sdr->getSubscribedCharArray(); - if (sca == NULL) { - - // no, so just emit the data as a float - float oldValue = sdr->getValue(); - sdr->setValue(nowTime, value); - - if (sdr->getFirst() == true || oldValue != value) { - receiverCallbackFloat(sdr->getDataRefName(), - value); - sdr->setFirst(false); - - } - - } else { - - // a bit more work if it is a char array. Iterate through - // each of the elements of the char array's datarefs, - // appending the values as chars to a string. - - // save the value and timestamp for this char - sdr->setValue(nowTime, value); - - ostringstream stringValue; - int i = sca->getRangeFrom(); - char c; - do { - uint32_t subEn = sca->getEn( - i - sca->getRangeFrom()); - SubscribedDataRef * subSdr = - dataRefByEnIndex.find(subEn)->second; - c = (char) subSdr->getValue(); - if (c != 0) { - stringValue << c; - } - i++; - } while ((i <= sca->getRangeTo()) && (c != '\0')); - - // emit as a string only if first time or value has changed. - if (sca->getFirst() == true - || sca->getValue() != stringValue.str()) { - receiverCallbackString(sca->getDataRefName(), - stringValue.str()); - sca->setFirst(false); - sca->setValue(stringValue.str()); - } - - } - } else { - // not emitable, just save it so we update the ts. - sdr->setValue(nowTime, value); - } - } - } - }; - - // else check for other messages here. Be sure to check recv_len first as - // we can also receive -1 if socket timeouts. - - // Check for unsubscribed RREFs. These are where we have datarefs that we think - // are subscribed, but have not received any data. If nothing has been received - // in more than 5 seconds, we should resend the subscribe message. - - if (nowTime > lastUnsubscribeCheck) { - for (auto dataRef : dataRefs) { - - // cerr << "CHECK lastUpdate=" << dataRef->getLastUpdate() - // << " and nowTime-1 " << nowTime - 1 << " for en " - // << dataRef->getEn() << endl; - - //if (dataRef->getLastUpdate() < nowTime - 6) { - if (dataRef->getLastUpdate() == 0) { - - // not received in last 2 seconds. Send a subscribe request again. - uint32_t dref_freq = dataRef->getFreq(); - uint32_t dref_en = dataRef->getEn(); - - char buf[413]; - memset(buf, 0, sizeof(buf)); - - strcpy(buf, "RREF"); - memcpy(buf + 5, &dref_freq, 4); - memcpy(buf + 9, &dref_en, 4); - memcpy(buf + 13, dataRef->getDataRefName().c_str(), - dataRef->getDataRefName().length() + 1); - - sendto(sock, (void *) buf, sizeof(buf), 0, - (struct sockaddr *) &serverAddr, slen); - - if (1 || debug) { - cerr << "Sent subscribe datagram RREF for freq=" - << dref_freq << ", en=" << dref_en << ", name:" - << dataRef->getDataRefName().c_str() << " last:" - << dataRef->getLastUpdate() << " now:" << nowTime << endl; - } - } - } - lastUnsubscribeCheck = nowTime; - } - - } - if (debug) { - cerr << "Done with XPlaneUDPClient listenerThread" << endl; - } - isRunning = false; + if(ERRNO == EINTR) + continue; //see http://250bpm.com/blog:12 + else if (ERRNO == ECONNRESET || ERRNO == 10054) // 10054 = WSAECONNRESET, quit normally on connection reset by pear + { + quitFlag = true; + continue; + } + else if(ERRNO != EWOULDBLOCK && ERRNO != EAGAIN && ERRNO != WSAETIMEDOUT) + { + ostringstream buf; + buf << "recvfrom returned " << recv_len << " errno is " << ERRNO << endl; + cerr << buf.str(); + fflush(stderr); + throw runtime_error(buf.str()); + } + + } + + time_t nowTime = time(NULL); + + // check for RREF + + if (recv_len >= 4 && buf[0] == 'R' && buf[1] == 'R' && buf[2] == 'E' + && buf[3] == 'F') { + + // 9, 10, 11, 12 = value + // 13, 14, 15, 16 = en + + // cerr << nowTime << " Received RREF of " << recv_len << " bytes" + // << endl; + + for (int idx = 5; idx < recv_len; idx += 8) { + + // get value + float value = xflt2float(&(buf[idx + 4])); + + // get en + uint32_t en = xint2uint32(&(buf[idx])); + + // cerr << " idx is " << idx << " en is " << en << " value is " + // << value << endl; + + // is it something we subscribed to? + auto sdri = dataRefByEnIndex.find(en); + if (sdri == dataRefByEnIndex.end()) { + + // not subscribed ... let's get rid of it. + char buf[413]; + memset(buf, 0, sizeof(buf)); + + uint32_t zero = 0; + strcpy(buf, "RREF"); + memcpy(buf + 5, &zero, 4); + memcpy(buf + 9, &en, 4); + + sendto(sock, (void *) buf, sizeof(buf), 0, + (struct sockaddr *) &serverAddr, slen); + + if (debug) { + cerr << "Unsubscribing from non-subscribed dataref en=" + << en << endl; + } + + } else { + // subscribed + SubscribedDataRef * sdr = (*sdri).second; + + // is it emitable? + if (sdr->getEmitable()) { + + // is it part of a char array? + SubscribedCharArray * sca = + sdr->getSubscribedCharArray(); + if (sca == NULL) { + + // no, so just emit the data as a float + float oldValue = sdr->getValue(); + sdr->setValue(nowTime, value); + + if (sdr->getFirst() == true || oldValue != value) { + receiverCallbackFloat(getServer(), + sdr->getDataRefName(), + value); + sdr->setFirst(false); + + } + + } else { + + // a bit more work if it is a char array. Iterate through + // each of the elements of the char array's datarefs, + // appending the values as chars to a string. + + // save the value and timestamp for this char + sdr->setValue(nowTime, value); + + ostringstream stringValue; + int i = sca->getRangeFrom(); + char c; + do { + uint32_t subEn = sca->getEn( + i - sca->getRangeFrom()); + SubscribedDataRef * subSdr = + dataRefByEnIndex.find(subEn)->second; + c = (char) subSdr->getValue(); + if (c != 0) { + stringValue << c; + } + i++; + } while ((i <= sca->getRangeTo()) && (c != '\0')); + + // emit as a string only if first time or value has changed. + if (sca->getFirst() == true + || sca->getValue() != stringValue.str()) { + receiverCallbackString(getServer(), + sca->getDataRefName(), + stringValue.str()); + sca->setFirst(false); + sca->setValue(stringValue.str()); + } + + } + } else { + // not emitable, just save it so we update the ts. + sdr->setValue(nowTime, value); + } + } + } + } + + // else check for other messages here. Be sure to check recv_len first as + // we can also receive -1 if socket timeouts. + + // Check for unsubscribed RREFs. These are where we have datarefs that we think + // are subscribed, but have not received any data. If nothing has been received + // in more than 5 seconds, we should resend the subscribe message. + + if (nowTime > lastUnsubscribeCheck + 5) { + for (auto dataRef : dataRefs) { + + // cerr << "CHECK lastUpdate=" << dataRef->getLastUpdate() + // << " and nowTime-1 " << nowTime - 1 << " for en " + // << dataRef->getEn() << endl; + + //if (dataRef->getLastUpdate() < nowTime - 6) { + if (dataRef->getLastUpdate() == 0) { + + // not received in last 2 seconds. Send a subscribe request again. + uint32_t dref_freq = dataRef->getFreq(); + uint32_t dref_en = dataRef->getEn(); + + char buf[413]; + memset(buf, 0, sizeof(buf)); + + strcpy(buf, "RREF"); + memcpy(buf + 5, &dref_freq, 4); + memcpy(buf + 9, &dref_en, 4); + memcpy(buf + 13, dataRef->getDataRefName().c_str(), + dataRef->getDataRefName().length() + 1); + + sendto(sock, (void *) buf, sizeof(buf), 0, + (struct sockaddr *) &serverAddr, slen); + + if (debug) { + cerr << "Sent subscribe datagram RREF for freq=" + << dref_freq << ", en=" << dref_en << ", name:" + << dataRef->getDataRefName().c_str() << " last:" + << dataRef->getLastUpdate() << " now:" << nowTime << endl; + } + } + } + lastUnsubscribeCheck = nowTime; + } + } + if (debug) { + cerr << "Done with XPlaneUDPClient listenerThread" << endl; + } + isRunning = false; } void XPlaneUDPClient::subscribeIndividualDataRef(std::string dataRef, - uint32_t minFreq, bool _emitable, SubscribedCharArray * _sca) { + uint32_t minFreq, bool _emitable, SubscribedCharArray * _sca) { + + if (debug) + cerr << __PRETTY_FUNCTION__ << " DataRef: " << dataRef << endl; + // does the subscription already exist? + bool needToSubscribe = false; + SubscribedDataRef * sdr; + uint32_t en; + auto sdrI = dataRefByNameIndex.find(dataRef); + if (sdrI != dataRefByNameIndex.end()) { + ostringstream buf; + buf << "Dataref \"" << dataRef << "\" is already subscribed." << endl; + }; - // does the subscription already exist? - bool needToSubscribe = false; - SubscribedDataRef * sdr; - uint32_t en; - auto sdrI = dataRefByNameIndex.find(dataRef); - if (sdrI != dataRefByNameIndex.end()) { - ostringstream buf; - buf << "Dataref \"" << dataRef << "\" is already subscribed." << endl; - }; + // we need to subscribe to it. First figure out what "en" to use by checking + // dataRefByEnIndex for next unused value. This can go really bad if you've + // subscribed to over 2^32 datarefs. - // we need to subscribe to it. First figure out what "en" to use by checking - // dataRefByEnIndex for next unused value. This can go really bad if you've - // subscribed to over 2^32 datarefs. + while (dataRefByEnIndex.find(lastEn) != dataRefByEnIndex.end()) { + lastEn++; + } + en = lastEn++; - while (dataRefByEnIndex.find(lastEn) != dataRefByEnIndex.end()) { - lastEn++; - } - en = lastEn++; + // create the subscribedDataRef record + sdr = new XPlaneUDPClient::SubscribedDataRef(dataRef, en, minFreq, + _emitable, _sca); - // create the subscribedDataRef record - sdr = new XPlaneUDPClient::SubscribedDataRef(dataRef, en, minFreq, - _emitable, _sca); - dataRefs.push_back(sdr); + dataRefs.push_back(sdr); - // update indexes - dataRefByEnIndex.insert(pair(en, sdr)); + // update indexes + dataRefByEnIndex.insert(pair(en, sdr)); - needToSubscribe = true; + needToSubscribe = true; - // send a UDP to subscribe to dataref. Here we assume that if we re-subscribe - // to a dataref, it will update with new specs (like freq). + // send a UDP to subscribe to dataref. Here we assume that if we re-subscribe + // to a dataref, it will update with new specs (like freq). - if (false && needToSubscribe) { + if (false && needToSubscribe) { - uint32_t dref_freq = sdr->getFreq(); - uint32_t dref_en = en; + uint32_t dref_freq = sdr->getFreq(); + uint32_t dref_en = en; - char buf[413]; - memset(buf, 0, sizeof(buf)); + char buf[413]; + memset(buf, 0, sizeof(buf)); - strcpy(buf, "RREF"); - memcpy(buf + 5, &dref_freq, 4); - memcpy(buf + 9, &dref_en, 4); - memcpy(buf + 13, dataRef.c_str(), dataRef.length() + 1); + strcpy(buf, "RREF"); + memcpy(buf + 5, &dref_freq, 4); + memcpy(buf + 9, &dref_en, 4); + memcpy(buf + 13, dataRef.c_str(), dataRef.length() + 1); - ssize_t res = sendto(sock, (void *) buf, sizeof(buf), 0, - (struct sockaddr *) &serverAddr, slen); + ssize_t res = sendto(sock, (void *) buf, sizeof(buf), 0, + (struct sockaddr *) &serverAddr, slen); - if (debug) { - cerr << "Sent subscribe datagram RREF for freq=" << dref_freq - << ", en=" << dref_en << ", name:" << dataRef - << " emitable=" << _emitable << ", returned [" << res << "]" - << endl; - } - } + if (debug) { + cerr << "Sent subscribe datagram RREF for freq=" << dref_freq + << ", en=" << dref_en << ", name:" << dataRef + << " emitable=" << _emitable << ", returned [" << res << "]" + << endl; + } + } } void XPlaneUDPClient::unsubscribeDataRef(std::string dataRefName) { - dataRefMutex.lock(); + dataRefMutex.lock(); - auto sdrI = dataRefByNameIndex.find(dataRefName); - if (sdrI != dataRefByNameIndex.end()) { + auto sdrI = dataRefByNameIndex.find(dataRefName); + if (sdrI != dataRefByNameIndex.end()) { - SubscribedDataRef * sdr = sdrI->second; + SubscribedDataRef * sdr = sdrI->second; - // Remove it from our dataref indexes. + // Remove it from our dataref indexes. - for (auto it = dataRefByNameIndex.cbegin(); - it != dataRefByNameIndex.cend();) { - if (it->first == dataRefName) { - it = dataRefByNameIndex.erase(it); - } else { - ++it; - } - } + for (auto it = dataRefByNameIndex.cbegin(); + it != dataRefByNameIndex.cend();) { + if (it->first == dataRefName) { + it = dataRefByNameIndex.erase(it); + } else { + ++it; + } + } - for (auto it = dataRefByEnIndex.cbegin(); it != dataRefByEnIndex.cend(); - ) { - if (it->second->getDataRefName() == dataRefName) { - it = dataRefByEnIndex.erase(it); - } else { - ++it; - } - } + for (auto it = dataRefByEnIndex.cbegin(); it != dataRefByEnIndex.cend(); + ) { + if (it->second->getDataRefName() == dataRefName) { + it = dataRefByEnIndex.erase(it); + } else { + ++it; + } + } - // Remove it our list of subscribed datarefs. + // Remove it our list of subscribed datarefs. - dataRefs.remove(sdr); + dataRefs.remove(sdr); - // don't worry about unsubscribing it from X-Plane. It will automatically - // be unsubscribed the next time we receive it as it is not in our - // index. + // don't worry about unsubscribing it from X-Plane. It will automatically + // be unsubscribed the next time we receive it as it is not in our + // index. - }; + }; - dataRefMutex.unlock(); + dataRefMutex.unlock(); } void XPlaneUDPClient::subscribeDataRef(std::string dataRefName, - uint32_t minFreq) { + uint32_t minFreq) { -// lock the subscribedDataRefs - dataRefMutex.lock(); + // lock the subscribedDataRefs + dataRefMutex.lock(); -// check to see if the dataRefName has a double subscript. - regex r("^(.*)\\[(\\d+)\\]\\[(\\d+)\\]$"); - smatch matches; + if (debug) + cerr << __PRETTY_FUNCTION__ << ": Dataref: " << dataRefName << endl; - if (regex_match(dataRefName, matches, r)) { + // check to see if the dataRefName has a double subscript. + regex r("^(.*)\\[(\\d+)\\]\\[(\\d+)\\]$"); + smatch matches; - // it is a char array + if (regex_match(dataRefName, matches, r)) { - string baseDataRefName = matches[1]; - int rangeFrom = stoi(matches[2]); - int rangeTo = stoi(matches[3]); + // it is a char array - if (debug) { - cerr << "Requesting charArray [" << baseDataRefName << "] from [" - << rangeFrom << "] to [" << rangeTo << "]" << endl; - } + string baseDataRefName = matches[1]; + int rangeFrom = stoi(matches[2]); + int rangeTo = stoi(matches[3]); - // create the subscribedCharArray record - SubscribedCharArray * sca = new SubscribedCharArray(dataRefName, - rangeFrom, rangeTo); + if (debug) { + cerr << "Requesting charArray [" << baseDataRefName << "] from [" + << rangeFrom << "] to [" << rangeTo << "]" << endl; + } - // subscribed to each of the individual elements - for (int idx = rangeFrom; idx <= rangeTo; idx++) { + // create the subscribedCharArray record + SubscribedCharArray * sca = new SubscribedCharArray(dataRefName, + rangeFrom, rangeTo); - ostringstream drfn; - drfn << baseDataRefName << "[" << idx << "]"; + // subscribed to each of the individual elements + for (int idx = rangeFrom; idx <= rangeTo; idx++) { - // link each subscription to the sca, and only emit on the last one. - subscribeIndividualDataRef(drfn.str(), minFreq, idx == rangeTo, - sca); + ostringstream drfn; + drfn << baseDataRefName << "[" << idx << "]"; - } + // link each subscription to the sca, and only emit on the last one. + subscribeIndividualDataRef(drfn.str(), minFreq, idx == rangeTo, + sca); - } else { + } - // just a regular dataref - subscribeIndividualDataRef(dataRefName, minFreq); - } + } else { -// release mutex - dataRefMutex.unlock(); + // just a regular dataref + subscribeIndividualDataRef(dataRefName, minFreq); + } + + // release mutex + dataRefMutex.unlock(); } void XPlaneUDPClient::sendCommand(std::string cmd) { - char buf[cmd.length() + 6]; - memset(buf, 0, sizeof(buf)); + char buf[cmd.length() + 6]; + memset(buf, 0, sizeof(buf)); - strcpy(buf, "CMND"); - strcpy(buf + 5, cmd.c_str()); + strcpy(buf, "CMND"); + strcpy(buf + 5, cmd.c_str()); - sendto(sock, (void *) buf, sizeof(buf), 0, (struct sockaddr *) &serverAddr, - slen); + sendto(sock, (void *) buf, sizeof(buf), 0, (struct sockaddr *) &serverAddr, + slen); - if (debug) { - cerr << "Sent datagram CMND with payload \"" << cmd << "\"" << endl; - } + if (debug) { + cerr << "Sent datagram CMND with payload \"" << cmd << "\"" << endl; + } } void XPlaneUDPClient::setDataRef(std::string dataRef, float value) { - char buf[5 + 4 + 500]; - strcpy(buf, "DREF"); - memcpy(buf + 5, &value, 4); - memset(buf + 9, ' ', sizeof(buf) - 9); - strcpy(buf + 9, dataRef.c_str()); + char buf[5 + 4 + 500]; + strcpy(buf, "DREF"); + memcpy(buf + 5, &value, 4); + memset(buf + 9, ' ', sizeof(buf) - 9); + strcpy(buf + 9, dataRef.c_str()); - sendto(sock, (void *) buf, sizeof(buf), 0, (struct sockaddr *) &serverAddr, - slen); + sendto(sock, (void *) buf, sizeof(buf), 0, (struct sockaddr *) &serverAddr, + slen); - if (debug) { - cerr << "Sent datagram DREF with value " << value << " dref \"" - << dataRef << "\"" << endl; - } + if (debug) { + cerr << "Sent datagram DREF with value " << value << " dref \"" + << dataRef << "\"" << endl; + } } -void XPlaneUDPClient::setDataRefString(std::string dataRef, std::string value) { - - // extract array index out of dataRef if there is one. - regex r("^(.*)\\[(\\d+)\\]\\[(\\d+)\\]$"); - smatch matches; +//int p The index of the airplane you want to control. use 0 for the main airplane that you fly to drive the visuals. +//double dat_lat latitude, in degrees +//double dat_lon longitude, in degrees +//double dat_ele elevation above sea level, in meters +//float veh_psi_true heading, degrees true +//float veh_the pitch, degrees +//float veh_phi roll, degrees +void XPlaneUDPClient::sendVEHX(int p, double dat_lat, double dat_lon, double dat_ele, float veh_psi_true, float veh_the, float veh_phi) { + + char buf[45]; + memset(buf,0, sizeof(buf)); + strcpy(buf, "VEHX"); + memcpy(buf + 5, &p, 4); + memcpy(buf + 9, &dat_lat, 8); + memcpy(buf + 17, &dat_lon, 8); + memcpy(buf + 25, &dat_ele, 8); + memcpy(buf + 33, &veh_psi_true, 4); + memcpy(buf + 37, &veh_the, 4); + memcpy(buf + 41, &veh_phi, 4); + + + sendto(sock, (void *) buf, sizeof(buf), 0, (struct sockaddr *) &serverAddr, + slen); + + // if (debug) { + // cerr << "Sent datagram VEHX with value " << dat_lat << " " << dat_lon << " " << dat_ele << " " + // << veh_psi_true << " " << veh_the << " " << veh_phi << " to " << p << endl; + // fflush(stderr); + // } - if (regex_match(dataRef, matches, r)) { +} - string baseDataRef = matches[1]; - int startIdx = stoi(matches[2]); - int stopIdx = stoi(matches[3]); +void XPlaneUDPClient::setDataRefString(std::string dataRef, std::string value) { - for (int i = startIdx; i < stopIdx; i++) { + // extract array index out of dataRef if there is one. + regex r("^(.*)\\[(\\d+)\\]\\[(\\d+)\\]$"); + smatch matches; + + if (regex_match(dataRef, matches, r)) { + + string baseDataRef = matches[1]; + int startIdx = stoi(matches[2]); + int stopIdx = stoi(matches[3]); + + for (int i = startIdx; i < stopIdx; i++) { + + unsigned int idx = i - startIdx; + float data; + + if (idx < value.length()) { + data = value.at(idx); + } else { + data = ' '; + } + + ostringstream dref; + dref << baseDataRef << "[" << i << "]"; + + char buf[5 + 4 + 500]; + strcpy(buf, "DREF"); + memcpy(buf + 5, &data, 4); + memset(buf + 9, ' ', sizeof(buf) - 9); + strcpy(buf + 9, dref.str().c_str()); + + sendto(sock, (void *) buf, sizeof(buf), 0, + (struct sockaddr *) &serverAddr, slen); + + // if (debug) { + // cerr << "Sent datagram DREF with value \"" << data + // << "\" dref \"" << dref.str() << "\"" << endl; + // } + } + } else { + for (int i = 0; i < value.length(); i++) { + float data; + data = value.at(i); + + ostringstream dref; + dref << dataRef << "[" << i << "]"; + + char buf[5 + 4 + 500]; + strcpy(buf, "DREF"); + memcpy(buf + 5, &data, 4); + memset(buf + 9, ' ', sizeof(buf) - 9); + strcpy(buf + 9, dref.str().c_str()); + + sendto(sock, (void *) buf, sizeof(buf), 0, + (struct sockaddr *) &serverAddr, slen); + + } + + if (debug) { + cerr << "Sent datagram DREF with value \"" << value + << "\" dref \"" << dataRef << "\"" << endl; + } + // ostringstream buf; + // buf + // << "In XPlaneUDPClient::setDataRefString (dataRef, value), dataRef is \"" + // << dataRef << "\", expecting \"^.*\\[(\\d+)\\]$\""; + // cerr << buf.str(); + // fflush(stderr); + // throw runtime_error(buf.str()); + } +} - unsigned int idx = i - startIdx; - float data; +// Load Aircraft +// acf_file: acf file to load including relative path from x-plane root folder +void XPlaneUDPClient::sendACFN(int p, std::string acf_file, int livery_index) +{ + const int32_t net_strDIM=150; // must be short enough to send over the net, long enough to hold a full ACF path from the x-system folder + struct acfn_struct + { + int32_t m_acfn_p; + char m_acfn_path_rel[net_strDIM]; + char pad[2]; + int32_t m_acfn_live_ind; + }; - if (idx < value.length()) { - data = value.at(idx); - } else { - data = ' '; - } + acfn_struct acfn; + char buf[165]; - ostringstream dref; - dref << baseDataRef << "[" << i << "]"; + memset(&acfn,0,sizeof(acfn)); + acfn.m_acfn_p = p; + acfn.m_acfn_live_ind = livery_index; + strncpy(acfn.m_acfn_path_rel,acf_file.c_str(),net_strDIM-1); - char buf[5 + 4 + 500]; - strcpy(buf, "DREF"); - memcpy(buf + 5, &data, 4); - memset(buf + 9, ' ', sizeof(buf) - 9); - strcpy(buf + 9, dref.str().c_str()); - sendto(sock, (void *) buf, sizeof(buf), 0, - (struct sockaddr *) &serverAddr, slen); + memset(buf,0, sizeof(buf)); + strcpy(buf, "ACFN"); + memcpy(buf + 5, &acfn, sizeof(acfn)); - if (debug) { - cerr << "Sent datagram DREF with value \"" << data - << "\" dref \"" << dref.str() << "\"" << endl; - } + sendto(sock, (void *) buf, sizeof(buf), 0, (struct sockaddr *) &serverAddr, + slen); - } + if (debug) { + cerr << "Load ACF file: \"" << acfn.m_acfn_path_rel << "\" with livery " << acfn.m_acfn_live_ind << " to A/C index " << p << endl; + } - } else { - ostringstream buf; - buf - << "In XPlaneUDPClient::setDataRefString (dataRef, value), dataRef is \"" - << dataRef << "\", expecting \"^.*\\[(\\d+)\\]$\""; - throw runtime_error(buf.str()); - } } diff --git a/src/libsrc/XPlaneUDPClient.h b/src/libsrc/XPlaneUDPClient.h index 2371179..983dc3b 100644 --- a/src/libsrc/XPlaneUDPClient.h +++ b/src/libsrc/XPlaneUDPClient.h @@ -28,7 +28,13 @@ #include #include #include +#ifdef _WIN32 +#include +#include +typedef int socklen_t; +#else #include +#endif #include #include #include @@ -69,8 +75,8 @@ class XPlaneUDPClient { int debug; // receiver callback - std::function receiverCallbackFloat; - std::function receiverCallbackString; + std::function receiverCallbackFloat; + std::function receiverCallbackString; class SubscribedCharArray { std::string dataRefName; @@ -182,8 +188,8 @@ class XPlaneUDPClient { public: XPlaneUDPClient(std::string _server, uint16_t _port, - std::function _receiverCallbackFloat, - std::function _receiverCallbackString + std::function _receiverCallbackFloat, + std::function _receiverCallbackString ); virtual ~XPlaneUDPClient(); @@ -198,7 +204,10 @@ class XPlaneUDPClient { void setDataRef (std::string dataRef, float value); void setDataRefString (std::string dataRef, std::string); - + void sendVEHX(int p, double dat_lat, double dat_lon, double dat_ele, float veh_psi_true, float veh_the, float veh_phi); + void sendACFN(int p, std::string acf_file, int livery_index); + std::string getServer() {return server;} + bool getRunning(void){return isRunning;} }; diff --git a/src/test/TestXPlaneUDPClient.cpp b/src/test/TestXPlaneUDPClient.cpp index c997124..2749107 100644 --- a/src/test/TestXPlaneUDPClient.cpp +++ b/src/test/TestXPlaneUDPClient.cpp @@ -31,6 +31,7 @@ #include "XPlaneUDPClient.h" #include +#include #include "XPUtils.h" @@ -38,6 +39,7 @@ using namespace std; // globals bool found = false; +string hostname; string host; uint16_t port; @@ -60,10 +62,26 @@ void receiverBeaconCallback(XPlaneBeaconListener::XPlaneServer server, << (exists ? "alive" : "dead") << "]" << endl; host = server.host; port = server.receivePort; + hostname = server.name; found = true; } +static volatile int s_interrupted = 0; +static void s_signal_handler (int signal_value) +{ + s_interrupted = 1; +} + +static void s_catch_signals (void) +{ + struct sigaction action; + action.sa_handler = s_signal_handler; + action.sa_flags = 0; + sigemptyset (&action.sa_mask); + sigaction (SIGINT, &action, NULL); + sigaction (SIGTERM, &action, NULL); +} int main() { @@ -72,34 +90,38 @@ int main() { std::placeholders::_2)); XPlaneBeaconListener::getInstance()->setDebug(0); + s_catch_signals(); + cout << "Press Control-C to abort." << endl; // wait for a server - while (!found) { + while (!found && s_interrupted == 0) { sleep (1); } - cout << "Found server " << host << ":" << port << endl; + cout << "Found server " << hostname << " " << host << ":" << port << endl; + return 0; XPlaneUDPClient xp(host, port, std::bind(receiverCallbackFloat, std::placeholders::_1, std::placeholders::_2), std::bind(receiverCallbackString, std::placeholders::_1, std::placeholders::_2)); - xp.setDebug(0); + xp.setDebug(1); xp.subscribeDataRef("sim/aircraft/view/acf_descrip[0][40]", 1); - xp.subscribeDataRef("sim/cockpit2/engine/actuators/throttle_ratio[0]", 10); + xp.subscribeDataRef("sim/cockpit2/engine/actuators/throttle_ratio[0]", 2); xp.sendCommand("sim/flight_controls/flaps_down"); xp.sendCommand("sim/flight_controls/flaps_down"); + xp.unsubscribeDataRef("sim/aircraft/view/acf_descrip[0][40]"); float r = 0; float i = 0.01; - while (1) { - usleep (1000 * 50); + while (s_interrupted == 0) { + usleep (1000 * 1000); xp.setDataRef("sim/multiplayer/controls/engine_throttle_request[0]", r); r += i; @@ -110,7 +132,10 @@ int main() { i = 0.01; } + if(r>0.05) + s_interrupted = 1; + } + return 0; } -