]> git.mxchange.org Git - flightgear.git/blob - src/Aircraft/replay.cxx
Merge branch 'next' of http://git.gitorious.org/fg/flightgear into next
[flightgear.git] / src / Aircraft / replay.cxx
1 // replay.cxx - a system to record and replay FlightGear flights
2 //
3 // Written by Curtis Olson, started Juley 2003.
4 //
5 // Copyright (C) 2003  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 // $Id$
22
23 #ifdef HAVE_CONFIG_H
24 #  include "config.h"
25 #endif
26
27 #include <float.h>
28 #include <simgear/constants.h>
29 #include <simgear/structure/exception.hxx>
30
31 #include <Main/fg_props.hxx>
32 #include <Network/native_ctrls.hxx>
33 #include <Network/native_fdm.hxx>
34 #include <Network/net_ctrls.hxx>
35 #include <Network/net_fdm.hxx>
36 #include <FDM/fdm_shell.hxx>
37
38 #include "replay.hxx"
39
40 const double FGReplay::st_list_time = 60.0;   // 60 secs of high res data
41 const double FGReplay::mt_list_time = 600.0;  // 10 mins of 1 fps data
42 const double FGReplay::lt_list_time = 3600.0; // 1 hr of 10 spf data
43
44 // short term sample rate is as every frame
45 const double FGReplay::mt_dt = 0.5; // medium term sample rate (sec)
46 const double FGReplay::lt_dt = 5.0; // long term sample rate (sec)
47
48 /**
49  * Constructor
50  */
51
52 FGReplay::FGReplay() :
53     last_replay_state(0)
54 {
55 }
56
57
58 /**
59  * Destructor
60  */
61
62 FGReplay::~FGReplay()
63 {
64     clear();
65 }
66
67 /**
68  * Clear all internal buffers.
69  */
70 void FGReplay::clear()
71 {
72     while ( !short_term.empty() )
73     {
74         delete short_term.front();
75         short_term.pop_front();
76     }
77     while ( !medium_term.empty() )
78     {
79         delete medium_term.front();
80         medium_term.pop_front();
81     }
82     while ( !long_term.empty() )
83     {
84         delete long_term.front();
85         long_term.pop_front();
86     }
87     while ( !recycler.empty() )
88     {
89         delete recycler.front();
90         recycler.pop_front();
91     }
92 }
93
94 /** 
95  * Initialize the data structures
96  */
97
98 void FGReplay::init()
99 {
100     disable_replay = fgGetNode( "/sim/replay/disable", true );
101     replay_master = fgGetNode( "/sim/freeze/replay-state", true );
102     replay_time = fgGetNode( "/sim/replay/time", true);
103     reinit();
104 }
105
106 /** 
107  * Reset replay queues.
108  */
109
110 void FGReplay::reinit()
111 {
112     sim_time = 0.0;
113     last_mt_time = 0.0;
114     last_lt_time = 0.0;
115
116     // Make sure all queues are flushed
117     clear();
118
119     // Create an estimated nr of required ReplayData objects
120     // 120 is an estimated maximum frame rate. 
121     int estNrObjects = (int) ((st_list_time*120) + (mt_list_time*mt_dt) +
122                              (lt_list_time*lt_dt)); 
123     for (int i = 0; i < estNrObjects; i++)
124     {
125         recycler.push_back(new FGReplayData);
126     }
127     replay_master->setIntValue(0);
128     disable_replay->setBoolValue(0);
129     replay_time->setDoubleValue(0);
130 }
131
132 /** 
133  * Bind to the property tree
134  */
135
136 void FGReplay::bind()
137 {
138 }
139
140
141 /** 
142  *  Unbind from the property tree
143  */
144
145 void FGReplay::unbind()
146 {
147     // nothing to unbind
148 }
149
150
151 /** 
152  *  Update the saved data
153  */
154
155 void FGReplay::update( double dt )
156 {
157     timingInfo.clear();
158     stamp("begin");
159
160     if ( disable_replay->getBoolValue() )
161     {
162         replay_master->setIntValue(0);
163         replay_time->setDoubleValue(0);
164         disable_replay->setBoolValue(0);
165     }
166
167     int replay_state = replay_master->getIntValue();
168
169     if ((replay_state > 0)&&
170        (last_replay_state == 0))
171     {
172         // replay is starting, suspend FDM
173         /* FIXME we need to suspend/resume the FDM - not the entire FDM shell.
174          * FDM isn't available via the global subsystem manager yet, so need a
175          * method at the FDMshell for now */
176         ((FDMShell*) globals->get_subsystem("flight"))->getFDM()->suspend();
177     }
178     else
179     if ((replay_state == 0)&&
180         (last_replay_state > 0))
181     {
182         // replay was active, restore most recent frame
183         replay(DBL_MAX);
184         // replay is finished, resume FDM
185         ((FDMShell*) globals->get_subsystem("flight"))->getFDM()->resume();
186     }
187
188     // remember recent state
189     last_replay_state = replay_state;
190
191     switch(replay_state)
192     {
193         case 0:
194             // replay inactive, keep recording
195             break;
196         case 1:
197             // replay active
198             replay( replay_time->getDoubleValue() );
199             replay_time->setDoubleValue( replay_time->getDoubleValue()
200                                          + ( dt * fgGetInt("/sim/speed-up") ) );
201             return; // don't record the replay session 
202         case 2:
203             // replay paused, no-op
204             return; // don't record the replay session
205         default:
206             throw sg_range_exception("unknown FGReplay state");
207     }
208
209     // flight recording
210
211     //cerr << "Recording replay" << endl;
212     sim_time += dt;
213
214     // build the replay record
215     //FGNetFDM f;
216     //FGProps2NetFDM( &f, false );
217
218     // sanity check, don't collect data if FDM data isn't good
219     if (!fgGetBool("/sim/fdm-initialized", false)) {
220         return;
221     }
222     
223     //FGNetCtrls c;
224     //FGProps2NetCtrls( &c, false, false );
225     //stamp("point_04ba");
226     FGReplayData *r;
227     //stamp("point_04bb");
228     if (!recycler.size()) {
229         stamp("Replay_01");
230         r = new FGReplayData;
231         stamp("Replay_02");
232     } else {
233         r = recycler.front();
234         recycler.pop_front();
235         //stamp("point_04be");
236     }
237
238     r->sim_time = sim_time;
239     //r->ctrls = c;
240     //stamp("point_04e");
241     FGProps2NetFDM( &(r->fdm), false );
242     FGProps2NetCtrls( &(r->ctrls), false, false );
243     //r->fdm = f;
244     //stamp("point_05");
245
246     // update the short term list
247     //stamp("point_06");
248     short_term.push_back( r );
249     //stamp("point_07");
250     FGReplayData *st_front = short_term.front();
251     if ( sim_time - st_front->sim_time > st_list_time ) {
252         while ( sim_time - st_front->sim_time > st_list_time ) {
253             st_front = short_term.front();
254             recycler.push_back(st_front);
255             short_term.pop_front();
256         }
257         //stamp("point_08");
258         // update the medium term list
259         if ( sim_time - last_mt_time > mt_dt ) {
260             last_mt_time = sim_time;
261             st_front = short_term.front();
262             medium_term.push_back( st_front );
263             short_term.pop_front();
264
265             FGReplayData *mt_front = medium_term.front();
266             if ( sim_time - mt_front->sim_time > mt_list_time ) {
267             //stamp("point_09");
268                 while ( sim_time - mt_front->sim_time > mt_list_time ) {
269                     mt_front = medium_term.front();
270                     recycler.push_back(mt_front);
271                     medium_term.pop_front();
272                 }
273                 // update the long term list
274                 if ( sim_time - last_lt_time > lt_dt ) {
275                     last_lt_time = sim_time;
276                     mt_front = medium_term.front();
277                     long_term.push_back( mt_front );
278                     medium_term.pop_front();
279
280                     FGReplayData *lt_front = long_term.front();
281                     if ( sim_time - lt_front->sim_time > lt_list_time ) {
282                         //stamp("point_10");
283                         while ( sim_time - lt_front->sim_time > lt_list_time ) {
284                             lt_front = long_term.front();
285                             recycler.push_back(lt_front);
286                             long_term.pop_front();
287                         }
288                     }
289                 }
290             }
291         }
292     }
293
294 #if 0
295     cout << "short term size = " << short_term.size()
296          << "  time = " << sim_time - short_term.front().sim_time
297          << endl;
298     cout << "medium term size = " << medium_term.size()
299          << "  time = " << sim_time - medium_term.front().sim_time
300          << endl;
301     cout << "long term size = " << long_term.size()
302          << "  time = " << sim_time - long_term.front().sim_time
303          << endl;
304 #endif
305    //stamp("point_finished");
306 }
307
308
309 static double weight( double data1, double data2, double ratio,
310                       bool rotational = false ) {
311     if ( rotational ) {
312         // special handling of rotational data
313         double tmp = data2 - data1;
314         if ( tmp > SGD_PI ) {
315             tmp -= SGD_2PI;
316         } else if ( tmp < -SGD_PI ) {
317             tmp += SGD_2PI;
318         }
319         return data1 + tmp * ratio;
320     } else {
321         // normal "linear" data
322         return data1 + ( data2 - data1 ) * ratio;
323     }
324 }
325
326 /** 
327  * given two FGReplayData elements and a time, interpolate between them
328  */
329 static void update_fdm( FGReplayData frame ) {
330     FGNetFDM2Props( &frame.fdm, false );
331     FGNetCtrls2Props( &frame.ctrls, false, false );
332 }
333
334 /** 
335  * given two FGReplayData elements and a time, interpolate between them
336  */
337 static FGReplayData interpolate( double time, FGReplayData f1, FGReplayData f2 )
338 {
339     FGReplayData result = f1;
340
341     FGNetFDM fdm1 = f1.fdm;
342     FGNetFDM fdm2 = f2.fdm;
343
344     FGNetCtrls ctrls1 = f1.ctrls;
345     FGNetCtrls ctrls2 = f2.ctrls;
346
347     double ratio = (time - f1.sim_time) / (f2.sim_time - f1.sim_time);
348
349     // Interpolate FDM data
350
351     // Positions
352     result.fdm.longitude = weight( fdm1.longitude, fdm2.longitude, ratio );
353     result.fdm.latitude = weight( fdm1.latitude, fdm2.latitude, ratio );
354     result.fdm.altitude = weight( fdm1.altitude, fdm2.altitude, ratio );
355     result.fdm.agl = weight( fdm1.agl, fdm2.agl, ratio );
356     result.fdm.phi = weight( fdm1.phi, fdm2.phi, ratio, true );
357     result.fdm.theta = weight( fdm1.theta, fdm2.theta, ratio, true );
358     result.fdm.psi = weight( fdm1.psi, fdm2.psi, ratio, true );
359
360     // Velocities
361     result.fdm.phidot = weight( fdm1.phidot, fdm2.phidot, ratio, true );
362     result.fdm.thetadot = weight( fdm1.thetadot, fdm2.thetadot, ratio, true );
363     result.fdm.psidot = weight( fdm1.psidot, fdm2.psidot, ratio, true );
364     result.fdm.vcas = weight( fdm1.vcas, fdm2.vcas, ratio );
365     result.fdm.climb_rate = weight( fdm1.climb_rate, fdm2.climb_rate, ratio );
366     result.fdm.v_north = weight( fdm1.v_north, fdm2.v_north, ratio );
367     result.fdm.v_east = weight( fdm1.v_east, fdm2.v_east, ratio );
368     result.fdm.v_down = weight( fdm1.v_down, fdm2.v_down, ratio );
369
370     result.fdm.v_wind_body_north
371         = weight( fdm1.v_wind_body_north, fdm2.v_wind_body_north, ratio );
372     result.fdm.v_wind_body_east
373         = weight( fdm1.v_wind_body_east, fdm2.v_wind_body_east, ratio );
374     result.fdm.v_wind_body_down
375         = weight( fdm1.v_wind_body_down, fdm2.v_wind_body_down, ratio );
376
377     // Stall
378     result.fdm.stall_warning
379         = weight( fdm1.stall_warning, fdm2.stall_warning, ratio );
380
381     // Accelerations
382     result.fdm.A_X_pilot = weight( fdm1.A_X_pilot, fdm2.A_X_pilot, ratio );
383     result.fdm.A_Y_pilot = weight( fdm1.A_Y_pilot, fdm2.A_Y_pilot, ratio );
384     result.fdm.A_Z_pilot = weight( fdm1.A_Z_pilot, fdm2.A_Z_pilot, ratio );
385
386     unsigned int i;
387
388     // Engine status
389     for ( i = 0; i < fdm1.num_engines; ++i ) {
390         result.fdm.eng_state[i] = fdm1.eng_state[i];
391         result.fdm.rpm[i] = weight( fdm1.rpm[i], fdm2.rpm[i], ratio );
392         result.fdm.fuel_flow[i]
393             = weight( fdm1.fuel_flow[i], fdm2.fuel_flow[i], ratio );
394         result.fdm.fuel_px[i]
395             = weight( fdm1.fuel_px[i], fdm2.fuel_px[i], ratio );
396         result.fdm.egt[i] = weight( fdm1.egt[i], fdm2.egt[i], ratio );
397         result.fdm.cht[i] = weight( fdm1.cht[i], fdm2.cht[i], ratio );
398         result.fdm.mp_osi[i] = weight( fdm1.mp_osi[i], fdm2.mp_osi[i], ratio );
399         result.fdm.tit[i] = weight( fdm1.tit[i], fdm2.tit[i], ratio );
400         result.fdm.oil_temp[i]
401             = weight( fdm1.oil_temp[i], fdm2.oil_temp[i], ratio );
402         result.fdm.oil_px[i] = weight( fdm1.oil_px[i], fdm2.oil_px[i], ratio );
403     }
404
405     // Consumables
406     for ( i = 0; i < fdm1.num_tanks; ++i ) {
407         result.fdm.fuel_quantity[i]
408             = weight( fdm1.fuel_quantity[i], fdm2.fuel_quantity[i], ratio );
409     }
410
411     // Gear status
412     for ( i = 0; i < fdm1.num_wheels; ++i ) {
413         result.fdm.wow[i] = (int)(weight( fdm1.wow[i], fdm2.wow[i], ratio ));
414         result.fdm.gear_pos[i]
415             = weight( fdm1.gear_pos[i], fdm2.gear_pos[i], ratio );
416         result.fdm.gear_steer[i]
417             = weight( fdm1.gear_steer[i], fdm2.gear_steer[i], ratio );
418         result.fdm.gear_compression[i]
419             = weight( fdm1.gear_compression[i], fdm2.gear_compression[i],
420                       ratio );
421     }
422
423     // Environment
424     result.fdm.cur_time = fdm1.cur_time;
425     result.fdm.warp = fdm1.warp;
426     result.fdm.visibility = weight( fdm1.visibility, fdm2.visibility, ratio );
427
428     // Control surface positions (normalized values)
429     result.fdm.elevator = weight( fdm1.elevator, fdm2.elevator, ratio );
430     result.fdm.left_flap = weight( fdm1.left_flap, fdm2.left_flap, ratio );
431     result.fdm.right_flap = weight( fdm1.right_flap, fdm2.right_flap, ratio );
432     result.fdm.left_aileron
433         = weight( fdm1.left_aileron, fdm2.left_aileron, ratio );
434     result.fdm.right_aileron
435         = weight( fdm1.right_aileron, fdm2.right_aileron, ratio );
436     result.fdm.rudder = weight( fdm1.rudder, fdm2.rudder, ratio );
437     result.fdm.speedbrake = weight( fdm1.speedbrake, fdm2.speedbrake, ratio );
438     result.fdm.spoilers = weight( fdm1.spoilers, fdm2.spoilers, ratio );
439      
440     // Interpolate Control input data
441
442     // Aero controls
443     result.ctrls.aileron = weight( ctrls1.aileron, ctrls2.aileron, ratio );
444     result.ctrls.elevator = weight( ctrls1.elevator, ctrls2.elevator, ratio );
445     result.ctrls.rudder = weight( ctrls1.rudder, ctrls2.rudder, ratio );
446     result.ctrls.aileron_trim
447         = weight( ctrls1.aileron_trim, ctrls2.aileron_trim, ratio );
448     result.ctrls.elevator_trim
449         = weight( ctrls1.elevator_trim, ctrls2.elevator_trim, ratio );
450     result.ctrls.rudder_trim
451         = weight( ctrls1.rudder_trim, ctrls2.rudder_trim, ratio );
452     result.ctrls.flaps = weight( ctrls1.flaps, ctrls2.flaps, ratio );
453     result.ctrls.flaps_power = ctrls1.flaps_power;
454     result.ctrls.flap_motor_ok = ctrls1.flap_motor_ok;
455
456     // Engine controls
457     for ( i = 0; i < ctrls1.num_engines; ++i ) {
458         result.ctrls.master_bat[i] = ctrls1.master_bat[i];
459         result.ctrls.master_alt[i] = ctrls1.master_alt[i];
460         result.ctrls.magnetos[i] = ctrls1.magnetos[i];
461         result.ctrls.starter_power[i] = ctrls1.starter_power[i];
462         result.ctrls.throttle[i]
463             = weight( ctrls1.throttle[i], ctrls2.throttle[i], ratio );
464         result.ctrls.mixture[i]
465             = weight( ctrls1.mixture[i], ctrls2.mixture[i], ratio );
466         result.ctrls.fuel_pump_power[i] = ctrls1.fuel_pump_power[i];
467         result.ctrls.prop_advance[i]
468             = weight( ctrls1.prop_advance[i], ctrls2.prop_advance[i], ratio );
469         result.ctrls.engine_ok[i] = ctrls1.engine_ok[i];
470         result.ctrls.mag_left_ok[i] = ctrls1.mag_left_ok[i];
471         result.ctrls.mag_right_ok[i] = ctrls1.mag_right_ok[i];
472         result.ctrls.spark_plugs_ok[i] = ctrls1.spark_plugs_ok[i];
473         result.ctrls.oil_press_status[i] = ctrls1.oil_press_status[i];
474         result.ctrls.fuel_pump_ok[i] = ctrls1.fuel_pump_ok[i];
475     }
476
477     // Fuel management
478     for ( i = 0; i < ctrls1.num_tanks; ++i ) {
479         result.ctrls.fuel_selector[i] = ctrls1.fuel_selector[i];
480     }
481
482     // Brake controls
483     result.ctrls.brake_left
484             = weight( ctrls1.brake_left, ctrls2.brake_left, ratio );
485     result.ctrls.brake_right
486             = weight( ctrls1.brake_right, ctrls2.brake_right, ratio );
487     result.ctrls.brake_parking
488             = weight( ctrls1.brake_parking, ctrls2.brake_parking, ratio );
489
490     // Landing Gear
491     result.ctrls.gear_handle = ctrls1.gear_handle;
492
493     // Switches
494     result.ctrls.turbulence_norm = ctrls1.turbulence_norm;
495
496     // wind and turbulance
497     result.ctrls.wind_speed_kt
498         = weight( ctrls1.wind_speed_kt, ctrls2.wind_speed_kt, ratio );
499     result.ctrls.wind_dir_deg
500         = weight( ctrls1.wind_dir_deg, ctrls2.wind_dir_deg, ratio );
501     result.ctrls.turbulence_norm
502         = weight( ctrls1.turbulence_norm, ctrls2.turbulence_norm, ratio );
503
504     // other information about environment
505     result.ctrls.hground = weight( ctrls1.hground, ctrls2.hground, ratio );
506     result.ctrls.magvar = weight( ctrls1.magvar, ctrls2.magvar, ratio );
507
508     // simulation control
509     result.ctrls.speedup = ctrls1.speedup;
510     result.ctrls.freeze = ctrls1.freeze;
511
512     return result;
513 }
514
515 /** 
516  * interpolate a specific time from a specific list
517  */
518 static void interpolate( double time, const replay_list_type &list ) {
519     // sanity checking
520     if ( list.size() == 0 ) {
521         // handle empty list
522         return;
523     } else if ( list.size() == 1 ) {
524         // handle list size == 1
525         update_fdm( (*list[0]) );
526         return;
527     }
528
529     unsigned int last = list.size() - 1;
530     unsigned int first = 0;
531     unsigned int mid = ( last + first ) / 2;
532
533
534     bool done = false;
535     while ( !done ) {
536         // cout << "  " << first << " <=> " << last << endl;
537         if ( last == first ) {
538             done = true;
539         } else if ( list[mid]->sim_time < time && list[mid+1]->sim_time < time ) {
540             // too low
541             first = mid;
542             mid = ( last + first ) / 2;
543         } else if ( list[mid]->sim_time > time && list[mid+1]->sim_time > time ) {
544             // too high
545             last = mid;
546             mid = ( last + first ) / 2;
547         } else {
548             done = true;
549         }
550     }
551
552     FGReplayData result = interpolate( time, (*list[mid]), (*list[mid+1]) );
553
554     update_fdm( result );
555 }
556
557
558 /** 
559  *  Replay a saved frame based on time, interpolate from the two
560  *  nearest saved frames.
561  */
562
563 void FGReplay::replay( double time ) {
564     // cout << "replay: " << time << " ";
565     // find the two frames to interpolate between
566     double t1, t2;
567
568     if ( short_term.size() > 0 ) {
569         t1 = short_term.back()->sim_time;
570         t2 = short_term.front()->sim_time;
571         if ( time > t1 ) {
572             // replay the most recent frame
573             update_fdm( (*short_term.back()) );
574             // cout << "first frame" << endl;
575         } else if ( time <= t1 && time >= t2 ) {
576             interpolate( time, short_term );
577             // cout << "from short term" << endl;
578         } else if ( medium_term.size() > 0 ) {
579             t1 = short_term.front()->sim_time;
580             t2 = medium_term.back()->sim_time;
581             if ( time <= t1 && time >= t2 ) {
582                 FGReplayData result = interpolate( time,
583                                                    (*medium_term.back()),
584                                                    (*short_term.front()) );
585                 update_fdm( result );
586                 // cout << "from short/medium term" << endl;
587             } else {
588                 t1 = medium_term.back()->sim_time;
589                 t2 = medium_term.front()->sim_time;
590                 if ( time <= t1 && time >= t2 ) {
591                     interpolate( time, medium_term );
592                     // cout << "from medium term" << endl;
593                 } else if ( long_term.size() > 0 ) {
594                     t1 = medium_term.front()->sim_time;
595                     t2 = long_term.back()->sim_time;
596                     if ( time <= t1 && time >= t2 ) {
597                         FGReplayData result = interpolate( time,
598                                                            (*long_term.back()),
599                                                            (*medium_term.front()));
600                         update_fdm( result );
601                         // cout << "from medium/long term" << endl;
602                     } else {
603                         t1 = long_term.back()->sim_time;
604                         t2 = long_term.front()->sim_time;
605                         if ( time <= t1 && time >= t2 ) {
606                             interpolate( time, long_term );
607                             // cout << "from long term" << endl;
608                         } else {
609                             // replay the oldest long term frame
610                             update_fdm( (*long_term.front()) );
611                             // cout << "oldest long term frame" << endl;
612                         }
613                     }
614                 } else {
615                     // replay the oldest medium term frame
616                     update_fdm( (*medium_term.front()) );
617                     // cout << "oldest medium term frame" << endl;
618                 }
619             }
620         } else {
621             // replay the oldest short term frame
622             update_fdm( (*short_term.front()) );
623             // cout << "oldest short term frame" << endl;
624         }
625     } else {
626         // nothing to replay
627     }
628 }
629
630
631 double FGReplay::get_start_time() {
632     if ( long_term.size() > 0 ) {
633         return (*long_term.front()).sim_time;
634     } else if ( medium_term.size() > 0 ) {
635         return (*medium_term.front()).sim_time;
636     } else if ( short_term.size() ) {
637         return (*short_term.front()).sim_time;
638     } else {
639         return 0.0;
640     }
641 }
642
643 double FGReplay::get_end_time() {
644     if ( short_term.size() ) {
645         return (*short_term.back()).sim_time;
646     } else {
647         return 0.0;
648     } 
649 }