iCub-main
bmsBattery.cpp
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 // Copyright: (C) 2015 iCub Facility
4 // Authors: Marco Randazzo <marco.randazzo@iit.it>
5 // CopyPolicy: Released under the terms of the GNU GPL v2.0.
6 
7 #include <bmsBattery.h>
8 
9 #include <yarp/os/Time.h>
10 #include <yarp/os/LogStream.h>
11 #include <yarp/os/Log.h>
12 #include <iostream>
13 #include <string.h>
14 #include <time.h>
15 #include <stdlib.h>
16 
17 using namespace std;
18 #define DEBUG_TEST 1
19 
20 bool BmsBattery::open(yarp::os::Searchable& config)
21 {
22  bool correct=true;
23 
24  //debug
25  yDebug("%s\n", config.toString().c_str());
26 
27  Bottle& group_general = config.findGroup("GENERAL");
28  Bottle& group_serial = config.findGroup("SERIAL_PORT");
29 
30  if (group_general.isNull())
31  {
32  yError() << "Insufficient parameters to BmsBattery, section GENERAL missing";
33  return false;
34  }
35 
36  if (group_serial.isNull())
37  {
38  yError() << "Insufficient parameters to BmsBattery, section SERIAL_PORT missing";
39  return false;
40  }
41 
42  int period=config.find("thread_period").asInt32();
43  setPeriod((double)period/1000.0);
44 
45  Property prop;
46  std::string ps = group_serial.toString();
47  prop.fromString(ps);
48  prop.put("device", "serialport");
49 
50  //open the driver
51  driver.open(prop);
52  if (!driver.isValid())
53  {
54  yError() << "Error opening PolyDriver check parameters";
55 #ifndef DEBUG_TEST
56  return false;
57 #endif
58  }
59 
60  //open the serial interface
61  driver.view(pSerial);
62  if (!pSerial)
63  {
64  yError("Error opening serial driver. Device not available");
65 #ifndef DEBUG_TEST
66  return false;
67 #endif
68  }
69 
70  // Other options
71  this->verboseEnable = group_general.check("verbose", Value(0), "enable/disable the verbose mode").asBool();
72  this->screenEnable = group_general.check("screen", Value(0), "enable/disable the screen output").asBool();
73  this->debugEnable = group_general.check("debug", Value(0), "enable/disable the debug mode").asBool();
74 
75  PeriodicThread::start();
76  return true;
77 }
78 
80 {
81  //stop the thread
83 
84  //stop the driver
85  driver.close();
86 
87  return true;
88 }
89 
91 {
92  battery_info = "icub battery system v1.0";
93  battery_voltage = 0.0;
94  battery_current = 0.0;
95  battery_charge = 0.0;
96  battery_temperature = 0.0;
97  timeStamp = yarp::os::Time::now();
98 
99  return true;
100 }
101 
102 bool BmsBattery::verify_checksum(int& raw_battery_current, int& raw_battery_voltage, int& raw_battery_charge, int& raw_battery_checksum)
103 {
104  if (raw_battery_checksum == raw_battery_current + raw_battery_voltage + raw_battery_charge)
105  return true;
106  return false;
107 }
108 
110 {
111  double timeNow=yarp::os::Time::now();
112  lock_guard<mutex> lck(mtx);
113 
114  //if 100ms have passed since the last received message
115  if (timeStamp+0.1<timeNow)
116  {
117  //status=IBattery::BATTERY_TIMEOUT;
118  }
119 
120  //read battery data.
121  //if nothing is received, rec=0, the while exits immediately. The string will be not valid, so the parser will skip it and it will leave unchanged the battery status (voltage/current/charge)
122  //if a text line is received, then try to receive more text to empty the buffer. If nothing else is received, serial_buff will be left unchanged from the previous value. The loop will exit and the sting will be parsed.
123  serial_buff[0] = 0;
124  int rec = 0;
125  do
126  {
127  rec = pSerial->receiveLine(serial_buff, 250);
128  if (debugEnable) yDebug("%d <%s> ", rec, serial_buff);
129  } while
130  (rec>0);
131 
132  int len = strlen(serial_buff);
133  bool reading_ok = false;
134  if (len>0)
135  {
136  int pars = 0;
137  int raw_battery_current = 0;
138  int raw_battery_voltage = 0;
139  int raw_battery_charge = 0;
140  int raw_battery_checksum = 0;
141  pars = sscanf(serial_buff, "%*s %d %*s %d %*s %d %*s %d", &raw_battery_current, &raw_battery_voltage, &raw_battery_charge, &raw_battery_checksum);
142 
143  if (pars == 4)
144  {
145  if (verify_checksum(raw_battery_current, raw_battery_voltage, raw_battery_charge, raw_battery_checksum))
146  {
147  time_t rawtime;
148  struct tm * timeinfo;
149  time(&rawtime);
150  timeinfo = localtime(&rawtime);
151  //battery_data.timestamp = asctime(timeinfo);
152  battery_voltage = double(battery_voltage) / 1024 * 66;
153  battery_current = (double(battery_current) - 512) / 128 * 20; //+- 60 is the maximum current that the sensor can read. 128+512 is the value of the AD
154  //when the current is 20A.
155  battery_charge = double(battery_charge) / 100; // the value coming from the BCS board goes from 0 to 100%
156  reading_ok = true;
157  }
158  else
159  {
160  yError("checksum error while reading battery data\n");
161  }
162  }
163  else
164  {
165  yError("error reading battery data: %d\n", pars);
166  }
167  }
168 
169  // print data to screen
170  if (screenEnable)
171  {
172  char buff[1024];
173  sprintf(buff, "battery status: %+6.1fA % 6.1fV charge:% 6.1f%%", battery_current, battery_voltage, battery_charge);
174  yDebug("BmsBattery::run() log_buffer is: %s", buff);
175  }
176 }
177 
178 bool BmsBattery::getBatteryVoltage(double &voltage)
179 {
180  lock_guard<mutex> lck(mtx);
181  voltage = battery_voltage;
182  return true;
183 }
184 
185 bool BmsBattery::getBatteryCurrent(double &current)
186 {
187  lock_guard<mutex> lck(mtx);
188  current = battery_current;
189  return true;
190 }
191 
192 bool BmsBattery::getBatteryCharge(double &charge)
193 {
194  lock_guard<mutex> lck(mtx);
195  charge = battery_charge;
196  return true;
197 }
198 
199 bool BmsBattery::getBatteryStatus(Battery_status &status)
200 {
201  //yError("Not yet implemented");
202  return false;
203 }
204 
205 bool BmsBattery::getBatteryTemperature(double &temperature)
206 {
207  //yError("Not yet implemented");
208  return false;
209 }
210 
212 {
213  lock_guard<mutex> lck(mtx);
214  info = battery_info;
215  return true;
216 }
217 
219 {
220  yTrace("BmsBattery Thread released\n");
221 }
virtual bool getBatteryVoltage(double &voltage)
Definition: bmsBattery.cpp:178
virtual bool getBatteryTemperature(double &temperature)
Definition: bmsBattery.cpp:205
virtual bool getBatteryStatus(Battery_status &status)
Definition: bmsBattery.cpp:199
virtual void threadRelease()
Definition: bmsBattery.cpp:218
virtual bool close()
Definition: bmsBattery.cpp:79
virtual bool open(yarp::os::Searchable &config)
Definition: bmsBattery.cpp:20
virtual bool getBatteryInfo(std::string &info)
Definition: bmsBattery.cpp:211
virtual bool threadInit()
Definition: bmsBattery.cpp:90
virtual bool getBatteryCharge(double &charge)
Definition: bmsBattery.cpp:192
virtual void run()
Definition: bmsBattery.cpp:109
virtual bool getBatteryCurrent(double &current)
Definition: bmsBattery.cpp:185
static int stop
Definition: iCub_Sim.cpp:41
degrees time
Definition: sine.m:5