split up binary and ascii message handling functions, mostly for readability
This commit is contained in:
ehofman
Tim Moore
parent
14f4354523
commit
a9c47a7b78
2 changed files with 290 additions and 225 deletions
|
|
@ -54,6 +54,12 @@ FGGeneric::FGGeneric(vector<string> tokens) : exitOnError(false)
|
|||
configToken = 6;
|
||||
}
|
||||
|
||||
if (configToken >= tokens.size()) {
|
||||
SG_LOG(SG_GENERAL, SG_ALERT,
|
||||
"Not enough tokens passed for generic protocol");
|
||||
return;
|
||||
}
|
||||
|
||||
string config = tokens[ configToken ];
|
||||
string file = config+".xml";
|
||||
|
||||
|
|
@ -93,7 +99,7 @@ FGGeneric::~FGGeneric() {
|
|||
|
||||
|
||||
// generate the message
|
||||
bool FGGeneric::gen_message() {
|
||||
bool FGGeneric::gen_message_binary() {
|
||||
string generic_sentence;
|
||||
char tmp[255];
|
||||
length = 0;
|
||||
|
|
@ -101,40 +107,30 @@ bool FGGeneric::gen_message() {
|
|||
double val;
|
||||
for (unsigned int i = 0; i < _out_message.size(); i++) {
|
||||
|
||||
if (i > 0 && !binary_mode)
|
||||
generic_sentence += var_separator;
|
||||
|
||||
switch (_out_message[i].type) {
|
||||
case FG_INT:
|
||||
val = _out_message[i].offset +
|
||||
_out_message[i].prop->getIntValue() * _out_message[i].factor;
|
||||
if (binary_mode) {
|
||||
|
||||
if (binary_byte_order == BYTE_ORDER_MATCHES_NETWORK_ORDER) {
|
||||
*((int32_t*)&buf[length]) = (int32_t)val;
|
||||
} else {
|
||||
*((uint32_t*)&buf[length]) = sg_bswap_32((uint32_t)val);
|
||||
}
|
||||
length += sizeof(int32_t);
|
||||
} else {
|
||||
snprintf(tmp, 255, _out_message[i].format.c_str(), (int)val);
|
||||
}
|
||||
break;
|
||||
|
||||
case FG_BOOL:
|
||||
if (binary_mode) {
|
||||
*((int8_t*)&buf[length])
|
||||
= _out_message[i].prop->getBoolValue() ? true : false;
|
||||
length += 1;
|
||||
} else {
|
||||
snprintf(tmp, 255, _out_message[i].format.c_str(),
|
||||
_out_message[i].prop->getBoolValue());
|
||||
}
|
||||
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 == BYTE_ORDER_MATCHES_NETWORK_ORDER) {
|
||||
*((int32_t*)&buf[length]) = (int32_t)fixed;
|
||||
|
|
@ -142,43 +138,33 @@ bool FGGeneric::gen_message() {
|
|||
*((uint32_t*)&buf[length]) = sg_bswap_32((uint32_t)val);
|
||||
}
|
||||
length += sizeof(int32_t);
|
||||
} else {
|
||||
snprintf(tmp, 255, _out_message[i].format.c_str(), (float)val);
|
||||
}
|
||||
break;
|
||||
|
||||
}
|
||||
case FG_FLOAT:
|
||||
val = _out_message[i].offset +
|
||||
_out_message[i].prop->getFloatValue() * _out_message[i].factor;
|
||||
if (binary_mode) {
|
||||
|
||||
if (binary_byte_order == BYTE_ORDER_MATCHES_NETWORK_ORDER) {
|
||||
*((float*)&buf[length]) = val;
|
||||
} else {
|
||||
*((float*)&buf[length]) = sg_bswap_32(*(uint32_t*)&val);
|
||||
}
|
||||
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) {
|
||||
|
||||
if (binary_byte_order == BYTE_ORDER_MATCHES_NETWORK_ORDER) {
|
||||
*((double*)&buf[length]) = val;
|
||||
} else {
|
||||
*((double*)&buf[length]) = sg_bswap_64(*(uint64_t*)&val);
|
||||
}
|
||||
length += sizeof(int64_t);
|
||||
} else {
|
||||
snprintf(tmp, 255, _out_message[i].format.c_str(), (float)val);
|
||||
}
|
||||
break;
|
||||
|
||||
default: // SG_STRING
|
||||
if (binary_mode) {
|
||||
const char *strdata = _out_message[i].prop->getStringValue();
|
||||
int strlength = strlen(strdata);
|
||||
|
||||
|
|
@ -200,26 +186,9 @@ bool FGGeneric::gen_message() {
|
|||
/* FIXME padding for alignment? Something like:
|
||||
* length += (strlength % 4 > 0 ? sizeof(int32_t) - strlength % 4 : 0;
|
||||
*/
|
||||
} else {
|
||||
snprintf(tmp, 255, _out_message[i].format.c_str(),
|
||||
_out_message[i].prop->getStringValue());
|
||||
}
|
||||
}
|
||||
|
||||
if (!binary_mode) {
|
||||
generic_sentence += tmp;
|
||||
}
|
||||
}
|
||||
|
||||
if (!binary_mode) {
|
||||
/* After each lot of variables has been added, put the line separator
|
||||
* char/string
|
||||
*/
|
||||
generic_sentence += line_separator;
|
||||
|
||||
length = generic_sentence.length();
|
||||
strncpy( buf, generic_sentence.c_str(), length );
|
||||
} else {
|
||||
// add the footer to the packet ("line")
|
||||
switch (binary_footer_type) {
|
||||
case FOOTER_LENGTH:
|
||||
|
|
@ -229,6 +198,7 @@ bool FGGeneric::gen_message() {
|
|||
case FOOTER_MAGIC:
|
||||
break;
|
||||
}
|
||||
|
||||
if (binary_footer_type != FOOTER_NONE) {
|
||||
if (binary_byte_order == BYTE_ORDER_MATCHES_NETWORK_ORDER) {
|
||||
*((int32_t*)&buf[length]) = binary_footer_value;
|
||||
|
|
@ -237,17 +207,163 @@ bool FGGeneric::gen_message() {
|
|||
}
|
||||
length += sizeof(int32_t);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool FGGeneric::gen_message_ascii() {
|
||||
string generic_sentence;
|
||||
char tmp[255];
|
||||
length = 0;
|
||||
|
||||
double val;
|
||||
for (unsigned int i = 0; i < _out_message.size(); i++) {
|
||||
|
||||
if (i > 0) {
|
||||
generic_sentence += var_separator;
|
||||
}
|
||||
|
||||
switch (_out_message[i].type) {
|
||||
case FG_INT:
|
||||
val = _out_message[i].offset +
|
||||
_out_message[i].prop->getIntValue() * _out_message[i].factor;
|
||||
snprintf(tmp, 255, _out_message[i].format.c_str(), (int)val);
|
||||
break;
|
||||
|
||||
case FG_BOOL:
|
||||
snprintf(tmp, 255, _out_message[i].format.c_str(),
|
||||
_out_message[i].prop->getBoolValue());
|
||||
break;
|
||||
|
||||
case FG_FIXED:
|
||||
val = _out_message[i].offset +
|
||||
_out_message[i].prop->getFloatValue() * _out_message[i].factor;
|
||||
snprintf(tmp, 255, _out_message[i].format.c_str(), (float)val);
|
||||
break;
|
||||
|
||||
case FG_FLOAT:
|
||||
val = _out_message[i].offset +
|
||||
_out_message[i].prop->getFloatValue() * _out_message[i].factor;
|
||||
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;
|
||||
snprintf(tmp, 255, _out_message[i].format.c_str(), (float)val);
|
||||
break;
|
||||
|
||||
default: // SG_STRING
|
||||
snprintf(tmp, 255, _out_message[i].format.c_str(),
|
||||
_out_message[i].prop->getStringValue());
|
||||
}
|
||||
|
||||
generic_sentence += tmp;
|
||||
}
|
||||
|
||||
/* After each lot of variables has been added, put the line separator
|
||||
* char/string
|
||||
*/
|
||||
generic_sentence += line_separator;
|
||||
|
||||
length = generic_sentence.length();
|
||||
strncpy( buf, generic_sentence.c_str(), length );
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool FGGeneric::gen_message() {
|
||||
if (binary_mode) {
|
||||
return gen_message_binary();
|
||||
} else {
|
||||
return gen_message_ascii();
|
||||
}
|
||||
}
|
||||
|
||||
bool FGGeneric::parse_message_binary() {
|
||||
char *p2, *p1 = buf;
|
||||
int64_t tmp;
|
||||
double val;
|
||||
int i = -1;
|
||||
|
||||
p2 = p1 + FG_MAX_MSG_SIZE;
|
||||
while ((++i < (int)_in_message.size()) && (p1 < p2)) {
|
||||
|
||||
switch (_in_message[i].type) {
|
||||
case FG_INT:
|
||||
if (binary_byte_order == BYTE_ORDER_NEEDS_CONVERSION) {
|
||||
tmp = sg_bswap_32(*(int32_t *)p1);
|
||||
} 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( p1[0] != 0 );
|
||||
p1 += 1;
|
||||
break;
|
||||
|
||||
case FG_FIXED:
|
||||
if (binary_byte_order == BYTE_ORDER_NEEDS_CONVERSION) {
|
||||
tmp = sg_bswap_32(*(int32_t *)p1);
|
||||
} 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;
|
||||
|
||||
case FG_FLOAT:
|
||||
if (binary_byte_order == BYTE_ORDER_NEEDS_CONVERSION) {
|
||||
tmp = sg_bswap_32(*(int32_t *)p1);
|
||||
} else {
|
||||
tmp = *(int32_t *)p1;
|
||||
}
|
||||
|
||||
val = _in_message[i].offset +
|
||||
*(float *)&tmp * _in_message[i].factor;
|
||||
|
||||
_in_message[i].prop->setFloatValue(val);
|
||||
p1 += sizeof(int32_t);
|
||||
break;
|
||||
|
||||
case FG_DOUBLE:
|
||||
if (binary_byte_order == BYTE_ORDER_NEEDS_CONVERSION) {
|
||||
tmp = sg_bswap_64(*(int64_t *)p1);
|
||||
} else {
|
||||
tmp = *(int64_t *)p1;
|
||||
}
|
||||
|
||||
val = _in_message[i].offset +
|
||||
*(double *)&tmp * _in_message[i].factor;
|
||||
|
||||
_in_message[i].prop->setDoubleValue(val);
|
||||
p1 += sizeof(int64_t);
|
||||
break;
|
||||
|
||||
default: // SG_STRING
|
||||
SG_LOG( SG_IO, SG_ALERT, "Generic protocol: "
|
||||
"Ignoring unsupported binary input chunk type.");
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool FGGeneric::parse_message() {
|
||||
bool FGGeneric::parse_message_ascii() {
|
||||
char *p2, *p1 = buf;
|
||||
double val;
|
||||
int i = -1;
|
||||
|
||||
if (!binary_mode) {
|
||||
while ((++i < (int)_in_message.size()) &&
|
||||
p1 && strcmp(p1, line_separator.c_str())) {
|
||||
|
||||
|
|
@ -279,78 +395,17 @@ bool FGGeneric::parse_message() {
|
|||
|
||||
p1 = p2;
|
||||
}
|
||||
} else {
|
||||
/* Binary mode */
|
||||
int64_t tmp;
|
||||
|
||||
while ((++i < (int)_in_message.size()) &&
|
||||
(p1 - buf < FG_MAX_MSG_SIZE)) {
|
||||
|
||||
switch (_in_message[i].type) {
|
||||
case FG_INT:
|
||||
if (binary_byte_order == BYTE_ORDER_NEEDS_CONVERSION) {
|
||||
tmp = sg_bswap_32(*(int32_t *)p1);
|
||||
} 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( p1[0] != 0 );
|
||||
p1 += 1;
|
||||
break;
|
||||
|
||||
case FG_FIXED:
|
||||
if (binary_byte_order == BYTE_ORDER_NEEDS_CONVERSION) {
|
||||
tmp = sg_bswap_32(*(int32_t *)p1);
|
||||
} 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;
|
||||
|
||||
case FG_FLOAT:
|
||||
if (binary_byte_order == BYTE_ORDER_NEEDS_CONVERSION) {
|
||||
tmp = sg_bswap_32(*(int32_t *)p1);
|
||||
} else {
|
||||
tmp = *(int32_t *)p1;
|
||||
}
|
||||
val = _in_message[i].offset +
|
||||
*(float *)&tmp * _in_message[i].factor;
|
||||
_in_message[i].prop->setFloatValue(val);
|
||||
p1 += sizeof(int32_t);
|
||||
break;
|
||||
|
||||
case FG_DOUBLE:
|
||||
if (binary_byte_order == BYTE_ORDER_NEEDS_CONVERSION) {
|
||||
tmp = sg_bswap_64(*(int64_t *)p1);
|
||||
} else {
|
||||
tmp = *(int64_t *)p1;
|
||||
}
|
||||
val = _in_message[i].offset +
|
||||
*(double *)&tmp * _in_message[i].factor;
|
||||
_in_message[i].prop->setDoubleValue(val);
|
||||
p1 += sizeof(int64_t);
|
||||
break;
|
||||
|
||||
default: // SG_STRING
|
||||
SG_LOG( SG_IO, SG_ALERT, "Generic protocol: "
|
||||
"Ignoring unsupported binary input chunk type.");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool FGGeneric::parse_message() {
|
||||
if (binary_mode) {
|
||||
return parse_message_binary();
|
||||
} else {
|
||||
return parse_message_ascii();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// open hailing frequencies
|
||||
|
|
@ -468,38 +523,41 @@ FGGeneric::read_config(SGPropertyNode *root, vector<_serial_prot> &msg)
|
|||
var_sep_string = fgUnescape(root->getStringValue("var_separator"));
|
||||
line_sep_string = fgUnescape(root->getStringValue("line_separator"));
|
||||
|
||||
if ( var_sep_string == "newline" )
|
||||
if ( var_sep_string == "newline" ) {
|
||||
var_separator = '\n';
|
||||
else if ( var_sep_string == "tab" )
|
||||
} else if ( var_sep_string == "tab" ) {
|
||||
var_separator = '\t';
|
||||
else if ( var_sep_string == "space" )
|
||||
} else if ( var_sep_string == "space" ) {
|
||||
var_separator = ' ';
|
||||
else if ( var_sep_string == "formfeed" )
|
||||
} else if ( var_sep_string == "formfeed" ) {
|
||||
var_separator = '\f';
|
||||
else if ( var_sep_string == "carriagereturn" )
|
||||
} else if ( var_sep_string == "carriagereturn" ) {
|
||||
var_sep_string = '\r';
|
||||
else if ( var_sep_string == "verticaltab" )
|
||||
} else if ( var_sep_string == "verticaltab" ) {
|
||||
var_separator = '\v';
|
||||
else
|
||||
var_separator = var_sep_string;
|
||||
|
||||
if ( line_sep_string == "newline" )
|
||||
line_separator = '\n';
|
||||
else if ( line_sep_string == "tab" )
|
||||
line_separator = '\t';
|
||||
else if ( line_sep_string == "space" )
|
||||
line_separator = ' ';
|
||||
else if ( line_sep_string == "formfeed" )
|
||||
line_separator = '\f';
|
||||
else if ( line_sep_string == "carriagereturn" )
|
||||
line_separator = '\r';
|
||||
else if ( line_sep_string == "verticaltab" )
|
||||
line_separator = '\v';
|
||||
else
|
||||
line_separator = line_sep_string;
|
||||
} else {
|
||||
binary_footer_type = FOOTER_NONE; // default choice
|
||||
binary_record_length = -1; // default choice = sizeof(representation)
|
||||
var_separator = var_sep_string;
|
||||
}
|
||||
|
||||
if ( line_sep_string == "newline" ) {
|
||||
line_separator = '\n';
|
||||
} else if ( line_sep_string == "tab" ) {
|
||||
line_separator = '\t';
|
||||
} else if ( line_sep_string == "space" ) {
|
||||
line_separator = ' ';
|
||||
} else if ( line_sep_string == "formfeed" ) {
|
||||
line_separator = '\f';
|
||||
} else if ( line_sep_string == "carriagereturn" ) {
|
||||
line_separator = '\r';
|
||||
} else if ( line_sep_string == "verticaltab" ) {
|
||||
line_separator = '\v';
|
||||
} else {
|
||||
line_separator = line_sep_string;
|
||||
}
|
||||
} else {
|
||||
// default values: no footer and record_length = sizeof(representation)
|
||||
binary_footer_type = FOOTER_NONE;
|
||||
binary_record_length = -1;
|
||||
|
||||
// default choice is network byte order (big endian)
|
||||
if (sgIsLittleEndian()) {
|
||||
|
|
@ -510,20 +568,23 @@ FGGeneric::read_config(SGPropertyNode *root, vector<_serial_prot> &msg)
|
|||
|
||||
if ( root->hasValue("binary_footer") ) {
|
||||
string footer_type = root->getStringValue("binary_footer");
|
||||
if ( footer_type == "length" )
|
||||
if ( footer_type == "length" ) {
|
||||
binary_footer_type = FOOTER_LENGTH;
|
||||
else if ( footer_type.substr(0, 5) == "magic" ) {
|
||||
} else if ( footer_type.substr(0, 5) == "magic" ) {
|
||||
binary_footer_type = FOOTER_MAGIC;
|
||||
binary_footer_value = strtol(footer_type.substr(6,
|
||||
footer_type.length() - 6).c_str(), (char**)0, 0);
|
||||
} else if ( footer_type != "none" )
|
||||
} else if ( footer_type != "none" ) {
|
||||
SG_LOG(SG_IO, SG_ALERT,
|
||||
"generic protocol: Undefined generic binary protocol"
|
||||
"footer, using no footer.");
|
||||
"generic protocol: Unknown generic binary protocol "
|
||||
"footer '" << footer_type << "', 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" ) {
|
||||
|
|
|
|||
|
|
@ -88,6 +88,10 @@ private:
|
|||
int binary_record_length;
|
||||
enum {BYTE_ORDER_NEEDS_CONVERSION, BYTE_ORDER_MATCHES_NETWORK_ORDER} binary_byte_order;
|
||||
|
||||
bool gen_message_ascii();
|
||||
bool gen_message_binary();
|
||||
bool parse_message_ascii();
|
||||
bool parse_message_binary();
|
||||
void read_config(SGPropertyNode *root, vector<_serial_prot> &msg);
|
||||
bool exitOnError;
|
||||
};
|
||||
|
|
|
|||
Loading…
Reference in a new issue