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