src/Cockpit/kr_87.cxx

   1 // kr-87.cxx -- class to impliment the King KR 87 Digital ADF
   2 //
   3 // Written by Curtis Olson, started April 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 <stdio.h>      // snprintf
  29
  30 #include <simgear/compiler.h>
  31 #include <simgear/math/sg_random.h>
  32
  33 #include <Aircraft/aircraft.hxx>
  34 #include <Navaids/navlist.hxx>
  35 #include <Time/FGEventMgr.hxx>
  36
  37 #include "kr_87.hxx"
  38
  39 #include <string>
  40 SG_USING_STD(string);
  41
  42 static int play_count = 0;
  43 static time_t last_time = 0;
  44
  45
  46 /**
  47  * Boy, this is ugly!  Make the VOR range vary by altitude difference.
  48  */
  49 static double kludgeRange ( double stationElev, double aircraftElev,
  50                             double nominalRange)
  51 {
  52                                 // Assume that the nominal range (usually
  53                                 // 50nm) applies at a 5,000 ft difference.
  54                                 // Just a wild guess!
  55   double factor = ((aircraftElev*SG_METER_TO_FEET) - stationElev) / 5000.0;
  56   double range = fabs(nominalRange * factor);
  57
  58                                 // Clamp the range to keep it sane; for
  59                                 // now, never less than 50% or more than
  60                                 // 500% of nominal range.
  61   if (range < nominalRange/2.0) {
  62     range = nominalRange/2.0;
  63   } else if (range > nominalRange*5.0) {
  64     range = nominalRange*5.0;
  65   }
  66
  67   return range;
  68 }
  69
  70
  71 // Constructor
  72 FGKR_87::FGKR_87() :
  73     lon_node(fgGetNode("/position/longitude-deg", true)),
  74     lat_node(fgGetNode("/position/latitude-deg", true)),
  75     alt_node(fgGetNode("/position/altitude-ft", true)),
  76     need_update(true),
  77     valid(false),
  78     inrange(false),
  79     goal_needle_deg(0.0),
  80     et_flash_time(0.0),
  81     ant_mode(0),
  82     stby_mode(0),
  83     timer_mode(0),
  84     count_mode(0),
  85     rotation(0),
  86     on_off_vol_btn(0.5),
  87     adf_btn(true),
  88     bfo_btn(false),
  89     frq_btn(false),
  90     last_frq_btn(false),
  91     flt_et_btn(false),
  92     last_flt_et_btn(false),
  93     set_rst_btn(false),
  94     last_set_rst_btn(false),
  95     freq(0.0),
  96     stby_freq(0.0),
  97     needle_deg(0.0),
  98     flight_timer(0.0),
  99     elapsed_timer(0.0),
 100     tmp_timer(0.0)
 101 {
 102     SGPath path( globals->get_fg_root() );
 103     SGPath term = path;
 104     term.append( "Navaids/range.term" );
 105     SGPath low = path;
 106     low.append( "Navaids/range.low" );
 107     SGPath high = path;
 108     high.append( "Navaids/range.high" );
 109
 110     term_tbl = new SGInterpTable( term.str() );
 111     low_tbl = new SGInterpTable( low.str() );
 112     high_tbl = new SGInterpTable( high.str() );
 113 }
 114
 115
 116 // Destructor
 117 FGKR_87::~FGKR_87()
 118 {
 119     delete term_tbl;
 120     delete low_tbl;
 121     delete high_tbl;
 122 }
 123
 124
 125 void
 126 FGKR_87::init ()
 127 {
 128     morse.init();
 129
 130     update(0);                  // FIXME: use dt
 131 }
 132
 133 void
 134 FGKR_87::bind ()
 135 {
 136     // internal values
 137     fgTie("/radios/kr-87/internal/ident", this,
 138           &FGKR_87::get_ident_btn,
 139           &FGKR_87::set_ident_btn);
 140     fgSetArchivable("/radios/kr-87/internal/ident");
 141     fgTie("/radios/kr-87/internal/inrange", this, &FGKR_87::get_inrange);
 142     fgTie("/radios/kr-87/internal/heading", this, &FGKR_87::get_heading);
 143
 144     // modes
 145     fgTie("/radios/kr-87/modes/ant", this,
 146           &FGKR_87::get_ant_mode);
 147     fgTie("/radios/kr-87/modes/stby", this,
 148           &FGKR_87::get_stby_mode);
 149     fgTie("/radios/kr-87/modes/timer", this,
 150           &FGKR_87::get_timer_mode);
 151     fgTie("/radios/kr-87/modes/count", this,
 152           &FGKR_87::get_count_mode);
 153
 154     // input and buttons
 155     fgTie("/radios/kr-87/inputs/rotation-deg", this,
 156           &FGKR_87::get_rotation, &FGKR_87::set_rotation);
 157     fgSetArchivable("/radios/kr-87/inputs/rotation-deg");
 158     fgTie("/radios/kr-87/inputs/on-off-volume", this,
 159           &FGKR_87::get_on_off_vol_btn,
 160           &FGKR_87::set_on_off_vol_btn);
 161     fgSetArchivable("/radios/kr-87/inputs/on-off-volume");
 162     fgTie("/radios/kr-87/inputs/adf-btn", this,
 163           &FGKR_87::get_adf_btn,
 164           &FGKR_87::set_adf_btn);
 165     fgTie("/radios/kr-87/inputs/bfo-btn", this,
 166           &FGKR_87::get_bfo_btn,
 167           &FGKR_87::set_bfo_btn);
 168     fgTie("/radios/kr-87/inputs/frq-btn", this,
 169           &FGKR_87::get_frq_btn,
 170           &FGKR_87::set_frq_btn);
 171     fgTie("/radios/kr-87/inputs/flt-et-btn", this,
 172           &FGKR_87::get_flt_et_btn,
 173           &FGKR_87::set_flt_et_btn);
 174     fgTie("/radios/kr-87/inputs/set-rst-btn", this,
 175           &FGKR_87::get_set_rst_btn,
 176           &FGKR_87::set_set_rst_btn);
 177
 178     // outputs
 179     fgTie("/radios/kr-87/outputs/selected-khz", this,
 180           &FGKR_87::get_freq, &FGKR_87::set_freq);
 181     fgSetArchivable("/radios/kr-87/outputs/selected-khz");
 182     fgTie("/radios/kr-87/outputs/standby-khz", this,
 183           &FGKR_87::get_stby_freq, &FGKR_87::set_stby_freq);
 184     fgSetArchivable("/radios/kr-87/outputs/standby-khz");
 185     fgTie("/radios/kr-87/outputs/needle-deg", this,
 186           &FGKR_87::get_needle_deg);
 187     fgTie("/radios/kr-87/outputs/flight-timer", this, &FGKR_87::get_flight_timer);
 188     fgTie("/radios/kr-87/outputs/elapsed-timer", this,
 189           &FGKR_87::get_elapsed_timer,
 190           &FGKR_87::set_elapsed_timer);
 191
 192     // annunciators
 193     fgTie("/radios/kr-87/annunciators/ant", this, &FGKR_87::get_ant_ann );
 194     fgTie("/radios/kr-87/annunciators/adf", this, &FGKR_87::get_adf_ann );
 195     fgTie("/radios/kr-87/annunciators/bfo", this, &FGKR_87::get_bfo_ann );
 196     fgTie("/radios/kr-87/annunciators/frq", this, &FGKR_87::get_frq_ann );
 197     fgTie("/radios/kr-87/annunciators/flt", this, &FGKR_87::get_flt_ann );
 198     fgTie("/radios/kr-87/annunciators/et", this, &FGKR_87::get_et_ann );
 199 }
 200
 201 void
 202 FGKR_87::unbind ()
 203 {
 204     // internal values
 205     fgUntie("/radios/kr-87/internal/ident");
 206     fgUntie("/radios/kr-87/internal/inrange");
 207     fgUntie("/radios/kr-87/internal/heading");
 208
 209     // modes
 210     fgUntie("/radios/kr-87/modes/ant");
 211     fgUntie("/radios/kr-87/modes/stby");
 212     fgUntie("/radios/kr-87/modes/timer");
 213     fgUntie("/radios/kr-87/modes/count");
 214
 215     // input and buttons
 216     fgUntie("/radios/kr-87/inputs/rotation-deg");
 217     fgUntie("/radios/kr-87/inputs/on-off-volume");
 218     fgUntie("/radios/kr-87/inputs/adf-btn");
 219     fgUntie("/radios/kr-87/inputs/bfo-btn");
 220     fgUntie("/radios/kr-87/inputs/frq-btn");
 221     fgUntie("/radios/kr-87/inputs/flt-et-btn");
 222     fgUntie("/radios/kr-87/inputs/set-rst-btn");
 223
 224     // outputs
 225     fgUntie("/radios/kr-87/outputs/selected-khz");
 226     fgUntie("/radios/kr-87/outputs/standby-khz");
 227     fgUntie("/radios/kr-87/outputs/needle-deg");
 228     fgUntie("/radios/kr-87/outputs/flight-timer");
 229     fgUntie("/radios/kr-87/outputs/elapsed-timer");
 230
 231     // annunciators
 232     fgUntie("/radios/kr-87/annunciators/ant");
 233     fgUntie("/radios/kr-87/annunciators/adf");
 234     fgUntie("/radios/kr-87/annunciators/bfo");
 235     fgUntie("/radios/kr-87/annunciators/frq");
 236     fgUntie("/radios/kr-87/annunciators/flt");
 237     fgUntie("/radios/kr-87/annunciators/et");
 238 }
 239
 240
 241 // Update the various nav values based on position and valid tuned in navs
 242 void
 243 FGKR_87::update( double dt )
 244 {
 245     double acft_lon = lon_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 246     double acft_lat = lat_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 247     double acft_elev = alt_node->getDoubleValue() * SG_FEET_TO_METER;
 248
 249     need_update = false;
 250
 251     Point3D aircraft = sgGeodToCart( Point3D( acft_lon, acft_lat, acft_elev ) );
 252     Point3D station;
 253     double az1, az2, s;
 254
 255     ////////////////////////////////////////////////////////////////////////
 256     // Radio
 257     ////////////////////////////////////////////////////////////////////////
 258
 259     if ( on_off_vol_btn >= 0.01 ) {
 260         // buttons
 261         if ( adf_btn == 0 ) {
 262             ant_mode = 1;
 263         } else {
 264             ant_mode = 0;
 265         }
 266
 267         if ( frq_btn && frq_btn != last_frq_btn && stby_mode == 0 ) {
 268             double tmp = freq;
 269             freq = stby_freq;
 270             stby_freq = tmp;
 271         } else if ( frq_btn ) {
 272             stby_mode = 0;
 273             count_mode = 0;
 274         }
 275         last_frq_btn = frq_btn;
 276
 277         if ( flt_et_btn && flt_et_btn != last_flt_et_btn ) {
 278             if ( stby_mode == 0 ) {
 279                 timer_mode = 0;
 280             } else {
 281                 timer_mode = !timer_mode;
 282             }
 283             stby_mode = 1;
 284         }
 285         last_flt_et_btn = flt_et_btn;
 286
 287         if ( set_rst_btn == 1 && set_rst_btn != last_set_rst_btn ) {
 288             // button depressed
 289            tmp_timer = 0.0;
 290         }
 291         if ( set_rst_btn == 1 && set_rst_btn == last_set_rst_btn ) {
 292             // button depressed and was last iteration too
 293             tmp_timer += dt;
 294             cout << "tmp_timer = " << tmp_timer << endl;
 295             if ( tmp_timer > 2.0 ) {
 296                 // button held depressed for 2 seconds
 297                 cout << "entering elapsed count down mode" << endl;
 298                 timer_mode = 1;
 299                 count_mode = 2;
 300                 elapsed_timer = 0.0;
 301             }
 302         }
 303         if ( set_rst_btn == 0 && set_rst_btn != last_set_rst_btn ) {
 304             // button released
 305             if ( tmp_timer > 2.0 ) {
 306                 // button held depressed for 2 seconds, don't adjust
 307                 // mode, just exit
 308             } else if ( count_mode == 2 ) {
 309                 count_mode = 1;
 310             } else {
 311                 count_mode = 0;
 312                 elapsed_timer = 0.0;
 313             }
 314         }
 315         last_set_rst_btn = set_rst_btn;
 316
 317         // timers
 318         flight_timer += dt;
 319
 320         if ( set_rst_btn == 0 ) {
 321             // only count if set/rst button not depressed
 322             if ( count_mode == 0 ) {
 323                 elapsed_timer += dt;
 324             } else if ( count_mode == 1 ) {
 325                 elapsed_timer -= dt;
 326                 if ( elapsed_timer < 1.0 ) {
 327                     count_mode = 0;
 328                     elapsed_timer = 0.0;
 329                 }
 330             }
 331         }
 332
 333         // annunciators
 334         ant_ann = adf_btn;
 335         adf_ann = !adf_btn;
 336         bfo_ann = bfo_btn;
 337         frq_ann = !stby_mode;
 338         flt_ann = stby_mode && !timer_mode;
 339         if ( count_mode < 2 ) {
 340             et_ann = stby_mode && timer_mode;
 341         } else {
 342             et_flash_time += dt;
 343             if ( et_ann && et_flash_time > 0.5 ) {
 344                 et_ann = false;
 345                 et_flash_time -= 0.5;
 346             } else if ( !et_ann && et_flash_time > 0.2 ) {
 347                 et_ann = true;
 348                 et_flash_time -= 0.2;
 349             }
 350         }
 351
 352         if ( valid ) {
 353             // staightline distance
 354             station = Point3D( x, y, z );
 355             dist = aircraft.distance3D( station );
 356
 357             // wgs84 heading
 358             geo_inverse_wgs_84( acft_elev,
 359                                 acft_lat * SGD_RADIANS_TO_DEGREES,
 360                                 acft_lon * SGD_RADIANS_TO_DEGREES,
 361                                 stn_lat, stn_lon,
 362                                 &az1, &az2, &s );
 363             heading = az1;
 364             // cout << " heading = " << heading
 365             //      << " dist = " << dist << endl;
 366
 367             effective_range = kludgeRange(stn_elev, acft_elev, range);
 368             if ( dist < effective_range * SG_NM_TO_METER ) {
 369                 inrange = true;
 370             } else if ( dist < 2 * effective_range * SG_NM_TO_METER ) {
 371                 inrange = sg_random() <
 372                     ( 2 * effective_range * SG_NM_TO_METER - dist ) /
 373                     (effective_range * SG_NM_TO_METER);
 374             } else {
 375                 inrange = false;
 376             }
 377
 378             if ( inrange ) {
 379                 goal_needle_deg = heading
 380                     - fgGetDouble("/orientation/heading-deg");
 381             }
 382         } else {
 383             inrange = false;
 384         }
 385
 386         if ( ant_mode ) {
 387             goal_needle_deg = 90.0;
 388         }
 389     } else {
 390         // unit turned off
 391         goal_needle_deg = 0.0;
 392         flight_timer = 0.0;
 393         elapsed_timer = 0.0;
 394         ant_ann = false;
 395         adf_ann = false;
 396         bfo_ann = false;
 397         frq_ann = false;
 398         flt_ann = false;
 399         et_ann = false;
 400     }
 401
 402
 403     while ( goal_needle_deg < 0.0 ) { goal_needle_deg += 360.0; }
 404     while ( goal_needle_deg >= 360.0 ) { goal_needle_deg -= 360.0; }
 405
 406     double diff = goal_needle_deg - needle_deg;
 407     while ( diff < -180.0 ) { diff += 360.0; }
 408     while ( diff > 180.0 ) { diff -= 360.0; }
 409
 410     needle_deg += diff * dt * 4;
 411     while ( needle_deg < 0.0 ) { needle_deg += 360.0; }
 412     while ( needle_deg >= 360.0 ) { needle_deg -= 360.0; }
 413
 414     // cout << "goal = " << goal_needle_deg << " actual = " << needle_deg
 415     //      << endl;
 416     // cout << "flt = " << flight_timer << " et = " << elapsed_timer
 417     //      << " needle = " << needle_deg << endl;
 418
 419 #ifdef ENABLE_AUDIO_SUPPORT
 420     if ( valid && inrange ) {
 421         // play station ident via audio system if on + ident,
 422         // otherwise turn it off
 423         if ( on_off_vol_btn >= 0.01 && ident_btn ) {
 424             FGSimpleSound *sound;
 425             sound = globals->get_soundmgr()->find( "adf-ident" );
 426             if ( sound != NULL ) {
 427                 sound->set_volume( on_off_vol_btn );
 428             } else {
 429                 SG_LOG( SG_COCKPIT, SG_ALERT, "Can't find adf-ident sound" );
 430             }
 431             if ( last_time <
 432                  globals->get_time_params()->get_cur_time() - 30 ) {
 433                 last_time = globals->get_time_params()->get_cur_time();
 434                 play_count = 0;
 435             }
 436             if ( play_count < 4 ) {
 437                 // play ADF ident
 438                 if ( !globals->get_soundmgr()->is_playing("adf-ident") ) {
 439                     globals->get_soundmgr()->play_once( "adf-ident" );
 440                     ++play_count;
 441                 }
 442             }
 443         } else {
 444             globals->get_soundmgr()->stop( "adf-ident" );
 445         }
 446     }
 447 #endif
 448 }
 449
 450
 451 // Update current nav/adf radio stations based on current postition
 452 void FGKR_87::search()
 453 {
 454     double acft_lon = lon_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 455     double acft_lat = lat_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 456     double acft_elev = alt_node->getDoubleValue() * SG_FEET_TO_METER;
 457
 458                                 // FIXME: the panel should handle this
 459     FGNav nav;
 460
 461     static string last_ident = "";
 462
 463     ////////////////////////////////////////////////////////////////////////
 464     // ADF.
 465     ////////////////////////////////////////////////////////////////////////
 466
 467     if ( current_navlist->query( acft_lon, acft_lat, acft_elev, freq, &nav ) ) {
 468         char sfreq[128];
 469         snprintf( sfreq, 10, "%.0f", freq );
 470         ident = sfreq;
 471         ident += nav.get_ident();
 472 //      cout << "adf ident = " << ident << endl;
 473         valid = true;
 474         if ( last_ident != ident ) {
 475             last_ident = ident;
 476
 477             trans_ident = nav.get_trans_ident();
 478             stn_lon = nav.get_lon();
 479             stn_lat = nav.get_lat();
 480             stn_elev = nav.get_elev();
 481             range = nav.get_range();
 482             effective_range = kludgeRange(stn_elev, acft_elev, range);
 483             x = nav.get_x();
 484             y = nav.get_y();
 485             z = nav.get_z();
 486
 487 #ifdef ENABLE_AUDIO_SUPPORT
 488             if ( globals->get_soundmgr()->exists( "adf-ident" ) ) {
 489                 globals->get_soundmgr()->remove( "adf-ident" );
 490             }
 491             FGSimpleSound *sound;
 492             sound = morse.make_ident( trans_ident, LO_FREQUENCY );
 493             sound->set_volume( 0.3 );
 494             globals->get_soundmgr()->add( sound, "adf-ident" );
 495
 496             int offset = (int)(sg_random() * 30.0);
 497             play_count = offset / 4;
 498             last_time = globals->get_time_params()->get_cur_time() -
 499                 offset;
 500             // cout << "offset = " << offset << " play_count = "
 501             //      << play_count << " last_time = "
 502             //      << last_time << " current time = "
 503             //      << globals->get_time_params()->get_cur_time() << endl;
 504 #endif
 505
 506             // cout << "Found an adf station in range" << endl;
 507             // cout << " id = " << nav.get_ident() << endl;
 508         }
 509     } else {
 510         valid = false;
 511         ident = "";
 512         trans_ident = "";
 513 #ifdef ENABLE_AUDIO_SUPPORT
 514         globals->get_soundmgr()->remove( "adf-ident" );
 515 #endif
 516         last_ident = "";
 517         // cout << "not picking up adf. :-(" << endl;
 518     }
 519 }
 520
 521
 522 double FGKR_87::get_stby_freq() const {
 523     if ( stby_mode == 0 ) {
 524         return stby_freq;
 525     } else {
 526         if ( timer_mode == 0 ) {
 527             return flight_timer;
 528         } else {
 529             return elapsed_timer;
 530         }
 531     }
 532 }