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