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