1 /*
2 * CRRCsim - the Charles River Radio Control Club Flight Simulator Project
3 * Copyright (C) 2010 - Jens Wilhelm Wulf (original author)
4 *
5 * This program is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License version 2
7 * as published by the Free Software Foundation.
8 *
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty off
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
13 *
14 * You should have received a copy of the GNU General Public License
15 * along with this program; if not, write to the Free Software
16 * Foundation, Inc., 59 Temple Place, Suite 330,
17 * Boston, MA 02111-1307, USA.
18 *
19 */
20
21 #include "fdm_playback.h"
22 #include "marker.h"
23 #include "robotfile.h"
24
25 #include <math.h>
26 #include <iostream>
27
28
update(TSimInputs * inputs,double dt,int multiloop)29 void CRRC_AirplaneSim_Playback::update(TSimInputs* inputs,
30 double dt,
31 int multiloop)
32 {
33 if (eF3FState != eF3F_WaitForUser)
34 {
35 CRRCMath::Vector3 v3PosNew;
36
37 dDeltaT -= dt * multiloop;
38
39 while ((dDeltaT < 0 || eF3FState == eF3F_Jump) && !infile.eof() && eF3FState != eF3F_WaitForUser)
40 {
41 int marker;
42 do
43 {
44 infile.read(buf, 1);
45 switch (buf[0])
46 {
47 case 0x02: // marker
48 marker = RobotFile::ReadInt32(infile);
49
50 switch (marker)
51 {
52 case RECMARK_F3F_START:
53 switch (eF3FState)
54 {
55 case eF3F_Prep:
56 eF3FState = eF3F_WaitForUser;
57 break;
58
59 default:
60 eF3FState = eF3F_Done;
61 break;
62 }
63 break;
64
65 default: // ignore
66 break;
67 }
68 break;
69
70 case 0x03: // xml
71 {
72 SimpleXMLTransfer* data = 0;
73 try
74 {
75 data = new SimpleXMLTransfer(infile);
76 }
77 catch (XMLException e)
78 {
79 data = 0;
80 }
81 // todo: tell someone about this data...
82 if (data)
83 delete data;
84 // skip trailing '\n'
85 infile.read(&(buf[1]), 1);
86 }
87 break;
88
89 case 0x00:
90 break;
91
92 default:
93 std::cerr << "unknown record type: " << (int)(buf[0]) << "\n";
94 break;
95 }
96 }
97 while (buf[0] != 0 && !infile.eof());
98
99 if (!infile.eof())
100 {
101 double timestep = RobotFile::ReadDouble(infile);
102 if (eF3FState != eF3F_Jump)
103 dDeltaT += timestep;
104 v3PosNew.r[0] = RobotFile::ReadFloat(infile);
105 v3PosNew.r[1] = RobotFile::ReadFloat(infile);
106 v3PosNew.r[2] = RobotFile::ReadFloat(infile);
107 v3Euler.r[0] = RobotFile::ReadInt16(infile) / ROBOT_EULER_TO_INT16;
108 v3Euler.r[1] = RobotFile::ReadInt16(infile) / ROBOT_EULER_TO_INT16;
109 v3Euler.r[2] = RobotFile::ReadInt16(infile) / ROBOT_EULER_TO_INT16;
110
111 if (fFirstPos)
112 {
113 v3PosOld = v3PosNew;
114 fFirstPos = false;
115 }
116
117 // Interpolate position. v3Euler can't be interpolated; this would
118 // be possible if a quaternion had been used, but is far too much
119 // trouble.
120 // v3PosNew is the current position, current time is dDeltaT.
121 // v3PosOld is the position of timestep ago. We need the position
122 // at time=zero.
123 v3Pos = v3PosOld + (v3PosNew - v3PosOld) * ((timestep-dDeltaT)/timestep);
124 v3PosOld = v3PosNew;
125 }
126 }
127 }
128 }
129
CRRC_AirplaneSim_Playback(const char * filename)130 CRRC_AirplaneSim_Playback::CRRC_AirplaneSim_Playback(const char* filename) : RobotBase()
131 {
132 header = 0;
133 infile.open(filename, std::ios::binary);
134 if (!infile)
135 {
136 throw XMLException("error opening infile");
137 }
138 // Read mandatory XML header
139 header = new SimpleXMLTransfer(infile);
140 // skip trailing '\n'
141 infile.read(buf, 1);
142
143 if (header->getName().compare("CRRCSim_record") != 0)
144 {
145 throw XMLException("wrong file format");
146 }
147 this->filename = filename;
148 }
149
150
~CRRC_AirplaneSim_Playback()151 CRRC_AirplaneSim_Playback::~CRRC_AirplaneSim_Playback()
152 {
153 if (header)
154 delete header;
155 infile.close();
156 }
157
initAirplaneState(double dRelVel,double dPhi,double dTheta,double dPsi,double X,double Y,double Z,double R_X,double R_Y,double R_Z)158 void CRRC_AirplaneSim_Playback::initAirplaneState(double dRelVel,
159 double dPhi,
160 double dTheta,
161 double dPsi,
162 double X,
163 double Y,
164 double Z,
165 double R_X,
166 double R_Y,
167 double R_Z)
168 {
169 dDeltaT = 0;
170 if (infile.eof())
171 {
172 infile.close();
173 infile.open(filename.c_str());
174 }
175 else
176 infile.seekg(0);
177 // read XML header
178 SimpleXMLTransfer* dummy = new SimpleXMLTransfer(infile);
179 delete dummy;
180 // skip trailing '\n'
181 infile.read(buf, 1);
182
183 eF3FState = eF3F_Off;
184 fFirstPos = true;
185 }
186
ReceiveMarker(int id)187 void CRRC_AirplaneSim_Playback::ReceiveMarker(int id)
188 {
189 switch (id)
190 {
191 case RECMARK_F3F_RESET:
192 eF3FState = eF3F_Prep;
193 break;
194
195 case RECMARK_F3F_START:
196 if (eF3FState == eF3F_Prep)
197 // fast forward: read file until marker is found
198 eF3FState = eF3F_Jump;
199 else
200 eF3FState = eF3F_Done;
201 break;
202 }
203 }
204