9 #include <yarp/os/Time.h>
10 #include <yarp/os/LogStream.h>
11 #include <yarp/os/Log.h>
25 yDebug(
"%s\n", config.toString().c_str());
27 Bottle& group_general = config.findGroup(
"GENERAL");
28 Bottle& group_serial = config.findGroup(
"SERIAL_PORT");
30 if (group_general.isNull())
32 yError() <<
"Insufficient parameters to BmsBattery, section GENERAL missing";
36 if (group_serial.isNull())
38 yError() <<
"Insufficient parameters to BmsBattery, section SERIAL_PORT missing";
42 int period=config.find(
"thread_period").asInt32();
43 setPeriod((
double)period/1000.0);
46 std::string ps = group_serial.toString();
48 prop.put(
"device",
"serialport");
52 if (!driver.isValid())
54 yError() <<
"Error opening PolyDriver check parameters";
64 yError(
"Error opening serial driver. Device not available");
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();
75 PeriodicThread::start();
82 PeriodicThread::stop();
92 battery_info =
"icub battery system v1.0";
93 battery_voltage = 0.0;
94 battery_current = 0.0;
96 battery_temperature = 0.0;
97 timeStamp = yarp::os::Time::now();
102 bool BmsBattery::verify_checksum(
int& raw_battery_current,
int& raw_battery_voltage,
int& raw_battery_charge,
int& raw_battery_checksum)
104 if (raw_battery_checksum == raw_battery_current + raw_battery_voltage + raw_battery_charge)
111 double timeNow=yarp::os::Time::now();
112 lock_guard<mutex> lck(mtx);
115 if (timeStamp+0.1<timeNow)
127 rec = pSerial->receiveLine(serial_buff, 250);
128 if (debugEnable) yDebug(
"%d <%s> ", rec, serial_buff);
132 int len = strlen(serial_buff);
133 bool reading_ok =
false;
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);
145 if (verify_checksum(raw_battery_current, raw_battery_voltage, raw_battery_charge, raw_battery_checksum))
148 struct tm * timeinfo;
150 timeinfo = localtime(&rawtime);
152 battery_voltage = double(battery_voltage) / 1024 * 66;
153 battery_current = (double(battery_current) - 512) / 128 * 20;
155 battery_charge = double(battery_charge) / 100;
160 yError(
"checksum error while reading battery data\n");
165 yError(
"error reading battery data: %d\n", pars);
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);
180 lock_guard<mutex> lck(mtx);
181 voltage = battery_voltage;
187 lock_guard<mutex> lck(mtx);
188 current = battery_current;
194 lock_guard<mutex> lck(mtx);
195 charge = battery_charge;
213 lock_guard<mutex> lck(mtx);
220 yTrace(
"BmsBattery Thread released\n");
virtual bool getBatteryVoltage(double &voltage)
virtual bool getBatteryTemperature(double &temperature)
virtual bool getBatteryStatus(Battery_status &status)
virtual void threadRelease()
virtual bool open(yarp::os::Searchable &config)
virtual bool getBatteryInfo(std::string &info)
virtual bool threadInit()
virtual bool getBatteryCharge(double &charge)
virtual bool getBatteryCurrent(double ¤t)