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