diff --git a/src/Network/generic.cxx b/src/Network/generic.cxx index 8058ed29c..63b5526c2 100644 --- a/src/Network/generic.cxx +++ b/src/Network/generic.cxx @@ -94,7 +94,14 @@ bool FGGeneric::gen_message() { val = _out_message[i].offset + _out_message[i].prop->getIntValue() * _out_message[i].factor; if (binary_mode) { - *((int32_t*)&buf[length]) = (int32_t)val; + if (binary_byte_order == HOST_BYTE_ORDER) { + *((int32_t*)&buf[length]) = (int32_t)val; + } else { + buf[length + 0] = (int8_t)((int32_t)val >> 24); + buf[length + 1] = (int8_t)((int32_t)val >> 16); + buf[length + 2] = (int8_t)((int32_t)val >> 8); + buf[length + 3] = (int8_t)val; + } length += sizeof(int32_t); } else { snprintf(tmp, 255, _out_message[i].format.c_str(), (int)val); @@ -112,11 +119,36 @@ bool FGGeneric::gen_message() { } break; + case FG_FIXED: + val = _out_message[i].offset + + _out_message[i].prop->getFloatValue() * _out_message[i].factor; + if (binary_mode) { + int fixed = (int)(val * 65536.0f); + if (binary_byte_order == HOST_BYTE_ORDER) { + *((int32_t*)&buf[length]) = (int32_t)fixed; + } else { + buf[length + 0] = (int8_t)(fixed >> 24); + buf[length + 1] = (int8_t)(fixed >> 16); + buf[length + 2] = (int8_t)(fixed >> 8); + buf[length + 3] = (int8_t)fixed; + } + length += sizeof(int32_t); + } else { + snprintf(tmp, 255, _out_message[i].format.c_str(), (float)val); + } + break; + case FG_DOUBLE: val = _out_message[i].offset + _out_message[i].prop->getFloatValue() * _out_message[i].factor; if (binary_mode) { - *((double*)&buf[length]) = val; + if (binary_byte_order == HOST_BYTE_ORDER) { + *((double*)&buf[length]) = val; + } else { + SG_LOG( SG_IO, SG_ALERT, "Generic protocol: " + "FG_DOUBLE will be written in host byte order."); + *((double*)&buf[length]) = val; + } length += sizeof(double); } else { snprintf(tmp, 255, _out_message[i].format.c_str(), (float)val); @@ -128,13 +160,17 @@ bool FGGeneric::gen_message() { const char *strdata = _out_message[i].prop->getStringValue(); int strlength = strlen(strdata); + if (binary_byte_order == NETWORK_BYTE_ORDER) { + SG_LOG( SG_IO, SG_ALERT, "Generic protocol: " + "FG_STRING will be written in host byte order."); + } /* Format for strings is * [length as int, 4 bytes][ASCII data, length bytes] */ *((int32_t*)&buf[length]) = strlength; - length += sizeof(int32_t); - strncpy(&buf[length], strdata, strlength); - length += strlength; + length += sizeof(int32_t); + strncpy(&buf[length], strdata, strlength); + length += strlength; /* FIXME padding for alignment? Something like: * length += (strlength % 4 > 0 ? sizeof(int32_t) - strlength % 4 : 0; */ @@ -180,40 +216,90 @@ bool FGGeneric::parse_message() { char *p2, *p1 = buf; double val; int i = -1; + int tmp; - if (binary_mode) - SG_LOG( SG_IO, SG_ALERT, - "generic protocol: binary mode input is not yet implemented."); - - while ((++i < (int)_in_message.size()) && - p1 && strcmp(p1, line_separator.c_str())) { + if (!binary_mode) { + while ((++i < (int)_in_message.size()) && + p1 && strcmp(p1, line_separator.c_str())) { - p2 = strstr(p1, var_separator.c_str()); - if (p2) { - *p2 = 0; - p2 += var_separator.length(); + p2 = strstr(p1, var_separator.c_str()); + if (p2) { + *p2 = 0; + p2 += var_separator.length(); + } + + switch (_in_message[i].type) { + case FG_INT: + val = _in_message[i].offset + atoi(p1) * _in_message[i].factor; + _in_message[i].prop->setIntValue((int)val); + break; + + case FG_BOOL: + _in_message[i].prop->setBoolValue( atof(p1) != 0.0 ); + break; + + case FG_FIXED: + case FG_DOUBLE: + val = _in_message[i].offset + strtod(p1, 0) * _in_message[i].factor; + _in_message[i].prop->setFloatValue((float)val); + break; + + default: // SG_STRING + _in_message[i].prop->setStringValue(p1); + } + + p1 = p2; } + } else { + /* Binary mode */ + while ((++i < (int)_in_message.size()) && + (p1 - buf < FG_MAX_MSG_SIZE)) { - switch (_in_message[i].type) { - case FG_INT: - val = _in_message[i].offset + atoi(p1) * _in_message[i].factor; - _in_message[i].prop->setIntValue((int)val); - break; + switch (_in_message[i].type) { + case FG_INT: + if (binary_byte_order == NETWORK_BYTE_ORDER) { + tmp = + (((p1[0]) & 0xff) << 24) | + (((p1[1]) & 0xff) << 16) | + (((p1[2]) & 0xff) << 8) | + ((p1[3]) & 0xff); + } else { + tmp = *(int32_t *)p1; + } + val = _in_message[i].offset + + (double)tmp * + _in_message[i].factor; + _in_message[i].prop->setIntValue((int)val); + p1 += sizeof(int32_t); + break; - case FG_BOOL: - _in_message[i].prop->setBoolValue( atof(p1) != 0.0 ); - break; + case FG_BOOL: + _in_message[i].prop->setBoolValue( p1[0] != 0.0 ); + p1 += 1; + break; - case FG_DOUBLE: - val = _in_message[i].offset + strtod(p1, 0) * _in_message[i].factor; - _in_message[i].prop->setFloatValue((float)val); - break; + case FG_FIXED: + if (binary_byte_order == NETWORK_BYTE_ORDER) { + tmp = + (((p1[0]) & 0xff) << 24) | + (((p1[1]) & 0xff) << 16) | + (((p1[2]) & 0xff) << 8) | + ((p1[3]) & 0xff); + } else { + tmp = *(int32_t *)p1; + } + val = _in_message[i].offset + + ((double)tmp / 65536.0f) * _in_message[i].factor; + _in_message[i].prop->setFloatValue(val); + p1 += sizeof(int32_t); + break; - default: // SG_STRING - _in_message[i].prop->setStringValue(p1); + case FG_DOUBLE: + default: // SG_STRING + SG_LOG( SG_IO, SG_ALERT, "Generic protocol: " + "Ignoring unsupported binary input chunk type."); + } } - - p1 = p2; } return true; @@ -260,11 +346,30 @@ bool FGGeneric::process() { goto error_out; } } else if ( get_direction() == SG_IO_IN ) { - if ( (length = io->readline( buf, FG_MAX_MSG_SIZE )) > 0 ) { - parse_message(); + if (!binary_mode) { + if ( (length = io->readline( buf, FG_MAX_MSG_SIZE )) > 0 ) { + parse_message(); + } else { + SG_LOG( SG_IO, SG_ALERT, "Error reading data." ); + return false; + } } else { - SG_LOG( SG_IO, SG_ALERT, "Error reading data." ); - goto error_out; + if ( (length = io->read( buf, binary_record_length )) > 0 ) { + if (length != binary_record_length) { + SG_LOG( SG_IO, SG_ALERT, + "Generic protocol: Received binary " + "record of unexpected size." ); + } else { + SG_LOG( SG_IO, SG_DEBUG, + "Generic protocol: received record of " << length << + " bytes."); + parse_message(); + } + } else { + SG_LOG( SG_IO, SG_INFO, + "Generic protocol: Error reading data." ); + return false; + } } } return true; @@ -346,6 +451,8 @@ FGGeneric::read_config(SGPropertyNode *root, vector<_serial_prot> &msg) line_separator = line_sep_string; } else { binary_footer_type = FOOTER_NONE; // default choice + binary_record_length = -1; // default choice = sizeof(representation) + binary_byte_order = HOST_BYTE_ORDER; // default choice if ( root->hasValue("binary_footer") ) { string footer_type = root->getStringValue("binary_footer"); if ( footer_type == "length" ) @@ -359,8 +466,24 @@ FGGeneric::read_config(SGPropertyNode *root, vector<_serial_prot> &msg) "generic protocol: Undefined generic binary protocol" "footer, using no footer."); } + if ( root->hasValue("record_length") ) { + binary_record_length = root->getIntValue("record_length"); + } + if ( root->hasValue("byte_order") ) { + string byte_order = root->getStringValue("byte_order"); + if ( byte_order == "network" ) { + binary_byte_order = NETWORK_BYTE_ORDER; + } else if ( byte_order == "host" ) { + binary_byte_order = HOST_BYTE_ORDER; + } else { + SG_LOG(SG_IO, SG_ALERT, + "generic protocol: Undefined generic binary protocol" + "byte order, using HOST byte order."); + } + } } + int record_length = 0; // Only used for binary protocols. vector chunks = root->getChildren("chunk"); for (unsigned int i = 0; i < chunks.size(); i++) { @@ -375,18 +498,31 @@ FGGeneric::read_config(SGPropertyNode *root, vector<_serial_prot> &msg) chunk.prop = fgGetNode(node.c_str(), true); string type = chunks[i]->getStringValue("type"); - if (type == "bool") + if (type == "bool") { chunk.type = FG_BOOL; - else if (type == "float") + record_length += 1; + } else if (type == "float") { chunk.type = FG_DOUBLE; - else if (type == "string") + record_length += sizeof(double); + } else if (type == "fixed") { + chunk.type = FG_FIXED; + record_length += sizeof(int32_t); + } else if (type == "string") chunk.type = FG_STRING; - else + else { chunk.type = FG_INT; - + record_length += sizeof(int32_t); + } msg.push_back(chunk); } + if (binary_record_length == -1) { + binary_record_length = record_length; + } else if (binary_record_length < record_length) { + SG_LOG(SG_IO, SG_ALERT, + "generic protocol: Requested binary record length shorter than" + " requested record representation."); + binary_record_length = record_length; + } } - diff --git a/src/Network/generic.hxx b/src/Network/generic.hxx index 1d07b1265..f706bbb83 100644 --- a/src/Network/generic.hxx +++ b/src/Network/generic.hxx @@ -57,7 +57,7 @@ public: bool getExitOnError() { return exitOnError; } protected: - enum e_type { FG_BOOL=0, FG_INT, FG_DOUBLE, FG_STRING }; + enum e_type { FG_BOOL=0, FG_INT, FG_DOUBLE, FG_STRING, FG_FIXED }; typedef struct { // string name; @@ -85,6 +85,8 @@ private: bool binary_mode; enum {FOOTER_NONE, FOOTER_LENGTH, FOOTER_MAGIC} binary_footer_type; int binary_footer_value; + int binary_record_length; + enum {NETWORK_BYTE_ORDER, HOST_BYTE_ORDER} binary_byte_order; void read_config(SGPropertyNode *root, vector<_serial_prot> &msg); bool exitOnError;