Program Listing for File PowerSensor.cc

↰ Return to documentation for file (host/src/PowerSensor.cc)

#include <fcntl.h>
#include <omp.h>
#include <termios.h>
#include <unistd.h>
#include <sys/file.h>

#include <chrono>
#include <iostream>
#include <cstring>

#include "PowerSensor.hpp"


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 secondState.timeAtRead - firstState.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(omp_get_wtime()) {
      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 retVal, bytesRead = 0;
      // 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::writeMarker() {
    std::unique_lock<std::mutex> lock(dumpFileMutex);
    *dumpFile << "M " << markers.front() << std::endl;
    markers.pop();
  }

  void PowerSensor::mark(const State &startState, const State &stopState, std::string name, unsigned int tag) const {
    if (dumpFile != nullptr) {
      std::unique_lock<std::mutex> lock(dumpFileMutex);
      *dumpFile << "M " << startState.timeAtRead - startTime << ' ' << stopState.timeAtRead - startTime << ' ' \
        << tag << " \"" << name << '"' << std::endl;
    }
  }

  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();

        if (dumpFile != nullptr) {
          if (marker != 0) {
            writeMarker();
            marker = 0;
          }
          dumpCurrentWattToFile();
        }
      }
    }
  }

  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() {
    if (thread != nullptr) {
      writeCharToDevice('X');
      thread->join();
      delete thread;
      thread = nullptr;
    }
  }

  void PowerSensor::dump(std::string dumpFileName) {
    std::unique_lock<std::mutex> lock(dumpFileMutex);
    dumpFile = std::unique_ptr<std::ofstream>(dumpFileName.empty() ? nullptr: new std::ofstream(dumpFileName));
    if (!dumpFileName.empty()) {
      *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() {
    std::unique_lock<std::mutex> lock(dumpFileMutex);
    double totalWatt = 0;
    double time = omp_get_wtime();
    static double previousTime = startTime;

    *dumpFile << "S " << time - startTime;
    *dumpFile << ' ' << static_cast<int>(1e6 * (time - previousTime));
    *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() {
    double now = omp_get_wtime();
    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 * (now - sensorPair.timeAtLastMeasurement);
        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 *
      (omp_get_wtime() - sensorPairs[pairID].timeAtLastMeasurement);

    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;
    }
    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