src/Network/atc610x.cxx

   1 // atc610x.cxx -- FGFS interface to ATC 610x hardware
   2 //
   3 // Written by Curtis Olson, started January 2002
   4 //
   5 // Copyright (C) 2002  Curtis L. Olson - curt@flightgear.org
   6 //
   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.
  11 //
  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.
  16 //
  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.
  20 //
  21 // $Id$
  22
  23
  24 #ifdef HAVE_CONFIG_H
  25 #  include <config.h>
  26 #endif
  27
  28 #include <simgear/compiler.h>
  29
  30 #include <stdlib.h>             // atoi() atof() abs()
  31 #include <sys/types.h>
  32 #include <sys/stat.h>
  33 #include <fcntl.h>
  34 #include <stdio.h>              //snprintf
  35 #if defined( _MSC_VER ) || defined(__MINGW32__)
  36 #  include <io.h>                 //lseek, read, write
  37 #endif
  38
  39 #include STL_STRING
  40
  41 #include <plib/ul.h>
  42
  43 #include <simgear/debug/logstream.hxx>
  44 #include <simgear/io/iochannel.hxx>
  45 #include <simgear/math/sg_types.hxx>
  46 #include <simgear/misc/props.hxx>
  47 #include <simgear/misc/sg_path.hxx>
  48
  49 #include <Main/fg_props.hxx>
  50 #include <Main/globals.hxx>
  51
  52 #include "atc610x.hxx"
  53
  54 SG_USING_STD(string);
  55
  56
  57 // Lock the ATC 610 hardware
  58 static int ATC610xLock( int fd ) {
  59     // rewind
  60     lseek( fd, 0, SEEK_SET );
  61
  62     char tmp[2];
  63     int result = read( fd, tmp, 1 );
  64     if ( result != 1 ) {
  65         SG_LOG( SG_IO, SG_DEBUG, "Lock failed" );
  66     }
  67
  68     return result;
  69 }
  70
  71
  72 // Write a radios command
  73 static int ATC610xRelease( int fd ) {
  74     // rewind
  75     lseek( fd, 0, SEEK_SET );
  76
  77     char tmp[2];
  78     tmp[0] = tmp[1] = 0;
  79     int result = write( fd, tmp, 1 );
  80
  81     if ( result != 1 ) {
  82         SG_LOG( SG_IO, SG_DEBUG, "Release failed" );
  83     }
  84
  85     return result;
  86 }
  87
  88
  89 // Read analog inputs
  90 static void ATC610xReadAnalogInputs( int fd, unsigned char *analog_in_bytes ) {
  91     // rewind
  92     lseek( fd, 0, SEEK_SET );
  93
  94     int result = read( fd, analog_in_bytes, ATC_ANAL_IN_BYTES );
  95     if ( result != ATC_ANAL_IN_BYTES ) {
  96         SG_LOG( SG_IO, SG_ALERT, "Read failed" );
  97         exit( -1 );
  98     }
  99 }
 100
 101
 102 // Write a radios command
 103 static int ATC610xSetRadios( int fd,
 104                              unsigned char data[ATC_RADIO_DISPLAY_BYTES] )
 105 {
 106     // rewind
 107     lseek( fd, 0, SEEK_SET );
 108
 109     int result = write( fd, data, ATC_RADIO_DISPLAY_BYTES );
 110
 111     if ( result != ATC_RADIO_DISPLAY_BYTES ) {
 112         SG_LOG( SG_IO, SG_DEBUG, "Write failed" );
 113     }
 114
 115     return result;
 116 }
 117
 118
 119 // Read status of last radios written to
 120 static void ATC610xReadRadios( int fd, unsigned char *switch_data ) {
 121     // rewind
 122     lseek( fd, 0, SEEK_SET );
 123
 124     int result = read( fd, switch_data, ATC_RADIO_SWITCH_BYTES );
 125     if ( result != ATC_RADIO_SWITCH_BYTES ) {
 126         SG_LOG( SG_IO, SG_ALERT, "Read failed" );
 127         exit( -1 );
 128     }
 129 }
 130
 131 // Write a stepper command
 132 static int ATC610xSetStepper( int fd, unsigned char channel,
 133                               unsigned char value )
 134 {
 135     // rewind
 136     lseek( fd, 0, SEEK_SET );
 137
 138     // Write the value
 139     unsigned char buf[3];
 140     buf[0] = channel;
 141     buf[1] = value;
 142     buf[2] = 0;
 143     int result = write( fd, buf, 2 );
 144     if ( result != 2 ) {
 145         SG_LOG( SG_IO, SG_INFO, "Write failed" );
 146     }
 147     SG_LOG( SG_IO, SG_DEBUG,
 148             "Sent cmd = " << (int)channel << " value = " << (int)value );
 149     return result;
 150 }
 151
 152
 153 // Read status of last stepper written to
 154 static unsigned char ATC610xReadStepper( int fd ) {
 155     int result;
 156
 157     // rewind
 158     lseek( fd, 0, SEEK_SET );
 159
 160     // Write the value
 161     unsigned char buf[2];
 162     result = read( fd, buf, 1 );
 163     if ( result != 1 ) {
 164         SG_LOG( SG_IO, SG_ALERT, "Read failed" );
 165         exit( -1 );
 166     }
 167     SG_LOG( SG_IO, SG_DEBUG, "Read result = " << (int)buf[0] );
 168
 169     return buf[0];
 170 }
 171
 172
 173 // Read switch inputs
 174 static void ATC610xReadSwitches( int fd, unsigned char *switch_bytes ) {
 175     // rewind
 176     lseek( fd, 0, SEEK_SET );
 177
 178     int result = read( fd, switch_bytes, ATC_SWITCH_BYTES );
 179     if ( result != ATC_SWITCH_BYTES ) {
 180         SG_LOG( SG_IO, SG_ALERT, "Read failed" );
 181         exit( -1 );
 182     }
 183 }
 184
 185
 186 // Turn a lamp on or off
 187 void ATC610xSetLamp( int fd, int channel, bool value ) {
 188     // lamp channels 0-63 are written to LampPort0, channels 64-127
 189     // are written to LampPort1
 190
 191     // bits 0-6 are the lamp address
 192     // bit 7 is the value (on/off)
 193
 194     int result;
 195
 196     // Write the value
 197     unsigned char buf[3];
 198     buf[0] = channel;
 199     buf[1] = value;
 200     buf[2] = 0;
 201     result = write( fd, buf, 2 );
 202     if ( result != 2 ) {
 203         SG_LOG( SG_IO, SG_ALERT,  "Write failed" );
 204         exit( -1 );
 205     }
 206 }
 207
 208
 209 void FGATC610x::init_config() {
 210 #if defined( unix ) || defined( __CYGWIN__ )
 211     // Next check home directory for .fgfsrc.hostname file
 212     char *envp = ::getenv( "HOME" );
 213     if ( envp != NULL ) {
 214         SGPath atc610x_config( envp );
 215         atc610x_config.append( ".fgfs-atc610x.xml" );
 216         readProperties( atc610x_config.str(), globals->get_props() );
 217     }
 218 #endif
 219 }
 220
 221
 222 // Open and initialize ATC 610x hardware
 223 bool FGATC610x::open() {
 224     if ( is_enabled() ) {
 225         SG_LOG( SG_IO, SG_ALERT, "This shouldn't happen, but the channel "
 226                 << "is already in use, ignoring" );
 227         return false;
 228     }
 229
 230     // This loads the config parameters generated by "simcal"
 231     init_config();
 232
 233     SG_LOG( SG_IO, SG_ALERT,
 234             "Initializing ATC 610x hardware, please wait ..." );
 235
 236     set_hz( 30 );               // default to processing requests @ 30Hz
 237     set_enabled( true );
 238
 239     board = 0;                  // 610x uses a single board number = 0
 240
 241     snprintf( lock_file, 256, "/proc/atc610x/board%d/lock", board );
 242     snprintf( analog_in_file, 256, "/proc/atc610x/board%d/analog_in", board );
 243     snprintf( lamps_file, 256, "/proc/atc610x/board%d/lamps", board );
 244     snprintf( radios_file, 256, "/proc/atc610x/board%d/radios", board );
 245     snprintf( stepper_file, 256, "/proc/atc610x/board%d/steppers", board );
 246     snprintf( switches_file, 256, "/proc/atc610x/board%d/switches", board );
 247
 248     /////////////////////////////////////////////////////////////////////
 249     // Open the /proc files
 250     /////////////////////////////////////////////////////////////////////
 251
 252     lock_fd = ::open( lock_file, O_RDWR );
 253     if ( lock_fd == -1 ) {
 254         SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
 255         char msg[256];
 256         snprintf( msg, 256, "Error opening %s", lock_file );
 257         perror( msg );
 258         exit( -1 );
 259     }
 260
 261     analog_in_fd = ::open( analog_in_file, O_RDONLY );
 262     if ( analog_in_fd == -1 ) {
 263         SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
 264         char msg[256];
 265         snprintf( msg, 256, "Error opening %s", analog_in_file );
 266         perror( msg );
 267         exit( -1 );
 268     }
 269
 270     lamps_fd = ::open( lamps_file, O_WRONLY );
 271     if ( lamps_fd == -1 ) {
 272         SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
 273         char msg[256];
 274         snprintf( msg, 256, "Error opening %s", lamps_file );
 275         perror( msg );
 276         exit( -1 );
 277     }
 278
 279     radios_fd = ::open( radios_file, O_RDWR );
 280     if ( radios_fd == -1 ) {
 281         SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
 282         char msg[256];
 283         snprintf( msg, 256, "Error opening %s", radios_file );
 284         perror( msg );
 285         exit( -1 );
 286     }
 287
 288     stepper_fd = ::open( stepper_file, O_RDWR );
 289     if ( stepper_fd == -1 ) {
 290         SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
 291         char msg[256];
 292         snprintf( msg, 256, "Error opening %s", stepper_file );
 293         perror( msg );
 294         exit( -1 );
 295     }
 296
 297     switches_fd = ::open( switches_file, O_RDONLY );
 298     if ( switches_fd == -1 ) {
 299         SG_LOG( SG_IO, SG_ALERT, "errno = " << errno );
 300         char msg[256];
 301         snprintf( msg, 256, "Error opening %s", switches_file );
 302         perror( msg );
 303         exit( -1 );
 304     }
 305
 306     /////////////////////////////////////////////////////////////////////
 307     // Home the compass stepper motor
 308     /////////////////////////////////////////////////////////////////////
 309
 310     SG_LOG( SG_IO, SG_ALERT,
 311             "  - Homing the compass stepper motor" );
 312
 313     // Lock the hardware, keep trying until we succeed
 314     while ( ATC610xLock( lock_fd ) <= 0 );
 315
 316     // Send the stepper home command
 317     ATC610xSetStepper( stepper_fd, ATC_COMPASS_CH, ATC_STEPPER_HOME );
 318
 319     // Release the hardware
 320     ATC610xRelease( lock_fd );
 321
 322     SG_LOG( SG_IO, SG_ALERT,
 323             "  - Waiting for compass to come home." );
 324
 325     bool home = false;
 326     int timeout = 900;          // about 30 seconds
 327     timeout = 0;
 328     while ( ! home && timeout > 0 ) {
 329         if ( timeout % 150 == 0 ) {
 330             SG_LOG( SG_IO, SG_INFO, "waiting for compass = " << timeout );
 331         } else {
 332             SG_LOG( SG_IO, SG_DEBUG, "Checking if compass home ..." );
 333         }
 334
 335         while ( ATC610xLock( lock_fd ) <= 0 );
 336
 337         unsigned char result = ATC610xReadStepper( stepper_fd );
 338         if ( result == 0 ) {
 339             home = true;
 340         }
 341
 342         ATC610xRelease( lock_fd );
 343
 344 #if defined( _MSC_VER )
 345         ulMilliSecondSleep(33);
 346 #elif defined (WIN32) && !defined(__CYGWIN__)
 347         Sleep (33);
 348 #else
 349         usleep(33);
 350 #endif
 351
 352         --timeout;
 353     }
 354
 355     compass_position = 0.0;
 356
 357     /////////////////////////////////////////////////////////////////////
 358     // Blank the radio display
 359     /////////////////////////////////////////////////////////////////////
 360
 361     SG_LOG( SG_IO, SG_ALERT,
 362             "  - Clearing the radios displays." );
 363
 364     // Prepair the data
 365     unsigned char value = 0xff;
 366     for ( int channel = 0; channel < ATC_RADIO_DISPLAY_BYTES; ++channel ) {
 367         radio_display_data[channel] = value;
 368     }
 369
 370     // Lock the hardware, keep trying until we succeed
 371     while ( ATC610xLock( lock_fd ) <= 0 );
 372
 373     // Set radio display
 374     ATC610xSetRadios( radios_fd, radio_display_data );
 375
 376     ATC610xRelease( lock_fd );
 377
 378     /////////////////////////////////////////////////////////////////////
 379     // Blank the lamps
 380     /////////////////////////////////////////////////////////////////////
 381
 382     for ( int i = 0; i < 128; ++i ) {
 383         ATC610xSetLamp( lamps_fd, i, false );
 384     }
 385
 386     /////////////////////////////////////////////////////////////////////
 387     // Finished initing hardware
 388     /////////////////////////////////////////////////////////////////////
 389
 390     SG_LOG( SG_IO, SG_ALERT,
 391             "Done initializing ATC 610x hardware." );
 392
 393     /////////////////////////////////////////////////////////////////////
 394     // Connect up to property values
 395     /////////////////////////////////////////////////////////////////////
 396
 397     mag_compass = fgGetNode( "/instrumentation/magnetic-compass/indicated-heading-deg", true );
 398
 399     dme_min = fgGetNode( "/radios/dme/ete-min", true );
 400     dme_kt = fgGetNode( "/radios/dme/speed-kt", true );
 401     dme_nm = fgGetNode( "/radios/dme/distance-nm", true );
 402
 403     adf_bus_power = fgGetNode( "/systems/electrical/outputs/adf", true );
 404     dme_bus_power = fgGetNode( "/systems/electrical/outputs/dme", true );
 405     navcom1_bus_power = fgGetNode( "/systems/electrical/outputs/navcom[0]",
 406                                    true );
 407     navcom2_bus_power = fgGetNode( "/systems/electrical/outputs/navcom[1]",
 408                                    true );
 409     xpdr_bus_power = fgGetNode( "/systems/electrical/outputs/transponder",
 410                                  true );
 411
 412     navcom1_power_btn = fgGetNode( "/radios/comm[0]/inputs/power-btn", true );
 413     navcom2_power_btn = fgGetNode( "/radios/comm[1]/inputs/power-btn", true );
 414
 415     com1_freq = fgGetNode( "/radios/comm[0]/frequencies/selected-mhz", true );
 416     com1_stby_freq
 417         = fgGetNode( "/radios/comm[0]/frequencies/standby-mhz", true );
 418
 419     com2_freq = fgGetNode( "/radios/comm[1]/frequencies/selected-mhz", true );
 420     com2_stby_freq
 421         = fgGetNode( "/radios/comm[1]/frequencies/standby-mhz", true );
 422
 423     nav1_freq = fgGetNode( "/radios/nav[0]/frequencies/selected-mhz", true );
 424     nav1_stby_freq
 425         = fgGetNode( "/radios/nav[0]/frequencies/standby-mhz", true );
 426     nav1_obs = fgGetNode( "/radios/nav[0]/radials/selected-deg", true );
 427
 428     nav2_freq = fgGetNode( "/radios/nav[1]/frequencies/selected-mhz", true );
 429     nav2_stby_freq
 430         = fgGetNode( "/radios/nav[1]/frequencies/standby-mhz", true );
 431     nav2_obs = fgGetNode( "/radios/nav[1]/radials/selected-deg", true );
 432
 433     adf_power_btn = fgGetNode( "/radios/kr-87/inputs/power-btn", true );
 434     adf_vol = fgGetNode( "/radios/kr-87/inputs/volume", true );
 435     adf_adf_btn = fgGetNode( "/radios/kr-87/inputs/adf-btn", true );
 436     adf_bfo_btn = fgGetNode( "/radios/kr-87/inputs/bfo-btn", true );
 437     adf_freq = fgGetNode( "/radios/kr-87/outputs/selected-khz", true );
 438     adf_stby_freq = fgGetNode( "/radios/kr-87/outputs/standby-khz", true );
 439     adf_stby_mode = fgGetNode( "/radios/kr-87/modes/stby", true );
 440     adf_timer_mode = fgGetNode( "/radios/kr-87/modes/timer", true );
 441     adf_count_mode = fgGetNode( "/radios/kr-87/modes/count", true );
 442     adf_flight_timer = fgGetNode( "/radios/kr-87/outputs/flight-timer", true );
 443     adf_elapsed_timer = fgGetNode( "/radios/kr-87/outputs/elapsed-timer",
 444                                    true );
 445     adf_ant_ann = fgGetNode( "/radios/kr-87/annunciators/ant", true );
 446     adf_adf_ann = fgGetNode( "/radios/kr-87/annunciators/adf", true );
 447     adf_bfo_ann = fgGetNode( "/radios/kr-87/annunciators/bfo", true );
 448     adf_frq_ann = fgGetNode( "/radios/kr-87/annunciators/frq", true );
 449     adf_flt_ann = fgGetNode( "/radios/kr-87/annunciators/flt", true );
 450     adf_et_ann = fgGetNode( "/radios/kr-87/annunciators/et", true );
 451
 452     inner = fgGetNode( "/radios/marker-beacon/inner", true );
 453     middle = fgGetNode( "/radios/marker-beacon/middle", true );
 454     outer = fgGetNode( "/radios/marker-beacon/outer", true );
 455
 456     xpdr_ident_btn = fgGetNode( "/radios/kt-70/inputs/ident-btn", true );
 457     xpdr_digit1 = fgGetNode( "/radios/kt-70/inputs/digit1", true );
 458     xpdr_digit2 = fgGetNode( "/radios/kt-70/inputs/digit2", true );
 459     xpdr_digit3 = fgGetNode( "/radios/kt-70/inputs/digit3", true );
 460     xpdr_digit4 = fgGetNode( "/radios/kt-70/inputs/digit4", true );
 461     xpdr_func_knob = fgGetNode( "/radios/kt-70/inputs/func-knob", true );
 462     xpdr_id_code = fgGetNode( "/radios/kt-70/outputs/id-code", true );
 463     xpdr_flight_level = fgGetNode( "/radios/kt-70/outputs/flight-level", true );
 464     xpdr_fl_ann = fgGetNode( "/radios/kt-70/annunciators/fl", true );
 465     xpdr_alt_ann = fgGetNode( "/radios/kt-70/annunciators/alt", true );
 466     xpdr_gnd_ann = fgGetNode( "/radios/kt-70/annunciators/gnd", true );
 467     xpdr_on_ann = fgGetNode( "/radios/kt-70/annunciators/on", true );
 468     xpdr_sby_ann = fgGetNode( "/radios/kt-70/annunciators/sby", true );
 469     xpdr_reply_ann = fgGetNode( "/radios/kt-70/annunciators/reply", true );
 470
 471     ati_bird
 472       = fgGetNode( "/instrumentation/attitude-indicator/horizon-offset-deg",
 473                    true );
 474     alt_press = fgGetNode( "/instrumentation/altimeter/setting-inhg", true );
 475     adf_hdg = fgGetNode( "/radios/kr-87/inputs/rotation-deg", true );
 476
 477     elevator_center = fgGetNode( "/input/atc610x/elevator/center", true );
 478     elevator_min = fgGetNode( "/input/atc610x/elevator/min", true );
 479     elevator_max = fgGetNode( "/input/atc610x/elevator/max", true );
 480
 481     ailerons_center = fgGetNode( "/input/atc610x/ailerons/center", true );
 482     ailerons_min = fgGetNode( "/input/atc610x/ailerons/min", true );
 483     ailerons_max = fgGetNode( "/input/atc610x/ailerons/max", true );
 484
 485     rudder_center = fgGetNode( "/input/atc610x/rudder/center", true );
 486     rudder_min = fgGetNode( "/input/atc610x/rudder/min", true );
 487     rudder_max = fgGetNode( "/input/atc610x/rudder/max", true );
 488
 489     brake_left_min = fgGetNode( "/input/atc610x/brake-left/min", true );
 490     brake_left_max = fgGetNode( "/input/atc610x/brake-left/max", true );
 491
 492     brake_right_min = fgGetNode( "/input/atc610x/brake-right/min", true );
 493     brake_right_max = fgGetNode( "/input/atc610x/brake-right/max", true );
 494
 495     throttle_min = fgGetNode( "/input/atc610x/throttle/min", true );
 496     throttle_max = fgGetNode( "/input/atc610x/throttle/max", true );
 497
 498     mixture_min = fgGetNode( "/input/atc610x/mixture/min", true );
 499     mixture_max = fgGetNode( "/input/atc610x/mixture/max", true );
 500
 501     trim_center = fgGetNode( "/input/atc610x/trim/center", true );
 502     trim_min = fgGetNode( "/input/atc610x/trim/min", true );
 503     trim_max = fgGetNode( "/input/atc610x/trim/max", true );
 504
 505     nav1vol_min = fgGetNode( "/input/atc610x/nav1vol/min", true );
 506     nav1vol_max = fgGetNode( "/input/atc610x/nav1vol/max", true );
 507
 508     nav2vol_min = fgGetNode( "/input/atc610x/nav2vol/min", true );
 509     nav2vol_max = fgGetNode( "/input/atc610x/nav2vol/max", true );
 510
 511     comm1_servicable = fgGetNode( "/instrumentation/comm[0]/servicable", true );
 512     comm2_servicable = fgGetNode( "/instrumentation/comm[1]/servicable", true );
 513     nav1_servicable = fgGetNode( "/instrumentation/nav[0]/servicable", true );
 514     nav2_servicable = fgGetNode( "/instrumentation/nav[1]/servicable", true );
 515     adf_servicable = fgGetNode( "/instrumentation/adf/servicable", true );
 516     xpdr_servicable = fgGetNode( "/instrumentation/transponder/servicable",
 517                                  true );
 518     dme_servicable = fgGetNode( "/instrumentation/dme/servicable", true );
 519
 520     // default to having everything servicable
 521     comm1_servicable->setBoolValue( true );
 522     comm2_servicable->setBoolValue( true );
 523     nav1_servicable->setBoolValue( true );
 524     nav2_servicable->setBoolValue( true );
 525     adf_servicable->setBoolValue( true );
 526     xpdr_servicable->setBoolValue( true );
 527     dme_servicable->setBoolValue( true );
 528
 529     return true;
 530 }
 531
 532
 533 /////////////////////////////////////////////////////////////////////
 534 // Read analog inputs
 535 /////////////////////////////////////////////////////////////////////
 536
 537 // scale a number between min and max (with center defined) to a scale
 538 // from -1.0 to 1.0
 539 static double scale( int center, int min, int max, int value ) {
 540     // cout << center << " " << min << " " << max << " " << value << " ";
 541     double result;
 542     double range;
 543
 544     if ( value <= center ) {
 545         range = center - min;
 546         result = (value - center) / range;
 547     } else {
 548         range = max - center;
 549         result = (value - center) / range;
 550     }
 551
 552     if ( result < -1.0 ) result = -1.0;
 553     if ( result > 1.0 ) result = 1.0;
 554
 555     // cout << result << endl;
 556
 557     return result;
 558 }
 559
 560
 561 // scale a number between min and max to a scale from 0.0 to 1.0
 562 static double scale( int min, int max, int value ) {
 563     // cout << center << " " << min << " " << max << " " << value << " ";
 564     double result;
 565     double range;
 566
 567     range = max - min;
 568     result = (value - min) / range;
 569
 570     if ( result < 0.0 ) result = 0.0;
 571     if ( result > 1.0 ) result = 1.0;
 572
 573     // cout << result << endl;
 574
 575     return result;
 576 }
 577
 578
 579 static int tony_magic( int raw, int obs[3] ) {
 580     int result = 0;
 581
 582     obs[0] = raw;
 583
 584     if ( obs[1] < 30 ) {
 585         if ( obs[2] >= 68 && obs[2] < 480 ) {
 586             result = -6;
 587         } else if ( obs[2] >= 480 ) {
 588             result = 6;
 589         }
 590         obs[2] = obs[1];
 591         obs[1] = obs[0];
 592     } else if ( obs[1] < 68 ) {
 593         // do nothing
 594         obs[1] = obs[0];
 595     } else if ( obs[2] < 30 ) {
 596         if ( obs[1] >= 68 && obs[1] < 480 ) {
 597             result = 6;
 598             obs[2] = obs[1];
 599             obs[1] = obs[0];
 600         } else if ( obs[1] >= 480 ) {
 601             result = -6;
 602             if ( obs[0] < obs[1] ) {
 603                 obs[2] = obs[1];
 604                 obs[1] = obs[0];
 605             } else {
 606                 obs[2] = obs[0];
 607                 obs[1] = obs[0];
 608             }
 609         }
 610     } else if ( obs[1] > 980 ) {
 611         if ( obs[2] <= 956 && obs[2] > 480 ) {
 612             result = 6;
 613         } else if ( obs[2] <= 480 ) {
 614             result = -6;
 615         }
 616         obs[2] = obs[1];
 617         obs[1] = obs[0];
 618     } else if ( obs[1] > 956 ) {
 619         // do nothing
 620         obs[1] = obs[0];
 621     } else if ( obs[2] > 980 ) {
 622         if ( obs[1] <= 956 && obs[1] > 480 ) {
 623             result = -6;
 624             obs[2] = obs[1];
 625             obs[1] = obs[0];
 626         } else if ( obs[1] <= 480 ) {
 627             result = 6;
 628             if ( obs[0] > obs[1] ) {
 629                 obs[2] = obs[1];
 630                 obs[1] = obs[0];
 631             } else {
 632                 obs[2] = obs[0];
 633                 obs[1] = obs[0];
 634             }
 635         }
 636     } else {
 637         if ( obs[1] < 480 && obs[2] > 480 ) {
 638             // crossed gap going up
 639             if ( obs[0] < obs[1] ) {
 640                 // caught a bogus intermediate value coming out of the gap
 641                 obs[1] = obs[0];
 642             }
 643         } else if ( obs[1] > 480 && obs[2] < 480 ) {
 644             // crossed gap going down
 645             if ( obs[0] > obs[1] ) {
 646                 // caught a bogus intermediate value coming out of the gap
 647               obs[1] = obs[0];
 648             }
 649         } else if ( obs[0] > 480 && obs[1] < 480 && obs[2] < 480 ) {
 650             // crossed the gap going down
 651             if ( obs[1] > obs[2] ) {
 652                 // caught a bogus intermediate value coming out of the gap
 653                 obs[1] = obs[2];
 654             }
 655         } else if ( obs[0] < 480 && obs[1] > 480 && obs[2] > 480 ) {
 656             // crossed the gap going up
 657             if ( obs[1] < obs[2] ) {
 658                 // caught a bogus intermediate value coming out of the gap
 659                 obs[1] = obs[2];
 660             }
 661         }
 662         result = obs[1] - obs[2];
 663         if ( abs(result) > 200 ) {
 664             // ignore
 665             result = 0;
 666         }
 667         obs[2] = obs[1];
 668         obs[1] = obs[0];
 669     }
 670
 671     // cout << " result = " << result << endl;
 672     if ( result < -500 ) { result += 1024; }
 673     if ( result > 500 ) { result -= 1024; }
 674
 675     return result;
 676 }
 677
 678
 679 static double instr_pot_filter( double ave, double val ) {
 680     if ( fabs(ave - val) < 200 || fabs(val) < fabs(ave) ) {
 681         return 0.66 * ave + 0.34 * val;
 682     } else {
 683         return ave;
 684     }
 685 }
 686
 687
 688 bool FGATC610x::do_analog_in() {
 689     // Read raw data in byte form
 690     ATC610xReadAnalogInputs( analog_in_fd, analog_in_bytes );
 691
 692     // Convert to integer values
 693     for ( int channel = 0; channel < ATC_ANAL_IN_VALUES; ++channel ) {
 694         unsigned char hi = analog_in_bytes[2 * channel] & 0x03;
 695         unsigned char lo = analog_in_bytes[2 * channel + 1];
 696         analog_in_data[channel] = hi * 256 + lo;
 697
 698         // printf("%02x %02x ", hi, lo );
 699         // printf("%04d ", value );
 700     }
 701
 702     float tmp;
 703
 704     // aileron
 705     tmp = scale( ailerons_center->getIntValue(), ailerons_min->getIntValue(),
 706                  ailerons_max->getIntValue(), analog_in_data[0] );
 707     fgSetFloat( "/controls/aileron", tmp );
 708     // cout << "aileron = " << analog_in_data[0] << " = " << tmp;
 709     // elevator
 710     tmp = -scale( elevator_center->getIntValue(), elevator_min->getIntValue(),
 711                   elevator_max->getIntValue(), analog_in_data[5] );
 712     fgSetFloat( "/controls/elevator", tmp );
 713     // cout << "trim = " << analog_in_data[4] << " = " << tmp;
 714
 715     // elevator trim
 716     tmp = scale( trim_center->getIntValue(), trim_min->getIntValue(),
 717                  trim_max->getIntValue(), analog_in_data[4] );
 718     fgSetFloat( "/controls/elevator-trim", tmp );
 719     // cout << " elev = " << analog_in_data[5] << " = " << tmp << endl;
 720
 721     // mixture
 722     tmp = scale( mixture_min->getIntValue(), mixture_max->getIntValue(),
 723                  analog_in_data[6] );
 724     fgSetFloat( "/controls/mixture[0]", tmp );
 725     fgSetFloat( "/controls/mixture[1]", tmp );
 726
 727     // throttle
 728     tmp = scale( throttle_min->getIntValue(), throttle_max->getIntValue(),
 729                  analog_in_data[8] );
 730     fgSetFloat( "/controls/throttle[0]", tmp );
 731     fgSetFloat( "/controls/throttle[1]", tmp );
 732     // cout << "throttle = " << tmp << endl;
 733
 734     // rudder
 735     tmp = scale( rudder_center->getIntValue(), rudder_min->getIntValue(),
 736                  rudder_max->getIntValue(), analog_in_data[10] );
 737     fgSetFloat( "/controls/rudder", -tmp );
 738
 739     // toe brakes
 740     tmp = scale( brake_left_min->getIntValue(), brake_left_max->getIntValue(),
 741                  analog_in_data[20] );
 742     fgSetFloat( "/controls/brakes[0]", tmp );
 743     tmp = scale( brake_right_min->getIntValue(), brake_right_max->getIntValue(),
 744                  analog_in_data[21] );
 745     fgSetFloat( "/controls/brakes[1]", tmp );
 746
 747     // nav1 volume
 748     tmp = (float)analog_in_data[25] / 1024.0f;
 749     fgSetFloat( "/radios/nav[0]/volume", tmp );
 750
 751     // nav2 volume
 752     tmp = (float)analog_in_data[24] / 1024.0f;
 753     fgSetFloat( "/radios/nav[1]/volume", tmp );
 754
 755     // adf volume
 756     tmp = (float)analog_in_data[26] / 1024.0f;
 757     fgSetFloat( "/radios/kr-87/inputs/volume", tmp );
 758
 759     // instrument panel pots
 760     static bool first = true;
 761     static int obs1[3], obs2[3], obs3[3], obs4[3], obs5[3];
 762     static double diff1_ave = 0.0;
 763     static double diff2_ave = 0.0;
 764     static double diff3_ave = 0.0;
 765     static double diff4_ave = 0.0;
 766     static double diff5_ave = 0.0;
 767
 768     if ( first ) {
 769         first = false;
 770         obs1[0] = obs1[1] = obs1[2] = analog_in_data[11];
 771         obs2[0] = obs2[1] = obs2[2] = analog_in_data[28];
 772         obs3[0] = obs3[1] = obs3[2] = analog_in_data[29];
 773         obs4[0] = obs4[1] = obs4[2] = analog_in_data[30];
 774         obs5[0] = obs5[1] = obs5[2] = analog_in_data[31];
 775     }
 776
 777     int diff1 = tony_magic( analog_in_data[11], obs1 );
 778     int diff2 = tony_magic( analog_in_data[28], obs2 );
 779     int diff3 = tony_magic( analog_in_data[39], obs3 );
 780     int diff4 = tony_magic( analog_in_data[30], obs4 );
 781     int diff5 = tony_magic( analog_in_data[31], obs5 );
 782
 783     diff1_ave = instr_pot_filter( diff1_ave, diff1 );
 784     diff2_ave = instr_pot_filter( diff2_ave, diff2 );
 785     diff3_ave = instr_pot_filter( diff3_ave, diff3 );
 786     diff4_ave = instr_pot_filter( diff4_ave, diff4 );
 787     diff5_ave = instr_pot_filter( diff5_ave, diff5 );
 788
 789     tmp = ati_bird->getDoubleValue() + (diff1_ave * (20.0/880.0) );
 790     if ( tmp < -10.0 ) { tmp = -10.0; }
 791     if ( tmp > 10.0 ) { tmp = 10.0; }
 792     fgSetFloat( "/instrumentation/attitude-indicator/horizon-offset-deg", tmp );
 793
 794     tmp = alt_press->getDoubleValue() + (diff2_ave * (1.125/880.0) );
 795     if ( tmp < 27.9 ) { tmp = 27.9; }
 796     if ( tmp > 31.4 ) { tmp = 31.4; }
 797     fgSetFloat( "/instrumentation/altimeter/setting-inhg", tmp );
 798
 799     tmp = nav1_obs->getDoubleValue() + (diff3_ave * (72.0/880.0) );
 800     while ( tmp >= 360.0 ) { tmp -= 360.0; }
 801     while ( tmp < 0.0 ) { tmp += 360.0; }
 802     // cout << " obs = " << tmp << endl;
 803     fgSetFloat( "/radios/nav[0]/radials/selected-deg", tmp );
 804
 805     tmp = nav2_obs->getDoubleValue() + (diff4_ave * (72.0/880.0) );
 806     while ( tmp >= 360.0 ) { tmp -= 360.0; }
 807     while ( tmp < 0.0 ) { tmp += 360.0; }
 808     // cout << " obs = " << tmp << endl;
 809     fgSetFloat( "/radios/nav[1]/radials/selected-deg", tmp );
 810
 811     tmp = adf_hdg->getDoubleValue() + (diff5_ave * (72.0/880.0) );
 812     while ( tmp >= 360.0 ) { tmp -= 360.0; }
 813     while ( tmp < 0.0 ) { tmp += 360.0; }
 814     // cout << " obs = " << tmp << endl;
 815     fgSetFloat( "/radios/kr-87/inputs/rotation-deg", tmp );
 816
 817     return true;
 818 }
 819
 820
 821 /////////////////////////////////////////////////////////////////////
 822 // Write the lights
 823 /////////////////////////////////////////////////////////////////////
 824
 825 bool FGATC610x::do_lights() {
 826
 827     // Marker beacons
 828     ATC610xSetLamp( lamps_fd, 4, inner->getBoolValue() );
 829     ATC610xSetLamp( lamps_fd, 5, middle->getBoolValue() );
 830     ATC610xSetLamp( lamps_fd, 3, outer->getBoolValue() );
 831
 832     // ADF annunciators
 833     ATC610xSetLamp( lamps_fd, 11, adf_ant_ann->getBoolValue() ); // ANT
 834     ATC610xSetLamp( lamps_fd, 12, adf_adf_ann->getBoolValue() ); // ADF
 835     ATC610xSetLamp( lamps_fd, 13, adf_bfo_ann->getBoolValue() ); // BFO
 836     ATC610xSetLamp( lamps_fd, 14, adf_frq_ann->getBoolValue() ); // FRQ
 837     ATC610xSetLamp( lamps_fd, 15, adf_flt_ann->getBoolValue() ); // FLT
 838     ATC610xSetLamp( lamps_fd, 16, adf_et_ann->getBoolValue() ); // ET
 839
 840     // Transponder annunciators
 841     ATC610xSetLamp( lamps_fd, 17, xpdr_fl_ann->getBoolValue() ); // FL
 842     ATC610xSetLamp( lamps_fd, 18, xpdr_alt_ann->getBoolValue() ); // ALT
 843     ATC610xSetLamp( lamps_fd, 19, xpdr_gnd_ann->getBoolValue() ); // GND
 844     ATC610xSetLamp( lamps_fd, 20, xpdr_on_ann->getBoolValue() ); // ON
 845     ATC610xSetLamp( lamps_fd, 21, xpdr_sby_ann->getBoolValue() ); // SBY
 846     ATC610xSetLamp( lamps_fd, 22, xpdr_reply_ann->getBoolValue() ); // R
 847
 848     return true;
 849 }
 850
 851
 852 /////////////////////////////////////////////////////////////////////
 853 // Read radio switches
 854 /////////////////////////////////////////////////////////////////////
 855
 856 bool FGATC610x::do_radio_switches() {
 857     double freq, coarse_freq, fine_freq, value;
 858     int diff;
 859
 860     ATC610xReadRadios( radios_fd, radio_switch_data );
 861
 862     // DME Switch
 863     dme_switch = (radio_switch_data[7] >> 4) & 0x03;
 864     if ( dme_switch == 0 ) {
 865         // off
 866         fgSetInt( "/radios/dme/switch-position", 0 );
 867     } else if ( dme_switch == 2 ) {
 868         // nav1
 869         fgSetInt( "/radios/dme/switch-position", 1 );
 870     } else if ( dme_switch == 1 ) {
 871         // nav2
 872         fgSetInt( "/radios/dme/switch-position", 3 );
 873     }
 874
 875     // NavCom1 Power
 876     fgSetBool( "/radios/comm[0]/inputs/power-btn",
 877                radio_switch_data[7] & 0x01 );
 878
 879     if ( navcom1_has_power() && comm1_servicable->getBoolValue() ) {
 880         // Com1 Swap
 881         int com1_swap = !((radio_switch_data[7] >> 1) & 0x01);
 882         static int last_com1_swap;
 883         if ( com1_swap && (last_com1_swap != com1_swap) ) {
 884             float tmp = com1_freq->getFloatValue();
 885             fgSetFloat( "/radios/comm[0]/frequencies/selected-mhz",
 886                         com1_stby_freq->getFloatValue() );
 887             fgSetFloat( "/radios/comm[0]/frequencies/standby-mhz", tmp );
 888         }
 889         last_com1_swap = com1_swap;
 890     }
 891
 892     // NavCom2 Power
 893     fgSetBool( "/radios/comm[1]/inputs/power-btn",
 894                radio_switch_data[15] & 0x01 );
 895
 896     if ( navcom2_has_power() && comm2_servicable->getBoolValue() ) {
 897         // Com2 Swap
 898         int com2_swap = !((radio_switch_data[15] >> 1) & 0x01);
 899         static int last_com2_swap;
 900         if ( com2_swap && (last_com2_swap != com2_swap) ) {
 901             float tmp = com2_freq->getFloatValue();
 902             fgSetFloat( "/radios/comm[1]/frequencies/selected-mhz",
 903                         com2_stby_freq->getFloatValue() );
 904             fgSetFloat( "/radios/comm[1]/frequencies/standby-mhz", tmp );
 905         }
 906         last_com2_swap = com2_swap;
 907     }
 908
 909     if ( navcom1_has_power() && nav1_servicable->getBoolValue() ) {
 910         // Nav1 Swap
 911         int nav1_swap = radio_switch_data[11] & 0x01;
 912         static int last_nav1_swap;
 913         if ( nav1_swap && (last_nav1_swap != nav1_swap) ) {
 914             float tmp = nav1_freq->getFloatValue();
 915             fgSetFloat( "/radios/nav[0]/frequencies/selected-mhz",
 916                         nav1_stby_freq->getFloatValue() );
 917             fgSetFloat( "/radios/nav[0]/frequencies/standby-mhz", tmp );
 918         }
 919         last_nav1_swap = nav1_swap;
 920     }
 921
 922     if ( navcom2_has_power() && nav2_servicable->getBoolValue() ) {
 923         // Nav2 Swap
 924         int nav2_swap = !(radio_switch_data[19] & 0x01);
 925         static int last_nav2_swap;
 926         if ( nav2_swap && (last_nav2_swap != nav2_swap) ) {
 927             float tmp = nav2_freq->getFloatValue();
 928             fgSetFloat( "/radios/nav[1]/frequencies/selected-mhz",
 929                         nav2_stby_freq->getFloatValue() );
 930             fgSetFloat( "/radios/nav[1]/frequencies/standby-mhz", tmp );
 931         }
 932         last_nav2_swap = nav2_swap;
 933     }
 934
 935     if ( navcom1_has_power() && comm1_servicable->getBoolValue() ) {
 936         // Com1 Tuner
 937         int com1_tuner_fine = ((radio_switch_data[5] >> 4) & 0x0f) - 1;
 938         int com1_tuner_coarse = (radio_switch_data[5] & 0x0f) - 1;
 939         static int last_com1_tuner_fine = com1_tuner_fine;
 940         static int last_com1_tuner_coarse = com1_tuner_coarse;
 941
 942         freq = com1_stby_freq->getFloatValue();
 943         coarse_freq = (int)freq;
 944         fine_freq = (int)((freq - coarse_freq) * 40 + 0.5);
 945
 946         if ( com1_tuner_fine != last_com1_tuner_fine ) {
 947             diff = com1_tuner_fine - last_com1_tuner_fine;
 948             if ( abs(diff) > 4 ) {
 949                 // roll over
 950                 if ( com1_tuner_fine < last_com1_tuner_fine ) {
 951                     // going up
 952                     diff = 12 - last_com1_tuner_fine + com1_tuner_fine;
 953                 } else {
 954                     // going down
 955                     diff = com1_tuner_fine - 12 - last_com1_tuner_fine;
 956                 }
 957             }
 958             fine_freq += diff;
 959         }
 960         while ( fine_freq >= 40.0 ) { fine_freq -= 40.0; }
 961         while ( fine_freq < 0.0 )  { fine_freq += 40.0; }
 962
 963         if ( com1_tuner_coarse != last_com1_tuner_coarse ) {
 964             diff = com1_tuner_coarse - last_com1_tuner_coarse;
 965             if ( abs(diff) > 4 ) {
 966                 // roll over
 967                 if ( com1_tuner_coarse < last_com1_tuner_coarse ) {
 968                     // going up
 969                     diff = 12 - last_com1_tuner_coarse + com1_tuner_coarse;
 970                 } else {
 971                     // going down
 972                     diff = com1_tuner_coarse - 12 - last_com1_tuner_coarse;
 973                 }
 974             }
 975             coarse_freq += diff;
 976         }
 977         if ( coarse_freq < 118.0 ) { coarse_freq += 19.0; }
 978         if ( coarse_freq > 136.0 ) { coarse_freq -= 19.0; }
 979
 980         last_com1_tuner_fine = com1_tuner_fine;
 981         last_com1_tuner_coarse = com1_tuner_coarse;
 982
 983         fgSetFloat( "/radios/comm[0]/frequencies/standby-mhz",
 984                     coarse_freq + fine_freq / 40.0 );
 985     }
 986
 987     if ( navcom2_has_power() && comm2_servicable->getBoolValue() ) {
 988         // Com2 Tuner
 989         int com2_tuner_fine = ((radio_switch_data[13] >> 4) & 0x0f) - 1;
 990         int com2_tuner_coarse = (radio_switch_data[13] & 0x0f) - 1;
 991         static int last_com2_tuner_fine = com2_tuner_fine;
 992         static int last_com2_tuner_coarse = com2_tuner_coarse;
 993
 994         freq = com2_stby_freq->getFloatValue();
 995         coarse_freq = (int)freq;
 996         fine_freq = (int)((freq - coarse_freq) * 40 + 0.5);
 997
 998         if ( com2_tuner_fine != last_com2_tuner_fine ) {
 999             diff = com2_tuner_fine - last_com2_tuner_fine;
1000             if ( abs(diff) > 4 ) {
1001                 // roll over
1002                 if ( com2_tuner_fine < last_com2_tuner_fine ) {
1003                     // going up
1004                     diff = 12 - last_com2_tuner_fine + com2_tuner_fine;
1005                 } else {
1006                     // going down
1007                     diff = com2_tuner_fine - 12 - last_com2_tuner_fine;
1008                 }
1009             }
1010             fine_freq += diff;
1011         }
1012         while ( fine_freq >= 40.0 ) { fine_freq -= 40.0; }
1013         while ( fine_freq < 0.0 )  { fine_freq += 40.0; }
1014
1015         if ( com2_tuner_coarse != last_com2_tuner_coarse ) {
1016             diff = com2_tuner_coarse - last_com2_tuner_coarse;
1017             if ( abs(diff) > 4 ) {
1018                 // roll over
1019                 if ( com2_tuner_coarse < last_com2_tuner_coarse ) {
1020                     // going up
1021                     diff = 12 - last_com2_tuner_coarse + com2_tuner_coarse;
1022                 } else {
1023                     // going down
1024                     diff = com2_tuner_coarse - 12 - last_com2_tuner_coarse;
1025                 }
1026             }
1027             coarse_freq += diff;
1028         }
1029         if ( coarse_freq < 118.0 ) { coarse_freq += 19.0; }
1030         if ( coarse_freq > 136.0 ) { coarse_freq -= 19.0; }
1031
1032         last_com2_tuner_fine = com2_tuner_fine;
1033         last_com2_tuner_coarse = com2_tuner_coarse;
1034
1035         fgSetFloat( "/radios/comm[1]/frequencies/standby-mhz",
1036                     coarse_freq + fine_freq / 40.0 );
1037     }
1038
1039     if ( navcom1_has_power() && nav1_servicable->getBoolValue() ) {
1040         // Nav1 Tuner
1041         int nav1_tuner_fine = ((radio_switch_data[9] >> 4) & 0x0f) - 1;
1042         int nav1_tuner_coarse = (radio_switch_data[9] & 0x0f) - 1;
1043         static int last_nav1_tuner_fine = nav1_tuner_fine;
1044         static int last_nav1_tuner_coarse = nav1_tuner_coarse;
1045
1046         freq = nav1_stby_freq->getFloatValue();
1047         coarse_freq = (int)freq;
1048         fine_freq = (int)((freq - coarse_freq) * 20 + 0.5);
1049
1050         if ( nav1_tuner_fine != last_nav1_tuner_fine ) {
1051             diff = nav1_tuner_fine - last_nav1_tuner_fine;
1052             if ( abs(diff) > 4 ) {
1053                 // roll over
1054                 if ( nav1_tuner_fine < last_nav1_tuner_fine ) {
1055                     // going up
1056                     diff = 12 - last_nav1_tuner_fine + nav1_tuner_fine;
1057                 } else {
1058                     // going down
1059                     diff = nav1_tuner_fine - 12 - last_nav1_tuner_fine;
1060                 }
1061             }
1062             fine_freq += diff;
1063         }
1064         while ( fine_freq >= 20.0 ) { fine_freq -= 20.0; }
1065         while ( fine_freq < 0.0 )  { fine_freq += 20.0; }
1066
1067         if ( nav1_tuner_coarse != last_nav1_tuner_coarse ) {
1068             diff = nav1_tuner_coarse - last_nav1_tuner_coarse;
1069             if ( abs(diff) > 4 ) {
1070                 // roll over
1071                 if ( nav1_tuner_coarse < last_nav1_tuner_coarse ) {
1072                     // going up
1073                     diff = 12 - last_nav1_tuner_coarse + nav1_tuner_coarse;
1074                 } else {
1075                     // going down
1076                     diff = nav1_tuner_coarse - 12 - last_nav1_tuner_coarse;
1077                 }
1078             }
1079             coarse_freq += diff;
1080         }
1081         if ( coarse_freq < 108.0 ) { coarse_freq += 10.0; }
1082         if ( coarse_freq > 117.0 ) { coarse_freq -= 10.0; }
1083
1084         last_nav1_tuner_fine = nav1_tuner_fine;
1085         last_nav1_tuner_coarse = nav1_tuner_coarse;
1086
1087         fgSetFloat( "/radios/nav[0]/frequencies/standby-mhz",
1088                     coarse_freq + fine_freq / 20.0 );
1089     }
1090
1091     if ( navcom2_has_power() && nav2_servicable->getBoolValue() ) {
1092         // Nav2 Tuner
1093         int nav2_tuner_fine = ((radio_switch_data[17] >> 4) & 0x0f) - 1;
1094         int nav2_tuner_coarse = (radio_switch_data[17] & 0x0f) - 1;
1095         static int last_nav2_tuner_fine = nav2_tuner_fine;
1096         static int last_nav2_tuner_coarse = nav2_tuner_coarse;
1097
1098         freq = nav2_stby_freq->getFloatValue();
1099         coarse_freq = (int)freq;
1100         fine_freq = (int)((freq - coarse_freq) * 20 + 0.5);
1101
1102         if ( nav2_tuner_fine != last_nav2_tuner_fine ) {
1103             diff = nav2_tuner_fine - last_nav2_tuner_fine;
1104             if ( abs(diff) > 4 ) {
1105                 // roll over
1106                 if ( nav2_tuner_fine < last_nav2_tuner_fine ) {
1107                     // going up
1108                     diff = 12 - last_nav2_tuner_fine + nav2_tuner_fine;
1109                 } else {
1110                     // going down
1111                     diff = nav2_tuner_fine - 12 - last_nav2_tuner_fine;
1112                 }
1113             }
1114             fine_freq += diff;
1115         }
1116         while ( fine_freq >= 20.0 ) { fine_freq -= 20.0; }
1117         while ( fine_freq < 0.0 )  { fine_freq += 20.0; }
1118
1119         if ( nav2_tuner_coarse != last_nav2_tuner_coarse ) {
1120             diff = nav2_tuner_coarse - last_nav2_tuner_coarse;
1121             if ( abs(diff) > 4 ) {
1122                 // roll over
1123                 if ( nav2_tuner_coarse < last_nav2_tuner_coarse ) {
1124                     // going up
1125                     diff = 12 - last_nav2_tuner_coarse + nav2_tuner_coarse;
1126                 } else {
1127                     // going down
1128                     diff = nav2_tuner_coarse - 12 - last_nav2_tuner_coarse;
1129                 }
1130             }
1131             coarse_freq += diff;
1132         }
1133         if ( coarse_freq < 108.0 ) { coarse_freq += 10.0; }
1134         if ( coarse_freq > 117.0 ) { coarse_freq -= 10.0; }
1135
1136         last_nav2_tuner_fine = nav2_tuner_fine;
1137         last_nav2_tuner_coarse = nav2_tuner_coarse;
1138
1139         fgSetFloat( "/radios/nav[1]/frequencies/standby-mhz",
1140                     coarse_freq + fine_freq / 20.0);
1141     }
1142
1143     // ADF Tuner
1144
1145     int adf_tuner_fine = ((radio_switch_data[21] >> 4) & 0x0f) - 1;
1146     int adf_tuner_coarse = (radio_switch_data[21] & 0x0f) - 1;
1147     static int last_adf_tuner_fine = adf_tuner_fine;
1148     static int last_adf_tuner_coarse = adf_tuner_coarse;
1149
1150     if ( adf_has_power() && adf_servicable->getBoolValue() ) {
1151         // cout << "adf_stby_mode = " << adf_stby_mode->getIntValue() << endl;
1152         if ( adf_count_mode->getIntValue() == 2 ) {
1153             // tune count down timer
1154             value = adf_elapsed_timer->getDoubleValue();
1155         } else {
1156             // tune frequency
1157             if ( adf_stby_mode->getIntValue() == 1 ) {
1158                 value = adf_freq->getFloatValue();
1159             } else {
1160                 value = adf_stby_freq->getFloatValue();
1161             }
1162         }
1163
1164         if ( adf_tuner_fine != last_adf_tuner_fine ) {
1165             diff = adf_tuner_fine - last_adf_tuner_fine;
1166             if ( abs(diff) > 4 ) {
1167                 // roll over
1168                 if ( adf_tuner_fine < last_adf_tuner_fine ) {
1169                     // going up
1170                     diff = 12 - last_adf_tuner_fine + adf_tuner_fine;
1171                 } else {
1172                     // going down
1173                     diff = adf_tuner_fine - 12 - last_adf_tuner_fine;
1174                 }
1175             }
1176             value += diff;
1177         }
1178
1179         if ( adf_tuner_coarse != last_adf_tuner_coarse ) {
1180             diff = adf_tuner_coarse - last_adf_tuner_coarse;
1181             if ( abs(diff) > 4 ) {
1182                 // roll over
1183                 if ( adf_tuner_coarse < last_adf_tuner_coarse ) {
1184                     // going up
1185                     diff = 12 - last_adf_tuner_coarse + adf_tuner_coarse;
1186                 } else {
1187                     // going down
1188                     diff = adf_tuner_coarse - 12 - last_adf_tuner_coarse;
1189                 }
1190             }
1191             if ( adf_count_mode->getIntValue() == 2 ) {
1192                 value += 60 * diff;
1193             } else {
1194                 value += 25 * diff;
1195             }
1196         }
1197         if ( adf_count_mode->getIntValue() == 2 ) {
1198             if ( value < 0 ) { value += 3600; }
1199             if ( value > 3599 ) { value -= 3600; }
1200         } else {
1201             if ( value < 200 ) { value += 1600; }
1202             if ( value > 1799 ) { value -= 1600; }
1203         }
1204
1205         if ( adf_count_mode->getIntValue() == 2 ) {
1206             fgSetFloat( "/radios/kr-87/outputs/elapsed-timer", value );
1207         } else {
1208             if ( adf_stby_mode->getIntValue() == 1 ) {
1209                 fgSetFloat( "/radios/kr-87/outputs/selected-khz", value );
1210             } else {
1211                 fgSetFloat( "/radios/kr-87/outputs/standby-khz", value );
1212             }
1213         }
1214     }
1215     last_adf_tuner_fine = adf_tuner_fine;
1216     last_adf_tuner_coarse = adf_tuner_coarse;
1217
1218
1219     // ADF buttons
1220     fgSetInt( "/radios/kr-87/inputs/adf-btn",
1221               !(radio_switch_data[23] & 0x01) );
1222     fgSetInt( "/radios/kr-87/inputs/bfo-btn",
1223               !(radio_switch_data[23] >> 1 & 0x01) );
1224     fgSetInt( "/radios/kr-87/inputs/frq-btn",
1225               !(radio_switch_data[23] >> 2 & 0x01) );
1226     fgSetInt( "/radios/kr-87/inputs/flt-et-btn",
1227               !(radio_switch_data[23] >> 3 & 0x01) );
1228     fgSetInt( "/radios/kr-87/inputs/set-rst-btn",
1229               !(radio_switch_data[23] >> 4 & 0x01) );
1230     fgSetInt( "/radios/kr-87/inputs/power-btn",
1231               radio_switch_data[23] >> 5 & 0x01 );
1232     /* cout << "adf = " << !(radio_switch_data[23] & 0x01)
1233          << " bfo = " << !(radio_switch_data[23] >> 1 & 0x01)
1234          << " frq = " << !(radio_switch_data[23] >> 2 & 0x01)
1235          << " flt/et = " << !(radio_switch_data[23] >> 3 & 0x01)
1236          << " set/rst = " << !(radio_switch_data[23] >> 4 & 0x01)
1237          << endl; */
1238
1239     // Transponder Tuner
1240     int i;
1241     int digit_tuner[4];
1242     digit_tuner[0] = radio_switch_data[25] & 0x0f;
1243     digit_tuner[1] = ( radio_switch_data[25] >> 4 ) & 0x0f;
1244     digit_tuner[2] = radio_switch_data[29] & 0x0f;
1245     digit_tuner[3] = ( radio_switch_data[29] >> 4 ) & 0x0f;
1246
1247     static int last_digit_tuner[4];
1248     static bool first_time = true;
1249     if ( first_time ) {
1250         first_time = false;
1251         for ( i = 0; i < 4; ++i ) {
1252             last_digit_tuner[i] = digit_tuner[i];
1253         }
1254     }
1255
1256     if ( xpdr_has_power() && xpdr_servicable->getBoolValue() ) {
1257         int id_code = xpdr_id_code->getIntValue();
1258         int digit[4];
1259         int place = 1000;
1260         for ( i = 0; i < 4; ++i ) {
1261             digit[i] = id_code / place;
1262             id_code -= digit[i] * place;
1263             place /= 10;
1264         }
1265
1266         for ( i = 0; i < 4; ++i ) {
1267             if ( digit_tuner[i] != last_digit_tuner[i] ) {
1268                 diff = digit_tuner[i] - last_digit_tuner[i];
1269                 if ( abs(diff) > 4 ) {
1270                     // roll over
1271                     if ( digit_tuner[i] < last_digit_tuner[i] ) {
1272                         // going up
1273                         diff = 15 - last_digit_tuner[i] + digit_tuner[i];
1274                     } else {
1275                         // going down
1276                         diff = digit_tuner[i] - 15 - last_digit_tuner[i];
1277                     }
1278                 }
1279                 digit[i] += diff;
1280             }
1281             while ( digit[i] >= 8 ) { digit[i] -= 8; }
1282             while ( digit[i] < 0 )  { digit[i] += 8; }
1283         }
1284
1285         fgSetInt( "/radios/kt-70/inputs/digit1", digit[0] );
1286         fgSetInt( "/radios/kt-70/inputs/digit2", digit[1] );
1287         fgSetInt( "/radios/kt-70/inputs/digit3", digit[2] );
1288         fgSetInt( "/radios/kt-70/inputs/digit4", digit[3] );
1289     }
1290     for ( i = 0; i < 4; ++i ) {
1291         last_digit_tuner[i] = digit_tuner[i];
1292     }
1293
1294     int tmp = 0;
1295     for ( i = 0; i < 5; ++i ) {
1296         if ( radio_switch_data[27] >> i & 0x01 ) {
1297             tmp = i + 1;
1298         }
1299     }
1300     fgSetInt( "/radios/kt-70/inputs/func-knob", tmp );
1301     fgSetInt( "/radios/kt-70/inputs/ident-btn",
1302               !(radio_switch_data[27] >> 5 & 0x01) );
1303
1304     // Audio panel switches
1305     fgSetInt( "/radios/nav[0]/audio-btn",
1306               (radio_switch_data[3] & 0x01) );
1307     fgSetInt( "/radios/nav[1]/audio-btn",
1308               (radio_switch_data[3] >> 2 & 0x01) );
1309     fgSetInt( "/radios/kr-87/inputs/audio-btn",
1310               (radio_switch_data[3] >> 4 & 0x01) );
1311     fgSetInt( "/radios/marker-beacon/audio-btn",
1312               (radio_switch_data[3] >> 6 & 0x01) );
1313
1314     return true;
1315 }
1316
1317
1318 /////////////////////////////////////////////////////////////////////
1319 // Update the radio display
1320 /////////////////////////////////////////////////////////////////////
1321
1322 bool FGATC610x::do_radio_display() {
1323
1324     char digits[10];
1325     int i;
1326
1327     if ( dme_has_power() && dme_servicable->getBoolValue() ) {
1328         // DME minutes
1329         float minutes = dme_min->getFloatValue();
1330         if ( minutes > 999 ) {
1331             minutes = 999.0;
1332         }
1333         snprintf(digits, 7, "%03.0f", minutes);
1334         for ( i = 0; i < 6; ++i ) {
1335             digits[i] -= '0';
1336         }
1337         radio_display_data[0] = digits[1] << 4 | digits[2];
1338         radio_display_data[1] = 0xf0 | digits[0];
1339
1340         // DME knots
1341         float knots = dme_kt->getFloatValue();
1342         if ( knots > 999 ) {
1343             knots = 999.0;
1344         }
1345         snprintf(digits, 7, "%03.0f", knots);
1346         for ( i = 0; i < 6; ++i ) {
1347             digits[i] -= '0';
1348         }
1349         radio_display_data[2] = digits[1] << 4 | digits[2];
1350         radio_display_data[3] = 0xf0 | digits[0];
1351
1352         // DME distance (nm)
1353         float nm = dme_nm->getFloatValue();
1354         if ( nm > 99 ) {
1355             nm = 99.0;
1356         }
1357         snprintf(digits, 7, "%04.1f", nm);
1358         for ( i = 0; i < 6; ++i ) {
1359             digits[i] -= '0';
1360         }
1361         radio_display_data[4] = digits[1] << 4 | digits[3];
1362         radio_display_data[5] = 0x00 | digits[0];
1363         // the 0x00 in the upper nibble of the 6th byte of each
1364         // display turns on the decimal point
1365     } else {
1366         // blank dem display
1367         for ( i = 0; i < 6; ++i ) {
1368             radio_display_data[i] = 0xff;
1369         }
1370     }
1371
1372     if ( navcom1_has_power() && comm1_servicable->getBoolValue() ) {
1373         // Com1 standby frequency
1374         float com1_stby = com1_stby_freq->getFloatValue();
1375         if ( fabs(com1_stby) > 999.99 ) {
1376             com1_stby = 0.0;
1377         }
1378         snprintf(digits, 7, "%06.3f", com1_stby);
1379         for ( i = 0; i < 6; ++i ) {
1380             digits[i] -= '0';
1381         }
1382         radio_display_data[6] = digits[4] << 4 | digits[5];
1383         radio_display_data[7] = digits[1] << 4 | digits[2];
1384         radio_display_data[8] = 0xf0 | digits[0];
1385
1386         // Com1 in use frequency
1387         float com1 = com1_freq->getFloatValue();
1388         if ( fabs(com1) > 999.99 ) {
1389             com1 = 0.0;
1390         }
1391         snprintf(digits, 7, "%06.3f", com1);
1392         for ( i = 0; i < 6; ++i ) {
1393             digits[i] -= '0';
1394         }
1395         radio_display_data[9] = digits[4] << 4 | digits[5];
1396         radio_display_data[10] = digits[1] << 4 | digits[2];
1397         radio_display_data[11] = 0x00 | digits[0];
1398         // the 0x00 in the upper nibble of the 6th byte of each display
1399         // turns on the decimal point
1400     } else {
1401         radio_display_data[6] = 0xff;
1402         radio_display_data[7] = 0xff;
1403         radio_display_data[8] = 0xff;
1404         radio_display_data[9] = 0xff;
1405         radio_display_data[10] = 0xff;
1406         radio_display_data[11] = 0xff;
1407     }
1408
1409     if ( navcom2_has_power() && comm2_servicable->getBoolValue() ) {
1410         // Com2 standby frequency
1411         float com2_stby = com2_stby_freq->getFloatValue();
1412         if ( fabs(com2_stby) > 999.99 ) {
1413             com2_stby = 0.0;
1414         }
1415         snprintf(digits, 7, "%06.3f", com2_stby);
1416         for ( i = 0; i < 6; ++i ) {
1417             digits[i] -= '0';
1418         }
1419         radio_display_data[18] = digits[4] << 4 | digits[5];
1420         radio_display_data[19] = digits[1] << 4 | digits[2];
1421         radio_display_data[20] = 0xf0 | digits[0];
1422
1423         // Com2 in use frequency
1424         float com2 = com2_freq->getFloatValue();
1425         if ( fabs(com2) > 999.99 ) {
1426         com2 = 0.0;
1427         }
1428         snprintf(digits, 7, "%06.3f", com2);
1429         for ( i = 0; i < 6; ++i ) {
1430             digits[i] -= '0';
1431         }
1432         radio_display_data[21] = digits[4] << 4 | digits[5];
1433         radio_display_data[22] = digits[1] << 4 | digits[2];
1434         radio_display_data[23] = 0x00 | digits[0];
1435         // the 0x00 in the upper nibble of the 6th byte of each display
1436         // turns on the decimal point
1437     } else {
1438         radio_display_data[18] = 0xff;
1439         radio_display_data[19] = 0xff;
1440         radio_display_data[20] = 0xff;
1441         radio_display_data[21] = 0xff;
1442         radio_display_data[22] = 0xff;
1443         radio_display_data[23] = 0xff;
1444     }
1445
1446     if ( navcom1_has_power() && nav1_servicable->getBoolValue() ) {
1447         // Nav1 standby frequency
1448         float nav1_stby = nav1_stby_freq->getFloatValue();
1449         if ( fabs(nav1_stby) > 999.99 ) {
1450         nav1_stby = 0.0;
1451         }
1452         snprintf(digits, 7, "%06.2f", nav1_stby);
1453         for ( i = 0; i < 6; ++i ) {
1454             digits[i] -= '0';
1455         }
1456         radio_display_data[12] = digits[4] << 4 | digits[5];
1457         radio_display_data[13] = digits[1] << 4 | digits[2];
1458         radio_display_data[14] = 0xf0 | digits[0];
1459
1460         // Nav1 in use frequency
1461         float nav1 = nav1_freq->getFloatValue();
1462         if ( fabs(nav1) > 999.99 ) {
1463             nav1 = 0.0;
1464         }
1465         snprintf(digits, 7, "%06.2f", nav1);
1466         for ( i = 0; i < 6; ++i ) {
1467             digits[i] -= '0';
1468         }
1469         radio_display_data[15] = digits[4] << 4 | digits[5];
1470         radio_display_data[16] = digits[1] << 4 | digits[2];
1471         radio_display_data[17] = 0x00 | digits[0];
1472         // the 0x00 in the upper nibble of the 6th byte of each display
1473         // turns on the decimal point
1474     } else {
1475         radio_display_data[12] = 0xff;
1476         radio_display_data[13] = 0xff;
1477         radio_display_data[14] = 0xff;
1478         radio_display_data[15] = 0xff;
1479         radio_display_data[16] = 0xff;
1480         radio_display_data[17] = 0xff;
1481     }
1482
1483     if ( navcom2_has_power() && nav2_servicable->getBoolValue() ) {
1484         // Nav2 standby frequency
1485         float nav2_stby = nav2_stby_freq->getFloatValue();
1486         if ( fabs(nav2_stby) > 999.99 ) {
1487             nav2_stby = 0.0;
1488         }
1489         snprintf(digits, 7, "%06.2f", nav2_stby);
1490         for ( i = 0; i < 6; ++i ) {
1491             digits[i] -= '0';
1492         }
1493         radio_display_data[24] = digits[4] << 4 | digits[5];
1494         radio_display_data[25] = digits[1] << 4 | digits[2];
1495         radio_display_data[26] = 0xf0 | digits[0];
1496
1497         // Nav2 in use frequency
1498         float nav2 = nav2_freq->getFloatValue();
1499         if ( fabs(nav2) > 999.99 ) {
1500             nav2 = 0.0;
1501         }
1502         snprintf(digits, 7, "%06.2f", nav2);
1503         for ( i = 0; i < 6; ++i ) {
1504             digits[i] -= '0';
1505         }
1506         radio_display_data[27] = digits[4] << 4 | digits[5];
1507         radio_display_data[28] = digits[1] << 4 | digits[2];
1508         radio_display_data[29] = 0x00 | digits[0];
1509         // the 0x00 in the upper nibble of the 6th byte of each display
1510         // turns on the decimal point
1511     } else {
1512         radio_display_data[24] = 0xff;
1513         radio_display_data[25] = 0xff;
1514         radio_display_data[26] = 0xff;
1515         radio_display_data[27] = 0xff;
1516         radio_display_data[28] = 0xff;
1517         radio_display_data[29] = 0xff;
1518     }
1519
1520     // ADF standby frequency / timer
1521     if ( adf_has_power() && adf_servicable->getBoolValue() ) {
1522         if ( adf_stby_mode->getIntValue() == 0 ) {
1523             // frequency
1524             float adf_stby = adf_stby_freq->getFloatValue();
1525             if ( fabs(adf_stby) > 1799 ) {
1526                 adf_stby = 1799;
1527             }
1528             snprintf(digits, 7, "%04.0f", adf_stby);
1529             for ( i = 0; i < 6; ++i ) {
1530                 digits[i] -= '0';
1531             }
1532             radio_display_data[30] = digits[3] << 4 | 0x0f;
1533             radio_display_data[31] = digits[1] << 4 | digits[2];
1534             if ( digits[0] == 0 ) {
1535                 radio_display_data[32] = 0xff;
1536             } else {
1537                 radio_display_data[32] = 0xf0 | digits[0];
1538             }
1539         } else {
1540             // timer
1541             double time;
1542             int hours, min, sec;
1543             if ( adf_timer_mode->getIntValue() == 0 ) {
1544                 time = adf_flight_timer->getDoubleValue();
1545             } else {
1546                 time = adf_elapsed_timer->getDoubleValue();
1547             }
1548             // cout << time << endl;
1549             hours = (int)(time / 3600.0);
1550             time -= hours * 3600.00;
1551             min = (int)(time / 60.0);
1552             time -= min * 60.0;
1553             sec = (int)time;
1554             int big, little;
1555             if ( hours > 0 ) {
1556                 big = hours;
1557                 if ( big > 99 ) {
1558                     big = 99;
1559                 }
1560                 little = min;
1561             } else {
1562                 big = min;
1563                 little = sec;
1564             }
1565             if ( big > 99 ) {
1566                 big = 99;
1567             }
1568             // cout << big << ":" << little << endl;
1569             snprintf(digits, 7, "%02d%02d", big, little);
1570             for ( i = 0; i < 6; ++i ) {
1571                 digits[i] -= '0';
1572             }
1573             radio_display_data[30] = digits[2] << 4 | digits[3];
1574             radio_display_data[31] = digits[0] << 4 | digits[1];
1575             radio_display_data[32] = 0xff;
1576         }
1577
1578         // ADF in use frequency
1579         float adf = adf_freq->getFloatValue();
1580         if ( fabs(adf) > 1799 ) {
1581             adf = 1799;
1582         }
1583         snprintf(digits, 7, "%04.0f", adf);
1584         for ( i = 0; i < 6; ++i ) {
1585             digits[i] -= '0';
1586         }
1587         radio_display_data[33] = digits[2] << 4 | digits[3];
1588         if ( digits[0] == 0 ) {
1589             radio_display_data[34] = 0xf0 | digits[1];
1590         } else {
1591             radio_display_data[34] = digits[0] << 4 | digits[1];
1592         }
1593         if ( adf_stby_mode->getIntValue() == 0 ) {
1594           radio_display_data[35] = 0xff;
1595         } else {
1596           radio_display_data[35] = 0x0f;
1597         }
1598     } else {
1599         radio_display_data[30] = 0xff;
1600         radio_display_data[31] = 0xff;
1601         radio_display_data[32] = 0xff;
1602         radio_display_data[33] = 0xff;
1603         radio_display_data[34] = 0xff;
1604         radio_display_data[35] = 0xff;
1605     }
1606
1607     // Transponder code and flight level
1608     if ( xpdr_has_power() && xpdr_servicable->getBoolValue() ) {
1609         if ( xpdr_func_knob->getIntValue() == 2 ) {
1610             // test mode
1611             radio_display_data[36] = 8 << 4 | 8;
1612             radio_display_data[37] = 8 << 4 | 8;
1613             radio_display_data[38] = 0xff;
1614             radio_display_data[39] = 8 << 4 | 0x0f;
1615             radio_display_data[40] = 8 << 4 | 8;
1616         } else {
1617             // other on modes
1618             int id_code = xpdr_id_code->getIntValue();
1619             int place = 1000;
1620             for ( i = 0; i < 4; ++i ) {
1621                 digits[i] = id_code / place;
1622                 id_code -= digits[i] * place;
1623                 place /= 10;
1624             }
1625             radio_display_data[36] = digits[2] << 4 | digits[3];
1626             radio_display_data[37] = digits[0] << 4 | digits[1];
1627             radio_display_data[38] = 0xff;
1628
1629             if ( xpdr_func_knob->getIntValue() == 3 ||
1630                  xpdr_func_knob->getIntValue() == 5 )
1631             {
1632                 // do flight level display
1633                 snprintf(digits, 7, "%03d", xpdr_flight_level->getIntValue() );
1634                 for ( i = 0; i < 6; ++i ) {
1635                     digits[i] -= '0';
1636                 }
1637                 radio_display_data[39] = digits[2] << 4 | 0x0f;
1638                 radio_display_data[40] = digits[0] << 4 | digits[1];
1639             } else {
1640                 // blank flight level display
1641                 radio_display_data[39] = 0xff;
1642                 radio_display_data[40] = 0xff;
1643             }
1644         }
1645     } else {
1646         // off
1647         radio_display_data[36] = 0xff;
1648         radio_display_data[37] = 0xff;
1649         radio_display_data[38] = 0xff;
1650         radio_display_data[39] = 0xff;
1651         radio_display_data[40] = 0xff;
1652     }
1653
1654     ATC610xSetRadios( radios_fd, radio_display_data );
1655
1656     return true;
1657 }
1658
1659
1660 /////////////////////////////////////////////////////////////////////
1661 // Drive the stepper motors
1662 /////////////////////////////////////////////////////////////////////
1663
1664 bool FGATC610x::do_steppers() {
1665     float diff = mag_compass->getFloatValue() - compass_position;
1666     while ( diff < -180.0 ) { diff += 360.0; }
1667     while ( diff >  180.0 ) { diff -= 360.0; }
1668
1669     int steps = (int)(diff * 4);
1670     // cout << "steps = " << steps << endl;
1671     if ( steps > 4 ) { steps = 4; }
1672     if ( steps < -4 ) { steps = -4; }
1673
1674     if ( abs(steps) > 0 ) {
1675         unsigned char cmd = 0x80;       // stepper command
1676         if ( steps > 0 ) {
1677             cmd |= 0x20;                // go up
1678         } else {
1679             cmd |= 0x00;                // go down
1680         }
1681         cmd |= abs(steps);
1682
1683         // sync compass_position with hardware position
1684         compass_position += (float)steps / 4.0;
1685
1686         ATC610xSetStepper( stepper_fd, ATC_COMPASS_CH, cmd );
1687     }
1688
1689     return true;
1690 }
1691
1692
1693 /////////////////////////////////////////////////////////////////////
1694 // Read the switch positions
1695 /////////////////////////////////////////////////////////////////////
1696
1697 // decode the packed switch data
1698 static void update_switch_matrix(
1699         int board,
1700         unsigned char switch_data[ATC_SWITCH_BYTES],
1701         int switch_matrix[2][ATC_NUM_COLS][ATC_SWITCH_BYTES] )
1702 {
1703     for ( int row = 0; row < ATC_SWITCH_BYTES; ++row ) {
1704         unsigned char switches = switch_data[row];
1705
1706         for( int column = 0; column < ATC_NUM_COLS; ++column ) {
1707             switch_matrix[board][column][row] = switches & 1;
1708             switches = switches >> 1;
1709         }
1710     }
1711 }
1712
1713 bool FGATC610x::do_switches() {
1714     ATC610xReadSwitches( switches_fd, switch_data );
1715
1716     // unpack the switch data
1717     int switch_matrix[2][ATC_NUM_COLS][ATC_SWITCH_BYTES];
1718     update_switch_matrix( board, switch_data, switch_matrix );
1719
1720     // master switches
1721     fgSetBool( "/controls/switches/master-bat", switch_matrix[board][4][1] );
1722     fgSetBool( "/controls/switches/master-alt", switch_matrix[board][5][1] );
1723     fgSetBool( "/controls/switches/master-avionics",
1724                switch_matrix[board][0][3] );
1725
1726     // magnetos and starter switch
1727     int magnetos = 0;
1728     bool starter = false;
1729     if ( switch_matrix[board][3][1] == 1 ) {
1730         magnetos = 3;
1731         starter = true;
1732     } else if ( switch_matrix[board][2][1] == 1 ) {
1733         magnetos = 3;
1734         starter = false;
1735     } else if ( switch_matrix[board][1][1] == 1 ) {
1736         magnetos = 2;
1737         starter = false;
1738     } else if ( switch_matrix[board][0][1] == 1 ) {
1739         magnetos = 1;
1740         starter = false;
1741     } else {
1742         magnetos = 0;
1743         starter = false;
1744     }
1745
1746     // do a bit of filtering on the magneto/starter switch and the
1747     // flap lever because these are not well debounced in hardware
1748     static int mag1, mag2, mag3;
1749     mag3 = mag2;
1750     mag2 = mag1;
1751     mag1 = magnetos;
1752     if ( mag1 == mag2 && mag2 == mag3 ) {
1753         fgSetInt( "/controls/magnetos[0]", magnetos );
1754     }
1755     static bool start1, start2, start3;
1756     start3 = start2;
1757     start2 = start1;
1758     start1 = starter;
1759     if ( start1 == start2 && start2 == start3 ) {
1760         fgSetBool( "/controls/starter[0]", starter );
1761     }
1762
1763     // other toggle switches
1764     fgSetBool( "/controls/fuel-pump[0]", switch_matrix[board][0][2] );
1765     fgSetBool( "/controls/switches/flashing-beacon",
1766                switch_matrix[board][1][2] );
1767     fgSetBool( "/controls/switches/landing-light", switch_matrix[board][2][2] );
1768     fgSetBool( "/controls/switches/taxi-lights", switch_matrix[board][3][2] );
1769     fgSetBool( "/controls/switches/nav-lights",
1770                switch_matrix[board][4][2] );
1771     fgSetBool( "/controls/switches/strobe-lights", switch_matrix[board][5][2] );
1772     fgSetBool( "/controls/switches/pitot-heat", switch_matrix[board][6][2] );
1773
1774     // flaps
1775     float flaps = 0.0;
1776     if ( switch_matrix[board][6][3] ) {
1777         flaps = 1.0;
1778     } else if ( switch_matrix[board][5][3] ) {
1779         flaps = 2.0 / 3.0;
1780     } else if ( switch_matrix[board][4][3] ) {
1781         flaps = 1.0 / 3.0;
1782     } else if ( !switch_matrix[board][4][3] ) {
1783         flaps = 0.0;
1784     }
1785
1786     // do a bit of filtering on the magneto/starter switch and the
1787     // flap lever because these are not well debounced in hardware
1788     static float flap1, flap2, flap3;
1789     flap3 = flap2;
1790     flap2 = flap1;
1791     flap1 = flaps;
1792     if ( flap1 == flap2 && flap2 == flap3 ) {
1793         fgSetFloat( "/controls/flaps", flaps );
1794     }
1795
1796     // fuel selector (also filtered)
1797     int fuel = 0;
1798     if ( switch_matrix[board][2][3] ) {
1799         // both
1800         fuel = 3;
1801     } else if ( switch_matrix[board][1][3] ) {
1802         // left
1803         fuel = 1;
1804     } else if ( switch_matrix[board][3][3] ) {
1805         // right
1806         fuel = 2;
1807     } else {
1808         // fuel cutoff
1809         fuel = 0;
1810     }
1811
1812     static int fuel1, fuel2, fuel3;
1813     fuel3 = fuel2;
1814     fuel2 = fuel1;
1815     fuel1 = fuel;
1816     if ( fuel1 == fuel2 && fuel2 == fuel3 ) {
1817         fgSetBool( "/controls/fuel-selector[0]", (fuel & 0x01) > 0 );
1818         fgSetBool( "/controls/fuel-selector[1]", (fuel & 0x02) > 0 );
1819     }
1820
1821     // circuit breakers
1822 #ifdef ATC_SUPPORT_CIRCUIT_BREAKERS_NOT_THE_DEFAULT
1823     fgSetBool( "/controls/circuit-breakers/cabin-lights-pwr",
1824                switch_matrix[board][0][0] );
1825     fgSetBool( "/controls/circuit-breakers/instr-ignition-switch",
1826                switch_matrix[board][1][0] );
1827     fgSetBool( "/controls/circuit-breakers/flaps",
1828                switch_matrix[board][2][0] );
1829     fgSetBool( "/controls/circuit-breakers/avn-bus-1",
1830                switch_matrix[board][3][0] );
1831     fgSetBool( "/controls/circuit-breakers/avn-bus-2",
1832                switch_matrix[board][4][0] );
1833     fgSetBool( "/controls/circuit-breakers/turn-coordinator",
1834                switch_matrix[board][5][0] );
1835     fgSetBool( "/controls/circuit-breakers/instrument-lights",
1836                switch_matrix[board][6][0] );
1837     fgSetBool( "/controls/circuit-breakers/annunciators",
1838                switch_matrix[board][7][0] );
1839 #else
1840     fgSetBool( "/controls/circuit-breakers/cabin-lights-pwr", true );
1841     fgSetBool( "/controls/circuit-breakers/instr-ignition-switch", true );
1842     fgSetBool( "/controls/circuit-breakers/flaps", true );
1843     fgSetBool( "/controls/circuit-breakers/avn-bus-1", true );
1844     fgSetBool( "/controls/circuit-breakers/avn-bus-2", true );
1845     fgSetBool( "/controls/circuit-breakers/turn-coordinator", true );
1846     fgSetBool( "/controls/circuit-breakers/instrument-lights", true );
1847     fgSetBool( "/controls/circuit-breakers/annunciators", true );
1848 #endif
1849
1850     fgSetDouble( "/controls/parking-brake",
1851                  switch_matrix[board][7][3] );
1852     fgSetDouble( "/radios/marker-beacon/power-btn",
1853                  switch_matrix[board][6][1] );
1854
1855     return true;
1856 }
1857
1858
1859 bool FGATC610x::process() {
1860     // Lock the hardware, skip if it's not ready yet
1861     if ( ATC610xLock( lock_fd ) > 0 ) {
1862
1863         do_analog_in();
1864         do_lights();
1865         do_radio_switches();
1866         do_radio_display();
1867         do_steppers();
1868         do_switches();
1869
1870         ATC610xRelease( lock_fd );
1871
1872         return true;
1873     } else {
1874         return false;
1875     }
1876 }
1877
1878
1879 bool FGATC610x::close() {
1880
1881     return true;
1882 }