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     m_usingDefaultConfig(false)
  48 {
  49 }
  50
  51 FGFlightRecorder::~FGFlightRecorder()
  52 {
  53 }
  54
  55 void
  56 FGFlightRecorder::reinit(void)
  57 {
  58     m_ConfigNode = 0;
  59     m_usingDefaultConfig = false;
  60
  61     SGPropertyNode_ptr ConfigNode;
  62     int Selected = m_RecorderNode->getIntValue(m_ConfigName, 0);
  63     SG_LOG(SG_SYSTEMS, SG_INFO, "FlightRecorder: Recorder configuration #" << Selected);
  64     if (Selected >= 0)
  65         ConfigNode = m_RecorderNode->getChild("config", Selected);
  66
  67     if (!ConfigNode.valid())
  68         ConfigNode = getDefault();
  69
  70     reinit(ConfigNode);
  71 }
  72
  73 void
  74 FGFlightRecorder::reinit(SGPropertyNode_ptr ConfigNode)
  75 {
  76     m_TotalRecordSize = 0;
  77
  78     m_CaptureDouble.clear();
  79     m_CaptureFloat.clear();
  80     m_CaptureInteger.clear();
  81     m_CaptureInt16.clear();
  82     m_CaptureInt8.clear();
  83     m_CaptureBool.clear();
  84
  85     m_ConfigNode = ConfigNode;
  86
  87     if (!m_ConfigNode.valid())
  88     {
  89         SG_LOG(SG_SYSTEMS, SG_ALERT, "FlightRecorder: Configuration is invalid. Flight recorder disabled.");
  90     }
  91     else
  92     {
  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);
  99
 100         // get signals
 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 );
 107     }
 108
 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
 117
 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");
 121 }
 122
 123 /** Check if SignalList already contains the given property */
 124 bool
 125 FGFlightRecorder::haveProperty(FlightRecorder::TSignalList& SignalList,SGPropertyNode* pProperty)
 126 {
 127     unsigned int Count = SignalList.size();
 128     for (unsigned int i=0; i<Count; i++)
 129     {
 130         if (SignalList[i].Signal.get() == pProperty)
 131         {
 132             return true;
 133         }
 134     }
 135     return false;
 136 }
 137
 138 /** Check if any signal list already contains the given property */
 139 bool
 140 FGFlightRecorder::haveProperty(SGPropertyNode* pProperty)
 141 {
 142     if (haveProperty(m_CaptureDouble,  pProperty))
 143         return true;
 144     if (haveProperty(m_CaptureFloat,   pProperty))
 145         return true;
 146     if (haveProperty(m_CaptureInteger, pProperty))
 147         return true;
 148     if (haveProperty(m_CaptureInt16,   pProperty))
 149         return true;
 150     if (haveProperty(m_CaptureInt8,    pProperty))
 151         return true;
 152     if (haveProperty(m_CaptureBool,    pProperty))
 153         return true;
 154     return false;
 155 }
 156
 157 /** Read default flight-recorder configuration.
 158  * Default should match properties as hard coded for versions up to FG2.4.0. */
 159 SGPropertyNode_ptr
 160 FGFlightRecorder::getDefault(void)
 161 {
 162     SGPropertyNode_ptr ConfigNode;
 163
 164     // set name of active flight recorder type
 165     SG_LOG(SG_SYSTEMS, SG_INFO, "FlightRecorder: No custom configuration. Loading generic default recorder.");
 166
 167     const char* Path = m_RecorderNode->getStringValue("default-config",NULL);
 168     if (!Path)
 169     {
 170         SG_LOG(SG_SYSTEMS, SG_ALERT, "FlightRecorder: No default flight recorder specified! Check preferences.xml!");
 171     }
 172     else
 173     {
 174         SGPath path = globals->resolve_aircraft_path(Path);
 175         if (path.isNull())
 176         {
 177             SG_LOG(SG_SYSTEMS, SG_ALERT, "FlightRecorder: Cannot find file '" << Path << "'.");
 178         }
 179         else
 180         {
 181             try
 182             {
 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)
 186             {
 187                 SG_LOG(SG_SYSTEMS, SG_ALERT, "FlightRecorder: Error reading file '" <<
 188                         Path << ": " << e.getFormattedMessage());
 189             }
 190         }
 191     }
 192
 193     m_usingDefaultConfig = true;
 194     return ConfigNode;
 195 }
 196
 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. */
 201 void
 202 FGFlightRecorder::initSignalList(const char* pSignalType, TSignalList& SignalList, SGPropertyNode_ptr BaseNode)
 203 {
 204     // clear old signals
 205     SignalList.clear();
 206
 207     processSignalList(pSignalType, SignalList, BaseNode);
 208
 209     SG_LOG(SG_SYSTEMS, SG_DEBUG, "FlightRecorder: " << SignalList.size() << " signals of type " << pSignalType );
 210 }
 211
 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. */
 216 void
 217 FGFlightRecorder::processSignalList(const char* pSignalType, TSignalList& SignalList, SGPropertyNode_ptr SignalListNode,
 218                                     string PropPrefix, int Count)
 219 {
 220     // get the list of signal sources (property paths) for this signal type
 221     SGPropertyNode_ptr SignalNode;
 222     int Index=0;
 223
 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] != '/'))
 227         PropPrefix += "/";
 228
 229     do
 230     {
 231         SignalNode = SignalListNode->getChild("signal",Index,false);
 232         if (SignalNode.valid()&&
 233             (0==strcmp(pSignalType, SignalNode->getStringValue("type","float"))))
 234         {
 235             string PropertyPath = SignalNode->getStringValue("property","");
 236             if (!PropertyPath.empty())
 237             {
 238                 PropertyPath = PropPrefix + PropertyPath;
 239                 const char* pInterpolation = SignalNode->getStringValue("interpolation","linear");
 240
 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)
 245                     SignalCount = 1;
 246
 247                 for (int IndexValue=0;IndexValue<SignalCount;IndexValue++)
 248                 {
 249                     string PPath = PropertyPath;
 250                     if (IndexPos != string::npos)
 251                     {
 252                         char strbuf[20];
 253                         snprintf(strbuf, 20, "%d", IndexValue);
 254                         PPath = PPath.replace(IndexPos,2,strbuf);
 255                     }
 256                     TCapture Capture;
 257                     Capture.Signal = fgGetNode(PPath.c_str(),false);
 258                     if (!Capture.Signal.valid())
 259                     {
 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
 262                         // aircraft model.
 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 << "'.");
 267                         }
 268
 269                         Capture.Signal = fgGetNode(PPath.c_str(),true);
 270                     }
 271
 272                     if (0==strcmp(pInterpolation,"discrete"))
 273                         Capture.Interpolation = discrete;
 274                     else
 275                     if ((0==strcmp(pInterpolation,"angular"))||
 276                         (0==strcmp(pInterpolation,"angular-rad")))
 277                         Capture.Interpolation = angular_rad;
 278                     else
 279                     if (0==strcmp(pInterpolation,"angular-deg"))
 280                         Capture.Interpolation = angular_deg;
 281                     else
 282                     if (0==strcmp(pInterpolation,"linear"))
 283                         Capture.Interpolation = linear;
 284                     else
 285                     {
 286                         SG_LOG(SG_SYSTEMS, SG_ALERT, "FlightRecorder: Unsupported interpolation type '"
 287                                 << pInterpolation<< "' of signal '" << PPath << "'");
 288                         Capture.Interpolation = linear;
 289                     }
 290                     if (haveProperty(Capture.Signal))
 291                     {
 292                         SG_LOG(SG_SYSTEMS, SG_ALERT, "FlightRecorder: Property '"
 293                                 << PPath << "' specified multiple times. Check flight recorder configuration.");
 294                     }
 295                     else
 296                         SignalList.push_back(Capture);
 297                 }
 298             }
 299         }
 300         Index++;
 301     } while (SignalNode.valid());
 302
 303     // allow recursive definition of signal lists
 304     simgear::PropertyList Nodes = SignalListNode->getChildren("signals");
 305     for (unsigned int i=0;i<Nodes.size();i++)
 306     {
 307         processSignalList(pSignalType, SignalList, Nodes[i], PropPrefix, Count);
 308     }
 309 }
 310
 311 /** Get an empty container for a single capture. */
 312 FGReplayData*
 313 FGFlightRecorder::createEmptyRecord(void)
 314 {
 315     if (!m_TotalRecordSize)
 316         return NULL;
 317     FGReplayData* p = (FGReplayData*) new unsigned char[m_TotalRecordSize];
 318     return p;
 319 }
 320
 321 /** Free given container with capture data. */
 322 void
 323 FGFlightRecorder::deleteRecord(FGReplayData* pRecord)
 324 {
 325     delete[] pRecord;
 326 }
 327
 328 /** Capture data.
 329  * When pBuffer==NULL new memory is allocated.
 330  * If pBuffer!=NULL memory of given buffer is reused.
 331  */
 332 FGReplayData*
 333 FGFlightRecorder::capture(double SimTime, FGReplayData* pRecycledBuffer)
 334 {
 335     if (!pRecycledBuffer)
 336     {
 337         pRecycledBuffer = createEmptyRecord();
 338         if (!pRecycledBuffer)
 339             return NULL;
 340     }
 341     unsigned char* pBuffer = (unsigned char*) pRecycledBuffer;
 342
 343     int Offset = 0;
 344     pRecycledBuffer->sim_time = SimTime;
 345     Offset += sizeof(double);
 346
 347     // 64bit aligned data first!
 348     {
 349         // capture doubles
 350         double* pDoubles = (double*) &pBuffer[Offset];
 351         unsigned int SignalCount = m_CaptureDouble.size();
 352         for (unsigned int i=0; i<SignalCount; i++)
 353         {
 354             pDoubles[i] = m_CaptureDouble[i].Signal->getDoubleValue();
 355         }
 356         Offset += SignalCount * sizeof(double);
 357     }
 358
 359     // 32bit aligned data comes second...
 360     {
 361         // capture floats
 362         float* pFloats = (float*) &pBuffer[Offset];
 363         unsigned int SignalCount = m_CaptureFloat.size();
 364         for (unsigned int i=0; i<SignalCount; i++)
 365         {
 366             pFloats[i] = m_CaptureFloat[i].Signal->getFloatValue();
 367         }
 368         Offset += SignalCount * sizeof(float);
 369     }
 370
 371     {
 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++)
 376         {
 377             pInt[i] = m_CaptureInteger[i].Signal->getIntValue();
 378         }
 379         Offset += SignalCount * sizeof(int);
 380     }
 381
 382     // 16bit aligned data is next...
 383     {
 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++)
 388         {
 389             pShortInt[i] = (short int) m_CaptureInt16[i].Signal->getIntValue();
 390         }
 391         Offset += SignalCount * sizeof(short int);
 392     }
 393
 394     // finally: byte aligned data is last...
 395     {
 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++)
 400         {
 401             pChar[i] = (signed char) m_CaptureInt8[i].Signal->getIntValue();
 402         }
 403         Offset += SignalCount * sizeof(signed char);
 404     }
 405
 406     {
 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;
 411         Offset += Size;
 412         memset(pFlags,0,Size);
 413         for (unsigned int i=0; i<SignalCount; i++)
 414         {
 415             if (m_CaptureBool[i].Signal->getBoolValue())
 416                 pFlags[i>>3] |= 1 << (i&7);
 417         }
 418     }
 419
 420     assert(Offset == m_TotalRecordSize);
 421
 422     return (FGReplayData*) pBuffer;
 423 }
 424
 425 /** Do interpolation as defined by given interpolation type and weighting ratio. */
 426 static double
 427 weighting(TInterpolation interpolation, double ratio, double v1,double v2)
 428 {
 429     switch (interpolation)
 430     {
 431         case linear:
 432             return v1 + ratio*(v2-v1);
 433
 434         case angular_deg:
 435         {
 436             // special handling of angular data
 437             double tmp = v2 - v1;
 438             if ( tmp > 180 )
 439                 tmp -= 360;
 440             else if ( tmp < -180 )
 441                 tmp += 360;
 442             return v1 + tmp * ratio;
 443         }
 444
 445         case angular_rad:
 446         {
 447             // special handling of angular data
 448             double tmp = v2 - v1;
 449             if ( tmp > SGD_PI )
 450                 tmp -= SGD_2PI;
 451             else if ( tmp < -SGD_PI )
 452                 tmp += SGD_2PI;
 453             return v1 + tmp * ratio;
 454         }
 455
 456         case discrete:
 457             // fall through
 458         default:
 459             return v2;
 460     }
 461 }
 462
 463 /** Replay.
 464  * Restore all properties with data from given buffer. */
 465 void
 466 FGFlightRecorder::replay(double SimTime, const FGReplayData* _pNextBuffer, const FGReplayData* _pLastBuffer)
 467 {
 468     const char* pLastBuffer = (const char*) _pLastBuffer;
 469     const char* pBuffer = (const char*) _pNextBuffer;
 470     if (!pBuffer)
 471         return;
 472
 473     int Offset = 0;
 474     double ratio = 1.0;
 475     if (pLastBuffer)
 476     {
 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))
 483         {
 484             ratio = Numerator / dt;
 485             if (ratio > 1.0)
 486                 ratio = 1.0;
 487         }
 488     }
 489
 490     Offset += sizeof(double);
 491
 492     // 64bit aligned data first!
 493     {
 494         // restore doubles
 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++)
 499         {
 500             double v = pDoubles[i];
 501             if (pLastBuffer)
 502             {
 503                 v = weighting(m_CaptureDouble[i].Interpolation, ratio,
 504                               pLastDoubles[i], v);
 505             }
 506             m_CaptureDouble[i].Signal->setDoubleValue(v);
 507         }
 508         Offset += SignalCount * sizeof(double);
 509     }
 510
 511     // 32bit aligned data comes second...
 512     {
 513         // restore floats
 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++)
 518         {
 519             float v = pFloats[i];
 520             if (pLastBuffer)
 521             {
 522                 v = weighting(m_CaptureFloat[i].Interpolation, ratio,
 523                               pLastFloats[i], v);
 524             }
 525             m_CaptureFloat[i].Signal->setDoubleValue(v);//setFloatValue
 526         }
 527         Offset += SignalCount * sizeof(float);
 528     }
 529
 530     {
 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++)
 535         {
 536             m_CaptureInteger[i].Signal->setIntValue(pInt[i]);
 537         }
 538         Offset += SignalCount * sizeof(int);
 539     }
 540
 541     // 16bit aligned data is next...
 542     {
 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++)
 547         {
 548             m_CaptureInt16[i].Signal->setIntValue(pShortInt[i]);
 549         }
 550         Offset += SignalCount * sizeof(short int);
 551     }
 552
 553     // finally: byte aligned data is last...
 554     {
 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++)
 559         {
 560             m_CaptureInt8[i].Signal->setIntValue(pChar[i]);
 561         }
 562         Offset += SignalCount * sizeof(signed char);
 563     }
 564
 565     {
 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;
 570         Offset += Size;
 571         for (unsigned int i=0; i<SignalCount; i++)
 572         {
 573             m_CaptureBool[i].Signal->setBoolValue(0 != (pFlags[i>>3] & (1 << (i&7))));
 574         }
 575     }
 576 }
 577
 578 int
 579 FGFlightRecorder::getConfig(SGPropertyNode* root, const char* typeStr, const FlightRecorder::TSignalList& SignalList)
 580 {
 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++)
 585     {
 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());
 590     }
 591     SG_LOG(SG_SYSTEMS, SG_DEBUG, "FlightRecorder: Have " << SignalCount << " signals of type " << typeStr);
 592     root->setIntValue(typeStr, SignalCount);
 593     return SignalCount;
 594 }
 595
 596 void
 597 FGFlightRecorder::getConfig(SGPropertyNode* root)
 598 {
 599     root->setStringValue("name", m_RecorderNode->getStringValue("active-config-name", ""));
 600     int SignalCount = 0;
 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);
 607
 608     root->setIntValue("recorder/record-size", getRecordSize());
 609     root->setIntValue("recorder/signal-count", SignalCount);
 610 }