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