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