Module: sphericalPendulum

Executive Summary

This class in an instantiation of the state effector class and implements an effector representing spherical pendulum.

The module PDF Description contains further information on this module’s function, how to run it, as well as testing.

Message Connection Descriptions

This state effector does not have any input or output messages


class SphericalPendulum : public StateEffector, public SysModel, public FuelSlosh
#include <sphericalPendulum.h>

spherical pendulum state effector model

Public Functions

SphericalPendulum()

&#8212; Contructor

This is the constructor, setting variables to default values

~SphericalPendulum()

&#8212; Destructor

This is the destructor, nothing to report here

void registerStates(DynParamManager &states)

&#8212; Method for FSP to register its states

This is the method for the spherical pendulum to register its states: l and lDot

void linkInStates(DynParamManager &states)

&#8212; Method for FSP to get access of other states

Method for spherical pendulum to access the states that it needs. It needs gravity and the hub states

void updateEffectorMassProps(double integTime)

&#8212; Method for FSP to add its contributions to mass props

This is the method for the FSP to add its contributions to the mass props and mass prop rates of the vehicle

void modifyStates(double integTime)

&#8212; Method to force states modification during integration

void retrieveMassValue(double integTime)

This is method is used to pass mass properties information to the fuelTank

void updateContributions(double integTime, BackSubMatrices &backSubContr, Eigen::Vector3d sigma_BN, Eigen::Vector3d omega_BN_B, Eigen::Vector3d g_N)

&#8212; Back-sub contributions

This method is for the FSP to add its contributions to the back-sub method

void updateEnergyMomContributions(double integTime, Eigen::Vector3d &rotAngMomPntCContr_B, double &rotEnergyContr, Eigen::Vector3d omega_BN_B)

&#8212; Energy and momentum calculations

This method is for the FSP to add its contributions to energy and momentum

void computeDerivatives(double integTime, Eigen::Vector3d rDDot_BN_N, Eigen::Vector3d omegaDot_BN_B, Eigen::Vector3d sigma_BN)

&#8212; Method for each stateEffector to calculate derivatives

This method is used to define the derivatives of the FSP. One is the trivial kinematic derivative and the other is derived using the back-sub method

Public Members

double pendulumRadius

[m] distance between the center of the tank and the spherical pendulum mass

Eigen::Matrix3d D

[N*s/m] linear damping matrix for spherical pendulum

double phiDotInit

[rad/s] Initial value for spherical pendulum pendulum offset derivative

double thetaDotInit

[rad/s] Initial value for spherical pendulum pendulum offset derivative

double massInit

[m] Initial value for spherical pendulum pendulum mass

std::string nameOfPhiState

[-] Identifier for the phi state data container

std::string nameOfThetaState

[-] Identifier for the theta state data container

std::string nameOfPhiDotState

[-] Identifier for the phiDot state data container

std::string nameOfThetaDotState

[-] Identifier for the thetaDot state data container

std::string nameOfMassState

[-] Identifier for the mass state data container

Eigen::Vector3d d

[m] position vector from B point to tank center , T, in body frame

StateData *massState

&#8212; state data for the pendulums mass

Eigen::Vector3d pHat_01

&#8212; first vector of the P0 frame in B frame components

Eigen::Vector3d pHat_02

&#8212; second vector of the P0 frame in B frame components

Eigen::Vector3d pHat_03

&#8212; third vector of the P0 frame in B frame components

BSKLogger bskLogger

&#8212; BSK Logging

Private Members

double phiInit

[rad] Initial value for spherical pendulum pendulum offset

double thetaInit

[rad] Initial value for spherical pendulum pendulum offset

double phi

[rad] spherical pendulum displacement in P0 frame

double theta

[rad] spherical pendulum displacement in P0 frame

double phiDot

[rad/s] time derivative of displacement in P0 frame

double thetaDot

[rad/s] time derivative of displacement in P0 frame

double massFSP

[kg] mass of spherical pendulum pendulum

Eigen::Vector3d r_PcB_B

[m] position vector form B to center of mass location of pendulum

Eigen::Matrix3d rTilde_PcB_B

[m] tilde matrix of r_Pc_B

Eigen::Vector3d rPrime_PcB_B

[m/s] Body time derivative of r_Pc_B

Eigen::Matrix3d rPrimeTilde_PcB_B

[m/s] Tilde matrix of rPrime_PcB_B

Eigen::Vector3d aPhi

&#8212; Term needed for back-sub method

Eigen::Vector3d bPhi

&#8212; Term needed for back-sub method

Eigen::Vector3d aTheta

&#8212; Term needed for back-sub method

Eigen::Vector3d bTheta

&#8212; Term needed for back-sub method

double cPhi

&#8212; Term needed for back-sub method

double cTheta

&#8212; Term needed for back-sub method

Eigen::MatrixXd *g_N

[m/s^2] Gravitational acceleration in N frame components

Eigen::Vector3d l_B

[m] vector from the center of the tank to the spherical pendulum pendulum in B frame

Eigen::Vector3d lPrime_B

[m/s] derivative of l respect to B frame

Eigen::Vector3d lPrime_P0

[m/s] derivative of l in P0 frame

StateData *phiState

&#8212; state data for spherical pendulum displacement

StateData *thetaState

&#8212; state data for spherical pendulum displacement

StateData *phiDotState

&#8212; state data for time derivative of phi;

StateData *thetaDotState

&#8212; state data for time derivative of theta;

StateData *omegaState

&#8212; state data for the hubs omega_BN_B

StateData *sigmaState

&#8212; state data for the hubs sigma_BN

StateData *velocityState

&#8212; state data for the hubs rDot_BN_N

Eigen::Matrix3d dcm_B_P0

Private Static Attributes

static uint64_t effectorID = 1

[] ID number of this panel