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