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 
…953)
  • Loading branch information
@MSECode
MSECode committed Mar 26, 2024
1 parent 5586584 commit 4a0e4a4
Show file tree
Hide file tree
Showing 4 changed files with 64 additions and 67 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
103 changes: 44 additions & 59 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 @@ -436,13 +418,13 @@ bool embObjBattery::getBatteryVoltage(double &voltage)

bool embObjBattery::getBatteryCurrent(double &current)
{
current = (canBatteryData_.current_ != 0) ? canBatteryData_.current_ : NAN;
current = canBatteryData_.current_;
return true;
}

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 4a0e4a4

Please sign in to comment.