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>
45 #include <simgear/debug/logstream.hxx>
46 #include <simgear/misc/sg_path.hxx>
47 #include <simgear/props/props_io.hxx>
49 #include <Main/fg_props.hxx>
51 #include "ATC-Inputs.hxx"
57 // Constructor: The _board parameter specifies which board to
58 // reference. Possible values are 0 or 1. The _config_file parameter
59 // specifies the location of the input config file (xml)
60 FGATCInput::FGATCInput( const int _board, const SGPath &_config_file ) :
62 ignore_flight_controls(NULL),
63 ignore_pedal_controls(NULL),
69 config = _config_file;
74 static void ATCReadAnalogInputs( int fd, unsigned char *analog_in_bytes ) {
75 #if defined( unix ) || defined( __CYGWIN__ )
77 lseek( fd, 0, SEEK_SET );
79 int result = read( fd, analog_in_bytes, ATC_ANAL_IN_BYTES );
80 if ( result != ATC_ANAL_IN_BYTES ) {
81 SG_LOG( SG_IO, SG_ALERT, "Read failed" );
88 // Read status of radio switches and knobs
89 static void ATCReadRadios( int fd, unsigned char *switch_data ) {
90 #if defined( unix ) || defined( __CYGWIN__ )
92 lseek( fd, 0, SEEK_SET );
94 int result = read( fd, switch_data, ATC_RADIO_SWITCH_BYTES );
95 if ( result != ATC_RADIO_SWITCH_BYTES ) {
96 SG_LOG( SG_IO, SG_ALERT, "Read failed" );
103 // Read switch inputs
104 static void ATCReadSwitches( int fd, unsigned char *switch_bytes ) {
105 #if defined( unix ) || defined( __CYGWIN__ )
107 lseek( fd, 0, SEEK_SET );
109 int result = read( fd, switch_bytes, ATC_SWITCH_BYTES );
110 if ( result != ATC_SWITCH_BYTES ) {
111 SG_LOG( SG_IO, SG_ALERT, "Read failed" );
118 void FGATCInput::init_config() {
119 #if defined( unix ) || defined( __CYGWIN__ )
120 if ( config.str()[0] != '/' ) {
121 // not an absolute path, prepend the standard location
123 char *envp = ::getenv( "HOME" );
124 if ( envp != NULL ) {
126 tmp.append( ".atcflightsim" );
127 tmp.append( config.str() );
131 readProperties( config.str(), globals->get_props() );
136 // Open and initialize the ATC hardware
137 bool FGATCInput::open() {
139 SG_LOG( SG_IO, SG_ALERT, "This board is already open for input! "
144 // This loads the config parameters generated by "simcal"
147 SG_LOG( SG_IO, SG_ALERT,
148 "Initializing ATC input hardware, please wait ..." );
150 snprintf( analog_in_file, 256,
151 "/proc/atcflightsim/board%d/analog_in", board );
152 snprintf( radios_file, 256,
153 "/proc/atcflightsim/board%d/radios", board );
154 snprintf( switches_file, 256,
155 "/proc/atcflightsim/board%d/switches", board );
157 #if defined( unix ) || defined( __CYGWIN__ )
159 /////////////////////////////////////////////////////////////////////
160 // Open the /proc files
161 /////////////////////////////////////////////////////////////////////
163 analog_in_fd = ::open( analog_in_file, O_RDONLY );
164 if ( analog_in_fd == -1 ) {
165 SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
167 snprintf( msg, 256, "Error opening %s", analog_in_file );
172 radios_fd = ::open( radios_file, O_RDWR );
173 if ( radios_fd == -1 ) {
174 SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
176 snprintf( msg, 256, "Error opening %s", radios_file );
181 switches_fd = ::open( switches_file, O_RDONLY );
182 if ( switches_fd == -1 ) {
183 SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
185 snprintf( msg, 256, "Error opening %s", switches_file );
192 /////////////////////////////////////////////////////////////////////
193 // Finished initing hardware
194 /////////////////////////////////////////////////////////////////////
196 SG_LOG( SG_IO, SG_ALERT,
197 "Done initializing ATC input hardware." );
201 /////////////////////////////////////////////////////////////////////
202 // Connect up to property values
203 /////////////////////////////////////////////////////////////////////
205 ignore_flight_controls
206 = fgGetNode( "/input/atcsim/ignore-flight-controls", true );
207 ignore_pedal_controls
208 = fgGetNode( "/input/atcsim/ignore-pedal-controls", true );
212 snprintf( base_name, 256, "/input/atc-board[%d]/analog-in", board );
213 analog_in_node = fgGetNode( base_name );
215 snprintf( base_name, 256, "/input/atc-board[%d]/radio-switches", board );
216 radio_in_node = fgGetNode( base_name );
218 snprintf( base_name, 256, "/input/atc-board[%d]/switches", board );
219 switches_node = fgGetNode( base_name );
225 /////////////////////////////////////////////////////////////////////
226 // Read analog inputs
227 /////////////////////////////////////////////////////////////////////
229 // scale a number between min and max (with center defined) to a scale
230 // from -1.0 to 1.0. The deadband value is symmetric, so specifying
231 // '1' will give you a deadband of +/-1
232 static double scale( int center, int deadband, int min, int max, int value ) {
233 // cout << center << " " << min << " " << max << " " << value << " ";
237 if ( value <= (center - deadband) ) {
238 range = (center - deadband) - min;
239 result = (value - (center - deadband)) / range;
240 } else if ( value >= (center + deadband) ) {
241 range = max - (center + deadband);
242 result = (value - (center + deadband)) / range;
247 if ( result < -1.0 ) result = -1.0;
248 if ( result > 1.0 ) result = 1.0;
250 // cout << result << endl;
256 // scale a number between min and max to a scale from 0.0 to 1.0
257 static double scale( int min, int max, int value ) {
258 // cout << center << " " << min << " " << max << " " << value << " ";
263 result = (value - min) / range;
265 if ( result < 0.0 ) result = 0.0;
266 if ( result > 1.0 ) result = 1.0;
268 // cout << result << endl;
274 static double clamp( double min, double max, double value ) {
275 double result = value;
277 if ( result < min ) result = min;
278 if ( result > max ) result = max;
280 // cout << result << endl;
286 static int tony_magic( int raw, int obs[3] ) {
292 if ( obs[2] >= 68 && obs[2] < 480 ) {
294 } else if ( obs[2] >= 480 ) {
299 } else if ( obs[1] < 68 ) {
302 } else if ( obs[2] < 30 ) {
303 if ( obs[1] >= 68 && obs[1] < 480 ) {
307 } else if ( obs[1] >= 480 ) {
309 if ( obs[0] < obs[1] ) {
317 } else if ( obs[1] > 980 ) {
318 if ( obs[2] <= 956 && obs[2] > 480 ) {
320 } else if ( obs[2] <= 480 ) {
325 } else if ( obs[1] > 956 ) {
328 } else if ( obs[2] > 980 ) {
329 if ( obs[1] <= 956 && obs[1] > 480 ) {
333 } else if ( obs[1] <= 480 ) {
335 if ( obs[0] > obs[1] ) {
344 if ( obs[1] < 480 && obs[2] > 480 ) {
345 // crossed gap going up
346 if ( obs[0] < obs[1] ) {
347 // caught a bogus intermediate value coming out of the gap
350 } else if ( obs[1] > 480 && obs[2] < 480 ) {
351 // crossed gap going down
352 if ( obs[0] > obs[1] ) {
353 // caught a bogus intermediate value coming out of the gap
356 } else if ( obs[0] > 480 && obs[1] < 480 && obs[2] < 480 ) {
357 // crossed the gap going down
358 if ( obs[1] > obs[2] ) {
359 // caught a bogus intermediate value coming out of the gap
362 } else if ( obs[0] < 480 && obs[1] > 480 && obs[2] > 480 ) {
363 // crossed the gap going up
364 if ( obs[1] < obs[2] ) {
365 // caught a bogus intermediate value coming out of the gap
369 result = obs[1] - obs[2];
370 if ( abs(result) > 400 ) {
378 // cout << " result = " << result << endl;
379 if ( result < -500 ) { result += 1024; }
380 if ( result > 500 ) { result -= 1024; }
386 static double instr_pot_filter( double ave, double val ) {
387 if ( fabs(ave - val) < 400 || fabs(val) < fabs(ave) ) {
388 return 0.5 * ave + 0.5 * val;
395 bool FGATCInput::do_analog_in() {
396 // Read raw data in byte form
397 ATCReadAnalogInputs( analog_in_fd, analog_in_bytes );
399 // Convert to integer values
400 for ( int channel = 0; channel < ATC_ANAL_IN_VALUES; ++channel ) {
401 unsigned char hi = analog_in_bytes[2 * channel] & 0x03;
402 unsigned char lo = analog_in_bytes[2 * channel + 1];
403 analog_in_data[channel] = hi * 256 + lo;
405 // printf("%02x %02x ", hi, lo );
406 // printf("%04d ", value );
409 // Process analog inputs
410 if ( analog_in_node != NULL ) {
411 for ( int i = 0; i < analog_in_node->nChildren(); ++i ) {
412 // read the next config entry from the property tree
414 SGPropertyNode *child = analog_in_node->getChild(i);
415 string cname = child->getName();
416 int index = child->getIndex();
420 vector <SGPropertyNode *> output_nodes;
428 if ( cname == "channel" ) {
429 SGPropertyNode *prop;
430 prop = child->getChild( "name" );
431 if ( prop != NULL ) {
432 name = prop->getStringValue();
434 prop = child->getChild( "type", 0 );
435 if ( prop != NULL ) {
436 type = prop->getStringValue();
438 prop = child->getChild( "type", 1 );
439 if ( prop != NULL ) {
440 subtype = prop->getStringValue();
443 while ( (prop = child->getChild("prop", j)) != NULL ) {
445 = fgGetNode( prop->getStringValue(), true );
446 output_nodes.push_back( tmp );
449 prop = child->getChild( "center" );
450 if ( prop != NULL ) {
451 center = prop->getIntValue();
453 prop = child->getChild( "min" );
454 if ( prop != NULL ) {
455 min = prop->getIntValue();
457 prop = child->getChild( "max" );
458 if ( prop != NULL ) {
459 max = prop->getIntValue();
461 prop = child->getChild( "deadband" );
462 if ( prop != NULL ) {
463 deadband = prop->getIntValue();
465 prop = child->getChild( "hysteresis" );
466 if ( prop != NULL ) {
467 hysteresis = prop->getIntValue();
469 prop = child->getChild( "offset" );
470 if ( prop != NULL ) {
471 offset = prop->getFloatValue();
473 prop = child->getChild( "factor" );
474 if ( prop != NULL ) {
475 factor = prop->getFloatValue();
478 // Fetch the raw value
480 int raw_value = analog_in_data[index];
482 // Update the target properties
484 if ( type == "flight"
485 && !ignore_flight_controls->getBoolValue() )
487 if ( subtype != "pedals" ||
488 ( subtype == "pedals"
489 && !ignore_pedal_controls->getBoolValue() ) )
491 // "Cook" the raw value
492 float scaled_value = 0.0f;
494 if ( hysteresis > 0 ) {
495 int last_raw_value = 0;
496 prop = child->getChild( "last-raw-value", 0, true );
497 last_raw_value = prop->getIntValue();
499 if ( abs(raw_value - last_raw_value) < hysteresis )
501 // not enough movement stay put
502 raw_value = last_raw_value;
504 // update last raw value
505 prop->setIntValue( raw_value );
510 scaled_value = scale( center, deadband,
511 min, max, raw_value );
513 scaled_value = scale( min, max, raw_value );
515 scaled_value *= factor;
516 scaled_value += offset;
518 // final sanity clamp
520 scaled_value = clamp( -1.0, 1.0, scaled_value );
522 scaled_value = clamp( 0.0, 1.0, scaled_value );
525 // update the property tree values
526 for ( j = 0; j < (int)output_nodes.size(); ++j ) {
527 output_nodes[j]->setDoubleValue( scaled_value );
530 } else if ( type == "avionics-simple" ) {
531 // "Cook" the raw value
532 float scaled_value = 0.0f;
534 scaled_value = scale( center, deadband,
535 min, max, raw_value );
537 scaled_value = scale( min, max, raw_value );
539 scaled_value *= factor;
540 scaled_value += offset;
542 // final sanity clamp
544 scaled_value = clamp( -1.0, 1.0, scaled_value );
546 scaled_value = clamp( 0.0, 1.0, scaled_value );
549 // update the property tree values
550 for ( j = 0; j < (int)output_nodes.size(); ++j ) {
551 output_nodes[j]->setDoubleValue( scaled_value );
553 } else if ( type == "avionics-resolver" ) {
554 // this type of analog input impliments a
555 // rotational knob. We first caclulate the amount
556 // of knob rotation (slightly complex to work with
557 // hardware specific goofiness) and then multiply
558 // that amount of movement by a scaling factor,
559 // and finally add the result to the original
562 bool do_init = false;
563 float scaled_value = 0.0f;
565 // fetch intermediate values from property tree
567 prop = child->getChild( "is-inited", 0 );
568 if ( prop == NULL ) {
570 prop = child->getChild( "is-inited", 0, true );
571 prop->setBoolValue( true );
575 for ( j = 0; j < 3; ++j ) {
576 prop = child->getChild( "raw", j, true );
578 raw[j] = analog_in_data[index];
580 raw[j] = prop->getIntValue();
584 // do Tony's magic to calculate knob movement
585 // based on current analog input position and
587 int diff = tony_magic( analog_in_data[index], raw );
589 // write raw intermediate values (updated by
590 // tony_magic()) back to property tree
591 for ( j = 0; j < 3; ++j ) {
592 prop = child->getChild( "raw", j, true );
593 prop->setIntValue( raw[j] );
596 // filter knob position
597 prop = child->getChild( "diff-average", 0, true );
598 double diff_ave = prop->getDoubleValue();
599 diff_ave = instr_pot_filter( diff_ave, diff );
600 prop->setDoubleValue( diff_ave );
602 // calculate value adjustment in real world units
603 scaled_value = diff_ave * factor;
605 // update the property tree values
606 for ( j = 0; j < (int)output_nodes.size(); ++j ) {
607 float value = output_nodes[j]->getDoubleValue();
608 value += scaled_value;
610 prop = child->getChild( "min-clamp" );
611 if ( prop != NULL ) {
612 double min = prop->getDoubleValue();
613 if ( value < min ) { value = min; }
616 prop = child->getChild( "max-clamp" );
617 if ( prop != NULL ) {
618 double max = prop->getDoubleValue();
619 if ( value > max ) { value = max; }
622 prop = child->getChild( "compass-heading" );
623 if ( prop != NULL ) {
624 bool compass = prop->getBoolValue();
626 while ( value >= 360.0 ) { value -= 360.0; }
627 while ( value < 0.0 ) { value += 360.0; }
631 output_nodes[j]->setDoubleValue( value );
635 SG_LOG( SG_IO, SG_DEBUG, "Invalid channel type = "
639 SG_LOG( SG_IO, SG_DEBUG,
640 "Input config error, expecting 'channel' but found "
650 /////////////////////////////////////////////////////////////////////
651 // Read the switch positions
652 /////////////////////////////////////////////////////////////////////
654 // decode the packed switch data
655 static void update_switch_matrix(
657 unsigned char switch_data[ATC_SWITCH_BYTES],
658 int switch_matrix[2][ATC_NUM_COLS][ATC_SWITCH_BYTES] )
660 for ( int row = 0; row < ATC_SWITCH_BYTES; ++row ) {
661 unsigned char switches = switch_data[row];
663 for( int column = 0; column < ATC_NUM_COLS; ++column ) {
665 switch_matrix[board][column][row] = switches & 1;
667 switch_matrix[board][row-8][8+column] = switches & 1;
669 switches = switches >> 1;
674 bool FGATCInput::do_switches() {
676 ATCReadSwitches( switches_fd, switch_data );
678 // unpack the switch data
679 int switch_matrix[2][ATC_NUM_COLS][ATC_SWITCH_BYTES];
680 update_switch_matrix( board, switch_data, switch_matrix );
682 // Process the switch inputs
683 if ( switches_node != NULL ) {
684 for ( int i = 0; i < switches_node->nChildren(); ++i ) {
685 // read the next config entry from the property tree
687 SGPropertyNode *child = switches_node->getChild(i);
688 string cname = child->getName();
691 vector <SGPropertyNode *> output_nodes;
696 float scaled_value = 0.0f;
699 // get common options
701 SGPropertyNode *prop;
702 prop = child->getChild( "name" );
703 if ( prop != NULL ) {
704 name = prop->getStringValue();
706 prop = child->getChild( "type" );
707 if ( prop != NULL ) {
708 type = prop->getStringValue();
711 while ( (prop = child->getChild("prop", j)) != NULL ) {
713 = fgGetNode( prop->getStringValue(), true );
714 output_nodes.push_back( tmp );
717 prop = child->getChild( "factor" );
718 if ( prop != NULL ) {
719 factor = prop->getFloatValue();
721 prop = child->getChild( "invert" );
722 if ( prop != NULL ) {
723 invert = prop->getBoolValue();
725 prop = child->getChild( "steady-state-filter" );
726 if ( prop != NULL ) {
727 filter = prop->getIntValue();
730 // handle different types of switches
732 if ( cname == "switch" ) {
733 prop = child->getChild( "row" );
734 if ( prop != NULL ) {
735 row = prop->getIntValue();
737 prop = child->getChild( "col" );
738 if ( prop != NULL ) {
739 col = prop->getIntValue();
742 // Fetch the raw value
743 int raw_value = switch_matrix[board][row][col];
747 raw_value = !raw_value;
751 scaled_value = (float)raw_value * factor;
753 } else if ( cname == "combo-switch" ) {
754 float combo_value = 0.0f;
758 while ( (pos = child->getChild("position", k++)) != NULL ) {
759 // read the combo position entries from the property tree
761 prop = pos->getChild( "row" );
762 if ( prop != NULL ) {
763 row = prop->getIntValue();
765 prop = pos->getChild( "col" );
766 if ( prop != NULL ) {
767 col = prop->getIntValue();
769 prop = pos->getChild( "value" );
770 if ( prop != NULL ) {
771 combo_value = prop->getFloatValue();
774 // Fetch the raw value
775 int raw_value = switch_matrix[board][row][col];
776 // cout << "sm[" << board << "][" << row << "][" << col
777 // << "] = " << raw_value << endl;
780 // set scaled_value to the first combo_value
781 // that matches and jump out of loop.
782 scaled_value = combo_value;
788 scaled_value *= factor;
789 } else if ( cname == "additive-switch" ) {
790 float additive_value = 0.0f;
791 float increment = 0.0f;
795 while ( (pos = child->getChild("position", k++)) != NULL ) {
796 // read the combo position entries from the property tree
798 prop = pos->getChild( "row" );
799 if ( prop != NULL ) {
800 row = prop->getIntValue();
802 prop = pos->getChild( "col" );
803 if ( prop != NULL ) {
804 col = prop->getIntValue();
806 prop = pos->getChild( "value" );
807 if ( prop != NULL ) {
808 increment = prop->getFloatValue();
811 // Fetch the raw value
812 int raw_value = switch_matrix[board][row][col];
813 // cout << "sm[" << board << "][" << row << "][" << col
814 // << "] = " << raw_value << endl;
817 // set scaled_value to the first combo_value
818 // that matches and jump out of loop.
819 additive_value += increment;
824 scaled_value = additive_value * factor;
827 // handle filter request. The value of the switch must be
828 // steady-state for "n" frames before the property value
831 bool update_prop = true;
834 SGPropertyNode *fv = child->getChild( "filter-value", 0, true );
835 float filter_value = fv->getFloatValue();
836 SGPropertyNode *fc = child->getChild( "filter-count", 0, true );
837 int filter_count = fc->getIntValue();
839 if ( fabs(scaled_value - filter_value) < 0.0001 ) {
845 if ( filter_count < filter ) {
849 fv->setFloatValue( scaled_value );
850 fc->setIntValue( filter_count );
854 if ( type == "engine" || type == "flight" ) {
855 if ( ! ignore_flight_controls->getBoolValue() ) {
856 // update the property tree values
857 for ( j = 0; j < (int)output_nodes.size(); ++j ) {
858 output_nodes[j]->setDoubleValue( scaled_value );
861 } else if ( type == "avionics" ) {
862 // update the property tree values
863 for ( j = 0; j < (int)output_nodes.size(); ++j ) {
864 output_nodes[j]->setDoubleValue( scaled_value );
875 /////////////////////////////////////////////////////////////////////
876 // Read radio switches
877 /////////////////////////////////////////////////////////////////////
879 bool FGATCInput::do_radio_switches() {
881 ATCReadRadios( radios_fd, radio_switch_data );
883 // Process the radio switch/knob inputs
884 if ( radio_in_node != NULL ) {
885 for ( int i = 0; i < radio_in_node->nChildren(); ++i ) {
886 // read the next config entry from the property tree
888 SGPropertyNode *child = radio_in_node->getChild(i);
889 string cname = child->getName();
891 if ( cname == "switch" ) {
894 vector <SGPropertyNode *> output_nodes;
901 int scaled_value = 0;
902 // get common options
904 SGPropertyNode *prop;
905 prop = child->getChild( "name" );
906 if ( prop != NULL ) {
907 name = prop->getStringValue();
909 prop = child->getChild( "type" );
910 if ( prop != NULL ) {
911 type = prop->getStringValue();
914 while ( (prop = child->getChild("prop", j)) != NULL ) {
916 = fgGetNode( prop->getStringValue(), true );
917 output_nodes.push_back( tmp );
920 prop = child->getChild( "byte" );
921 if ( prop != NULL ) {
922 byte_num = prop->getIntValue();
924 prop = child->getChild( "right-shift" );
925 if ( prop != NULL ) {
926 right_shift = prop->getIntValue();
928 prop = child->getChild( "mask" );
929 if ( prop != NULL ) {
930 mask = prop->getIntValue();
932 prop = child->getChild( "factor" );
933 if ( prop != NULL ) {
934 factor = prop->getIntValue();
936 prop = child->getChild( "offset" );
937 if ( prop != NULL ) {
938 offset = prop->getIntValue();
940 prop = child->getChild( "invert" );
941 if ( prop != NULL ) {
942 invert = prop->getBoolValue();
945 // Fetch the raw value
947 = (radio_switch_data[byte_num] >> right_shift) & mask;
951 raw_value = !raw_value;
953 scaled_value = raw_value * factor + offset;
955 // update the property tree values
956 for ( j = 0; j < (int)output_nodes.size(); ++j ) {
957 output_nodes[j]->setIntValue( scaled_value );
967 // process the hardware inputs. This code assumes the calling layer
968 // will lock the hardware.
969 bool FGATCInput::process() {
971 SG_LOG( SG_IO, SG_ALERT, "This board has not been opened for input! "
984 bool FGATCInput::close() {
986 #if defined( unix ) || defined( __CYGWIN__ )
994 result = ::close( analog_in_fd );
995 if ( result == -1 ) {
996 SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
998 snprintf( msg, 256, "Error closing %s", analog_in_file );
1003 result = ::close( radios_fd );
1004 if ( result == -1 ) {
1005 SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
1007 snprintf( msg, 256, "Error closing %s", radios_file );
1012 result = ::close( switches_fd );
1013 if ( result == -1 ) {
1014 SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
1016 snprintf( msg, 256, "Error closing %s", switches_file );