12 #include <yarp/os/Log.h>
13 #include <yarp/os/LogStream.h>
21 #include "EoAnalogSensors.h"
22 #include "EoProtocol.h"
23 #include "EoProtocolAS.h"
24 #include "EoProtocolMN.h"
27 #pragma warning(once : 4355)
31 using namespace yarp::os;
36 temperature_ =
data->temperature / 10;
37 voltage_ = std::trunc(10 *
data->voltage) / 10;
38 current_ =
data->current;
39 charge_ =
data->charge;
40 status_ =
data->status;
41 timeStamp_ = timestamp;
44 embObjBattery::embObjBattery()
46 yInfo() <<
"CanBatterySensors has been created";
47 device_ = std::make_shared<yarp::dev::embObjDevPrivData>(
"embObjBattery");
50 embObjBattery::embObjBattery(std::shared_ptr<yarp::dev::embObjDevPrivData> device) : device_(device)
66 yInfo() <<
"embObjBattery::open(): preparing ETH resource";
67 if (!
device_->prerareEthService(config,
this))
70 yInfo() <<
device_->getBoardInfo() <<
" embObjBattery::open(): browsing xml files which describe the service";
72 if (!parser.
parse(config))
74 yError() <<
device_->getBoardInfo() <<
"open() fails to parse xml... cannot continue ";
78 yInfo() <<
device_->getBoardInfo() <<
" embObjBattery::open(): verify the presence of the board and if its protocol version is correct";
79 if (!
device_->res->verifyEPprotocol(eoprot_endpoint_analogsensors))
81 yError() <<
device_->getBoardInfo() <<
" open() fails to verifyEPprotocol... cannot continue ";
86 yInfo() <<
device_->getBoardInfo() <<
" embObjBattery::open(): verify and activate the FT service";
87 eOmn_serv_parameter_t canBatteryData;
88 canBatteryData.configuration.type = eomn_serv_AS_battery;
89 canBatteryData.configuration.diagnosticsmode = eomn_serv_diagn_mode_NONE;
90 canBatteryData.configuration.diagnosticsparam = 0;
91 parser.
toEomn(canBatteryData.configuration.data.as.battery);
92 if (!
device_->res->serviceVerifyActivate(eomn_serv_category_battery, &canBatteryData, 5.0))
94 yError() <<
device_->getBoardInfo() <<
" open() fails to serviceVerifyActivate... cannot continue ";
99 yInfo() <<
device_->getBoardInfo() <<
" embObjBattery::open(): configure the FT service";
102 yError() <<
device_->getBoardInfo() <<
" open() fails to sendConfig2boards... cannot continue";
107 yInfo() <<
device_->getBoardInfo() <<
" embObjBattery::open(): impose the network variable which the ETH bord must stream up";
110 yError() <<
device_->getBoardInfo() <<
" open() fails to initRegulars... cannot continue";
115 yInfo() <<
device_->getBoardInfo() <<
" embObjBattery::open(): start the FT service";
116 if (!
device_->res->serviceStart(eomn_serv_category_battery))
118 yError() <<
device_->getBoardInfo() <<
" open() fails to serviceStart... cannot continue";
126 yDebug() <<
device_->getBoardInfo() <<
" open() correctly starts service";
130 yInfo() <<
device_->getBoardInfo() <<
" embObjBattery::open(): start streaming of FT data";
133 yError() <<
device_->getBoardInfo() <<
" open() fails to sendStart2boards... cannot continue";
146 eOprotID32_t id32 = eo_prot_ID32dummy;
147 eOas_battery_config_t cfg{0, 0};
149 id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_battery, 0, eoprot_tag_as_battery_config);
153 yError() <<
device_->getBoardInfo() <<
" sendConfig2boards() while try to configure fperiod=" << cfg.period;
159 yDebug() <<
device_->getBoardInfo() <<
" sendConfig2boards() correctly configured boards with period=" << cfg.period;
166 eOprotID32_t id32 = eo_prot_ID32dummy;
172 id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_battery, 0, eoprot_tag_as_battery_cmmnds_enable);
176 yError() <<
device_->getBoardInfo() <<
" sendStart2boards() while try to enable the boards transmission";
182 yDebug() <<
device_->getBoardInfo() <<
" sendStart2boards() correctly enabled the boards transmission";
191 vector<eOprotID32_t> id32v;
192 eOprotID32_t id32 = eo_prot_ID32dummy;
195 id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_battery, 0, eoprot_tag_as_battery_status_timedvalue);
196 id32v.push_back(id32);
200 yError() <<
device_->getBoardInfo() <<
" initRegulars() fails to add its variables to regulars: cannot proceed any further";
206 yDebug() <<
device_->getBoardInfo() <<
" initRegulars() added" << id32v.size() <<
"regular rops ";
208 for (
size_t r = 0; r < id32v.size(); r++)
210 uint32_t item = id32v.at(r);
211 eoprot_ID2information(item, nvinfo,
sizeof(nvinfo));
212 yDebug() <<
device_->getBoardInfo() <<
"\t it added regular rop for" << nvinfo;
228 eOprotIndex_t eoprotIndex = eoprot_ID2index(id32);
231 yError() <<
device_->getBoardInfo() <<
" update() index too big";
234 eOprotEntity_t entity = eoprot_ID2entity(id32);
235 if (entity != eoprot_entity_as_battery)
237 yError() <<
device_->getBoardInfo() <<
" update() wrong entity";
241 eOprotTag_t tag = eoprot_ID2tag(id32);
242 if (tag != eoprot_tag_as_battery_status_timedvalue)
244 yError() <<
device_->getBoardInfo() <<
" update() wrong tag";
248 eOas_battery_timedvalue_t *
data = (eOas_battery_timedvalue_t *)rxdata;
254 std::unique_lock<std::shared_mutex> lck(
mutex_);
281 yError() <<
device_->getBoardInfo() <<
" update timeout for index:" << eoprot_ID2index(id32);
283 masStatus_[eoprot_ID2index(id32)] = MAS_TIMEOUT;
295 return yarp::os::Time::now();
340 std::stringstream ss;
344 battery_info = ss.str();
352 if ((
int)(voltage_*10) != (
int)(other.
voltage_*10))
bool operator==(const BatteryInfo &right, const BatteryInfo &left)
bool operator==(const CanBatteryData &other) const
void decode(eOas_battery_timedvalue_t *data, double timestamp)
bool operator!=(const CanBatteryData &other) const
bool parse(const yarp::os::Searchable &config)
BatteryInfo & getBatteryInfo()
bool toEomn(eOmn_serv_config_data_as_battery_t &out) const
virtual bool setcheckRemoteValue(const eOprotID32_t id32, void *value, const unsigned int retries=10, const double waitbeforecheck=0.001, const double timeout=0.050)=0
virtual bool serviceSetRegulars(eOmn_serv_category_t category, vector< eOprotID32_t > &id32vector, double timeout=0.500)=0
eth::iethresType_t type() override
bool open(yarp::os::Searchable &config)
eOabstime_t firstCanTimestamp_
bool initialised() override
std::map< eOprotID32_t, eOabstime_t > timeoutUpdate_
double firstYarpTimestamp_
bool sendConfig2boards(ServiceParserCanBattery &parser, eth::AbstractEthResource *deviceRes)
bool getBatteryVoltage(double &voltage) override
static constexpr bool useBoardTimeFlag_
static constexpr eOabstime_t updateTimeout_
virtual double calculateBoardTime(eOabstime_t current)
bool getBatteryStatus(Battery_status &status) override
bool getBatteryCurrent(double ¤t) override
bool checkUpdateTimeout(eOprotID32_t id32, eOabstime_t current)
std::shared_ptr< yarp::dev::embObjDevPrivData > device_
bool getBatteryInfo(std::string &battery_info) override
bool sendStart2boards(ServiceParserCanBattery &parser, eth::AbstractEthResource *deviceRes)
std::vector< yarp::dev::MAS_status > masStatus_
bool initRegulars(ServiceParserCanBattery &parser, eth::AbstractEthResource *deviceRes)
bool update(eOprotID32_t id32, double timestamp, void *rxdata) override
static constexpr bool checkUpdateTimeoutFlag_
CanBatteryData canBatteryData_
bool getBatteryTemperature(double &temperature) override
bool getBatteryCharge(double &charge) override
Copyright (C) 2008 RobotCub Consortium.