simpleRotaryWingPilot.h

Go to the documentation of this file.

/*******************************************************************************
** Copyright (c) 2006 MAK Technologies, Inc.
** All rights reserved.
*******************************************************************************/



#pragma once

#include "vrfmodel/rotaryWingPilotController.h"
#include <matrix/vlDcm.h>
#include <matrix/vlVector.h>
#include "vrfmodel/vrfmodelDefines.h"

class DtString;
class DtSimulationServices;
class DtRotaryWingPilotDescriptor;
class Coordinate_System;
class DtVectorInputPort;
class DtRotaryWingFlightPSR;

class DT_DLL_vrfmodel DtSimpleRotaryWingPilot : public DtRotaryWingPilotController
{
public:

   DtSimpleRotaryWingPilot(const DtString& name, 
      DtLocalObject* owner,
      DtSimulationServices* simManager, 
      DtComponentDescriptor* desc = 0,
      DtReaderWriterRegistry* parentRegistry = 0);

   virtual ~DtSimpleRotaryWingPilot();

   virtual bool init();

   virtual bool createPorts();

   virtual const char* type() const;

   virtual void tick();

   static DtSimComponent * creator(const DtString& name, 
      DtLocalObject* owner,
      DtSimulationServices* simManager, 
      DtComponentDescriptor* desc = 0,
      DtReaderWriterRegistry* parentRegistry = 0);

   virtual bool taskVisualizationsActive() const;
   virtual void updateTaskVisualizations();

private:
   DtSimpleRotaryWingPilot();

   DtSimpleRotaryWingPilot(const DtSimpleRotaryWingPilot& orig);

   DtSimpleRotaryWingPilot& operator=(const DtSimpleRotaryWingPilot& orig);

protected:
   virtual void computeDesiredLinearAccel(DtVector& desiredHorizBodyAccel,
      double& desiredYawAccel);

   virtual void computeDesiredTopoAccelToPosition(const double maxAccel, const double period,
      DtVector& desiredTopoAccel, double& yawAccel);

   virtual void computeDesiredTopoAccelToVelocity(const DtVector& desiredVel, const double period,
      DtVector& desiredTopoAccel);

   virtual double desiredHeadingWithNoMoveGoal();

   virtual void computeDesiredTopoAccelToHeading(const double desiredHeading, const double desiredSpeed,
      const double period, DtVector& desiredTopoAccel);


   virtual void checkLandNoFuel();

   virtual void findEarthCurveDeltaPosition(DtVector fromPosition,
      DtVector toPosition, DtVector& deltaPosition,
      const Coordinate_System* coordinateSystem);

   virtual void limitAccelFromMaxForwardSpeed(const DtVector& topoVelocity,
      const DtVector& entityToGoal,
      const double speedAtGoal,
      const double distToGoal,
      const double period,
      DtVector& desiredTopoAccel);

   virtual void limitAccelForGoalDistanceAndVelocity(
      const DtVector& goalVelocity, const DtVector& horizBodyEntToGoal,
      const DtVector& horizBodyCurrVelocity, const double period,
      DtVector& desiredAccel);


   //virtual double limitAccelFromMaxAcceleration(const double desiredAccel);
  
   virtual void limitAccelFromMaxAcceleration(DtVector& horizBodyAccel);

   virtual void computeTopoDeltaPosition(DtVector& topoDeltaPosition);

   virtual void computeTopoDesiredEndVelocity(const DtVector& topoXyDeltaPos,
      DtVector& topoXyEndDirection,
      double& topoXyEndSpeed);

   virtual double limitedVerticalControlAcceleration(const double verticalControlAccel,
      const double verticalError, const double currentVerticalVelocity, const double period);

   virtual bool isCloseToGoalPosition(const DtVector& topoDeltaPos);


   virtual void setCollectiveAndCyclicFromAccel(const DtVector& desiredAcceleration);

   virtual void setPedalFromDesiredYawAcceleration(const double yawAccel);

   virtual void setThrottle();


   void DtSimpleRotaryWingPilot::limitAccelForFixedFlightSlope(DtVector& topoEntToGoal,
      DtVector& topoCurrentVel, double period, DtVector& desiredTopoAccel);
      
   virtual DtReal terrainAvoidanceVelocity() const;

   virtual void calculateTargetAlignmentAngle(DtReal& headingToTarget) const;

   virtual void computeYawAccelerationToHeading(const double headingToPoint,
      const double period,
      double& yawAccel);

   virtual void getTransformationMatrices();

   virtual void clearControlState();


   virtual double stationaryGoalSpeedThreshold(){ return 0.5; }

   virtual double catchUpSpeedFactor(){ return 1.5; }

   virtual void lookupAndCacheProcessSR();

   virtual bool postAddComponentsInit() override;

  protected:

   DtVectorInputPort* myTargetLocationInputPort;

   DtRotaryWingPilotDescriptor *myPilotDescriptor;

   DtDcm myBodyToRef;

   DtDcm myLocalToTopoDcm;

   DtDcm myTopoToLocalDcm;

   DtDcm myTopoToBodyHeadingDcm;

   DtRotaryWingFlightPSR*              myProcessState;

   DtVector myLastDistError;
   DtVector myLastIntegralDistError;

   double myLastYawError;
   

public:

   static DtReal xyDistSquared(DtVector a, DtVector b);

};