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>
43 #include <simgear/debug/logstream.hxx>
44 #include <simgear/misc/sg_path.hxx>
45 #include <simgear/props/props_io.hxx>
47 #include <Main/fg_props.hxx>
49 #include "ATC-Inputs.hxx"
55 // Constructor: The _board parameter specifies which board to
56 // reference. Possible values are 0 or 1. The _config_file parameter
57 // specifies the location of the input config file (xml)
58 FGATCInput::FGATCInput( const int _board, const SGPath &_config_file ) :
60 ignore_flight_controls(NULL),
61 ignore_pedal_controls(NULL),
67 config = _config_file;
72 static void ATCReadAnalogInputs( int fd, unsigned char *analog_in_bytes ) {
73 #if defined( unix ) || defined( __CYGWIN__ )
75 lseek( fd, 0, SEEK_SET );
77 int result = read( fd, analog_in_bytes, ATC_ANAL_IN_BYTES );
78 if ( result != ATC_ANAL_IN_BYTES ) {
79 SG_LOG( SG_IO, SG_ALERT, "Read failed" );
86 // Read status of radio switches and knobs
87 static void ATCReadRadios( int fd, unsigned char *switch_data ) {
88 #if defined( unix ) || defined( __CYGWIN__ )
90 lseek( fd, 0, SEEK_SET );
92 int result = read( fd, switch_data, ATC_RADIO_SWITCH_BYTES );
93 if ( result != ATC_RADIO_SWITCH_BYTES ) {
94 SG_LOG( SG_IO, SG_ALERT, "Read failed" );
101 // Read switch inputs
102 static void ATCReadSwitches( int fd, unsigned char *switch_bytes ) {
103 #if defined( unix ) || defined( __CYGWIN__ )
105 lseek( fd, 0, SEEK_SET );
107 int result = read( fd, switch_bytes, ATC_SWITCH_BYTES );
108 if ( result != ATC_SWITCH_BYTES ) {
109 SG_LOG( SG_IO, SG_ALERT, "Read failed" );
116 void FGATCInput::init_config() {
117 #if defined( unix ) || defined( __CYGWIN__ )
118 if ( config.str()[0] != '/' ) {
119 // not an absolute path, prepend the standard location
121 char *envp = ::getenv( "HOME" );
122 if ( envp != NULL ) {
124 tmp.append( ".atcflightsim" );
125 tmp.append( config.str() );
129 readProperties( config.str(), globals->get_props() );
134 // Open and initialize the ATC hardware
135 bool FGATCInput::open() {
137 SG_LOG( SG_IO, SG_ALERT, "This board is already open for input! "
142 // This loads the config parameters generated by "simcal"
145 SG_LOG( SG_IO, SG_ALERT,
146 "Initializing ATC input hardware, please wait ..." );
148 snprintf( analog_in_file, 256,
149 "/proc/atcflightsim/board%d/analog_in", board );
150 snprintf( radios_file, 256,
151 "/proc/atcflightsim/board%d/radios", board );
152 snprintf( switches_file, 256,
153 "/proc/atcflightsim/board%d/switches", board );
155 #if defined( unix ) || defined( __CYGWIN__ )
157 /////////////////////////////////////////////////////////////////////
158 // Open the /proc files
159 /////////////////////////////////////////////////////////////////////
161 analog_in_fd = ::open( analog_in_file, O_RDONLY );
162 if ( analog_in_fd == -1 ) {
163 SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
165 snprintf( msg, 256, "Error opening %s", analog_in_file );
170 radios_fd = ::open( radios_file, O_RDWR );
171 if ( radios_fd == -1 ) {
172 SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
174 snprintf( msg, 256, "Error opening %s", radios_file );
179 switches_fd = ::open( switches_file, O_RDONLY );
180 if ( switches_fd == -1 ) {
181 SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
183 snprintf( msg, 256, "Error opening %s", switches_file );
190 /////////////////////////////////////////////////////////////////////
191 // Finished initing hardware
192 /////////////////////////////////////////////////////////////////////
194 SG_LOG( SG_IO, SG_ALERT,
195 "Done initializing ATC input hardware." );
199 /////////////////////////////////////////////////////////////////////
200 // Connect up to property values
201 /////////////////////////////////////////////////////////////////////
203 ignore_flight_controls
204 = fgGetNode( "/input/atcsim/ignore-flight-controls", true );
205 ignore_pedal_controls
206 = fgGetNode( "/input/atcsim/ignore-pedal-controls", true );
210 snprintf( base_name, 256, "/input/atc-board[%d]/analog-in", board );
211 analog_in_node = fgGetNode( base_name );
213 snprintf( base_name, 256, "/input/atc-board[%d]/radio-switches", board );
214 radio_in_node = fgGetNode( base_name );
216 snprintf( base_name, 256, "/input/atc-board[%d]/switches", board );
217 switches_node = fgGetNode( base_name );
223 /////////////////////////////////////////////////////////////////////
224 // Read analog inputs
225 /////////////////////////////////////////////////////////////////////
227 // scale a number between min and max (with center defined) to a scale
228 // from -1.0 to 1.0. The deadband value is symmetric, so specifying
229 // '1' will give you a deadband of +/-1
230 static double scale( int center, int deadband, int min, int max, int value ) {
231 // cout << center << " " << min << " " << max << " " << value << " ";
235 if ( value <= (center - deadband) ) {
236 range = (center - deadband) - min;
237 result = (value - (center - deadband)) / range;
238 } else if ( value >= (center + deadband) ) {
239 range = max - (center + deadband);
240 result = (value - (center + deadband)) / range;
245 if ( result < -1.0 ) result = -1.0;
246 if ( result > 1.0 ) result = 1.0;
248 // cout << result << endl;
254 // scale a number between min and max to a scale from 0.0 to 1.0
255 static double scale( int min, int max, int value ) {
256 // cout << center << " " << min << " " << max << " " << value << " ";
261 result = (value - min) / range;
263 if ( result < 0.0 ) result = 0.0;
264 if ( result > 1.0 ) result = 1.0;
266 // cout << result << endl;
272 static double clamp( double min, double max, double value ) {
273 double result = value;
275 if ( result < min ) result = min;
276 if ( result > max ) result = max;
278 // cout << result << endl;
284 static int tony_magic( int raw, int obs[3] ) {
290 if ( obs[2] >= 68 && obs[2] < 480 ) {
292 } else if ( obs[2] >= 480 ) {
297 } else if ( obs[1] < 68 ) {
300 } else if ( obs[2] < 30 ) {
301 if ( obs[1] >= 68 && obs[1] < 480 ) {
305 } else if ( obs[1] >= 480 ) {
307 if ( obs[0] < obs[1] ) {
315 } else if ( obs[1] > 980 ) {
316 if ( obs[2] <= 956 && obs[2] > 480 ) {
318 } else if ( obs[2] <= 480 ) {
323 } else if ( obs[1] > 956 ) {
326 } else if ( obs[2] > 980 ) {
327 if ( obs[1] <= 956 && obs[1] > 480 ) {
331 } else if ( obs[1] <= 480 ) {
333 if ( obs[0] > obs[1] ) {
342 if ( obs[1] < 480 && obs[2] > 480 ) {
343 // crossed gap going up
344 if ( obs[0] < obs[1] ) {
345 // caught a bogus intermediate value coming out of the gap
348 } else if ( obs[1] > 480 && obs[2] < 480 ) {
349 // crossed gap going down
350 if ( obs[0] > obs[1] ) {
351 // caught a bogus intermediate value coming out of the gap
354 } else if ( obs[0] > 480 && obs[1] < 480 && obs[2] < 480 ) {
355 // crossed the gap going down
356 if ( obs[1] > obs[2] ) {
357 // caught a bogus intermediate value coming out of the gap
360 } else if ( obs[0] < 480 && obs[1] > 480 && obs[2] > 480 ) {
361 // crossed the gap going up
362 if ( obs[1] < obs[2] ) {
363 // caught a bogus intermediate value coming out of the gap
367 result = obs[1] - obs[2];
368 if ( abs(result) > 400 ) {
376 // cout << " result = " << result << endl;
377 if ( result < -500 ) { result += 1024; }
378 if ( result > 500 ) { result -= 1024; }
384 static double instr_pot_filter( double ave, double val ) {
385 if ( fabs(ave - val) < 400 || fabs(val) < fabs(ave) ) {
386 return 0.5 * ave + 0.5 * val;
393 bool FGATCInput::do_analog_in() {
394 // Read raw data in byte form
395 ATCReadAnalogInputs( analog_in_fd, analog_in_bytes );
397 // Convert to integer values
398 for ( int channel = 0; channel < ATC_ANAL_IN_VALUES; ++channel ) {
399 unsigned char hi = analog_in_bytes[2 * channel] & 0x03;
400 unsigned char lo = analog_in_bytes[2 * channel + 1];
401 analog_in_data[channel] = hi * 256 + lo;
403 // printf("%02x %02x ", hi, lo );
404 // printf("%04d ", value );
407 // Process analog inputs
408 if ( analog_in_node != NULL ) {
409 for ( int i = 0; i < analog_in_node->nChildren(); ++i ) {
410 // read the next config entry from the property tree
412 SGPropertyNode *child = analog_in_node->getChild(i);
413 string cname = child->getName();
414 int index = child->getIndex();
418 vector <SGPropertyNode *> output_nodes;
426 if ( cname == "channel" ) {
427 SGPropertyNode *prop;
428 prop = child->getChild( "name" );
429 if ( prop != NULL ) {
430 name = prop->getStringValue();
432 prop = child->getChild( "type", 0 );
433 if ( prop != NULL ) {
434 type = prop->getStringValue();
436 prop = child->getChild( "type", 1 );
437 if ( prop != NULL ) {
438 subtype = prop->getStringValue();
441 while ( (prop = child->getChild("prop", j)) != NULL ) {
443 = fgGetNode( prop->getStringValue(), true );
444 output_nodes.push_back( tmp );
447 prop = child->getChild( "center" );
448 if ( prop != NULL ) {
449 center = prop->getIntValue();
451 prop = child->getChild( "min" );
452 if ( prop != NULL ) {
453 min = prop->getIntValue();
455 prop = child->getChild( "max" );
456 if ( prop != NULL ) {
457 max = prop->getIntValue();
459 prop = child->getChild( "deadband" );
460 if ( prop != NULL ) {
461 deadband = prop->getIntValue();
463 prop = child->getChild( "hysteresis" );
464 if ( prop != NULL ) {
465 hysteresis = prop->getIntValue();
467 prop = child->getChild( "offset" );
468 if ( prop != NULL ) {
469 offset = prop->getFloatValue();
471 prop = child->getChild( "factor" );
472 if ( prop != NULL ) {
473 factor = prop->getFloatValue();
476 // Fetch the raw value
478 int raw_value = analog_in_data[index];
480 // Update the target properties
482 if ( type == "flight"
483 && !ignore_flight_controls->getBoolValue() )
485 if ( subtype != "pedals" ||
486 ( subtype == "pedals"
487 && !ignore_pedal_controls->getBoolValue() ) )
489 // "Cook" the raw value
490 float scaled_value = 0.0f;
492 if ( hysteresis > 0 ) {
493 int last_raw_value = 0;
494 prop = child->getChild( "last-raw-value", 0, true );
495 last_raw_value = prop->getIntValue();
497 if ( abs(raw_value - last_raw_value) < hysteresis )
499 // not enough movement stay put
500 raw_value = last_raw_value;
502 // update last raw value
503 prop->setIntValue( raw_value );
508 scaled_value = scale( center, deadband,
509 min, max, raw_value );
511 scaled_value = scale( min, max, raw_value );
513 scaled_value *= factor;
514 scaled_value += offset;
516 // final sanity clamp
518 scaled_value = clamp( -1.0, 1.0, scaled_value );
520 scaled_value = clamp( 0.0, 1.0, scaled_value );
523 // update the property tree values
524 for ( j = 0; j < (int)output_nodes.size(); ++j ) {
525 output_nodes[j]->setDoubleValue( scaled_value );
528 } else if ( type == "avionics-simple" ) {
529 // "Cook" the raw value
530 float scaled_value = 0.0f;
532 scaled_value = scale( center, deadband,
533 min, max, raw_value );
535 scaled_value = scale( min, max, raw_value );
537 scaled_value *= factor;
538 scaled_value += offset;
540 // final sanity clamp
542 scaled_value = clamp( -1.0, 1.0, scaled_value );
544 scaled_value = clamp( 0.0, 1.0, scaled_value );
547 // update the property tree values
548 for ( j = 0; j < (int)output_nodes.size(); ++j ) {
549 output_nodes[j]->setDoubleValue( scaled_value );
551 } else if ( type == "avionics-resolver" ) {
552 // this type of analog input impliments a
553 // rotational knob. We first caclulate the amount
554 // of knob rotation (slightly complex to work with
555 // hardware specific goofiness) and then multiply
556 // that amount of movement by a scaling factor,
557 // and finally add the result to the original
560 bool do_init = false;
561 float scaled_value = 0.0f;
563 // fetch intermediate values from property tree
565 prop = child->getChild( "is-inited", 0 );
566 if ( prop == NULL ) {
568 prop = child->getChild( "is-inited", 0, true );
569 prop->setBoolValue( true );
573 for ( j = 0; j < 3; ++j ) {
574 prop = child->getChild( "raw", j, true );
576 raw[j] = analog_in_data[index];
578 raw[j] = prop->getIntValue();
582 // do Tony's magic to calculate knob movement
583 // based on current analog input position and
585 int diff = tony_magic( analog_in_data[index], raw );
587 // write raw intermediate values (updated by
588 // tony_magic()) back to property tree
589 for ( j = 0; j < 3; ++j ) {
590 prop = child->getChild( "raw", j, true );
591 prop->setIntValue( raw[j] );
594 // filter knob position
595 prop = child->getChild( "diff-average", 0, true );
596 double diff_ave = prop->getDoubleValue();
597 diff_ave = instr_pot_filter( diff_ave, diff );
598 prop->setDoubleValue( diff_ave );
600 // calculate value adjustment in real world units
601 scaled_value = diff_ave * factor;
603 // update the property tree values
604 for ( j = 0; j < (int)output_nodes.size(); ++j ) {
605 float value = output_nodes[j]->getDoubleValue();
606 value += scaled_value;
608 prop = child->getChild( "min-clamp" );
609 if ( prop != NULL ) {
610 double min = prop->getDoubleValue();
611 if ( value < min ) { value = min; }
614 prop = child->getChild( "max-clamp" );
615 if ( prop != NULL ) {
616 double max = prop->getDoubleValue();
617 if ( value > max ) { value = max; }
620 prop = child->getChild( "compass-heading" );
621 if ( prop != NULL ) {
622 bool compass = prop->getBoolValue();
624 while ( value >= 360.0 ) { value -= 360.0; }
625 while ( value < 0.0 ) { value += 360.0; }
629 output_nodes[j]->setDoubleValue( value );
633 SG_LOG( SG_IO, SG_DEBUG, "Invalid channel type = "
637 SG_LOG( SG_IO, SG_DEBUG,
638 "Input config error, expecting 'channel' but found "
648 /////////////////////////////////////////////////////////////////////
649 // Read the switch positions
650 /////////////////////////////////////////////////////////////////////
652 // decode the packed switch data
653 static void update_switch_matrix(
655 unsigned char switch_data[ATC_SWITCH_BYTES],
656 int switch_matrix[2][ATC_NUM_COLS][ATC_SWITCH_BYTES] )
658 for ( int row = 0; row < ATC_SWITCH_BYTES; ++row ) {
659 unsigned char switches = switch_data[row];
661 for( int column = 0; column < ATC_NUM_COLS; ++column ) {
663 switch_matrix[board][column][row] = switches & 1;
665 switch_matrix[board][row-8][8+column] = switches & 1;
667 switches = switches >> 1;
672 bool FGATCInput::do_switches() {
674 ATCReadSwitches( switches_fd, switch_data );
676 // unpack the switch data
677 int switch_matrix[2][ATC_NUM_COLS][ATC_SWITCH_BYTES];
678 update_switch_matrix( board, switch_data, switch_matrix );
680 // Process the switch inputs
681 if ( switches_node != NULL ) {
682 for ( int i = 0; i < switches_node->nChildren(); ++i ) {
683 // read the next config entry from the property tree
685 SGPropertyNode *child = switches_node->getChild(i);
686 string cname = child->getName();
689 vector <SGPropertyNode *> output_nodes;
694 float scaled_value = 0.0f;
697 // get common options
699 SGPropertyNode *prop;
700 prop = child->getChild( "name" );
701 if ( prop != NULL ) {
702 name = prop->getStringValue();
704 prop = child->getChild( "type" );
705 if ( prop != NULL ) {
706 type = prop->getStringValue();
709 while ( (prop = child->getChild("prop", j)) != NULL ) {
711 = fgGetNode( prop->getStringValue(), true );
712 output_nodes.push_back( tmp );
715 prop = child->getChild( "factor" );
716 if ( prop != NULL ) {
717 factor = prop->getFloatValue();
719 prop = child->getChild( "invert" );
720 if ( prop != NULL ) {
721 invert = prop->getBoolValue();
723 prop = child->getChild( "steady-state-filter" );
724 if ( prop != NULL ) {
725 filter = prop->getIntValue();
728 // handle different types of switches
730 if ( cname == "switch" ) {
731 prop = child->getChild( "row" );
732 if ( prop != NULL ) {
733 row = prop->getIntValue();
735 prop = child->getChild( "col" );
736 if ( prop != NULL ) {
737 col = prop->getIntValue();
740 // Fetch the raw value
741 int raw_value = switch_matrix[board][row][col];
745 raw_value = !raw_value;
749 scaled_value = (float)raw_value * factor;
751 } else if ( cname == "combo-switch" ) {
752 float combo_value = 0.0f;
756 while ( (pos = child->getChild("position", k++)) != NULL ) {
757 // read the combo position entries from the property tree
759 prop = pos->getChild( "row" );
760 if ( prop != NULL ) {
761 row = prop->getIntValue();
763 prop = pos->getChild( "col" );
764 if ( prop != NULL ) {
765 col = prop->getIntValue();
767 prop = pos->getChild( "value" );
768 if ( prop != NULL ) {
769 combo_value = prop->getFloatValue();
772 // Fetch the raw value
773 int raw_value = switch_matrix[board][row][col];
774 // cout << "sm[" << board << "][" << row << "][" << col
775 // << "] = " << raw_value << endl;
778 // set scaled_value to the first combo_value
779 // that matches and jump out of loop.
780 scaled_value = combo_value;
786 scaled_value *= factor;
787 } else if ( cname == "additive-switch" ) {
788 float additive_value = 0.0f;
789 float increment = 0.0f;
793 while ( (pos = child->getChild("position", k++)) != NULL ) {
794 // read the combo position entries from the property tree
796 prop = pos->getChild( "row" );
797 if ( prop != NULL ) {
798 row = prop->getIntValue();
800 prop = pos->getChild( "col" );
801 if ( prop != NULL ) {
802 col = prop->getIntValue();
804 prop = pos->getChild( "value" );
805 if ( prop != NULL ) {
806 increment = prop->getFloatValue();
809 // Fetch the raw value
810 int raw_value = switch_matrix[board][row][col];
811 // cout << "sm[" << board << "][" << row << "][" << col
812 // << "] = " << raw_value << endl;
815 // set scaled_value to the first combo_value
816 // that matches and jump out of loop.
817 additive_value += increment;
822 scaled_value = additive_value * factor;
825 // handle filter request. The value of the switch must be
826 // steady-state for "n" frames before the property value
829 bool update_prop = true;
832 SGPropertyNode *fv = child->getChild( "filter-value", 0, true );
833 float filter_value = fv->getFloatValue();
834 SGPropertyNode *fc = child->getChild( "filter-count", 0, true );
835 int filter_count = fc->getIntValue();
837 if ( fabs(scaled_value - filter_value) < 0.0001 ) {
843 if ( filter_count < filter ) {
847 fv->setFloatValue( scaled_value );
848 fc->setIntValue( filter_count );
852 if ( type == "engine" || type == "flight" ) {
853 if ( ! ignore_flight_controls->getBoolValue() ) {
854 // update the property tree values
855 for ( j = 0; j < (int)output_nodes.size(); ++j ) {
856 output_nodes[j]->setDoubleValue( scaled_value );
859 } else if ( type == "avionics" ) {
860 // update the property tree values
861 for ( j = 0; j < (int)output_nodes.size(); ++j ) {
862 output_nodes[j]->setDoubleValue( scaled_value );
873 /////////////////////////////////////////////////////////////////////
874 // Read radio switches
875 /////////////////////////////////////////////////////////////////////
877 bool FGATCInput::do_radio_switches() {
879 ATCReadRadios( radios_fd, radio_switch_data );
881 // Process the radio switch/knob inputs
882 if ( radio_in_node != NULL ) {
883 for ( int i = 0; i < radio_in_node->nChildren(); ++i ) {
884 // read the next config entry from the property tree
886 SGPropertyNode *child = radio_in_node->getChild(i);
887 string cname = child->getName();
889 if ( cname == "switch" ) {
892 vector <SGPropertyNode *> output_nodes;
899 int scaled_value = 0;
900 // get common options
902 SGPropertyNode *prop;
903 prop = child->getChild( "name" );
904 if ( prop != NULL ) {
905 name = prop->getStringValue();
907 prop = child->getChild( "type" );
908 if ( prop != NULL ) {
909 type = prop->getStringValue();
912 while ( (prop = child->getChild("prop", j)) != NULL ) {
914 = fgGetNode( prop->getStringValue(), true );
915 output_nodes.push_back( tmp );
918 prop = child->getChild( "byte" );
919 if ( prop != NULL ) {
920 byte_num = prop->getIntValue();
922 prop = child->getChild( "right-shift" );
923 if ( prop != NULL ) {
924 right_shift = prop->getIntValue();
926 prop = child->getChild( "mask" );
927 if ( prop != NULL ) {
928 mask = prop->getIntValue();
930 prop = child->getChild( "factor" );
931 if ( prop != NULL ) {
932 factor = prop->getIntValue();
934 prop = child->getChild( "offset" );
935 if ( prop != NULL ) {
936 offset = prop->getIntValue();
938 prop = child->getChild( "invert" );
939 if ( prop != NULL ) {
940 invert = prop->getBoolValue();
943 // Fetch the raw value
945 = (radio_switch_data[byte_num] >> right_shift) & mask;
949 raw_value = !raw_value;
951 scaled_value = raw_value * factor + offset;
953 // update the property tree values
954 for ( j = 0; j < (int)output_nodes.size(); ++j ) {
955 output_nodes[j]->setIntValue( scaled_value );
965 // process the hardware inputs. This code assumes the calling layer
966 // will lock the hardware.
967 bool FGATCInput::process() {
969 SG_LOG( SG_IO, SG_ALERT, "This board has not been opened for input! "
982 bool FGATCInput::close() {
984 #if defined( unix ) || defined( __CYGWIN__ )
992 result = ::close( analog_in_fd );
993 if ( result == -1 ) {
994 SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
996 snprintf( msg, 256, "Error closing %s", analog_in_file );
1001 result = ::close( radios_fd );
1002 if ( result == -1 ) {
1003 SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
1005 snprintf( msg, 256, "Error closing %s", radios_file );
1010 result = ::close( switches_fd );
1011 if ( result == -1 ) {
1012 SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
1014 snprintf( msg, 256, "Error closing %s", switches_file );