Vtol dynamics simulator class. More...

#include <vtolDynamicsSim.hpp>

Inheritance diagram for InnoVtolDynamicsSim:

Public Member Functions
void	calculateAerodynamics (const Eigen::Vector3d &airspeed, double AoA, double AoS, double aileron_pos, double elevator_pos, double rudder_pos, 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 > &actuator, 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	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...

	InnoVtolDynamicsSim ()

void	land () override

void	process (double dt_secs, const std::vector< double > &motorSpeedCommandIn, bool isCmdPercent) override

void	setInitialPosition (const Eigen::Vector3d &position, const Eigen::Quaterniond &attitude) override

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

void	setWindParameter (Eigen::Vector3d windMeanVelocity, double wind_velocityVariance)

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

	~InnoVtolDynamicsSim () final=default

Public Member Functions inherited from UavDynamicsSimBase
	UavDynamicsSimBase ()=default

virtual	~UavDynamicsSimBase ()=default

Private Member Functions
Eigen::Vector3d	calculateAirSpeed (const Eigen::Matrix3d &rotationMatrix, const Eigen::Vector3d &estimatedVelocity, const Eigen::Vector3d &windSpeed) const

void	loadParams (const std::string &path)

void	loadTables (const std::string &path)

std::vector< double >	mapCmdToActuatorInnoVTOL (const std::vector< double > &cmd) const

std::vector< double >	mapCmdToActuatorStandardVTOL (const std::vector< double > &cmd) const

void	updateActuators (std::vector< double > &cmd, double dtSecs)

Private Attributes
std::normal_distribution< double >	distribution_ {0.0, 1.0}

Environment	environment_

std::default_random_engine	generator_

VtolParameters	params_

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 138 of file vtolDynamicsSim.hpp.

Constructor & Destructor Documentation

◆ InnoVtolDynamicsSim()

InnoVtolDynamicsSim::InnoVtolDynamicsSim ( )

Definition at line 30 of file vtolDynamicsSim.cpp.

◆ ~InnoVtolDynamicsSim()

InnoVtolDynamicsSim::~InnoVtolDynamicsSim ( )

finaldefault

Member Function Documentation

◆ calculateAerodynamics()

void InnoVtolDynamicsSim::calculateAerodynamics	(	const Eigen::Vector3d &	airspeed,
		double	AoA,
		double	AoS,
		double	aileron_pos,
		double	elevator_pos,
		double	rudder_pos,
		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 439 of file vtolDynamicsSim.cpp.

◆ calculateAirSpeed()

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

private

Definition at line 390 of file vtolDynamicsSim.cpp.

◆ calculateAnglesOfAtack()

double InnoVtolDynamicsSim::calculateAnglesOfAtack ( const Eigen::Vector3d & airSpeed ) 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 412 of file vtolDynamicsSim.cpp.

◆ calculateAnglesOfSideslip()

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

Definition at line 423 of file vtolDynamicsSim.cpp.

◆ calculateAngularAccel()

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

Definition at line 633 of file vtolDynamicsSim.cpp.

◆ calculateCDPolynomial()

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

Definition at line 600 of file vtolDynamicsSim.cpp.

◆ calculateCLPolynomial()

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

Definition at line 592 of file vtolDynamicsSim.cpp.

◆ calculateCmxAileron()

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

Definition at line 622 of file vtolDynamicsSim.cpp.

◆ calculateCmxPolynomial()

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

Definition at line 604 of file vtolDynamicsSim.cpp.

◆ calculateCmyElevator()

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

Definition at line 625 of file vtolDynamicsSim.cpp.

◆ calculateCmyPolynomial()

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

Definition at line 608 of file vtolDynamicsSim.cpp.

◆ calculateCmzPolynomial()

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

Definition at line 612 of file vtolDynamicsSim.cpp.

◆ calculateCmzRudder()

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

Definition at line 628 of file vtolDynamicsSim.cpp.

◆ calculateCSBeta()

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

Definition at line 619 of file vtolDynamicsSim.cpp.

◆ calculateCSPolynomial()

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

Definition at line 596 of file vtolDynamicsSim.cpp.

◆ calculateCSRudder()

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

Definition at line 616 of file vtolDynamicsSim.cpp.

◆ calculateDynamicPressure()

double InnoVtolDynamicsSim::calculateDynamicPressure ( double airSpeedMod ) const

Definition at line 403 of file vtolDynamicsSim.cpp.

◆ calculateNewState()

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

Definition at line 531 of file vtolDynamicsSim.cpp.

◆ calculateNormalForceWithoutMass()

Eigen::Vector3d InnoVtolDynamicsSim::calculateNormalForceWithoutMass ( ) const

Note: The methods below are should be public for test only think about making test as friend

Definition at line 587 of file vtolDynamicsSim.cpp.

◆ calculateRotationMatrix()

Eigen::Matrix3d InnoVtolDynamicsSim::calculateRotationMatrix ( ) const

Definition at line 386 of file vtolDynamicsSim.cpp.

◆ calculateWind()

Eigen::Vector3d InnoVtolDynamicsSim::calculateWind ( )

Note: Implement own gust logic innopolis_vtol_indi logic doesn't suit us

Definition at line 370 of file vtolDynamicsSim.cpp.

◆ calibrate()

int8_t InnoVtolDynamicsSim::calibrate ( SimMode_t calibrationType )

overridevirtual

Reimplemented from UavDynamicsSimBase.

Definition at line 138 of file vtolDynamicsSim.cpp.

◆ getAngularAcceleration()

Eigen::Vector3d InnoVtolDynamicsSim::getAngularAcceleration ( ) const

Definition at line 686 of file vtolDynamicsSim.cpp.

◆ getBodyLinearVelocity()

Eigen::Vector3d InnoVtolDynamicsSim::getBodyLinearVelocity ( ) const

Definition at line 699 of file vtolDynamicsSim.cpp.

◆ getForces()

const Forces & InnoVtolDynamicsSim::getForces ( ) const

Note: For RVIZ visualization only

Definition at line 692 of file vtolDynamicsSim.cpp.

◆ getIMUMeasurement()

void InnoVtolDynamicsSim::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 662 of file vtolDynamicsSim.cpp.

◆ getLinearAcceleration()

Eigen::Vector3d InnoVtolDynamicsSim::getLinearAcceleration ( ) const

Definition at line 689 of file vtolDynamicsSim.cpp.

◆ getMoments()

const Moments & InnoVtolDynamicsSim::getMoments ( ) const

Definition at line 695 of file vtolDynamicsSim.cpp.

◆ getMotorsRpm()

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

overridevirtual

Reimplemented from UavDynamicsSimBase.

Definition at line 703 of file vtolDynamicsSim.cpp.

◆ getVehicleAngularVelocity()

Eigen::Vector3d InnoVtolDynamicsSim::getVehicleAngularVelocity ( void ) const

overridevirtual

Implements UavDynamicsSimBase.

Definition at line 655 of file vtolDynamicsSim.cpp.

◆ getVehicleAttitude()

Eigen::Quaterniond InnoVtolDynamicsSim::getVehicleAttitude ( void ) const

overridevirtual

Note: These methods should return in FRD format

Implements UavDynamicsSimBase.

Definition at line 652 of file vtolDynamicsSim.cpp.

◆ getVehiclePosition()

Eigen::Vector3d InnoVtolDynamicsSim::getVehiclePosition ( void ) const

overridevirtual

Note: These methods should return in NED format

Implements UavDynamicsSimBase.

Definition at line 642 of file vtolDynamicsSim.cpp.

◆ getVehicleVelocity()

Eigen::Vector3d InnoVtolDynamicsSim::getVehicleVelocity ( void ) const

overridevirtual

Implements UavDynamicsSimBase.

Definition at line 645 of file vtolDynamicsSim.cpp.

◆ init()

int8_t InnoVtolDynamicsSim::init ( )

overridevirtual

Use rosparam here to initialize sim.

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

Implements UavDynamicsSimBase.

Definition at line 44 of file vtolDynamicsSim.cpp.

◆ land()

void InnoVtolDynamicsSim::land ( )

overridevirtual

Reimplemented from UavDynamicsSimBase.

Definition at line 127 of file vtolDynamicsSim.cpp.

◆ loadParams()

void InnoVtolDynamicsSim::loadParams ( const std::string & path )

private

Definition at line 84 of file vtolDynamicsSim.cpp.

◆ loadTables()

void InnoVtolDynamicsSim::loadTables ( const std::string & path )

private

Definition at line 62 of file vtolDynamicsSim.cpp.

◆ mapCmdToActuatorInnoVTOL()

std::vector< double > InnoVtolDynamicsSim::mapCmdToActuatorInnoVTOL ( const std::vector< double > & cmd ) const

private

Note: Map motors indexes from InnoVTOL mixer into internal representation

Parameters

cmd Input indexes should correspond InnoVTOL PX4 mixer Few notes: 4 - aileron default value is 0.5 and it can be [0, +1], where 0 wants to rotate to the right 5 - elevator default value is 0 and it can be [-1, +1], where -1 wants ... 6 - rudder default value is 0 and it can be [-1, +1], where -1 wants ... 7 - throttle default value is 0 and it can be [0, +1]

Returns: Output indexes will be: 0 - 3 are the same copter indexes 4 - throttle 5 - aileron 6 - elevator 7 - rudder

Definition at line 329 of file vtolDynamicsSim.cpp.

◆ mapCmdToActuatorStandardVTOL()

std::vector< double > InnoVtolDynamicsSim::mapCmdToActuatorStandardVTOL ( const std::vector< double > & cmd ) const

private

Note: Map motors indexes from StandardVTOL mixer into internal representation Output indexes will be: 0-3 - copter indexes, where 0 - right forward, 1 - left backward, 2 - left forward, 3 - right backward 4 - throttle 5 - aileron 6 - elevator 7 - rudder (always equal to zero, because there is no control for it)

Definition at line 284 of file vtolDynamicsSim.cpp.

◆ process()

void InnoVtolDynamicsSim::process	(	double	dt_secs,
		const std::vector< double > &	motorSpeedCommandIn,
		bool	isCmdPercent
	)

overridevirtual

Implements UavDynamicsSimBase.

Definition at line 259 of file vtolDynamicsSim.cpp.

◆ setInitialPosition()

void InnoVtolDynamicsSim::setInitialPosition	(	const Eigen::Vector3d &	position,
		const Eigen::Quaterniond &	attitude
	)

overridevirtual

Implements UavDynamicsSimBase.

Definition at line 114 of file vtolDynamicsSim.cpp.

◆ setInitialVelocity()

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

Definition at line 121 of file vtolDynamicsSim.cpp.

◆ setWindParameter()

void InnoVtolDynamicsSim::setWindParameter	(	Eigen::Vector3d	windMeanVelocity,
		double	windVariance
	)

Note: These methods should be private

Definition at line 681 of file vtolDynamicsSim.cpp.

◆ thruster()

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

Definition at line 512 of file vtolDynamicsSim.cpp.

◆ updateActuators()

void InnoVtolDynamicsSim::updateActuators	(	std::vector< double > &	cmd,
		double	dtSecs
	)

private

Definition at line 361 of file vtolDynamicsSim.cpp.

Member Data Documentation

◆ distribution_

std::normal_distribution<double> InnoVtolDynamicsSim::distribution_ {0.0, 1.0}

private

Definition at line 235 of file vtolDynamicsSim.hpp.

◆ environment_

Environment InnoVtolDynamicsSim::environment_

private

Definition at line 232 of file vtolDynamicsSim.hpp.

◆ generator_

std::default_random_engine InnoVtolDynamicsSim::generator_

private

Definition at line 234 of file vtolDynamicsSim.hpp.

◆ params_

VtolParameters InnoVtolDynamicsSim::params_

private

Definition at line 229 of file vtolDynamicsSim.hpp.

◆ state_

State InnoVtolDynamicsSim::state_

private

Definition at line 230 of file vtolDynamicsSim.hpp.

◆ tables_

TablesWithCoeffs InnoVtolDynamicsSim::tables_

private

Definition at line 231 of file vtolDynamicsSim.hpp.

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

Public Member Functions

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ InnoVtolDynamicsSim()

◆ ~InnoVtolDynamicsSim()

Member Function Documentation

◆ calculateAerodynamics()

◆ calculateAirSpeed()

◆ calculateAnglesOfAtack()

◆ calculateAnglesOfSideslip()

◆ calculateAngularAccel()

◆ calculateCDPolynomial()

◆ calculateCLPolynomial()

◆ calculateCmxAileron()

◆ calculateCmxPolynomial()

◆ calculateCmyElevator()

◆ calculateCmyPolynomial()

◆ calculateCmzPolynomial()

◆ calculateCmzRudder()

◆ calculateCSBeta()

◆ calculateCSPolynomial()

◆ calculateCSRudder()

◆ calculateDynamicPressure()

◆ calculateNewState()

◆ calculateNormalForceWithoutMass()

◆ calculateRotationMatrix()

◆ calculateWind()

◆ calibrate()

◆ getAngularAcceleration()

◆ getBodyLinearVelocity()

◆ getForces()

◆ getIMUMeasurement()

◆ getLinearAcceleration()

◆ getMoments()

◆ getMotorsRpm()

◆ getVehicleAngularVelocity()

◆ getVehicleAttitude()

◆ getVehiclePosition()

◆ getVehicleVelocity()

◆ init()

◆ land()

◆ loadParams()

◆ loadTables()

◆ mapCmdToActuatorInnoVTOL()

◆ mapCmdToActuatorStandardVTOL()

◆ process()

◆ setInitialPosition()

◆ setInitialVelocity()

◆ setWindParameter()

◆ thruster()

◆ updateActuators()

Member Data Documentation

◆ distribution_

◆ environment_

◆ generator_

◆ params_

◆ state_

◆ tables_