TrawlGeometryController.h

#pragma once
 
#include "sfh/filters/FIR.h"
 
#include <array>
#include <fhsim/simobject/SimObject.h>
#include <memory>
#include <string>
#include <vector>
 
class TrawlGeometryController : public SimObject
{
public:
   TrawlGeometryController(const std::string& simObjectName, ISimObjectCreator* const creator);
 
   void OdeFcn(const double T, const double* const X, double* const XDot) const override;
   void InitialConditionSetup(const double T, const double* const currentIC, double* const updatedIC, ISimObjectCreator* const creator) override;
   void AcceptedStep(const double T, const double* const X) override;
 
#ifdef FH_VISUALIZATION
   void RenderInit(Ogre::Root* const ogreRoot, ISimObjectCreator* const creator) { }
   void RenderUpdate(const double T, const double* const X) { }
#endif
 
protected:
   class RateLimitedFIR
   {
   public:
      RateLimitedFIR(int size, int order, double updatePeriod);
      bool          AddMeasurement(const double measurement[], double time);
      const double* GetValue() { return m_filter.GetValue(); }
 
   private:
      sfh::filters::FIR m_filter;
      double            m_updatePeriod;
      double            m_lastUpdate = 0.0;
      bool              m_hasUpdate  = false;
   };
 
   std::array<double, 2> MeanWingPosition(const double T, const double* const X) const;
 
   std::vector<double> WarpsTooLong(const double T, const double* const X) const;
 
   double BottomContactError() const;
 
   const double* OutWarpLength(const double T, const double* const X, const int index);
   const double* OutNetWidth(const double T, const double* const X, const int index);
   const double* OutMinBottomContact(const double T, const double* const X);
 
   std::vector<ISignalPort*>       m_inNetPortPos;
   std::vector<ISignalPort*>       m_inNetStbPos;
   std::vector<ISignalPort*>       m_inBottomContact;
   ISignalPort*                    m_inWarpLength     = nullptr;
   ISignalPort*                    m_inExternalEnable = nullptr;
   std::unique_ptr<RateLimitedFIR> m_bottomContactFilter;
 
   int    m_numNets;
   int    m_numWarps;
   int    m_numBottomContactMeasurements;
   bool   m_controlBottomContact;
   double m_initialWarpLength = 0.0;
   double m_minBottomContactGoal;
   double m_sinkTimeConstant;
   double m_controllerMaxSpeedDepth;
   double m_controllerMaxSpeedGeometry;
   double m_controllerConstant;
   double m_controllerDamping;
   int    m_indexStateWarpLength;
 
   // Updated in AcceptedStep.
   bool                  m_hasLastStep            = false;
   double                m_lastTime               = 0.0;
   double                m_minBottomContact       = 0.0;
   double                m_lastBottomContactError = 0.0;
   double                m_bottomContactErrorRate = 0.0; 
   double                m_lastBottomContactUpdate = 0.0;
   bool                  m_hasBottomContactUpdate  = false;
   std::vector<double>   m_lastWarpTooLong;
   bool                  m_hasTravelReference = false;
   std::array<double, 2> m_travelReference    = {0.0, 0.0}; 
   std::array<double, 2> m_travelDirection    = {0.0, 0.0}; 
 
   double m_outNetWidth = 0.0;
};