.. _program_listing_file_host_src_PowerSensor.cc:

Program Listing for File PowerSensor.cc
=======================================

|exhale_lsh| :ref:`Return to documentation for file <file_host_src_PowerSensor.cc>` (``host/src/PowerSensor.cc``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   #include <fcntl.h>
   #include <termios.h>
   #include <unistd.h>
   #include <sys/file.h>
   
   #include <chrono>
   #include <cstring>
   #include <iostream>
   #include <iomanip>
   
   #include "PowerSensor.hpp"
   
   namespace {
   double elapsedSeconds(const std::chrono::time_point<std::chrono::high_resolution_clock> &tstart,
                         const std::chrono::time_point<std::chrono::high_resolution_clock> &tend) {
       return (std::chrono::duration_cast<std::chrono::nanoseconds>(tend - tstart).count()) / 1e9;
       }
   }  // namespace
   
   
   namespace PowerSensor3 {
   void checkPairID(int pairID) {
     if (pairID >= (signed)MAX_PAIRS) {
       std::cerr << "Invalid pairID: " << pairID << ", maximum value is " << MAX_PAIRS - 1 << std::endl;
       exit(1);
     }
   }
   
   double Joules(const State &firstState, const State &secondState, int pairID) {
     checkPairID(pairID);
   
     if (pairID >= 0) {
       return secondState.consumedEnergy[pairID] - firstState.consumedEnergy[pairID];
     }
   
     double joules = 0;
     for (double consumedEnergy : secondState.consumedEnergy) {
       joules += consumedEnergy;
     }
     for (double consumedEnergy : firstState.consumedEnergy) {
       joules -= consumedEnergy;
     }
     return joules;
   }
   
   double seconds(const State &firstState, const State &secondState) {
     return elapsedSeconds(firstState.timeAtRead, secondState.timeAtRead);
   }
   
   double Watt(const State &firstState, const State &secondState, int pairID) {
     return Joules(firstState, secondState, pairID) / seconds(firstState, secondState);
   }
   
   PowerSensor::PowerSensor(std::string device):
     fd(openDevice(device)),
     pipe_fd(startCleanupProcess()),
     thread(nullptr),
     startTime(std::chrono::high_resolution_clock::now()) {
       readSensorsFromEEPROM();
       initializeSensorPairs();
       startIOThread();
     }
   
   PowerSensor::~PowerSensor() {
     stopIOThread();
     if (close(pipe_fd)) {
       perror("close child pipe fd");
     }
   
     if (close(fd)) {
       perror("close device");
     }
   }
   
   State PowerSensor::read() const {
     State state;
   
     std::unique_lock<std::mutex> lock(mutex);
   
     for (uint8_t pairID=0; pairID < MAX_PAIRS; pairID++) {
       state.consumedEnergy[pairID] = sensorPairs[pairID].consumedEnergy;
       state.current[pairID] = sensorPairs[pairID].currentAtLastMeasurement;
       state.voltage[pairID] = sensorPairs[pairID].voltageAtLastMeasurement;
       // Note: timeAtLastMeasurement is the same for each _active_ sensor pair
       if (sensorPairs[pairID].inUse) {
         state.timeAtRead = sensorPairs[pairID].timeAtLastMeasurement;
       }
     }
     return state;
   }
   
   int PowerSensor::openDevice(std::string device) {
     int fileDescriptor;
   
     // opens the file specified by pathname;
     if ((fileDescriptor = open(device.c_str(), O_RDWR)) < 0) {
       perror(device.c_str());
       exit(1);
     }
     // block if an incompatible lock is held by another process;
     if (flock(fileDescriptor, LOCK_EX) < 0) {
       perror("flock");
       exit(1);
     }
   
     // struct for configuring the port for communication with stm32;
     struct termios terminalOptions;
   
     // gets the current options for the port;
     tcgetattr(fileDescriptor, &terminalOptions);
   
     // set control mode flags;
     terminalOptions.c_cflag = (terminalOptions.c_cflag & ~CSIZE) | CS8;
     terminalOptions.c_cflag |= CLOCAL | CREAD;
     terminalOptions.c_cflag &= ~(PARENB | PARODD);
   
   
     // set input mode flags;
     terminalOptions.c_iflag |= IGNBRK;
     terminalOptions.c_iflag &= ~(IXON | IXOFF | IXANY);
   
     // clear local mode flag
     terminalOptions.c_lflag = 0;
   
     // clear output mode flag;
     terminalOptions.c_oflag = 0;
   
     // set control characters;
     terminalOptions.c_cc[VMIN] = 1;
     terminalOptions.c_cc[VTIME] = 0;
   
     // commit the options;
     tcsetattr(fileDescriptor, TCSANOW, &terminalOptions);
   
     // flush anything already in the serial buffer;
     tcflush(fileDescriptor, TCIFLUSH);
   
     return fileDescriptor;
   }
   
   inline char PowerSensor::readCharFromDevice() {
     ssize_t bytesRead;
     char buffer;
     do {
       if ((bytesRead = ::read(fd, &buffer, 1)) < 0) {
         perror("read");
         exit(1);
       }
     } while ((bytesRead) < 1);
     return buffer;
   }
   
   inline void PowerSensor::writeCharToDevice(char buffer) {
     if (write(fd, &buffer, 1) != 1) {
       perror("write device");
       exit(1);
     }
   }
   
   void PowerSensor::readSensorsFromEEPROM() {
     // signal device to send EEPROM data
     writeCharToDevice('R');
     // read data per sensor
     for (Sensor& sensor : sensors) {
       sensor.readFromEEPROM(fd);
       // trigger device to send next sensor
       // it does not matter what char is sent
       writeCharToDevice('s');
     }
     // when done, the device sends D
     char buffer;
     if ((buffer = readCharFromDevice()) != 'D') {
       std::cerr << "Expected to receive 'D' from device after reading configuration, but got " << buffer << std::endl;
       exit(1);
     }
   }
   
   void PowerSensor::writeSensorsToEEPROM() {
     // ensure no data is streaming to host
     stopIOThread();
     std::this_thread::sleep_for(std::chrono::milliseconds(10));
     // drain any remaining incoming data
     tcflush(fd, TCIFLUSH);
     // signal device to receive EEPROM data
     writeCharToDevice('W');
     // send EEPROM data
     // device sends S after each sensor and D when completely done
     char buffer;
     for (const Sensor& sensor : sensors) {
       sensor.writeToEEPROM(fd);
       if ((buffer = readCharFromDevice()) != 'S') {
         std::cerr << "Expected to receive S from device, but got " << buffer << std::endl;
         exit(1);
       }
     }
   
     if ((buffer = readCharFromDevice()) != 'D') {
       std::cerr << "Expected to receive 'D' from device after writing configuration, but got " << buffer << std::endl;
       exit(1);
     }
   
     // restart IO thread
     startIOThread();
   }
   
   void PowerSensor::initializeSensorPairs() {
     for (uint8_t pairID = 0; pairID < MAX_PAIRS; pairID++) {
       sensorPairs[pairID].timeAtLastMeasurement = startTime;
       sensorPairs[pairID].wattAtLastMeasurement = 0;
       sensorPairs[pairID].consumedEnergy = 0;
       sensorPairs[pairID].currentAtLastMeasurement = 0;
       sensorPairs[pairID].voltageAtLastMeasurement = 0;
   
   
       bool currentSensorActive = sensors[2*pairID].inUse;
       bool voltageSensorActive = sensors[2*pairID+1].inUse;
   
       if (currentSensorActive && voltageSensorActive) {
         sensorPairs[pairID].inUse = true;
       } else if (currentSensorActive ^ voltageSensorActive) {
         std::cerr << "Found incompatible sensor pair: current sensor (ID " << 2*pairID << ") "
         "is " << (currentSensorActive ? "" : "not ") << "active, while "
         "voltage sensor (ID " << 2*pairID+1 << ") is " << (voltageSensorActive ? "" : "not ") << "active. "
         "Please check sensor configuration." << std::endl;
       } else {
         sensorPairs[pairID].inUse = false;
       }
     }
   }
   
   bool PowerSensor::readLevelFromDevice(unsigned int* sensorNumber, uint16_t* level, unsigned int* marker) {
       // buffer for one set of sensor data (2 bytes)
       uint8_t buffer[2];
       unsigned int bytesRead = 0;
       int retVal;
       // loop exits when a valid value has been read from the device
       while (true) {
         // read full buffer
         do {
           if ((retVal = ::read(fd, reinterpret_cast<char*>(&buffer) + bytesRead, sizeof(buffer) - bytesRead)) < 0) {
             perror("read");
             exit(1);
           }
         } while ((bytesRead += retVal) < sizeof buffer);
   
         // buffer is full, check if stop was received
         if (buffer[0] == 0xFF && buffer[1] == 0x3F) {
           return false;
         } else if (((buffer[0] >> 7) == 1) & ((buffer[1] >> 7) == 0)) {
           // marker bits are ok, extract the values
           *sensorNumber = (buffer[0] >> 4) & 0x7;
           *level = ((buffer[0] & 0xF) << 6) | (buffer[1] & 0x3F);
           *marker |= (buffer[1] >> 6) & 0x1;
           return true;
         } else {
           // marker bits are wrong. Assume a byte was dropped: drop first byte and try again
           buffer[0] = buffer[1];
           bytesRead = 1;
         }
       }
     }
   
   void PowerSensor::mark(char name) {
     markers.push(name);
     writeCharToDevice('M');
   }
   
   void PowerSensor::waitForMarkers(int timeout) {
     auto tstart = std::chrono::high_resolution_clock::now();
     while (markers.size() != 0) {
       std::this_thread::sleep_for(std::chrono::milliseconds(10));
       auto elapsed = std::chrono::high_resolution_clock::now() - tstart;
       if (elapsed.count() > timeout) {
         // clear any remaining markers
         while (!markers.empty()) {
           markers.pop();
         }
         break;
       }
     }
   }
   
   void PowerSensor::IOThread() {
     unsigned int sensorNumber, marker = 0, sensorsRead = 0;
     uint16_t level;
   
     // wait until we can read values from the device
     readLevelFromDevice(&sensorNumber, &level, &marker);
     threadStarted.up();
   
     while (readLevelFromDevice(&sensorNumber, &level, &marker)) {
       std::unique_lock<std::mutex> lock(mutex);
   
       // detect timestamp packet, value is in microseconds
       // note that an actual marker can only set set for sensor zero in the device firmware
       if ((marker == 1) && (sensorNumber == (MAX_SENSORS-1))) {
         timestamp = level;
         marker = 0;
         continue;
       }
   
       sensors[sensorNumber].updateLevel(level);
       sensorsRead++;
   
       if (sensorsRead >= MAX_SENSORS) {
         sensorsRead = 0;
         updateSensorPairs();
   
         char markerChar = 'S';
         if (marker != 0) {
           markerChar = markers.front();
           markers.pop();
           marker = 0;
         }
         if (dumpFile != nullptr) {
           dumpCurrentWattToFile(markerChar);
         }
       }
     }
   }
   
   void PowerSensor::startIOThread() {
     if (thread == nullptr) {
       thread = new std::thread(&PowerSensor::IOThread, this);
     }
     writeCharToDevice('S');
     threadStarted.down();  // wait for the IOthread to run smoothly
   }
   
   void PowerSensor::stopIOThread() {
     // first ensure there are no markers still to be sent from the device
     waitForMarkers();
     if (thread != nullptr) {
       writeCharToDevice('X');
       thread->join();
       delete thread;
       thread = nullptr;
     }
   }
   
   void PowerSensor::dump(std::string dumpFileName, bool useAbsoluteTimestamp) {
     // if dumping to a new file or dumping should be stopped, first wait until all markers are written
     if (dumpFile != nullptr || dumpFileName.empty()) {
       waitForMarkers();
     }
   
     std::unique_lock<std::mutex> lock(dumpFileMutex);
     dumpFile = std::unique_ptr<std::ofstream>(dumpFileName.empty() ? nullptr: new std::ofstream(dumpFileName));
     if (!dumpFileName.empty()) {
       this->useAbsoluteTimestamp = useAbsoluteTimestamp;
       *dumpFile << "marker time dt_micro device_timestamp";
       for (unsigned int pairID=0; pairID < MAX_PAIRS; pairID++) {
         if (sensorPairs[pairID].inUse)
           *dumpFile << " current" << pairID << " voltage" << pairID << " power" << pairID;
       }
       *dumpFile << " power_total" << std::endl;
     }
   }
   
   void PowerSensor::dumpCurrentWattToFile(const char markerChar) {
     std::unique_lock<std::mutex> lock(dumpFileMutex);
     if (dumpFile == nullptr) {
       return;
     }
     double totalWatt = 0;
     auto time = std::chrono::high_resolution_clock::now();
     static auto previousTime = startTime;
   
     *dumpFile << markerChar << ' ';
     if (useAbsoluteTimestamp) {
       *dumpFile << std::fixed << std::setprecision(6) << \
           std::chrono::duration_cast<std::chrono::microseconds>(time.time_since_epoch()).count() / 1.0e6;
     } else {
       *dumpFile << elapsedSeconds(startTime, time);
     }
     *dumpFile << ' ' << static_cast<int>(1e6 * elapsedSeconds(previousTime, time));
     *dumpFile << ' ' << timestamp;
     previousTime = time;
   
     for (uint8_t pairID=0; pairID < MAX_PAIRS; pairID++) {
       if (sensorPairs[pairID].inUse) {
         totalWatt += sensorPairs[pairID].wattAtLastMeasurement;
         *dumpFile << ' ' << sensorPairs[pairID].currentAtLastMeasurement;
         *dumpFile << ' ' << sensorPairs[pairID].voltageAtLastMeasurement;
         *dumpFile << ' ' << sensorPairs[pairID].wattAtLastMeasurement;
       }
     }
     *dumpFile << ' ' << totalWatt << std::endl;
   }
   
   void PowerSensor::updateSensorPairs() {
     auto now = std::chrono::high_resolution_clock::now();
     for (unsigned int pairID=0; pairID < MAX_PAIRS; pairID++) {
       if (sensorPairs[pairID].inUse) {
         Sensor currentSensor = sensors[2*pairID];
         Sensor voltageSensor = sensors[2*pairID+1];
         SensorPair& sensorPair = sensorPairs[pairID];
   
         sensorPair.currentAtLastMeasurement = currentSensor.valueAtLastMeasurement;
         sensorPair.voltageAtLastMeasurement = voltageSensor.valueAtLastMeasurement;
         sensorPair.wattAtLastMeasurement = currentSensor.valueAtLastMeasurement * voltageSensor.valueAtLastMeasurement;
         sensorPair.consumedEnergy += sensorPair.wattAtLastMeasurement *
           elapsedSeconds(sensorPair.timeAtLastMeasurement, now);
         sensorPair.timeAtLastMeasurement = now;
       }
     }
   }
   
   int PowerSensor::startCleanupProcess() {
     int pipe_fds[2];
   
     if (pipe(pipe_fds) < 0) {
       perror("pipe");
       exit(1);
     }
   
     switch (fork()) {
       case -1:
         perror("fork");
         exit(1);
   
       case 0:
         // detach from the parent process, so signals to the parent are not caught by the child
         setsid();
   
         // close all file descriptors, except pipe read end and device fd
         for (int i = 3, last = getdtablesize(); i < last; i++) {
           if (i != fd && i != pipe_fds[0]) {
             close(i);
           }
         }
   
         // wait until parent closes pipe_fds[1] so that read fails
         char byte;
         ::read(pipe_fds[0], &byte, sizeof byte);
   
         // tell device to stop sending data
         writeCharToDevice('T');
   
         // drain garbage
         std::this_thread::sleep_for(std::chrono::milliseconds(100));
         tcflush(fd, TCIFLUSH);
   
         exit(0);
   
     default:
       return pipe_fds[1];
     }
   }
   
   double PowerSensor::totalEnergy(unsigned int pairID) const {
     double energy = sensorPairs[pairID].wattAtLastMeasurement *
       elapsedSeconds(sensorPairs[pairID].timeAtLastMeasurement, std::chrono::high_resolution_clock::now());
     return sensorPairs[pairID].consumedEnergy + energy;
   }
   
   void PowerSensor::reset(bool dfuMode) {
     stopIOThread();  // to avoid writing to device _after_ reset
     if (dfuMode) {
         writeCharToDevice('Y');
     } else {
         writeCharToDevice('Z');
     }
   }
   
   std::string PowerSensor::getVersion() {
     std::string version;
     stopIOThread();
     std::this_thread::sleep_for(std::chrono::milliseconds(10));
     // drain any remaining incoming data
     tcflush(fd, TCIFLUSH);
     writeCharToDevice('V');
     char c = readCharFromDevice();
     version += c;
     while (c != '\n') {
         c = readCharFromDevice();
         version += c;
     }
     // remove newline from version string
     version.pop_back();
     startIOThread();
     return version;
   }
   
   std::string PowerSensor::getType(unsigned int sensorID) const {
     return sensors[sensorID].type;
   }
   
   std::string PowerSensor::getPairName(unsigned int pairID) const {
     checkPairID(pairID);
     // warn if sensor pair names of the two sensors are not equal
     if (sensors[2 * pairID].pairName != sensors[2 * pairID + 1].pairName) {
       std::cerr << "Sensor pair names of pair " << pairID << " do not match, use psconfig to correct the values. "
         "Returning value of first sensor" << std::endl;
     }
     return sensors[2 * pairID].pairName;
   }
   
   float PowerSensor::getVref(unsigned int sensorID) const {
     return sensors[sensorID].vref;
   }
   
   float PowerSensor::getSensitivity(unsigned int sensorID) const {
     // negative sensitivity corresponds to inverted polarity
     // polarity is considered a separate variable instead so we return the abs value here
     return std::abs(sensors[sensorID].sensitivity);
   }
   
   bool PowerSensor::getInUse(unsigned int sensorID) const {
     return sensors[sensorID].inUse;
   }
   
   int PowerSensor::getPolarity(unsigned int sensorID) const {
     int polarity;
     if (sensors[sensorID].sensitivity >= 0) {
       polarity = 1;
     } else {
       polarity = -1;
     }
     return polarity;
   }
   
   void PowerSensor::setType(unsigned int sensorID, const std::string type) {
     sensors[sensorID].setType(type);
   }
   
   void PowerSensor::setPairName(unsigned int pairID, const std::string pairName) {
     checkPairID(pairID);
     // set name of both sensors of the pair
     sensors[2 * pairID].setPairName(pairName);
     sensors[2 * pairID + 1].setPairName(pairName);
   }
   
   void PowerSensor::setVref(unsigned int sensorID, const float vref) {
     sensors[sensorID].setVref(vref);
   }
   
   void PowerSensor::setSensitivity(unsigned int sensorID, const float sensitivity) {
     sensors[sensorID].setSensitivity(sensitivity);
   }
   
   void PowerSensor::setInUse(unsigned int sensorID, const bool inUse) {
     sensors[sensorID].setInUse(inUse);
   }
   
   void PowerSensor::setPolarity(unsigned int sensorID, const int polarity) {
     sensors[sensorID].setPolarity(polarity);
   }
   }  // namespace PowerSensor3