Public Member Functions | Private Member Functions | Private Attributes | List of all members
VtolDynamics Class Reference

Vtol dynamics simulator class. More...

#include <vtolDynamicsSim.hpp>

Inheritance diagram for VtolDynamics:
Inheritance graph
[legend]

Public Member Functions

void calculateAerodynamics (const Eigen::Vector3d &airspeed, double AoA, double AoS, const std::array< double, 3 > &servos, Eigen::Vector3d &Faero, Eigen::Vector3d &Maero)
 
double calculateAnglesOfAtack (const Eigen::Vector3d &airSpeed) const
 
double calculateAnglesOfSideslip (const Eigen::Vector3d &airSpeed) const
 
Eigen::Vector3d calculateAngularAccel (const Eigen::Matrix< double, 3, 3, Eigen::RowMajor > &inertia, const Eigen::Vector3d &moment, const Eigen::Vector3d &prevAngVel) const
 
void calculateCDPolynomial (double airSpeedMod, Eigen::VectorXd &polynomialCoeffs) const
 
void calculateCLPolynomial (double airSpeedMod, Eigen::VectorXd &polynomialCoeffs) const
 
double calculateCmxAileron (double aileron_pos, double airspeed) const
 
void calculateCmxPolynomial (double airSpeedMod, Eigen::VectorXd &polynomialCoeffs) const
 
double calculateCmyElevator (double elevator_pos, double airspeed) const
 
void calculateCmyPolynomial (double airSpeedMod, Eigen::VectorXd &polynomialCoeffs) const
 
void calculateCmzPolynomial (double airSpeedMod, Eigen::VectorXd &polynomialCoeffs) const
 
double calculateCmzRudder (double rudder_pos, double airspeed) const
 
double calculateCSBeta (double AoS_deg, double airspeed) const
 
void calculateCSPolynomial (double airSpeedMod, Eigen::VectorXd &polynomialCoeffs) const
 
double calculateCSRudder (double rudder_pos, double airspeed) const
 
double calculateDynamicPressure (double airSpeedMod) const
 
void calculateNewState (const Eigen::Vector3d &Maero, const Eigen::Vector3d &Faero, const std::vector< double > &motors, double dt_sec)
 
Eigen::Vector3d calculateNormalForceWithoutMass () const
 
Eigen::Matrix3d calculateRotationMatrix () const
 
Eigen::Vector3d calculateWind ()
 
int8_t calibrate (SimMode_t calibrationType) override
 
Eigen::Vector3d getAngularAcceleration () const
 
Eigen::Vector3d getBodyLinearVelocity () const
 
const Forces & getForces () const
 
void getIMUMeasurement (Eigen::Vector3d &accOut, Eigen::Vector3d &gyroOut) override
 
Eigen::Vector3d getLinearAcceleration () const
 
const Moments & getMoments () const
 
bool getMotorsRpm (std::vector< double > &motorsRpm) override
 
Eigen::Vector3d getVehicleAirspeed () const override
 
Eigen::Vector3d getVehicleAngularVelocity () const override
 
Eigen::Quaterniond getVehicleAttitude () const override
 
Eigen::Vector3d getVehiclePosition () const override
 
Eigen::Vector3d getVehicleVelocity () const override
 
int8_t init () override
 Use rosparam here to initialize sim. More...
 
void land () override
 
void process (double dt_secs, const std::vector< double > &unitless_setpoint) override
 
void setInitialPosition (const Eigen::Vector3d &position, const Eigen::Quaterniond &attitudeXYZW) override
 
void setInitialVelocity (const Eigen::Vector3d &linearVelocity, const Eigen::Vector3d &angularVelocity)
 
void setWindParameter (Eigen::Vector3d windMeanVelocityNED, double wind_velocityVariance) override
 
void thruster (double actuator, double &thrust, double &torque, double &rpm) const
 
 VtolDynamics ()
 
 ~VtolDynamics () final=default
 
- Public Member Functions inherited from UavDynamicsSimBase
 UavDynamicsSimBase ()=default
 
virtual ~UavDynamicsSimBase ()=default
 

Private Member Functions

void _mapUnitlessSetpointToInternal (const std::vector< double > &cmd)
 cmd is flexible, but it follows these rules: More...
 
Eigen::Vector3d calculateAirSpeed (const Eigen::Matrix3d &rotationMatrix, const Eigen::Vector3d &estimatedVelocity, const Eigen::Vector3d &windSpeed) const
 
void loadMotorsGeometry (const std::string &path)
 
void loadParams (const std::string &path)
 
void loadTables (const std::string &path)
 
void updateActuators (double dtSecs)
 

Private Attributes

std::normal_distribution< double > _distribution {0.0, 1.0}
 
Environment _environment
 
std::default_random_engine _generator
 
std::vector< double > _motorsSpeed
 
VtolParameters _params
 
std::array< double, 3 > _servosValues {0.0, 0.0, 0.0}
 
State _state
 
TablesWithCoeffs _tables
 

Additional Inherited Members

- Public Types inherited from UavDynamicsSimBase
enum  SimMode_t {
  SimMode_t::NORMAL = 0, SimMode_t::MAG_1_NORMAL = 1, SimMode_t::MAG_2_OVERTURNED = 2, SimMode_t::MAG_3_HEAD_DOWN = 3,
  SimMode_t::MAG_4_HEAD_UP = 4, SimMode_t::MAG_5_TURNED_LEFT = 5, SimMode_t::MAG_6_TURNED_RIGHT = 6, SimMode_t::MAG_7_ARDUPILOT = 7,
  SimMode_t::MAG_8_ARDUPILOT = 8, SimMode_t::MAG_9_ARDUPILOT = 9, SimMode_t::ACC_1_NORMAL = 11, SimMode_t::ACC_2_OVERTURNED = 12,
  SimMode_t::ACC_3_HEAD_DOWN = 13, SimMode_t::ACC_4_HEAD_UP = 14, SimMode_t::ACC_5_TURNED_LEFT = 15, SimMode_t::ACC_6_TURNED_RIGHT = 16,
  SimMode_t::AIRSPEED = 21
}
 

Detailed Description

Vtol dynamics simulator class.

Definition at line 147 of file vtolDynamicsSim.hpp.

Constructor & Destructor Documentation

◆ VtolDynamics()

VtolDynamics::VtolDynamics ( )

Definition at line 37 of file vtolDynamicsSim.cpp.

◆ ~VtolDynamics()

VtolDynamics::~VtolDynamics ( )
finaldefault

Member Function Documentation

◆ _mapUnitlessSetpointToInternal()

void VtolDynamics::_mapUnitlessSetpointToInternal ( const std::vector< double > &  cmd)
private

cmd is flexible, but it follows these rules:

  • first 5 values are reserved for motors
  • if we have >= 8 values then 3 last values are reserved for servos
  • if setpoint size is less than required, follow the Implicit Zero Extension rule
    Note
    Setpoint mapping table: Index Type Input setpoint Internal representation [0; N-4] Motors [ 0.0, +1.0] -> [0.0, RAD_PER_SEC_MAX] N-3 Ailerons [-1.0, +1.0] -> [-MAX_RANGE, +MAX_RANGE] N-2 Elevators [-1.0, +1.0] -> [-MAX_RANGE, +MAX_RANGE] N-1 Rudders [-1.0, +1.0] -> [-MAX_RANGE, +MAX_RANGE]

Definition at line 327 of file vtolDynamicsSim.cpp.

◆ calculateAerodynamics()

void VtolDynamics::calculateAerodynamics ( const Eigen::Vector3d &  airspeed,
double  AoA,
double  AoS,
const std::array< double, 3 > &  servos,
Eigen::Vector3d &  Faero,
Eigen::Vector3d &  Maero 
)
Note
definitions: FD, CD - drug force and drug coeeficient respectively FL - lift force and lift coeeficient respectively FS - side force and side coeeficient respectively
Note
InnoDynamics from octave has some mistake in elevator logic It always generate non positive moment in both positive and negative position Temporary decision is to create positive moment in positive position and negative moment in negative position

Definition at line 434 of file vtolDynamicsSim.cpp.

◆ calculateAirSpeed()

Eigen::Vector3d VtolDynamics::calculateAirSpeed ( const Eigen::Matrix3d &  rotationMatrix,
const Eigen::Vector3d &  estimatedVelocity,
const Eigen::Vector3d &  windSpeed 
) const
private

Definition at line 384 of file vtolDynamicsSim.cpp.

◆ calculateAnglesOfAtack()

double VtolDynamics::calculateAnglesOfAtack ( const Eigen::Vector3d &  airspeed_frd) const
Returns
AoA it must be [0, 3.14] if angle is [0, +180] it must be [0, -3.14] if angle is [0, -180]

Definition at line 407 of file vtolDynamicsSim.cpp.

◆ calculateAnglesOfSideslip()

double VtolDynamics::calculateAnglesOfSideslip ( const Eigen::Vector3d &  airSpeed) const

Definition at line 418 of file vtolDynamicsSim.cpp.

◆ calculateAngularAccel()

Eigen::Vector3d VtolDynamics::calculateAngularAccel ( const Eigen::Matrix< double, 3, 3, Eigen::RowMajor > &  inertia,
const Eigen::Vector3d &  moment,
const Eigen::Vector3d &  prevAngVel 
) const

Definition at line 620 of file vtolDynamicsSim.cpp.

◆ calculateCDPolynomial()

void VtolDynamics::calculateCDPolynomial ( double  airSpeedMod,
Eigen::VectorXd &  polynomialCoeffs 
) const

Definition at line 587 of file vtolDynamicsSim.cpp.

◆ calculateCLPolynomial()

void VtolDynamics::calculateCLPolynomial ( double  airSpeedMod,
Eigen::VectorXd &  polynomialCoeffs 
) const

Definition at line 579 of file vtolDynamicsSim.cpp.

◆ calculateCmxAileron()

double VtolDynamics::calculateCmxAileron ( double  aileron_pos,
double  airspeed 
) const

Definition at line 609 of file vtolDynamicsSim.cpp.

◆ calculateCmxPolynomial()

void VtolDynamics::calculateCmxPolynomial ( double  airSpeedMod,
Eigen::VectorXd &  polynomialCoeffs 
) const

Definition at line 591 of file vtolDynamicsSim.cpp.

◆ calculateCmyElevator()

double VtolDynamics::calculateCmyElevator ( double  elevator_pos,
double  airspeed 
) const

Definition at line 612 of file vtolDynamicsSim.cpp.

◆ calculateCmyPolynomial()

void VtolDynamics::calculateCmyPolynomial ( double  airSpeedMod,
Eigen::VectorXd &  polynomialCoeffs 
) const

Definition at line 595 of file vtolDynamicsSim.cpp.

◆ calculateCmzPolynomial()

void VtolDynamics::calculateCmzPolynomial ( double  airSpeedMod,
Eigen::VectorXd &  polynomialCoeffs 
) const

Definition at line 599 of file vtolDynamicsSim.cpp.

◆ calculateCmzRudder()

double VtolDynamics::calculateCmzRudder ( double  rudder_pos,
double  airspeed 
) const

Definition at line 615 of file vtolDynamicsSim.cpp.

◆ calculateCSBeta()

double VtolDynamics::calculateCSBeta ( double  AoS_deg,
double  airspeed 
) const

Definition at line 606 of file vtolDynamicsSim.cpp.

◆ calculateCSPolynomial()

void VtolDynamics::calculateCSPolynomial ( double  airSpeedMod,
Eigen::VectorXd &  polynomialCoeffs 
) const

Definition at line 583 of file vtolDynamicsSim.cpp.

◆ calculateCSRudder()

double VtolDynamics::calculateCSRudder ( double  rudder_pos,
double  airspeed 
) const

Definition at line 603 of file vtolDynamicsSim.cpp.

◆ calculateDynamicPressure()

double VtolDynamics::calculateDynamicPressure ( double  airSpeedMod) const

Definition at line 398 of file vtolDynamicsSim.cpp.

◆ calculateNewState()

void VtolDynamics::calculateNewState ( const Eigen::Vector3d &  Maero,
const Eigen::Vector3d &  Faero,
const std::vector< double > &  motors,
double  dt_sec 
)

Definition at line 526 of file vtolDynamicsSim.cpp.

◆ calculateNormalForceWithoutMass()

Eigen::Vector3d VtolDynamics::calculateNormalForceWithoutMass ( ) const
Note
The methods below are should be public for test only think about making test as friend

Definition at line 574 of file vtolDynamicsSim.cpp.

◆ calculateRotationMatrix()

Eigen::Matrix3d VtolDynamics::calculateRotationMatrix ( ) const

Definition at line 380 of file vtolDynamicsSim.cpp.

◆ calculateWind()

Eigen::Vector3d VtolDynamics::calculateWind ( )
Note
Implement own gust logic innopolis_vtol_indi logic doesn't suit us

Definition at line 364 of file vtolDynamicsSim.cpp.

◆ calibrate()

int8_t VtolDynamics::calibrate ( SimMode_t  calibrationType)
overridevirtual

Reimplemented from UavDynamicsSimBase.

Definition at line 179 of file vtolDynamicsSim.cpp.

◆ getAngularAcceleration()

Eigen::Vector3d VtolDynamics::getAngularAcceleration ( ) const

Definition at line 676 of file vtolDynamicsSim.cpp.

◆ getBodyLinearVelocity()

Eigen::Vector3d VtolDynamics::getBodyLinearVelocity ( ) const

Definition at line 689 of file vtolDynamicsSim.cpp.

◆ getForces()

const Forces & VtolDynamics::getForces ( ) const
Note
For RVIZ visualization only

Definition at line 682 of file vtolDynamicsSim.cpp.

◆ getIMUMeasurement()

void VtolDynamics::getIMUMeasurement ( Eigen::Vector3d &  accOutFrd,
Eigen::Vector3d &  gyroOutFrd 
)
overridevirtual
Note
We consider that z=0 means ground, so if position <=0, Normal force is appeared, it means that in any way specific force will be equal to Gravity force.

Implements UavDynamicsSimBase.

Definition at line 652 of file vtolDynamicsSim.cpp.

◆ getLinearAcceleration()

Eigen::Vector3d VtolDynamics::getLinearAcceleration ( ) const

Definition at line 679 of file vtolDynamicsSim.cpp.

◆ getMoments()

const Moments & VtolDynamics::getMoments ( ) const

Definition at line 685 of file vtolDynamicsSim.cpp.

◆ getMotorsRpm()

bool VtolDynamics::getMotorsRpm ( std::vector< double > &  motorsRpm)
overridevirtual

Reimplemented from UavDynamicsSimBase.

Definition at line 693 of file vtolDynamicsSim.cpp.

◆ getVehicleAirspeed()

Eigen::Vector3d VtolDynamics::getVehicleAirspeed ( ) const
overridevirtual

Implements UavDynamicsSimBase.

Definition at line 635 of file vtolDynamicsSim.cpp.

◆ getVehicleAngularVelocity()

Eigen::Vector3d VtolDynamics::getVehicleAngularVelocity ( ) const
overridevirtual

Implements UavDynamicsSimBase.

Definition at line 645 of file vtolDynamicsSim.cpp.

◆ getVehicleAttitude()

Eigen::Quaterniond VtolDynamics::getVehicleAttitude ( ) const
overridevirtual
Note
These methods should return in FRD format

Implements UavDynamicsSimBase.

Definition at line 642 of file vtolDynamicsSim.cpp.

◆ getVehiclePosition()

Eigen::Vector3d VtolDynamics::getVehiclePosition ( ) const
overridevirtual
Note
These methods should return in NED format

Implements UavDynamicsSimBase.

Definition at line 629 of file vtolDynamicsSim.cpp.

◆ getVehicleVelocity()

Eigen::Vector3d VtolDynamics::getVehicleVelocity ( ) const
overridevirtual

Implements UavDynamicsSimBase.

Definition at line 632 of file vtolDynamicsSim.cpp.

◆ init()

int8_t VtolDynamics::init ( )
overridevirtual

Use rosparam here to initialize sim.

Returns
-1 if error occures and simulation can't start

Implements UavDynamicsSimBase.

Definition at line 48 of file vtolDynamicsSim.cpp.

◆ land()

void VtolDynamics::land ( )
overridevirtual

Reimplemented from UavDynamicsSimBase.

Definition at line 164 of file vtolDynamicsSim.cpp.

◆ loadMotorsGeometry()

void VtolDynamics::loadMotorsGeometry ( const std::string &  path)
private

Definition at line 112 of file vtolDynamicsSim.cpp.

◆ loadParams()

void VtolDynamics::loadParams ( const std::string &  path)
private

Definition at line 94 of file vtolDynamicsSim.cpp.

◆ loadTables()

void VtolDynamics::loadTables ( const std::string &  path)
private

Definition at line 75 of file vtolDynamicsSim.cpp.

◆ process()

void VtolDynamics::process ( double  dt_secs,
const std::vector< double > &  unitless_setpoint 
)
overridevirtual

Implements UavDynamicsSimBase.

Definition at line 300 of file vtolDynamicsSim.cpp.

◆ setInitialPosition()

void VtolDynamics::setInitialPosition ( const Eigen::Vector3d &  position,
const Eigen::Quaterniond &  attitudeXYZW 
)
overridevirtual

Implements UavDynamicsSimBase.

Definition at line 151 of file vtolDynamicsSim.cpp.

◆ setInitialVelocity()

void VtolDynamics::setInitialVelocity ( const Eigen::Vector3d &  linearVelocity,
const Eigen::Vector3d &  angularVelocity 
)

Definition at line 158 of file vtolDynamicsSim.cpp.

◆ setWindParameter()

void VtolDynamics::setWindParameter ( Eigen::Vector3d  windMeanVelocityNED,
double  windVariance 
)
overridevirtual
Note
These methods should be private

Reimplemented from UavDynamicsSimBase.

Definition at line 671 of file vtolDynamicsSim.cpp.

◆ thruster()

void VtolDynamics::thruster ( double  actuator,
double &  thrust,
double &  torque,
double &  rpm 
) const

Definition at line 507 of file vtolDynamicsSim.cpp.

◆ updateActuators()

void VtolDynamics::updateActuators ( double  dtSecs)
private

Definition at line 348 of file vtolDynamicsSim.cpp.

Member Data Documentation

◆ _distribution

std::normal_distribution<double> VtolDynamics::_distribution {0.0, 1.0}
private

Definition at line 244 of file vtolDynamicsSim.hpp.

◆ _environment

Environment VtolDynamics::_environment
private

Definition at line 241 of file vtolDynamicsSim.hpp.

◆ _generator

std::default_random_engine VtolDynamics::_generator
private

Definition at line 243 of file vtolDynamicsSim.hpp.

◆ _motorsSpeed

std::vector<double> VtolDynamics::_motorsSpeed
private

Definition at line 235 of file vtolDynamicsSim.hpp.

◆ _params

VtolParameters VtolDynamics::_params
private

Definition at line 238 of file vtolDynamicsSim.hpp.

◆ _servosValues

std::array<double, 3> VtolDynamics::_servosValues {0.0, 0.0, 0.0}
private

Definition at line 236 of file vtolDynamicsSim.hpp.

◆ _state

State VtolDynamics::_state
private

Definition at line 239 of file vtolDynamicsSim.hpp.

◆ _tables

TablesWithCoeffs VtolDynamics::_tables
private

Definition at line 240 of file vtolDynamicsSim.hpp.

The documentation for this class was generated from the following files:

inno_vtol_dynamics
Author(s): Roman Fedorenko, Dmitry Ponomarev, Ezra Tal, Winter Guerra
autogenerated on Mon Dec 9 2024 03:13:35