iCub-main
embObjBattery.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT)
3  * All rights reserved.
4  * Author: Luca Tricerri
5  * This software may be modified and distributed under the terms of the
6  * BSD-3-Clause license. See the accompanying LICENSE file for details.
7  */
8 
9 #include <embObjBattery.h>
10 #include <ethManager.h>
11 
12 #include <yarp/os/Log.h>
13 #include <yarp/os/LogStream.h>
14 
15 #include <iostream>
16 #include <string>
17 #include <sstream>
18 #include <cmath>
19 
20 #include "EOnv_hid.h"
21 #include "EoAnalogSensors.h"
22 #include "EoProtocol.h"
23 #include "EoProtocolAS.h"
24 #include "EoProtocolMN.h"
25 
26 #ifdef WIN32
27 #pragma warning(once : 4355)
28 #endif
29 
30 using namespace yarp;
31 using namespace yarp::os;
32 using namespace yarp::dev;
33 
34 void CanBatteryData::decode(eOas_battery_timedvalue_t *data, double timestamp)
35 {
36  temperature_ = data->temperature / 10; // in steps of 0.1 celsius degree (pos and neg).
37  voltage_ = std::trunc(10 * data->voltage) / 10;
38  current_ = data->current;
39  charge_ = data->charge;
40  status_ = data->status;
41  timeStamp_ = timestamp;
42 }
43 
44 embObjBattery::embObjBattery()
45 {
46  yInfo() << "CanBatterySensors has been created";
47  device_ = std::make_shared<yarp::dev::embObjDevPrivData>("embObjBattery");
48 }
49 
50 embObjBattery::embObjBattery(std::shared_ptr<yarp::dev::embObjDevPrivData> device) : device_(device)
51 {
52 }
53 
55 {
56  close();
57 }
58 
60 {
61  return device_->isOpen();
62 }
63 
64 bool embObjBattery::open(yarp::os::Searchable &config)
65 {
66  yInfo() << "embObjBattery::open(): preparing ETH resource";
67  if (!device_->prerareEthService(config, this))
68  return false;
69 
70  yInfo() << device_->getBoardInfo() << " embObjBattery::open(): browsing xml files which describe the service";
72  if (!parser.parse(config))
73  {
74  yError() << device_->getBoardInfo() << "open() fails to parse xml... cannot continue ";
75  return false;
76  }
77 
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))
80  {
81  yError() << device_->getBoardInfo() << " open() fails to verifyEPprotocol... cannot continue ";
82  cleanup();
83  return false;
84  }
85 
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))
93  {
94  yError() << device_->getBoardInfo() << " open() fails to serviceVerifyActivate... cannot continue ";
95  cleanup();
96  return false;
97  }
98 
99  yInfo() << device_->getBoardInfo() << " embObjBattery::open(): configure the FT service";
100  if (false == sendConfig2boards(parser, device_->res))
101  {
102  yError() << device_->getBoardInfo() << " open() fails to sendConfig2boards... cannot continue";
103  cleanup();
104  return false;
105  }
106 
107  yInfo() << device_->getBoardInfo() << " embObjBattery::open(): impose the network variable which the ETH bord must stream up";
108  if (false == initRegulars(parser, device_->res))
109  {
110  yError() << device_->getBoardInfo() << " open() fails to initRegulars... cannot continue";
111  cleanup();
112  return false;
113  }
114 
115  yInfo() << device_->getBoardInfo() << " embObjBattery::open(): start the FT service";
116  if (!device_->res->serviceStart(eomn_serv_category_battery))
117  {
118  yError() << device_->getBoardInfo() << " open() fails to serviceStart... cannot continue";
119  cleanup();
120  return false;
121  }
122  else
123  {
124  if (device_->isVerbose())
125  {
126  yDebug() << device_->getBoardInfo() << " open() correctly starts service";
127  }
128  }
129 
130  yInfo() << device_->getBoardInfo() << " embObjBattery::open(): start streaming of FT data";
131  if (!sendStart2boards(parser, device_->res))
132  {
133  yError() << device_->getBoardInfo() << " open() fails to sendStart2boards... cannot continue";
134  cleanup();
135  return false;
136  }
137 
138  device_->setOpen(true);
139  return true;
140 }
141 
143 {
144  auto &canBattery = parser.getBatteryInfo();
145 
146  eOprotID32_t id32 = eo_prot_ID32dummy;
147  eOas_battery_config_t cfg{0, 0};
148  cfg.period = canBattery.acquisitionRate;
149  id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_battery, 0, eoprot_tag_as_battery_config);
150 
151  if (false == deviceRes->setcheckRemoteValue(id32, &cfg, 10, 0.010, 0.050))
152  {
153  yError() << device_->getBoardInfo() << " sendConfig2boards() while try to configure fperiod=" << cfg.period;
154  return false;
155  }
156 
157  if (device_->isVerbose())
158  {
159  yDebug() << device_->getBoardInfo() << " sendConfig2boards() correctly configured boards with period=" << cfg.period;
160  }
161  return true;
162 }
163 
165 {
166  eOprotID32_t id32 = eo_prot_ID32dummy;
167 
168  uint8_t enable = 1;
169 
170  const auto &batteryInfos = parser.getBatteryInfo();
171 
172  id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_battery, 0, eoprot_tag_as_battery_cmmnds_enable);
173 
174  if (false == deviceRes->setcheckRemoteValue(id32, &enable, 10, 0.010, 0.050))
175  {
176  yError() << device_->getBoardInfo() << " sendStart2boards() while try to enable the boards transmission";
177  return false;
178  }
179 
180  if (device_->isVerbose())
181  {
182  yDebug() << device_->getBoardInfo() << " sendStart2boards() correctly enabled the boards transmission";
183  }
184  return true;
185 }
186 
188 {
189  // configure regular rops
190 
191  vector<eOprotID32_t> id32v;
192  eOprotID32_t id32 = eo_prot_ID32dummy;
193 
194  const auto &batteryInfos = parser.getBatteryInfo();
195  id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_battery, 0, eoprot_tag_as_battery_status_timedvalue);
196  id32v.push_back(id32);
197 
198  if (false == deviceRes->serviceSetRegulars(eomn_serv_category_battery, id32v))
199  {
200  yError() << device_->getBoardInfo() << " initRegulars() fails to add its variables to regulars: cannot proceed any further";
201  return false;
202  }
203 
204  if (device_->isVerbose())
205  {
206  yDebug() << device_->getBoardInfo() << " initRegulars() added" << id32v.size() << "regular rops ";
207  char nvinfo[128];
208  for (size_t r = 0; r < id32v.size(); r++)
209  {
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;
213  }
214  }
215 
216  return true;
217 }
218 
220 {
222 }
223 
224 bool embObjBattery::update(eOprotID32_t id32, double timestamp, void *rxdata)
225 {
226  if (!device_->isOpen())
227  return false;
228  eOprotIndex_t eoprotIndex = eoprot_ID2index(id32);
229  if (eoprotIndex > 1)
230  {
231  yError() << device_->getBoardInfo() << " update() index too big";
232  return false;
233  }
234  eOprotEntity_t entity = eoprot_ID2entity(id32);
235  if (entity != eoprot_entity_as_battery)
236  {
237  yError() << device_->getBoardInfo() << " update() wrong entity";
238  return false;
239  }
240 
241  eOprotTag_t tag = eoprot_ID2tag(id32);
242  if (tag != eoprot_tag_as_battery_status_timedvalue)
243  {
244  yError() << device_->getBoardInfo() << " update() wrong tag";
245  return false;
246  }
247 
248  eOas_battery_timedvalue_t *data = (eOas_battery_timedvalue_t *)rxdata;
249  if (!checkUpdateTimeout(id32, data->age))
250  {
251  return false;
252  }
253 
254  std::unique_lock<std::shared_mutex> lck(mutex_);
256 
257  return true;
258 }
259 
261 {
262  cleanup();
263  return true;
264 }
265 
267 {
268  device_->cleanup(static_cast<eth::IethResource *>(this));
269 }
270 
271 bool embObjBattery::checkUpdateTimeout(eOprotID32_t id32, eOabstime_t current)
272 {
274  {
275  return true;
276  }
277 
278  eOabstime_t diff = current - timeoutUpdate_[id32];
279  if (timeoutUpdate_[id32] != 0 && current > timeoutUpdate_[id32] + updateTimeout_)
280  {
281  yError() << device_->getBoardInfo() << " update timeout for index:" << eoprot_ID2index(id32);
282  timeoutUpdate_[id32] = current;
283  masStatus_[eoprot_ID2index(id32)] = MAS_TIMEOUT;
284  return false;
285  }
286  masStatus_[eoprot_ID2index(id32)] = MAS_OK;
287  timeoutUpdate_[id32] = current;
288  return true;
289 }
290 
291 double embObjBattery::calculateBoardTime(eOabstime_t current)
292 {
293  if (!useBoardTimeFlag_)
294  {
295  return yarp::os::Time::now();
296  }
297 
298  // Simulate real board time
299  if (firstYarpTimestamp_ == 0)
300  {
301  firstYarpTimestamp_ = yarp::os::Time::now();
302  firstCanTimestamp_ = current;
303  }
304  double realtime = firstYarpTimestamp_ + (double)(current - firstCanTimestamp_) / 1000000; // Simulate real board time
305  return realtime;
306 }
307 
308 bool embObjBattery::getBatteryVoltage(double &voltage)
309 {
310  voltage = canBatteryData_.voltage_;
311  return true;
312 }
313 
314 bool embObjBattery::getBatteryCurrent(double &current)
315 {
316  current = canBatteryData_.current_;
317  return true;
318 }
319 
321 {
322  charge = canBatteryData_.charge_;
323  return true;
324 }
325 
326 bool embObjBattery::getBatteryStatus(Battery_status &status)
327 {
328  status = static_cast<Battery_status>(canBatteryData_.status_);
329  return true;
330 }
331 
332 bool embObjBattery::getBatteryTemperature(double &temperature)
333 {
334  temperature = canBatteryData_.temperature_;
335  return true;
336 }
337 
338 bool embObjBattery::getBatteryInfo(std::string &battery_info)
339 {
340  std::stringstream ss;
341  ss << "{\"temperature\":" << canBatteryData_.temperature_ << ",\"voltage\":" << canBatteryData_.voltage_ << ",\"charge\":" << canBatteryData_.charge_ << ",\"status\":" << canBatteryData_.status_
342  << ",\"ts\":" << canBatteryData_.timeStamp_ << "}" << std::endl;
343 
344  battery_info = ss.str();
345  return true;
346 }
347 
349 {
350  if (temperature_ != other.temperature_)
351  return false;
352  if ((int)(voltage_*10) != (int)(other.voltage_*10)) //Only one digit after dot
353  return false;
354  if (current_ != other.current_)
355  return false;
356  if (charge_ != other.charge_)
357  return false;
358  if (status_ != other.status_)
359  return false;
360  if (timeStamp_ != other.timeStamp_)
361  return false;
362  if (sensorName_ != other.sensorName_)
363  return false;
364  return true;
365 }
366 
368 {
369  return !operator==(other);
370 }
@ data
bool operator==(const BatteryInfo &right, const BatteryInfo &left)
Definition: batteryInfo.cpp:44
uint8_t acquisitionRate
Definition: batteryInfo.h:19
int16_t temperature_
Definition: embObjBattery.h:33
bool operator==(const CanBatteryData &other) const
float32_t current_
Definition: embObjBattery.h:35
void decode(eOas_battery_timedvalue_t *data, double timestamp)
std::string sensorName_
Definition: embObjBattery.h:39
float32_t charge_
Definition: embObjBattery.h:36
float32_t voltage_
Definition: embObjBattery.h:34
bool operator!=(const CanBatteryData &other) const
bool parse(const yarp::os::Searchable &config)
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_
Definition: embObjBattery.h:89
bool initialised() override
std::map< eOprotID32_t, eOabstime_t > timeoutUpdate_
Definition: embObjBattery.h:75
bool sendConfig2boards(ServiceParserCanBattery &parser, eth::AbstractEthResource *deviceRes)
bool getBatteryVoltage(double &voltage) override
static constexpr bool useBoardTimeFlag_
Definition: embObjBattery.h:86
static constexpr eOabstime_t updateTimeout_
Definition: embObjBattery.h:82
virtual double calculateBoardTime(eOabstime_t current)
std::shared_mutex mutex_
Definition: embObjBattery.h:73
bool getBatteryStatus(Battery_status &status) override
bool getBatteryCurrent(double &current) override
bool checkUpdateTimeout(eOprotID32_t id32, eOabstime_t current)
std::shared_ptr< yarp::dev::embObjDevPrivData > device_
Definition: embObjBattery.h:72
bool getBatteryInfo(std::string &battery_info) override
bool sendStart2boards(ServiceParserCanBattery &parser, eth::AbstractEthResource *deviceRes)
std::vector< yarp::dev::MAS_status > masStatus_
Definition: embObjBattery.h:83
bool initRegulars(ServiceParserCanBattery &parser, eth::AbstractEthResource *deviceRes)
bool update(eOprotID32_t id32, double timestamp, void *rxdata) override
static constexpr bool checkUpdateTimeoutFlag_
Definition: embObjBattery.h:85
CanBatteryData canBatteryData_
Definition: embObjBattery.h:74
bool getBatteryTemperature(double &temperature) override
bool getBatteryCharge(double &charge) override
iethresType_t
Definition: IethResource.h:61
@ iethres_analogbattery
Definition: IethResource.h:76
Copyright (C) 2008 RobotCub Consortium.