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  - curt@flightgear.org
   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 <Network/native_ctrls.hxx>
  28 #include <Network/native_fdm.hxx>
  29 #include <Network/net_ctrls.hxx>
  30 #include <Network/net_fdm.hxx>
  31
  32 #include "replay.hxx"
  33
  34 const double FGReplay::st_list_time = 60.0;   // 60 secs of high res data
  35 const double FGReplay::mt_list_time = 600.0;  // 10 mins of 1 fps data
  36 const double FGReplay::lt_list_time = 3600.0; // 1 hr of 10 spf data
  37
  38 // short term sample rate is as every frame
  39 const double FGReplay::mt_dt = 0.5; // medium term sample rate (sec)
  40 const double FGReplay::lt_dt = 5.0; // long term sample rate (sec)
  41
  42 /**
  43  * Constructor
  44  */
  45
  46 FGReplay::FGReplay() {
  47 }
  48
  49
  50 /**
  51  * Destructor
  52  */
  53
  54 FGReplay::~FGReplay() {
  55     // no dynamically allocated memory to free
  56 }
  57
  58
  59 /**
  60  * Initialize the data structures
  61  */
  62
  63 void FGReplay::init() {
  64     sim_time = 0.0;
  65     last_mt_time = 0.0;
  66     last_lt_time = 0.0;
  67
  68     // Make sure all queues are flushed
  69     while ( !short_term.empty() ) {
  70         short_term.pop_front();
  71     }
  72     while ( !medium_term.empty() ) {
  73         medium_term.pop_front();
  74     }
  75     while ( !medium_term.empty() ) {
  76         medium_term.pop_front();
  77     }
  78 }
  79
  80
  81 /**
  82  * Bind to the property tree
  83  */
  84
  85 void FGReplay::bind() {
  86     // nothing to bind
  87 }
  88
  89
  90 /**
  91  *  Unbind from the property tree
  92  */
  93
  94 void FGReplay::unbind() {
  95     // nothing to unbind
  96 }
  97
  98
  99 /**
 100  *  Update the saved data
 101  */
 102
 103 void FGReplay::update( double dt ) {
 104
 105     if ( dt <= 0 ) {
 106         // don't save data if nothing is going on ...
 107
 108         return;
 109     }
 110
 111     sim_time += dt;
 112
 113     // build the replay record
 114     FGNetFDM f;
 115     FGProps2NetFDM( &f, false );
 116
 117     // sanity check, don't collect data if FDM data isn't good
 118     if ( !cur_fdm_state->get_inited() ) {
 119         return;
 120     }
 121
 122     FGNetCtrls c;
 123     FGProps2NetCtrls( &c, false, false );
 124
 125     FGReplayData r;
 126     r.sim_time = sim_time;
 127     r.ctrls = c;
 128     r.fdm = f;
 129
 130     // update the short term list
 131     short_term.push_back( r );
 132
 133     FGReplayData st_front = short_term.front();
 134     if ( sim_time - st_front.sim_time > st_list_time ) {
 135         while ( sim_time - st_front.sim_time > st_list_time ) {
 136             st_front = short_term.front();
 137             short_term.pop_front();
 138         }
 139
 140         // update the medium term list
 141         if ( sim_time - last_mt_time > mt_dt ) {
 142             last_mt_time = sim_time;
 143             medium_term.push_back( st_front );
 144
 145             FGReplayData mt_front = medium_term.front();
 146             if ( sim_time - mt_front.sim_time > mt_list_time ) {
 147                 while ( sim_time - mt_front.sim_time > mt_list_time ) {
 148                     mt_front = medium_term.front();
 149                     medium_term.pop_front();
 150                 }
 151
 152                 // update the long term list
 153                 if ( sim_time - last_lt_time > lt_dt ) {
 154                     last_lt_time = sim_time;
 155                     long_term.push_back( mt_front );
 156
 157                     FGReplayData lt_front = long_term.front();
 158                     if ( sim_time - lt_front.sim_time > lt_list_time ) {
 159                         while ( sim_time - lt_front.sim_time > lt_list_time ) {
 160                             lt_front = long_term.front();
 161                             long_term.pop_front();
 162                         }
 163                     }
 164                 }
 165             }
 166         }
 167     }
 168
 169 #if 0
 170     cout << "short term size = " << short_term.size()
 171          << "  time = " << sim_time - short_term.front().sim_time
 172          << endl;
 173     cout << "medium term size = " << medium_term.size()
 174          << "  time = " << sim_time - medium_term.front().sim_time
 175          << endl;
 176     cout << "long term size = " << long_term.size()
 177          << "  time = " << sim_time - long_term.front().sim_time
 178          << endl;
 179 #endif
 180 }
 181
 182
 183 static double weight( double data1, double data2, double ratio,
 184                       bool rotational = false ) {
 185     if ( rotational ) {
 186         // special handling of rotational data
 187         double tmp = data2 - data1;
 188         if ( tmp > SGD_PI ) {
 189             tmp -= SGD_2PI;
 190         } else if ( tmp < -SGD_PI ) {
 191             tmp += SGD_2PI;
 192         }
 193         return data1 + tmp * ratio;
 194     } else {
 195         // normal "linear" data
 196         return data1 + ( data2 - data1 ) * ratio;
 197     }
 198 }
 199
 200 /**
 201  * given two FGReplayData elements and a time, interpolate between them
 202  */
 203 static void update_fdm( FGReplayData frame ) {
 204     FGNetFDM2Props( &frame.fdm, false );
 205     FGNetCtrls2Props( &frame.ctrls, false, false );
 206 }
 207
 208 /**
 209  * given two FGReplayData elements and a time, interpolate between them
 210  */
 211 static FGReplayData interpolate( double time, FGReplayData f1, FGReplayData f2 )
 212 {
 213     FGReplayData result = f1;
 214
 215     FGNetFDM fdm1 = f1.fdm;
 216     FGNetFDM fdm2 = f2.fdm;
 217
 218     double ratio = (time - f1.sim_time) / (f2.sim_time - f1.sim_time);
 219
 220     cout << fdm1.longitude << " " << fdm2.longitude << endl;
 221     result.fdm.longitude = weight( fdm1.longitude, fdm2.longitude, ratio );
 222     result.fdm.latitude = weight( fdm1.latitude, fdm2.latitude, ratio );
 223     result.fdm.altitude = weight( fdm1.altitude, fdm2.altitude, ratio );
 224     result.fdm.agl = weight( fdm1.agl, fdm2.agl, ratio );
 225     result.fdm.phi = weight( fdm1.phi, fdm2.phi, ratio, true );
 226     result.fdm.theta = weight( fdm1.theta, fdm2.theta, ratio, true );
 227     result.fdm.psi = weight( fdm1.psi, fdm2.psi, ratio, true );
 228
 229     result.fdm.phidot = weight( fdm1.phidot, fdm2.phidot, ratio, true );
 230     result.fdm.thetadot = weight( fdm1.thetadot, fdm2.thetadot, ratio, true );
 231     result.fdm.psidot = weight( fdm1.psidot, fdm2.psidot, ratio, true );
 232     result.fdm.vcas = weight( fdm1.vcas, fdm2.vcas, ratio );
 233     result.fdm.climb_rate = weight( fdm1.climb_rate, fdm2.climb_rate, ratio );
 234     result.fdm.v_north = weight( fdm1.v_north, fdm2.v_north, ratio );
 235     result.fdm.v_east = weight( fdm1.v_east, fdm2.v_east, ratio );
 236     result.fdm.v_down = weight( fdm1.v_down, fdm2.v_down, ratio );
 237
 238     result.fdm.v_wind_body_north
 239         = weight( fdm1.v_wind_body_north, fdm2.v_wind_body_north, ratio );
 240     result.fdm.v_wind_body_east
 241         = weight( fdm1.v_wind_body_east, fdm2.v_wind_body_east, ratio );
 242     result.fdm.v_wind_body_down
 243         = weight( fdm1.v_wind_body_down, fdm2.v_wind_body_down, ratio );
 244
 245     result.fdm.stall_warning
 246         = weight( fdm1.stall_warning, fdm2.stall_warning, ratio );
 247
 248     result.fdm.A_X_pilot = weight( fdm1.A_X_pilot, fdm2.A_X_pilot, ratio );
 249     result.fdm.A_Y_pilot = weight( fdm1.A_Y_pilot, fdm2.A_Y_pilot, ratio );
 250     result.fdm.A_Z_pilot = weight( fdm1.A_Z_pilot, fdm2.A_Z_pilot, ratio );
 251
 252     return result;
 253 }
 254
 255 /**
 256  * interpolate a specific time from a specific list
 257  */
 258 static void interpolate( double time, replay_list_type list ) {
 259     // sanity checking
 260     if ( list.size() == 0 ) {
 261         // handle empty list
 262         return;
 263     } else if ( list.size() == 1 ) {
 264         // handle list size == 1
 265         update_fdm( list[0] );
 266         return;
 267     }
 268
 269     unsigned int last = list.size() - 1;
 270     unsigned int first = 0;
 271     unsigned int mid = ( last + first ) / 2;
 272
 273
 274     bool done = false;
 275     while ( !done ) {
 276         // cout << "  " << first << " <=> " << last << endl;
 277         if ( last == first ) {
 278             done = true;
 279         } else if ( list[mid].sim_time < time && list[mid+1].sim_time < time ) {
 280             // too low
 281             first = mid;
 282             mid = ( last + first ) / 2;
 283         } else if ( list[mid].sim_time > time && list[mid+1].sim_time > time ) {
 284             // too high
 285             last = mid;
 286             mid = ( last + first ) / 2;
 287         } else {
 288             done = true;
 289         }
 290     }
 291
 292     FGReplayData result = interpolate( time, list[mid], list[mid+1] );
 293
 294     update_fdm( result );
 295 }
 296
 297
 298 /**
 299  *  Replay a saved frame based on time, interpolate from the two
 300  *  nearest saved frames.
 301  */
 302
 303 void FGReplay::replay( double time ) {
 304     cout << "replay: " << time << " ";
 305     // find the two frames to interpolate between
 306     double t1, t2;
 307
 308     if ( short_term.size() > 0 ) {
 309         t1 = short_term.back().sim_time;
 310         t2 = short_term.front().sim_time;
 311         if ( time > t1 ) {
 312             // replay the most recent frame
 313             update_fdm( short_term.back() );
 314             cout << "first frame" << endl;
 315         } else if ( time <= t1 && time >= t2 ) {
 316             interpolate( time, short_term );
 317             cout << "from short term" << endl;
 318         } else if ( medium_term.size() > 0 ) {
 319             t1 = short_term.front().sim_time;
 320             t2 = medium_term.back().sim_time;
 321             if ( time <= t1 && time >= t2 ) {
 322                 FGReplayData result = interpolate( time,
 323                                                    medium_term.back(),
 324                                                    short_term.front() );
 325                 update_fdm( result );
 326                 cout << "from short/medium term" << endl;
 327             } else {
 328                 t1 = medium_term.back().sim_time;
 329                 t2 = medium_term.front().sim_time;
 330                 if ( time <= t1 && time >= t2 ) {
 331                     interpolate( time, medium_term );
 332                     cout << "from medium term" << endl;
 333                 } else if ( long_term.size() > 0 ) {
 334                     t1 = medium_term.front().sim_time;
 335                     t2 = long_term.back().sim_time;
 336                     if ( time <= t1 && time >= t2 ) {
 337                         FGReplayData result = interpolate( time,
 338                                                            long_term.back(),
 339                                                            medium_term.front());
 340                         update_fdm( result );
 341                        cout << "from medium/long term" << endl;
 342                     } else {
 343                         t1 = long_term.back().sim_time;
 344                         t2 = long_term.front().sim_time;
 345                         if ( time <= t1 && time >= t2 ) {
 346                             interpolate( time, long_term );
 347                             cout << "from long term" << endl;
 348                         } else {
 349                             // replay the oldest long term frame
 350                             update_fdm( long_term.front() );
 351                             cout << "oldest long term frame" << endl;
 352                         }
 353                     }
 354                 } else {
 355                     // replay the oldest medium term frame
 356                     update_fdm( medium_term.front() );
 357                     cout << "oldest medium term frame" << endl;
 358                 }
 359             }
 360         } else {
 361             // replay the oldest short term frame
 362             update_fdm( short_term.front() );
 363             cout << "oldest short term frame" << endl;
 364         }
 365     } else {
 366         // nothing to replay
 367     }
 368 }
 369
 370
 371 double FGReplay::get_start_time() {
 372     if ( long_term.size() > 0 ) {
 373         return long_term.front().sim_time;
 374     } else if ( medium_term.size() > 0 ) {
 375         return medium_term.front().sim_time;
 376     } else if ( short_term.size() ) {
 377         return short_term.front().sim_time;
 378     } else {
 379         return 0.0;
 380     }
 381 }
 382
 383 double FGReplay::get_end_time() {
 384     if ( short_term.size() ) {
 385         return short_term.back().sim_time;
 386     } else {
 387         return 0.0;
 388     }
 389 }