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., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA.
21 ///////////////////////////////////////////////////////////////////////////////
31 #include <simgear/debug/logstream.hxx>
32 #include <simgear/props/props_io.hxx>
33 #include <simgear/misc/ResourceManager.hxx>
34 #include <simgear/misc/strutils.hxx>
35 #include <simgear/structure/exception.hxx>
36 #include <simgear/math/SGMath.hxx>
37 #include <Main/fg_props.hxx>
38 #include "flightrecorder.hxx"
40 using namespace FlightRecorder;
43 FGFlightRecorder::FGFlightRecorder(const char* pConfigName) :
44 m_RecorderNode(fgGetNode("/sim/flight-recorder", true)),
46 m_ConfigName(pConfigName),
47 m_usingDefaultConfig(false)
51 FGFlightRecorder::~FGFlightRecorder()
56 FGFlightRecorder::reinit(void)
59 m_usingDefaultConfig = false;
61 SGPropertyNode_ptr ConfigNode;
62 int Selected = m_RecorderNode->getIntValue(m_ConfigName, 0);
63 SG_LOG(SG_SYSTEMS, SG_INFO, "FlightRecorder: Recorder configuration #" << Selected);
65 ConfigNode = m_RecorderNode->getChild("config", Selected);
67 if (!ConfigNode.valid())
68 ConfigNode = getDefault();
74 FGFlightRecorder::reinit(SGPropertyNode_ptr ConfigNode)
76 m_TotalRecordSize = 0;
78 m_CaptureDouble.clear();
79 m_CaptureFloat.clear();
80 m_CaptureInteger.clear();
81 m_CaptureInt16.clear();
82 m_CaptureInt8.clear();
83 m_CaptureBool.clear();
85 m_ConfigNode = ConfigNode;
87 if (!m_ConfigNode.valid())
89 SG_LOG(SG_SYSTEMS, SG_ALERT, "FlightRecorder: Configuration is invalid. Flight recorder disabled.");
93 // set name of active flight recorder type
94 const char* pRecorderName =
95 m_ConfigNode->getStringValue("name",
96 "aircraft-specific flight recorder");
97 SG_LOG(SG_SYSTEMS, SG_INFO, "FlightRecorder: Using custom recorder configuration: " << pRecorderName);
98 m_RecorderNode->setStringValue("active-config-name", pRecorderName);
101 initSignalList("double", m_CaptureDouble, m_ConfigNode );
102 initSignalList("float", m_CaptureFloat , m_ConfigNode );
103 initSignalList("int", m_CaptureInteger, m_ConfigNode );
104 initSignalList("int16", m_CaptureInt16 , m_ConfigNode );
105 initSignalList("int8", m_CaptureInt8 , m_ConfigNode );
106 initSignalList("bool", m_CaptureBool , m_ConfigNode );
109 // calculate size of a single record
110 m_TotalRecordSize = sizeof(double) * 1 /* sim time */ +
111 sizeof(double) * m_CaptureDouble.size() +
112 sizeof(float) * m_CaptureFloat.size() +
113 sizeof(int) * m_CaptureInteger.size() +
114 sizeof(short int) * m_CaptureInt16.size() +
115 sizeof(signed char) * m_CaptureInt8.size() +
116 sizeof(unsigned char) * ((m_CaptureBool.size()+7)/8); // 8 bools per byte
118 // expose size of actual flight recorder record
119 m_RecorderNode->setIntValue("record-size", m_TotalRecordSize);
120 SG_LOG(SG_SYSTEMS, SG_INFO, "FlightRecorder: record size is " << m_TotalRecordSize << " bytes");
123 /** Check if SignalList already contains the given property */
125 FGFlightRecorder::haveProperty(FlightRecorder::TSignalList& SignalList,SGPropertyNode* pProperty)
127 unsigned int Count = SignalList.size();
128 for (unsigned int i=0; i<Count; i++)
130 if (SignalList[i].Signal.get() == pProperty)
138 /** Check if any signal list already contains the given property */
140 FGFlightRecorder::haveProperty(SGPropertyNode* pProperty)
142 if (haveProperty(m_CaptureDouble, pProperty))
144 if (haveProperty(m_CaptureFloat, pProperty))
146 if (haveProperty(m_CaptureInteger, pProperty))
148 if (haveProperty(m_CaptureInt16, pProperty))
150 if (haveProperty(m_CaptureInt8, pProperty))
152 if (haveProperty(m_CaptureBool, pProperty))
157 /** Read default flight-recorder configuration.
158 * Default should match properties as hard coded for versions up to FG2.4.0. */
160 FGFlightRecorder::getDefault(void)
162 SGPropertyNode_ptr ConfigNode;
164 // set name of active flight recorder type
165 SG_LOG(SG_SYSTEMS, SG_INFO, "FlightRecorder: No custom configuration. Loading generic default recorder.");
167 const char* Path = m_RecorderNode->getStringValue("default-config",NULL);
170 SG_LOG(SG_SYSTEMS, SG_ALERT, "FlightRecorder: No default flight recorder specified! Check preferences.xml!");
174 SGPath path = globals->resolve_aircraft_path(Path);
177 SG_LOG(SG_SYSTEMS, SG_ALERT, "FlightRecorder: Cannot find file '" << Path << "'.");
183 readProperties(path.str(), m_RecorderNode->getChild("config", 0 ,true), 0);
184 ConfigNode = m_RecorderNode->getChild("config", 0 ,false);
185 } catch (sg_io_exception &e)
187 SG_LOG(SG_SYSTEMS, SG_ALERT, "FlightRecorder: Error reading file '" <<
188 Path << ": " << e.getFormattedMessage());
193 m_usingDefaultConfig = true;
197 /** Read signal list below given base node.
198 * Only process properties of given signal type and add all signals to the given list.
199 * This method is called for all supported signal types - properties of each type are
200 * kept in separate lists for efficiency reasons. */
202 FGFlightRecorder::initSignalList(const char* pSignalType, TSignalList& SignalList, SGPropertyNode_ptr BaseNode)
207 processSignalList(pSignalType, SignalList, BaseNode);
209 SG_LOG(SG_SYSTEMS, SG_DEBUG, "FlightRecorder: " << SignalList.size() << " signals of type " << pSignalType );
212 /** Process signal list below given base node.
213 * Only process properties of given signal type and add all signals to the given list.
214 * This method is called for all supported signal types - properties of each type are
215 * kept in separate lists for efficiency reasons. */
217 FGFlightRecorder::processSignalList(const char* pSignalType, TSignalList& SignalList, SGPropertyNode_ptr SignalListNode,
218 string PropPrefix, int Count)
220 // get the list of signal sources (property paths) for this signal type
221 SGPropertyNode_ptr SignalNode;
224 Count = SignalListNode->getIntValue("count",Count);
225 PropPrefix = simgear::strutils::strip(SignalListNode->getStringValue("prefix",PropPrefix.c_str()));
226 if ((!PropPrefix.empty())&&(PropPrefix[PropPrefix.size()-1] != '/'))
231 SignalNode = SignalListNode->getChild("signal",Index,false);
232 if (SignalNode.valid()&&
233 (0==strcmp(pSignalType, SignalNode->getStringValue("type","float"))))
235 string PropertyPath = SignalNode->getStringValue("property","");
236 if (!PropertyPath.empty())
238 PropertyPath = PropPrefix + PropertyPath;
239 const char* pInterpolation = SignalNode->getStringValue("interpolation","linear");
241 // Check if current signal has a "%i" place holder. Otherwise count is 1.
242 string::size_type IndexPos = PropertyPath.find("%i");
243 int SignalCount = Count;
244 if (IndexPos == string::npos)
247 for (int IndexValue=0;IndexValue<SignalCount;IndexValue++)
249 string PPath = PropertyPath;
250 if (IndexPos != string::npos)
253 snprintf(strbuf, 20, "%d", IndexValue);
254 PPath = PPath.replace(IndexPos,2,strbuf);
257 Capture.Signal = fgGetNode(PPath.c_str(),false);
258 if (!Capture.Signal.valid())
260 // JMT - only warn if using a custom config, not the default one. Since the generic config of
261 // course requests many props, but we do want this warning for custom configs tailored to the
263 if (!m_usingDefaultConfig) {
264 // warn user: we're maybe going to record useless data
265 // Or maybe the data is only initialized later. Warn anyway, so we can catch useless data.
266 SG_LOG(SG_SYSTEMS, SG_INFO, "FlightRecorder: Recording non-existent property '" << PPath << "'.");
269 Capture.Signal = fgGetNode(PPath.c_str(),true);
272 if (0==strcmp(pInterpolation,"discrete"))
273 Capture.Interpolation = discrete;
275 if ((0==strcmp(pInterpolation,"angular"))||
276 (0==strcmp(pInterpolation,"angular-rad")))
277 Capture.Interpolation = angular_rad;
279 if (0==strcmp(pInterpolation,"angular-deg"))
280 Capture.Interpolation = angular_deg;
282 if (0==strcmp(pInterpolation,"linear"))
283 Capture.Interpolation = linear;
286 SG_LOG(SG_SYSTEMS, SG_ALERT, "FlightRecorder: Unsupported interpolation type '"
287 << pInterpolation<< "' of signal '" << PPath << "'");
288 Capture.Interpolation = linear;
290 if (haveProperty(Capture.Signal))
292 SG_LOG(SG_SYSTEMS, SG_ALERT, "FlightRecorder: Property '"
293 << PPath << "' specified multiple times. Check flight recorder configuration.");
296 SignalList.push_back(Capture);
301 } while (SignalNode.valid());
303 // allow recursive definition of signal lists
304 simgear::PropertyList Nodes = SignalListNode->getChildren("signals");
305 for (unsigned int i=0;i<Nodes.size();i++)
307 processSignalList(pSignalType, SignalList, Nodes[i], PropPrefix, Count);
311 /** Get an empty container for a single capture. */
313 FGFlightRecorder::createEmptyRecord(void)
315 if (!m_TotalRecordSize)
317 FGReplayData* p = (FGReplayData*) new unsigned char[m_TotalRecordSize];
321 /** Free given container with capture data. */
323 FGFlightRecorder::deleteRecord(FGReplayData* pRecord)
329 * When pBuffer==NULL new memory is allocated.
330 * If pBuffer!=NULL memory of given buffer is reused.
333 FGFlightRecorder::capture(double SimTime, FGReplayData* pRecycledBuffer)
335 if (!pRecycledBuffer)
337 pRecycledBuffer = createEmptyRecord();
338 if (!pRecycledBuffer)
341 unsigned char* pBuffer = (unsigned char*) pRecycledBuffer;
344 pRecycledBuffer->sim_time = SimTime;
345 Offset += sizeof(double);
347 // 64bit aligned data first!
350 double* pDoubles = (double*) &pBuffer[Offset];
351 unsigned int SignalCount = m_CaptureDouble.size();
352 for (unsigned int i=0; i<SignalCount; i++)
354 pDoubles[i] = m_CaptureDouble[i].Signal->getDoubleValue();
356 Offset += SignalCount * sizeof(double);
359 // 32bit aligned data comes second...
362 float* pFloats = (float*) &pBuffer[Offset];
363 unsigned int SignalCount = m_CaptureFloat.size();
364 for (unsigned int i=0; i<SignalCount; i++)
366 pFloats[i] = m_CaptureFloat[i].Signal->getFloatValue();
368 Offset += SignalCount * sizeof(float);
372 // capture integers (32bit aligned)
373 int* pInt = (int*) &pBuffer[Offset];
374 unsigned int SignalCount = m_CaptureInteger.size();
375 for (unsigned int i=0; i<SignalCount; i++)
377 pInt[i] = m_CaptureInteger[i].Signal->getIntValue();
379 Offset += SignalCount * sizeof(int);
382 // 16bit aligned data is next...
384 // capture 16bit short integers
385 short int* pShortInt = (short int*) &pBuffer[Offset];
386 unsigned int SignalCount = m_CaptureInt16.size();
387 for (unsigned int i=0; i<SignalCount; i++)
389 pShortInt[i] = (short int) m_CaptureInt16[i].Signal->getIntValue();
391 Offset += SignalCount * sizeof(short int);
394 // finally: byte aligned data is last...
396 // capture 8bit chars
397 signed char* pChar = (signed char*) &pBuffer[Offset];
398 unsigned int SignalCount = m_CaptureInt8.size();
399 for (unsigned int i=0; i<SignalCount; i++)
401 pChar[i] = (signed char) m_CaptureInt8[i].Signal->getIntValue();
403 Offset += SignalCount * sizeof(signed char);
407 // capture 1bit booleans (8bit aligned)
408 unsigned char* pFlags = (unsigned char*) &pBuffer[Offset];
409 unsigned int SignalCount = m_CaptureBool.size();
410 int Size = (SignalCount+7)/8;
412 memset(pFlags,0,Size);
413 for (unsigned int i=0; i<SignalCount; i++)
415 if (m_CaptureBool[i].Signal->getBoolValue())
416 pFlags[i>>3] |= 1 << (i&7);
420 assert(Offset == m_TotalRecordSize);
422 return (FGReplayData*) pBuffer;
425 /** Do interpolation as defined by given interpolation type and weighting ratio. */
427 weighting(TInterpolation interpolation, double ratio, double v1,double v2)
429 switch (interpolation)
432 return v1 + ratio*(v2-v1);
436 // special handling of angular data
437 double tmp = v2 - v1;
440 else if ( tmp < -180 )
442 return v1 + tmp * ratio;
447 // special handling of angular data
448 double tmp = v2 - v1;
451 else if ( tmp < -SGD_PI )
453 return v1 + tmp * ratio;
464 * Restore all properties with data from given buffer. */
466 FGFlightRecorder::replay(double SimTime, const FGReplayData* _pNextBuffer, const FGReplayData* _pLastBuffer)
468 const char* pLastBuffer = (const char*) _pLastBuffer;
469 const char* pBuffer = (const char*) _pNextBuffer;
477 double NextSimTime = _pNextBuffer->sim_time;
478 double LastSimTime = _pLastBuffer->sim_time;
479 double Numerator = SimTime - LastSimTime;
480 double dt = NextSimTime - LastSimTime;
481 // avoid divide by zero and other quirks
482 if ((Numerator > 0.0)&&(dt != 0.0))
484 ratio = Numerator / dt;
490 Offset += sizeof(double);
492 // 64bit aligned data first!
495 const double* pDoubles = (const double*) &pBuffer[Offset];
496 const double* pLastDoubles = (const double*) &pLastBuffer[Offset];
497 unsigned int SignalCount = m_CaptureDouble.size();
498 for (unsigned int i=0; i<SignalCount; i++)
500 double v = pDoubles[i];
503 v = weighting(m_CaptureDouble[i].Interpolation, ratio,
506 m_CaptureDouble[i].Signal->setDoubleValue(v);
508 Offset += SignalCount * sizeof(double);
511 // 32bit aligned data comes second...
514 const float* pFloats = (const float*) &pBuffer[Offset];
515 const float* pLastFloats = (const float*) &pLastBuffer[Offset];
516 unsigned int SignalCount = m_CaptureFloat.size();
517 for (unsigned int i=0; i<SignalCount; i++)
519 float v = pFloats[i];
522 v = weighting(m_CaptureFloat[i].Interpolation, ratio,
525 m_CaptureFloat[i].Signal->setDoubleValue(v);//setFloatValue
527 Offset += SignalCount * sizeof(float);
531 // restore integers (32bit aligned)
532 const int* pInt = (const int*) &pBuffer[Offset];
533 unsigned int SignalCount = m_CaptureInteger.size();
534 for (unsigned int i=0; i<SignalCount; i++)
536 m_CaptureInteger[i].Signal->setIntValue(pInt[i]);
538 Offset += SignalCount * sizeof(int);
541 // 16bit aligned data is next...
543 // restore 16bit short integers
544 const short int* pShortInt = (const short int*) &pBuffer[Offset];
545 unsigned int SignalCount = m_CaptureInt16.size();
546 for (unsigned int i=0; i<SignalCount; i++)
548 m_CaptureInt16[i].Signal->setIntValue(pShortInt[i]);
550 Offset += SignalCount * sizeof(short int);
553 // finally: byte aligned data is last...
555 // restore 8bit chars
556 const signed char* pChar = (const signed char*) &pBuffer[Offset];
557 unsigned int SignalCount = m_CaptureInt8.size();
558 for (unsigned int i=0; i<SignalCount; i++)
560 m_CaptureInt8[i].Signal->setIntValue(pChar[i]);
562 Offset += SignalCount * sizeof(signed char);
566 // restore 1bit booleans (8bit aligned)
567 const unsigned char* pFlags = (const unsigned char*) &pBuffer[Offset];
568 unsigned int SignalCount = m_CaptureBool.size();
569 int Size = (SignalCount+7)/8;
571 for (unsigned int i=0; i<SignalCount; i++)
573 m_CaptureBool[i].Signal->setBoolValue(0 != (pFlags[i>>3] & (1 << (i&7))));
579 FGFlightRecorder::getConfig(SGPropertyNode* root, const char* typeStr, const FlightRecorder::TSignalList& SignalList)
581 static const char* InterpolationTypes[] = {"discrete", "linear", "angular-rad", "angular-deg"};
582 size_t SignalCount = SignalList.size();
583 SGPropertyNode* Signals = root->getNode("signals", true);
584 for (size_t i=0; i<SignalCount; i++)
586 SGPropertyNode* SignalProp = Signals->addChild("signal");
587 SignalProp->setStringValue("type", typeStr);
588 SignalProp->setStringValue("interpolation", InterpolationTypes[SignalList[i].Interpolation]);
589 SignalProp->setStringValue("property", SignalList[i].Signal->getPath());
591 SG_LOG(SG_SYSTEMS, SG_DEBUG, "FlightRecorder: Have " << SignalCount << " signals of type " << typeStr);
592 root->setIntValue(typeStr, SignalCount);
597 FGFlightRecorder::getConfig(SGPropertyNode* root)
599 root->setStringValue("name", m_RecorderNode->getStringValue("active-config-name", ""));
601 SignalCount += getConfig(root, "double", m_CaptureDouble);
602 SignalCount += getConfig(root, "float", m_CaptureFloat);
603 SignalCount += getConfig(root, "int", m_CaptureInteger);
604 SignalCount += getConfig(root, "int16", m_CaptureInt16);
605 SignalCount += getConfig(root, "int8", m_CaptureInt8);
606 SignalCount += getConfig(root, "bool", m_CaptureBool);
608 root->setIntValue("recorder/record-size", getRecordSize());
609 root->setIntValue("recorder/signal-count", SignalCount);