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