src/Aircraft/flightrecorder.cxx

   1 // flightrecorder.cxx
   2 //
   3 // Written by Thorsten Brehm, started August 2011.
   4 //
   5 // Copyright (C) 2011 Thorsten Brehm - brehmt (at) gmail com
   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., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301, USA.
  20 //
  21 ///////////////////////////////////////////////////////////////////////////////
  22
  23 #ifdef HAVE_CONFIG_H
  24 #  include "config.h"
  25 #endif
  26
  27 #include <stdio.h>
  28 #include <string.h>
  29 #include <assert.h>
  30
  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"
  39
  40 using namespace FlightRecorder;
  41 using std::string;
  42
  43 FGFlightRecorder::FGFlightRecorder(const char* pConfigName) :
  44     m_RecorderNode(fgGetNode("/sim/flight-recorder", true)),
  45     m_TotalRecordSize(0),
  46     m_ConfigName(pConfigName)
  47 {
  48 }
  49
  50 FGFlightRecorder::~FGFlightRecorder()
  51 {
  52 }
  53
  54 void
  55 FGFlightRecorder::reinit(void)
  56 {
  57     m_ConfigNode = 0;
  58
  59     SGPropertyNode_ptr ConfigNode;
  60     int Selected = m_RecorderNode->getIntValue(m_ConfigName, 0);
  61     SG_LOG(SG_SYSTEMS, SG_INFO, "FlightRecorder: Recorder configuration #" << Selected);
  62     if (Selected >= 0)
  63         ConfigNode = m_RecorderNode->getChild("config", Selected);
  64
  65     if (!ConfigNode.valid())
  66         ConfigNode = getDefault();
  67
  68     reinit(ConfigNode);
  69 }
  70
  71 void
  72 FGFlightRecorder::reinit(SGPropertyNode_ptr ConfigNode)
  73 {
  74     m_TotalRecordSize = 0;
  75
  76     m_CaptureDouble.clear();
  77     m_CaptureFloat.clear();
  78     m_CaptureInteger.clear();
  79     m_CaptureInt16.clear();
  80     m_CaptureInt8.clear();
  81     m_CaptureBool.clear();
  82
  83     m_ConfigNode = ConfigNode;
  84
  85     if (!m_ConfigNode.valid())
  86     {
  87         SG_LOG(SG_SYSTEMS, SG_ALERT, "FlightRecorder: Configuration is invalid. Flight recorder disabled.");
  88     }
  89     else
  90     {
  91         // set name of active flight recorder type
  92         const char* pRecorderName =
  93                 m_ConfigNode->getStringValue("name",
  94                                              "aircraft-specific flight recorder");
  95         SG_LOG(SG_SYSTEMS, SG_INFO, "FlightRecorder: Using custom recorder configuration: " << pRecorderName);
  96         m_RecorderNode->setStringValue("active-config-name", pRecorderName);
  97
  98         // get signals
  99         initSignalList("double", m_CaptureDouble,  m_ConfigNode );
 100         initSignalList("float",  m_CaptureFloat ,  m_ConfigNode );
 101         initSignalList("int",    m_CaptureInteger, m_ConfigNode );
 102         initSignalList("int16",  m_CaptureInt16  , m_ConfigNode );
 103         initSignalList("int8",   m_CaptureInt8   , m_ConfigNode );
 104         initSignalList("bool",   m_CaptureBool   , m_ConfigNode );
 105     }
 106
 107     // calculate size of a single record
 108     m_TotalRecordSize = sizeof(double)        * 1 /* sim time */        +
 109                         sizeof(double)        * m_CaptureDouble.size()  +
 110                         sizeof(float)         * m_CaptureFloat.size()   +
 111                         sizeof(int)           * m_CaptureInteger.size() +
 112                         sizeof(short int)     * m_CaptureInt16.size()   +
 113                         sizeof(signed char)   * m_CaptureInt8.size()    +
 114                         sizeof(unsigned char) * ((m_CaptureBool.size()+7)/8); // 8 bools per byte
 115
 116     // expose size of actual flight recorder record
 117     m_RecorderNode->setIntValue("record-size", m_TotalRecordSize);
 118     SG_LOG(SG_SYSTEMS, SG_INFO, "FlightRecorder: record size is " << m_TotalRecordSize << " bytes");
 119 }
 120
 121 /** Check if SignalList already contains the given property */
 122 bool
 123 FGFlightRecorder::haveProperty(FlightRecorder::TSignalList& SignalList,SGPropertyNode* pProperty)
 124 {
 125     unsigned int Count = SignalList.size();
 126     for (unsigned int i=0; i<Count; i++)
 127     {
 128         if (SignalList[i].Signal.get() == pProperty)
 129         {
 130             return true;
 131         }
 132     }
 133     return false;
 134 }
 135
 136 /** Check if any signal list already contains the given property */
 137 bool
 138 FGFlightRecorder::haveProperty(SGPropertyNode* pProperty)
 139 {
 140     if (haveProperty(m_CaptureDouble,  pProperty))
 141         return true;
 142     if (haveProperty(m_CaptureFloat,   pProperty))
 143         return true;
 144     if (haveProperty(m_CaptureInteger, pProperty))
 145         return true;
 146     if (haveProperty(m_CaptureInt16,   pProperty))
 147         return true;
 148     if (haveProperty(m_CaptureInt8,    pProperty))
 149         return true;
 150     if (haveProperty(m_CaptureBool,    pProperty))
 151         return true;
 152     return false;
 153 }
 154
 155 /** Read default flight-recorder configuration.
 156  * Default should match properties as hard coded for versions up to FG2.4.0. */
 157 SGPropertyNode_ptr
 158 FGFlightRecorder::getDefault(void)
 159 {
 160     SGPropertyNode_ptr ConfigNode;
 161
 162     // set name of active flight recorder type
 163     SG_LOG(SG_SYSTEMS, SG_INFO, "FlightRecorder: No custom configuration. Loading generic default recorder.");
 164
 165     const char* Path = m_RecorderNode->getStringValue("default-config",NULL);
 166     if (!Path)
 167     {
 168         SG_LOG(SG_SYSTEMS, SG_ALERT, "FlightRecorder: No default flight recorder specified! Check preferences.xml!");
 169     }
 170     else
 171     {
 172         SGPath path = globals->resolve_aircraft_path(Path);
 173         if (path.isNull())
 174         {
 175             SG_LOG(SG_SYSTEMS, SG_ALERT, "FlightRecorder: Cannot find file '" << Path << "'.");
 176         }
 177         else
 178         {
 179             try
 180             {
 181                 readProperties(path.str(), m_RecorderNode->getChild("config", 0 ,true), 0);
 182                 ConfigNode = m_RecorderNode->getChild("config", 0 ,false);
 183             } catch (sg_io_exception &e)
 184             {
 185                 SG_LOG(SG_SYSTEMS, SG_ALERT, "FlightRecorder: Error reading file '" <<
 186                         Path << ": " << e.getFormattedMessage());
 187             }
 188         }
 189     }
 190
 191     return ConfigNode;
 192 }
 193
 194 /** Read signal list below given base node.
 195  * Only process properties of given signal type and add all signals to the given list.
 196  * This method is called for all supported signal types - properties of each type are
 197  * kept in separate lists for efficiency reasons. */
 198 void
 199 FGFlightRecorder::initSignalList(const char* pSignalType, TSignalList& SignalList, SGPropertyNode_ptr BaseNode)
 200 {
 201     // clear old signals
 202     SignalList.clear();
 203
 204     processSignalList(pSignalType, SignalList, BaseNode);
 205
 206     SG_LOG(SG_SYSTEMS, SG_DEBUG, "FlightRecorder: " << SignalList.size() << " signals of type " << pSignalType );
 207 }
 208
 209 /** Process signal list below given base node.
 210  * Only process properties of given signal type and add all signals to the given list.
 211  * This method is called for all supported signal types - properties of each type are
 212  * kept in separate lists for efficiency reasons. */
 213 void
 214 FGFlightRecorder::processSignalList(const char* pSignalType, TSignalList& SignalList, SGPropertyNode_ptr SignalListNode,
 215                                     string PropPrefix, int Count)
 216 {
 217     // get the list of signal sources (property paths) for this signal type
 218     SGPropertyNode_ptr SignalNode;
 219     int Index=0;
 220
 221     Count = SignalListNode->getIntValue("count",Count);
 222     PropPrefix = simgear::strutils::strip(SignalListNode->getStringValue("prefix",PropPrefix.c_str()));
 223     if ((!PropPrefix.empty())&&(PropPrefix[PropPrefix.size()-1] != '/'))
 224         PropPrefix += "/";
 225
 226     do
 227     {
 228         SignalNode = SignalListNode->getChild("signal",Index,false);
 229         if (SignalNode.valid()&&
 230             (0==strcmp(pSignalType, SignalNode->getStringValue("type","float"))))
 231         {
 232             string PropertyPath = SignalNode->getStringValue("property","");
 233             if (!PropertyPath.empty())
 234             {
 235                 PropertyPath = PropPrefix + PropertyPath;
 236                 const char* pInterpolation = SignalNode->getStringValue("interpolation","linear");
 237
 238                 // Check if current signal has a "%i" place holder. Otherwise count is 1.
 239                 string::size_type IndexPos = PropertyPath.find("%i");
 240                 int SignalCount = Count;
 241                 if (IndexPos == string::npos)
 242                     SignalCount = 1;
 243
 244                 for (int IndexValue=0;IndexValue<SignalCount;IndexValue++)
 245                 {
 246                     string PPath = PropertyPath;
 247                     if (IndexPos != string::npos)
 248                     {
 249                         char strbuf[20];
 250                         snprintf(strbuf, 20, "%d", IndexValue);
 251                         PPath = PPath.replace(IndexPos,2,strbuf);
 252                     }
 253                     TCapture Capture;
 254                     Capture.Signal = fgGetNode(PPath.c_str(),false);
 255                     if (!Capture.Signal.valid())
 256                     {
 257                         // warn user: we're maybe going to record useless data
 258                         // Or maybe the data is only initialized later. Warn anyway, so we can catch useless data.
 259                         SG_LOG(SG_SYSTEMS, SG_INFO, "FlightRecorder: Recording non-existent property '" << PPath << "'.");
 260                         Capture.Signal = fgGetNode(PPath.c_str(),true);
 261                     }
 262
 263                     if (0==strcmp(pInterpolation,"discrete"))
 264                         Capture.Interpolation = discrete;
 265                     else
 266                     if ((0==strcmp(pInterpolation,"angular"))||
 267                         (0==strcmp(pInterpolation,"angular-rad")))
 268                         Capture.Interpolation = angular_rad;
 269                     else
 270                     if (0==strcmp(pInterpolation,"angular-deg"))
 271                         Capture.Interpolation = angular_deg;
 272                     else
 273                     if (0==strcmp(pInterpolation,"linear"))
 274                         Capture.Interpolation = linear;
 275                     else
 276                     {
 277                         SG_LOG(SG_SYSTEMS, SG_ALERT, "FlightRecorder: Unsupported interpolation type '"
 278                                 << pInterpolation<< "' of signal '" << PPath << "'");
 279                         Capture.Interpolation = linear;
 280                     }
 281                     if (haveProperty(Capture.Signal))
 282                     {
 283                         SG_LOG(SG_SYSTEMS, SG_ALERT, "FlightRecorder: Property '"
 284                                 << PPath << "' specified multiple times. Check flight recorder configuration.");
 285                     }
 286                     else
 287                         SignalList.push_back(Capture);
 288                 }
 289             }
 290         }
 291         Index++;
 292     } while (SignalNode.valid());
 293
 294     // allow recursive definition of signal lists
 295     simgear::PropertyList Nodes = SignalListNode->getChildren("signals");
 296     for (unsigned int i=0;i<Nodes.size();i++)
 297     {
 298         processSignalList(pSignalType, SignalList, Nodes[i], PropPrefix, Count);
 299     }
 300 }
 301
 302 /** Get an empty container for a single capture. */
 303 FGReplayData*
 304 FGFlightRecorder::createEmptyRecord(void)
 305 {
 306     if (!m_TotalRecordSize)
 307         return NULL;
 308     FGReplayData* p = (FGReplayData*) new unsigned char[m_TotalRecordSize];
 309     return p;
 310 }
 311
 312 /** Free given container with capture data. */
 313 void
 314 FGFlightRecorder::deleteRecord(FGReplayData* pRecord)
 315 {
 316     delete[] pRecord;
 317 }
 318
 319 /** Capture data.
 320  * When pBuffer==NULL new memory is allocated.
 321  * If pBuffer!=NULL memory of given buffer is reused.
 322  */
 323 FGReplayData*
 324 FGFlightRecorder::capture(double SimTime, FGReplayData* pRecycledBuffer)
 325 {
 326     if (!pRecycledBuffer)
 327     {
 328         pRecycledBuffer = createEmptyRecord();
 329         if (!pRecycledBuffer)
 330             return NULL;
 331     }
 332     unsigned char* pBuffer = (unsigned char*) pRecycledBuffer;
 333
 334     int Offset = 0;
 335     pRecycledBuffer->sim_time = SimTime;
 336     Offset += sizeof(double);
 337
 338     // 64bit aligned data first!
 339     {
 340         // capture doubles
 341         double* pDoubles = (double*) &pBuffer[Offset];
 342         unsigned int SignalCount = m_CaptureDouble.size();
 343         for (unsigned int i=0; i<SignalCount; i++)
 344         {
 345             pDoubles[i] = m_CaptureDouble[i].Signal->getDoubleValue();
 346         }
 347         Offset += SignalCount * sizeof(double);
 348     }
 349
 350     // 32bit aligned data comes second...
 351     {
 352         // capture floats
 353         float* pFloats = (float*) &pBuffer[Offset];
 354         unsigned int SignalCount = m_CaptureFloat.size();
 355         for (unsigned int i=0; i<SignalCount; i++)
 356         {
 357             pFloats[i] = m_CaptureFloat[i].Signal->getFloatValue();
 358         }
 359         Offset += SignalCount * sizeof(float);
 360     }
 361
 362     {
 363         // capture integers (32bit aligned)
 364         int* pInt = (int*) &pBuffer[Offset];
 365         unsigned int SignalCount = m_CaptureInteger.size();
 366         for (unsigned int i=0; i<SignalCount; i++)
 367         {
 368             pInt[i] = m_CaptureInteger[i].Signal->getIntValue();
 369         }
 370         Offset += SignalCount * sizeof(int);
 371     }
 372
 373     // 16bit aligned data is next...
 374     {
 375         // capture 16bit short integers
 376         short int* pShortInt = (short int*) &pBuffer[Offset];
 377         unsigned int SignalCount = m_CaptureInt16.size();
 378         for (unsigned int i=0; i<SignalCount; i++)
 379         {
 380             pShortInt[i] = (short int) m_CaptureInt16[i].Signal->getIntValue();
 381         }
 382         Offset += SignalCount * sizeof(short int);
 383     }
 384
 385     // finally: byte aligned data is last...
 386     {
 387         // capture 8bit chars
 388         signed char* pChar = (signed char*) &pBuffer[Offset];
 389         unsigned int SignalCount = m_CaptureInt8.size();
 390         for (unsigned int i=0; i<SignalCount; i++)
 391         {
 392             pChar[i] = (signed char) m_CaptureInt8[i].Signal->getIntValue();
 393         }
 394         Offset += SignalCount * sizeof(signed char);
 395     }
 396
 397     {
 398         // capture 1bit booleans (8bit aligned)
 399         unsigned char* pFlags = (unsigned char*) &pBuffer[Offset];
 400         unsigned int SignalCount = m_CaptureBool.size();
 401         int Size = (SignalCount+7)/8;
 402         Offset += Size;
 403         memset(pFlags,0,Size);
 404         for (unsigned int i=0; i<SignalCount; i++)
 405         {
 406             if (m_CaptureBool[i].Signal->getBoolValue())
 407                 pFlags[i>>3] |= 1 << (i&7);
 408         }
 409     }
 410
 411     assert(Offset == m_TotalRecordSize);
 412
 413     return (FGReplayData*) pBuffer;
 414 }
 415
 416 /** Do interpolation as defined by given interpolation type and weighting ratio. */
 417 static double
 418 weighting(TInterpolation interpolation, double ratio, double v1,double v2)
 419 {
 420     switch (interpolation)
 421     {
 422         case linear:
 423             return v1 + ratio*(v2-v1);
 424
 425         case angular_deg:
 426         {
 427             // special handling of angular data
 428             double tmp = v2 - v1;
 429             if ( tmp > 180 )
 430                 tmp -= 360;
 431             else if ( tmp < -180 )
 432                 tmp += 360;
 433             return v1 + tmp * ratio;
 434         }
 435
 436         case angular_rad:
 437         {
 438             // special handling of angular data
 439             double tmp = v2 - v1;
 440             if ( tmp > SGD_PI )
 441                 tmp -= SGD_2PI;
 442             else if ( tmp < -SGD_PI )
 443                 tmp += SGD_2PI;
 444             return v1 + tmp * ratio;
 445         }
 446
 447         case discrete:
 448             // fall through
 449         default:
 450             return v2;
 451     }
 452 }
 453
 454 /** Replay.
 455  * Restore all properties with data from given buffer. */
 456 void
 457 FGFlightRecorder::replay(double SimTime, const FGReplayData* _pNextBuffer, const FGReplayData* _pLastBuffer)
 458 {
 459     const char* pLastBuffer = (const char*) _pLastBuffer;
 460     const char* pBuffer = (const char*) _pNextBuffer;
 461     if (!pBuffer)
 462         return;
 463
 464     int Offset = 0;
 465     double ratio = 1.0;
 466     if (pLastBuffer)
 467     {
 468         double NextSimTime = _pNextBuffer->sim_time;
 469         double LastSimTime = _pLastBuffer->sim_time;
 470         double Numerator = SimTime - LastSimTime;
 471         double dt = NextSimTime - LastSimTime;
 472         // avoid divide by zero and other quirks
 473         if ((Numerator > 0.0)&&(dt != 0.0))
 474         {
 475             ratio = Numerator / dt;
 476             if (ratio > 1.0)
 477                 ratio = 1.0;
 478         }
 479     }
 480
 481     Offset += sizeof(double);
 482
 483     // 64bit aligned data first!
 484     {
 485         // restore doubles
 486         const double* pDoubles = (const double*) &pBuffer[Offset];
 487         const double* pLastDoubles = (const double*) &pLastBuffer[Offset];
 488         unsigned int SignalCount = m_CaptureDouble.size();
 489         for (unsigned int i=0; i<SignalCount; i++)
 490         {
 491             double v = pDoubles[i];
 492             if (pLastBuffer)
 493             {
 494                 v = weighting(m_CaptureDouble[i].Interpolation, ratio,
 495                               pLastDoubles[i], v);
 496             }
 497             m_CaptureDouble[i].Signal->setDoubleValue(v);
 498         }
 499         Offset += SignalCount * sizeof(double);
 500     }
 501
 502     // 32bit aligned data comes second...
 503     {
 504         // restore floats
 505         const float* pFloats = (const float*) &pBuffer[Offset];
 506         const float* pLastFloats = (const float*) &pLastBuffer[Offset];
 507         unsigned int SignalCount = m_CaptureFloat.size();
 508         for (unsigned int i=0; i<SignalCount; i++)
 509         {
 510             float v = pFloats[i];
 511             if (pLastBuffer)
 512             {
 513                 v = weighting(m_CaptureFloat[i].Interpolation, ratio,
 514                               pLastFloats[i], v);
 515             }
 516             m_CaptureFloat[i].Signal->setDoubleValue(v);//setFloatValue
 517         }
 518         Offset += SignalCount * sizeof(float);
 519     }
 520
 521     {
 522         // restore integers (32bit aligned)
 523         const int* pInt = (const int*) &pBuffer[Offset];
 524         unsigned int SignalCount = m_CaptureInteger.size();
 525         for (unsigned int i=0; i<SignalCount; i++)
 526         {
 527             m_CaptureInteger[i].Signal->setIntValue(pInt[i]);
 528         }
 529         Offset += SignalCount * sizeof(int);
 530     }
 531
 532     // 16bit aligned data is next...
 533     {
 534         // restore 16bit short integers
 535         const short int* pShortInt = (const short int*) &pBuffer[Offset];
 536         unsigned int SignalCount = m_CaptureInt16.size();
 537         for (unsigned int i=0; i<SignalCount; i++)
 538         {
 539             m_CaptureInt16[i].Signal->setIntValue(pShortInt[i]);
 540         }
 541         Offset += SignalCount * sizeof(short int);
 542     }
 543
 544     // finally: byte aligned data is last...
 545     {
 546         // restore 8bit chars
 547         const signed char* pChar = (const signed char*) &pBuffer[Offset];
 548         unsigned int SignalCount = m_CaptureInt8.size();
 549         for (unsigned int i=0; i<SignalCount; i++)
 550         {
 551             m_CaptureInt8[i].Signal->setIntValue(pChar[i]);
 552         }
 553         Offset += SignalCount * sizeof(signed char);
 554     }
 555
 556     {
 557         // restore 1bit booleans (8bit aligned)
 558         const unsigned char* pFlags = (const unsigned char*) &pBuffer[Offset];
 559         unsigned int SignalCount = m_CaptureBool.size();
 560         int Size = (SignalCount+7)/8;
 561         Offset += Size;
 562         for (unsigned int i=0; i<SignalCount; i++)
 563         {
 564             m_CaptureBool[i].Signal->setBoolValue(0 != (pFlags[i>>3] & (1 << (i&7))));
 565         }
 566     }
 567 }
 568
 569 int
 570 FGFlightRecorder::getConfig(SGPropertyNode* root, const char* typeStr, const FlightRecorder::TSignalList& SignalList)
 571 {
 572     static const char* InterpolationTypes[] = {"discrete", "linear", "angular-rad", "angular-deg"};
 573     size_t SignalCount = SignalList.size();
 574     SGPropertyNode* Signals = root->getNode("signals", true);
 575     for (size_t i=0; i<SignalCount; i++)
 576     {
 577         SGPropertyNode* SignalProp = Signals->addChild("signal");
 578         SignalProp->setStringValue("type", typeStr);
 579         SignalProp->setStringValue("interpolation", InterpolationTypes[SignalList[i].Interpolation]);
 580         SignalProp->setStringValue("property", SignalList[i].Signal->getPath());
 581     }
 582     SG_LOG(SG_SYSTEMS, SG_DEBUG, "FlightRecorder: Have " << SignalCount << " signals of type " << typeStr);
 583     root->setIntValue(typeStr, SignalCount);
 584     return SignalCount;
 585 }
 586
 587 void
 588 FGFlightRecorder::getConfig(SGPropertyNode* root)
 589 {
 590     root->setStringValue("name", m_RecorderNode->getStringValue("active-config-name", ""));
 591     int SignalCount = 0;
 592     SignalCount += getConfig(root, "double",  m_CaptureDouble);
 593     SignalCount += getConfig(root, "float",   m_CaptureFloat);
 594     SignalCount += getConfig(root, "int", m_CaptureInteger);
 595     SignalCount += getConfig(root, "int16",   m_CaptureInt16);
 596     SignalCount += getConfig(root, "int8",    m_CaptureInt8);
 597     SignalCount += getConfig(root, "bool",    m_CaptureBool);
 598
 599     root->setIntValue("recorder/record-size", getRecordSize());
 600     root->setIntValue("recorder/signal-count", SignalCount);
 601 }