Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Implemented power supply status estimation #173

Open
wants to merge 1 commit into
base: noetic-devel
Choose a base branch
from
Open

Implemented power supply status estimation #173

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions include/ubiquity_motor/motor_hardware.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -143,6 +143,7 @@ class MotorHardware : public hardware_interface::RobotHW {
void writeSpeedsInRadians(double left_radians, double right_radians);
void publishFirmwareInfo();
float calculateBatteryPercentage(float voltage, int cells, const float* type);
uint8_t getPowerSupplyStatus(float battery_voltage);
int areWheelSpeedsLower(double wheelSpeedRadPerSec);
void requestFirmwareVersion();
void requestFirmwareDate();
Expand Down
98 changes: 95 additions & 3 deletions src/motor_hardware.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <ubiquity_motor/motor_message.h>
#include <boost/assign.hpp>
#include <boost/math/special_functions/round.hpp>
#include <boost/circular_buffer.hpp>

// To access I2C we need some system includes
#include <linux/i2c-dev.h>
Expand Down Expand Up @@ -68,6 +69,8 @@ int32_t g_odomLeft = 0;
int32_t g_odomRight = 0;
int32_t g_odomEvent = 0;

// 2x6S(12 V) Batteries connected in series
const uint8_t SLA_AGM_CELLS = 6 * 2;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I suppose you could use this but any of our batteries alone at 12V is 'low'. I do see later you use 2.175 sort of number that would be 13V. A nominal 12V gel cell is near 12.6V actually.
I leave this up to you to think about and you can leave this if you like except the 2.175 factor should be a factor to come out to more like 12.6 but again batteries brand new fresh totally charted can read 13v so up to you.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If you are referring to SLA_AGM_CELLS, this number only indicates the number of cells in the used battery bank - and is not used directly anywhere in the code, but rather only to determine voltages levels with factors as seen below.

Perhaps my choice in variable names wasn't great, but these three constants below are used only to determine the charging state and I found that these factors work best for that purpose alone.

//lead acid battery percentage levels for a single cell
const static float SLA_AGM[11] = {
1.800, // 0
Expand All @@ -83,6 +86,9 @@ const static float SLA_AGM[11] = {
2.175, // 100
};

const float SLA_AGM_V_NOM = 2.175f * SLA_AGM_CELLS; // NOMINAL battery bank voltage
const float SLA_AGM_V_CHG = 2.45f * SLA_AGM_CELLS; // CHARGING battery bank voltage
const float SLA_AGM_V_TRI = 2.333f * SLA_AGM_CELLS; // Trickle charging battery bank voltage

//li-ion battery percentage levels for a single cell
const static float LI_ION[11] = {
Expand Down Expand Up @@ -491,9 +497,9 @@ void MotorHardware::readInputs(uint32_t index) {
bstate.capacity = std::numeric_limits<float>::quiet_NaN();
bstate.design_capacity = std::numeric_limits<float>::quiet_NaN();

// Hardcoded to a sealed lead acid 12S battery, but adjustable for future use
bstate.percentage = calculateBatteryPercentage(bstate.voltage, 12, SLA_AGM);
bstate.power_supply_status = sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN;
// Hardcoded to a sealed lead acid 2x6S(12V) batteries, but adjustable for future use
bstate.percentage = calculateBatteryPercentage(bstate.voltage, SLA_AGM_CELLS, SLA_AGM);
bstate.power_supply_status = getPowerSupplyStatus(bstate.voltage);
bstate.power_supply_health = sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN;
bstate.power_supply_technology = sensor_msgs::BatteryState::POWER_SUPPLY_TECHNOLOGY_UNKNOWN;
battery_state.publish(bstate);
Expand Down Expand Up @@ -607,6 +613,92 @@ float MotorHardware::calculateBatteryPercentage(float voltage, int cells, const
return (float)lower * 0.1 + between_percent * 0.1;
}

uint8_t MotorHardware::getPowerSupplyStatus(float battery_voltage) {
// For more details check: https://github.com/UbiquityRobotics/charge_status_research

// Number of samples to average was chosen to minimize high-frequency noise
// while retaing as much responsiveness as possible
const uint16_t AVG_SAMPLE_NUM = 40U;
// Number of samples to buffer in order to track long term changes
const uint16_t SAMPLE_BUF_SIZE = 10000U;
// Exponentialy Weighted Moving Average (EWMA) params
const float ALPHA = 2.0f / (AVG_SAMPLE_NUM + 1);
const float BETA = 1.0f - ALPHA;

const float V_TOL = 0.2f; // [V] Voltage threshold tolerance
// When BELOW the absolute value of this voltage change rate
// the voltage is considered steady
const float STEADY_VOLT_RATE = 0.006f; // [V/s]
// When ABOVE the absolute value of this voltage change rate
// the voltage is considered as changing
const float UNSTEADY_VOLT_RATE = 0.04f; // [V/s]
const ros::Duration TRICKLE_DUR_THRESH(60U * 10U); // 10 minutes

static bool init = false;
static boost::circular_buffer<float> voltage_buf(SAMPLE_BUF_SIZE);
static uint8_t status = sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN;
static float voltage_avg; // Averaged measured voltage with EWMA
static ros::Time last_loop_time;
static uint16_t sample_num = 0;
static ros::Duration trickle_dur(0);
// Initialize power status estimation
if (!init) {
voltage_avg = battery_voltage;
last_loop_time = ros::Time::now();
init = true;
}
// Calculate delta-time
ros::Time t = ros::Time::now();
ros::Duration dt_r = t - last_loop_time;
float dt = dt_r.toSec();
last_loop_time = t;
// Average measured voltage with EWMA to filter out high-frequency noise
voltage_avg = ALPHA * battery_voltage + BETA * voltage_avg;
voltage_buf.push_back(voltage_avg);
// Wait until there are enough samples to start estimation
// and voltage average has settled
if (sample_num < AVG_SAMPLE_NUM * 2) {
sample_num++;
return status;
}
// Past-looking Voltage average derivative
// This is done under the assumption that the samples contain predominantly
// high-frequency noise of a small and fairly constant amplitude
float d = (voltage_avg - voltage_buf.front()) / (AVG_SAMPLE_NUM * 2 * dt);

bool is_charging = status == sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_CHARGING;
bool is_tri_charging = status == sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_FULL;
bool is_v_rising = d > UNSTEADY_VOLT_RATE;
bool is_v_dropping = d < -UNSTEADY_VOLT_RATE;
bool is_v_idle = std::abs(d) < STEADY_VOLT_RATE;
bool is_in_nom_range = voltage_avg < SLA_AGM_V_NOM;
bool is_in_chg_range = voltage_avg >= SLA_AGM_V_CHG;
bool is_in_trickle_range = voltage_avg > (SLA_AGM_V_TRI - V_TOL) && voltage_avg < (SLA_AGM_V_TRI + V_TOL);
bool was_unplugged = (is_charging || is_tri_charging) && is_v_dropping;

if (is_v_rising || is_in_chg_range) {
// Charging
status = sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_CHARGING;
trickle_dur = ros::Duration(0);
} else if (was_unplugged || (is_v_idle && is_in_nom_range && !is_charging)) {
// Not charging
status = sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_NOT_CHARGING;
trickle_dur = ros::Duration(0);
} else if (is_v_idle && is_in_trickle_range) {
// Trickle charging
if (trickle_dur > TRICKLE_DUR_THRESH) {
status = sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_FULL;
} else {
trickle_dur += dt_r;
}
} else if (is_v_dropping && is_in_nom_range && !is_charging) {
// Discharging
status = sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING;
trickle_dur = ros::Duration(0);
}
return status;
}

// writeSpeedsInRadians() Take in radians per sec for wheels and send in message to controller
//
// A direct write speeds that allows caller setting speeds in radians
Expand Down