diff --git a/src/libraries/icubmod/bcbBattery/bcbBattery.cpp b/src/libraries/icubmod/bcbBattery/bcbBattery.cpp index 28237bf0d5..c3730ccd60 100644 --- a/src/libraries/icubmod/bcbBattery/bcbBattery.cpp +++ b/src/libraries/icubmod/bcbBattery/bcbBattery.cpp @@ -13,10 +13,26 @@ #include #include #include +#include using namespace std; + //#define DEBUG_TEST 1 +#ifdef DEBUG_TEST +int test_buffer(unsigned char* tmp_buff, int buff_len) +{ + // 012345678901234567890123456789 + // 567890123456789012345678901234 + string s = "MMN...AABBCCD...EEFFGGH...IILL"; + strcpy((char*)(tmp_buff), s.c_str()); + (tmp_buff)[03] = '\r'; (tmp_buff)[04] = '\n'; (tmp_buff)[05] = '\0'; + (tmp_buff)[13] = '\r'; (tmp_buff)[14] = '\n'; (tmp_buff)[15] = '\0'; + (tmp_buff)[23] = '\r'; (tmp_buff)[24] = '\n'; (tmp_buff)[25] = '\0'; + return (int)(s.size()); +} +#endif + bool BcbBattery::open(yarp::os::Searchable& config) { //debug @@ -38,7 +54,6 @@ bool BcbBattery::open(yarp::os::Searchable& config) } int period=group_general.find("thread_period").asInt(); - setPeriod((double)period/1000.0); Property prop; std::string ps = group_serial.toString(); @@ -56,8 +71,8 @@ bool BcbBattery::open(yarp::os::Searchable& config) } //open the serial interface - driver.view(pSerial); - if (!pSerial) + driver.view(iSerial); + if (!iSerial) { yError("Error opening serial driver. Device not available"); #ifndef DEBUG_TEST @@ -65,214 +80,126 @@ bool BcbBattery::open(yarp::os::Searchable& config) #endif } + //create the thread + batteryReader = new batteryReaderThread(iSerial, (double)period / 1000.0); // Other options - this->verboseEnable = group_general.check("verbose", Value(0), "enable/disable the verbose mode").asBool(); - this->screenEnable = group_general.check("screen", Value(0), "enable/disable the screen output").asBool(); - this->silenceSyncWarnings = group_general.check("silence_sync_warnings", Value(0), "enable/disable the print of warnings in case of sync errors.").asBool(); + batteryReader->verboseEnable = group_general.check("verbose", Value(0), "enable/disable the verbose mode").asBool(); + batteryReader->screenEnable = group_general.check("screen", Value(0), "enable/disable the screen output").asBool(); - this->isClosing = false; - PeriodicThread::start(); + //start the thread + batteryReader->start(); return true; } bool BcbBattery::close() { - isClosing = true; - + //stop the thread + if (batteryReader) { - lock_guard lck(mtx); - - //stop the thread - PeriodicThread::stop(); - - char c = 0x00; - if (pSerial) - { - bool ret = pSerial->send(&c, 1); - if (ret == false) { yError("BcbBattery problems while stopping the transmission");} - } - - //stop the driver - driver.close(); + batteryReader->stop(); + delete batteryReader; } + //stop the driver + driver.close(); + return true; } -bool BcbBattery::threadInit() +bool batteryReaderThread::threadInit() { - battery_info = "icub battery system v1.0"; - battery_voltage = 0.0; - battery_current = 0.0; - battery_charge = 0.0; timeStamp = yarp::os::Time::now(); - //start the transmission - char c = 0x01; - if (pSerial) + if (iSerial) { - bool ret = pSerial->send(&c, 1); - if (ret == false) { yError("BcbBattery problems starting the transmission"); return false; } - - //empty the buffer - char c = 0; - int r = 0; - do - { - r = pSerial->receiveChar(c); - } while (r != 0); + yInfo("BcbBattery starting transmission"); + startTransmission(); + yInfo("BcbBattery started successfully"); } else { - yError("BcbBattery pSerial == NULL"); +#ifndef DEBUG_TEST + yError("BcbBattery iSerial == NULL"); return false; +#endif } - return true; } -void BcbBattery::run() +void batteryReaderThread::run() { double timeNow=yarp::os::Time::now(); - lock_guard lck(mtx); - - //if 100ms have passed since the last received message - if (timeStamp+0.1receiveChar(serial_buff[0]); - sync0 = output[0] && (serial_buff[0] == '\0'); - ++i; - } while (!(sync0) && (i < 20) && !isClosing); - - if (isClosing) - { - return; - } - - if (sync0) - { - output[1] = pSerial->receiveChar(serial_buff[1]); //voltage - output[2] = pSerial->receiveChar(serial_buff[2]); //voltage - output[3] = pSerial->receiveChar(serial_buff[3]); //current - output[4] = pSerial->receiveChar(serial_buff[4]); //current - output[5] = pSerial->receiveChar(serial_buff[5]); //charge - output[6] = pSerial->receiveChar(serial_buff[6]); //charge - output[7] = pSerial->receiveChar(serial_buff[7]); //status - } - - i = 0; - do - { - output[8] = pSerial->receiveChar(serial_buff[8]); - syncr = output[8] && (serial_buff[8] == '\r'); - if (syncr) - { - output[9] = pSerial->receiveChar(serial_buff[9]); - syncn = output[9] && (serial_buff[9] == '\n'); - - if (!syncn) - { - if (!syncError && !silenceSyncWarnings) - { - yDebug() << "Sync error (n)"; - } - syncError = true; - } - } - else - { - if (!syncError && !silenceSyncWarnings) - { - yDebug() << "Sync error (r)"; - } - syncError = true; - } - ++i; - } while (!(syncn) && (i < 20) && !isClosing); - - if (isClosing) - { - return; - } - - serial_buff[10] = 0; + recb = iSerial->receiveBytes(tmp_buff, buff_len); } else { - yError("BcbBattery pSerial == NULL"); + yError("BcbBattery iSerial == NULL"); } - -#if DEBUG_TEST - battery_voltage = 40.0; - battery_current = 5.0; - battery_charge = 72.0; - battery_temperature = 35.0; #else + recb = test_buffer((unsigned char*)(tmp_buff), buff_len); +#endif if (verboseEnable) { - char hexBuffer[31]; - char decBuffer[41]; - for (size_t i = 0; i < 10; ++i) - { - if (output[i]) - { - sprintf(hexBuffer + 3*i,"%02X ", (unsigned int)(serial_buff[i] & 0xFF)); - sprintf(decBuffer + 4*i,"%03u ", (unsigned int)(serial_buff[i] & 0xFF)); - } - else - { - sprintf(hexBuffer + 3*i,"-- "); - sprintf(decBuffer + 4*i,"--- "); - } - } - - yDebug("BcbBattery::run() serial_buffer is: (hex) %s, (dec) %s", hexBuffer, decBuffer); + snprintf(debugTextBuffer, debugTextBufferSize, "Internal buffer: "); + for (size_t i = 0; i < recb; i++) + snprintf(debugTextBuffer+strlen(debugTextBuffer), debugTextBufferSize-strlen(debugTextBuffer), "%02X ", (unsigned int)(tmp_buff[i] & 0xFF)); + yDebug() << debugTextBuffer; } - if (output[1] && output[2] && !syncError) - { - battery_voltage = ((unsigned char) serial_buff[1] * 256 + (unsigned char) serial_buff[2])/1000.0; - } + //parse battery data + std::cmatch cmatch; + bool b = std::regex_search((const char*)tmp_buff, (const char*)tmp_buff+recb, cmatch, r_exp); - if (output[3] && output[4] && !syncError) + if (b) { - battery_current = ((unsigned char) serial_buff[3] * 256 + (unsigned char) serial_buff[4])/1000.0; - } + //get the data + for (size_t i=0; i< packet_len; i++) + { + packet[i] = cmatch.str().c_str()[i]; + } - if (output[5] && output[6] && !syncError) - { - battery_charge = (unsigned char) serial_buff[5] * 256 + (unsigned char) serial_buff[6]; - } + //add checksum verification. + //... - if (output[7] && !syncError) + if (verboseEnable) + { + snprintf(debugTextBuffer, debugTextBufferSize, "Found: "); + snprintf(debugTextBuffer + strlen(debugTextBuffer), debugTextBufferSize - strlen(debugTextBuffer), "<%02X> ", (unsigned int)(packet[0] & 0xFF)); + snprintf(debugTextBuffer + strlen(debugTextBuffer), debugTextBufferSize - strlen(debugTextBuffer), "(%02X %02X) ", (unsigned int)(packet[1] & 0xFF), (unsigned int)(packet[2] & 0xFF)); + snprintf(debugTextBuffer + strlen(debugTextBuffer), debugTextBufferSize - strlen(debugTextBuffer), "(%02X %02X) ", (unsigned int)(packet[3] & 0xFF), (unsigned int)(packet[4] & 0xFF)); + snprintf(debugTextBuffer + strlen(debugTextBuffer), debugTextBufferSize - strlen(debugTextBuffer), "(%02X %02X) ", (unsigned int)(packet[5] & 0xFF), (unsigned int)(packet[6] & 0xFF)); + snprintf(debugTextBuffer + strlen(debugTextBuffer), debugTextBufferSize - strlen(debugTextBuffer), "(%02X) ", (unsigned int)(packet[7] & 0xFF)); + snprintf(debugTextBuffer + strlen(debugTextBuffer), debugTextBufferSize - strlen(debugTextBuffer), "<%02X %02X>", (unsigned int)(packet[8] & 0xFF), (unsigned int)(packet[9] & 0xFF)); + yDebug() << debugTextBuffer; + } + + //parse values + datamut.lock(); + battery_voltage = ((unsigned int)(packet[1])) << 8; + battery_voltage += (unsigned char)(packet[2]); + battery_voltage /= 1000.0; + battery_current = ((unsigned int)(packet[3])) << 8; + battery_current += (unsigned char)(packet[4]); + battery_current /= 1000.0; + battery_charge = ((unsigned int)(packet[5])) << 8; + battery_charge += (unsigned char)(packet[6]); + backpack_status = (unsigned int)(packet[7]); + battery_status = IBattery::Battery_status::BATTERY_OK_IN_USE; + datamut.unlock(); + } + else { - backpack_status = (unsigned char) serial_buff[7]; + //do nothing } -#endif - - //add checksum verification - //... - // print data to screen if (screenEnable) { @@ -282,53 +209,93 @@ void BcbBattery::run() timeinfo = localtime(&rawtime); char* battery_timestamp = asctime(timeinfo); char buff[1024]; - sprintf(buff, "battery status: %+6.1fA % 6.1fV charge:% 6.1f%% time: %s", battery_current, battery_voltage, battery_charge, battery_timestamp); + snprintf(buff, 1024, "%6.1fV %+6.1fA, charge:%6.1f%%, time: %s", battery_voltage, battery_current, battery_charge, battery_timestamp); yDebug("BcbBattery::run() log_buffer is: %s", buff); } + + //flush the buffer +#ifndef DEBUG_TEST + if (iSerial) + { + iSerial->flush(); + } +#endif } bool BcbBattery::getBatteryVoltage(double &voltage) { - lock_guard lck(mtx); - voltage = battery_voltage; + if (!batteryReader) return false; + std::lock_guard lg(batteryReader->datamut); + voltage = batteryReader->battery_voltage; return true; } bool BcbBattery::getBatteryCurrent(double ¤t) { - lock_guard lck(mtx); - current = battery_current; + if (!batteryReader) return false; + std::lock_guard lg(batteryReader->datamut); + current = batteryReader->battery_current; return true; } bool BcbBattery::getBatteryCharge(double &charge) { - lock_guard lck(mtx); - charge = battery_charge; + if (!batteryReader) return false; + std::lock_guard lg(batteryReader->datamut); + charge = batteryReader->battery_charge; return true; } bool BcbBattery::getBatteryStatus(Battery_status &status) { - //The BCB battery indicator does not provide this info, so we simply return BATTERY_OK_IN_USE - status=Battery_status::BATTERY_OK_IN_USE; + if (!batteryReader) return false; + std::lock_guard lg(batteryReader->datamut); + status= batteryReader->battery_status; return true; } -bool BcbBattery::getBatteryTemperature(double &) +bool BcbBattery::getBatteryTemperature(double &temperature) { - //The BCB battery indicator does not provide this info, so we simply return true - return true; + //yError("Not yet implemented"); + temperature = std::nan(""); + return false; } bool BcbBattery::getBatteryInfo(string &info) { - lock_guard lck(mtx); - info = battery_info; + if (!batteryReader) return false; + std::lock_guard lg(batteryReader->datamut); + info = batteryReader->battery_info; return true; } -void BcbBattery::threadRelease() +void batteryReaderThread::threadRelease() +{ + stopTransmission(); +} + +void batteryReaderThread::startTransmission() { - yTrace("BcbBattery Thread released\n"); + if (!iSerial) return; + + //start the transmission + char cmd = 0x01; + bool ret = iSerial->send(&cmd, 1); + if (ret == false) + { + yError("BcbBattery problems starting the transmission"); + return; + } + + //empty the buffer + iSerial->flush(); +} + +void batteryReaderThread::stopTransmission() +{ + if (!iSerial) return; + + char c = 0x00; + bool ret = iSerial->send(&c, 1); + if (ret == false) { yError("BcbBattery problems while stopping the transmission"); } } diff --git a/src/libraries/icubmod/bcbBattery/bcbBattery.h b/src/libraries/icubmod/bcbBattery/bcbBattery.h index 25adf362ec..befbeaebf5 100644 --- a/src/libraries/icubmod/bcbBattery/bcbBattery.h +++ b/src/libraries/icubmod/bcbBattery/bcbBattery.h @@ -8,61 +8,87 @@ #define __BCBBATTERY_H__ #include -#include #include #include #include #include #include #include +#include using namespace yarp::os; using namespace yarp::dev; -class BcbBattery : public PeriodicThread, public yarp::dev::IBattery, public DeviceDriver +class batteryReaderThread : public PeriodicThread { -protected: - std::mutex mtx; + public: + //debug + static const int debugTextBufferSize = 10000; + char debugTextBuffer[debugTextBufferSize]; + + //configuration options + bool verboseEnable = false; + bool screenEnable = true; + //the buffer double timeStamp; - double battery_charge; - double battery_voltage; - double battery_current; - std::string battery_info; - unsigned char backpack_status; + static const int buff_len = 10000; + unsigned char tmp_buff[buff_len]; + static const int packet_len =10; + unsigned char packet[packet_len]; + std::regex r_exp; + + ISerialDevice* iSerial = nullptr; + std::mutex datamut; + double battery_charge = 0; + double battery_voltage = 0; + double battery_current = 0; + std::string battery_info = "icub battery system v1.0"; + int backpack_status = 0; + IBattery::Battery_status battery_status = IBattery::Battery_status::BATTERY_OK_STANBY; + + batteryReaderThread (ISerialDevice *_iSerial, double period) : + PeriodicThread((double)period), + iSerial(_iSerial) + { + // the following regedit expressions means: search for an occurrence of the pattern: + // \0........\r\n which is not followed by any other occurrence of the same pattern. + // See for example: https://docs.pexip.com/admin/regex_reference.htm + // + // 01234567 8 9***** 01234567 8 9 + std::string c_exp ("\\0.......\\r\\n(?!.*\\0.......\\r\\n)"); + r_exp=c_exp; + } - bool verboseEnable; - bool screenEnable; - bool silenceSyncWarnings; + void startTransmission(); + void stopTransmission(); + virtual bool threadInit() override; + virtual void threadRelease() override; + virtual void run() override; +}; + +class BcbBattery: public yarp::dev::IBattery, public DeviceDriver +{ +protected: + batteryReaderThread* batteryReader =nullptr; ResourceFinder rf; PolyDriver driver; - ISerialDevice *pSerial; - char serial_buff[255]; - - std::atomic isClosing; + ISerialDevice *iSerial = nullptr; public: - BcbBattery(int period = 20) : PeriodicThread((double)period/1000.0) - {} - - ~BcbBattery() - { - } + BcbBattery() {} + ~BcbBattery() {} virtual bool open(yarp::os::Searchable& config); virtual bool close(); - virtual bool getBatteryVoltage (double &voltage); - virtual bool getBatteryCurrent (double ¤t); - virtual bool getBatteryCharge (double &charge); - virtual bool getBatteryStatus (Battery_status &status); - virtual bool getBatteryInfo (std::string &info); - virtual bool getBatteryTemperature (double &temperature); - - virtual bool threadInit(); - virtual void threadRelease(); - virtual void run(); + virtual bool getBatteryVoltage (double &voltage) override; + virtual bool getBatteryCurrent (double ¤t) override; + virtual bool getBatteryCharge (double &charge) override; + virtual bool getBatteryStatus (Battery_status &status) override; + virtual bool getBatteryInfo (std::string &info) override; + virtual bool getBatteryTemperature (double &temperature) override; };