Program Listing for File PowerSensor.cc

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

#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