diff --git a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp index 590c1346..97bb8bc6 100644 --- a/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp +++ b/buoy_gazebo/src/ElectroHydraulicPTO/ElectroHydraulicPTO.cpp @@ -376,6 +376,9 @@ void ElectroHydraulicPTO::PreUpdate( const double N = this->dataPtr->x[0U]; double deltaP = this->dataPtr->x[1U]; double VBus = this->dataPtr->x[2U]; + const double eff_m = this->hyd_eff_m.eval(fabs(N)); + const double eff_v = this->hyd_eff_v.eval(fabs(deltaP)); + VBus = std::min(VBus, this->dataPtr->MaxTargetVoltage); double BusPower = this->dataPtr->functor.BusPower; @@ -436,6 +439,8 @@ void ElectroHydraulicPTO::PreUpdate( latent_data.electro_hydraulic.motor_drive_switching_loss = pto_loss.motor_drive_switching_loss; latent_data.electro_hydraulic.motor_drive_friction_loss = pto_loss.motor_drive_friction_loss; latent_data.electro_hydraulic.battery_i2r_loss = pto_loss.battery_i2r_loss; + latent_data.electro_hydraulic.eff_m = eff_m; + latent_data.electro_hydraulic.eff_v = eff_v; _ecm.SetComponentData( this->dataPtr->PrismaticJointEntity, diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp index f8fd3cec..0a579158 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentData.hpp @@ -106,10 +106,12 @@ struct ElectroHydraulic double inst_power{0.0}; // Watts double rpm{0.0}; double force{0.0}; // Newtons - double motor_drive_i2r_loss{0.0}; - double motor_drive_friction_loss{0.0}; - double motor_drive_switching_loss{0.0}; - double battery_i2r_loss{0.0}; + double motor_drive_i2r_loss{0.0}; // Watts + double motor_drive_friction_loss{0.0}; // Watts + double motor_drive_switching_loss{0.0}; // Watts + double battery_i2r_loss{0.0}; // Watts + double eff_m{0.0}; // mechanical efficiency + double eff_v{0.0}; // volumetric efficiency bool operator==(const ElectroHydraulic & that) const { diff --git a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp index 49705430..64984200 100644 --- a/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp +++ b/buoy_gazebo/src/LatentData/LatentData/LatentDataPublisher.cpp @@ -279,6 +279,12 @@ void LatentDataPublisher::PostUpdate( latent_data.electro_hydraulic.motor_drive_friction_loss; this->dataPtr->latent_data_.electro_hydraulic.battery_i2r_loss = latent_data.electro_hydraulic.battery_i2r_loss; + this->dataPtr->latent_data_.electro_hydraulic.eff_m = + latent_data.electro_hydraulic.eff_m; + this->dataPtr->latent_data_.electro_hydraulic.eff_v = + latent_data.electro_hydraulic.eff_v; + + this->dataPtr->latent_data_.wave_body.buoyancy.force.x = latent_data.wave_body.buoyant_force.X(); diff --git a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp index 5f392ffe..0f995e86 100644 --- a/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp +++ b/buoy_gazebo/src/PolytropicPneumaticSpring/PolytropicPneumaticSpring.cpp @@ -40,6 +40,8 @@ #include "SpringState.hpp" #include +#include + using namespace std::chrono_literals; @@ -528,8 +530,8 @@ void PolytropicPneumaticSpring::PreUpdate( const int dt_nano = static_cast(std::chrono::duration_cast(_info.dt).count()); - static const double PASCAL_TO_PSI = 1.450377e-4; // PSI/Pascal - const double pressure_diff = (spring_state.lower_psi - spring_state.upper_psi) / PASCAL_TO_PSI; + const double pressure_diff = + (spring_state.lower_psi - spring_state.upper_psi) * buoy_utils::PASCAL_PER_PSI; if (spring_state.valve_command) { openValve( @@ -597,6 +599,7 @@ void PolytropicPneumaticSpring::PreUpdate( latent_data.upper_spring.dQ_dt = this->dataPtr->Q_rate; latent_data.upper_spring.piston_position = x; latent_data.upper_spring.piston_velocity = v; + latent_data.upper_spring.mass = this->dataPtr->mass; } else { latent_data.lower_spring.valid = true; latent_data.lower_spring.force = this->dataPtr->F; @@ -604,6 +607,7 @@ void PolytropicPneumaticSpring::PreUpdate( latent_data.lower_spring.dQ_dt = this->dataPtr->Q_rate; latent_data.lower_spring.piston_position = this->dataPtr->config_->stroke - x; latent_data.lower_spring.piston_velocity = -v; + latent_data.lower_spring.mass = this->dataPtr->mass; } _ecm.SetComponentData( diff --git a/buoy_gazebo/src/buoy_utils/Constants.hpp b/buoy_gazebo/src/buoy_utils/Constants.hpp index 68810e92..49d5e21d 100644 --- a/buoy_gazebo/src/buoy_utils/Constants.hpp +++ b/buoy_gazebo/src/buoy_utils/Constants.hpp @@ -27,6 +27,7 @@ static constexpr double NM_PER_INLB{0.112984829}; static constexpr double INLB_PER_NM{8.851}; static constexpr double INCHES_PER_METER{39.4}; static constexpr double NEWTONS_PER_LB{4.4482}; +static constexpr double PASCAL_PER_PSI = 6894.757; // Pascal/PSI } // namespace buoy_utils