diff --git a/src/systems/battery_plugin/LinearBatteryPlugin.cc b/src/systems/battery_plugin/LinearBatteryPlugin.cc index 0f84e1423a..bc4f1a3656 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 Flag to invert the current sign + public: bool invertCurrentSign{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("invert_current_sign")) + this->dataPtr->invertCurrentSign = + _sdf->Get("invert_current_sign"); + 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->invertCurrentSign) + 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->invertCurrentSign) + 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->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); // 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->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; // 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->invertCurrentSign) + this->dataPtr->soc = this->dataPtr->soc + isum / this->dataPtr->c; + else + this->dataPtr->soc = this->dataPtr->soc - isum / this->dataPtr->c; } // Throttle debug messages