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