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