3 // Written by Thorsten Brehm, started August 2011.
5 // Copyright (C) 2011 Thorsten Brehm - brehmt (at) gmail com
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., 675 Mass Ave, Cambridge, MA 02139, USA.
21 ///////////////////////////////////////////////////////////////////////////////
27 #include <simgear/debug/logstream.hxx>
28 #include <simgear/props/props_io.hxx>
29 #include <simgear/misc/ResourceManager.hxx>
30 #include <simgear/misc/strutils.hxx>
31 #include <simgear/structure/exception.hxx>
32 #include <Main/fg_props.hxx>
33 #include "flightrecorder.hxx"
35 using namespace FlightRecorder;
37 FGFlightRecorder::FGFlightRecorder(const char* pConfigName) :
38 m_RecorderNode(fgGetNode("/sim/flight-recorder", true)),
40 m_ConfigName(pConfigName)
44 FGFlightRecorder::~FGFlightRecorder()
49 FGFlightRecorder::reinit(void)
53 m_TotalRecordSize = 0;
55 m_CaptureDouble.clear();
56 m_CaptureFloat.clear();
57 m_CaptureInteger.clear();
58 m_CaptureInt16.clear();
59 m_CaptureInt8.clear();
60 m_CaptureBool.clear();
62 int Selected = m_RecorderNode->getIntValue(m_ConfigName, 0);
63 SG_LOG(SG_SYSTEMS, SG_INFO, "FlightRecorder: Recorder configuration #" << Selected);
65 m_ConfigNode = m_RecorderNode->getChild("config", Selected);
67 if (!m_ConfigNode.valid())
70 if (!m_ConfigNode.valid())
72 SG_LOG(SG_SYSTEMS, SG_ALERT, "FlightRecorder: Configuration is invalid. Flight recorder disabled.");
76 // set name of active flight recorder type
77 const char* pRecorderName =
78 m_ConfigNode->getStringValue("name",
79 "aircraft-specific flight recorder");
80 SG_LOG(SG_SYSTEMS, SG_INFO, "FlightRecorder: Using custom recorder configuration: " << pRecorderName);
81 m_RecorderNode->setStringValue("active-config-name", pRecorderName);
84 initSignalList("double", m_CaptureDouble, m_ConfigNode );
85 initSignalList("float", m_CaptureFloat , m_ConfigNode );
86 initSignalList("int", m_CaptureInteger, m_ConfigNode );
87 initSignalList("int16", m_CaptureInt16 , m_ConfigNode );
88 initSignalList("int8", m_CaptureInt8 , m_ConfigNode );
89 initSignalList("bool", m_CaptureBool , m_ConfigNode );
92 // calculate size of a single record
93 m_TotalRecordSize = sizeof(double) * 1 /* sim time */ +
94 sizeof(double) * m_CaptureDouble.size() +
95 sizeof(float) * m_CaptureFloat.size() +
96 sizeof(int) * m_CaptureInteger.size() +
97 sizeof(short int) * m_CaptureInt16.size() +
98 sizeof(signed char) * m_CaptureInt8.size() +
99 sizeof(unsigned char) * ((m_CaptureBool.size()+7)/8); // 8 bools per byte
101 // expose size of actual flight recorder record
102 m_RecorderNode->setIntValue("record-size", m_TotalRecordSize);
103 SG_LOG(SG_SYSTEMS, SG_INFO, "FlightRecorder: record size is " << m_TotalRecordSize << " bytes");
106 /** Check if SignalList already contains the given property */
108 FGFlightRecorder::haveProperty(FlightRecorder::TSignalList& SignalList,SGPropertyNode* pProperty)
110 unsigned int Count = SignalList.size();
111 for (unsigned int i=0; i<Count; i++)
113 if (SignalList[i].Signal.get() == pProperty)
121 /** Check if any signal list already contains the given property */
123 FGFlightRecorder::haveProperty(SGPropertyNode* pProperty)
125 if (haveProperty(m_CaptureDouble, pProperty))
127 if (haveProperty(m_CaptureFloat, pProperty))
129 if (haveProperty(m_CaptureInteger, pProperty))
131 if (haveProperty(m_CaptureInt16, pProperty))
133 if (haveProperty(m_CaptureInt8, pProperty))
135 if (haveProperty(m_CaptureBool, pProperty))
140 /** Read default flight-recorder configuration.
141 * Default should match properties as hard coded for versions up to FG2.4.0. */
143 FGFlightRecorder::initDefault(void)
145 // set name of active flight recorder type
146 SG_LOG(SG_SYSTEMS, SG_INFO, "FlightRecorder: No custom configuration. Loading generic default recorder.");
148 const char* Path = m_RecorderNode->getStringValue("default-config",NULL);
151 SG_LOG(SG_SYSTEMS, SG_ALERT, "FlightRecorder: No default flight recorder specified! Check preferences.xml!");
155 SGPath path = globals->resolve_aircraft_path(Path);
158 SG_LOG(SG_SYSTEMS, SG_ALERT, "FlightRecorder: Cannot find file '" << Path << "'.");
164 readProperties(path.str(), m_RecorderNode->getChild("config", 0 ,true), 0);
165 m_ConfigNode = m_RecorderNode->getChild("config", 0 ,false);
166 } catch (sg_io_exception &e)
168 SG_LOG(SG_SYSTEMS, SG_ALERT, "FlightRecorder: Error reading file '" <<
169 Path << ": " << e.getFormattedMessage());
175 /** Read signal list below given base node.
176 * Only process properties of given signal type and add all signals to the given list.
177 * This method is called for all supported signal types - properties of each type are
178 * kept in separate lists for efficiency reasons. */
180 FGFlightRecorder::initSignalList(const char* pSignalType, TSignalList& SignalList, SGPropertyNode_ptr BaseNode)
185 processSignalList(pSignalType, SignalList, BaseNode);
187 SG_LOG(SG_SYSTEMS, SG_DEBUG, "FlightRecorder: " << SignalList.size() << " signals of type " << pSignalType );
190 /** Process signal list below given base node.
191 * Only process properties of given signal type and add all signals to the given list.
192 * This method is called for all supported signal types - properties of each type are
193 * kept in separate lists for efficiency reasons. */
195 FGFlightRecorder::processSignalList(const char* pSignalType, TSignalList& SignalList, SGPropertyNode_ptr SignalListNode,
196 string PropPrefix, int Count)
198 // get the list of signal sources (property paths) for this signal type
199 SGPropertyNode_ptr SignalNode;
202 Count = SignalListNode->getIntValue("count",Count);
203 PropPrefix = simgear::strutils::strip(SignalListNode->getStringValue("prefix",PropPrefix.c_str()));
204 if ((!PropPrefix.empty())&&(PropPrefix[PropPrefix.size()-1] != '/'))
209 SignalNode = SignalListNode->getChild("signal",Index,false);
210 if (SignalNode.valid()&&
211 (0==strcmp(pSignalType, SignalNode->getStringValue("type","float"))))
213 string PropertyPath = SignalNode->getStringValue("property","");
214 if (!PropertyPath.empty())
216 PropertyPath = PropPrefix + PropertyPath;
217 const char* pInterpolation = SignalNode->getStringValue("interpolation","linear");
219 // Check if current signal has a "%i" place holder. Otherwise count is 1.
220 string::size_type IndexPos = PropertyPath.find("%i");
221 int SignalCount = Count;
222 if (IndexPos == string::npos)
225 for (int IndexValue=0;IndexValue<SignalCount;IndexValue++)
227 string PPath = PropertyPath;
228 if (IndexPos != string::npos)
231 snprintf(strbuf, 20, "%d", IndexValue);
232 PPath = PPath.replace(IndexPos,2,strbuf);
235 Capture.Signal = fgGetNode(PPath.c_str(),false);
236 if (!Capture.Signal.valid())
238 // warn user: we're maybe going to record useless data
239 // Or maybe the data is only initialized later. Warn anyway, so we can catch useless data.
240 SG_LOG(SG_SYSTEMS, SG_INFO, "FlightRecorder: Recording non-existent property '" << PPath << "'.");
241 Capture.Signal = fgGetNode(PPath.c_str(),true);
244 if (0==strcmp(pInterpolation,"discrete"))
245 Capture.Interpolation = discrete;
247 if ((0==strcmp(pInterpolation,"angular"))||
248 (0==strcmp(pInterpolation,"angular-rad")))
249 Capture.Interpolation = angular_rad;
251 if (0==strcmp(pInterpolation,"angular-deg"))
252 Capture.Interpolation = angular_deg;
254 if (0==strcmp(pInterpolation,"linear"))
255 Capture.Interpolation = linear;
258 SG_LOG(SG_SYSTEMS, SG_ALERT, "FlightRecorder: Unsupported interpolation type '"
259 << pInterpolation<< "' of signal '" << PPath << "'");
260 Capture.Interpolation = linear;
262 if (haveProperty(Capture.Signal))
264 SG_LOG(SG_SYSTEMS, SG_ALERT, "FlightRecorder: Property '"
265 << PPath << "' specified multiple times. Check flight recorder configuration.");
268 SignalList.push_back(Capture);
273 } while (SignalNode.valid());
275 // allow recursive definition of signal lists
276 simgear::PropertyList Nodes = SignalListNode->getChildren("signals");
277 for (unsigned int i=0;i<Nodes.size();i++)
279 processSignalList(pSignalType, SignalList, Nodes[i], PropPrefix, Count);
283 /** Get an empty container for a single capture. */
285 FGFlightRecorder::createEmptyRecord(void)
287 if (!m_TotalRecordSize)
289 FGReplayData* p = (FGReplayData*) new unsigned char[m_TotalRecordSize];
293 /** Free given container with capture data. */
295 FGFlightRecorder::deleteRecord(FGReplayData* pRecord)
301 * When pBuffer==NULL new memory is allocated.
302 * If pBuffer!=NULL memory of given buffer is reused.
305 FGFlightRecorder::capture(double SimTime, FGReplayData* pRecycledBuffer)
307 if (!pRecycledBuffer)
309 pRecycledBuffer = createEmptyRecord();
310 if (!pRecycledBuffer)
313 unsigned char* pBuffer = (unsigned char*) pRecycledBuffer;
316 pRecycledBuffer->sim_time = SimTime;
317 Offset += sizeof(double);
319 // 64bit aligned data first!
322 double* pDoubles = (double*) &pBuffer[Offset];
323 unsigned int SignalCount = m_CaptureDouble.size();
324 for (unsigned int i=0; i<SignalCount; i++)
326 pDoubles[i] = m_CaptureDouble[i].Signal->getDoubleValue();
328 Offset += SignalCount * sizeof(double);
331 // 32bit aligned data comes second...
334 float* pFloats = (float*) &pBuffer[Offset];
335 unsigned int SignalCount = m_CaptureFloat.size();
336 for (unsigned int i=0; i<SignalCount; i++)
338 pFloats[i] = m_CaptureFloat[i].Signal->getFloatValue();
340 Offset += SignalCount * sizeof(float);
344 // capture integers (32bit aligned)
345 int* pInt = (int*) &pBuffer[Offset];
346 unsigned int SignalCount = m_CaptureInteger.size();
347 for (unsigned int i=0; i<SignalCount; i++)
349 pInt[i] = m_CaptureInteger[i].Signal->getIntValue();
351 Offset += SignalCount * sizeof(int);
354 // 16bit aligned data is next...
356 // capture 16bit short integers
357 short int* pShortInt = (short int*) &pBuffer[Offset];
358 unsigned int SignalCount = m_CaptureInt16.size();
359 for (unsigned int i=0; i<SignalCount; i++)
361 pShortInt[i] = (short int) m_CaptureInt16[i].Signal->getIntValue();
363 Offset += SignalCount * sizeof(short int);
366 // finally: byte aligned data is last...
368 // capture 8bit chars
369 signed char* pChar = (signed char*) &pBuffer[Offset];
370 unsigned int SignalCount = m_CaptureInt8.size();
371 for (unsigned int i=0; i<SignalCount; i++)
373 pChar[i] = (signed char) m_CaptureInt8[i].Signal->getIntValue();
375 Offset += SignalCount * sizeof(signed char);
379 // capture 1bit booleans (8bit aligned)
380 unsigned char* pFlags = (unsigned char*) &pBuffer[Offset];
381 unsigned int SignalCount = m_CaptureBool.size();
382 int Size = (SignalCount+7)/8;
384 memset(pFlags,0,Size);
385 for (unsigned int i=0; i<SignalCount; i++)
387 if (m_CaptureBool[i].Signal->getBoolValue())
388 pFlags[i>>3] |= 1 << (i&7);
392 assert(Offset == m_TotalRecordSize);
394 return (FGReplayData*) pBuffer;
397 /** Do interpolation as defined by given interpolation type and weighting ratio. */
399 weighting(TInterpolation interpolation, double ratio, double v1,double v2)
401 switch (interpolation)
404 return v1 + ratio*(v2-v1);
408 // special handling of angular data
409 double tmp = v2 - v1;
412 else if ( tmp < -180 )
414 return v1 + tmp * ratio;
419 // special handling of angular data
420 double tmp = v2 - v1;
423 else if ( tmp < -SGD_PI )
425 return v1 + tmp * ratio;
436 * Restore all properties with data from given buffer. */
438 FGFlightRecorder::replay(double SimTime, const FGReplayData* _pNextBuffer, const FGReplayData* _pLastBuffer)
440 const char* pLastBuffer = (const char*) _pLastBuffer;
441 const char* pBuffer = (const char*) _pNextBuffer;
449 double NextSimTime = _pNextBuffer->sim_time;
450 double LastSimTime = _pLastBuffer->sim_time;
451 ratio = (SimTime - LastSimTime) / (NextSimTime - LastSimTime);
458 Offset += sizeof(double);
460 // 64bit aligned data first!
463 const double* pDoubles = (const double*) &pBuffer[Offset];
464 const double* pLastDoubles = (const double*) &pLastBuffer[Offset];
465 unsigned int SignalCount = m_CaptureDouble.size();
466 for (unsigned int i=0; i<SignalCount; i++)
468 double v = pDoubles[i];
471 v = weighting(m_CaptureDouble[i].Interpolation, ratio,
474 m_CaptureDouble[i].Signal->setDoubleValue(v);
476 Offset += SignalCount * sizeof(double);
479 // 32bit aligned data comes second...
482 const float* pFloats = (const float*) &pBuffer[Offset];
483 const float* pLastFloats = (const float*) &pLastBuffer[Offset];
484 unsigned int SignalCount = m_CaptureFloat.size();
485 for (unsigned int i=0; i<SignalCount; i++)
487 float v = pFloats[i];
490 v = weighting(m_CaptureFloat[i].Interpolation, ratio,
493 m_CaptureFloat[i].Signal->setDoubleValue(v);//setFloatValue
495 Offset += SignalCount * sizeof(float);
499 // restore integers (32bit aligned)
500 const int* pInt = (const int*) &pBuffer[Offset];
501 unsigned int SignalCount = m_CaptureInteger.size();
502 for (unsigned int i=0; i<SignalCount; i++)
504 m_CaptureInteger[i].Signal->setIntValue(pInt[i]);
506 Offset += SignalCount * sizeof(int);
509 // 16bit aligned data is next...
511 // restore 16bit short integers
512 const short int* pShortInt = (const short int*) &pBuffer[Offset];
513 unsigned int SignalCount = m_CaptureInt16.size();
514 for (unsigned int i=0; i<SignalCount; i++)
516 m_CaptureInt16[i].Signal->setIntValue(pShortInt[i]);
518 Offset += SignalCount * sizeof(short int);
521 // finally: byte aligned data is last...
523 // restore 8bit chars
524 const signed char* pChar = (const signed char*) &pBuffer[Offset];
525 unsigned int SignalCount = m_CaptureInt8.size();
526 for (unsigned int i=0; i<SignalCount; i++)
528 m_CaptureInt8[i].Signal->setIntValue(pChar[i]);
530 Offset += SignalCount * sizeof(signed char);
534 // restore 1bit booleans (8bit aligned)
535 const unsigned char* pFlags = (const unsigned char*) &pBuffer[Offset];
536 unsigned int SignalCount = m_CaptureBool.size();
537 int Size = (SignalCount+7)/8;
539 for (unsigned int i=0; i<SignalCount; i++)
541 m_CaptureBool[i].Signal->setBoolValue(0 != (pFlags[i>>3] & (1 << (i&7))));