1 // ATC-Inputs.hxx -- Translate ATC hardware inputs to FGFS properties
3 // Written by Curtis Olson, started November 2004.
5 // Copyright (C) 2004 Curtis L. Olson - http://www.flightgear.org/~curt
7 // This program is free software; you can redistribute it and/or
8 // modify it under the terms of the GNU General Public License as
9 // published by the Free Software Foundation; either version 2 of the
10 // License, or (at your option) any later version.
12 // This program is distributed in the hope that it will be useful, but
13 // WITHOUT ANY WARRANTY; without even the implied warranty of
14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 // General Public License for more details.
17 // You should have received a copy of the GNU General Public License
18 // along with this program; if not, write to the Free Software
19 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
28 #include <simgear/compiler.h>
30 #if defined( unix ) || defined( __CYGWIN__ )
31 # include <sys/types.h>
32 # include <sys/stat.h>
44 #include <simgear/debug/logstream.hxx>
45 #include <simgear/misc/sg_path.hxx>
46 #include <simgear/props/props_io.hxx>
48 #include <Main/fg_props.hxx>
50 #include "ATC-Inputs.hxx"
56 // Constructor: The _board parameter specifies which board to
57 // reference. Possible values are 0 or 1. The _config_file parameter
58 // specifies the location of the input config file (xml)
59 FGATCInput::FGATCInput( const int _board, const SGPath &_config_file ) :
61 ignore_flight_controls(NULL),
62 ignore_pedal_controls(NULL),
68 config = _config_file;
73 static void ATCReadAnalogInputs( int fd, unsigned char *analog_in_bytes ) {
74 #if defined( unix ) || defined( __CYGWIN__ )
76 lseek( fd, 0, SEEK_SET );
78 int result = read( fd, analog_in_bytes, ATC_ANAL_IN_BYTES );
79 if ( result != ATC_ANAL_IN_BYTES ) {
80 SG_LOG( SG_IO, SG_ALERT, "Read failed" );
87 // Read status of radio switches and knobs
88 static void ATCReadRadios( int fd, unsigned char *switch_data ) {
89 #if defined( unix ) || defined( __CYGWIN__ )
91 lseek( fd, 0, SEEK_SET );
93 int result = read( fd, switch_data, ATC_RADIO_SWITCH_BYTES );
94 if ( result != ATC_RADIO_SWITCH_BYTES ) {
95 SG_LOG( SG_IO, SG_ALERT, "Read failed" );
102 // Read switch inputs
103 static void ATCReadSwitches( int fd, unsigned char *switch_bytes ) {
104 #if defined( unix ) || defined( __CYGWIN__ )
106 lseek( fd, 0, SEEK_SET );
108 int result = read( fd, switch_bytes, ATC_SWITCH_BYTES );
109 if ( result != ATC_SWITCH_BYTES ) {
110 SG_LOG( SG_IO, SG_ALERT, "Read failed" );
117 void FGATCInput::init_config() {
118 #if defined( unix ) || defined( __CYGWIN__ )
119 if ( config.str()[0] != '/' ) {
120 // not an absolute path, prepend the standard location
122 char *envp = ::getenv( "HOME" );
123 if ( envp != NULL ) {
125 tmp.append( ".atcflightsim" );
126 tmp.append( config.str() );
130 readProperties( config.str(), globals->get_props() );
135 // Open and initialize the ATC hardware
136 bool FGATCInput::open() {
138 SG_LOG( SG_IO, SG_ALERT, "This board is already open for input! "
143 // This loads the config parameters generated by "simcal"
146 SG_LOG( SG_IO, SG_ALERT,
147 "Initializing ATC input hardware, please wait ..." );
149 snprintf( analog_in_file, 256,
150 "/proc/atcflightsim/board%d/analog_in", board );
151 snprintf( radios_file, 256,
152 "/proc/atcflightsim/board%d/radios", board );
153 snprintf( switches_file, 256,
154 "/proc/atcflightsim/board%d/switches", board );
156 #if defined( unix ) || defined( __CYGWIN__ )
158 /////////////////////////////////////////////////////////////////////
159 // Open the /proc files
160 /////////////////////////////////////////////////////////////////////
162 analog_in_fd = ::open( analog_in_file, O_RDONLY );
163 if ( analog_in_fd == -1 ) {
164 SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
166 snprintf( msg, 256, "Error opening %s", analog_in_file );
171 radios_fd = ::open( radios_file, O_RDWR );
172 if ( radios_fd == -1 ) {
173 SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
175 snprintf( msg, 256, "Error opening %s", radios_file );
180 switches_fd = ::open( switches_file, O_RDONLY );
181 if ( switches_fd == -1 ) {
182 SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
184 snprintf( msg, 256, "Error opening %s", switches_file );
191 /////////////////////////////////////////////////////////////////////
192 // Finished initing hardware
193 /////////////////////////////////////////////////////////////////////
195 SG_LOG( SG_IO, SG_ALERT,
196 "Done initializing ATC input hardware." );
200 /////////////////////////////////////////////////////////////////////
201 // Connect up to property values
202 /////////////////////////////////////////////////////////////////////
204 ignore_flight_controls
205 = fgGetNode( "/input/atcsim/ignore-flight-controls", true );
206 ignore_pedal_controls
207 = fgGetNode( "/input/atcsim/ignore-pedal-controls", true );
211 snprintf( base_name, 256, "/input/atc-board[%d]/analog-in", board );
212 analog_in_node = fgGetNode( base_name );
214 snprintf( base_name, 256, "/input/atc-board[%d]/radio-switches", board );
215 radio_in_node = fgGetNode( base_name );
217 snprintf( base_name, 256, "/input/atc-board[%d]/switches", board );
218 switches_node = fgGetNode( base_name );
224 /////////////////////////////////////////////////////////////////////
225 // Read analog inputs
226 /////////////////////////////////////////////////////////////////////
228 // scale a number between min and max (with center defined) to a scale
229 // from -1.0 to 1.0. The deadband value is symmetric, so specifying
230 // '1' will give you a deadband of +/-1
231 static double scale( int center, int deadband, int min, int max, int value ) {
232 // cout << center << " " << min << " " << max << " " << value << " ";
236 if ( value <= (center - deadband) ) {
237 range = (center - deadband) - min;
238 result = (value - (center - deadband)) / range;
239 } else if ( value >= (center + deadband) ) {
240 range = max - (center + deadband);
241 result = (value - (center + deadband)) / range;
246 if ( result < -1.0 ) result = -1.0;
247 if ( result > 1.0 ) result = 1.0;
249 // cout << result << endl;
255 // scale a number between min and max to a scale from 0.0 to 1.0
256 static double scale( int min, int max, int value ) {
257 // cout << center << " " << min << " " << max << " " << value << " ";
262 result = (value - min) / range;
264 if ( result < 0.0 ) result = 0.0;
265 if ( result > 1.0 ) result = 1.0;
267 // cout << result << endl;
273 static double clamp( double min, double max, double value ) {
274 double result = value;
276 if ( result < min ) result = min;
277 if ( result > max ) result = max;
279 // cout << result << endl;
285 static int tony_magic( int raw, int obs[3] ) {
291 if ( obs[2] >= 68 && obs[2] < 480 ) {
293 } else if ( obs[2] >= 480 ) {
298 } else if ( obs[1] < 68 ) {
301 } else if ( obs[2] < 30 ) {
302 if ( obs[1] >= 68 && obs[1] < 480 ) {
306 } else if ( obs[1] >= 480 ) {
308 if ( obs[0] < obs[1] ) {
316 } else if ( obs[1] > 980 ) {
317 if ( obs[2] <= 956 && obs[2] > 480 ) {
319 } else if ( obs[2] <= 480 ) {
324 } else if ( obs[1] > 956 ) {
327 } else if ( obs[2] > 980 ) {
328 if ( obs[1] <= 956 && obs[1] > 480 ) {
332 } else if ( obs[1] <= 480 ) {
334 if ( obs[0] > obs[1] ) {
343 if ( obs[1] < 480 && obs[2] > 480 ) {
344 // crossed gap going up
345 if ( obs[0] < obs[1] ) {
346 // caught a bogus intermediate value coming out of the gap
349 } else if ( obs[1] > 480 && obs[2] < 480 ) {
350 // crossed gap going down
351 if ( obs[0] > obs[1] ) {
352 // caught a bogus intermediate value coming out of the gap
355 } else if ( obs[0] > 480 && obs[1] < 480 && obs[2] < 480 ) {
356 // crossed the gap going down
357 if ( obs[1] > obs[2] ) {
358 // caught a bogus intermediate value coming out of the gap
361 } else if ( obs[0] < 480 && obs[1] > 480 && obs[2] > 480 ) {
362 // crossed the gap going up
363 if ( obs[1] < obs[2] ) {
364 // caught a bogus intermediate value coming out of the gap
368 result = obs[1] - obs[2];
369 if ( abs(result) > 400 ) {
377 // cout << " result = " << result << endl;
378 if ( result < -500 ) { result += 1024; }
379 if ( result > 500 ) { result -= 1024; }
385 static double instr_pot_filter( double ave, double val ) {
386 if ( fabs(ave - val) < 400 || fabs(val) < fabs(ave) ) {
387 return 0.5 * ave + 0.5 * val;
394 bool FGATCInput::do_analog_in() {
395 // Read raw data in byte form
396 ATCReadAnalogInputs( analog_in_fd, analog_in_bytes );
398 // Convert to integer values
399 for ( int channel = 0; channel < ATC_ANAL_IN_VALUES; ++channel ) {
400 unsigned char hi = analog_in_bytes[2 * channel] & 0x03;
401 unsigned char lo = analog_in_bytes[2 * channel + 1];
402 analog_in_data[channel] = hi * 256 + lo;
404 // printf("%02x %02x ", hi, lo );
405 // printf("%04d ", value );
408 // Process analog inputs
409 if ( analog_in_node != NULL ) {
410 for ( int i = 0; i < analog_in_node->nChildren(); ++i ) {
411 // read the next config entry from the property tree
413 SGPropertyNode *child = analog_in_node->getChild(i);
414 string cname = child->getName();
415 int index = child->getIndex();
419 vector <SGPropertyNode *> output_nodes;
427 if ( cname == "channel" ) {
428 SGPropertyNode *prop;
429 prop = child->getChild( "name" );
430 if ( prop != NULL ) {
431 name = prop->getStringValue();
433 prop = child->getChild( "type", 0 );
434 if ( prop != NULL ) {
435 type = prop->getStringValue();
437 prop = child->getChild( "type", 1 );
438 if ( prop != NULL ) {
439 subtype = prop->getStringValue();
442 while ( (prop = child->getChild("prop", j)) != NULL ) {
444 = fgGetNode( prop->getStringValue(), true );
445 output_nodes.push_back( tmp );
448 prop = child->getChild( "center" );
449 if ( prop != NULL ) {
450 center = prop->getIntValue();
452 prop = child->getChild( "min" );
453 if ( prop != NULL ) {
454 min = prop->getIntValue();
456 prop = child->getChild( "max" );
457 if ( prop != NULL ) {
458 max = prop->getIntValue();
460 prop = child->getChild( "deadband" );
461 if ( prop != NULL ) {
462 deadband = prop->getIntValue();
464 prop = child->getChild( "hysteresis" );
465 if ( prop != NULL ) {
466 hysteresis = prop->getIntValue();
468 prop = child->getChild( "offset" );
469 if ( prop != NULL ) {
470 offset = prop->getFloatValue();
472 prop = child->getChild( "factor" );
473 if ( prop != NULL ) {
474 factor = prop->getFloatValue();
477 // Fetch the raw value
479 int raw_value = analog_in_data[index];
481 // Update the target properties
483 if ( type == "flight"
484 && !ignore_flight_controls->getBoolValue() )
486 if ( subtype != "pedals" ||
487 ( subtype == "pedals"
488 && !ignore_pedal_controls->getBoolValue() ) )
490 // "Cook" the raw value
491 float scaled_value = 0.0f;
493 if ( hysteresis > 0 ) {
494 int last_raw_value = 0;
495 prop = child->getChild( "last-raw-value", 0, true );
496 last_raw_value = prop->getIntValue();
498 if ( abs(raw_value - last_raw_value) < hysteresis )
500 // not enough movement stay put
501 raw_value = last_raw_value;
503 // update last raw value
504 prop->setIntValue( raw_value );
509 scaled_value = scale( center, deadband,
510 min, max, raw_value );
512 scaled_value = scale( min, max, raw_value );
514 scaled_value *= factor;
515 scaled_value += offset;
517 // final sanity clamp
519 scaled_value = clamp( -1.0, 1.0, scaled_value );
521 scaled_value = clamp( 0.0, 1.0, scaled_value );
524 // update the property tree values
525 for ( j = 0; j < (int)output_nodes.size(); ++j ) {
526 output_nodes[j]->setDoubleValue( scaled_value );
529 } else if ( type == "avionics-simple" ) {
530 // "Cook" the raw value
531 float scaled_value = 0.0f;
533 scaled_value = scale( center, deadband,
534 min, max, raw_value );
536 scaled_value = scale( min, max, raw_value );
538 scaled_value *= factor;
539 scaled_value += offset;
541 // final sanity clamp
543 scaled_value = clamp( -1.0, 1.0, scaled_value );
545 scaled_value = clamp( 0.0, 1.0, scaled_value );
548 // update the property tree values
549 for ( j = 0; j < (int)output_nodes.size(); ++j ) {
550 output_nodes[j]->setDoubleValue( scaled_value );
552 } else if ( type == "avionics-resolver" ) {
553 // this type of analog input impliments a
554 // rotational knob. We first caclulate the amount
555 // of knob rotation (slightly complex to work with
556 // hardware specific goofiness) and then multiply
557 // that amount of movement by a scaling factor,
558 // and finally add the result to the original
561 bool do_init = false;
562 float scaled_value = 0.0f;
564 // fetch intermediate values from property tree
566 prop = child->getChild( "is-inited", 0 );
567 if ( prop == NULL ) {
569 prop = child->getChild( "is-inited", 0, true );
570 prop->setBoolValue( true );
574 for ( j = 0; j < 3; ++j ) {
575 prop = child->getChild( "raw", j, true );
577 raw[j] = analog_in_data[index];
579 raw[j] = prop->getIntValue();
583 // do Tony's magic to calculate knob movement
584 // based on current analog input position and
586 int diff = tony_magic( analog_in_data[index], raw );
588 // write raw intermediate values (updated by
589 // tony_magic()) back to property tree
590 for ( j = 0; j < 3; ++j ) {
591 prop = child->getChild( "raw", j, true );
592 prop->setIntValue( raw[j] );
595 // filter knob position
596 prop = child->getChild( "diff-average", 0, true );
597 double diff_ave = prop->getDoubleValue();
598 diff_ave = instr_pot_filter( diff_ave, diff );
599 prop->setDoubleValue( diff_ave );
601 // calculate value adjustment in real world units
602 scaled_value = diff_ave * factor;
604 // update the property tree values
605 for ( j = 0; j < (int)output_nodes.size(); ++j ) {
606 float value = output_nodes[j]->getDoubleValue();
607 value += scaled_value;
609 prop = child->getChild( "min-clamp" );
610 if ( prop != NULL ) {
611 double min = prop->getDoubleValue();
612 if ( value < min ) { value = min; }
615 prop = child->getChild( "max-clamp" );
616 if ( prop != NULL ) {
617 double max = prop->getDoubleValue();
618 if ( value > max ) { value = max; }
621 prop = child->getChild( "compass-heading" );
622 if ( prop != NULL ) {
623 bool compass = prop->getBoolValue();
625 while ( value >= 360.0 ) { value -= 360.0; }
626 while ( value < 0.0 ) { value += 360.0; }
630 output_nodes[j]->setDoubleValue( value );
634 SG_LOG( SG_IO, SG_DEBUG, "Invalid channel type = "
638 SG_LOG( SG_IO, SG_DEBUG,
639 "Input config error, expecting 'channel' but found "
649 /////////////////////////////////////////////////////////////////////
650 // Read the switch positions
651 /////////////////////////////////////////////////////////////////////
653 // decode the packed switch data
654 static void update_switch_matrix(
656 unsigned char switch_data[ATC_SWITCH_BYTES],
657 int switch_matrix[2][ATC_NUM_COLS][ATC_SWITCH_BYTES] )
659 for ( int row = 0; row < ATC_SWITCH_BYTES; ++row ) {
660 unsigned char switches = switch_data[row];
662 for( int column = 0; column < ATC_NUM_COLS; ++column ) {
664 switch_matrix[board][column][row] = switches & 1;
666 switch_matrix[board][row-8][8+column] = switches & 1;
668 switches = switches >> 1;
673 bool FGATCInput::do_switches() {
675 ATCReadSwitches( switches_fd, switch_data );
677 // unpack the switch data
678 int switch_matrix[2][ATC_NUM_COLS][ATC_SWITCH_BYTES];
679 update_switch_matrix( board, switch_data, switch_matrix );
681 // Process the switch inputs
682 if ( switches_node != NULL ) {
683 for ( int i = 0; i < switches_node->nChildren(); ++i ) {
684 // read the next config entry from the property tree
686 SGPropertyNode *child = switches_node->getChild(i);
687 string cname = child->getName();
690 vector <SGPropertyNode *> output_nodes;
695 float scaled_value = 0.0f;
698 // get common options
700 SGPropertyNode *prop;
701 prop = child->getChild( "name" );
702 if ( prop != NULL ) {
703 name = prop->getStringValue();
705 prop = child->getChild( "type" );
706 if ( prop != NULL ) {
707 type = prop->getStringValue();
710 while ( (prop = child->getChild("prop", j)) != NULL ) {
712 = fgGetNode( prop->getStringValue(), true );
713 output_nodes.push_back( tmp );
716 prop = child->getChild( "factor" );
717 if ( prop != NULL ) {
718 factor = prop->getFloatValue();
720 prop = child->getChild( "invert" );
721 if ( prop != NULL ) {
722 invert = prop->getBoolValue();
724 prop = child->getChild( "steady-state-filter" );
725 if ( prop != NULL ) {
726 filter = prop->getIntValue();
729 // handle different types of switches
731 if ( cname == "switch" ) {
732 prop = child->getChild( "row" );
733 if ( prop != NULL ) {
734 row = prop->getIntValue();
736 prop = child->getChild( "col" );
737 if ( prop != NULL ) {
738 col = prop->getIntValue();
741 // Fetch the raw value
742 int raw_value = switch_matrix[board][row][col];
746 raw_value = !raw_value;
750 scaled_value = (float)raw_value * factor;
752 } else if ( cname == "combo-switch" ) {
753 float combo_value = 0.0f;
757 while ( (pos = child->getChild("position", k++)) != NULL ) {
758 // read the combo position entries from the property tree
760 prop = pos->getChild( "row" );
761 if ( prop != NULL ) {
762 row = prop->getIntValue();
764 prop = pos->getChild( "col" );
765 if ( prop != NULL ) {
766 col = prop->getIntValue();
768 prop = pos->getChild( "value" );
769 if ( prop != NULL ) {
770 combo_value = prop->getFloatValue();
773 // Fetch the raw value
774 int raw_value = switch_matrix[board][row][col];
775 // cout << "sm[" << board << "][" << row << "][" << col
776 // << "] = " << raw_value << endl;
779 // set scaled_value to the first combo_value
780 // that matches and jump out of loop.
781 scaled_value = combo_value;
787 scaled_value *= factor;
788 } else if ( cname == "additive-switch" ) {
789 float additive_value = 0.0f;
790 float increment = 0.0f;
794 while ( (pos = child->getChild("position", k++)) != NULL ) {
795 // read the combo position entries from the property tree
797 prop = pos->getChild( "row" );
798 if ( prop != NULL ) {
799 row = prop->getIntValue();
801 prop = pos->getChild( "col" );
802 if ( prop != NULL ) {
803 col = prop->getIntValue();
805 prop = pos->getChild( "value" );
806 if ( prop != NULL ) {
807 increment = prop->getFloatValue();
810 // Fetch the raw value
811 int raw_value = switch_matrix[board][row][col];
812 // cout << "sm[" << board << "][" << row << "][" << col
813 // << "] = " << raw_value << endl;
816 // set scaled_value to the first combo_value
817 // that matches and jump out of loop.
818 additive_value += increment;
823 scaled_value = additive_value * factor;
826 // handle filter request. The value of the switch must be
827 // steady-state for "n" frames before the property value
830 bool update_prop = true;
833 SGPropertyNode *fv = child->getChild( "filter-value", 0, true );
834 float filter_value = fv->getFloatValue();
835 SGPropertyNode *fc = child->getChild( "filter-count", 0, true );
836 int filter_count = fc->getIntValue();
838 if ( fabs(scaled_value - filter_value) < 0.0001 ) {
844 if ( filter_count < filter ) {
848 fv->setFloatValue( scaled_value );
849 fc->setIntValue( filter_count );
853 if ( type == "engine" || type == "flight" ) {
854 if ( ! ignore_flight_controls->getBoolValue() ) {
855 // update the property tree values
856 for ( j = 0; j < (int)output_nodes.size(); ++j ) {
857 output_nodes[j]->setDoubleValue( scaled_value );
860 } else if ( type == "avionics" ) {
861 // update the property tree values
862 for ( j = 0; j < (int)output_nodes.size(); ++j ) {
863 output_nodes[j]->setDoubleValue( scaled_value );
874 /////////////////////////////////////////////////////////////////////
875 // Read radio switches
876 /////////////////////////////////////////////////////////////////////
878 bool FGATCInput::do_radio_switches() {
880 ATCReadRadios( radios_fd, radio_switch_data );
882 // Process the radio switch/knob inputs
883 if ( radio_in_node != NULL ) {
884 for ( int i = 0; i < radio_in_node->nChildren(); ++i ) {
885 // read the next config entry from the property tree
887 SGPropertyNode *child = radio_in_node->getChild(i);
888 string cname = child->getName();
890 if ( cname == "switch" ) {
893 vector <SGPropertyNode *> output_nodes;
900 int scaled_value = 0;
901 // get common options
903 SGPropertyNode *prop;
904 prop = child->getChild( "name" );
905 if ( prop != NULL ) {
906 name = prop->getStringValue();
908 prop = child->getChild( "type" );
909 if ( prop != NULL ) {
910 type = prop->getStringValue();
913 while ( (prop = child->getChild("prop", j)) != NULL ) {
915 = fgGetNode( prop->getStringValue(), true );
916 output_nodes.push_back( tmp );
919 prop = child->getChild( "byte" );
920 if ( prop != NULL ) {
921 byte_num = prop->getIntValue();
923 prop = child->getChild( "right-shift" );
924 if ( prop != NULL ) {
925 right_shift = prop->getIntValue();
927 prop = child->getChild( "mask" );
928 if ( prop != NULL ) {
929 mask = prop->getIntValue();
931 prop = child->getChild( "factor" );
932 if ( prop != NULL ) {
933 factor = prop->getIntValue();
935 prop = child->getChild( "offset" );
936 if ( prop != NULL ) {
937 offset = prop->getIntValue();
939 prop = child->getChild( "invert" );
940 if ( prop != NULL ) {
941 invert = prop->getBoolValue();
944 // Fetch the raw value
946 = (radio_switch_data[byte_num] >> right_shift) & mask;
950 raw_value = !raw_value;
952 scaled_value = raw_value * factor + offset;
954 // update the property tree values
955 for ( j = 0; j < (int)output_nodes.size(); ++j ) {
956 output_nodes[j]->setIntValue( scaled_value );
966 // process the hardware inputs. This code assumes the calling layer
967 // will lock the hardware.
968 bool FGATCInput::process() {
970 SG_LOG( SG_IO, SG_ALERT, "This board has not been opened for input! "
983 bool FGATCInput::close() {
985 #if defined( unix ) || defined( __CYGWIN__ )
993 result = ::close( analog_in_fd );
994 if ( result == -1 ) {
995 SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
997 snprintf( msg, 256, "Error closing %s", analog_in_file );
1002 result = ::close( radios_fd );
1003 if ( result == -1 ) {
1004 SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
1006 snprintf( msg, 256, "Error closing %s", radios_file );
1011 result = ::close( switches_fd );
1012 if ( result == -1 ) {
1013 SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
1015 snprintf( msg, 256, "Error closing %s", switches_file );