Diff: XPlaneIO/XPlaneIO.cpp

Revision:

0:a5d13af495af

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/XPlaneIO/XPlaneIO.cpp	Wed Dec 21 22:29:59 2011 +0000
@@ -0,0 +1,300 @@
+#include "EthernetNetIf.h"
+#include "UDPSocket.h"
+#include "LocalConfigFile.h"
+#include "Scaler.h"
+#include "XPlaneUdp.h"
+#include "XPlaneUdpDATA.h"
+#include "XPlaneUdpDecoder.h"
+#include "XPlaneUdpEncoder.h"
+#include "XPlaneIO.h"
+
+XPlaneIO::~XPlaneIO() {
+    if (_eth != NULL) {
+        free(_eth);
+        _eth = NULL;
+    }
+
+    for (int i = 0; i < _xpAnalogInCount; i++) {
+        if (_xpAnalogIn[i] != NULL) {
+            free(_xpAnalogIn[i]);
+        }
+    }
+    
+    for (XPlaneUdpDATAMap::iterator it = _recvDATAMap.begin(); it != _recvDATAMap.end(); it++) {
+        if ((it->second) != NULL) {
+            free(it->second);
+        }
+    }
+    _recvDATAMap.clear();
+
+}
+
+bool XPlaneIO::setup(char * configFilename, Ethernet * ethernet) {
+    
+    printf("X-Plane I/O ... initializing \n");
+    
+    bool returnStatus = true;
+    
+    LocalConfigFile cfg(configFilename);
+    
+    _debug = cfg.getBool("debug", false);
+    printf("debug: %d \n", _debug);
+
+    _debugVerbose = cfg.getBool("debugVerbose", false);
+    printf("debugVerbose: %d \n", _debugVerbose);
+
+    IpAddr ipAddr;
+    int intIp[4];
+
+    bool dhcp = true;
+    if (cfg.fillIntArray4("mbed_ip_address", intIp)) {
+        ipAddr = IpAddr(intIp[0], intIp[1], intIp[2], intIp[3]);
+        // Continue gathering IP configuration.
+        if (cfg.fillIntArray4("mbed_ip_netmask", intIp)) {
+            IpAddr ipMask = IpAddr(intIp[0], intIp[1], intIp[2], intIp[3]);
+            if (cfg.fillIntArray4("mbed_ip_gateway", intIp)) {
+                IpAddr ipGtwy = IpAddr(intIp[0], intIp[1], intIp[2], intIp[3]);
+                if (cfg.fillIntArray4("mbed_ip_dnssrvr", intIp)) {
+                    IpAddr ipDns = IpAddr(intIp[0], intIp[1], intIp[2], intIp[3]);
+                    
+                    _eth = new EthernetNetIf(ipAddr, ipMask, ipGtwy, ipDns);
+                    dhcp = false;
+                }
+            }
+        }
+    }
+    if (dhcp) {
+        if (_debug) printf("mbed_ip_* address config not found or invalid, so defaulting to DHCP \n");
+        _eth = new EthernetNetIf;
+    }
+    
+    // Set up the mbed ethernet interface.
+    EthernetErr ethErr = _eth->setup();
+    if (ethErr != ETH_OK) {
+        printf("Ethernet setup failed (error = %d) \n", ethErr);
+        returnStatus = false;
+    }
+
+    if (_debug) {
+        ipAddr = _eth->getIp();
+        printf("mbed IP address is %d.%d.%d.%d \n", ipAddr[0], ipAddr[1], ipAddr[2], ipAddr[3]);
+    }
+
+    const Ethernet::Mode linkModes[] = {Ethernet::AutoNegotiate, Ethernet::HalfDuplex10, Ethernet::FullDuplex10, Ethernet::HalfDuplex100, Ethernet::FullDuplex100};
+    int linkMode = cfg.getInt("ethernet_link_mode", 0);
+    ethernet->set_link(linkModes[linkMode]);
+    if (_debug) printf("ethernet_link_mode set to %d \n", linkMode);
+
+    if (_debug) printf("Setting recvHost and sendHost \n");
+    _recvHost.setIp(ipAddr);
+    _recvHost.setPort(cfg.getInt("recv_port", 49000));
+    if (cfg.fillIntArray4("xplane_ip_address", intIp)) {
+        _sendHost.setIp(IpAddr(intIp[0], intIp[1], intIp[2], intIp[3]));
+    }
+    _sendHost.setPort(cfg.getInt("xplane_ip_port", 49000));
+    
+    if (_debug) printf("Setting event callback handler \n");
+    _udpSocket.setOnEvent(this, &XPlaneIO::onUDPSocketEvent);
+
+    if (_debug) printf("Binding to recvHost \n");
+    UDPSocketErr udpErr = _udpSocket.bind(_recvHost);
+    if (udpErr != UDPSOCKET_OK) {
+        printf("Failure in binding to recvHost (error = %d) \n", udpErr);
+        returnStatus = false;
+    }
+
+    _sendInterval = cfg.getInt("send_interval", 200);
+    printf("send_interval: %d ms \n", _sendInterval);
+
+    _reverseByteOrder = cfg.getBool("reverse_byte_order", false);
+    printf("reverse_byte_order: %d \n", _reverseByteOrder);
+
+    char key[33];
+    float f4[4];
+
+    // Scan for Analog Inputs (up to 6).
+    _xpAnalogInCount = 0;
+    for (int a = 1; a <= 6; a++) {
+        sprintf(key, "ain_%d_pin", a);
+        PinName pin = cfg.getPin(key, NC);
+        if (pin != NC) {
+            sprintf(key, "ain_%d_scale1", a);
+            if (cfg.fillFloatArray4(key, f4)) {
+                Scaler<float> scale1(f4[0], f4[1], f4[2], f4[3]);
+                sprintf(key, "ain_%d_scale2", a);
+                if (cfg.fillFloatArray4(key, f4)) {
+                    Scaler<float> scale2(f4[0], f4[1], f4[2], f4[3]);
+                    sprintf(key, "ain_%d_msg_idx", a);
+                    int msgIdx = cfg.getInt(key, -1);
+                    if (msgIdx >= 0) {
+                        sprintf(key, "ain_%d_float_idx", a);
+                        int floatIdx = cfg.getInt(key, -1);
+                        if (floatIdx >= 0) {
+                            // found all valid config items
+                            printf("Setting Analog Input # %d \n", a);
+                            _xpAnalogIn[_xpAnalogInCount] = new XPlaneAnalogIn(pin, scale1, scale2, msgIdx, floatIdx);
+                            _xpAnalogInCount++;
+                            
+                            addDATAToSend(msgIdx);
+                        }
+                    }
+                }
+            }
+        }
+    }
+    for (int i = _xpAnalogInCount; i < 6; i++) {
+        _xpAnalogIn[i] = NULL;
+    }
+
+    _udpDecoder.setDebug(_debugVerbose);
+    _udpDecoder.setReverseByteOrder(_reverseByteOrder);
+    
+    _udpEncoder.setDebug(_debugVerbose);
+    
+    return returnStatus;
+}
+
+
+bool XPlaneIO::addDATAToReceive(int msgIndex) {
+    return addXPlaneUdpDATAToMap(_recvDATAMap, msgIndex, _reverseByteOrder);
+}
+
+bool XPlaneIO::addDATAToSend(int msgIndex) {
+    return addXPlaneUdpDATAToMap(_sendDATAMap, msgIndex, _reverseByteOrder);
+}
+
+XPlaneUdpDATA * XPlaneIO::getReceiveDATA(int msgIndex) const {
+    return getXPlaneUdpDATA(_recvDATAMap, msgIndex);
+}
+
+XPlaneUdpDATA * XPlaneIO::getSendDATA(int msgIndex) const {
+    return getXPlaneUdpDATA(_sendDATAMap, msgIndex);
+}
+
+float XPlaneIO::getReceiveDATAValue(int msgIndex, int floatIndex) const {
+    XPlaneUdpDATA *DATAmsg = getReceiveDATA(msgIndex);
+    if (DATAmsg != NULL) {
+        return DATAmsg->getData(floatIndex);
+    }
+    else {
+        return -999.999f;
+    }
+}
+
+void XPlaneIO::setSendDATAValue(int msgIndex, int floatIndex, float f) {
+    XPlaneUdpDATA *DATAmsg = getSendDATA(msgIndex);
+    if (DATAmsg != NULL) {
+        DATAmsg->setData(floatIndex, f);
+    }
+}
+
+
+void XPlaneIO::pollAnalogIn() {
+    int scaleRange;
+    bool noChange;
+    for (int i = 0; i < _xpAnalogInCount; i++) {
+        XPlaneAnalogIn * xpAIn = _xpAnalogIn[i];
+        float aout = xpAIn->read_scaled(scaleRange, noChange);
+        if (! noChange) {
+            setSendDATAValue(xpAIn->xplaneDATAMsgIdx(), xpAIn->xplaneDATAMsgFloatIdx(), aout);
+        }
+    }
+}
+
+
+void XPlaneIO::sendDREF(XPlaneUdpDREF & dref) {
+    _sendDREFQueue.push(dref);
+}
+
+
+void XPlaneIO::startSendingUdp() {
+    _sendTicker.attach_us(this, &XPlaneIO::sendUdp, _sendInterval * 1000);
+}
+
+void XPlaneIO::stopSendingUdp() {
+    _sendTicker.detach();
+}
+
+void XPlaneIO::sendUdp() {
+    if (_debugVerbose) printf("sendUdp() called \n");
+    
+    pollAnalogIn();
+    
+    int len = _udpEncoder.encodeFromDATAMap(_udpSendBuffer, UDP_SEND_BUFFER_SIZE, _sendDATAMap, true);
+    if (len > 5) {
+        udpSocketSend(_udpSendBuffer, len);
+        resetDataChangedInMap(_sendDATAMap);
+    }
+    
+    while (_sendDREFQueue.size() > 0) {
+        len = _udpEncoder.encodeDataRef(_udpSendBuffer, UDP_SEND_BUFFER_SIZE, _sendDREFQueue.front());
+        if (len > 0) {
+            udpSocketSend(_udpSendBuffer, len);
+        }
+        _sendDREFQueue.pop();
+    }
+}
+
+
+/**
+ * Callback handler for received UDP data.
+ */
+void XPlaneIO::onUDPSocketEvent(UDPSocketEvent e) {
+    if (_debugVerbose) printf("onUDPSocketEvent() called \n");
+    
+    if (e == UDPSOCKET_READABLE) {
+        if (_debugVerbose) printf("onUDPSocketEvent() : event=UDPSOCKET_READABLE \n");
+        
+        Host host;
+        while (int len = _udpSocket.recvfrom(_udpRecvBuffer, UDP_RECV_BUFFER_SIZE, &host)) {
+            if (len <= 0) {
+                break;
+            }
+            
+            if (_debugVerbose) {
+                IpAddr hostIp = host.getIp();
+                //printf("From %d.%d.%d.%d: len=%d: %s\n", hostIp[0], hostIp[1], hostIp[2], hostIp[3], len, buf);
+                printf("From %d.%d.%d.%d: len=%d \n", hostIp[0], hostIp[1], hostIp[2], hostIp[3], len);
+            }
+            
+            XPlaneUdpMessageType pktMsgType = _udpDecoder.setUdpBuffer(_udpRecvBuffer, len);
+            if (pktMsgType == DATA) {
+                _udpDecoder.putIntoDATAMap(_recvDATAMap, true);
+            }
+        }
+    }
+}
+
+
+int XPlaneIO::udpSocketSend(char * buf, int len) {
+    if (_debugVerbose) printf("udpSocketSend(): sending %d bytes \n", len);
+    
+    int nSent = _udpSocket.sendto(buf, len, &_sendHost);
+
+    if ( nSent < 0 ) {
+        if (_debug) printf("PROBLEM SENDING UDP DATA (error = %d) \n", (UDPSocketErr)nSent);
+    }
+    else if ( nSent != len ) {
+        if (_debug) printf("WARNING: %d bytes were sent \n", nSent);
+    }
+    
+    return nSent;
+}
+
+
+
+bool XPlaneIO::debug() const {
+    return _debug;
+}
+
+bool XPlaneIO::reverseByteOrder() const {
+    return _reverseByteOrder;
+}
+
+XPlaneUdpDATAMap & XPlaneIO::recvDATAMap() {
+    return _recvDATAMap;
+}
+
+XPlaneUdpDATAMap & XPlaneIO::sendDATAMap() {
+    return _sendDATAMap;
+}