9 #include <yarp/os/Time.h>
10 #include <yarp/os/LogStream.h>
11 #include <yarp/os/Log.h>
23 int test_buffer(
unsigned char* tmp_buff,
int buff_len)
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());
39 yDebug(
"%s\n", config.toString().c_str());
41 Bottle& group_general = config.findGroup(
"GENERAL");
42 Bottle& group_serial = config.findGroup(
"SERIAL_PORT");
44 if (group_general.isNull())
46 yError() <<
"Insufficient parameters to BcbBattery, section GENERAL missing";
50 if (group_serial.isNull())
52 yError() <<
"Insufficient parameters to BcbBattery, section SERIAL_PORT missing";
56 int period=group_general.find(
"thread_period").asInt32();
59 std::string ps = group_serial.toString();
61 prop.put(
"device",
"serialport");
65 if (!driver.isValid())
67 yError() <<
"Error opening PolyDriver check parameters";
77 yError(
"Error opening serial driver. Device not available");
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();
90 batteryReader->start();
99 batteryReader->stop();
100 delete batteryReader;
111 timeStamp = yarp::os::Time::now();
115 yInfo(
"BcbBattery starting transmission");
117 yInfo(
"BcbBattery started successfully");
122 yError(
"BcbBattery iSerial == NULL");
132 double timeNow=yarp::os::Time::now();
139 recb = iSerial->receiveBytes(tmp_buff, buff_len);
143 yError(
"BcbBattery iSerial == NULL");
146 recb = test_buffer((
unsigned char*)(tmp_buff), buff_len);
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;
159 bool b = std::regex_search((
const char*)tmp_buff, (
const char*)tmp_buff+recb, cmatch, r_exp);
164 for (
size_t i=0; i< packet_len; i++)
166 packet[i] = cmatch.str().c_str()[i];
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;
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;
207 struct tm * timeinfo;
209 timeinfo = localtime(&rawtime);
210 char* battery_timestamp = asctime(timeinfo);
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);
227 if (!batteryReader)
return false;
228 std::lock_guard<std::mutex> lg(batteryReader->datamut);
229 voltage = batteryReader->battery_voltage;
235 if (!batteryReader)
return false;
236 std::lock_guard<std::mutex> lg(batteryReader->datamut);
237 current = batteryReader->battery_current;
243 if (!batteryReader)
return false;
244 std::lock_guard<std::mutex> lg(batteryReader->datamut);
245 charge = batteryReader->battery_charge;
251 if (!batteryReader)
return false;
252 std::lock_guard<std::mutex> lg(batteryReader->datamut);
253 status= batteryReader->battery_status;
260 temperature = std::nan(
"");
266 if (!batteryReader)
return false;
267 std::lock_guard<std::mutex> lg(batteryReader->datamut);
268 info = batteryReader->battery_info;
279 if (!iSerial)
return;
283 bool ret = iSerial->send(&
cmd, 1);
286 yError(
"BcbBattery problems starting the transmission");
296 if (!iSerial)
return;
299 bool ret = iSerial->send(&c, 1);
300 if (ret ==
false) { yError(
"BcbBattery problems while stopping the transmission"); }
virtual bool getBatteryVoltage(double &voltage) override
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 ¤t) override
virtual bool getBatteryStatus(Battery_status &status) override
virtual void run() override
virtual void threadRelease() override
virtual bool threadInit() override