iCub-main
Loading...
Searching...
No Matches
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
17using namespace std;
18#define DEBUG_TEST 1
19
20bool 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
82 PeriodicThread::stop();
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;
97 timeStamp = yarp::os::Time::now();
98
99 return true;
100}
101
102bool 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
178bool BmsBattery::getBatteryVoltage(double &voltage)
179{
180 lock_guard<mutex> lck(mtx);
181 voltage = battery_voltage;
182 return true;
183}
184
185bool BmsBattery::getBatteryCurrent(double &current)
186{
187 lock_guard<mutex> lck(mtx);
188 current = battery_current;
189 return true;
190}
191
193{
194 lock_guard<mutex> lck(mtx);
195 charge = battery_charge;
196 return true;
197}
198
199bool BmsBattery::getBatteryStatus(Battery_status &status)
200{
201 //yError("Not yet implemented");
202 return false;
203}
204
205bool 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)
virtual bool getBatteryTemperature(double &temperature)
double timeStamp
Definition bmsBattery.h:28
std::string battery_info
Definition bmsBattery.h:34
double battery_voltage
Definition bmsBattery.h:31
bool verboseEnable
Definition bmsBattery.h:37
bool screenEnable
Definition bmsBattery.h:38
virtual bool getBatteryStatus(Battery_status &status)
virtual void threadRelease()
virtual bool close()
char serial_buff[255]
Definition bmsBattery.h:44
virtual bool open(yarp::os::Searchable &config)
virtual bool getBatteryInfo(std::string &info)
virtual bool threadInit()
double battery_current
Definition bmsBattery.h:32
bool debugEnable
Definition bmsBattery.h:39
virtual bool getBatteryCharge(double &charge)
std::mutex mtx
Definition bmsBattery.h:24
ISerialDevice * pSerial
Definition bmsBattery.h:43
double battery_temperature
Definition bmsBattery.h:33
PolyDriver driver
Definition bmsBattery.h:42
virtual void run()
double battery_charge
Definition bmsBattery.h:30
virtual bool getBatteryCurrent(double &current)
degrees time
Definition sine.m:5