From d64bcf0175dfbd44186e1454c1ba068a00eed80b Mon Sep 17 00:00:00 2001 From: ThorstenB Date: Sat, 1 Oct 2011 22:41:53 +0200 Subject: [PATCH] Replay upgrade, part I: Add new flight recorder. --- projects/VC90/FlightGear/FlightGear.vcproj | 8 + src/Aircraft/CMakeLists.txt | 4 +- src/Aircraft/Makefile.am | 3 +- src/Aircraft/flightrecorder.cxx | 544 +++++++++++++++++++++ src/Aircraft/flightrecorder.hxx | 89 ++++ 5 files changed, 646 insertions(+), 2 deletions(-) create mode 100644 src/Aircraft/flightrecorder.cxx create mode 100644 src/Aircraft/flightrecorder.hxx diff --git a/projects/VC90/FlightGear/FlightGear.vcproj b/projects/VC90/FlightGear/FlightGear.vcproj index a4c24e46a..6859879e4 100644 --- a/projects/VC90/FlightGear/FlightGear.vcproj +++ b/projects/VC90/FlightGear/FlightGear.vcproj @@ -393,6 +393,14 @@ RelativePath="..\..\..\src\Aircraft\replay.hxx" > + + + + +#include +#include + +#include +#include +#include +#include +#include +#include
+#include "flightrecorder.hxx" + +using namespace FlightRecorder; + +FGFlightRecorder::FGFlightRecorder(const char* pConfigName) : + m_RecorderNode(fgGetNode("/sim/flight-recorder", true)), + m_TotalRecordSize(0), + m_ConfigName(pConfigName) +{ +} + +FGFlightRecorder::~FGFlightRecorder() +{ +} + +void +FGFlightRecorder::reinit(void) +{ + m_ConfigNode = 0; + + m_TotalRecordSize = 0; + + m_CaptureDouble.clear(); + m_CaptureFloat.clear(); + m_CaptureInteger.clear(); + m_CaptureInt16.clear(); + m_CaptureInt8.clear(); + m_CaptureBool.clear(); + + int Selected = m_RecorderNode->getIntValue(m_ConfigName, 0); + SG_LOG(SG_SYSTEMS, SG_INFO, "FlightRecorder: Recorder configuration #" << Selected); + if (Selected >= 0) + m_ConfigNode = m_RecorderNode->getChild("config", Selected); + + if (!m_ConfigNode.valid()) + initDefault(); + + if (!m_ConfigNode.valid()) + { + SG_LOG(SG_SYSTEMS, SG_ALERT, "FlightRecorder: Configuration is invalid. Flight recorder disabled."); + } + else + { + // set name of active flight recorder type + const char* pRecorderName = + m_ConfigNode->getStringValue("name", + "aircraft-specific flight recorder"); + SG_LOG(SG_SYSTEMS, SG_INFO, "FlightRecorder: Using custom recorder configuration: " << pRecorderName); + m_RecorderNode->setStringValue("active-config-name", pRecorderName); + + // get signals + initSignalList("double", m_CaptureDouble, m_ConfigNode ); + initSignalList("float", m_CaptureFloat , m_ConfigNode ); + initSignalList("int", m_CaptureInteger, m_ConfigNode ); + initSignalList("int16", m_CaptureInt16 , m_ConfigNode ); + initSignalList("int8", m_CaptureInt8 , m_ConfigNode ); + initSignalList("bool", m_CaptureBool , m_ConfigNode ); + } + + // calculate size of a single record + m_TotalRecordSize = sizeof(double) * 1 /* sim time */ + + sizeof(double) * m_CaptureDouble.size() + + sizeof(float) * m_CaptureFloat.size() + + sizeof(int) * m_CaptureInteger.size() + + sizeof(short int) * m_CaptureInt16.size() + + sizeof(signed char) * m_CaptureInt8.size() + + sizeof(unsigned char) * ((m_CaptureBool.size()+7)/8); // 8 bools per byte + + // expose size of actual flight recorder record + m_RecorderNode->setIntValue("record-size", m_TotalRecordSize); + SG_LOG(SG_SYSTEMS, SG_INFO, "FlightRecorder: record size is " << m_TotalRecordSize << " bytes"); +} + +/** Check if SignalList already contains the given property */ +bool +FGFlightRecorder::haveProperty(FlightRecorder::TSignalList& SignalList,SGPropertyNode* pProperty) +{ + unsigned int Count = SignalList.size(); + for (unsigned int i=0; igetStringValue("default-config",NULL); + if (!Path) + { + SG_LOG(SG_SYSTEMS, SG_ALERT, "FlightRecorder: No default flight recorder specified! Check preferences.xml!"); + } + else + { + SGPath path = globals->resolve_aircraft_path(Path); + if (path.isNull()) + { + SG_LOG(SG_SYSTEMS, SG_ALERT, "FlightRecorder: Cannot find file '" << Path << "'."); + } + else + { + try + { + readProperties(path.str(), m_RecorderNode->getChild("config", 0 ,true), 0); + m_ConfigNode = m_RecorderNode->getChild("config", 0 ,false); + } catch (sg_io_exception &e) + { + SG_LOG(SG_SYSTEMS, SG_ALERT, "FlightRecorder: Error reading file '" << + Path << ": " << e.getFormattedMessage()); + } + } + } +} + +/** Read signal list below given base node. + * Only process properties of given signal type and add all signals to the given list. + * This method is called for all supported signal types - properties of each type are + * kept in separate lists for efficiency reasons. */ +void +FGFlightRecorder::initSignalList(const char* pSignalType, TSignalList& SignalList, SGPropertyNode_ptr BaseNode) +{ + // clear old signals + SignalList.clear(); + + processSignalList(pSignalType, SignalList, BaseNode); + + SG_LOG(SG_SYSTEMS, SG_DEBUG, "FlightRecorder: " << SignalList.size() << " signals of type " << pSignalType ); +} + +/** Process signal list below given base node. + * Only process properties of given signal type and add all signals to the given list. + * This method is called for all supported signal types - properties of each type are + * kept in separate lists for efficiency reasons. */ +void +FGFlightRecorder::processSignalList(const char* pSignalType, TSignalList& SignalList, SGPropertyNode_ptr SignalListNode, + string PropPrefix, int Count) +{ + // get the list of signal sources (property paths) for this signal type + SGPropertyNode_ptr SignalNode; + int Index=0; + + Count = SignalListNode->getIntValue("count",Count); + PropPrefix = simgear::strutils::strip(SignalListNode->getStringValue("prefix",PropPrefix.c_str())); + if ((!PropPrefix.empty())&&(PropPrefix[PropPrefix.size()-1] != '/')) + PropPrefix += "/"; + + do + { + SignalNode = SignalListNode->getChild("signal",Index,false); + if (SignalNode.valid()&& + (0==strcmp(pSignalType, SignalNode->getStringValue("type","float")))) + { + string PropertyPath = SignalNode->getStringValue("property",""); + if (!PropertyPath.empty()) + { + PropertyPath = PropPrefix + PropertyPath; + const char* pInterpolation = SignalNode->getStringValue("interpolation","linear"); + + // Check if current signal has a "%i" place holder. Otherwise count is 1. + string::size_type IndexPos = PropertyPath.find("%i"); + int SignalCount = Count; + if (IndexPos == string::npos) + SignalCount = 1; + + for (int IndexValue=0;IndexValuegetChildren("signals"); + for (unsigned int i=0;isim_time = SimTime; + Offset += sizeof(double); + + // 64bit aligned data first! + { + // capture doubles + double* pDoubles = (double*) &pBuffer[Offset]; + unsigned int SignalCount = m_CaptureDouble.size(); + for (unsigned int i=0; igetDoubleValue(); + } + Offset += SignalCount * sizeof(double); + } + + // 32bit aligned data comes second... + { + // capture floats + float* pFloats = (float*) &pBuffer[Offset]; + unsigned int SignalCount = m_CaptureFloat.size(); + for (unsigned int i=0; igetFloatValue(); + } + Offset += SignalCount * sizeof(float); + } + + { + // capture integers (32bit aligned) + int* pInt = (int*) &pBuffer[Offset]; + unsigned int SignalCount = m_CaptureInteger.size(); + for (unsigned int i=0; igetIntValue(); + } + Offset += SignalCount * sizeof(int); + } + + // 16bit aligned data is next... + { + // capture 16bit short integers + short int* pShortInt = (short int*) &pBuffer[Offset]; + unsigned int SignalCount = m_CaptureInt16.size(); + for (unsigned int i=0; igetIntValue(); + } + Offset += SignalCount * sizeof(short int); + } + + // finally: byte aligned data is last... + { + // capture 8bit chars + signed char* pChar = (signed char*) &pBuffer[Offset]; + unsigned int SignalCount = m_CaptureInt8.size(); + for (unsigned int i=0; igetIntValue(); + } + Offset += SignalCount * sizeof(signed char); + } + + { + // capture 1bit booleans (8bit aligned) + unsigned char* pFlags = (unsigned char*) &pBuffer[Offset]; + unsigned int SignalCount = m_CaptureBool.size(); + int Size = (SignalCount+7)/8; + Offset += Size; + memset(pFlags,0,Size); + for (unsigned int i=0; igetBoolValue()) + pFlags[i>>3] |= 1 << (i&7); + } + } + + assert(Offset == m_TotalRecordSize); + + return (FGReplayData*) pBuffer; +} + +/** Do interpolation as defined by given interpolation type and weighting ratio. */ +static double +weighting(TInterpolation interpolation, double ratio, double v1,double v2) +{ + switch (interpolation) + { + case linear: + return v1 + ratio*(v2-v1); + + case angular_deg: + { + // special handling of angular data + double tmp = v2 - v1; + if ( tmp > 180 ) + tmp -= 360; + else if ( tmp < -180 ) + tmp += 360; + return v1 + tmp * ratio; + } + + case angular_rad: + { + // special handling of angular data + double tmp = v2 - v1; + if ( tmp > SGD_PI ) + tmp -= SGD_2PI; + else if ( tmp < -SGD_PI ) + tmp += SGD_2PI; + return v1 + tmp * ratio; + } + + case discrete: + // fall through + default: + return v2; + } +} + +/** Replay. + * Restore all properties with data from given buffer. */ +void +FGFlightRecorder::replay(double SimTime, const FGReplayData* _pNextBuffer, const FGReplayData* _pLastBuffer) +{ + const char* pLastBuffer = (const char*) _pLastBuffer; + const char* pBuffer = (const char*) _pNextBuffer; + if (!pBuffer) + return; + + int Offset = 0; + double ratio; + if (pLastBuffer) + { + double NextSimTime = _pNextBuffer->sim_time; + double LastSimTime = _pLastBuffer->sim_time; + ratio = (SimTime - LastSimTime) / (NextSimTime - LastSimTime); + } + else + { + ratio = 1.0; + } + + Offset += sizeof(double); + + // 64bit aligned data first! + { + // restore doubles + const double* pDoubles = (const double*) &pBuffer[Offset]; + const double* pLastDoubles = (const double*) &pLastBuffer[Offset]; + unsigned int SignalCount = m_CaptureDouble.size(); + for (unsigned int i=0; isetDoubleValue(v); + } + Offset += SignalCount * sizeof(double); + } + + // 32bit aligned data comes second... + { + // restore floats + const float* pFloats = (const float*) &pBuffer[Offset]; + const float* pLastFloats = (const float*) &pLastBuffer[Offset]; + unsigned int SignalCount = m_CaptureFloat.size(); + for (unsigned int i=0; isetDoubleValue(v);//setFloatValue + } + Offset += SignalCount * sizeof(float); + } + + { + // restore integers (32bit aligned) + const int* pInt = (const int*) &pBuffer[Offset]; + unsigned int SignalCount = m_CaptureInteger.size(); + for (unsigned int i=0; isetIntValue(pInt[i]); + } + Offset += SignalCount * sizeof(int); + } + + // 16bit aligned data is next... + { + // restore 16bit short integers + const short int* pShortInt = (const short int*) &pBuffer[Offset]; + unsigned int SignalCount = m_CaptureInt16.size(); + for (unsigned int i=0; isetIntValue(pShortInt[i]); + } + Offset += SignalCount * sizeof(short int); + } + + // finally: byte aligned data is last... + { + // restore 8bit chars + const signed char* pChar = (const signed char*) &pBuffer[Offset]; + unsigned int SignalCount = m_CaptureInt8.size(); + for (unsigned int i=0; isetIntValue(pChar[i]); + } + Offset += SignalCount * sizeof(signed char); + } + + { + // restore 1bit booleans (8bit aligned) + const unsigned char* pFlags = (const unsigned char*) &pBuffer[Offset]; + unsigned int SignalCount = m_CaptureBool.size(); + int Size = (SignalCount+7)/8; + Offset += Size; + for (unsigned int i=0; isetBoolValue(0 != (pFlags[i>>3] & (1 << (i&7)))); + } + } +} diff --git a/src/Aircraft/flightrecorder.hxx b/src/Aircraft/flightrecorder.hxx new file mode 100644 index 000000000..a2045f64a --- /dev/null +++ b/src/Aircraft/flightrecorder.hxx @@ -0,0 +1,89 @@ +// flightrecorder.hxx +// +// Written by Thorsten Brehm, started August 2011. +// +// Copyright (C) 2011 Thorsten Brehm - brehmt (at) gmail com +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +// +/////////////////////////////////////////////////////////////////////////////// + +#ifndef FLIGHTRECORDER_HXX_ +#define FLIGHTRECORDER_HXX_ + +#include +#include "replay.hxx" + +namespace FlightRecorder +{ + + typedef enum + { + discrete = 0, // no interpolation + linear = 1, // linear interpolation + angular_rad = 2, // angular interpolation, value in radians + angular_deg = 3 // angular interpolation, value in degrees + } TInterpolation; + + typedef struct + { + SGPropertyNode_ptr Signal; + TInterpolation Interpolation; + } TCapture; + + typedef std::vector TSignalList; + +} + +class FGFlightRecorder +{ +public: + FGFlightRecorder(const char* pConfigName); + virtual ~FGFlightRecorder(); + + void reinit (void); + FGReplayData* createEmptyRecord (void); + FGReplayData* capture (double SimTime, FGReplayData* pRecycledBuffer); + void replay (double SimTime, const FGReplayData* pNextBuffer, + const FGReplayData* pLastBuffer = NULL); + void deleteRecord (FGReplayData* pRecord); + + int getRecordSize (void) { return m_TotalRecordSize;} + +private: + void initDefault(void); + void initSignalList(const char* pSignalType, FlightRecorder::TSignalList& SignalList, + SGPropertyNode_ptr BaseNode); + void processSignalList(const char* pSignalType, FlightRecorder::TSignalList& SignalList, + SGPropertyNode_ptr SignalListNode, + string PropPrefix="", int Count = 1); + bool haveProperty(FlightRecorder::TSignalList& Capture,SGPropertyNode* pProperty); + bool haveProperty(SGPropertyNode* pProperty); + + SGPropertyNode_ptr m_RecorderNode; + SGPropertyNode_ptr m_ConfigNode; + + FlightRecorder::TSignalList m_CaptureDouble; + FlightRecorder::TSignalList m_CaptureFloat; + FlightRecorder::TSignalList m_CaptureInteger; + FlightRecorder::TSignalList m_CaptureInt16; + FlightRecorder::TSignalList m_CaptureInt8; + FlightRecorder::TSignalList m_CaptureBool; + + int m_TotalRecordSize; + string m_ConfigName; +}; + +#endif /* FLIGHTRECORDER_HXX_ */