![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
The xplane_io (X-Plane I/O) program is used to establish network communications, via UDP, with the X-Plane flight simulator running on a computer. The code consists of class libraries that abstract the lower-level UDP packet encoding and decoding details, according to the UDP protocol specifications in X-Plane version 9. Any X-Plane DATA packets can be sent and received, and any X-Plane DataRefs can be set by sending DREF packets to X-Plane.
Dependencies: EthernetNetIf mbed ConfigFile
Revision 0:a5d13af495af, committed 2011-12-21
- Comitter:
- bapowell
- Date:
- Wed Dec 21 22:29:59 2011 +0000
- Commit message:
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/EthernetNetIf.lib Wed Dec 21 22:29:59 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mamezu/code/EthernetNetIf/#0f6c82fcde82
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LocalConfigFile/ConfigFile.lib Wed Dec 21 22:29:59 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/shintamainjp/code/ConfigFile/#f6ceafabe9f8
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LocalConfigFile/LocalConfigFile.cpp Wed Dec 21 22:29:59 2011 +0000 @@ -0,0 +1,114 @@ +#include <ctype.h> +#include "LocalConfigFile.h" + +LocalConfigFile::LocalConfigFile(char *filename) { + LocalFileSystem localFileSys("local"); // create the local filesystem under the name "local" + sprintf(_buf, "/local/%s", filename); // set full file path into _buf + printf("Reading config file: %s ... ", _buf); + if (read(_buf)) { + printf("Success \n"); + } + else { + printf("Problem \n"); + } +} + +char LocalConfigFile::getChar(char *key, char defaultVal) { + if (getValueIntoBuf(key)) { + if (strlen(_buf) > 0) { + return _buf[0]; + } + else { + printf("getChar(key='%s'): Problem \n", key); + } + } + return defaultVal; +} + +bool LocalConfigFile::getBool(char *key, bool defaultVal) { + char c = getChar(key, '.'); + if (c != '.') { + c = toupper(c); + return (c == 'T' || c == 'Y' || c == '1'); + } + else { + printf("getBool(key='%s'): Problem \n", key); + } + return defaultVal; +} + +int LocalConfigFile::getInt(char *key, int defaultVal) { + if (getValueIntoBuf(key)) { + int val; + int cnt = sscanf(_buf, "%d", &val); + if (cnt == 1) { + return val; + } + else { + printf("getInt(key='%s'): Problem \n", key); + } + } + return defaultVal; +} + +PinName LocalConfigFile::getPin(char *key, PinName defaultVal) { + const PinName pinName[] = {p5, p6, p7, p8, p9, p10, p11, p12, p13, p14, p15, p16, p17, p18, + p19, p20, p21, p22, p23, p24, p25, p26, p27, p28, p29, p30}; + int p = getInt(key, -1); + if (p == -1) { + printf("getPin(key='%s'): Problem \n", key); + } + else if (p < 5 || p > 30) { + printf("getPin(key='%s'): Invalid pin#: %d. Pin# must be between 5 and 30, inclusive.\n", key, p); + } + else { + return pinName[p - 5]; + } + return defaultVal; +} + +bool LocalConfigFile::fillIntArray4(char *key, int *intArray) { + if (getValueIntoBuf(key)) { + int a0, a1, a2, a3; + int cnt = sscanf(_buf, "%d %d %d %d", &a0, &a1, &a2, &a3); + if (cnt == 4) { + intArray[0] = a0; + intArray[1] = a1; + intArray[2] = a2; + intArray[3] = a3; + return true; + } + else { + printf("fillIntArray4(key='%s'): Problem \n", key); + } + } + return false; +} + +bool LocalConfigFile::fillFloatArray4(char *key, float *floatArray) { + if (getValueIntoBuf(key)) { + float a0, a1, a2, a3; + int cnt = sscanf(_buf, "%f %f %f %f", &a0, &a1, &a2, &a3); + if (cnt == 4) { + floatArray[0] = a0; + floatArray[1] = a1; + floatArray[2] = a2; + floatArray[3] = a3; + return true; + } + else { + printf("fillFloatArray4(key='%s'): Problem \n", key); + } + } + return false; +} + +bool LocalConfigFile::getValueIntoBuf(char *key) { + if (getValue(key, _buf, sizeof(_buf))) { + return true; + } + else { + printf("LocalConfigFile.getValueIntoBuf(key='%s'): Problem, or config key not found \n", key); + return false; + } +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LocalConfigFile/LocalConfigFile.h Wed Dec 21 22:29:59 2011 +0000 @@ -0,0 +1,25 @@ +#ifndef LOCALCONFIGFILE_H_INCLUDED +#define LOCALCONFIGFILE_H_INCLUDED + +#include "mbed.h" +#include "ConfigFile.h" + +class LocalConfigFile : public ConfigFile { +public: + + LocalConfigFile(char *filename); + char getChar(char *key, char defaultVal); + bool getBool(char *key, bool defaulVal); + int getInt(char *key, int defaultVal); + PinName getPin(char *key, PinName defaultVal); + bool fillIntArray4(char *key, int *intArray); + bool fillFloatArray4(char *key, float *floatArray); + +private: + + char _buf[100]; + + bool getValueIntoBuf(char *key); +}; + +#endif // LOCALCONFIGFILE_H_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Ranger/Ranger.h Wed Dec 21 22:29:59 2011 +0000 @@ -0,0 +1,159 @@ +#ifndef RANGER_H_INCLUDED +#define RANGER_H_INCLUDED + +#include <vector> +#include <stdarg.h> + +/** + * Perform range-type functions on a list of numbers. + * The numbers must be in ascending order. + * + * Example: + * @code + * + * #include "mbed.h" + * #include "Ranger.h" + * + * AnalogIn hatSwitch(p16); + * + * int main() { + * + * Ranger<float> hatSwitchInputRanger(4, 0.59, 0.68, 0.78, 0.92); + * int hatSwitchDegreesByRange[5] = {0, -90, 180, 90, 0}; + * + * while(1) { + * int newRange = hatSwitchInputRanger.range(hatSwitch.read()); + * if (hatSwitchInputRanger.rangeChanged()) { + * int degrees = hatSwitchDegreesByRange[newRange]; + * } + * wait(0.1); + * } + * } + * @endcode + */ + +template<typename N> +class Ranger { +public: + + /** + * Create a Ranger object from the given set of passed-in numbers. + * The numbers in the variable argument list must be in ascending order. + * + * @param argCount number of arguments that follow + * @param ... numbers, in ascending order, which define the ranges + */ + Ranger(int argCount, ... ); + + /** + * Create a Ranger object from the given vector. + * The numbers in the vector must be in ascending order. + * + * @param vect vector to copy from + */ + Ranger(vector<N> vect); + + /** + * Create a Ranger object from the given array. + * The numbers in the array must be in ascending order. + * + * @param count number of elements to copy from the array + * @param arr array to copy from + */ + Ranger(int count, N arr[]); + + /** + * Return the range that the given number falls in. + * The range is a slice index (zero-based), based on the Ranger's vector/array. + * + * @param input value to determine range from + */ + int range(N input); + + /** + * Check if the last call to range() resulted in a change of range from the previous call. + */ + bool rangeChanged() const; + + /** + * Print a representation of this instance to outputStream. + */ + void toString(FILE * outputStream); + +private: + + /** + * Common initialization, called from each constructor. + */ + void init(); + + vector<N> _nbrVector; + N _lastInput; + int _lastRange; + bool _rangeChanged; +}; + + +template<typename N> +void Ranger<N>::init() { + _lastRange = -1; + _rangeChanged = false; +} + +template<typename N> +Ranger<N>::Ranger(int argCount, ... ) { + va_list argList; + va_start(argList, argCount); + for (int i = 0; i < argCount; i++) { + _nbrVector.push_back(va_arg(argList, N)); + } + va_end(argList); + + init(); +} + +template<typename N> +Ranger<N>::Ranger(vector<N> vect) : _nbrVector(vect) { + init(); +} + +template<typename N> +Ranger<N>::Ranger(int count, N arr[]) : _nbrVector(arr, arr + count) { + init(); +} + +template<typename N> +int Ranger<N>::range(N input) { + + int newRange = -1; + + for (int i=0; i < _nbrVector.size(); i++) { + if (input < _nbrVector[i]) { + newRange = i; + break; + } + } + + if (newRange == -1) { + newRange = _nbrVector.size(); + } + + _rangeChanged = (_lastRange != newRange); + _lastInput = input; + _lastRange = newRange; + + return newRange; +} + +template<typename N> +bool Ranger<N>::rangeChanged() const { + return _rangeChanged; +} + +template<typename N> +void Ranger<N>::toString(FILE * outputStream) { + fprintf(outputStream, "Ranger: size=%d, lastRange=%d, changed=%d", + _nbrVector.size(), _lastRange, _rangeChanged); +} + +#endif // RANGER_H_INCLUDED \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Scaler/Scaler.h Wed Dec 21 22:29:59 2011 +0000 @@ -0,0 +1,113 @@ +#ifndef SCALER_H_INCLUDED +#define SCALER_H_INCLUDED + +/** + * Use linear projection (interpolation) to map/scale values from one range into another. + * + * Example: + * @code + * + * #include "mbed.h" + * #include "Scaler.h" + * + * AnalogIn pot(p20); + * + * Scaler<float> left(0.965, 0.653, -1, 0); + * Scaler<float> right(0.653, 0.491, 0, 1); + * + * int main() { + * while(1) { + * float scaledPotValue; + * float potValue = pot.read(); + * printf("Pot value = %f, %d", potValue, pot.read_u16()); + * if (potValue > 0.653) { + * scaledPotValue = left.scale(potValue); + * } + * else { + * scaledPotValue = right.scale(potValue); + * } + * printf("... scaled: %f \n", scaledPotValue); + * wait(0.1); + * } + * } + * @endcode + */ + +template<typename N> +class Scaler { +public: + + /** + * Create a Scaler object using the specified input and output ranges. + * + * @param inputFrom input range "from" + * @param inputTo input range "to" + * @param outputFrom output range "from" + * @param outputTo output range "to" + */ + Scaler(N inputFrom, N inputTo, N outputFrom, N outputTo); + + /** + * Map the input value, within the input range, to the output range. + * + * @param input value to project to the output range + */ + N scale(N input) const; + + // Accessors + N inputFrom() const; + N inputTo() const; + N outputFrom() const; + N outputTo() const; + N slope() const; + +private: + + N _inputFrom; + N _inputTo; + N _outputFrom; + N _outputTo; + N _slope; +}; + + +template<typename N> +Scaler<N>::Scaler(N inputFrom, N inputTo, N outputFrom, N outputTo) { + _inputFrom = inputFrom; + _inputTo = inputTo; + _outputFrom = outputFrom; + _outputTo = outputTo; + _slope = (outputTo - outputFrom) / (inputTo - inputFrom); +} + +template<typename N> +N Scaler<N>::scale(N input) const { + return _slope * (input - _inputFrom) + _outputFrom; +} + +template<typename N> +N Scaler<N>::inputFrom() const { + return _inputFrom; +} + +template<typename N> +N Scaler<N>::inputTo() const { + return _inputTo; +} + +template<typename N> +N Scaler<N>::outputFrom() const { + return _outputFrom; +} + +template<typename N> +N Scaler<N>::outputTo() const { + return _outputTo; +} + +template<typename N> +N Scaler<N>::slope() const { + return _slope; +} + +#endif // SCALER_H_INCLUDED \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TimeoutPrompt/TimeoutPrompt.cpp Wed Dec 21 22:29:59 2011 +0000 @@ -0,0 +1,28 @@ +#include "TimeoutPrompt.h" + +TimeoutPrompt::TimeoutPrompt(Serial & serialInOut, int defaultTimeoutSeconds) : + _serialInOut(serialInOut), _defaultTimeoutSeconds(defaultTimeoutSeconds) +{ + _timer.reset(); +} + +char TimeoutPrompt::prompt(int timeoutSeconds, char *promptString, char* validChars) { + _serialInOut.printf("%s\n", promptString); + _timer.reset(); + _timer.start(); + while (_timer.read() < timeoutSeconds) { + if (_serialInOut.readable()) { + char c = _serialInOut.getc(); + if (strchr(validChars, c) != NULL) { + _timer.stop(); + return c; + } + } + } + _timer.stop(); + return NULL; +} + +char TimeoutPrompt::prompt(char *promptString, char* validChars) { + return prompt(_defaultTimeoutSeconds, promptString, validChars); +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TimeoutPrompt/TimeoutPrompt.h Wed Dec 21 22:29:59 2011 +0000 @@ -0,0 +1,30 @@ +#ifndef TIMEOUTPROMPT_H_INCLUDED +#define TIMEOUTPROMPT_H_INCLUDED + +#include "mbed.h" + +class TimeoutPrompt { +public: + + TimeoutPrompt(Serial & serialInOut, int defaultTimeoutSeconds); + + /** + * Output the given prompt for timeoutSeconds. If a character is input that matches + * one of the validChars, then return it. Otherwise, if no valid character is + * pressed within the timeout, then return NULL. + */ + char prompt(int timeoutSeconds, char *promptString, char* validChars); + + /** + * Overloaded method that uses defaultTimeoutSeconds. + */ + char prompt(char *promptString, char* validChars); + +private: + + Serial & _serialInOut; + int _defaultTimeoutSeconds; + Timer _timer; +}; + +#endif // TIMEOUTPROMPT_H_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/XPlaneIO/XPlaneAnalogIn.cpp Wed Dec 21 22:29:59 2011 +0000 @@ -0,0 +1,85 @@ +#include "Scaler.h" +#include "XPlaneAnalogIn.h" + +XPlaneAnalogIn::XPlaneAnalogIn(PinName pin, Scaler<float> scale1, Scaler<float> scale2, int msgIdx, int msgFloatIdx) : + _pin(pin), _scale1(scale1), _scale2(scale2), _xplaneDATAMsgIdx(msgIdx), _xplaneDATAMsgFloatIdx(msgFloatIdx), + _ain(pin) +{ + _lastUnscaledRead = -1.0; + _lastScaledRead = -999.0; + _lastScaleRange = -1; +} + +float XPlaneAnalogIn::read_unscaled() { + _lastUnscaledRead = _ain.read(); + return _lastUnscaledRead; +} + +float XPlaneAnalogIn::read_scaled(int & range, bool & scaledOutputNoChange) { + float scaledOutput; + float rawInput = read_unscaled(); + + if (rawInput > _scale1.inputTo() && rawInput < _scale2.inputFrom()) { + range = 0; + scaledOutput = (_scale1.outputTo() + _scale2.outputFrom()) / 2.0f; + } + else if (rawInput <= _scale1.inputTo()) { + if (rawInput < _scale1.inputFrom()) { + range = 3; + scaledOutput = _scale1.outputFrom(); + } + else { + range = 1; + scaledOutput = _scale1.scale(rawInput); + } + } + else { // rawInput >= _scale2.inputFrom + if (rawInput > _scale2.inputTo()) { + range = 4; + scaledOutput = _scale2.outputTo(); + } + else { + range = 2; + scaledOutput = _scale2.scale(rawInput); + } + } + + scaledOutputNoChange = ((scaledOutput == _lastScaledRead) || + (range == _lastScaleRange && (range == 0 || range == 3 || range == 4))); + + _lastScaledRead = scaledOutput; + _lastScaleRange = range; + + return scaledOutput; +} + +/* +void XPlaneAnalogIn::scale1(Scaler<float> scale1) { + _scale1 = scale1; +} + +void XPlaneAnalogIn::scale2(Scaler<float> scale2) { + _scale2 = scale2; +} + +void XPlaneAnalogIn::xplaneDATAMsgIdx(int xplaneDATAMsgIdx) { + _xplaneDATAMsgIdx = xplaneDATAMsgIdx; +} + +void XPlaneAnalogIn::xplaneDATAMsgFloatIdx(int xplaneDATAMsgFloatIdx) { + _xplaneDATAMsgFloatIdx = xplaneDATAMsgFloatIdx; +} +*/ + +int XPlaneAnalogIn::xplaneDATAMsgIdx() { + return _xplaneDATAMsgIdx; +} + +int XPlaneAnalogIn::xplaneDATAMsgFloatIdx() { + return _xplaneDATAMsgFloatIdx; +} + +void XPlaneAnalogIn::toString(FILE * outputStream) { + fprintf(outputStream, "lastRawRead=%f lastScaledRead=%f lastScaleRange=%d", + _lastUnscaledRead, _lastScaledRead, _lastScaleRange); +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/XPlaneIO/XPlaneAnalogIn.h Wed Dec 21 22:29:59 2011 +0000 @@ -0,0 +1,65 @@ +#ifndef XPLANEANALOGIN_H_INCLUDED +#define XPLANEANALOGIN_H_INCLUDED + +#include "mbed.h" +#include "Scaler.h" +#include "XPlaneUdpDATA.h" + +class XPlaneAnalogIn { +public: + + XPlaneAnalogIn(PinName pin, Scaler<float> scale1, Scaler<float> scale2, int msgIdx, int msgFloatIdx); + + /** + * Perform a read() on the AnalogIn pin, and return that value. + */ + float read_unscaled(); + + /** + * Perform a read() on the AnalogIn pin, and return the value scaled by the scale1 and scale2 ranges. + * The range used for the scaling is returned via the reference parameter: + * 0 = input fell in between the ranges (i.e. deadband zone), so scaled output is midpoint between scale1 outputTo and scale2 outputFrom + * 1 = input fell in the scale1 range + * 2 = input fell in the scale2 range + * 3 = input was less than scale1 inputFrom, so scaled output is (bounded by) scale1 outputFrom + * 4 = input was greater than scale2 inputTo, so scaled output is (bounded by) scale2 outputTo + * The scaledOutputNoChange reference parameter will be set true if the scaled value didn't changed since the last read. + */ + float read_scaled(int & range, bool & scaledOutputNoChange); + + + // Property accessors and mutators + + //Scaler<float> scale1(); + //void scale1(Scaler<float> scale1); + + //Scaler<float> scale2(); + //void scale2(Scaler<float> scale2); + + int xplaneDATAMsgIdx(); + //void xplaneDATAMsgIdx(int xplaneDATAMsgIdx); + + int xplaneDATAMsgFloatIdx(); + //void xplaneDATAMsgFloatIdx(int xplaneDATAMsgFloatIdx); + + /** + * Print a representation of this instance to outputStream. + */ + void toString(FILE * outputStream); + +private: + + PinName _pin; + Scaler<float> _scale1; + Scaler<float> _scale2; + int _xplaneDATAMsgIdx; + int _xplaneDATAMsgFloatIdx; + + AnalogIn _ain; + + float _lastUnscaledRead; + float _lastScaledRead; + int _lastScaleRange; +}; + +#endif // XPLANEANALOGIN_H_INCLUDED
--- /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; +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/XPlaneIO/XPlaneIO.h Wed Dec 21 22:29:59 2011 +0000 @@ -0,0 +1,223 @@ +#ifndef XPLANEIO_H_INCLUDED +#define XPLANEIO_H_INCLUDED + +#include <queue> +#include "mbed.h" +#include "EthernetNetIf.h" +#include "UDPSocket.h" +#include "XPlaneUdpDATA.h" +#include "XPlaneUdpDecoder.h" +#include "XPlaneUdpEncoder.h" +#include "XPlaneAnalogIn.h" + +#define UDP_RECV_BUFFER_SIZE 512 +#define UDP_SEND_BUFFER_SIZE 512 + +/** + * XPlaneIO is used to establish networked communication with X-Plane. + * Methods in this class allow the the client to send data to X-Plane as well as + * read data that has been sent from X-Plane. + */ + +class XPlaneIO { +public: + + //XPlaneIO(); + ~XPlaneIO(); + + /** + * Read configuration file and initialize. + */ + bool setup(char * configFilename, Ethernet * ethernet); + + /** + * Check if user wants to perform interactive XPlaneIO diagnostics. If so, enter diagnostics mode. + */ + void diagnostics(Serial & serialInOut); + + /** + * Include a DATA message, of the given index, in the UDP map -- for receiving from X-Plane. + * If a message of the given index already exists in the map, then do nothing. + * Returns true if a new DATA message was created and added to the map. + */ + bool addDATAToReceive(int msgIndex); + + /** + * Include a DATA message, of the given index, in the UDP map -- for sending to X-Plane. + * If a message of the given index already exists in the map, then do nothing. + * Returns true if a new DATA message was created and added to the map. + */ + bool addDATAToSend(int msgIndex); + + /** + * Return a pointer to the DATA message, of the given index, in the UDP receive map (data received from X-Plane). + * If a message of the given index does not exist in the map, then return NULL. + */ + XPlaneUdpDATA * getReceiveDATA(int msgIndex) const; + + /** + * Return a pointer to the DATA message, of the given index, in the UDP send map (data to send to X-Plane). + * If a message of the given index does not exist in the map, then return NULL. + */ + XPlaneUdpDATA * getSendDATA(int msgIndex) const; + + /** + * Return the float data value, of the given DATA message index, at the given float index (0-7), in the UDP receive map (data received from X-Plane). + * If a message of the given index does not exist in the map, then return -999.999f. + */ + float getReceiveDATAValue(int msgIndex, int floatIndex) const; + + /** + * Set the float data value, of the given DATA message index, at the given float index (0-7), in the UDP send map (data to send to X-Plane). + * If a message of the given index does not exist in the map, then do nothing. + */ + void setSendDATAValue(int msgIndex, int floatIndex, float f); + + /** + * Add an X-Plane DataRef to the queue -- to be sent the next time the + * sendUdp() ticker function is called. + */ + void sendDREF(XPlaneUdpDREF & dref); + + /** + * Start sending UDP data to X-Plane. + */ + void startSendingUdp(); + + /** + * Stop sending UDP data to X-Plane. + */ + void stopSendingUdp(); + + /** + * Utility method to send data via the UDPSocket to the sendHost. + */ + int udpSocketSend(char * buf, int len); + + // Accessors + bool debug() const; + bool reverseByteOrder() const; + XPlaneUdpDATAMap & recvDATAMap(); + XPlaneUdpDATAMap & sendDATAMap(); + +private: + + bool _debug; + bool _debugVerbose; + + //Serial & _serialInOut; + + EthernetNetIf * _eth; // pointer, because don't want instantiated until gather config for it + + // Set up local and remote hosts. + Host _recvHost; // mbed local host + Host _sendHost; // IP address and port to which UDP will be sent + + // UDP socket to use for both binding to local address (to recv data) and to send data to remote host. + UDPSocket _udpSocket; + + int _sendInterval; // milliseconds + Ticker _sendTicker; + + bool _reverseByteOrder; + + XPlaneUdpDATAMap _recvDATAMap; // keyed by DATA message index + XPlaneUdpDATAMap _sendDATAMap; // keyed by DATA message index + + queue<XPlaneUdpDREF> _sendDREFQueue; + + char _udpRecvBuffer[UDP_RECV_BUFFER_SIZE]; + XPlaneUdpDecoder _udpDecoder; + + char _udpSendBuffer[UDP_SEND_BUFFER_SIZE]; + XPlaneUdpEncoder _udpEncoder; + + XPlaneAnalogIn * _xpAnalogIn[6]; + int _xpAnalogInCount; + + /** + * Callback handler for when UDP data has been received. + * Note that this handler is assigned within the setup() method. + */ + void onUDPSocketEvent(UDPSocketEvent e); + + /** + * Ticker function for sending UDP data. + */ + void sendUdp(); + + /** + * Poll analog inputs, and... + */ + void pollAnalogIn(); +}; + +#endif // XPLANEIO_H_INCLUDED + + + +/* Config file: + +debug=Y +debugVerbose=N + +# If mbed_ip_address is not specified, then DHCP will be used. +mbed_ip_address=192 168 10 202 +mbed_ip_netmask=255 255 255 0 +mbed_ip_gateway=192 168 10 99 +mbed_ip_dnssrvr=192 168 10 99 + +# Ethernet link mode: 0 = AutoNegotiate, 1 = HalfDuplex10, 2 = FullDuplex10, 3 = HalfDuplex100, 4 = FullDuplex100 +# Note that AutoNegotiate does not work for 10 Mbit hub. e.g. Use 2 (FullDuplex10) for 10 Mbit hub that does not auto-detect speed. +ethernet_link_mode=0 + +# Local IP port to bind to. X-Plane should be configured to send data to the mbed_ip_address and this port. +recv_port=49000 + +# IP address and port to which UDP data will be sent. +xplane_ip_address=192 168 10 106 +xplane_ip_port=49000 + +# Interval (milliseconds) for sending UDP data to X-Plane. +send_interval=250 + +# Depending on X-Plane host computer (e.g Mac), byte order in UDP packets may need to be reversed. +reverse_byte_order=N + +# Analog Inputs - for sending an analog value to X-Plane. Up to six are supported (# = 1-6). +# ain_#_pin : AnalogIn pin number, 15-20 (p15-20) +# ain_#_scale1 : Scale range 1: input_from input_to output_from output_to +# Note: A gap between range1's input_to and range2's input_from will be seen as deadband; no data will be sent to X-Plane in that case. +# ain_#_scale2 : Scale range 2: input_from input_to output_from output_to +# ain_#_msg_idx : X-Plane DATA message: message index +# ain_#_float_idx : X-Plane DATA message: float index (0-7) within the message + +# Analog Input #1: +ain_1_pin=20 +ain_1_scale1=0.525 0.610 1.0 0.0 +ain_1_scale2=0.615 0.910 0.0 -1.0 +ain_1_msg_idx=8 +ain_1_float_idx=1 + +# Analog Input #2: +ain_2_pin=19 +ain_2_scale1=0.520 0.605 1.0 0.0 +ain_2_scale2=0.615 0.920 0.0 -1.0 +ain_2_msg_idx=8 +ain_2_float_idx=0 + +# Analog Input #3: +ain_3_pin=17 +ain_3_scale1=0.510 0.615 1.0 0.0 +ain_3_scale2=0.620 0.915 0.0 -1.0 +ain_3_msg_idx=8 +ain_3_float_idx=2 + + +# X-Plane DATA +# MsgIdx 8 : 0=elev, 1=ailrn, 2=ruddr +# MsgIdx 13 : 0=elev trim, 1=ailrn trim, 2=ruddr trim, 3=flap handl, 4=flap postn, 5=slat ratio, 6=sbrak handl, 7=sbrak postn +# MsgIdx 25 : 0=throttle command +# MsgIdx 26 : 0=throttle actual + +*/ \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/XPlaneIO/XPlaneIO_diag.cpp Wed Dec 21 22:29:59 2011 +0000 @@ -0,0 +1,43 @@ +#include "XPlaneIO.h" +#include "TimeoutPrompt.h" + +void XPlaneIO::diagnostics(Serial & serialInOut) { + + TimeoutPrompt top(serialInOut, 2); + + if (top.prompt("\nPress 'd' within 2 seconds for interactive diagnostics", "Dd") == NULL) { + return; + } + + char c; + + while ((c = top.prompt(30, "\n 1. Analog Input \n 2. \n 3. \n q. Quit \n Select within 30 seconds", "123Qq")) != NULL + && c != 'q' && c != 'Q') + { + serialInOut.printf("%c was typed \n\n", c); + + switch (c) { + case '1': + for (int i = 0; i < _xpAnalogInCount; i++) { + pollAnalogIn(); + serialInOut.printf("Analog Input #%d: ", i + 1); + _xpAnalogIn[i]->toString(serialInOut); + serialInOut.printf("\n"); + } + break; + /* + case '2': + break; + case '3': + break; + case 'a': + case 'A': + break; + */ + default: + serialInOut.printf("%c is not an option \n"); + } + } + + serialInOut.printf("Done \n"); +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/XPlaneUdp/XPlaneUdp.h Wed Dec 21 22:29:59 2011 +0000 @@ -0,0 +1,14 @@ +#ifndef XPLANEUDP_H_INCLUDED +#define XPLANEUDP_H_INCLUDED + +enum XPlaneUdpMessageType { + DATA, + UNKNOWN +}; + +/** + * Utility function to reverse the order of the four bytes at the given location. + */ +void reverse4Bytes(char* bytes); + +#endif // XPLANEUDP_H_INCLUDED \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/XPlaneUdp/XPlaneUdpDATA.cpp Wed Dec 21 22:29:59 2011 +0000 @@ -0,0 +1,71 @@ +#include "XPlaneUdpDATA.h" + + +XPlaneUdpDATA::XPlaneUdpDATA(int messageIndex, bool reverseByteOrder) { + + _messageIndex = messageIndex; + _reverseByteOrder = reverseByteOrder; + for (int i = 0; i < 8; i++) { + _data[i] = -999.0f; + } + + _dataChanged = false; +} + +int XPlaneUdpDATA::messageIndex() const { + return _messageIndex; +} + +float XPlaneUdpDATA::getData(int i) const { + return _data[i]; +} + +void XPlaneUdpDATA::setData(int i, float f) { + _data[i] = f; + setDataChanged(); +} + +float& XPlaneUdpDATA::operator [] (int i) { + return _data[i]; +} + +void XPlaneUdpDATA::setAllData(char * DATAMessage) { + memcpy(_data, DATAMessage + 4, 32); + if (_reverseByteOrder) { + for (int d = 0; d < 8; d++) { + reverse4Bytes((char*) &(_data[d])); + } + } +} + +void XPlaneUdpDATA::populateDATAMessage(char * bytes) { + memcpy(bytes, &_messageIndex, 4); + if (_reverseByteOrder) { + reverse4Bytes((char*) bytes); + } + memcpy(bytes + 4, _data, 32); + if (_reverseByteOrder) { + for (int d = 1; d < 9; d++) { + reverse4Bytes((char*) bytes + (d * 4)); + } + } +} + +bool XPlaneUdpDATA::isDataChanged() const { + return _dataChanged; +} + +void XPlaneUdpDATA::setDataChanged() { + _dataChanged = true; +} + +void XPlaneUdpDATA::resetDataChanged() { + _dataChanged = false; +} + +void XPlaneUdpDATA::toString(FILE * outputStream) { + fprintf(outputStream, "idx=%d, 0:%f 1:%f 2:%f 3:%f 4:%f 5:%f 6:%f 7:%f, revBytes=%d", + _messageIndex, + _data[0], _data[1], _data[2], _data[3], _data[4], _data[5], _data[6], _data[7], + _reverseByteOrder); +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/XPlaneUdp/XPlaneUdpDATA.h Wed Dec 21 22:29:59 2011 +0000 @@ -0,0 +1,68 @@ +#ifndef XPLANEUDPDATA_H_INCLUDED +#define XPLANEUDPDATA_H_INCLUDED + +#include <map> +#include "mbed.h" +#include "XPlaneUdp.h" + +#define XPLANE_DATA_MSG_LENGTH 36 // 1 int + 8 floats + +class XPlaneUdpDATA { +public: + + XPlaneUdpDATA(int messageIndex, bool reverseByteOrder); + + /** + * Accessor for the message index of this DATA message. + */ + int messageIndex() const; + + /** + * Accessor for the float data at the given float index (0-7). + */ + float getData(int i) const; + + /** + * Mutator for the float data at the given float index (0-7). + * The dataChanged flag will also be set. + */ + void setData(int i, float f); + + /** + * Overload the [] operator to return a reference to the float data at the given float index (0-7). + * NOTE: The dataChanged flag will not be set if this is used to modify the float data. + */ + float& operator [] (int i); + + /** + * Fill data[8] with 32 bytes starting at (DATAMessage + 4). + * Byte order will be reversed, according to the reverseByteOrder property. + */ + void setAllData(char * DATAMessage); + + /** + * Starting at the given pointer location, populate 36 bytes from values in index and data[8]. + * Byte order will be reversed, according to the reverseByteOrder property. + */ + void populateDATAMessage(char * bytes); + + // Methods to read/write flag to track if data has been modified. + bool isDataChanged() const; + void setDataChanged(); + void resetDataChanged(); + + /** + * Print a representation of this instance to outputStream. + */ + void toString(FILE * outputStream); + +private: + + int _messageIndex; + float _data[8]; + bool _reverseByteOrder; + bool _dataChanged; +}; + + +#endif // XPLANEUDPDATA_H_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/XPlaneUdp/XPlaneUdpDATAMap.cpp Wed Dec 21 22:29:59 2011 +0000 @@ -0,0 +1,39 @@ +#include "XPlaneUdpDATAMap.h" + +XPlaneUdpDATA * getXPlaneUdpDATA(const XPlaneUdpDATAMap & DATAMap, int index) { + XPlaneUdpDATAMap::iterator iter = DATAMap.find(index); + if (iter != DATAMap.end()) { + return iter->second; + } + else { + return NULL; + } +} + +bool addXPlaneUdpDATAToMap(XPlaneUdpDATAMap & DATAMap, int index, bool reverseByteOrder) { + XPlaneUdpDATA *DATAmsg = getXPlaneUdpDATA(DATAMap, index); + if (DATAmsg != NULL) { + return false; + } + else { + DATAmsg = new XPlaneUdpDATA(index, reverseByteOrder); + DATAMap[index] = DATAmsg; + return true; + } +} + +void resetDataChangedInMap(XPlaneUdpDATAMap & DATAMap) { + for (XPlaneUdpDATAMap::iterator it = DATAMap.begin(); it != DATAMap.end(); it++) { + (it->second)->resetDataChanged(); + } +} + +void printXPlaneUdpDATAMap(FILE * outputStream, XPlaneUdpDATAMap & DATAMap) { + fprintf(outputStream, "Map:\n"); + for (XPlaneUdpDATAMap::iterator it = DATAMap.begin(); it != DATAMap.end(); it++) { + //(it->second)->resetDataChanged(); + fprintf(outputStream, " key: %d value: ", it->first); + (it->second)->toString(outputStream); + fprintf(outputStream, "\n"); + } +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/XPlaneUdp/XPlaneUdpDATAMap.h Wed Dec 21 22:29:59 2011 +0000 @@ -0,0 +1,35 @@ +#ifndef XPLANEUDPDATAMAP_H_INCLUDED +#define XPLANEUDPDATAMAP_H_INCLUDED + +#include <map> +#include "XPlaneUdpDATA.h" + +/** + * Map of XPlaneUdpDATA pointers, keyed by int (e.g. keyed by the DATA message index). + */ +typedef std::map<int, XPlaneUdpDATA *> XPlaneUdpDATAMap; + +/** + * Function to return the XPlaneUdpDATA pointer in the given map, keyed by the given int index. + * If no map entry exists for the given key index, then returns NULL. + */ +XPlaneUdpDATA * getXPlaneUdpDATA(const XPlaneUdpDATAMap & DATAMap, int index); + +/** + * Function to include a DATA message, of the given index, in the given map. + * If a message of the given index already exists in the map, then do nothing. + * Returns true if a new DATA message was created and added to the map. + */ +bool addXPlaneUdpDATAToMap(XPlaneUdpDATAMap & DATAMap, int index, bool reverseByteOrder); + +/** + * Function to reset the dataChanged flag in all DATA messages in the given map. + */ +void resetDataChangedInMap(XPlaneUdpDATAMap & DATAMap); + +/** + * Utility function to print the DATA messages in the given map. + */ +void printXPlaneUdpDATAMap(FILE * outputStream, XPlaneUdpDATAMap & DATAMap); + +#endif // XPLANEUDPDATAMAP_H_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/XPlaneUdp/XPlaneUdpDREF.cpp Wed Dec 21 22:29:59 2011 +0000 @@ -0,0 +1,75 @@ +#include "XPlaneUdpDREF.h" + + +XPlaneUdpDREF::XPlaneUdpDREF(char * drefName, bool reverseByteOrder) { + + _drefName = drefName; + _reverseByteOrder = reverseByteOrder; + + _dataChanged = false; + _drefFloat = DREF_VALUE_NA; + _drefInt = DREF_VALUE_NA; +} + + +char * XPlaneUdpDREF::drefName() const { + return _drefName; +} + +float XPlaneUdpDREF::drefFloat() const { + return _drefFloat; +} + +int XPlaneUdpDREF::drefInt() const { + return _drefInt; +} + +void XPlaneUdpDREF::drefFloat(float floatVal) { + _drefFloat = floatVal; + _drefInt = DREF_VALUE_NA; + setDataChanged(); +} + +void XPlaneUdpDREF::drefInt(int intVal) { + _drefInt = intVal; + _drefFloat = DREF_VALUE_NA; + setDataChanged(); +} + + /** + * Starting at the given pointer location, populate bytes from the dataRef value and name. + * Byte order will be reversed, according to the reverseBytes property. + */ + +void XPlaneUdpDREF::populateDREFMessage(char * bytes) { + + if (_drefInt != DREF_VALUE_NA) { + memcpy(bytes, &_drefInt, 4); + } + else { + memcpy(bytes, &_drefFloat, 4); + } + + if (_reverseByteOrder) { + reverse4Bytes((char*) bytes); + } + + strcpy(bytes + 4, _drefName); +} + +bool XPlaneUdpDREF::isDataChanged() const { + return _dataChanged; +} + +void XPlaneUdpDREF::setDataChanged() { + _dataChanged = true; +} + +void XPlaneUdpDREF::resetDataChanged() { + _dataChanged = false; +} + +void XPlaneUdpDREF::toString(FILE * outputStream) { + fprintf(outputStream, "DataRef: name=%s, intVal=%d, floatVal=%f, revBytes=%d", + _drefName, _drefInt, _drefFloat, _reverseByteOrder); +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/XPlaneUdp/XPlaneUdpDREF.h Wed Dec 21 22:29:59 2011 +0000 @@ -0,0 +1,75 @@ +#ifndef XPLANEUDPDREF_H_INCLUDED +#define XPLANEUDPDREF_H_INCLUDED + +#include <map> +#include "mbed.h" +#include "XPlaneUdp.h" + +#define DREF_VALUE_NA -999 + +/** + * X-Plane DataRef + */ +class XPlaneUdpDREF { +public: + + XPlaneUdpDREF(char * drefName, bool reverseByteOrder); + + /** + * Accessor for the DataRef name. + */ + char * drefName() const; + + /** + * Accessor for the DataRef float data value. + * Returns -999 if not applicable (this DataRef uses an int value). + */ + float drefFloat() const; + + /** + * Accessor for the DataRef int data value. + * Returns -999 if not applicable (this DataRef uses a float value). + */ + int drefInt() const; + + /** + * Mutator for the DataRef float data value. + * The DataRef int data will be set to -999. + * The dataChanged flag will also be set. + */ + void drefFloat(float floatVal); + + /** + * Mutator for the DataRef int data value. + * The DataRef float data will be set to -999. + * The dataChanged flag will also be set. + */ + void drefInt(int intVal); + + /** + * Starting at the given pointer location, populate bytes from the DataRef value and name. + * Byte order will be reversed, according to the reverseByteOrder property. + */ + void populateDREFMessage(char * bytes); + + // Methods to read/write flag to track if data has been modified. + bool isDataChanged() const; + void setDataChanged(); + void resetDataChanged(); + + /** + * Print a representation of this instance to outputStream. + */ + void toString(FILE * outputStream); + +private: + + char * _drefName; + float _drefFloat; + int _drefInt; + bool _reverseByteOrder; + bool _dataChanged; +}; + + +#endif // XPLANEUDPDREF_H_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/XPlaneUdp/XPlaneUdpDecoder.cpp Wed Dec 21 22:29:59 2011 +0000 @@ -0,0 +1,69 @@ +#include <stdio.h> +#include <string.h> +#include "XPlaneUdp.h" +#include "XPlaneUdpDecoder.h" +#include "XPlaneUdpDATA.h" + +void XPlaneUdpDecoder::setDebug(bool debug) { + _debug = debug; +} + +void XPlaneUdpDecoder::setReverseByteOrder(bool reverseByteOrder) { + _reverseByteOrder = reverseByteOrder; +} + +XPlaneUdpMessageType XPlaneUdpDecoder::setUdpBuffer(char * buf, int bufLen) { + _udpPacketMsgType = UNKNOWN; + _udpBuffer = buf; + _udpBufferLen = bufLen; + //_udpBuffer[bufLen] = 0; // only need to do this if treating buffer like a string, e.g. printing it + + if (_udpBufferLen >= 5) { + if (strncmp(_udpBuffer, "DATA", 4) == 0) { + if (_debug) printf("Got DATA pkt \n"); + _udpPacketMsgType = DATA; + } + } + + return _udpPacketMsgType; +} + +int XPlaneUdpDecoder::putIntoDATAMap(XPlaneUdpDATAMap & DATAMap, bool filter) { + int updateCount = 0; + + if (_udpPacketMsgType != DATA) { + return 0; + } + + if (filter && (DATAMap.size() == 0)) { + return 0; + } + + int dataMsgCount = (_udpBufferLen - 5) / XPLANE_DATA_MSG_LENGTH; + if (_debug) printf("%d msgs in pkt \n", dataMsgCount); + int bufOffset = 5; + for (int msgNbr = 0; msgNbr < dataMsgCount; msgNbr++) { + + int index; + memcpy(&index, _udpBuffer + bufOffset, 4); + if (_reverseByteOrder) { + reverse4Bytes((char*) &index); + } + + XPlaneUdpDATA *DATAmsg = getXPlaneUdpDATA(DATAMap, index); + if (DATAmsg != NULL) { + DATAmsg->setAllData(_udpBuffer + bufOffset); + updateCount++; + } + else if (! filter) { + DATAmsg = new XPlaneUdpDATA(index, _reverseByteOrder); + DATAmsg->setAllData(_udpBuffer + bufOffset); + DATAMap[index] = DATAmsg; + updateCount++; + } + + bufOffset += XPLANE_DATA_MSG_LENGTH; + } + + return updateCount; +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/XPlaneUdp/XPlaneUdpDecoder.h Wed Dec 21 22:29:59 2011 +0000 @@ -0,0 +1,43 @@ +#ifndef XPLANEUDPDECODER_H_INCLUDED +#define XPLANEUDPDECODER_H_INCLUDED + +#include "XPlaneUdp.h" +#include "XPlaneUdpDATA.h" +#include "XPlaneUdpDATAMap.h" + +class XPlaneUdpDecoder { +public: + + //XPlaneUdpDecoder(); + + void setDebug(bool debug); + void setReverseByteOrder(bool reverseByteOrder); + + /** + * This must be called prior to other methods such as putIntoDATAMap(). + * Point this decoder to the UDP data buffer, and read the first four bytes + * to determine the X-Plane UDP message type (e.g. DATA). + */ + XPlaneUdpMessageType setUdpBuffer(char * buf, int bufLen); + + /** + * Note: The setUdpBuffer() method must be called prior to calling this method. + * Decode the UDP packet. If it is a DATA packet from X-Plane, then populate + * XPlaneUdpDATA structure(s) in the map. + * If filter==true, then only existing structures in the map (keyed by message DATA msg index) + * will be populated, i.e. no new structures will be created in the map. + * Return the number of XPlaneUdpDATA structures populated/updated in the map. + */ + int putIntoDATAMap(XPlaneUdpDATAMap & DATAMap, bool filter); + +private: + + bool _debug; + bool _reverseByteOrder; + char *_udpBuffer; + int _udpBufferLen; + XPlaneUdpMessageType _udpPacketMsgType; +}; + + +#endif // XPLANEUDPDECODER_H_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/XPlaneUdp/XPlaneUdpEncoder.cpp Wed Dec 21 22:29:59 2011 +0000 @@ -0,0 +1,57 @@ +#include <stdio.h> +#include <string.h> +#include "XPlaneUdp.h" +#include "XPlaneUdpEncoder.h" +#include "XPlaneUdpDATA.h" + +void XPlaneUdpEncoder::setDebug(bool debug) { + _debug = debug; +} + +int XPlaneUdpEncoder::encodeFromDATAMap(char * udpBuffer, int maxBufferLen, XPlaneUdpDATAMap & DATAMap, bool changedDataOnly) { + if (maxBufferLen < 5 || DATAMap.size() == 0) { + return 0; + } + + strcpy(udpBuffer, "DATA"); + int bufOffset = 5; + + for (XPlaneUdpDATAMap::iterator it = DATAMap.begin(); it != DATAMap.end(); it++) { + if ((bufOffset + XPLANE_DATA_MSG_LENGTH) > maxBufferLen) { + break; + } + else { + XPlaneUdpDATA * DATAmsg = it->second; + if ((!changedDataOnly) || DATAmsg->isDataChanged()) { + DATAmsg->populateDATAMessage(udpBuffer + bufOffset); + bufOffset += XPLANE_DATA_MSG_LENGTH; + } + } + } + + if (_debug) printf("encodeFromDATAMap(): Encoded %d bytes into UDP buffer \n", bufOffset); + + return bufOffset; +} + + +int XPlaneUdpEncoder::encodeDataRef(char * udpBuffer, int maxBufferLen, XPlaneUdpDREF & dref) { + if (maxBufferLen < DATAREF_PACKET_SIZE) { + return 0; + } + + memset(udpBuffer, '\0' , DATAREF_PACKET_SIZE); // zero out the buffer + + strcpy(udpBuffer, "DREF"); + int bufOffset = 5; + + dref.populateDREFMessage(udpBuffer + bufOffset); + + if (_debug) { + printf("encodeDataRef(): Encoded DREF packet into UDP buffer: "); + dref.toString(stdout); + printf("\n"); + } + + return DATAREF_PACKET_SIZE; +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/XPlaneUdp/XPlaneUdpEncoder.h Wed Dec 21 22:29:59 2011 +0000 @@ -0,0 +1,41 @@ +#ifndef XPLANEUDPENCODER_H_INCLUDED +#define XPLANEUDPENCODER_H_INCLUDED + +#include "XPlaneUdp.h" +#include "XPlaneUdpDATA.h" +#include "XPlaneUdpDATAMap.h" +#include "XPlaneUdpDREF.h" + +#define DATAREF_PACKET_SIZE 509 // "DREF" + '\x00' + 4-byte_value + 500-char_name + +class XPlaneUdpEncoder { +public: + + //XPlaneUdpEncoder(); + + void setDebug(bool debug); + + /** + * Encode a DATA packet from the messages in the given XPlaneUdpDATAMap. + * The encoded data will be placed at the location given by udpBuffer. + * If changedDataOnly is true, then only XPlaneUdpDATA messages with dataChanged will be encoded. + * Return number of bytes encoded into udpBuffer. + * Return -1 if there's more data in the map than what will fit in udpBuffer (> maxBufferLen). + */ + int encodeFromDATAMap(char * udpBuffer, int maxBufferLen, XPlaneUdpDATAMap & DATAMap, bool changedDataOnly); + + /** + * Encode a DataRef (DREF) packet, using the given DataRef. + * The encoded data will be placed at the location given by udpBuffer. + * Return number of bytes encoded into udpBuffer. + * Return -1 if there's more data in the map than what will fit in udpBuffer (> maxBufferLen). + */ + int encodeDataRef(char * udpBuffer, int maxBufferLen, XPlaneUdpDREF & dref); + +private: + + bool _debug; +}; + + +#endif // XPLANEUDPENCODER_H_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/XPlaneUdp/XPlaneUdp_util.cpp Wed Dec 21 22:29:59 2011 +0000 @@ -0,0 +1,11 @@ + +void reverse4Bytes(char* bytes) +{ + char temp; + temp = bytes[0]; + bytes[0] = bytes[3]; + bytes[3] = temp; + temp = bytes[1]; + bytes[1] = bytes[2]; + bytes[2] = temp; +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Dec 21 22:29:59 2011 +0000 @@ -0,0 +1,74 @@ +#include "mbed.h" +#include "EthernetNetIf.h" +#include "Ranger.h" +#include "XPlaneIO.h" + +Serial pc(USBTX, USBRX); + +int main() { + + pc.baud(57600); + + Ethernet ethernet; + + XPlaneIO xpio; + if (! xpio.setup("XPlaneIO.cfg", ðernet)) { + printf("Config/setup failed \n"); + return -1; + } + + // Check if user wants to perform interactive XPlaneIO diagnostics. + xpio.diagnostics(pc); + + AnalogOut throttleActual(p18); + xpio.addDATAToReceive(26); // "throttle actual" in msg 26, data[0] + + AnalogIn hatSwitch(p16); + XPlaneUdpDREF pilotHeadHeading("sim/graphics/view/pilots_head_psi", xpio.reverseByteOrder()); + Ranger<float> hatSwitchInputRanger(4, 0.59, 0.68, 0.78, 0.92); + float hatSwitchDegreesByRange[5] = {0, -90, 180, 90, 0}; + + Timer timerThrottle; + timerThrottle.start(); + + Timer timerHatSwitch; + timerHatSwitch.start(); + + Timer timerDebugPrint; + if (xpio.debug()) { + timerDebugPrint.start(); + } + + xpio.startSendingUdp(); + + while (1) { + Net::poll(); + + if (timerThrottle.read_ms() > 500) { + timerThrottle.reset(); + + throttleActual.write(xpio.getReceiveDATAValue(26, 0)); // throttle actual + } + + if (timerHatSwitch.read_ms() > 250) { + timerHatSwitch.reset(); + + int newRange = hatSwitchInputRanger.range(hatSwitch.read()); + if (hatSwitchInputRanger.rangeChanged()) { + pilotHeadHeading.drefFloat(hatSwitchDegreesByRange[newRange]); + xpio.sendDREF(pilotHeadHeading); + } + } + + if (xpio.debug()) { + if (timerDebugPrint.read() > 2) { + timerDebugPrint.reset(); + + printf("Receive "); + printXPlaneUdpDATAMap(stdout, xpio.recvDATAMap()); + printf("Send "); + printXPlaneUdpDATAMap(stdout, xpio.sendDATAMap()); + } + } + } +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Dec 21 22:29:59 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/63bcd7ba4912