886 lines
28 KiB
C++
886 lines
28 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>
|
|||
|
|
|
|||
|
|
#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 ) {
|
|||
|
|
// 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 );
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
// Read status of radio switches and knobs
|
|||
|
|
static void ATCReadRadios( int fd, unsigned char *switch_data ) {
|
|||
|
|
// 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 );
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
// Read switch inputs
|
|||
|
|
static void ATCReadSwitches( int fd, unsigned char *switch_bytes ) {
|
|||
|
|
// 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 );
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
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 );
|
|||
|
|
|
|||
|
|
/////////////////////////////////////////////////////////////////////
|
|||
|
|
// 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 );
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/////////////////////////////////////////////////////////////////////
|
|||
|
|
// 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 = -1;
|
|||
|
|
int mask = 0xff;
|
|||
|
|
int factor = 1;
|
|||
|
|
int offset = 0;
|
|||
|
|
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();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// Fetch the raw value
|
|||
|
|
int raw_value
|
|||
|
|
= (radio_switch_data[byte_num] >> right_shift) & mask;
|
|||
|
|
|
|||
|
|
// Cook the 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() {
|
|||
|
|
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 );
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return true;
|
|||
|
|
}
|