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_view>
17 #include <cmath>
18 #include <array>
19 
20 #include "EOnv_hid.h"
21 #include "EoProtocol.h"
22 #include "EoProtocolAS.h"
23 #include "EoProtocolMN.h"
24 #include "EoAnalogSensors.h"
25 
26 #include "embot_core_binary.h"
27 
28 #ifdef WIN32
29 #pragma warning(once : 4355)
30 #endif
31 
32 using namespace yarp;
33 using namespace yarp::os;
34 using namespace yarp::dev;
35 
36 void CanBatteryData::decode(eOas_battery_timedvalue_t *data, double timestamp)
37 {
38  temperature_ = data->temperature / 10; // in steps of 0.1 celsius degree (pos and neg).
39  voltage_ = std::trunc(10 * data->voltage) / 10;
40  current_ = std::trunc(10 * data->current) / 10;
41  charge_ = data->charge;
42  status_ = data->status;
43  timeStamp_ = timestamp;
44 }
45 
46 embObjBattery::embObjBattery()
47 {
48  yInfo() << "CanBatterySensors has been created";
49  device_ = std::make_shared<yarp::dev::embObjDevPrivData>("embObjBattery");
50 }
51 
52 embObjBattery::embObjBattery(std::shared_ptr<yarp::dev::embObjDevPrivData> device) : device_(device)
53 {
54 }
55 
57 {
58  close();
59 }
60 
62 {
63  return device_->isOpen();
64 }
65 
66 bool embObjBattery::open(yarp::os::Searchable &config)
67 {
68  yInfo() << "embObjBattery::open(): preparing ETH resource";
69  if (!device_->prerareEthService(config, this))
70  return false;
71 
72  yInfo() << device_->getBoardInfo() << " embObjBattery::open(): browsing xml files which describe the service";
74  if (!parser.parse(config))
75  {
76  yError() << device_->getBoardInfo() << "open() fails to parse xml... cannot continue ";
77  return false;
78  }
79 
80  yInfo() << device_->getBoardInfo() << " embObjBattery::open(): verify the presence of the board and if its protocol version is correct";
81  if (!device_->res->verifyEPprotocol(eoprot_endpoint_analogsensors))
82  {
83  yError() << device_->getBoardInfo() << " open() fails to verifyEPprotocol... cannot continue ";
84  cleanup();
85  return false;
86  }
87 
88  yInfo() << device_->getBoardInfo() << " embObjBattery::open(): verify and activate the FT service";
89  eOmn_serv_parameter_t canBatteryData;
90  canBatteryData.configuration.type = eomn_serv_AS_battery;
91  canBatteryData.configuration.diagnosticsmode = eomn_serv_diagn_mode_NONE;
92  canBatteryData.configuration.diagnosticsparam = 0;
93  parser.toEomn(canBatteryData.configuration.data.as.battery);
94  if (!device_->res->serviceVerifyActivate(eomn_serv_category_battery, &canBatteryData, 5.0))
95  {
96  yError() << device_->getBoardInfo() << " open() fails to serviceVerifyActivate... cannot continue ";
97  cleanup();
98  return false;
99  }
100 
101  yInfo() << device_->getBoardInfo() << " embObjBattery::open(): configure the FT service";
102  if (false == sendConfig2boards(parser, device_->res))
103  {
104  yError() << device_->getBoardInfo() << " open() fails to sendConfig2boards... cannot continue";
105  cleanup();
106  return false;
107  }
108 
109  yInfo() << device_->getBoardInfo() << " embObjBattery::open(): impose the network variable which the ETH bord must stream up";
110  if (false == initRegulars(parser, device_->res))
111  {
112  yError() << device_->getBoardInfo() << " open() fails to initRegulars... cannot continue";
113  cleanup();
114  return false;
115  }
116 
117  yInfo() << device_->getBoardInfo() << " embObjBattery::open(): start the FT service";
118  if (!device_->res->serviceStart(eomn_serv_category_battery))
119  {
120  yError() << device_->getBoardInfo() << " open() fails to serviceStart... cannot continue";
121  cleanup();
122  return false;
123  }
124  else
125  {
126  if (device_->isVerbose())
127  {
128  yDebug() << device_->getBoardInfo() << " open() correctly starts service";
129  }
130  }
131 
132  yInfo() << device_->getBoardInfo() << " embObjBattery::open(): start streaming of FT data";
133  if (!sendStart2boards(parser, device_->res))
134  {
135  yError() << device_->getBoardInfo() << " open() fails to sendStart2boards... cannot continue";
136  cleanup();
137  return false;
138  }
139 
140  canBatteryData_.sensorName_ = eoboards_type2string(parser.getBatteryInfo().board);
142  device_->setOpen(true);
143  return true;
144 }
145 
147 {
148  auto &canBattery = parser.getBatteryInfo();
149 
150  eOprotID32_t id32 = eo_prot_ID32dummy;
151  eOas_battery_config_t cfg{0, 0};
152  cfg.period = canBattery.acquisitionRate;
153  id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_battery, 0, eoprot_tag_as_battery_config);
154 
155  if (false == deviceRes->setcheckRemoteValue(id32, &cfg, 10, 0.010, 0.050))
156  {
157  yError() << device_->getBoardInfo() << " sendConfig2boards() while try to configure fperiod=" << cfg.period;
158  return false;
159  }
160 
161  if (device_->isVerbose())
162  {
163  yDebug() << device_->getBoardInfo() << " sendConfig2boards() correctly configured boards with period=" << cfg.period;
164  }
165  return true;
166 }
167 
169 {
170  eOprotID32_t id32 = eo_prot_ID32dummy;
171 
172  uint8_t enable = 1;
173 
174  const auto &batteryInfos = parser.getBatteryInfo();
175 
176  id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_battery, 0, eoprot_tag_as_battery_cmmnds_enable);
177 
178  if (false == deviceRes->setcheckRemoteValue(id32, &enable, 10, 0.010, 0.050))
179  {
180  yError() << device_->getBoardInfo() << " sendStart2boards() while try to enable the boards transmission";
181  return false;
182  }
183 
184  if (device_->isVerbose())
185  {
186  yDebug() << device_->getBoardInfo() << " sendStart2boards() correctly enabled the boards transmission";
187  }
188  return true;
189 }
190 
192 {
193  // configure regular rops
194 
195  vector<eOprotID32_t> id32v;
196  eOprotID32_t id32 = eo_prot_ID32dummy;
197 
198  const auto &batteryInfos = parser.getBatteryInfo();
199  id32 = eoprot_ID_get(eoprot_endpoint_analogsensors, eoprot_entity_as_battery, 0, eoprot_tag_as_battery_status_timedvalue);
200  id32v.push_back(id32);
201 
202  if (false == deviceRes->serviceSetRegulars(eomn_serv_category_battery, id32v))
203  {
204  yError() << device_->getBoardInfo() << " initRegulars() fails to add its variables to regulars: cannot proceed any further";
205  return false;
206  }
207 
208  if (device_->isVerbose())
209  {
210  yDebug() << device_->getBoardInfo() << " initRegulars() added" << id32v.size() << "regular rops ";
211  char nvinfo[128];
212  for (size_t r = 0; r < id32v.size(); r++)
213  {
214  uint32_t item = id32v.at(r);
215  eoprot_ID2information(item, nvinfo, sizeof(nvinfo));
216  yDebug() << device_->getBoardInfo() << "\t it added regular rop for" << nvinfo;
217  }
218  }
219 
220  return true;
221 }
222 
224 {
226 }
227 
228 bool embObjBattery::update(eOprotID32_t id32, double timestamp, void *rxdata)
229 {
230  if (!device_->isOpen())
231  return false;
232  eOprotIndex_t eoprotIndex = eoprot_ID2index(id32);
233  if (eoprotIndex > 1)
234  {
235  yError() << device_->getBoardInfo() << " update() index too big";
236  return false;
237  }
238  eOprotEntity_t entity = eoprot_ID2entity(id32);
239  if (entity != eoprot_entity_as_battery)
240  {
241  yError() << device_->getBoardInfo() << " update() wrong entity";
242  return false;
243  }
244 
245  eOprotTag_t tag = eoprot_ID2tag(id32);
246  if (tag != eoprot_tag_as_battery_status_timedvalue)
247  {
248  yError() << device_->getBoardInfo() << " update() wrong tag";
249  return false;
250  }
251 
252  eOas_battery_timedvalue_t *data = (eOas_battery_timedvalue_t *)rxdata;
253  if (!checkUpdateTimeout(id32, data->age))
254  {
255  return false;
256  }
257 
258  if (!isPastFirstPrint && (data->age == 0))
259  {
260  yDebug("CAN DATA NOT YET AVAILABLE");
261  isPastFirstPrint = true;
262  }
263  else if (!isCanDataAvailable && (data->age != 0))
264  {
265  yDebug() << "First Status are:\n" << updateStatusStringStream(data->status, canBatteryData_.prevStatus_, true);
266 
267  canBatteryData_.prevStatus_ = data->status;
268  isCanDataAvailable = true;
269  }
270  else if (data->status != canBatteryData_.prevStatus_)
271  {
272  yDebug() << "Status changed to:\n" << updateStatusStringStream(data->status, canBatteryData_.prevStatus_, false);
273 
274  canBatteryData_.prevStatus_ = data->status;
275  }
276 
277  std::unique_lock<std::shared_mutex> lck(mutex_);
279 
280  return true;
281 }
282 
284 {
285  cleanup();
286  return true;
287 }
288 
290 {
291  device_->cleanup(static_cast<eth::IethResource *>(this));
292 }
293 
294 bool embObjBattery::checkUpdateTimeout(eOprotID32_t id32, eOabstime_t current)
295 {
297  {
298  return true;
299  }
300 
301  eOabstime_t diff = current - timeoutUpdate_[id32];
302  if (timeoutUpdate_[id32] != 0 && current > timeoutUpdate_[id32] + updateTimeout_)
303  {
304  yError() << device_->getBoardInfo() << " update timeout for index:" << eoprot_ID2index(id32);
305  timeoutUpdate_[id32] = current;
306  masStatus_[eoprot_ID2index(id32)] = MAS_TIMEOUT;
307  return false;
308  }
309  masStatus_[eoprot_ID2index(id32)] = MAS_OK;
310  timeoutUpdate_[id32] = current;
311  return true;
312 }
313 
314 std::string embObjBattery::updateStatusStringStream(const uint16_t &currStatus, const uint16_t &prevStatus, bool isFirstLoop)
315 {
316  // Initialize the first time the static map
317  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 =
318  {
319  {{eoas_bms_general_alarm_lowvoltage, "eoas_bms_general_alarm_lowvoltage"},
320  {eoas_bms_general_alarm_highvoltage, "eoas_bms_general_alarm_highvoltage"},
321  {eoas_bms_general_alarm_overcurrent_discharge, "eoas_bms_general_alarm_overcurrent_discharge"},
322  {eoas_bms_general_alarm_overcurrent_charge, "eoas_bms_general_alarm_overcurrent_charge"},
323  {eoas_bms_general_alarm_lowSOC, "eoas_bms_general_alarm_lowSOC"},
324  {eoas_bms_general_alarm_lowtemperature, "eoas_bms_general_alarm_lowtemperature"},
325  {eoas_bms_general_alarm_hightemperature, "eoas_bms_general_alarm_hightemperature"},
326  {eoas_bat_status_hsm_mosfet_broken, "eoas_bat_status_hsm_mosfet_broken"},
327  {eoas_bat_status_hsm_mosfet_normal, "eoas_bat_status_hsm_mosfet_normal"},
328  {eoas_bat_status_hsm_overcurrent_overvoltage, "eoas_bat_status_hsm_overcurrent_overvoltage"},
329  {eoas_bat_status_hsm_normal, "eoas_bat_status_hsm_normal"},
330  {eoas_bat_status_hsm_voltage_power_good, "eoas_bat_status_hsm_voltage_power_good"},
331  {eoas_bat_status_hsm_voltage_not_guaranteed, "eoas_bat_status_hsm_voltage_not_guaranteed"},
332  {eoas_bat_status_hsm_status_on, "eoas_bat_status_hsm_status_on"},
333  {eoas_bat_status_hsm_status_off, "eoas_bat_status_hsm_status_off"},
334  {eoas_bat_status_motor_regulator_overcurrent, "eoas_bat_status_motor_regulator_overcurrent"},
335  {eoas_bat_status_motor_regulator_normal, "eoas_bat_status_motor_regulator_normal"},
336  {eoas_bat_status_motor_on, "eoas_bat_status_motor_on"},
337  {eoas_bat_status_motor_off, "eoas_bat_status_motor_off"},
338  {eoas_bat_status_board_regulator_overcurrent, "eoas_bat_status_board_regulator_overcurrent"},
339  {eoas_bat_status_board_regulator_normal, "eoas_bat_status_board_regulator_normal"},
340  {eoas_bat_status_board_on, "eoas_bat_status_board_on"},
341  {eoas_bat_status_board_off, "eoas_bat_status_board_off"},
342  {eoas_bat_status_btn_2_start_up_phase, "eoas_bat_status_btn_2_start_up_phase"},
343  {eoas_bat_status_btn_2_stable_op, "eoas_bat_status_btn_2_stable_op"},
344  {eoas_bat_status_btn_1_start_up_phase, "eoas_bat_status_btn_1_start_up_phase"},
345  {eoas_bat_status_btn_1_stable_op, "eoas_bat_status_btn_1_stable_op"}}
346  };
347 
348  // Clear and reserve space for buffer for BAT and BMS
349  std::string statusstring = {};
350  statusstring.reserve(512);
351 
352  if (canBatteryData_.sensorType_ == eobrd_cantype_bms)
353  {
354  for (uint8_t i = 0; i < eoas_bms_alarm_numberof; i++)
355  {
356  if((embot::core::binary::bit::check(currStatus, i)))
357  {
358  statusstring.append("\t");
359  statusstring.append(s_boards_map_of_battery_alarm_status.at(i).second);
360  statusstring.append("\n");
361  }
362  }
363  }
364  else if(canBatteryData_.sensorType_ == eobrd_cantype_bat)
365  {
366  uint8_t bit_pos = 0;
367  for (uint8_t i = eoas_bms_alarm_numberof; i < eoas_battery_alarm_status_numberof; i = i+2)
368  {
369  if ((embot::core::binary::bit::check(currStatus, bit_pos) != embot::core::binary::bit::check(prevStatus, bit_pos)) || isFirstLoop)
370  {
371  statusstring.append("\t");
372  if((embot::core::binary::bit::check(currStatus, bit_pos)))
373  {
374  statusstring.append(s_boards_map_of_battery_alarm_status.at(i).second);
375  }
376  else
377  {
378  statusstring.append(s_boards_map_of_battery_alarm_status.at(i+1).second);
379  }
380  statusstring.append("\n");
381  }
382 
383  ++bit_pos;
384  }
385  }
386 
387  if(statusstring.empty())
388  {
389  statusstring.append("\tNo Faults Detected. All Alarms Bit Down\n");
390  }
391 
392 
393  return statusstring;
394 }
395 
396 double embObjBattery::calculateBoardTime(eOabstime_t current)
397 {
398  if (!useBoardTimeFlag_)
399  {
400  return yarp::os::Time::now();
401  }
402 
403  // Simulate real board time
404  if (firstYarpTimestamp_ == 0)
405  {
406  firstYarpTimestamp_ = yarp::os::Time::now();
407  firstCanTimestamp_ = current;
408  }
409  double realtime = firstYarpTimestamp_ + (double)(current - firstCanTimestamp_) / 1000000; // Simulate real board time
410  return realtime;
411 }
412 
413 bool embObjBattery::getBatteryVoltage(double &voltage)
414 {
415  voltage = canBatteryData_.voltage_;
416  return true;
417 }
418 
419 bool embObjBattery::getBatteryCurrent(double &current)
420 {
421  current = canBatteryData_.current_;
422  return true;
423 }
424 
426 {
427  charge = (canBatteryData_.charge_ != 0.0) ? canBatteryData_.charge_ : NAN;
428  return true;
429 }
430 
431 bool embObjBattery::getBatteryStatus(Battery_status &status)
432 {
433  status = static_cast<Battery_status>(canBatteryData_.status_);
434  return true;
435 }
436 
437 bool embObjBattery::getBatteryTemperature(double &temperature)
438 {
439  temperature = (canBatteryData_.temperature_ != 0) ? canBatteryData_.temperature_ : NAN;
440  return true;
441 }
442 
443 bool embObjBattery::getBatteryInfo(std::string &battery_info)
444 {
445  std::stringstream ss;
446  ss << "{\"temperature\":" << canBatteryData_.temperature_ << ",\"voltage\":" << canBatteryData_.voltage_ << ",\"current\":" << canBatteryData_.current_ << ",\"charge\":" << canBatteryData_.charge_ << ",\"status\":" << canBatteryData_.status_
447  << ",\"ts\":" << canBatteryData_.timeStamp_ << "}" << std::endl;
448 
449  battery_info = ss.str();
450  return true;
451 }
452 
454 {
455  if (temperature_ != other.temperature_)
456  return false;
457  if ((int)(voltage_ * 10) != (int)(other.voltage_ * 10)) // Only one digit after dot
458  return false;
459  if ((int)(current_ * 10) != (int)(other.current_ * 10))
460  return false;
461  if (charge_ != other.charge_)
462  return false;
463  if (status_ != other.status_)
464  return false;
465  if (timeStamp_ != other.timeStamp_)
466  return false;
467  if (sensorName_ != other.sensorName_)
468  return false;
469  if (sensorType_ != other.sensorType_)
470  return false;
471 
472  return true;
473 }
474 
476 {
477  return !operator==(other);
478 }
@ data
bool operator==(const BatteryInfo &right, const BatteryInfo &left)
Definition: batteryInfo.cpp:44
eObrd_type_t board
Definition: batteryInfo.h:20
uint8_t acquisitionRate
Definition: batteryInfo.h:19
uint16_t status_
Definition: embObjBattery.h:38
int16_t temperature_
Definition: embObjBattery.h:34
bool operator==(const CanBatteryData &other) const
float32_t current_
Definition: embObjBattery.h:36
eObrd_type_t sensorType_
Definition: embObjBattery.h:42
void decode(eOas_battery_timedvalue_t *data, double timestamp)
std::string sensorName_
Definition: embObjBattery.h:41
float32_t charge_
Definition: embObjBattery.h:37
float32_t voltage_
Definition: embObjBattery.h:35
uint16_t prevStatus_
Definition: embObjBattery.h:39
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:97
std::string updateStatusStringStream(const uint16_t &currStatus, const uint16_t &prevStatus, bool isFirstLoop)
bool initialised() override
std::map< eOprotID32_t, eOabstime_t > timeoutUpdate_
Definition: embObjBattery.h:79
bool sendConfig2boards(ServiceParserCanBattery &parser, eth::AbstractEthResource *deviceRes)
bool getBatteryVoltage(double &voltage) override
static constexpr bool useBoardTimeFlag_
Definition: embObjBattery.h:91
static constexpr eOabstime_t updateTimeout_
Definition: embObjBattery.h:87
virtual double calculateBoardTime(eOabstime_t current)
std::shared_mutex mutex_
Definition: embObjBattery.h:77
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:76
bool getBatteryInfo(std::string &battery_info) override
bool sendStart2boards(ServiceParserCanBattery &parser, eth::AbstractEthResource *deviceRes)
std::vector< yarp::dev::MAS_status > masStatus_
Definition: embObjBattery.h:88
bool initRegulars(ServiceParserCanBattery &parser, eth::AbstractEthResource *deviceRes)
bool update(eOprotID32_t id32, double timestamp, void *rxdata) override
static constexpr bool checkUpdateTimeoutFlag_
Definition: embObjBattery.h:90
CanBatteryData canBatteryData_
Definition: embObjBattery.h:78
bool getBatteryTemperature(double &temperature) override
bool getBatteryCharge(double &charge) override
iethresType_t
Definition: IethResource.h:62
@ iethres_analogbattery
Definition: IethResource.h:76
Copyright (C) 2008 RobotCub Consortium.