1
0
Fork 0
flightgear/src/Network/ATC-Inputs.cxx
curt 225af83b32 This code is written on top of a driver interface only available with linux
(/proc file system) so it doesn't make sense to have other OS's trying to
open these files because they could never exist.
2004-11-18 17:40:03 +00:00

913 lines
29 KiB
C++

// ATC-Inputs.hxx -- Translate ATC hardware inputs to FGFS properties
//
// Written by Curtis Olson, started November 2004.
//
// Copyright (C) 2004 Curtis L. Olson - http://www.flightgear.org/~curt
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of the
// License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
//
// $Id$
#ifdef HAVE_CONFIG_H
# include <config.h>
#endif
#include <simgear/compiler.h>
#if defined( unix ) || defined( __CYGWIN__ )
# include <sys/types.h>
# include <sys/stat.h>
# include <fcntl.h>
#endif
#include STL_STRING
#include <simgear/debug/logstream.hxx>
#include <Main/fg_props.hxx>
#include "ATC-Inputs.hxx"
SG_USING_STD(string);
// Constructor: The _board parameter specifies which board to
// reference. Possible values are 0 or 1. The _config_file parameter
// specifies the location of the input config file (xml)
FGATCInput::FGATCInput( const int _board, const SGPath &_config_file ) :
is_open(false),
ignore_flight_controls(NULL),
ignore_pedal_controls(NULL),
analog_in_node(NULL),
radio_in_node(NULL),
switches_node(NULL)
{
board = _board;
config = _config_file;
}
// Read analog inputs
static void ATCReadAnalogInputs( int fd, unsigned char *analog_in_bytes ) {
#if defined( unix ) || defined( __CYGWIN__ )
// rewind
lseek( fd, 0, SEEK_SET );
int result = read( fd, analog_in_bytes, ATC_ANAL_IN_BYTES );
if ( result != ATC_ANAL_IN_BYTES ) {
SG_LOG( SG_IO, SG_ALERT, "Read failed" );
exit( -1 );
}
#endif
}
// Read status of radio switches and knobs
static void ATCReadRadios( int fd, unsigned char *switch_data ) {
#if defined( unix ) || defined( __CYGWIN__ )
// rewind
lseek( fd, 0, SEEK_SET );
int result = read( fd, switch_data, ATC_RADIO_SWITCH_BYTES );
if ( result != ATC_RADIO_SWITCH_BYTES ) {
SG_LOG( SG_IO, SG_ALERT, "Read failed" );
exit( -1 );
}
#endif
}
// Read switch inputs
static void ATCReadSwitches( int fd, unsigned char *switch_bytes ) {
#if defined( unix ) || defined( __CYGWIN__ )
// rewind
lseek( fd, 0, SEEK_SET );
int result = read( fd, switch_bytes, ATC_SWITCH_BYTES );
if ( result != ATC_SWITCH_BYTES ) {
SG_LOG( SG_IO, SG_ALERT, "Read failed" );
exit( -1 );
}
#endif
}
void FGATCInput::init_config() {
#if defined( unix ) || defined( __CYGWIN__ )
if ( config.str()[0] != '/' ) {
// not an absolute path, prepend the standard location
SGPath tmp;
char *envp = ::getenv( "HOME" );
if ( envp != NULL ) {
tmp = envp;
tmp.append( ".atcflightsim" );
tmp.append( config.str() );
config = tmp;
}
}
readProperties( config.str(), globals->get_props() );
#endif
}
// Open and initialize the ATC hardware
bool FGATCInput::open() {
if ( is_open ) {
SG_LOG( SG_IO, SG_ALERT, "This board is already open for input! "
<< board );
return false;
}
// This loads the config parameters generated by "simcal"
init_config();
SG_LOG( SG_IO, SG_ALERT,
"Initializing ATC 610x hardware, please wait ..." );
snprintf( lock_file, 256, "/proc/atc610x/board%d/lock", board );
snprintf( analog_in_file, 256, "/proc/atc610x/board%d/analog_in", board );
snprintf( radios_file, 256, "/proc/atc610x/board%d/radios", board );
snprintf( switches_file, 256, "/proc/atc610x/board%d/switches", board );
#if defined( unix ) || defined( __CYGWIN__ )
/////////////////////////////////////////////////////////////////////
// Open the /proc files
/////////////////////////////////////////////////////////////////////
lock_fd = ::open( lock_file, O_RDWR );
if ( lock_fd == -1 ) {
SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
char msg[256];
snprintf( msg, 256, "Error opening %s", lock_file );
perror( msg );
exit( -1 );
}
analog_in_fd = ::open( analog_in_file, O_RDONLY );
if ( analog_in_fd == -1 ) {
SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
char msg[256];
snprintf( msg, 256, "Error opening %s", analog_in_file );
perror( msg );
exit( -1 );
}
radios_fd = ::open( radios_file, O_RDWR );
if ( radios_fd == -1 ) {
SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
char msg[256];
snprintf( msg, 256, "Error opening %s", radios_file );
perror( msg );
exit( -1 );
}
switches_fd = ::open( switches_file, O_RDONLY );
if ( switches_fd == -1 ) {
SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
char msg[256];
snprintf( msg, 256, "Error opening %s", switches_file );
perror( msg );
exit( -1 );
}
#endif
/////////////////////////////////////////////////////////////////////
// Finished initing hardware
/////////////////////////////////////////////////////////////////////
SG_LOG( SG_IO, SG_ALERT,
"Done initializing ATC 610x hardware." );
is_open = true;
/////////////////////////////////////////////////////////////////////
// Connect up to property values
/////////////////////////////////////////////////////////////////////
ignore_flight_controls
= fgGetNode( "/input/atcsim/ignore-flight-controls", true );
ignore_pedal_controls
= fgGetNode( "/input/atcsim/ignore-pedal-controls", true );
char base_name[256];
snprintf( base_name, 256, "/input/atc-board[%d]/analog-in", board );
analog_in_node = fgGetNode( base_name );
snprintf( base_name, 256, "/input/atc-board[%d]/radio-switches", board );
radio_in_node = fgGetNode( base_name );
snprintf( base_name, 256, "/input/atc-board[%d]/switches", board );
switches_node = fgGetNode( base_name );
return true;
}
/////////////////////////////////////////////////////////////////////
// Read analog inputs
/////////////////////////////////////////////////////////////////////
// scale a number between min and max (with center defined) to a scale
// from -1.0 to 1.0
static double scale( int center, int min, int max, int value ) {
// cout << center << " " << min << " " << max << " " << value << " ";
double result;
double range;
if ( value <= center ) {
range = center - min;
result = (value - center) / range;
} else {
range = max - center;
result = (value - center) / range;
}
if ( result < -1.0 ) result = -1.0;
if ( result > 1.0 ) result = 1.0;
// cout << result << endl;
return result;
}
// scale a number between min and max to a scale from 0.0 to 1.0
static double scale( int min, int max, int value ) {
// cout << center << " " << min << " " << max << " " << value << " ";
double result;
double range;
range = max - min;
result = (value - min) / range;
if ( result < 0.0 ) result = 0.0;
if ( result > 1.0 ) result = 1.0;
// cout << result << endl;
return result;
}
static int tony_magic( int raw, int obs[3] ) {
int result = 0;
obs[0] = raw;
if ( obs[1] < 30 ) {
if ( obs[2] >= 68 && obs[2] < 480 ) {
result = -6;
} else if ( obs[2] >= 480 ) {
result = 6;
}
obs[2] = obs[1];
obs[1] = obs[0];
} else if ( obs[1] < 68 ) {
// do nothing
obs[1] = obs[0];
} else if ( obs[2] < 30 ) {
if ( obs[1] >= 68 && obs[1] < 480 ) {
result = 6;
obs[2] = obs[1];
obs[1] = obs[0];
} else if ( obs[1] >= 480 ) {
result = -6;
if ( obs[0] < obs[1] ) {
obs[2] = obs[1];
obs[1] = obs[0];
} else {
obs[2] = obs[0];
obs[1] = obs[0];
}
}
} else if ( obs[1] > 980 ) {
if ( obs[2] <= 956 && obs[2] > 480 ) {
result = 6;
} else if ( obs[2] <= 480 ) {
result = -6;
}
obs[2] = obs[1];
obs[1] = obs[0];
} else if ( obs[1] > 956 ) {
// do nothing
obs[1] = obs[0];
} else if ( obs[2] > 980 ) {
if ( obs[1] <= 956 && obs[1] > 480 ) {
result = -6;
obs[2] = obs[1];
obs[1] = obs[0];
} else if ( obs[1] <= 480 ) {
result = 6;
if ( obs[0] > obs[1] ) {
obs[2] = obs[1];
obs[1] = obs[0];
} else {
obs[2] = obs[0];
obs[1] = obs[0];
}
}
} else {
if ( obs[1] < 480 && obs[2] > 480 ) {
// crossed gap going up
if ( obs[0] < obs[1] ) {
// caught a bogus intermediate value coming out of the gap
obs[1] = obs[0];
}
} else if ( obs[1] > 480 && obs[2] < 480 ) {
// crossed gap going down
if ( obs[0] > obs[1] ) {
// caught a bogus intermediate value coming out of the gap
obs[1] = obs[0];
}
} else if ( obs[0] > 480 && obs[1] < 480 && obs[2] < 480 ) {
// crossed the gap going down
if ( obs[1] > obs[2] ) {
// caught a bogus intermediate value coming out of the gap
obs[1] = obs[2];
}
} else if ( obs[0] < 480 && obs[1] > 480 && obs[2] > 480 ) {
// crossed the gap going up
if ( obs[1] < obs[2] ) {
// caught a bogus intermediate value coming out of the gap
obs[1] = obs[2];
}
}
result = obs[1] - obs[2];
if ( abs(result) > 400 ) {
// ignore
result = 0;
}
obs[2] = obs[1];
obs[1] = obs[0];
}
// cout << " result = " << result << endl;
if ( result < -500 ) { result += 1024; }
if ( result > 500 ) { result -= 1024; }
return result;
}
static double instr_pot_filter( double ave, double val ) {
if ( fabs(ave - val) < 400 || fabs(val) < fabs(ave) ) {
return 0.5 * ave + 0.5 * val;
} else {
return ave;
}
}
bool FGATCInput::do_analog_in() {
// Read raw data in byte form
ATCReadAnalogInputs( analog_in_fd, analog_in_bytes );
// Convert to integer values
for ( int channel = 0; channel < ATC_ANAL_IN_VALUES; ++channel ) {
unsigned char hi = analog_in_bytes[2 * channel] & 0x03;
unsigned char lo = analog_in_bytes[2 * channel + 1];
analog_in_data[channel] = hi * 256 + lo;
// printf("%02x %02x ", hi, lo );
// printf("%04d ", value );
}
// Process analog inputs
if ( analog_in_node != NULL ) {
for ( int i = 0; i < analog_in_node->nChildren(); ++i ) {
// read the next config entry from the property tree
SGPropertyNode *child = analog_in_node->getChild(i);
string cname = child->getName();
int index = child->getIndex();
string name = "";
string type = "";
string subtype = "";
vector <SGPropertyNode *> output_nodes; output_nodes.clear();
int center = -1;
int min = 0;
int max = 1023;
float factor = 1.0;
if ( cname == "channel" ) {
SGPropertyNode *prop;
prop = child->getChild( "name" );
if ( prop != NULL ) {
name = prop->getStringValue();
}
prop = child->getChild( "type", 0 );
if ( prop != NULL ) {
type = prop->getStringValue();
}
prop = child->getChild( "type", 1 );
if ( prop != NULL ) {
subtype = prop->getStringValue();
}
int j = 0;
while ( (prop = child->getChild("prop", j)) != NULL ) {
SGPropertyNode *tmp
= fgGetNode( prop->getStringValue(), true );
output_nodes.push_back( tmp );
j++;
}
prop = child->getChild( "center" );
if ( prop != NULL ) {
center = prop->getIntValue();
}
prop = child->getChild( "min" );
if ( prop != NULL ) {
min = prop->getIntValue();
}
prop = child->getChild( "max" );
if ( prop != NULL ) {
max = prop->getIntValue();
}
prop = child->getChild( "factor" );
if ( prop != NULL ) {
factor = prop->getFloatValue();
}
// Fetch the raw value
int raw_value = analog_in_data[index];
// Update the target properties
if ( type == "flight"
&& !ignore_flight_controls->getBoolValue() )
{
if ( subtype != "pedals" ||
( subtype == "pedals"
&& !ignore_pedal_controls->getBoolValue() ) )
{
// "Cook" the raw value
float scaled_value = 0.0f;
if ( center >= 0 ) {
scaled_value = scale( center, min, max, raw_value );
} else {
scaled_value = scale( min, max, raw_value );
}
scaled_value *= factor;
// update the property tree values
for ( j = 0; j < (int)output_nodes.size(); ++j ) {
output_nodes[j]->setDoubleValue( scaled_value );
}
}
} else if ( type == "avionics-simple" ) {
// "Cook" the raw value
float scaled_value = 0.0f;
if ( center >= 0 ) {
scaled_value = scale( center, min, max, raw_value );
} else {
scaled_value = scale( min, max, raw_value );
}
scaled_value *= factor;
// update the property tree values
for ( j = 0; j < (int)output_nodes.size(); ++j ) {
output_nodes[j]->setDoubleValue( scaled_value );
}
} else if ( type == "avionics-resolver" ) {
// this type of analog input impliments a
// rotational knob. We first caclulate the amount
// of knob rotation (slightly complex to work with
// hardware specific goofiness) and then multiply
// that amount of movement by a scaling factor,
// and finally add the result to the original
// value.
bool do_init = false;
float scaled_value = 0.0f;
// fetch intermediate values from property tree
prop = child->getChild( "is-inited", 0 );
if ( prop == NULL ) {
do_init = true;
prop = child->getChild( "is-inited", 0, true );
prop->setBoolValue( true );
}
int raw[3];
for ( j = 0; j < 3; ++j ) {
prop = child->getChild( "raw", j, true );
if ( do_init ) {
raw[j] = analog_in_data[index];
} else {
raw[j] = prop->getIntValue();
}
}
// do Tony's magic to calculate knob movement
// based on current analog input position and
// historical data.
int diff = tony_magic( analog_in_data[index], raw );
// write raw intermediate values (updated by
// tony_magic()) back to property tree
for ( j = 0; j < 3; ++j ) {
prop = child->getChild( "raw", j, true );
prop->setIntValue( raw[j] );
}
// filter knob position
prop = child->getChild( "diff-average", 0, true );
double diff_ave = prop->getDoubleValue();
diff_ave = instr_pot_filter( diff_ave, diff );
prop->setDoubleValue( diff_ave );
// calculate value adjustment in real world units
scaled_value = diff_ave * factor;
// update the property tree values
for ( j = 0; j < (int)output_nodes.size(); ++j ) {
float value = output_nodes[j]->getDoubleValue();
value += scaled_value;
prop = child->getChild( "min-clamp" );
if ( prop != NULL ) {
double min = prop->getDoubleValue();
if ( value < min ) { value = min; }
}
prop = child->getChild( "max-clamp" );
if ( prop != NULL ) {
double max = prop->getDoubleValue();
if ( value > max ) { value = max; }
}
prop = child->getChild( "compass-heading" );
if ( prop != NULL ) {
bool compass = prop->getBoolValue();
if ( compass ) {
while ( value >= 360.0 ) { value -= 360.0; }
while ( value < 0.0 ) { value += 360.0; }
}
}
output_nodes[j]->setDoubleValue( value );
}
} else {
SG_LOG( SG_IO, SG_DEBUG, "Invalid channel type = "
<< type );
}
} else {
SG_LOG( SG_IO, SG_DEBUG,
"Input config error, expecting 'channel' but found "
<< cname );
}
}
}
return true;
}
/////////////////////////////////////////////////////////////////////
// Read the switch positions
/////////////////////////////////////////////////////////////////////
// decode the packed switch data
static void update_switch_matrix(
int board,
unsigned char switch_data[ATC_SWITCH_BYTES],
int switch_matrix[2][ATC_NUM_COLS][ATC_SWITCH_BYTES] )
{
for ( int row = 0; row < ATC_SWITCH_BYTES; ++row ) {
unsigned char switches = switch_data[row];
for( int column = 0; column < ATC_NUM_COLS; ++column ) {
switch_matrix[board][column][row] = switches & 1;
switches = switches >> 1;
}
}
}
bool FGATCInput::do_switches() {
// Read the raw data
ATCReadSwitches( switches_fd, switch_data );
// unpack the switch data
int switch_matrix[2][ATC_NUM_COLS][ATC_SWITCH_BYTES];
update_switch_matrix( board, switch_data, switch_matrix );
// Process the switch inputs
if ( switches_node != NULL ) {
for ( int i = 0; i < switches_node->nChildren(); ++i ) {
// read the next config entry from the property tree
SGPropertyNode *child = switches_node->getChild(i);
string cname = child->getName();
string name = "";
string type = "";
vector <SGPropertyNode *> output_nodes; output_nodes.clear();
int row = -1;
int col = -1;
float factor = 1.0;
int filter = -1;
float scaled_value = 0.0f;
// get common options
SGPropertyNode *prop;
prop = child->getChild( "name" );
if ( prop != NULL ) {
name = prop->getStringValue();
}
prop = child->getChild( "type" );
if ( prop != NULL ) {
type = prop->getStringValue();
}
int j = 0;
while ( (prop = child->getChild("prop", j)) != NULL ) {
SGPropertyNode *tmp
= fgGetNode( prop->getStringValue(), true );
output_nodes.push_back( tmp );
j++;
}
prop = child->getChild( "factor" );
if ( prop != NULL ) {
factor = prop->getFloatValue();
}
prop = child->getChild( "steady-state-filter" );
if ( prop != NULL ) {
filter = prop->getIntValue();
}
// handle different types of switches
if ( cname == "switch" ) {
prop = child->getChild( "row" );
if ( prop != NULL ) {
row = prop->getIntValue();
}
prop = child->getChild( "col" );
if ( prop != NULL ) {
col = prop->getIntValue();
}
// Fetch the raw value
int raw_value = switch_matrix[board][row][col];
// Cook the value
scaled_value = (float)raw_value * factor;
} else if ( cname == "combo-switch" ) {
float combo_value = 0.0f;
SGPropertyNode *pos;
int k = 0;
while ( (pos = child->getChild("position", k++)) != NULL ) {
// read the combo position entries from the property tree
prop = pos->getChild( "row" );
if ( prop != NULL ) {
row = prop->getIntValue();
}
prop = pos->getChild( "col" );
if ( prop != NULL ) {
col = prop->getIntValue();
}
prop = pos->getChild( "value" );
if ( prop != NULL ) {
combo_value = prop->getFloatValue();
}
// Fetch the raw value
int raw_value = switch_matrix[board][row][col];
// cout << "sm[" << board << "][" << row << "][" << col
// << "] = " << raw_value << endl;
if ( raw_value ) {
// set scaled_value to the first combo_value
// that matches and jump out of loop.
scaled_value = combo_value;
break;
}
}
// Cook the value
scaled_value *= factor;
}
// handle filter request. The value of the switch must be
// steady-state for "n" frames before the property value
// is updated.
bool update_prop = true;
if ( filter > 1 ) {
SGPropertyNode *fv = child->getChild( "filter-value", 0, true );
float filter_value = fv->getFloatValue();
SGPropertyNode *fc = child->getChild( "filter-count", 0, true );
int filter_count = fc->getIntValue();
if ( fabs(scaled_value - filter_value) < 0.0001 ) {
filter_count++;
} else {
filter_count = 0;
}
if ( filter_count < filter ) {
update_prop = false;
}
fv->setFloatValue( scaled_value );
fc->setIntValue( filter_count );
}
if ( update_prop ) {
if ( type == "engine" || type == "flight" ) {
if ( ! ignore_flight_controls->getBoolValue() ) {
// update the property tree values
for ( j = 0; j < (int)output_nodes.size(); ++j ) {
output_nodes[j]->setDoubleValue( scaled_value );
}
}
} else if ( type == "avionics" ) {
// update the property tree values
for ( j = 0; j < (int)output_nodes.size(); ++j ) {
output_nodes[j]->setDoubleValue( scaled_value );
}
}
}
}
}
return true;
}
/////////////////////////////////////////////////////////////////////
// Read radio switches
/////////////////////////////////////////////////////////////////////
bool FGATCInput::do_radio_switches() {
// Read the raw data
ATCReadRadios( radios_fd, radio_switch_data );
// Process the radio switch/knob inputs
if ( radio_in_node != NULL ) {
for ( int i = 0; i < radio_in_node->nChildren(); ++i ) {
// read the next config entry from the property tree
SGPropertyNode *child = radio_in_node->getChild(i);
string cname = child->getName();
if ( cname == "switch" ) {
string name = "";
string type = "";
vector <SGPropertyNode *> output_nodes; output_nodes.clear();
int byte_num = -1;
int right_shift = 0;
int mask = 0xff;
int factor = 1;
int offset = 0;
bool invert = false;
int scaled_value = 0;
// get common options
SGPropertyNode *prop;
prop = child->getChild( "name" );
if ( prop != NULL ) {
name = prop->getStringValue();
}
prop = child->getChild( "type" );
if ( prop != NULL ) {
type = prop->getStringValue();
}
int j = 0;
while ( (prop = child->getChild("prop", j)) != NULL ) {
SGPropertyNode *tmp
= fgGetNode( prop->getStringValue(), true );
output_nodes.push_back( tmp );
j++;
}
prop = child->getChild( "byte" );
if ( prop != NULL ) {
byte_num = prop->getIntValue();
}
prop = child->getChild( "right-shift" );
if ( prop != NULL ) {
right_shift = prop->getIntValue();
}
prop = child->getChild( "mask" );
if ( prop != NULL ) {
mask = prop->getIntValue();
}
prop = child->getChild( "factor" );
if ( prop != NULL ) {
factor = prop->getIntValue();
}
prop = child->getChild( "offset" );
if ( prop != NULL ) {
offset = prop->getIntValue();
}
prop = child->getChild( "invert" );
if ( prop != NULL ) {
invert = prop->getBoolValue();
}
// Fetch the raw value
int raw_value
= (radio_switch_data[byte_num] >> right_shift) & mask;
// Cook the value
if ( invert ) {
raw_value = !raw_value;
}
scaled_value = raw_value * factor + offset;
// update the property tree values
for ( j = 0; j < (int)output_nodes.size(); ++j ) {
output_nodes[j]->setIntValue( scaled_value );
}
}
}
}
return true;
}
// process the hardware inputs. This code assumes the calling layer
// will lock the hardware.
bool FGATCInput::process() {
if ( !is_open ) {
SG_LOG( SG_IO, SG_ALERT, "This board has not been opened for input! "
<< board );
return false;
}
do_analog_in();
do_switches();
do_radio_switches();
return true;
}
bool FGATCInput::close() {
#if defined( unix ) || defined( __CYGWIN__ )
int result;
result = ::close( lock_fd );
if ( result == -1 ) {
SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
char msg[256];
snprintf( msg, 256, "Error closing %s", lock_file );
perror( msg );
exit( -1 );
}
result = ::close( analog_in_fd );
if ( result == -1 ) {
SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
char msg[256];
snprintf( msg, 256, "Error closing %s", analog_in_file );
perror( msg );
exit( -1 );
}
result = ::close( radios_fd );
if ( result == -1 ) {
SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
char msg[256];
snprintf( msg, 256, "Error closing %s", radios_file );
perror( msg );
exit( -1 );
}
result = ::close( switches_fd );
if ( result == -1 ) {
SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
char msg[256];
snprintf( msg, 256, "Error closing %s", switches_file );
perror( msg );
exit( -1 );
}
#endif
return true;
}