From 211f2292f2634a2226adc001e7ba5ff81d70b10f Mon Sep 17 00:00:00 2001 From: Tatsuro Sakaguchi Date: Tue, 10 Dec 2024 10:27:36 +0900 Subject: [PATCH 1/4] Add parameter for adjust current sign for ROS Signed-off-by: Tatsuro Sakaguchi --- .../battery_plugin/LinearBatteryPlugin.cc | 44 +++++++++++++++---- 1 file changed, 36 insertions(+), 8 deletions(-) diff --git a/src/systems/battery_plugin/LinearBatteryPlugin.cc b/src/systems/battery_plugin/LinearBatteryPlugin.cc index 0f84e1423a..92fb626253 100644 --- a/src/systems/battery_plugin/LinearBatteryPlugin.cc +++ b/src/systems/battery_plugin/LinearBatteryPlugin.cc @@ -178,6 +178,9 @@ class gz::sim::systems::LinearBatteryPluginPrivate /// \brief Initial power load set trough config public: double initialPowerLoad = 0.0; + + /// \brief Adjusts the sign of the current to align with ROS conventions. + public: bool adjustCurrentSignForROS{false}; }; ///////////////////////////////////////////////// @@ -273,6 +276,10 @@ void LinearBatteryPlugin::Configure(const Entity &_entity, if (_sdf->HasElement("fix_issue_225")) this->dataPtr->fixIssue225 = _sdf->Get("fix_issue_225"); + if (_sdf->HasElement("adjust_current_sign_for_ros")) + this->dataPtr->adjustCurrentSignForROS = + _sdf->Get("adjust_current_sign_for_ros"); + if (_sdf->HasElement("battery_name") && _sdf->HasElement("voltage")) { this->dataPtr->batteryName = _sdf->Get("battery_name"); @@ -669,14 +676,22 @@ double LinearBatteryPlugin::OnUpdateVoltage( totalpower += powerLoad.second; } - this->dataPtr->iraw = totalpower / _battery->Voltage(); + if (this->dataPtr->adjustCurrentSignForROS) + this->dataPtr->iraw = -totalpower / _battery->Voltage(); + else + this->dataPtr->iraw = totalpower / _battery->Voltage(); // compute charging current auto iCharge = this->dataPtr->c / this->dataPtr->tCharge; // add charging current to battery if (this->dataPtr->startCharging && this->dataPtr->StateOfCharge() < 0.9) - this->dataPtr->iraw -= iCharge; + { + if (this->dataPtr->adjustCurrentSignForROS) + this->dataPtr->iraw += iCharge; + else + this->dataPtr->iraw -= iCharge; + } this->dataPtr->ismooth = this->dataPtr->ismooth + k * (this->dataPtr->iraw - this->dataPtr->ismooth); @@ -693,13 +708,23 @@ double LinearBatteryPlugin::OnUpdateVoltage( } // Convert dt to hours - this->dataPtr->q = this->dataPtr->q - ((dt * this->dataPtr->ismooth) / - 3600.0); + if (this->dataPtr->adjustCurrentSignForROS) + this->dataPtr->q = this->dataPtr->q + ((dt * this->dataPtr->ismooth) / + 3600.0); + else + this->dataPtr->q = this->dataPtr->q - ((dt * this->dataPtr->ismooth) / + 3600.0); // open circuit voltage - double voltage = this->dataPtr->e0 + this->dataPtr->e1 * ( - 1 - this->dataPtr->q / this->dataPtr->c) - - this->dataPtr->r * this->dataPtr->ismooth; + double voltage; + if (this->dataPtr->adjustCurrentSignForROS) + voltage = this->dataPtr->e0 + this->dataPtr->e1 * ( + 1 - this->dataPtr->q / this->dataPtr->c) + + this->dataPtr->r * this->dataPtr->ismooth; + else + voltage = this->dataPtr->e0 + this->dataPtr->e1 * ( + 1 - this->dataPtr->q / this->c) + - this->dataPtr->r * this->dataPtr->ismooth; // Estimate state of charge if (this->dataPtr->fixIssue225) @@ -709,7 +734,10 @@ double LinearBatteryPlugin::OnUpdateVoltage( double isum = 0.0; for (size_t i = 0; i < this->dataPtr->iList.size(); ++i) isum += (this->dataPtr->iList[i] * this->dataPtr->dtList[i] / 3600.0); - this->dataPtr->soc = this->dataPtr->soc - isum / this->dataPtr->c; + if (this->dataPtr->adjustCurrentSignForROS) + this->dataPtr->soc = this->dataPtr->soc + isum / this->dataPtr->c; + else + this->dataPtr->soc = this->dataPtr->soc - isum / this->dataPtr->c; } // Throttle debug messages From fc1a2d580ec873eab280e7d21c35d725cd582871 Mon Sep 17 00:00:00 2001 From: Tatsuro Sakaguchi Date: Tue, 10 Dec 2024 12:04:06 +0900 Subject: [PATCH 2/4] Fix missing member Signed-off-by: Tatsuro Sakaguchi --- src/systems/battery_plugin/LinearBatteryPlugin.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/battery_plugin/LinearBatteryPlugin.cc b/src/systems/battery_plugin/LinearBatteryPlugin.cc index 92fb626253..f11fecc063 100644 --- a/src/systems/battery_plugin/LinearBatteryPlugin.cc +++ b/src/systems/battery_plugin/LinearBatteryPlugin.cc @@ -723,7 +723,7 @@ double LinearBatteryPlugin::OnUpdateVoltage( + this->dataPtr->r * this->dataPtr->ismooth; else voltage = this->dataPtr->e0 + this->dataPtr->e1 * ( - 1 - this->dataPtr->q / this->c) + 1 - this->dataPtr->q / this->dataPtr->c) - this->dataPtr->r * this->dataPtr->ismooth; // Estimate state of charge From 5d6aaeb2bbb54046048aedd1e91b37ec2b679c23 Mon Sep 17 00:00:00 2001 From: Tatsuro Sakaguchi Date: Wed, 11 Dec 2024 09:28:52 +0900 Subject: [PATCH 3/4] Fix param name Signed-off-by: Tatsuro Sakaguchi --- .../battery_plugin/LinearBatteryPlugin.cc | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/systems/battery_plugin/LinearBatteryPlugin.cc b/src/systems/battery_plugin/LinearBatteryPlugin.cc index f11fecc063..bc4f1a3656 100644 --- a/src/systems/battery_plugin/LinearBatteryPlugin.cc +++ b/src/systems/battery_plugin/LinearBatteryPlugin.cc @@ -179,8 +179,8 @@ class gz::sim::systems::LinearBatteryPluginPrivate /// \brief Initial power load set trough config public: double initialPowerLoad = 0.0; - /// \brief Adjusts the sign of the current to align with ROS conventions. - public: bool adjustCurrentSignForROS{false}; + /// \brief Flag to invert the current sign + public: bool invertCurrentSign{false}; }; ///////////////////////////////////////////////// @@ -276,9 +276,9 @@ void LinearBatteryPlugin::Configure(const Entity &_entity, if (_sdf->HasElement("fix_issue_225")) this->dataPtr->fixIssue225 = _sdf->Get("fix_issue_225"); - if (_sdf->HasElement("adjust_current_sign_for_ros")) - this->dataPtr->adjustCurrentSignForROS = - _sdf->Get("adjust_current_sign_for_ros"); + if (_sdf->HasElement("invert_current_sign")) + this->dataPtr->invertCurrentSign = + _sdf->Get("invert_current_sign"); if (_sdf->HasElement("battery_name") && _sdf->HasElement("voltage")) { @@ -676,7 +676,7 @@ double LinearBatteryPlugin::OnUpdateVoltage( totalpower += powerLoad.second; } - if (this->dataPtr->adjustCurrentSignForROS) + if (this->dataPtr->invertCurrentSign) this->dataPtr->iraw = -totalpower / _battery->Voltage(); else this->dataPtr->iraw = totalpower / _battery->Voltage(); @@ -687,7 +687,7 @@ double LinearBatteryPlugin::OnUpdateVoltage( // add charging current to battery if (this->dataPtr->startCharging && this->dataPtr->StateOfCharge() < 0.9) { - if (this->dataPtr->adjustCurrentSignForROS) + if (this->dataPtr->invertCurrentSign) this->dataPtr->iraw += iCharge; else this->dataPtr->iraw -= iCharge; @@ -708,7 +708,7 @@ double LinearBatteryPlugin::OnUpdateVoltage( } // Convert dt to hours - if (this->dataPtr->adjustCurrentSignForROS) + if (this->dataPtr->invertCurrentSign) this->dataPtr->q = this->dataPtr->q + ((dt * this->dataPtr->ismooth) / 3600.0); else @@ -717,7 +717,7 @@ double LinearBatteryPlugin::OnUpdateVoltage( // open circuit voltage double voltage; - if (this->dataPtr->adjustCurrentSignForROS) + if (this->dataPtr->invertCurrentSign) voltage = this->dataPtr->e0 + this->dataPtr->e1 * ( 1 - this->dataPtr->q / this->dataPtr->c) + this->dataPtr->r * this->dataPtr->ismooth; @@ -734,7 +734,7 @@ double LinearBatteryPlugin::OnUpdateVoltage( double isum = 0.0; for (size_t i = 0; i < this->dataPtr->iList.size(); ++i) isum += (this->dataPtr->iList[i] * this->dataPtr->dtList[i] / 3600.0); - if (this->dataPtr->adjustCurrentSignForROS) + if (this->dataPtr->invertCurrentSign) this->dataPtr->soc = this->dataPtr->soc + isum / this->dataPtr->c; else this->dataPtr->soc = this->dataPtr->soc - isum / this->dataPtr->c; From 288b9c50edbbb4573be8fc758fbc0cdbac5c17aa Mon Sep 17 00:00:00 2001 From: Tatsuro Sakaguchi Date: Tue, 17 Dec 2024 09:15:10 +0900 Subject: [PATCH 4/4] Invert sign when the data is being published Signed-off-by: Tatsuro Sakaguchi --- .../battery_plugin/LinearBatteryPlugin.cc | 42 ++++++------------- 1 file changed, 12 insertions(+), 30 deletions(-) diff --git a/src/systems/battery_plugin/LinearBatteryPlugin.cc b/src/systems/battery_plugin/LinearBatteryPlugin.cc index bc4f1a3656..d4adcbd9f7 100644 --- a/src/systems/battery_plugin/LinearBatteryPlugin.cc +++ b/src/systems/battery_plugin/LinearBatteryPlugin.cc @@ -631,7 +631,10 @@ void LinearBatteryPlugin::PostUpdate(const UpdateInfo &_info, msg.mutable_header()->mutable_stamp()->CopyFrom( convert(_info.simTime)); msg.set_voltage(this->dataPtr->battery->Voltage()); - msg.set_current(this->dataPtr->ismooth); + if (this->dataPtr->invertCurrentSign) + msg.set_current(-this->dataPtr->ismooth); + else + msg.set_current(this->dataPtr->ismooth); msg.set_charge(this->dataPtr->q); msg.set_capacity(this->dataPtr->c); @@ -676,22 +679,14 @@ double LinearBatteryPlugin::OnUpdateVoltage( totalpower += powerLoad.second; } - if (this->dataPtr->invertCurrentSign) - this->dataPtr->iraw = -totalpower / _battery->Voltage(); - else - this->dataPtr->iraw = totalpower / _battery->Voltage(); + this->dataPtr->iraw = totalpower / _battery->Voltage(); // compute charging current auto iCharge = this->dataPtr->c / this->dataPtr->tCharge; // add charging current to battery if (this->dataPtr->startCharging && this->dataPtr->StateOfCharge() < 0.9) - { - if (this->dataPtr->invertCurrentSign) - this->dataPtr->iraw += iCharge; - else - this->dataPtr->iraw -= iCharge; - } + this->dataPtr->iraw -= iCharge; this->dataPtr->ismooth = this->dataPtr->ismooth + k * (this->dataPtr->iraw - this->dataPtr->ismooth); @@ -708,23 +703,13 @@ double LinearBatteryPlugin::OnUpdateVoltage( } // Convert dt to hours - if (this->dataPtr->invertCurrentSign) - this->dataPtr->q = this->dataPtr->q + ((dt * this->dataPtr->ismooth) / - 3600.0); - else - this->dataPtr->q = this->dataPtr->q - ((dt * this->dataPtr->ismooth) / - 3600.0); + this->dataPtr->q = this->dataPtr->q - ((dt * this->dataPtr->ismooth) / + 3600.0); // open circuit voltage - double voltage; - if (this->dataPtr->invertCurrentSign) - voltage = this->dataPtr->e0 + this->dataPtr->e1 * ( - 1 - this->dataPtr->q / this->dataPtr->c) - + this->dataPtr->r * this->dataPtr->ismooth; - else - voltage = this->dataPtr->e0 + this->dataPtr->e1 * ( - 1 - this->dataPtr->q / this->dataPtr->c) - - this->dataPtr->r * this->dataPtr->ismooth; + double voltage = this->dataPtr->e0 + this->dataPtr->e1 * ( + 1 - this->dataPtr->q / this->dataPtr->c) + - this->dataPtr->r * this->dataPtr->ismooth; // Estimate state of charge if (this->dataPtr->fixIssue225) @@ -734,10 +719,7 @@ double LinearBatteryPlugin::OnUpdateVoltage( double isum = 0.0; for (size_t i = 0; i < this->dataPtr->iList.size(); ++i) isum += (this->dataPtr->iList[i] * this->dataPtr->dtList[i] / 3600.0); - if (this->dataPtr->invertCurrentSign) - this->dataPtr->soc = this->dataPtr->soc + isum / this->dataPtr->c; - else - this->dataPtr->soc = this->dataPtr->soc - isum / this->dataPtr->c; + this->dataPtr->soc = this->dataPtr->soc - isum / this->dataPtr->c; } // Throttle debug messages