1 // replay.cxx - a system to record and replay FlightGear flights
3 // Written by Curtis Olson, started Juley 2003.
5 // Copyright (C) 2003 Curtis L. Olson - http://www.flightgear.org/~curt
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.
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.
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.
27 #include <simgear/constants.h>
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>
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
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)
50 FGReplay::FGReplay() {
58 FGReplay::~FGReplay() {
59 // no dynamically allocated memory to free
64 * Initialize the data structures
67 void FGReplay::init() {
72 // Make sure all queues are flushed
73 while ( !short_term.empty() ) {
74 short_term.pop_front();
76 while ( !medium_term.empty() ) {
77 medium_term.pop_front();
79 while ( !medium_term.empty() ) {
80 medium_term.pop_front();
86 * Bind to the property tree
89 void FGReplay::bind() {
90 disable_replay = fgGetNode( "/sim/replay/disable", true );
95 * Unbind from the property tree
98 void FGReplay::unbind() {
104 * Update the saved data
107 void FGReplay::update( double dt ) {
108 static SGPropertyNode *replay_master
109 = fgGetNode( "/sim/freeze/replay", true );
111 if( disable_replay->getBoolValue() ) {
112 if( sim_time != 0.0 ) {
113 // we were recording data
119 if ( replay_master->getBoolValue() ) {
120 // don't record the replay session
126 // build the replay record
128 FGProps2NetFDM( &f, false );
130 // sanity check, don't collect data if FDM data isn't good
131 if ( !cur_fdm_state->get_inited() ) {
136 FGProps2NetCtrls( &c, false, false );
139 r.sim_time = sim_time;
143 // update the short term list
144 short_term.push_back( r );
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();
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 );
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();
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 );
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();
183 cout << "short term size = " << short_term.size()
184 << " time = " << sim_time - short_term.front().sim_time
186 cout << "medium term size = " << medium_term.size()
187 << " time = " << sim_time - medium_term.front().sim_time
189 cout << "long term size = " << long_term.size()
190 << " time = " << sim_time - long_term.front().sim_time
196 static double weight( double data1, double data2, double ratio,
197 bool rotational = false ) {
199 // special handling of rotational data
200 double tmp = data2 - data1;
201 if ( tmp > SGD_PI ) {
203 } else if ( tmp < -SGD_PI ) {
206 return data1 + tmp * ratio;
208 // normal "linear" data
209 return data1 + ( data2 - data1 ) * ratio;
214 * given two FGReplayData elements and a time, interpolate between them
216 static void update_fdm( FGReplayData frame ) {
217 FGNetFDM2Props( &frame.fdm, false );
218 FGNetCtrls2Props( &frame.ctrls, false, false );
222 * given two FGReplayData elements and a time, interpolate between them
224 static FGReplayData interpolate( double time, FGReplayData f1, FGReplayData f2 )
226 FGReplayData result = f1;
228 FGNetFDM fdm1 = f1.fdm;
229 FGNetFDM fdm2 = f2.fdm;
231 FGNetCtrls ctrls1 = f1.ctrls;
232 FGNetCtrls ctrls2 = f2.ctrls;
234 double ratio = (time - f1.sim_time) / (f2.sim_time - f1.sim_time);
236 // Interpolate FDM data
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 );
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 );
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 );
265 result.fdm.stall_warning
266 = weight( fdm1.stall_warning, fdm2.stall_warning, ratio );
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 );
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 );
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 );
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],
311 result.fdm.cur_time = fdm1.cur_time;
312 result.fdm.warp = fdm1.warp;
313 result.fdm.visibility = weight( fdm1.visibility, fdm2.visibility, ratio );
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 );
327 // Interpolate Control input data
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;
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];
365 for ( i = 0; i < ctrls1.num_tanks; ++i ) {
366 result.ctrls.fuel_selector[i] = ctrls1.fuel_selector[i];
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 );
378 result.ctrls.gear_handle = ctrls1.gear_handle;
381 result.ctrls.turbulence_norm = ctrls1.turbulence_norm;
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 );
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 );
395 // simulation control
396 result.ctrls.speedup = ctrls1.speedup;
397 result.ctrls.freeze = ctrls1.freeze;
403 * interpolate a specific time from a specific list
405 static void interpolate( double time, const replay_list_type &list ) {
407 if ( list.size() == 0 ) {
410 } else if ( list.size() == 1 ) {
411 // handle list size == 1
412 update_fdm( list[0] );
416 unsigned int last = list.size() - 1;
417 unsigned int first = 0;
418 unsigned int mid = ( last + first ) / 2;
423 // cout << " " << first << " <=> " << last << endl;
424 if ( last == first ) {
426 } else if ( list[mid].sim_time < time && list[mid+1].sim_time < time ) {
429 mid = ( last + first ) / 2;
430 } else if ( list[mid].sim_time > time && list[mid+1].sim_time > time ) {
433 mid = ( last + first ) / 2;
439 FGReplayData result = interpolate( time, list[mid], list[mid+1] );
441 update_fdm( result );
446 * Replay a saved frame based on time, interpolate from the two
447 * nearest saved frames.
450 void FGReplay::replay( double time ) {
451 // cout << "replay: " << time << " ";
452 // find the two frames to interpolate between
455 if ( short_term.size() > 0 ) {
456 t1 = short_term.back().sim_time;
457 t2 = short_term.front().sim_time;
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,
471 short_term.front() );
472 update_fdm( result );
473 // cout << "from short/medium term" << endl;
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,
486 medium_term.front());
487 update_fdm( result );
488 // cout << "from medium/long term" << endl;
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;
496 // replay the oldest long term frame
497 update_fdm( long_term.front() );
498 // cout << "oldest long term frame" << endl;
502 // replay the oldest medium term frame
503 update_fdm( medium_term.front() );
504 // cout << "oldest medium term frame" << endl;
508 // replay the oldest short term frame
509 update_fdm( short_term.front() );
510 // cout << "oldest short term frame" << endl;
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;
530 double FGReplay::get_end_time() {
531 if ( short_term.size() ) {
532 return short_term.back().sim_time;