DiagnosticsSubUnit.cpp

Go to the documentation of this file.

#include "DiagnosticsSubUnit.h"
 
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBattery.h>
 
namespace armarx
{
 
    dto::BatteryStatus
    DiagnosticsSubUnit::getBatteryStatus(const Ice::Current& current) const
    {
        // This diagnostics sub unit should rather report the status report of each battery module,
        // and additionally an accumulated one for easy use. Detailed reports of each battery module
        // are important in order to identify faulty modules more quickly. At this point, due to
        // firmware limitations and only being allowed to return one sensor value per device in the
        // realtime part, the diagnostics unit now only reports the accumulated values and nothing
        // else. In future, a dto::BatteryStatus struct must be reported for each battery module, in
        // addition to one dto::BatteryStatus that accumulates all data. For this, the firmware of
        // the power board needs to be adjusted to report this data, the EtherCAT SDOs need to be
        // updated, and the device in devices::ethercat needs to be updated to report more than one
        // sensor value. Additionally, the BatteryManagement Ice interface needs to be adjusted to
        // report multiple BatteryStatus structs. Similar to this:
        //
        // struct BatteryManagementReport
        // {
        //     map<string, dto::BatteryStatus> detailedReports;
        //     dto::BatteryStatus cumulativeReport;
        // };
        //
        if (_lastBatteryStatuses.size() > 0 and
            _lastBatteryStatuses.find("PowerBoard") != _lastBatteryStatuses.end())
        {
            return _lastBatteryStatuses.at("PowerBoard");
        }
 
        return {.name = "PowerBoard",
                .state = dto::BatteryState::unavailable,
                .designEnergy_Wh = -1,
                .fullChargeEnergy_Wh = -1,
                .energy_Wh = -1,
                .energyFromFullCharge_pct = 0,
                .fullEnergyFromDesignEnergy_pct = 0,
                .temperature_degC = -1,
                .ratedVoltage_V = -1,
                .voltage_V = -1,
                .current_A = -1,
                .power_W = -1,
                .remainingTime_h = -1,
                .hasError = false,
                .error = ""};
    }
 
    std::string
    DiagnosticsSubUnit::getDefaultName() const
    {
        return "DiagnosticsSubUnit";
    }
 
    void
    DiagnosticsSubUnit::update(const SensorAndControl& sc, const JointAndNJointControllers& c)
    {
        for (auto& [deviceName, deviceIndex] : _batteryManagementDevices)
        {
            const SensorValueBase* sv = sc.sensors.at(deviceIndex).get();
            ARMARX_CHECK_EXPRESSION(sv->isA<armarx::SensorValueBattery>());
            const armarx::SensorValueBattery* s = sv->asA<armarx::SensorValueBattery>();
 
            auto& stat = _lastBatteryStatuses[deviceName];
            stat.name = deviceName;
            stat.state = static_cast<dto::BatteryState>(s->state);
            stat.designEnergy_Wh = s->designEnergy_Wh;
            stat.fullChargeEnergy_Wh = s->fullChargeEnergy_Wh;
            stat.energy_Wh = s->energy_Wh;
            stat.energyFromFullCharge_pct = s->energyFromFullCharge_pct;
            stat.fullEnergyFromDesignEnergy_pct = s->fullEnergyFromDesignEnergy_pct;
            stat.temperature_degC = s->temperature_degC;
            stat.ratedVoltage_V = s->ratedVoltage_V;
            stat.voltage_V = s->voltage_V;
            stat.current_A = s->current_A;
            stat.power_W = s->power_W;
            stat.remainingTime_h = s->remainingTime_h;
            stat.hasError = s->hasError;
            stat.error =
                ""; // TODO: How to transfer strings from RT? Maybe get rid of this or error codes.
        }
    }
 
    void
    DiagnosticsSubUnit::setBatteryManagementDevices(
        std::map<std::string, std::size_t> batteryManagementDevices)
    {
        _batteryManagementDevices = batteryManagementDevices;
 
        for (auto& [deviceName, deviceIndex] : _batteryManagementDevices)
        {
            _lastBatteryStatuses[deviceName] = {};
        }
    }
 
    void
    DiagnosticsSubUnit::onInitComponent()
    {
        ;
    }
 
    void
    DiagnosticsSubUnit::onConnectComponent()
    {
        ;
    }
 
} // namespace armarx