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., 675 Mass Ave, Cambridge, MA 02139, USA.
28 #include <simgear/compiler.h>
32 #include <simgear/debug/logstream.hxx>
34 #include <Main/fg_props.hxx>
36 #include "ATC-Inputs.hxx"
42 // Constructor: The _board parameter specifies which board to
43 // reference. Possible values are 0 or 1. The _config_file parameter
44 // specifies the location of the input config file (xml)
45 FGATCInput::FGATCInput( const int _board, const SGPath &_config_file ) :
47 ignore_flight_controls(NULL),
48 ignore_pedal_controls(NULL),
54 config = _config_file;
59 static void ATCReadAnalogInputs( int fd, unsigned char *analog_in_bytes ) {
61 lseek( fd, 0, SEEK_SET );
63 int result = read( fd, analog_in_bytes, ATC_ANAL_IN_BYTES );
64 if ( result != ATC_ANAL_IN_BYTES ) {
65 SG_LOG( SG_IO, SG_ALERT, "Read failed" );
71 // Read status of radio switches and knobs
72 static void ATCReadRadios( int fd, unsigned char *switch_data ) {
74 lseek( fd, 0, SEEK_SET );
76 int result = read( fd, switch_data, ATC_RADIO_SWITCH_BYTES );
77 if ( result != ATC_RADIO_SWITCH_BYTES ) {
78 SG_LOG( SG_IO, SG_ALERT, "Read failed" );
85 static void ATCReadSwitches( int fd, unsigned char *switch_bytes ) {
87 lseek( fd, 0, SEEK_SET );
89 int result = read( fd, switch_bytes, ATC_SWITCH_BYTES );
90 if ( result != ATC_SWITCH_BYTES ) {
91 SG_LOG( SG_IO, SG_ALERT, "Read failed" );
97 void FGATCInput::init_config() {
98 #if defined( unix ) || defined( __CYGWIN__ )
99 if ( config.str()[0] != '/' ) {
100 // not an absolute path, prepend the standard location
102 char *envp = ::getenv( "HOME" );
103 if ( envp != NULL ) {
105 tmp.append( ".atcflightsim" );
106 tmp.append( config.str() );
110 readProperties( config.str(), globals->get_props() );
115 // Open and initialize the ATC hardware
116 bool FGATCInput::open() {
118 SG_LOG( SG_IO, SG_ALERT, "This board is already open for input! "
123 // This loads the config parameters generated by "simcal"
126 SG_LOG( SG_IO, SG_ALERT,
127 "Initializing ATC 610x hardware, please wait ..." );
129 snprintf( lock_file, 256, "/proc/atc610x/board%d/lock", board );
130 snprintf( analog_in_file, 256, "/proc/atc610x/board%d/analog_in", board );
131 snprintf( radios_file, 256, "/proc/atc610x/board%d/radios", board );
132 snprintf( switches_file, 256, "/proc/atc610x/board%d/switches", board );
134 /////////////////////////////////////////////////////////////////////
135 // Open the /proc files
136 /////////////////////////////////////////////////////////////////////
138 lock_fd = ::open( lock_file, O_RDWR );
139 if ( lock_fd == -1 ) {
140 SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
142 snprintf( msg, 256, "Error opening %s", lock_file );
147 analog_in_fd = ::open( analog_in_file, O_RDONLY );
148 if ( analog_in_fd == -1 ) {
149 SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
151 snprintf( msg, 256, "Error opening %s", analog_in_file );
156 radios_fd = ::open( radios_file, O_RDWR );
157 if ( radios_fd == -1 ) {
158 SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
160 snprintf( msg, 256, "Error opening %s", radios_file );
165 switches_fd = ::open( switches_file, O_RDONLY );
166 if ( switches_fd == -1 ) {
167 SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
169 snprintf( msg, 256, "Error opening %s", switches_file );
174 /////////////////////////////////////////////////////////////////////
175 // Finished initing hardware
176 /////////////////////////////////////////////////////////////////////
178 SG_LOG( SG_IO, SG_ALERT,
179 "Done initializing ATC 610x hardware." );
183 /////////////////////////////////////////////////////////////////////
184 // Connect up to property values
185 /////////////////////////////////////////////////////////////////////
187 ignore_flight_controls
188 = fgGetNode( "/input/atcsim/ignore-flight-controls", true );
189 ignore_pedal_controls
190 = fgGetNode( "/input/atcsim/ignore-pedal-controls", true );
194 snprintf( base_name, 256, "/input/atc-board[%d]/analog-in", board );
195 analog_in_node = fgGetNode( base_name );
197 snprintf( base_name, 256, "/input/atc-board[%d]/radio-switches", board );
198 radio_in_node = fgGetNode( base_name );
200 snprintf( base_name, 256, "/input/atc-board[%d]/switches", board );
201 switches_node = fgGetNode( base_name );
207 /////////////////////////////////////////////////////////////////////
208 // Read analog inputs
209 /////////////////////////////////////////////////////////////////////
211 // scale a number between min and max (with center defined) to a scale
213 static double scale( int center, int min, int max, int value ) {
214 // cout << center << " " << min << " " << max << " " << value << " ";
218 if ( value <= center ) {
219 range = center - min;
220 result = (value - center) / range;
222 range = max - center;
223 result = (value - center) / range;
226 if ( result < -1.0 ) result = -1.0;
227 if ( result > 1.0 ) result = 1.0;
229 // cout << result << endl;
235 // scale a number between min and max to a scale from 0.0 to 1.0
236 static double scale( int min, int max, int value ) {
237 // cout << center << " " << min << " " << max << " " << value << " ";
242 result = (value - min) / range;
244 if ( result < 0.0 ) result = 0.0;
245 if ( result > 1.0 ) result = 1.0;
247 // cout << result << endl;
253 static int tony_magic( int raw, int obs[3] ) {
259 if ( obs[2] >= 68 && obs[2] < 480 ) {
261 } else if ( obs[2] >= 480 ) {
266 } else if ( obs[1] < 68 ) {
269 } else if ( obs[2] < 30 ) {
270 if ( obs[1] >= 68 && obs[1] < 480 ) {
274 } else if ( obs[1] >= 480 ) {
276 if ( obs[0] < obs[1] ) {
284 } else if ( obs[1] > 980 ) {
285 if ( obs[2] <= 956 && obs[2] > 480 ) {
287 } else if ( obs[2] <= 480 ) {
292 } else if ( obs[1] > 956 ) {
295 } else if ( obs[2] > 980 ) {
296 if ( obs[1] <= 956 && obs[1] > 480 ) {
300 } else if ( obs[1] <= 480 ) {
302 if ( obs[0] > obs[1] ) {
311 if ( obs[1] < 480 && obs[2] > 480 ) {
312 // crossed gap going up
313 if ( obs[0] < obs[1] ) {
314 // caught a bogus intermediate value coming out of the gap
317 } else if ( obs[1] > 480 && obs[2] < 480 ) {
318 // crossed gap going down
319 if ( obs[0] > obs[1] ) {
320 // caught a bogus intermediate value coming out of the gap
323 } else if ( obs[0] > 480 && obs[1] < 480 && obs[2] < 480 ) {
324 // crossed the gap going down
325 if ( obs[1] > obs[2] ) {
326 // caught a bogus intermediate value coming out of the gap
329 } else if ( obs[0] < 480 && obs[1] > 480 && obs[2] > 480 ) {
330 // crossed the gap going up
331 if ( obs[1] < obs[2] ) {
332 // caught a bogus intermediate value coming out of the gap
336 result = obs[1] - obs[2];
337 if ( abs(result) > 400 ) {
345 // cout << " result = " << result << endl;
346 if ( result < -500 ) { result += 1024; }
347 if ( result > 500 ) { result -= 1024; }
353 static double instr_pot_filter( double ave, double val ) {
354 if ( fabs(ave - val) < 400 || fabs(val) < fabs(ave) ) {
355 return 0.5 * ave + 0.5 * val;
362 bool FGATCInput::do_analog_in() {
363 // Read raw data in byte form
364 ATCReadAnalogInputs( analog_in_fd, analog_in_bytes );
366 // Convert to integer values
367 for ( int channel = 0; channel < ATC_ANAL_IN_VALUES; ++channel ) {
368 unsigned char hi = analog_in_bytes[2 * channel] & 0x03;
369 unsigned char lo = analog_in_bytes[2 * channel + 1];
370 analog_in_data[channel] = hi * 256 + lo;
372 // printf("%02x %02x ", hi, lo );
373 // printf("%04d ", value );
376 // Process analog inputs
377 if ( analog_in_node != NULL ) {
378 for ( int i = 0; i < analog_in_node->nChildren(); ++i ) {
379 // read the next config entry from the property tree
381 SGPropertyNode *child = analog_in_node->getChild(i);
382 string cname = child->getName();
383 int index = child->getIndex();
387 vector <SGPropertyNode *> output_nodes; output_nodes.clear();
392 if ( cname == "channel" ) {
393 SGPropertyNode *prop;
394 prop = child->getChild( "name" );
395 if ( prop != NULL ) {
396 name = prop->getStringValue();
398 prop = child->getChild( "type", 0 );
399 if ( prop != NULL ) {
400 type = prop->getStringValue();
402 prop = child->getChild( "type", 1 );
403 if ( prop != NULL ) {
404 subtype = prop->getStringValue();
407 while ( (prop = child->getChild("prop", j)) != NULL ) {
409 = fgGetNode( prop->getStringValue(), true );
410 output_nodes.push_back( tmp );
413 prop = child->getChild( "center" );
414 if ( prop != NULL ) {
415 center = prop->getIntValue();
417 prop = child->getChild( "min" );
418 if ( prop != NULL ) {
419 min = prop->getIntValue();
421 prop = child->getChild( "max" );
422 if ( prop != NULL ) {
423 max = prop->getIntValue();
425 prop = child->getChild( "factor" );
426 if ( prop != NULL ) {
427 factor = prop->getFloatValue();
430 // Fetch the raw value
432 int raw_value = analog_in_data[index];
434 // Update the target properties
436 if ( type == "flight"
437 && !ignore_flight_controls->getBoolValue() )
439 if ( subtype != "pedals" ||
440 ( subtype == "pedals"
441 && !ignore_pedal_controls->getBoolValue() ) )
443 // "Cook" the raw value
444 float scaled_value = 0.0f;
446 scaled_value = scale( center, min, max, raw_value );
448 scaled_value = scale( min, max, raw_value );
450 scaled_value *= factor;
452 // update the property tree values
453 for ( j = 0; j < (int)output_nodes.size(); ++j ) {
454 output_nodes[j]->setDoubleValue( scaled_value );
457 } else if ( type == "avionics-simple" ) {
458 // "Cook" the raw value
459 float scaled_value = 0.0f;
461 scaled_value = scale( center, min, max, raw_value );
463 scaled_value = scale( min, max, raw_value );
465 scaled_value *= factor;
467 // update the property tree values
468 for ( j = 0; j < (int)output_nodes.size(); ++j ) {
469 output_nodes[j]->setDoubleValue( scaled_value );
471 } else if ( type == "avionics-resolver" ) {
472 // this type of analog input impliments a
473 // rotational knob. We first caclulate the amount
474 // of knob rotation (slightly complex to work with
475 // hardware specific goofiness) and then multiply
476 // that amount of movement by a scaling factor,
477 // and finally add the result to the original
480 bool do_init = false;
481 float scaled_value = 0.0f;
483 // fetch intermediate values from property tree
485 prop = child->getChild( "is-inited", 0 );
486 if ( prop == NULL ) {
488 prop = child->getChild( "is-inited", 0, true );
489 prop->setBoolValue( true );
493 for ( j = 0; j < 3; ++j ) {
494 prop = child->getChild( "raw", j, true );
496 raw[j] = analog_in_data[index];
498 raw[j] = prop->getIntValue();
502 // do Tony's magic to calculate knob movement
503 // based on current analog input position and
505 int diff = tony_magic( analog_in_data[index], raw );
507 // write raw intermediate values (updated by
508 // tony_magic()) back to property tree
509 for ( j = 0; j < 3; ++j ) {
510 prop = child->getChild( "raw", j, true );
511 prop->setIntValue( raw[j] );
514 // filter knob position
515 prop = child->getChild( "diff-average", 0, true );
516 double diff_ave = prop->getDoubleValue();
517 diff_ave = instr_pot_filter( diff_ave, diff );
518 prop->setDoubleValue( diff_ave );
520 // calculate value adjustment in real world units
521 scaled_value = diff_ave * factor;
523 // update the property tree values
524 for ( j = 0; j < (int)output_nodes.size(); ++j ) {
525 float value = output_nodes[j]->getDoubleValue();
526 value += scaled_value;
528 prop = child->getChild( "min-clamp" );
529 if ( prop != NULL ) {
530 double min = prop->getDoubleValue();
531 if ( value < min ) { value = min; }
534 prop = child->getChild( "max-clamp" );
535 if ( prop != NULL ) {
536 double max = prop->getDoubleValue();
537 if ( value > max ) { value = max; }
540 prop = child
\0->getChild( "compass-heading" );
541 if ( prop != NULL ) {
542 bool compass = prop->getBoolValue();
544 while ( value >= 360.0 ) { value -= 360.0; }
545 while ( value < 0.0 ) { value += 360.0; }
549 output_nodes[j]->setDoubleValue( value );
553 SG_LOG( SG_IO, SG_DEBUG, "Invalid channel type = "
557 SG_LOG( SG_IO, SG_DEBUG,
558 "Input config error, expecting 'channel' but found "
568 /////////////////////////////////////////////////////////////////////
569 // Read the switch positions
570 /////////////////////////////////////////////////////////////////////
572 // decode the packed switch data
573 static void update_switch_matrix(
575 unsigned char switch_data[ATC_SWITCH_BYTES],
576 int switch_matrix[2][ATC_NUM_COLS][ATC_SWITCH_BYTES] )
578 for ( int row = 0; row < ATC_SWITCH_BYTES; ++row ) {
579 unsigned char switches = switch_data[row];
581 for( int column = 0; column < ATC_NUM_COLS; ++column ) {
582 switch_matrix[board][column][row] = switches & 1;
583 switches = switches >> 1;
588 bool FGATCInput::do_switches() {
590 ATCReadSwitches( switches_fd, switch_data );
592 // unpack the switch data
593 int switch_matrix[2][ATC_NUM_COLS][ATC_SWITCH_BYTES];
594 update_switch_matrix( board, switch_data, switch_matrix );
596 // Process the switch inputs
597 if ( switches_node != NULL ) {
598 for ( int i = 0; i < switches_node->nChildren(); ++i ) {
599 // read the next config entry from the property tree
601 SGPropertyNode *child = switches_node->getChild(i);
602 string cname = child->getName();
605 vector <SGPropertyNode *> output_nodes; output_nodes.clear();
610 float scaled_value = 0.0f;
612 // get common options
614 SGPropertyNode *prop;
615 prop = child->getChild( "name" );
616 if ( prop != NULL ) {
617 name = prop->getStringValue();
619 prop = child->getChild( "type" );
620 if ( prop != NULL ) {
621 type = prop->getStringValue();
624 while ( (prop = child->getChild("prop", j)) != NULL ) {
626 = fgGetNode( prop->getStringValue(), true );
627 output_nodes.push_back( tmp );
630 prop = child->getChild( "factor" );
631 if ( prop != NULL ) {
632 factor = prop->getFloatValue();
634 prop = child->getChild( "steady-state-filter" );
635 if ( prop != NULL ) {
636 filter = prop->getIntValue();
639 // handle different types of switches
641 if ( cname == "switch" ) {
642 prop = child->getChild( "row" );
643 if ( prop != NULL ) {
644 row = prop->getIntValue();
646 prop = child->getChild( "col" );
647 if ( prop != NULL ) {
648 col = prop->getIntValue();
651 // Fetch the raw value
652 int raw_value = switch_matrix[board][row][col];
655 scaled_value = (float)raw_value * factor;
657 } else if ( cname == "combo-switch" ) {
658 float combo_value = 0.0f;
662 while ( (pos = child->getChild("position", k++)) != NULL ) {
663 // read the combo position entries from the property tree
665 prop = pos->getChild( "row" );
666 if ( prop != NULL ) {
667 row = prop->getIntValue();
669 prop = pos->getChild( "col" );
670 if ( prop != NULL ) {
671 col = prop->getIntValue();
673 prop = pos->getChild( "value" );
674 if ( prop != NULL ) {
675 combo_value = prop->getFloatValue();
678 // Fetch the raw value
679 int raw_value = switch_matrix[board][row][col];
680 // cout << "sm[" << board << "][" << row << "][" << col
681 // << "] = " << raw_value << endl;
684 // set scaled_value to the first combo_value
685 // that matches and jump out of loop.
686 scaled_value = combo_value;
692 scaled_value *= factor;
695 // handle filter request. The value of the switch must be
696 // steady-state for "n" frames before the property value
699 bool update_prop = true;
702 SGPropertyNode *fv = child->getChild( "filter-value", 0, true );
703 float filter_value = fv->getFloatValue();
704 SGPropertyNode *fc = child->getChild( "filter-count", 0, true );
705 int filter_count = fc->getIntValue();
707 if ( fabs(scaled_value - filter_value) < 0.0001 ) {
713 if ( filter_count < filter ) {
717 fv->setFloatValue( scaled_value );
718 fc->setIntValue( filter_count );
722 if ( type == "engine" || type == "flight" ) {
723 if ( ! ignore_flight_controls->getBoolValue() ) {
724 // update the property tree values
725 for ( j = 0; j < (int)output_nodes.size(); ++j ) {
726 output_nodes[j]->setDoubleValue( scaled_value );
729 } else if ( type == "avionics" ) {
730 // update the property tree values
731 for ( j = 0; j < (int)output_nodes.size(); ++j ) {
732 output_nodes[j]->setDoubleValue( scaled_value );
743 /////////////////////////////////////////////////////////////////////
744 // Read radio switches
745 /////////////////////////////////////////////////////////////////////
747 bool FGATCInput::do_radio_switches() {
749 ATCReadRadios( radios_fd, radio_switch_data );
751 // Process the radio switch/knob inputs
752 if ( radio_in_node != NULL ) {
753 for ( int i = 0; i < radio_in_node->nChildren(); ++i ) {
754 // read the next config entry from the property tree
756 SGPropertyNode *child = radio_in_node->getChild(i);
757 string cname = child->getName();
759 if ( cname == "switch" ) {
762 vector <SGPropertyNode *> output_nodes; output_nodes.clear();
764 int right_shift = -1;
768 int scaled_value = 0;
770 // get common options
772 SGPropertyNode *prop;
773 prop = child->getChild( "name" );
774 if ( prop != NULL ) {
775 name = prop->getStringValue();
777 prop = child->getChild( "type" );
778 if ( prop != NULL ) {
779 type = prop->getStringValue();
782 while ( (prop = child->getChild("prop", j)) != NULL ) {
784 = fgGetNode( prop->getStringValue(), true );
785 output_nodes.push_back( tmp );
788 prop = child->getChild( "byte" );
789 if ( prop != NULL ) {
790 byte_num = prop->getIntValue();
792 prop = child->getChild( "right-shift" );
793 if ( prop != NULL ) {
794 right_shift = prop->getIntValue();
796 prop = child->getChild( "mask" );
797 if ( prop != NULL ) {
798 mask = prop->getIntValue();
800 prop = child->getChild( "factor" );
801 if ( prop != NULL ) {
802 factor = prop->getIntValue();
804 prop = child->getChild( "offset" );
805 if ( prop != NULL ) {
806 offset = prop->getIntValue();
809 // Fetch the raw value
811 = (radio_switch_data[byte_num] >> right_shift) & mask;
814 scaled_value = raw_value * factor + offset;
816 // update the property tree values
817 for ( j = 0; j < (int)output_nodes.size(); ++j ) {
818 output_nodes[j]->setIntValue( scaled_value );
828 // process the hardware inputs. This code assumes the calling layer
829 // will lock the hardware.
830 bool FGATCInput::process() {
832 SG_LOG( SG_IO, SG_ALERT, "This board has not been opened for input! "
845 bool FGATCInput::close() {
848 result = ::close( lock_fd );
849 if ( result == -1 ) {
850 SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
852 snprintf( msg, 256, "Error closing %s", lock_file );
857 result = ::close( analog_in_fd );
858 if ( result == -1 ) {
859 SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
861 snprintf( msg, 256, "Error closing %s", analog_in_file );
866 result = ::close( radios_fd );
867 if ( result == -1 ) {
868 SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
870 snprintf( msg, 256, "Error closing %s", radios_file );
875 result = ::close( switches_fd );
876 if ( result == -1 ) {
877 SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
879 snprintf( msg, 256, "Error closing %s", switches_file );