WsfNavigationErrors.hpp

Go to the documentation of this file.

// ****************************************************************************
// CUI
//
// The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
//
// Copyright 2003-2015 The Boeing Company. All rights reserved.
//
// The use, dissemination or disclosure of data in this file is subject to
// limitation or restriction. See accompanying README and LICENSE for details.
// ****************************************************************************
// ****************************************************************************
// Updated by Infoscitex, a DCS Company.
// ****************************************************************************
 
#ifndef WSFNAVIGATIONERRORS_HPP
#define WSFNAVIGATIONERRORS_HPP
 
#include "wsf_export.h"
 
#include <fstream>
#include <string>
#include <vector>
 
#include "UtCoords.hpp"
#include "UtEntity.hpp"
class UtInput;
#include "UtLog.hpp"
#include "UtRandom.hpp"
class WsfPlatform;
class WsfScenario;
#include "WsfSimpleComponent.hpp"
class WsfSimulation;
#include "WsfSinglePlatformObserver.hpp"
#include "WsfVariable.hpp"

class WSF_EXPORT WsfNavigationErrors : public WsfSimplePlatformComponent, public WsfSinglePlatformObserver
{
public:
   static void                 RegisterComponentFactory(WsfScenario& aScenario);
   static WsfNavigationErrors* Find(const WsfPlatform& aPlatform);
 
   WsfNavigationErrors();
   WsfNavigationErrors(const WsfNavigationErrors& aSrc);
   WsfNavigationErrors& operator=(const WsfNavigationErrors& aRhs);
   ~WsfNavigationErrors() override;


   WsfComponent* CloneComponent() const override { return new WsfNavigationErrors(*this); }
   void*         QueryInterface(int aRole) override { return (aRole == mRoles[0]) ? this : nullptr; }
   int           GetComponentInitializationOrder() const override { return cWSF_INITIALIZE_ORDER_NAVIGATION_ERRORS; }
   void          ComponentParentChanged(WsfPlatform* aPlatformPtr) override { mPlatformPtr = aPlatformPtr; }


   bool Initialize(double aSimTime) override;
   bool Initialize2(double aSimTime) override;
   bool ProcessInput(UtInput& aInput) override;
   void Update(double aSimTime);


   void OnPlatformUpdated(double aSimTime, WsfPlatform* aPlatformPtr) override;

   double GetLastUpdateTime() const { return mLastUpdateTime; }
 
   ut::coords::RSCS GetLocationErrorRSCS() const;
   ut::coords::WCS  GetLocationErrorWCS() const;
   void             SetLocationErrorWCS(const ut::coords::WCS& aLocationErrorWCS);
 
   ut::coords::WCS GetPerceivedLocationWCS() const;
   void            GetPerceivedLocationLLA(double& aLat, double& aLon, double& aAlt) const;
 
   UtEntity& GetPerceivedEntity() { return mPerceivedEntity; }
 
   void Handoff(double aSimTime, WsfNavigationErrors* aSourcePtr);
 
   enum GPS_Status
   {
      cGPS_INACTIVE = -1,
      cGPS_PERFECT  = 0,
      cGPS_ACTIVE   = 1,
      cGPS_DEGRADED = 2,
      cGPS_EXTERNAL = 3
   };
 
   virtual bool SetGPS_Status(double aSimTime, GPS_Status aStatus);
   GPS_Status   GetGPS_Status() const { return mGPS_Status; }
 
   static GPS_Status GetGPS_Status(int aStatus);
 
   WsfSimulation* GetSimulation() const;

   class Term
   {
   public:
      Term()
         : mCoefficient(0.0)
         , mExponent(0.0)
         , mIntExponent(-0)
      {
      }
 
      Term(double aCoefficient, double aExponent)
         : mCoefficient(aCoefficient)
         , mExponent(aExponent)
         , mIntExponent(-1)
      {
         int intExponent = static_cast<int>(mExponent);
         if (static_cast<double>(intExponent) == mExponent)
         {
            mIntExponent = intExponent;
         }
      }
      double mCoefficient;
      double mExponent;
      int    mIntExponent;
   };
 
   // Deprecated methods
   WSF_DEPRECATED void GetLocationErrorWCS(double aLocationErrorWCS[3]) const;
 
   WSF_DEPRECATED void GetLocationErrorXYZ(double aLocationErrorXYZ[3]) const;
 
   WSF_DEPRECATED void GetPerceivedLocationWCS(double aLocationWCS[3]) const;
 
protected:
   typedef std::vector<Term> Polynomial;
 
   virtual void InitializeGPS_Status(double aSimTime);
 
   ut::coords::RSCS ConvertNED_ToRSCS(UtEntity* aEntityPtr, const ut::coords::NED& aLocNED) const;
 
   ut::coords::NED ConvertRSCS_ToNED(UtEntity* aEntityPtr, const ut::coords::RSCS& aLocRSCS) const;
 
   double GlobalGaussian();
   double LocalGaussian();
 
   void          CompressPolynomial(Polynomial& aTerms);
   inline double EvaluatePolynomial(double aT, const Polynomial& aTerms) const;
   void          ReadPolynomial(UtInput& aInput, Polynomial& aTerms);
   std::string   StatusString(int aStatus) const;
 
   virtual void GPS_Update(double aSimTime);
 
   void INS_Initialize(double aSimTime);
   void INS_PrintEquations(const std::string& aString, const Polynomial aTerms[3], ut::log::MessageStream& aStream) const;
   void INS_Start(double aSimTime);
   void INS_Update(double aSimTime);
 
   void WriteTimeHistory(double aSimTime);

   WsfPlatform* mPlatformPtr;

   GPS_Status       mGPS_Status;
   WsfVariable<int> mGPS_StatusVar;

   WsfVariable<double> mGPS_InTrackError;

   WsfVariable<double> mGPS_CrossTrackError;

   WsfVariable<double> mGPS_VerticalError;

   WsfVariable<double> mGPS_DegradedMultiplier;

   WsfVariable<double> mINS_GyroscopeBiasError;

   WsfVariable<double> mINS_AccelerometerBiasError;

   WsfVariable<double> mINS_RandomWalkError;

   WsfVariable<double> mINS_VerticalError;


   Polynomial mINS_LocError[3];
   Polynomial mINS_VelError[3];
   Polynomial mINS_AclError[3];

   ut::coords::WCS mExternalLocationErrorWCS;

   double mINS_ScaleFactor[3];

   double mLastUpdateTime;

   double mINS_StartTime;

   bool mRandomnessEnabled;

   bool mShowStatusChanges;

   bool mPerceivedEntityValid;

   bool mINS_ErrorsInitialized;

   UtEntity mPerceivedEntity;

   UtEntity mTruthEntity;

   ut::Random mRandom;

   std::string mTimeHistoryPath;

   std::ofstream mTimeHistoryOfs;
};
 
WSF_DECLARE_COMPONENT_ROLE_TYPE(WsfNavigationErrors, cWSF_COMPONENT_NAVIGATION_ERRORS)
 
#endif