]> git.mxchange.org Git - flightgear.git/blob - src/Aircraft/replay.cxx
bd3bab7bf3ce52a45e6a3a7a35760137de81c60f
[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         if (replay_master->getIntValue())
163         {
164             // replay was active, restore most recent frame
165             replay(DBL_MAX);
166         }
167         replay_master->setIntValue(0);
168         replay_time->setDoubleValue(0);
169         disable_replay->setBoolValue(0);
170     }
171
172     int replay_state = replay_master->getIntValue();
173
174     if ((replay_state > 0)&&
175        (last_replay_state == 0))
176     {
177         // replay is starting, suspend FDM
178         /* FIXME we need to suspend/resume the FDM - not the entire FDM shell.
179          * FDM isn't available via the global subsystem manager yet, so need a
180          * method at the FDMshell for now */
181         ((FDMShell*) globals->get_subsystem("flight"))->getFDM()->suspend();
182     }
183     else
184     if ((replay_state == 0)&&
185         (last_replay_state > 0))
186     {
187         // replay is finished, resume FDM
188         ((FDMShell*) globals->get_subsystem("flight"))->getFDM()->resume();
189     }
190
191     // remember recent state
192     last_replay_state = replay_state;
193
194     switch(replay_state)
195     {
196         case 0:
197             // replay inactive, keep recording
198             break;
199         case 1:
200             // replay active
201             replay( replay_time->getDoubleValue() );
202             replay_time->setDoubleValue( replay_time->getDoubleValue()
203                                          + ( dt * fgGetInt("/sim/speed-up") ) );
204             return; // don't record the replay session 
205         case 2:
206             // replay paused, no-op
207             return; // don't record the replay session
208         default:
209             throw sg_range_exception("unknown FGReplay state");
210     }
211
212     // flight recording
213
214     //cerr << "Recording replay" << endl;
215     sim_time += dt;
216
217     // build the replay record
218     //FGNetFDM f;
219     //FGProps2NetFDM( &f, false );
220
221     // sanity check, don't collect data if FDM data isn't good
222     if (!fgGetBool("/sim/fdm-initialized", false)) {
223         return;
224     }
225     
226     //FGNetCtrls c;
227     //FGProps2NetCtrls( &c, false, false );
228     //stamp("point_04ba");
229     FGReplayData *r;
230     //stamp("point_04bb");
231     if (!recycler.size()) {
232         stamp("Replay_01");
233         r = new FGReplayData;
234         stamp("Replay_02");
235     } else {
236         r = recycler.front();
237         recycler.pop_front();
238         //stamp("point_04be");
239     }
240
241     r->sim_time = sim_time;
242     //r->ctrls = c;
243     //stamp("point_04e");
244     FGProps2NetFDM( &(r->fdm), false );
245     FGProps2NetCtrls( &(r->ctrls), false, false );
246     //r->fdm = f;
247     //stamp("point_05");
248
249     // update the short term list
250     //stamp("point_06");
251     short_term.push_back( r );
252     //stamp("point_07");
253     FGReplayData *st_front = short_term.front();
254     if ( sim_time - st_front->sim_time > st_list_time ) {
255         while ( sim_time - st_front->sim_time > st_list_time ) {
256             st_front = short_term.front();
257             recycler.push_back(st_front);
258             short_term.pop_front();
259         }
260         //stamp("point_08");
261         // update the medium term list
262         if ( sim_time - last_mt_time > mt_dt ) {
263             last_mt_time = sim_time;
264             st_front = short_term.front();
265             medium_term.push_back( st_front );
266             short_term.pop_front();
267
268             FGReplayData *mt_front = medium_term.front();
269             if ( sim_time - mt_front->sim_time > mt_list_time ) {
270             //stamp("point_09");
271                 while ( sim_time - mt_front->sim_time > mt_list_time ) {
272                     mt_front = medium_term.front();
273                     recycler.push_back(mt_front);
274                     medium_term.pop_front();
275                 }
276                 // update the long term list
277                 if ( sim_time - last_lt_time > lt_dt ) {
278                     last_lt_time = sim_time;
279                     mt_front = medium_term.front();
280                     long_term.push_back( mt_front );
281                     medium_term.pop_front();
282
283                     FGReplayData *lt_front = long_term.front();
284                     if ( sim_time - lt_front->sim_time > lt_list_time ) {
285                         //stamp("point_10");
286                         while ( sim_time - lt_front->sim_time > lt_list_time ) {
287                             lt_front = long_term.front();
288                             recycler.push_back(lt_front);
289                             long_term.pop_front();
290                         }
291                     }
292                 }
293             }
294         }
295     }
296
297 #if 0
298     cout << "short term size = " << short_term.size()
299          << "  time = " << sim_time - short_term.front().sim_time
300          << endl;
301     cout << "medium term size = " << medium_term.size()
302          << "  time = " << sim_time - medium_term.front().sim_time
303          << endl;
304     cout << "long term size = " << long_term.size()
305          << "  time = " << sim_time - long_term.front().sim_time
306          << endl;
307 #endif
308    //stamp("point_finished");
309 }
310
311
312 static double weight( double data1, double data2, double ratio,
313                       bool rotational = false ) {
314     if ( rotational ) {
315         // special handling of rotational data
316         double tmp = data2 - data1;
317         if ( tmp > SGD_PI ) {
318             tmp -= SGD_2PI;
319         } else if ( tmp < -SGD_PI ) {
320             tmp += SGD_2PI;
321         }
322         return data1 + tmp * ratio;
323     } else {
324         // normal "linear" data
325         return data1 + ( data2 - data1 ) * ratio;
326     }
327 }
328
329 /** 
330  * given two FGReplayData elements and a time, interpolate between them
331  */
332 static void update_fdm( FGReplayData frame ) {
333     FGNetFDM2Props( &frame.fdm, false );
334     FGNetCtrls2Props( &frame.ctrls, false, false );
335 }
336
337 /** 
338  * given two FGReplayData elements and a time, interpolate between them
339  */
340 static FGReplayData interpolate( double time, FGReplayData f1, FGReplayData f2 )
341 {
342     FGReplayData result = f1;
343
344     FGNetFDM fdm1 = f1.fdm;
345     FGNetFDM fdm2 = f2.fdm;
346
347     FGNetCtrls ctrls1 = f1.ctrls;
348     FGNetCtrls ctrls2 = f2.ctrls;
349
350     double ratio = (time - f1.sim_time) / (f2.sim_time - f1.sim_time);
351
352     // Interpolate FDM data
353
354     // Positions
355     result.fdm.longitude = weight( fdm1.longitude, fdm2.longitude, ratio );
356     result.fdm.latitude = weight( fdm1.latitude, fdm2.latitude, ratio );
357     result.fdm.altitude = weight( fdm1.altitude, fdm2.altitude, ratio );
358     result.fdm.agl = weight( fdm1.agl, fdm2.agl, ratio );
359     result.fdm.phi = weight( fdm1.phi, fdm2.phi, ratio, true );
360     result.fdm.theta = weight( fdm1.theta, fdm2.theta, ratio, true );
361     result.fdm.psi = weight( fdm1.psi, fdm2.psi, ratio, true );
362
363     // Velocities
364     result.fdm.phidot = weight( fdm1.phidot, fdm2.phidot, ratio, true );
365     result.fdm.thetadot = weight( fdm1.thetadot, fdm2.thetadot, ratio, true );
366     result.fdm.psidot = weight( fdm1.psidot, fdm2.psidot, ratio, true );
367     result.fdm.vcas = weight( fdm1.vcas, fdm2.vcas, ratio );
368     result.fdm.climb_rate = weight( fdm1.climb_rate, fdm2.climb_rate, ratio );
369     result.fdm.v_north = weight( fdm1.v_north, fdm2.v_north, ratio );
370     result.fdm.v_east = weight( fdm1.v_east, fdm2.v_east, ratio );
371     result.fdm.v_down = weight( fdm1.v_down, fdm2.v_down, ratio );
372
373     result.fdm.v_wind_body_north
374         = weight( fdm1.v_wind_body_north, fdm2.v_wind_body_north, ratio );
375     result.fdm.v_wind_body_east
376         = weight( fdm1.v_wind_body_east, fdm2.v_wind_body_east, ratio );
377     result.fdm.v_wind_body_down
378         = weight( fdm1.v_wind_body_down, fdm2.v_wind_body_down, ratio );
379
380     // Stall
381     result.fdm.stall_warning
382         = weight( fdm1.stall_warning, fdm2.stall_warning, ratio );
383
384     // Accelerations
385     result.fdm.A_X_pilot = weight( fdm1.A_X_pilot, fdm2.A_X_pilot, ratio );
386     result.fdm.A_Y_pilot = weight( fdm1.A_Y_pilot, fdm2.A_Y_pilot, ratio );
387     result.fdm.A_Z_pilot = weight( fdm1.A_Z_pilot, fdm2.A_Z_pilot, ratio );
388
389     unsigned int i;
390
391     // Engine status
392     for ( i = 0; i < fdm1.num_engines; ++i ) {
393         result.fdm.eng_state[i] = fdm1.eng_state[i];
394         result.fdm.rpm[i] = weight( fdm1.rpm[i], fdm2.rpm[i], ratio );
395         result.fdm.fuel_flow[i]
396             = weight( fdm1.fuel_flow[i], fdm2.fuel_flow[i], ratio );
397         result.fdm.fuel_px[i]
398             = weight( fdm1.fuel_px[i], fdm2.fuel_px[i], ratio );
399         result.fdm.egt[i] = weight( fdm1.egt[i], fdm2.egt[i], ratio );
400         result.fdm.cht[i] = weight( fdm1.cht[i], fdm2.cht[i], ratio );
401         result.fdm.mp_osi[i] = weight( fdm1.mp_osi[i], fdm2.mp_osi[i], ratio );
402         result.fdm.tit[i] = weight( fdm1.tit[i], fdm2.tit[i], ratio );
403         result.fdm.oil_temp[i]
404             = weight( fdm1.oil_temp[i], fdm2.oil_temp[i], ratio );
405         result.fdm.oil_px[i] = weight( fdm1.oil_px[i], fdm2.oil_px[i], ratio );
406     }
407
408     // Consumables
409     for ( i = 0; i < fdm1.num_tanks; ++i ) {
410         result.fdm.fuel_quantity[i]
411             = weight( fdm1.fuel_quantity[i], fdm2.fuel_quantity[i], ratio );
412     }
413
414     // Gear status
415     for ( i = 0; i < fdm1.num_wheels; ++i ) {
416         result.fdm.wow[i] = (int)(weight( fdm1.wow[i], fdm2.wow[i], ratio ));
417         result.fdm.gear_pos[i]
418             = weight( fdm1.gear_pos[i], fdm2.gear_pos[i], ratio );
419         result.fdm.gear_steer[i]
420             = weight( fdm1.gear_steer[i], fdm2.gear_steer[i], ratio );
421         result.fdm.gear_compression[i]
422             = weight( fdm1.gear_compression[i], fdm2.gear_compression[i],
423                       ratio );
424     }
425
426     // Environment
427     result.fdm.cur_time = fdm1.cur_time;
428     result.fdm.warp = fdm1.warp;
429     result.fdm.visibility = weight( fdm1.visibility, fdm2.visibility, ratio );
430
431     // Control surface positions (normalized values)
432     result.fdm.elevator = weight( fdm1.elevator, fdm2.elevator, ratio );
433     result.fdm.left_flap = weight( fdm1.left_flap, fdm2.left_flap, ratio );
434     result.fdm.right_flap = weight( fdm1.right_flap, fdm2.right_flap, ratio );
435     result.fdm.left_aileron
436         = weight( fdm1.left_aileron, fdm2.left_aileron, ratio );
437     result.fdm.right_aileron
438         = weight( fdm1.right_aileron, fdm2.right_aileron, ratio );
439     result.fdm.rudder = weight( fdm1.rudder, fdm2.rudder, ratio );
440     result.fdm.speedbrake = weight( fdm1.speedbrake, fdm2.speedbrake, ratio );
441     result.fdm.spoilers = weight( fdm1.spoilers, fdm2.spoilers, ratio );
442      
443     // Interpolate Control input data
444
445     // Aero controls
446     result.ctrls.aileron = weight( ctrls1.aileron, ctrls2.aileron, ratio );
447     result.ctrls.elevator = weight( ctrls1.elevator, ctrls2.elevator, ratio );
448     result.ctrls.rudder = weight( ctrls1.rudder, ctrls2.rudder, ratio );
449     result.ctrls.aileron_trim
450         = weight( ctrls1.aileron_trim, ctrls2.aileron_trim, ratio );
451     result.ctrls.elevator_trim
452         = weight( ctrls1.elevator_trim, ctrls2.elevator_trim, ratio );
453     result.ctrls.rudder_trim
454         = weight( ctrls1.rudder_trim, ctrls2.rudder_trim, ratio );
455     result.ctrls.flaps = weight( ctrls1.flaps, ctrls2.flaps, ratio );
456     result.ctrls.flaps_power = ctrls1.flaps_power;
457     result.ctrls.flap_motor_ok = ctrls1.flap_motor_ok;
458
459     // Engine controls
460     for ( i = 0; i < ctrls1.num_engines; ++i ) {
461         result.ctrls.master_bat[i] = ctrls1.master_bat[i];
462         result.ctrls.master_alt[i] = ctrls1.master_alt[i];
463         result.ctrls.magnetos[i] = ctrls1.magnetos[i];
464         result.ctrls.starter_power[i] = ctrls1.starter_power[i];
465         result.ctrls.throttle[i]
466             = weight( ctrls1.throttle[i], ctrls2.throttle[i], ratio );
467         result.ctrls.mixture[i]
468             = weight( ctrls1.mixture[i], ctrls2.mixture[i], ratio );
469         result.ctrls.fuel_pump_power[i] = ctrls1.fuel_pump_power[i];
470         result.ctrls.prop_advance[i]
471             = weight( ctrls1.prop_advance[i], ctrls2.prop_advance[i], ratio );
472         result.ctrls.engine_ok[i] = ctrls1.engine_ok[i];
473         result.ctrls.mag_left_ok[i] = ctrls1.mag_left_ok[i];
474         result.ctrls.mag_right_ok[i] = ctrls1.mag_right_ok[i];
475         result.ctrls.spark_plugs_ok[i] = ctrls1.spark_plugs_ok[i];
476         result.ctrls.oil_press_status[i] = ctrls1.oil_press_status[i];
477         result.ctrls.fuel_pump_ok[i] = ctrls1.fuel_pump_ok[i];
478     }
479
480     // Fuel management
481     for ( i = 0; i < ctrls1.num_tanks; ++i ) {
482         result.ctrls.fuel_selector[i] = ctrls1.fuel_selector[i];
483     }
484
485     // Brake controls
486     result.ctrls.brake_left
487             = weight( ctrls1.brake_left, ctrls2.brake_left, ratio );
488     result.ctrls.brake_right
489             = weight( ctrls1.brake_right, ctrls2.brake_right, ratio );
490     result.ctrls.brake_parking
491             = weight( ctrls1.brake_parking, ctrls2.brake_parking, ratio );
492
493     // Landing Gear
494     result.ctrls.gear_handle = ctrls1.gear_handle;
495
496     // Switches
497     result.ctrls.turbulence_norm = ctrls1.turbulence_norm;
498
499     // wind and turbulance
500     result.ctrls.wind_speed_kt
501         = weight( ctrls1.wind_speed_kt, ctrls2.wind_speed_kt, ratio );
502     result.ctrls.wind_dir_deg
503         = weight( ctrls1.wind_dir_deg, ctrls2.wind_dir_deg, ratio );
504     result.ctrls.turbulence_norm
505         = weight( ctrls1.turbulence_norm, ctrls2.turbulence_norm, ratio );
506
507     // other information about environment
508     result.ctrls.hground = weight( ctrls1.hground, ctrls2.hground, ratio );
509     result.ctrls.magvar = weight( ctrls1.magvar, ctrls2.magvar, ratio );
510
511     // simulation control
512     result.ctrls.speedup = ctrls1.speedup;
513     result.ctrls.freeze = ctrls1.freeze;
514
515     return result;
516 }
517
518 /** 
519  * interpolate a specific time from a specific list
520  */
521 static void interpolate( double time, const replay_list_type &list ) {
522     // sanity checking
523     if ( list.size() == 0 ) {
524         // handle empty list
525         return;
526     } else if ( list.size() == 1 ) {
527         // handle list size == 1
528         update_fdm( (*list[0]) );
529         return;
530     }
531
532     unsigned int last = list.size() - 1;
533     unsigned int first = 0;
534     unsigned int mid = ( last + first ) / 2;
535
536
537     bool done = false;
538     while ( !done ) {
539         // cout << "  " << first << " <=> " << last << endl;
540         if ( last == first ) {
541             done = true;
542         } else if ( list[mid]->sim_time < time && list[mid+1]->sim_time < time ) {
543             // too low
544             first = mid;
545             mid = ( last + first ) / 2;
546         } else if ( list[mid]->sim_time > time && list[mid+1]->sim_time > time ) {
547             // too high
548             last = mid;
549             mid = ( last + first ) / 2;
550         } else {
551             done = true;
552         }
553     }
554
555     FGReplayData result = interpolate( time, (*list[mid]), (*list[mid+1]) );
556
557     update_fdm( result );
558 }
559
560
561 /** 
562  *  Replay a saved frame based on time, interpolate from the two
563  *  nearest saved frames.
564  */
565
566 void FGReplay::replay( double time ) {
567     // cout << "replay: " << time << " ";
568     // find the two frames to interpolate between
569     double t1, t2;
570
571     if ( short_term.size() > 0 ) {
572         t1 = short_term.back()->sim_time;
573         t2 = short_term.front()->sim_time;
574         if ( time > t1 ) {
575             // replay the most recent frame
576             update_fdm( (*short_term.back()) );
577             // cout << "first frame" << endl;
578         } else if ( time <= t1 && time >= t2 ) {
579             interpolate( time, short_term );
580             // cout << "from short term" << endl;
581         } else if ( medium_term.size() > 0 ) {
582             t1 = short_term.front()->sim_time;
583             t2 = medium_term.back()->sim_time;
584             if ( time <= t1 && time >= t2 ) {
585                 FGReplayData result = interpolate( time,
586                                                    (*medium_term.back()),
587                                                    (*short_term.front()) );
588                 update_fdm( result );
589                 // cout << "from short/medium term" << endl;
590             } else {
591                 t1 = medium_term.back()->sim_time;
592                 t2 = medium_term.front()->sim_time;
593                 if ( time <= t1 && time >= t2 ) {
594                     interpolate( time, medium_term );
595                     // cout << "from medium term" << endl;
596                 } else if ( long_term.size() > 0 ) {
597                     t1 = medium_term.front()->sim_time;
598                     t2 = long_term.back()->sim_time;
599                     if ( time <= t1 && time >= t2 ) {
600                         FGReplayData result = interpolate( time,
601                                                            (*long_term.back()),
602                                                            (*medium_term.front()));
603                         update_fdm( result );
604                         // cout << "from medium/long term" << endl;
605                     } else {
606                         t1 = long_term.back()->sim_time;
607                         t2 = long_term.front()->sim_time;
608                         if ( time <= t1 && time >= t2 ) {
609                             interpolate( time, long_term );
610                             // cout << "from long term" << endl;
611                         } else {
612                             // replay the oldest long term frame
613                             update_fdm( (*long_term.front()) );
614                             // cout << "oldest long term frame" << endl;
615                         }
616                     }
617                 } else {
618                     // replay the oldest medium term frame
619                     update_fdm( (*medium_term.front()) );
620                     // cout << "oldest medium term frame" << endl;
621                 }
622             }
623         } else {
624             // replay the oldest short term frame
625             update_fdm( (*short_term.front()) );
626             // cout << "oldest short term frame" << endl;
627         }
628     } else {
629         // nothing to replay
630     }
631 }
632
633
634 double FGReplay::get_start_time() {
635     if ( long_term.size() > 0 ) {
636         return (*long_term.front()).sim_time;
637     } else if ( medium_term.size() > 0 ) {
638         return (*medium_term.front()).sim_time;
639     } else if ( short_term.size() ) {
640         return (*short_term.front()).sim_time;
641     } else {
642         return 0.0;
643     }
644 }
645
646 double FGReplay::get_end_time() {
647     if ( short_term.size() ) {
648         return (*short_term.back()).sim_time;
649     } else {
650         return 0.0;
651     } 
652 }