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