iCub-main
Loading...
Searching...
No Matches
bcbBattery.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 <bcbBattery.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#include <cmath>
17
18using namespace std;
19
20//#define DEBUG_TEST 1
21
22#ifdef DEBUG_TEST
23int test_buffer(unsigned char* tmp_buff, int buff_len)
24{
25 // 012345678901234567890123456789
26 // 567890123456789012345678901234
27 string s = "MMN...AABBCCD...EEFFGGH...IILL";
28 strcpy((char*)(tmp_buff), s.c_str());
29 (tmp_buff)[03] = '\r'; (tmp_buff)[04] = '\n'; (tmp_buff)[05] = '\0';
30 (tmp_buff)[13] = '\r'; (tmp_buff)[14] = '\n'; (tmp_buff)[15] = '\0';
31 (tmp_buff)[23] = '\r'; (tmp_buff)[24] = '\n'; (tmp_buff)[25] = '\0';
32 return (int)(s.size());
33}
34#endif
35
36bool BcbBattery::open(yarp::os::Searchable& config)
37{
38 //debug
39 yDebug("%s\n", config.toString().c_str());
40
41 Bottle& group_general = config.findGroup("GENERAL");
42 Bottle& group_serial = config.findGroup("SERIAL_PORT");
43
44 if (group_general.isNull())
45 {
46 yError() << "Insufficient parameters to BcbBattery, section GENERAL missing";
47 return false;
48 }
49
50 if (group_serial.isNull())
51 {
52 yError() << "Insufficient parameters to BcbBattery, section SERIAL_PORT missing";
53 return false;
54 }
55
56 int period=group_general.find("thread_period").asInt32();
57
58 Property prop;
59 std::string ps = group_serial.toString();
60 prop.fromString(ps);
61 prop.put("device", "serialport");
62
63 //open the driver
64 driver.open(prop);
65 if (!driver.isValid())
66 {
67 yError() << "Error opening PolyDriver check parameters";
68#ifndef DEBUG_TEST
69 return false;
70#endif
71 }
72
73 //open the serial interface
74 driver.view(iSerial);
75 if (!iSerial)
76 {
77 yError("Error opening serial driver. Device not available");
78#ifndef DEBUG_TEST
79 return false;
80#endif
81 }
82
83 //create the thread
84 batteryReader = new batteryReaderThread(iSerial, (double)period / 1000.0);
85 // Other options
86 batteryReader->verboseEnable = group_general.check("verbose", Value(0), "enable/disable the verbose mode").asBool();
87 batteryReader->screenEnable = group_general.check("screen", Value(0), "enable/disable the screen output").asBool();
88
89 //start the thread
90 batteryReader->start();
91 return true;
92}
93
95{
96 //stop the thread
97 if (batteryReader)
98 {
99 batteryReader->stop();
100 delete batteryReader;
101 }
102
103 //stop the driver
104 driver.close();
105
106 return true;
107}
108
110{
111 timeStamp = yarp::os::Time::now();
112
113 if (iSerial)
114 {
115 yInfo("BcbBattery starting transmission");
117 yInfo("BcbBattery started successfully");
118 }
119 else
120 {
121#ifndef DEBUG_TEST
122 yError("BcbBattery iSerial == NULL");
123 return false;
124#endif
125 }
126
127 return true;
128}
129
131{
132 double timeNow=yarp::os::Time::now();
133 int recb = 0;
134
135 //read battery data.
136#ifndef DEBUG_TEST
137 if (iSerial)
138 {
139 recb = iSerial->receiveBytes(tmp_buff, buff_len);
140 }
141 else
142 {
143 yError("BcbBattery iSerial == NULL");
144 }
145#else
146 recb = test_buffer((unsigned char*)(tmp_buff), buff_len);
147#endif
148
149 if (verboseEnable)
150 {
151 snprintf(debugTextBuffer, debugTextBufferSize, "Internal buffer: ");
152 for (size_t i = 0; i < recb; i++)
153 snprintf(debugTextBuffer+strlen(debugTextBuffer), debugTextBufferSize-strlen(debugTextBuffer), "%02X ", (unsigned int)(tmp_buff[i] & 0xFF));
154 yDebug() << debugTextBuffer;
155 }
156
157 //parse battery data
158 std::cmatch cmatch;
159 bool b = std::regex_search((const char*)tmp_buff, (const char*)tmp_buff+recb, cmatch, r_exp);
160
161 if (b)
162 {
163 //get the data
164 for (size_t i=0; i< packet_len; i++)
165 {
166 packet[i] = cmatch.str().c_str()[i];
167 }
168
169 //add checksum verification.
170 //...
171
172 if (verboseEnable)
173 {
174 snprintf(debugTextBuffer, debugTextBufferSize, "Found: ");
175 snprintf(debugTextBuffer + strlen(debugTextBuffer), debugTextBufferSize - strlen(debugTextBuffer), "<%02X> ", (unsigned int)(packet[0] & 0xFF));
176 snprintf(debugTextBuffer + strlen(debugTextBuffer), debugTextBufferSize - strlen(debugTextBuffer), "(%02X %02X) ", (unsigned int)(packet[1] & 0xFF), (unsigned int)(packet[2] & 0xFF));
177 snprintf(debugTextBuffer + strlen(debugTextBuffer), debugTextBufferSize - strlen(debugTextBuffer), "(%02X %02X) ", (unsigned int)(packet[3] & 0xFF), (unsigned int)(packet[4] & 0xFF));
178 snprintf(debugTextBuffer + strlen(debugTextBuffer), debugTextBufferSize - strlen(debugTextBuffer), "(%02X %02X) ", (unsigned int)(packet[5] & 0xFF), (unsigned int)(packet[6] & 0xFF));
179 snprintf(debugTextBuffer + strlen(debugTextBuffer), debugTextBufferSize - strlen(debugTextBuffer), "(%02X) ", (unsigned int)(packet[7] & 0xFF));
180 snprintf(debugTextBuffer + strlen(debugTextBuffer), debugTextBufferSize - strlen(debugTextBuffer), "<%02X %02X>", (unsigned int)(packet[8] & 0xFF), (unsigned int)(packet[9] & 0xFF));
181 yDebug() << debugTextBuffer;
182 }
183
184 //parse values
185 datamut.lock();
186 battery_voltage = ((unsigned int)(packet[1])) << 8;
187 battery_voltage += (unsigned char)(packet[2]);
188 battery_voltage /= 1000.0;
189 battery_current = ((unsigned int)(packet[3])) << 8;
190 battery_current += (unsigned char)(packet[4]);
191 battery_current /= 1000.0;
192 battery_charge = ((unsigned int)(packet[5])) << 8;
193 battery_charge += (unsigned char)(packet[6]);
194 backpack_status = (unsigned int)(packet[7]);
195 battery_status = IBattery::Battery_status::BATTERY_OK_IN_USE;
196 datamut.unlock();
197 }
198 else
199 {
200 //do nothing
201 }
202
203 // print data to screen
204 if (screenEnable)
205 {
206 time_t rawtime;
207 struct tm * timeinfo;
208 time(&rawtime);
209 timeinfo = localtime(&rawtime);
210 char* battery_timestamp = asctime(timeinfo);
211 char buff[1024];
212 snprintf(buff, 1024, "%6.1fV %+6.1fA, charge:%6.1f%%, time: %s", battery_voltage, battery_current, battery_charge, battery_timestamp);
213 yDebug("BcbBattery::run() log_buffer is: %s", buff);
214 }
215
216 //flush the buffer
217#ifndef DEBUG_TEST
218 if (iSerial)
219 {
220 iSerial->flush();
221 }
222#endif
223}
224
225bool BcbBattery::getBatteryVoltage(double &voltage)
226{
227 if (!batteryReader) return false;
228 std::lock_guard<std::mutex> lg(batteryReader->datamut);
230 return true;
231}
232
233bool BcbBattery::getBatteryCurrent(double &current)
234{
235 if (!batteryReader) return false;
236 std::lock_guard<std::mutex> lg(batteryReader->datamut);
238 return true;
239}
240
242{
243 if (!batteryReader) return false;
244 std::lock_guard<std::mutex> lg(batteryReader->datamut);
246 return true;
247}
248
249bool BcbBattery::getBatteryStatus(Battery_status &status)
250{
251 if (!batteryReader) return false;
252 std::lock_guard<std::mutex> lg(batteryReader->datamut);
254 return true;
255}
256
257bool BcbBattery::getBatteryTemperature(double &temperature)
258{
259 //yError("Not yet implemented");
260 temperature = std::nan("");
261 return false;
262}
263
265{
266 if (!batteryReader) return false;
267 std::lock_guard<std::mutex> lg(batteryReader->datamut);
269 return true;
270}
271
276
278{
279 if (!iSerial) return;
280
281 //start the transmission
282 char cmd = 0x01;
283 bool ret = iSerial->send(&cmd, 1);
284 if (ret == false)
285 {
286 yError("BcbBattery problems starting the transmission");
287 return;
288 }
289
290 //empty the buffer
291 iSerial->flush();
292}
293
295{
296 if (!iSerial) return;
297
298 char c = 0x00;
299 bool ret = iSerial->send(&c, 1);
300 if (ret == false) { yError("BcbBattery problems while stopping the transmission"); }
301}
virtual bool close()
ISerialDevice * iSerial
Definition bcbBattery.h:78
virtual bool getBatteryVoltage(double &voltage) override
PolyDriver driver
Definition bcbBattery.h:77
virtual bool getBatteryTemperature(double &temperature) override
virtual bool open(yarp::os::Searchable &config)
virtual bool getBatteryInfo(std::string &info) override
virtual bool getBatteryCharge(double &charge) override
virtual bool getBatteryCurrent(double &current) override
batteryReaderThread * batteryReader
Definition bcbBattery.h:74
virtual bool getBatteryStatus(Battery_status &status) override
IBattery::Battery_status battery_status
Definition bcbBattery.h:49
virtual void run() override
ISerialDevice * iSerial
Definition bcbBattery.h:42
virtual void threadRelease() override
virtual bool threadInit() override
unsigned char packet[packet_len]
Definition bcbBattery.h:39
std::mutex datamut
Definition bcbBattery.h:43
char debugTextBuffer[debugTextBufferSize]
Definition bcbBattery.h:28
static const int debugTextBufferSize
Definition bcbBattery.h:27
unsigned char tmp_buff[buff_len]
Definition bcbBattery.h:37
std::string battery_info
Definition bcbBattery.h:47
static const int packet_len
Definition bcbBattery.h:38
static const int buff_len
Definition bcbBattery.h:36
cmd
Definition dataTypes.h:30
degrees time
Definition sine.m:5