Status passed to embObjBattery.cpp

conf/iCubFindDependencies.cmake

@@ -65,8 +65,8 @@ checkandset_dependency(OpenCV)
 checkandset_dependency(Qt5)
 
 if(icub_firmware_shared_FOUND AND ICUB_USE_icub_firmware_shared)
-  if(icub_firmware_shared_VERSION VERSION_LESS 1.35.1)
-    message(FATAL_ERROR "An old version of icub-firmware-shared has been detected: at least 1.35.1 is required")
+  if(icub_firmware_shared_VERSION VERSION_LESS 1.35.2)
+    message(FATAL_ERROR "An old version of icub-firmware-shared has been detected: at least 1.35.2 is required")
   endif()
 endif()
 

src/libraries/icubmod/embObjBattery/CMakeLists.txt

@@ -19,7 +19,8 @@ if (NOT SKIP_embObjBattery)
 
   yarp_add_plugin(embObjBattery embObjBattery.cpp embObjBattery.h)
   target_link_libraries(embObjBattery ethResources
-                        YARP::YARP_os YARP::YARP_dev YARP::YARP_sig)
+                        YARP::YARP_os YARP::YARP_dev YARP::YARP_sig
+                        icub_firmware_shared::embot)
   icub_export_plugin(embObjBattery)
 
   yarp_install(TARGETS embObjBattery
@@ -32,10 +33,11 @@ if (NOT SKIP_embObjBattery)
    add_library(embObjBatteryUT STATIC embObjBattery.cpp embObjBattery.h)
     target_link_libraries(embObjBatteryUT ethResources
                           YARP::YARP_os YARP::YARP_dev YARP::YARP_sig
-                          icub_firmware_shared::embobj)
-
+                          icub_firmware_shared::embobj
+                          icub_firmware_shared::embot)
+
    target_include_directories(embObjBatteryUT PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>")
   endif()
-  
+
 endif()
 

src/libraries/icubmod/embObjBattery/embObjBattery.cpp

@@ -16,12 +16,15 @@
 #include <string>
 #include <sstream>
 #include <cmath>
+#include <array>
 
 #include "EOnv_hid.h"
-#include "EoAnalogSensors.h"
 #include "EoProtocol.h"
 #include "EoProtocolAS.h"
 #include "EoProtocolMN.h"
+#include "EoAnalogSensors.h"
+
+#include "embot_core_binary.h"
 
 #ifdef WIN32
 #pragma warning(once : 4355)
@@ -34,7 +37,7 @@ using namespace yarp::dev;
 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; 
+    voltage_ = std::trunc(10 * data->voltage) / 10;
     current_ = data->current;
     charge_ = data->charge;
     status_ = data->status;
@@ -251,6 +254,29 @@ bool embObjBattery::update(eOprotID32_t id32, double timestamp, void *rxdata)
         return false;
     }
 
+    if (!isPastFirstPrint && (data->age == 0))
+    {
+        yDebug("CAN DATA NOT YET AVAILABLE");
+        isPastFirstPrint = true;
+    }
+    else if (!isCanDataAvailable && (data->age != 0))
+    {
+        if (updateStatusStringStream(data->status, canBatteryData_.prevStatus_, true))
+        {
+            yDebug() << "First Status are:\n" << statusStreamBMS.str() << statusStreamBAT.str() << "\n";
+        }
+        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";
+        }
+        canBatteryData_.prevStatus_ = data->status;
+    }
+
     std::unique_lock<std::shared_mutex> lck(mutex_);
     canBatteryData_.decode(data, calculateBoardTime(data->age));
 
@@ -288,6 +314,103 @@ bool embObjBattery::checkUpdateTimeout(eOprotID32_t id32, eOabstime_t current)
     return true;
 }
 
+bool embObjBattery::updateStatusStringStream(const uint32_t &currStatus, const uint32_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 =
+    {
+        {{eoas_bms_general_alarm_lowvoltage, "eoas_bms_general_alarm_lowvoltage"},
+        {eoas_bms_general_alarm_highvoltage, "eoas_bms_general_alarm_highvoltage"},
+        {eoas_bms_general_alarm_overcurrent_discharge, "eoas_bms_general_alarm_overcurrent_discharge"},
+        {eoas_bms_general_alarm_overcurrent_charge, "eoas_bms_general_alarm_overcurrent_charge"},
+        {eoas_bms_general_alarm_lowSOC, "eoas_bms_general_alarm_lowSOC"},
+        {eoas_bms_general_alarm_lowtemperature, "eoas_bms_general_alarm_lowtemperature"},
+        {eoas_bms_general_alarm_hightemperature, "eoas_bms_general_alarm_hightemperature"},
+        {eoas_bat_status_hsm_mosfet_broken, "eoas_bat_status_hsm_mosfet_broken"},
+        {eoas_bat_status_hsm_mosfet_normal, "eoas_bat_status_hsm_mosfet_normal"},
+        {eoas_bat_status_hsm_overcurrent_overvoltage, "eoas_bat_status_hsm_overcurrent_overvoltage"},
+        {eoas_bat_status_hsm_normal, "eoas_bat_status_hsm_normal"},
+        {eoas_bat_status_hsm_voltage_power_good, "eoas_bat_status_hsm_voltage_power_good"},
+        {eoas_bat_status_hsm_voltage_not_guaranteed, "eoas_bat_status_hsm_voltage_not_guaranteed"},
+        {eoas_bat_status_hsm_status_on, "eoas_bat_status_hsm_status_on"},
+        {eoas_bat_status_hsm_status_off, "eoas_bat_status_hsm_status_off"},
+        {eoas_bat_status_motor_regulator_overcurrent, "eoas_bat_status_motor_regulator_overcurrent"},
+        {eoas_bat_status_motor_regulator_normal, "eoas_bat_status_motor_regulator_normal"},
+        {eoas_bat_status_motor_on, "eoas_bat_status_motor_on"},
+        {eoas_bat_status_motor_off, "eoas_bat_status_motor_off"},
+        {eoas_bat_status_board_regulator_overcurrent, "eoas_bat_status_board_regulator_overcurrent"},
+        {eoas_bat_status_board_regulator_normal, "eoas_bat_status_board_regulator_normal"},
+        {eoas_bat_status_board_on, "eoas_bat_status_board_on"},
+        {eoas_bat_status_board_off, "eoas_bat_status_board_off"},
+        {eoas_bat_status_btn_2_start_up_phase, "eoas_bat_status_btn_2_start_up_phase"},
+        {eoas_bat_status_btn_2_stable_op, "eoas_bat_status_btn_2_stable_op"},
+        {eoas_bat_status_btn_1_start_up_phase, "eoas_bat_status_btn_1_start_up_phase"},
+        {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);
+
+    if(isBmsSignatureBit)
+    {
+        // And add header string
+        statusStreamBMS << "STATUS_BMS:" << "\n";
+        if (!(embot::core::binary::mask::check(currStatus, static_cast<uint32_t>(0x0000ffff))))
+        {
+            statusStreamBMS << "\tNo Faults. All Alarms Bit Down\n";
+        }
+        else
+        {
+            for (uint8_t i = 1; i < eoas_bms_alarm_numberof; 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";
+                }
+            }
+        }
+    }
+    else
+    {
+        uint8_t map_pos = 0;
+        statusStreamBAT << "STATUS_BAT:" << "\n";
+
+        for (const auto& [k,v] : s_boards_map_of_battery_alarm_status)
+        {
+            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 (isFirstLoop)
+                {
+                    tmpString = embot::core::binary::bit::check(currStatus, bit_pos) ? v : (s_boards_map_of_battery_alarm_status.at(map_pos+1)).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";
+                }
+            }
+            ++map_pos;
+        }
+
+    }
+
+    return true;
+}
+
 double embObjBattery::calculateBoardTime(eOabstime_t current)
 {
     if (!useBoardTimeFlag_)
@@ -313,7 +436,7 @@ bool embObjBattery::getBatteryVoltage(double &voltage)
 
 bool embObjBattery::getBatteryCurrent(double &current)
 {
-    current = canBatteryData_.current_;
+    current = (canBatteryData_.current_ != 0) ? canBatteryData_.current_ : NAN;
     return true;
 }
 
@@ -331,7 +454,7 @@ bool embObjBattery::getBatteryStatus(Battery_status &status)
 
 bool embObjBattery::getBatteryTemperature(double &temperature)
 {
-    temperature = canBatteryData_.temperature_;
+    temperature = (canBatteryData_.temperature_ != 0) ? canBatteryData_.temperature_ : NAN;
     return true;
 }
 
@@ -349,7 +472,7 @@ bool CanBatteryData::operator==(const CanBatteryData &other) const
 {
     if (temperature_ != other.temperature_)
         return false;
-    if ((int)(voltage_*10) != (int)(other.voltage_*10)) //Only one digit after dot
+    if ((int)(voltage_ * 10) != (int)(other.voltage_ * 10))  // Only one digit after dot
         return false;
     if (current_ != other.current_)
         return false;

src/libraries/icubmod/embObjBattery/embObjBattery.h

@@ -18,6 +18,7 @@
 #include <shared_mutex>
 #include <memory>
 #include <string>
+#include <utility>
 
 #include "embObjGeneralDevPrivData.h"
 #include "serviceParserCanBattery.h"
@@ -34,7 +35,8 @@ class CanBatteryData
     float32_t voltage_{0};
     float32_t current_{0};
     float32_t charge_{0};
-    int8_t status_{0};
+    uint32_t status_{0};
+    uint32_t prevStatus_{0};
     double timeStamp_{0};
     std::string sensorName_;
 
@@ -79,14 +81,21 @@ 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);
     static constexpr eOabstime_t updateTimeout_{11000};
     std::vector<yarp::dev::MAS_status> masStatus_{MAS_OK, MAS_OK, MAS_OK, MAS_OK};
 
     static constexpr bool checkUpdateTimeoutFlag_{false};  // Check timer disabled
     static constexpr bool useBoardTimeFlag_{true};         // Calculate board time if true otherway use yarp time
 
+    bool isCanDataAvailable = false;
+    bool isPastFirstPrint = false;
+
     double firstYarpTimestamp_{0};
     eOabstime_t firstCanTimestamp_{0};
+
+    std::stringstream statusStreamBMS = {};
+    std::stringstream statusStreamBAT = {};
 };
 
 #endif

src/libraries/icubmod/embObjLib/ethResource.cpp

@@ -863,7 +863,7 @@ bool EthResource::CANPrintHandler(eOmn_info_basic_t *infobasic)
 bool EthResource::serviceCommand(eOmn_serv_operation_t operation, eOmn_serv_category_t category, const eOmn_serv_parameter_t* param, double timeout, int times)
 {
     eOprotID32_t id2send = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_service, 0, eoprot_tag_mn_service_cmmnds_command);
-    eOprotID32_t id2wait = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_service, 0, eoprot_tag_mn_service_status_commandresult);;
+    eOprotID32_t id2wait = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_service, 0, eoprot_tag_mn_service_status_commandresult);
 
     eOmn_service_cmmnds_command_t command = {0};
     eOmn_service_command_result_t result = {0};

src/unittest/testDeviceCanBatterySensor.cpp

@@ -171,8 +171,8 @@ TEST(CanBatterysensor, update_simple_positive_001)
     std::shared_ptr<embObjDevPrivData_Mock> privateData = std::make_shared<embObjDevPrivData_Mock>("test");
     embObjCanBatterysensor_Mock device(privateData);
     uint32_t id32First = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_battery, 0, eoprot_tag_as_ft_status_timedvalue);
-    eOas_battery_timedvalue_t data = {0 /*age*/, 10 /*temperature in dec C*/, 2, 3, 4, 5, 6};
-    CanBatteryData expected = {1, 4, 5, 6, 2, 7, ""};
+    eOas_battery_timedvalue_t data = {0 /*age*/, 10 /*temperature in dec C*/, 0, 2, 3, 4, 5, 0};
+    CanBatteryData expected = {1, 3, 4, 5, 2, 2, 7, ""};
 
     EXPECT_CALL(*privateData, isOpen()).WillRepeatedly(Return(true));
     EXPECT_CALL(device, calculateBoardTime(_)).WillRepeatedly(Return(7));
@@ -190,8 +190,8 @@ TEST(CanBatterysensor, update_simple_positive_trunk_voltage_001)
     std::shared_ptr<embObjDevPrivData_Mock> privateData = std::make_shared<embObjDevPrivData_Mock>("test");
     embObjCanBatterysensor_Mock device(privateData);
     uint32_t id32First = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_battery, 0, eoprot_tag_as_ft_status_timedvalue);
-    eOas_battery_timedvalue_t data = {0 /*age*/, 10 /*temperature in dec C*/, 2, 3, 4.123, 5, 6};
-    CanBatteryData expected = {1, 4.1, 5, 6, 2, 7, ""};
+    eOas_battery_timedvalue_t data = {0 /*age*/, 10 /*temperature in dec C*/, 0, 2, 3, 4.123, 5, 0};
+    CanBatteryData expected = {1, 3, 4.123, 5, 2, 2, 7, ""};
 
     EXPECT_CALL(*privateData, isOpen()).WillRepeatedly(Return(true));
     EXPECT_CALL(device, calculateBoardTime(_)).WillRepeatedly(Return(7));
@@ -210,7 +210,7 @@ TEST(CanBatterysensor, update_simple_negative_001)
     embObjCanBatterysensor_Mock device(privateData);
     uint32_t id32First = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_battery, 0, eoprot_tag_as_ft_status_timedvalue);
     eOas_battery_timedvalue_t data = {0 /*age*/, 10 /*temperature in dec C*/, 2, 3, 4, 9};
-    CanBatteryData expected = {1, 2, 3, 4, 5, 7, ""};
+    CanBatteryData expected = {1, 2, 3, 4, 5, 0, 7, ""};
 
     EXPECT_CALL(*privateData, isOpen()).WillRepeatedly(Return(true));
     EXPECT_CALL(device, calculateBoardTime(_)).WillRepeatedly(Return(7));
@@ -227,7 +227,7 @@ TEST(CanBatterysensor, getBatteryVoltage_positive_001)
     std::shared_ptr<embObjDevPrivData_Mock> privateData = std::make_shared<embObjDevPrivData_Mock>("test");
     embObjCanBatterysensor_Mock device(privateData);
 
-    device.canBatteryData_ = {1, 2, 3, 4, 5, 6, ""};
+    device.canBatteryData_ = {1, 2, 3, 4, 5, 0, 6, ""};
 
     EXPECT_CALL(*privateData, isOpen()).WillRepeatedly(Return(true));