Skip to content

Commit

Permalink
Add to diagnostic parser overheating error and Update embObjBattery
Browse files Browse the repository at this point in the history 
device

Re-elaborate status bat-bms using uint16_t as datatype
and using string_view instead of string for status messages
Update bat reading values adding total abosorbed current in BAT
and adjusting limits for BAT SoC calculation
Fix datatype in diagnostic to be aligned by raw value datatype
Set value of battery charge to NaN if equal to zero
Update MINIMUM_REQUIRED_icub_firmware_shared_VERSION to 1.38.2
  • Loading branch information
@MSECode
MSECode committed Mar 21, 2024
1 parent 5586584 commit ac82bba
Show file tree
Hide file tree
Showing 4 changed files with 63 additions and 66 deletions.
2 changes: 1 addition & 1 deletion conf/iCubFindDependencies.cmake
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@ checkandset_dependency(IPOPT)
checkandset_dependency(OpenCV)
checkandset_dependency(Qt5)

set(MINIMUM_REQUIRED_icub_firmware_shared_VERSION 1.38.1)
set(MINIMUM_REQUIRED_icub_firmware_shared_VERSION 1.38.2)

if(icub_firmware_shared_FOUND AND ICUB_USE_icub_firmware_shared)
if(icub_firmware_shared_VERSION VERSION_LESS ${MINIMUM_REQUIRED_icub_firmware_shared_VERSION})
Expand Down
101 changes: 43 additions & 58 deletions src/libraries/icubmod/embObjBattery/embObjBattery.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,8 +13,7 @@
#include <yarp/os/LogStream.h>

#include <iostream>
#include <string>
#include <sstream>
#include <string_view>
#include <cmath>
#include <array>

Expand All @@ -38,7 +37,7 @@ void CanBatteryData::decode(eOas_battery_timedvalue_t *data, double timestamp)
{
temperature_ = data->temperature / 10; // in steps of 0.1 celsius degree (pos and neg).
voltage_ = std::trunc(10 * data->voltage) / 10;
current_ = data->current;
current_ = std::trunc(10 * data->current) / 10;
charge_ = data->charge;
status_ = data->status;
timeStamp_ = timestamp;
Expand Down Expand Up @@ -138,6 +137,8 @@ bool embObjBattery::open(yarp::os::Searchable &config)
return false;
}

canBatteryData_.sensorName_ = eoboards_type2string(parser.getBatteryInfo().board);
canBatteryData_.sensorType_ = parser.getBatteryInfo().board;
device_->setOpen(true);
return true;
}
Expand Down Expand Up @@ -261,19 +262,15 @@ bool embObjBattery::update(eOprotID32_t id32, double timestamp, void *rxdata)
}
else if (!isCanDataAvailable && (data->age != 0))
{
if (updateStatusStringStream(data->status, canBatteryData_.prevStatus_, true))
{
yDebug() << "First Status are:\n" << statusStreamBMS.str() << statusStreamBAT.str() << "\n";
}
yDebug() << "First Status are:\n" << updateStatusStringStream(data->status, canBatteryData_.prevStatus_, true);

canBatteryData_.prevStatus_ = data->status;
isCanDataAvailable = true;
}
else if (data->status != canBatteryData_.prevStatus_)
{
if (updateStatusStringStream(data->status, canBatteryData_.prevStatus_, false))
{
yDebug() << "Status changed to:\n" << statusStreamBMS.str() << statusStreamBAT.str() << "\n";
}
yDebug() << "Status changed to:\n" << updateStatusStringStream(data->status, canBatteryData_.prevStatus_, false);

canBatteryData_.prevStatus_ = data->status;
}

Expand Down Expand Up @@ -314,10 +311,10 @@ bool embObjBattery::checkUpdateTimeout(eOprotID32_t id32, eOabstime_t current)
return true;
}

bool embObjBattery::updateStatusStringStream(const uint32_t &currStatus, const uint32_t &prevStatus, bool isFirstLoop)
std::string embObjBattery::updateStatusStringStream(const uint16_t &currStatus, const uint16_t &prevStatus, bool isFirstLoop)
{
// Initialize the first time the static map
static const std::array<std::pair<eOas_battery_alarm_status_t, std::string>, eoas_battery_alarm_status_numberof> s_boards_map_of_battery_alarm_status =
static const std::array<std::pair<eOas_battery_alarm_status_t, std::string_view>, eoas_battery_alarm_status_numberof> s_boards_map_of_battery_alarm_status =
{
{{eoas_bms_general_alarm_lowvoltage, "eoas_bms_general_alarm_lowvoltage"},
{eoas_bms_general_alarm_highvoltage, "eoas_bms_general_alarm_highvoltage"},
Expand Down Expand Up @@ -348,67 +345,52 @@ bool embObjBattery::updateStatusStringStream(const uint32_t &currStatus, const u
{eoas_bat_status_btn_1_stable_op, "eoas_bat_status_btn_1_stable_op"}}
};

// Clear buffer for BAT and BMS
statusStreamBMS.str("");
statusStreamBAT.str("");

bool isBmsSignatureBit = embot::core::binary::bit::check(currStatus, 0);
// Clear and reserve space for buffer for BAT and BMS
std::string statusstring = {};
statusstring.reserve(512);

if(isBmsSignatureBit)
if (canBatteryData_.sensorType_ == eobrd_cantype_bms)
{
// And add header string
statusStreamBMS << "STATUS_BMS:" << "\n";
if (!(embot::core::binary::mask::check(currStatus, static_cast<uint32_t>(0x0000ffff))))
for (uint8_t i = 0; i < eoas_bms_alarm_numberof; i++)
{
statusStreamBMS << "\tNo Faults. All Alarms Bit Down\n";
}
else
{
for (uint8_t i = 1; i < eoas_bms_alarm_numberof; i++)
if((embot::core::binary::bit::check(currStatus, i)))
{
std::string tmpString = (embot::core::binary::bit::check(currStatus, i)) ? (s_boards_map_of_battery_alarm_status.at(i)).second : "";

if (tmpString != "")
{
statusStreamBMS << "\t" << tmpString << "\n";
}
statusstring.append("\t");
statusstring.append(s_boards_map_of_battery_alarm_status.at(i).second);
statusstring.append("\n");
}
}
}
else
else if(canBatteryData_.sensorType_ == eobrd_cantype_bat)
{
uint8_t map_pos = 0;
statusStreamBAT << "STATUS_BAT:" << "\n";

for (const auto& [k,v] : s_boards_map_of_battery_alarm_status)
uint8_t bit_pos = 0;
for (uint8_t i = eoas_bms_alarm_numberof; i < eoas_battery_alarm_status_numberof; i = i+2)
{
std::string tmpString = "";
uint8_t bit_pos = (uint8_t)k;

if (bit_pos > eoas_bms_alarm_numberof && bit_pos < eoas_battery_alarm_status_numberof)
if ((embot::core::binary::bit::check(currStatus, bit_pos) != embot::core::binary::bit::check(prevStatus, bit_pos)) || isFirstLoop)
{
if (isFirstLoop)
statusstring.append("\t");
if((embot::core::binary::bit::check(currStatus, bit_pos)))
{
tmpString = embot::core::binary::bit::check(currStatus, bit_pos) ? v : (s_boards_map_of_battery_alarm_status.at(map_pos+1)).second;
}
statusstring.append(s_boards_map_of_battery_alarm_status.at(i).second);
}
else
{
if (embot::core::binary::bit::check(currStatus, bit_pos) != embot::core::binary::bit::check(prevStatus, bit_pos))
{
tmpString = embot::core::binary::bit::check(currStatus, bit_pos) ? v : (s_boards_map_of_battery_alarm_status.at(map_pos+1)).second;
}
}
if (tmpString != "")
{
statusStreamBAT << "\t" << tmpString << "\n";
statusstring.append(s_boards_map_of_battery_alarm_status.at(i+1).second);
}
statusstring.append("\n");
}
++map_pos;

++bit_pos;
}
}

if(statusstring.empty())
{
statusstring.append("\tNo Faults Detected. All Alarms Bit Down\n");
}

return true;

return statusstring;
}

double embObjBattery::calculateBoardTime(eOabstime_t current)
Expand Down Expand Up @@ -442,7 +424,7 @@ bool embObjBattery::getBatteryCurrent(double &current)

bool embObjBattery::getBatteryCharge(double &charge)
{
charge = canBatteryData_.charge_;
charge = (canBatteryData_.charge_ != 0.0) ? canBatteryData_.charge_ : NAN;
return true;
}

Expand All @@ -461,7 +443,7 @@ bool embObjBattery::getBatteryTemperature(double &temperature)
bool embObjBattery::getBatteryInfo(std::string &battery_info)
{
std::stringstream ss;
ss << "{\"temperature\":" << canBatteryData_.temperature_ << ",\"voltage\":" << canBatteryData_.voltage_ << ",\"charge\":" << canBatteryData_.charge_ << ",\"status\":" << canBatteryData_.status_
ss << "{\"temperature\":" << canBatteryData_.temperature_ << ",\"voltage\":" << canBatteryData_.voltage_ << ",\"current\":" << canBatteryData_.current_ << ",\"charge\":" << canBatteryData_.charge_ << ",\"status\":" << canBatteryData_.status_
<< ",\"ts\":" << canBatteryData_.timeStamp_ << "}" << std::endl;

battery_info = ss.str();
Expand All @@ -474,7 +456,7 @@ bool CanBatteryData::operator==(const CanBatteryData &other) const
return false;
if ((int)(voltage_ * 10) != (int)(other.voltage_ * 10)) // Only one digit after dot
return false;
if (current_ != other.current_)
if ((int)(current_ * 10) != (int)(other.current_ * 10))
return false;
if (charge_ != other.charge_)
return false;
Expand All @@ -484,6 +466,9 @@ bool CanBatteryData::operator==(const CanBatteryData &other) const
return false;
if (sensorName_ != other.sensorName_)
return false;
if (sensorType_ != other.sensorType_)
return false;

return true;
}

Expand Down
13 changes: 6 additions & 7 deletions src/libraries/icubmod/embObjBattery/embObjBattery.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,10 +35,12 @@ class CanBatteryData
float32_t voltage_{0};
float32_t current_{0};
float32_t charge_{0};
uint32_t status_{0};
uint32_t prevStatus_{0};
uint16_t status_{0};
uint16_t prevStatus_{0};
double timeStamp_{0};
std::string sensorName_;
std::string sensorName_{};
eObrd_type_t sensorType_{eobrd_unknown};


void decode(eOas_battery_timedvalue_t *data, double timestamp);
bool operator==(const CanBatteryData &other) const;
Expand Down Expand Up @@ -81,7 +83,7 @@ class yarp::dev::embObjBattery : public yarp::dev::DeviceDriver, public eth::Iet
bool initRegulars(ServiceParserCanBattery &parser, eth::AbstractEthResource *deviceRes);
void cleanup(void);
bool checkUpdateTimeout(eOprotID32_t id32, eOabstime_t current);
bool updateStatusStringStream(const uint32_t &currStatus, const uint32_t &prevStatus, bool isFirstLoop);
std::string updateStatusStringStream(const uint16_t &currStatus, const uint16_t &prevStatus, bool isFirstLoop);
static constexpr eOabstime_t updateTimeout_{11000};
std::vector<yarp::dev::MAS_status> masStatus_{MAS_OK, MAS_OK, MAS_OK, MAS_OK};

Expand All @@ -93,9 +95,6 @@ class yarp::dev::embObjBattery : public yarp::dev::DeviceDriver, public eth::Iet

double firstYarpTimestamp_{0};
eOabstime_t firstCanTimestamp_{0};

std::stringstream statusStreamBMS = {};
std::stringstream statusStreamBAT = {};
};

#endif
13 changes: 13 additions & 0 deletions src/libraries/icubmod/embObjLib/diagnosticInfoParsers.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -685,6 +685,19 @@ void MotionControlParser::parseInfo()
m_dnginfo.baseInfo.finalMessage.append(str);
} break;

case eoerror_value_MC_motor_overheating:
{
uint16_t joint_num = m_dnginfo.param16;
int16_t temp_raw_value = m_dnginfo.param64 & 0xffff;

m_entityNameProvider.getAxisName(joint_num, m_dnginfo.baseInfo.axisName);

snprintf(str, sizeof(str), " %s (Joint=%s (NIB=%d), Raw_temperature_value=%d)",
m_dnginfo.baseMessage.c_str(), m_dnginfo.baseInfo.axisName.c_str(), joint_num, temp_raw_value
);
m_dnginfo.baseInfo.finalMessage.append(str);
} break;

case EOERROR_VALUE_DUMMY:
{
m_dnginfo.baseInfo.finalMessage.append(": unrecognised eoerror_category_MotionControl error value.");
Expand Down

0 comments on commit ac82bba

Please sign in to comment.