1 // replay.cxx - a system to record and replay FlightGear flights
3 // Written by Curtis Olson, started Juley 2003.
5 // Copyright (C) 2003 Curtis L. Olson - http://www.flightgear.org/~curt
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.
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.
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.
28 #include <simgear/constants.h>
29 #include <simgear/structure/exception.hxx>
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>
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
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)
52 FGReplay::FGReplay() :
68 * Clear all internal buffers.
70 void FGReplay::clear()
72 while ( !short_term.empty() )
74 delete short_term.front();
75 short_term.pop_front();
77 while ( !medium_term.empty() )
79 delete medium_term.front();
80 medium_term.pop_front();
82 while ( !long_term.empty() )
84 delete long_term.front();
85 long_term.pop_front();
87 while ( !recycler.empty() )
89 delete recycler.front();
95 * Initialize the data structures
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 replay_looped = fgGetNode( "/sim/replay/looped", true);
108 * Reset replay queues.
111 void FGReplay::reinit()
117 // Make sure all queues are flushed
120 // Create an estimated nr of required ReplayData objects
121 // 120 is an estimated maximum frame rate.
122 int estNrObjects = (int) ((st_list_time*120) + (mt_list_time*mt_dt) +
123 (lt_list_time*lt_dt));
124 for (int i = 0; i < estNrObjects; i++)
126 recycler.push_back(new FGReplayData);
128 replay_master->setIntValue(0);
129 disable_replay->setBoolValue(0);
130 replay_time->setDoubleValue(0);
134 * Bind to the property tree
137 void FGReplay::bind()
143 * Unbind from the property tree
146 void FGReplay::unbind()
153 * Update the saved data
156 void FGReplay::update( double dt )
161 if ( disable_replay->getBoolValue() )
163 replay_master->setIntValue(0);
164 replay_time->setDoubleValue(0);
165 disable_replay->setBoolValue(0);
168 int replay_state = replay_master->getIntValue();
170 if ((replay_state > 0)&&
171 (last_replay_state == 0))
173 // replay is starting, suspend FDM
174 /* FIXME we need to suspend/resume the FDM - not the entire FDM shell.
175 * FDM isn't available via the global subsystem manager yet, so need a
176 * method at the FDMshell for now */
177 ((FDMShell*) globals->get_subsystem("flight"))->getFDM()->suspend();
180 if ((replay_state == 0)&&
181 (last_replay_state > 0))
183 // replay was active, restore most recent frame
185 // replay is finished, resume FDM
186 ((FDMShell*) globals->get_subsystem("flight"))->getFDM()->resume();
189 // remember recent state
190 last_replay_state = replay_state;
195 // replay inactive, keep recording
200 double current_time = replay_time->getDoubleValue();
201 if (current_time<0.0)
203 // initialize start time
204 fgSetDouble( "/sim/replay/start-time", get_start_time() );
205 fgSetDouble( "/sim/replay/end-time", get_end_time() );
206 double duration = fgGetDouble( "/sim/replay/duration" );
207 if( duration && duration < (get_end_time() - get_start_time()) ) {
208 current_time = get_end_time() - duration;
210 current_time = get_start_time();
213 bool IsFinished = replay( replay_time->getDoubleValue() );
214 if ((IsFinished)&&(replay_looped->getBoolValue()))
217 current_time += dt * fgGetInt("/sim/speed-up");
218 replay_time->setDoubleValue(current_time);
220 return; // don't record the replay session
222 // replay paused, no-op
223 return; // don't record the replay session
225 throw sg_range_exception("unknown FGReplay state");
230 //cerr << "Recording replay" << endl;
233 // build the replay record
235 //FGProps2NetFDM( &f, false );
237 // sanity check, don't collect data if FDM data isn't good
238 if (!fgGetBool("/sim/fdm-initialized", false)) {
243 //FGProps2NetCtrls( &c, false, false );
244 //stamp("point_04ba");
246 //stamp("point_04bb");
247 if (!recycler.size()) {
249 r = new FGReplayData;
252 r = recycler.front();
253 recycler.pop_front();
254 //stamp("point_04be");
257 r->sim_time = sim_time;
259 //stamp("point_04e");
260 FGProps2NetFDM( &(r->fdm), false );
261 FGProps2NetCtrls( &(r->ctrls), false, false );
265 // update the short term list
267 short_term.push_back( r );
269 FGReplayData *st_front = short_term.front();
270 if ( sim_time - st_front->sim_time > st_list_time ) {
271 while ( sim_time - st_front->sim_time > st_list_time ) {
272 st_front = short_term.front();
273 recycler.push_back(st_front);
274 short_term.pop_front();
277 // update the medium term list
278 if ( sim_time - last_mt_time > mt_dt ) {
279 last_mt_time = sim_time;
280 st_front = short_term.front();
281 medium_term.push_back( st_front );
282 short_term.pop_front();
284 FGReplayData *mt_front = medium_term.front();
285 if ( sim_time - mt_front->sim_time > mt_list_time ) {
287 while ( sim_time - mt_front->sim_time > mt_list_time ) {
288 mt_front = medium_term.front();
289 recycler.push_back(mt_front);
290 medium_term.pop_front();
292 // update the long term list
293 if ( sim_time - last_lt_time > lt_dt ) {
294 last_lt_time = sim_time;
295 mt_front = medium_term.front();
296 long_term.push_back( mt_front );
297 medium_term.pop_front();
299 FGReplayData *lt_front = long_term.front();
300 if ( sim_time - lt_front->sim_time > lt_list_time ) {
302 while ( sim_time - lt_front->sim_time > lt_list_time ) {
303 lt_front = long_term.front();
304 recycler.push_back(lt_front);
305 long_term.pop_front();
314 cout << "short term size = " << short_term.size()
315 << " time = " << sim_time - short_term.front().sim_time
317 cout << "medium term size = " << medium_term.size()
318 << " time = " << sim_time - medium_term.front().sim_time
320 cout << "long term size = " << long_term.size()
321 << " time = " << sim_time - long_term.front().sim_time
324 //stamp("point_finished");
328 static double weight( double data1, double data2, double ratio,
329 bool rotational = false ) {
331 // special handling of rotational data
332 double tmp = data2 - data1;
333 if ( tmp > SGD_PI ) {
335 } else if ( tmp < -SGD_PI ) {
338 return data1 + tmp * ratio;
340 // normal "linear" data
341 return data1 + ( data2 - data1 ) * ratio;
346 * given two FGReplayData elements and a time, interpolate between them
348 static void update_fdm( FGReplayData frame ) {
349 FGNetFDM2Props( &frame.fdm, false );
350 FGNetCtrls2Props( &frame.ctrls, false, false );
354 * given two FGReplayData elements and a time, interpolate between them
356 static FGReplayData interpolate( double time, FGReplayData f1, FGReplayData f2 )
358 FGReplayData result = f1;
360 FGNetFDM fdm1 = f1.fdm;
361 FGNetFDM fdm2 = f2.fdm;
363 FGNetCtrls ctrls1 = f1.ctrls;
364 FGNetCtrls ctrls2 = f2.ctrls;
366 double ratio = (time - f1.sim_time) / (f2.sim_time - f1.sim_time);
368 // Interpolate FDM data
371 result.fdm.longitude = weight( fdm1.longitude, fdm2.longitude, ratio );
372 result.fdm.latitude = weight( fdm1.latitude, fdm2.latitude, ratio );
373 result.fdm.altitude = weight( fdm1.altitude, fdm2.altitude, ratio );
374 result.fdm.agl = weight( fdm1.agl, fdm2.agl, ratio );
375 result.fdm.phi = weight( fdm1.phi, fdm2.phi, ratio, true );
376 result.fdm.theta = weight( fdm1.theta, fdm2.theta, ratio, true );
377 result.fdm.psi = weight( fdm1.psi, fdm2.psi, ratio, true );
380 result.fdm.phidot = weight( fdm1.phidot, fdm2.phidot, ratio, true );
381 result.fdm.thetadot = weight( fdm1.thetadot, fdm2.thetadot, ratio, true );
382 result.fdm.psidot = weight( fdm1.psidot, fdm2.psidot, ratio, true );
383 result.fdm.vcas = weight( fdm1.vcas, fdm2.vcas, ratio );
384 result.fdm.climb_rate = weight( fdm1.climb_rate, fdm2.climb_rate, ratio );
385 result.fdm.v_north = weight( fdm1.v_north, fdm2.v_north, ratio );
386 result.fdm.v_east = weight( fdm1.v_east, fdm2.v_east, ratio );
387 result.fdm.v_down = weight( fdm1.v_down, fdm2.v_down, ratio );
389 result.fdm.v_wind_body_north
390 = weight( fdm1.v_wind_body_north, fdm2.v_wind_body_north, ratio );
391 result.fdm.v_wind_body_east
392 = weight( fdm1.v_wind_body_east, fdm2.v_wind_body_east, ratio );
393 result.fdm.v_wind_body_down
394 = weight( fdm1.v_wind_body_down, fdm2.v_wind_body_down, ratio );
397 result.fdm.stall_warning
398 = weight( fdm1.stall_warning, fdm2.stall_warning, ratio );
401 result.fdm.A_X_pilot = weight( fdm1.A_X_pilot, fdm2.A_X_pilot, ratio );
402 result.fdm.A_Y_pilot = weight( fdm1.A_Y_pilot, fdm2.A_Y_pilot, ratio );
403 result.fdm.A_Z_pilot = weight( fdm1.A_Z_pilot, fdm2.A_Z_pilot, ratio );
408 for ( i = 0; i < fdm1.num_engines; ++i ) {
409 result.fdm.eng_state[i] = fdm1.eng_state[i];
410 result.fdm.rpm[i] = weight( fdm1.rpm[i], fdm2.rpm[i], ratio );
411 result.fdm.fuel_flow[i]
412 = weight( fdm1.fuel_flow[i], fdm2.fuel_flow[i], ratio );
413 result.fdm.fuel_px[i]
414 = weight( fdm1.fuel_px[i], fdm2.fuel_px[i], ratio );
415 result.fdm.egt[i] = weight( fdm1.egt[i], fdm2.egt[i], ratio );
416 result.fdm.cht[i] = weight( fdm1.cht[i], fdm2.cht[i], ratio );
417 result.fdm.mp_osi[i] = weight( fdm1.mp_osi[i], fdm2.mp_osi[i], ratio );
418 result.fdm.tit[i] = weight( fdm1.tit[i], fdm2.tit[i], ratio );
419 result.fdm.oil_temp[i]
420 = weight( fdm1.oil_temp[i], fdm2.oil_temp[i], ratio );
421 result.fdm.oil_px[i] = weight( fdm1.oil_px[i], fdm2.oil_px[i], ratio );
425 for ( i = 0; i < fdm1.num_tanks; ++i ) {
426 result.fdm.fuel_quantity[i]
427 = weight( fdm1.fuel_quantity[i], fdm2.fuel_quantity[i], ratio );
431 for ( i = 0; i < fdm1.num_wheels; ++i ) {
432 result.fdm.wow[i] = (int)(weight( fdm1.wow[i], fdm2.wow[i], ratio ));
433 result.fdm.gear_pos[i]
434 = weight( fdm1.gear_pos[i], fdm2.gear_pos[i], ratio );
435 result.fdm.gear_steer[i]
436 = weight( fdm1.gear_steer[i], fdm2.gear_steer[i], ratio );
437 result.fdm.gear_compression[i]
438 = weight( fdm1.gear_compression[i], fdm2.gear_compression[i],
443 result.fdm.cur_time = fdm1.cur_time;
444 result.fdm.warp = fdm1.warp;
445 result.fdm.visibility = weight( fdm1.visibility, fdm2.visibility, ratio );
447 // Control surface positions (normalized values)
448 result.fdm.elevator = weight( fdm1.elevator, fdm2.elevator, ratio );
449 result.fdm.left_flap = weight( fdm1.left_flap, fdm2.left_flap, ratio );
450 result.fdm.right_flap = weight( fdm1.right_flap, fdm2.right_flap, ratio );
451 result.fdm.left_aileron
452 = weight( fdm1.left_aileron, fdm2.left_aileron, ratio );
453 result.fdm.right_aileron
454 = weight( fdm1.right_aileron, fdm2.right_aileron, ratio );
455 result.fdm.rudder = weight( fdm1.rudder, fdm2.rudder, ratio );
456 result.fdm.speedbrake = weight( fdm1.speedbrake, fdm2.speedbrake, ratio );
457 result.fdm.spoilers = weight( fdm1.spoilers, fdm2.spoilers, ratio );
459 // Interpolate Control input data
462 result.ctrls.aileron = weight( ctrls1.aileron, ctrls2.aileron, ratio );
463 result.ctrls.elevator = weight( ctrls1.elevator, ctrls2.elevator, ratio );
464 result.ctrls.rudder = weight( ctrls1.rudder, ctrls2.rudder, ratio );
465 result.ctrls.aileron_trim
466 = weight( ctrls1.aileron_trim, ctrls2.aileron_trim, ratio );
467 result.ctrls.elevator_trim
468 = weight( ctrls1.elevator_trim, ctrls2.elevator_trim, ratio );
469 result.ctrls.rudder_trim
470 = weight( ctrls1.rudder_trim, ctrls2.rudder_trim, ratio );
471 result.ctrls.flaps = weight( ctrls1.flaps, ctrls2.flaps, ratio );
472 result.ctrls.flaps_power = ctrls1.flaps_power;
473 result.ctrls.flap_motor_ok = ctrls1.flap_motor_ok;
476 for ( i = 0; i < ctrls1.num_engines; ++i ) {
477 result.ctrls.master_bat[i] = ctrls1.master_bat[i];
478 result.ctrls.master_alt[i] = ctrls1.master_alt[i];
479 result.ctrls.magnetos[i] = ctrls1.magnetos[i];
480 result.ctrls.starter_power[i] = ctrls1.starter_power[i];
481 result.ctrls.throttle[i]
482 = weight( ctrls1.throttle[i], ctrls2.throttle[i], ratio );
483 result.ctrls.mixture[i]
484 = weight( ctrls1.mixture[i], ctrls2.mixture[i], ratio );
485 result.ctrls.fuel_pump_power[i] = ctrls1.fuel_pump_power[i];
486 result.ctrls.prop_advance[i]
487 = weight( ctrls1.prop_advance[i], ctrls2.prop_advance[i], ratio );
488 result.ctrls.engine_ok[i] = ctrls1.engine_ok[i];
489 result.ctrls.mag_left_ok[i] = ctrls1.mag_left_ok[i];
490 result.ctrls.mag_right_ok[i] = ctrls1.mag_right_ok[i];
491 result.ctrls.spark_plugs_ok[i] = ctrls1.spark_plugs_ok[i];
492 result.ctrls.oil_press_status[i] = ctrls1.oil_press_status[i];
493 result.ctrls.fuel_pump_ok[i] = ctrls1.fuel_pump_ok[i];
497 for ( i = 0; i < ctrls1.num_tanks; ++i ) {
498 result.ctrls.fuel_selector[i] = ctrls1.fuel_selector[i];
502 result.ctrls.brake_left
503 = weight( ctrls1.brake_left, ctrls2.brake_left, ratio );
504 result.ctrls.brake_right
505 = weight( ctrls1.brake_right, ctrls2.brake_right, ratio );
506 result.ctrls.brake_parking
507 = weight( ctrls1.brake_parking, ctrls2.brake_parking, ratio );
510 result.ctrls.gear_handle = ctrls1.gear_handle;
513 result.ctrls.turbulence_norm = ctrls1.turbulence_norm;
515 // wind and turbulance
516 result.ctrls.wind_speed_kt
517 = weight( ctrls1.wind_speed_kt, ctrls2.wind_speed_kt, ratio );
518 result.ctrls.wind_dir_deg
519 = weight( ctrls1.wind_dir_deg, ctrls2.wind_dir_deg, ratio );
520 result.ctrls.turbulence_norm
521 = weight( ctrls1.turbulence_norm, ctrls2.turbulence_norm, ratio );
523 // other information about environment
524 result.ctrls.hground = weight( ctrls1.hground, ctrls2.hground, ratio );
525 result.ctrls.magvar = weight( ctrls1.magvar, ctrls2.magvar, ratio );
527 // simulation control
528 result.ctrls.speedup = ctrls1.speedup;
529 result.ctrls.freeze = ctrls1.freeze;
535 * interpolate a specific time from a specific list
537 static void interpolate( double time, const replay_list_type &list ) {
539 if ( list.size() == 0 ) {
542 } else if ( list.size() == 1 ) {
543 // handle list size == 1
544 update_fdm( (*list[0]) );
548 unsigned int last = list.size() - 1;
549 unsigned int first = 0;
550 unsigned int mid = ( last + first ) / 2;
555 // cout << " " << first << " <=> " << last << endl;
556 if ( last == first ) {
558 } else if ( list[mid]->sim_time < time && list[mid+1]->sim_time < time ) {
561 mid = ( last + first ) / 2;
562 } else if ( list[mid]->sim_time > time && list[mid+1]->sim_time > time ) {
565 mid = ( last + first ) / 2;
571 FGReplayData result = interpolate( time, (*list[mid]), (*list[mid+1]) );
573 update_fdm( result );
578 * Replay a saved frame based on time, interpolate from the two
579 * nearest saved frames.
580 * Returns true when replay sequence has finished, false otherwise.
583 bool FGReplay::replay( double time ) {
584 // cout << "replay: " << time << " ";
585 // find the two frames to interpolate between
588 if ( short_term.size() > 0 ) {
589 t1 = short_term.back()->sim_time;
590 t2 = short_term.front()->sim_time;
592 // replay the most recent frame
593 update_fdm( (*short_term.back()) );
594 // replay is finished now
596 // cout << "first frame" << endl;
597 } else if ( time <= t1 && time >= t2 ) {
598 interpolate( time, short_term );
599 // cout << "from short term" << endl;
600 } else if ( medium_term.size() > 0 ) {
601 t1 = short_term.front()->sim_time;
602 t2 = medium_term.back()->sim_time;
603 if ( time <= t1 && time >= t2 ) {
604 FGReplayData result = interpolate( time,
605 (*medium_term.back()),
606 (*short_term.front()) );
607 update_fdm( result );
608 // cout << "from short/medium term" << endl;
610 t1 = medium_term.back()->sim_time;
611 t2 = medium_term.front()->sim_time;
612 if ( time <= t1 && time >= t2 ) {
613 interpolate( time, medium_term );
614 // cout << "from medium term" << endl;
615 } else if ( long_term.size() > 0 ) {
616 t1 = medium_term.front()->sim_time;
617 t2 = long_term.back()->sim_time;
618 if ( time <= t1 && time >= t2 ) {
619 FGReplayData result = interpolate( time,
621 (*medium_term.front()));
622 update_fdm( result );
623 // cout << "from medium/long term" << endl;
625 t1 = long_term.back()->sim_time;
626 t2 = long_term.front()->sim_time;
627 if ( time <= t1 && time >= t2 ) {
628 interpolate( time, long_term );
629 // cout << "from long term" << endl;
631 // replay the oldest long term frame
632 update_fdm( (*long_term.front()) );
633 // cout << "oldest long term frame" << endl;
637 // replay the oldest medium term frame
638 update_fdm( (*medium_term.front()) );
639 // cout << "oldest medium term frame" << endl;
643 // replay the oldest short term frame
644 update_fdm( (*short_term.front()) );
645 // cout << "oldest short term frame" << endl;
655 double FGReplay::get_start_time() {
656 if ( long_term.size() > 0 ) {
657 return (*long_term.front()).sim_time;
658 } else if ( medium_term.size() > 0 ) {
659 return (*medium_term.front()).sim_time;
660 } else if ( short_term.size() ) {
661 return (*short_term.front()).sim_time;
667 double FGReplay::get_end_time() {
668 if ( short_term.size() ) {
669 return (*short_term.back()).sim_time;