mlx90640.cpp

Go to the documentation of this file.

#include <mlx90640_thermal_camera/mlx90640.h>
 
MLX90640::MLX90640(int fps) {
  this->fps = fps;
  switch (this->fps) {
  case 1:
    this->setRefreshRate(MLX_I2C_ADDR, 0b001);
    break;
  case 2:
    this->setRefreshRate(MLX_I2C_ADDR, 0b010);
    break;
  case 4:
    this->setRefreshRate(MLX_I2C_ADDR, 0b011);
    break;
  case 8:
    this->setRefreshRate(MLX_I2C_ADDR, 0b100);
    break;
  case 16:
    this->setRefreshRate(MLX_I2C_ADDR, 0b101);
    break;
  case 32:
    this->setRefreshRate(MLX_I2C_ADDR, 0b110);
    break;
  case 64:
    this->setRefreshRate(MLX_I2C_ADDR, 0b111);
    break;
  default:
    ROS_ERROR("I2C unsupported frame rate!");
    exit(1);
  }
 
  this->setChessMode(MLX_I2C_ADDR);
  this->dumpEE(MLX_I2C_ADDR, this->eeMLX90640);
  this->extractParameters(this->eeMLX90640, &this->sensorParams);
}
 
void MLX90640::see() {
  this->getFrameData(MLX_I2C_ADDR, this->frame);
  this->eTa = this->getTa(frame, &this->sensorParams);
  this->calculateTo(frame, &this->sensorParams, this->emissivity, this->eTa,
                    this->mlx90640To);
  this->badPixelsCorrection((&this->sensorParams)->brokenPixels,
                            this->mlx90640To, 1, &this->sensorParams);
  this->badPixelsCorrection((&this->sensorParams)->outlierPixels,
                            this->mlx90640To, 1, &this->sensorParams);
  this->minTemp = 100.0;
  this->maxTemp = 0.0;
 
  for (int i = 0; i < 768; i++) {
    if (this->minTemp > this->mlx90640To[i]) {
      this->minTemp = this->mlx90640To[i];
    }
    if (this->maxTemp < this->mlx90640To[i]) {
      this->maxTemp = this->mlx90640To[i];
    }
  }
}
 
void MLX90640::copyRawImage(float *pointer) {
  for (int i = 0; i < 768; i++) {
    pointer[i] = this->mlx90640To[i];
  }
}
 
float MLX90640::getMin() { return this->minTemp; }
 
float MLX90640::getMax() { return this->maxTemp; }
 
int MLX90640::dumpEE(uint8_t slaveAddr, uint16_t *eeData) {
  return this->i2cDriver.read(slaveAddr, 0x2400, 832, eeData);
}
 
int MLX90640::synchFrame(uint8_t slaveAddr) {
  uint16_t dataReady = 0;
  uint16_t statusRegister;
  int error = 1;
 
  error = this->i2cDriver.write(slaveAddr, 0x8000, 0x0030);
  if (error == -1) {
    return error;
  }
 
  while (dataReady == 0) {
    error = this->i2cDriver.read(slaveAddr, 0x8000, 1, &statusRegister);
    if (error != 0) {
      return error;
    }
    dataReady = statusRegister & 0x0008;
  }
 
  return 0;
}
 
int MLX90640::triggerMeasurement(uint8_t slaveAddr) {
  int error = 1;
  uint16_t ctrlReg;
 
  error = this->i2cDriver.read(slaveAddr, 0x800D, 1, &ctrlReg);
  if (error != 0) {
    return error;
  }
 
  ctrlReg |= 0x8000;
  error = this->i2cDriver.write(slaveAddr, 0x800D, ctrlReg);
  if (error != 0) {
    return error;
  }
 
  error = this->i2cDriver.generalReset();
  if (error != 0) {
    return error;
  }
 
  error = this->i2cDriver.read(slaveAddr, 0x800D, 1, &ctrlReg);
  if (error != 0) {
    return error;
  }
  if ((ctrlReg & 0x8000) != 0) {
    return -9;
  }
 
  return 0;
}
 
int MLX90640::getFrameData(uint8_t slaveAddr, uint16_t *frameData) {
  uint16_t dataReady = 0;
  uint16_t controlRegister1;
  uint16_t statusRegister;
  int error = 1;
  uint16_t data[64];
  uint8_t cnt = 0;
 
  while (dataReady == 0) {
    error = this->i2cDriver.read(slaveAddr, 0x8000, 1, &statusRegister);
    if (error != 0) {
      return error;
    }
    dataReady = statusRegister & 0x0008;
  }
 
  error = this->i2cDriver.write(slaveAddr, 0x8000, 0x0030);
  if (error == -1) {
    return error;
  }
 
  error = this->i2cDriver.read(slaveAddr, 0x0400, 768, frameData);
  if (error != 0) {
    return error;
  }
 
  error = this->i2cDriver.read(slaveAddr, 0x0700, 64, data);
  if (error != 0) {
    return error;
  }
 
  error = this->i2cDriver.read(slaveAddr, 0x800D, 1, &controlRegister1);
  frameData[832] = controlRegister1;
  frameData[833] = statusRegister & 0x0001;
  if (error != 0) {
    return error;
  }
 
  error = this->validateAuxData(data);
  if (error == 0) {
    for (cnt = 0; cnt < 64; cnt++) {
      frameData[cnt + 768] = data[cnt];
    }
  }
 
  error = this->validateFrameData(frameData);
  if (error != 0) {
    return error;
  }
 
  return frameData[833];
}
 
int MLX90640::extractParameters(uint16_t *eeData, thermalParams *mlx90640) {
  int error = 0;
 
  this->extractVDDParameters(eeData, mlx90640);
  this->extractPTATParameters(eeData, mlx90640);
  this->extractGainParameters(eeData, mlx90640);
  this->extractTgcParameters(eeData, mlx90640);
  this->extractResolutionParameters(eeData, mlx90640);
  this->extractKsTaParameters(eeData, mlx90640);
  this->extractKsToParameters(eeData, mlx90640);
  this->extractCPParameters(eeData, mlx90640);
  this->extractAlphaParameters(eeData, mlx90640);
  this->extractOffsetParameters(eeData, mlx90640);
  this->extractKtaPixelParameters(eeData, mlx90640);
  this->extractKvPixelParameters(eeData, mlx90640);
  this->extractCILCParameters(eeData, mlx90640);
  error = this->extractDeviatingPixels(eeData, mlx90640);
 
  return error;
}
 
float MLX90640::getVdd(uint16_t *frameData, const thermalParams *params) {
  float vdd;
  float resolutionCorrection;
 
  int resolutionRAM;
 
  vdd = frameData[810];
  if (vdd > 32767) {
    vdd = vdd - 65536;
  }
  resolutionRAM = (frameData[832] & 0x0C00) >> 10;
  resolutionCorrection =
      pow(2, (double)params->resolutionEE) / pow(2, (double)resolutionRAM);
  vdd = (resolutionCorrection * vdd - params->vdd25) / params->kVdd + 3.3;
 
  return vdd;
}
 
float MLX90640::getTa(uint16_t *frameData, const thermalParams *params) {
  float ptat;
  float ptatArt;
  float vdd;
  float ta;
 
  vdd = this->getVdd(frameData, params);
  ptat = frameData[800];
  if (ptat > 32767) {
    ptat = ptat - 65536;
  }
 
  ptatArt = frameData[768];
  if (ptatArt > 32767) {
    ptatArt = ptatArt - 65536;
  }
  ptatArt = (ptat / (ptat * params->alphaPTAT + ptatArt)) * pow(2, (double)18);
 
  ta = (ptatArt / (1 + params->KvPTAT * (vdd - 3.3)) - params->vPTAT25);
  ta = ta / params->KtPTAT + 25;
 
  return ta;
}
 
void MLX90640::getImage(uint16_t *frameData, const thermalParams *params,
                        float *result) {
  float vdd;
  float ta;
  float gain;
  float irDataCP[2];
  float irData;
  float alphaCompensated;
  uint8_t mode;
  int8_t ilPattern;
  int8_t chessPattern;
  int8_t pattern;
  int8_t conversionPattern;
  float image;
  uint16_t subPage;
  float ktaScale;
  float kvScale;
  float kta;
  float kv;
 
  subPage = frameData[833];
  vdd = this->getVdd(frameData, params);
  ta = this->getTa(frameData, params);
 
  ktaScale = pow(2, (double)params->ktaScale);
  kvScale = pow(2, (double)params->kvScale);
 
  gain = frameData[778];
  if (gain > 32767) {
    gain = gain - 65536;
  }
  gain = params->gainEE / gain;
 
  mode = (frameData[832] & 0x1000) >> 5;
 
  irDataCP[0] = frameData[776];
  irDataCP[1] = frameData[808];
  for (int i = 0; i < 2; i++) {
    if (irDataCP[i] > 32767) {
      irDataCP[i] = irDataCP[i] - 65536;
    }
    irDataCP[i] = irDataCP[i] * gain;
  }
  irDataCP[0] = irDataCP[0] - params->cpOffset[0] *
                                  (1 + params->cpKta * (ta - 25)) *
                                  (1 + params->cpKv * (vdd - 3.3));
  if (mode == params->calibrationModeEE) {
    irDataCP[1] = irDataCP[1] - params->cpOffset[1] *
                                    (1 + params->cpKta * (ta - 25)) *
                                    (1 + params->cpKv * (vdd - 3.3));
  } else {
    irDataCP[1] = irDataCP[1] - (params->cpOffset[1] + params->ilChessC[0]) *
                                    (1 + params->cpKta * (ta - 25)) *
                                    (1 + params->cpKv * (vdd - 3.3));
  }
 
  for (int pixelNumber = 0; pixelNumber < 768; pixelNumber++) {
    ilPattern = pixelNumber / 32 - (pixelNumber / 64) * 2;
    chessPattern = ilPattern ^ (pixelNumber - (pixelNumber / 2) * 2);
    conversionPattern = ((pixelNumber + 2) / 4 - (pixelNumber + 3) / 4 +
                         (pixelNumber + 1) / 4 - pixelNumber / 4) *
                        (1 - 2 * ilPattern);
 
    if (mode == 0) {
      pattern = ilPattern;
    } else {
      pattern = chessPattern;
    }
 
    if (pattern == frameData[833]) {
      irData = frameData[pixelNumber];
      if (irData > 32767) {
        irData = irData - 65536;
      }
      irData = irData * gain;
 
      kta = params->kta[pixelNumber] / ktaScale;
      kv = params->kv[pixelNumber] / kvScale;
      irData = irData - params->offset[pixelNumber] * (1 + kta * (ta - 25)) *
                            (1 + kv * (vdd - 3.3));
 
      if (mode != params->calibrationModeEE) {
        irData = irData + params->ilChessC[2] * (2 * ilPattern - 1) -
                 params->ilChessC[1] * conversionPattern;
      }
 
      irData = irData - params->tgc * irDataCP[subPage];
 
      alphaCompensated = params->alpha[pixelNumber];
 
      image = irData * alphaCompensated;
 
      result[pixelNumber] = image;
    }
  }
}
 
void MLX90640::calculateTo(uint16_t *frameData, const thermalParams *params,
                           float emissivity, float tr, float *result) {
  float vdd;
  float ta;
  float ta4;
  float tr4;
  float taTr;
  float gain;
  float irDataCP[2];
  float irData;
  float alphaCompensated;
  uint8_t mode;
  int8_t ilPattern;
  int8_t chessPattern;
  int8_t pattern;
  int8_t conversionPattern;
  float Sx;
  float To;
  float alphaCorrR[4];
  int8_t range;
  uint16_t subPage;
  float ktaScale;
  float kvScale;
  float alphaScale;
  float kta;
  float kv;
 
  subPage = frameData[833];
  vdd = this->getVdd(frameData, params);
  ta = this->getTa(frameData, params);
 
  ta4 = (ta + 273.15);
  ta4 = ta4 * ta4;
  ta4 = ta4 * ta4;
  tr4 = (tr + 273.15);
  tr4 = tr4 * tr4;
  tr4 = tr4 * tr4;
  taTr = tr4 - (tr4 - ta4) / emissivity;
 
  ktaScale = pow(2, (double)params->ktaScale);
  kvScale = pow(2, (double)params->kvScale);
  alphaScale = pow(2, (double)params->alphaScale);
 
  alphaCorrR[0] = 1 / (1 + params->ksTo[0] * 40);
  alphaCorrR[1] = 1;
  alphaCorrR[2] = (1 + params->ksTo[1] * params->ct[2]);
  alphaCorrR[3] =
      alphaCorrR[2] * (1 + params->ksTo[2] * (params->ct[3] - params->ct[2]));
 
  gain = frameData[778];
  if (gain > 32767) {
    gain = gain - 65536;
  }
  gain = params->gainEE / gain;
 
  mode = (frameData[832] & 0x1000) >> 5;
 
  irDataCP[0] = frameData[776];
  irDataCP[1] = frameData[808];
  for (int i = 0; i < 2; i++) {
    if (irDataCP[i] > 32767) {
      irDataCP[i] = irDataCP[i] - 65536;
    }
    irDataCP[i] = irDataCP[i] * gain;
  }
  irDataCP[0] = irDataCP[0] - params->cpOffset[0] *
                                  (1 + params->cpKta * (ta - 25)) *
                                  (1 + params->cpKv * (vdd - 3.3));
  if (mode == params->calibrationModeEE) {
    irDataCP[1] = irDataCP[1] - params->cpOffset[1] *
                                    (1 + params->cpKta * (ta - 25)) *
                                    (1 + params->cpKv * (vdd - 3.3));
  } else {
    irDataCP[1] = irDataCP[1] - (params->cpOffset[1] + params->ilChessC[0]) *
                                    (1 + params->cpKta * (ta - 25)) *
                                    (1 + params->cpKv * (vdd - 3.3));
  }
 
  for (int pixelNumber = 0; pixelNumber < 768; pixelNumber++) {
    ilPattern = pixelNumber / 32 - (pixelNumber / 64) * 2;
    chessPattern = ilPattern ^ (pixelNumber - (pixelNumber / 2) * 2);
    conversionPattern = ((pixelNumber + 2) / 4 - (pixelNumber + 3) / 4 +
                         (pixelNumber + 1) / 4 - pixelNumber / 4) *
                        (1 - 2 * ilPattern);
 
    if (mode == 0) {
      pattern = ilPattern;
    } else {
      pattern = chessPattern;
    }
 
    if (pattern == frameData[833]) {
      irData = frameData[pixelNumber];
      if (irData > 32767) {
        irData = irData - 65536;
      }
      irData = irData * gain;
 
      kta = params->kta[pixelNumber] / ktaScale;
      kv = params->kv[pixelNumber] / kvScale;
      irData = irData - params->offset[pixelNumber] * (1 + kta * (ta - 25)) *
                            (1 + kv * (vdd - 3.3));
 
      if (mode != params->calibrationModeEE) {
        irData = irData + params->ilChessC[2] * (2 * ilPattern - 1) -
                 params->ilChessC[1] * conversionPattern;
      }
 
      irData = irData - params->tgc * irDataCP[subPage];
      irData = irData / emissivity;
 
      alphaCompensated = SCALEALPHA * alphaScale / params->alpha[pixelNumber];
      alphaCompensated = alphaCompensated * (1 + params->KsTa * (ta - 25));
 
      Sx = alphaCompensated * alphaCompensated * alphaCompensated *
           (irData + alphaCompensated * taTr);
      Sx = sqrt(sqrt(Sx)) * params->ksTo[1];
 
      To = sqrt(sqrt(
               irData /
                   (alphaCompensated * (1 - params->ksTo[1] * 273.15) + Sx) +
               taTr)) -
           273.15;
 
      if (To < params->ct[1]) {
        range = 0;
      } else if (To < params->ct[2]) {
        range = 1;
      } else if (To < params->ct[3]) {
        range = 2;
      } else {
        range = 3;
      }
 
      To = sqrt(sqrt(
               irData / (alphaCompensated * alphaCorrR[range] *
                         (1 + params->ksTo[range] * (To - params->ct[range]))) +
               taTr)) -
           273.15;
      result[pixelNumber] = To;
    }
  }
}
 
int MLX90640::setResolution(uint8_t slaveAddr, uint8_t resolution) {
  uint16_t controlRegister1;
  int value;
  int error;
 
  value = (resolution & 0x03) << 10;
  error = this->i2cDriver.read(slaveAddr, 0x800D, 1, &controlRegister1);
  if (error == 0) {
    value = (controlRegister1 & 0xF3FF) | value;
    error = this->i2cDriver.write(slaveAddr, 0x800D, value);
  }
 
  return error;
}
 
int MLX90640::getCurResolution(uint8_t slaveAddr) {
  uint16_t controlRegister1;
  int resolutionRAM;
  int error;
 
  error = this->i2cDriver.read(slaveAddr, 0x800D, 1, &controlRegister1);
  if (error != 0) {
    return error;
  }
  resolutionRAM = (controlRegister1 & 0x0C00) >> 10;
 
  return resolutionRAM;
}
 
int MLX90640::setRefreshRate(uint8_t slaveAddr, uint8_t refreshRate) {
  uint16_t controlRegister1;
  int value;
  int error;
 
  value = (refreshRate & 0x07) << 7;
  if (error == 0) {
    value = (controlRegister1 & 0xFC7F) | value;
    error = this->i2cDriver.write(slaveAddr, 0x800D, value);
  }
 
  return error;
}
 
int MLX90640::getRefreshRate(uint8_t slaveAddr) {
  uint16_t controlRegister1;
  int refreshRate;
  int error;
 
  error = this->i2cDriver.read(slaveAddr, 0x800D, 1, &controlRegister1);
  if (error != 0) {
    return error;
  }
  refreshRate = (controlRegister1 & 0x0380) >> 7;
 
  return refreshRate;
}
 
int MLX90640::getSubPageNumber(uint16_t *frameData) { return frameData[833]; }
 
int MLX90640::getCurMode(uint8_t slaveAddr) {
  uint16_t controlRegister1;
  int modeRAM;
  int error;
 
  error = this->i2cDriver.read(slaveAddr, 0x800D, 1, &controlRegister1);
  if (error != 0) {
    return error;
  }
  modeRAM = (controlRegister1 & 0x1000) >> 12;
 
  return modeRAM;
}
 
int MLX90640::setInterleavedMode(uint8_t slaveAddr) {
  uint16_t controlRegister1;
  int value;
  int error;
 
  error = this->i2cDriver.read(slaveAddr, 0x800D, 1, &controlRegister1);
  if (error == 0) {
    value = (controlRegister1 & 0xEFFF);
    error = this->i2cDriver.write(slaveAddr, 0x800D, value);
  }
 
  return error;
}
 
int MLX90640::setChessMode(uint8_t slaveAddr) {
  uint16_t controlRegister1;
  int value;
  int error;
 
  error = this->i2cDriver.read(slaveAddr, 0x800D, 1, &controlRegister1);
  if (error == 0) {
    value = (controlRegister1 | 0x1000);
    error = this->i2cDriver.write(slaveAddr, 0x800D, value);
  }
 
  return error;
}
 
void MLX90640::badPixelsCorrection(uint16_t *pixels, float *to, int mode,
                                   thermalParams *params) {
  float ap[4];
  uint8_t pix;
  uint8_t line;
  uint8_t column;
 
  pix = 0;
  while (pixels[pix] != 0xFFFF) {
    line = pixels[pix] >> 5;
    column = pixels[pix] - (line << 5);
 
    if (mode == 1) {
      if (line == 0) {
        if (column == 0) {
          to[pixels[pix]] = to[33];
        } else if (column == 31) {
          to[pixels[pix]] = to[62];
        } else {
          to[pixels[pix]] = (to[pixels[pix] + 31] + to[pixels[pix] + 33]) / 2.0;
        }
      } else if (line == 23) {
        if (column == 0) {
          to[pixels[pix]] = to[705];
        } else if (column == 31) {
          to[pixels[pix]] = to[734];
        } else {
          to[pixels[pix]] = (to[pixels[pix] - 33] + to[pixels[pix] - 31]) / 2.0;
        }
      } else if (column == 0) {
        to[pixels[pix]] = (to[pixels[pix] - 31] + to[pixels[pix] + 33]) / 2.0;
      } else if (column == 31) {
        to[pixels[pix]] = (to[pixels[pix] - 33] + to[pixels[pix] + 31]) / 2.0;
      } else {
        ap[0] = to[pixels[pix] - 33];
        ap[1] = to[pixels[pix] - 31];
        ap[2] = to[pixels[pix] + 31];
        ap[3] = to[pixels[pix] + 33];
        to[pixels[pix]] = this->getMedian(ap, 4);
      }
    } else {
      if (column == 0) {
        to[pixels[pix]] = to[pixels[pix] + 1];
      } else if (column == 1 || column == 30) {
        to[pixels[pix]] = (to[pixels[pix] - 1] + to[pixels[pix] + 1]) / 2.0;
      } else if (column == 31) {
        to[pixels[pix]] = to[pixels[pix] - 1];
      } else {
        if (this->isPixelBad(pixels[pix] - 2, params) == 0 &&
            this->isPixelBad(pixels[pix] + 2, params) == 0) {
          ap[0] = to[pixels[pix] + 1] - to[pixels[pix] + 2];
          ap[1] = to[pixels[pix] - 1] - to[pixels[pix] - 2];
          if (fabs(ap[0]) > fabs(ap[1])) {
            to[pixels[pix]] = to[pixels[pix] - 1] + ap[1];
          } else {
            to[pixels[pix]] = to[pixels[pix] + 1] + ap[0];
          }
        } else {
          to[pixels[pix]] = (to[pixels[pix] - 1] + to[pixels[pix] + 1]) / 2.0;
        }
      }
    }
    pix = pix + 1;
  }
}
 
void MLX90640::extractVDDParameters(uint16_t *eeData, thermalParams *mlx90640) {
  int16_t kVdd;
  int16_t vdd25;
 
  kVdd = eeData[51];
 
  kVdd = (eeData[51] & 0xFF00) >> 8;
  if (kVdd > 127) {
    kVdd = kVdd - 256;
  }
  kVdd = 32 * kVdd;
  vdd25 = eeData[51] & 0x00FF;
  vdd25 = ((vdd25 - 256) << 5) - 8192;
 
  mlx90640->kVdd = kVdd;
  mlx90640->vdd25 = vdd25;
}
 
void MLX90640::extractPTATParameters(uint16_t *eeData,
                                     thermalParams *mlx90640) {
  float KvPTAT;
  float KtPTAT;
  int16_t vPTAT25;
  float alphaPTAT;
 
  KvPTAT = (eeData[50] & 0xFC00) >> 10;
  if (KvPTAT > 31) {
    KvPTAT = KvPTAT - 64;
  }
  KvPTAT = KvPTAT / 4096;
 
  KtPTAT = eeData[50] & 0x03FF;
  if (KtPTAT > 511) {
    KtPTAT = KtPTAT - 1024;
  }
  KtPTAT = KtPTAT / 8;
 
  vPTAT25 = eeData[49];
 
  alphaPTAT = (eeData[16] & 0xF000) / pow(2, (double)14) + 8.0f;
 
  mlx90640->KvPTAT = KvPTAT;
  mlx90640->KtPTAT = KtPTAT;
  mlx90640->vPTAT25 = vPTAT25;
  mlx90640->alphaPTAT = alphaPTAT;
}
 
void MLX90640::extractGainParameters(uint16_t *eeData,
                                     thermalParams *mlx90640) {
  int16_t gainEE;
 
  gainEE = eeData[48];
  if (gainEE > 32767) {
    gainEE = gainEE - 65536;
  }
 
  mlx90640->gainEE = gainEE;
}
 
void MLX90640::extractTgcParameters(uint16_t *eeData, thermalParams *mlx90640) {
  float tgc;
  tgc = eeData[60] & 0x00FF;
  if (tgc > 127) {
    tgc = tgc - 256;
  }
  tgc = tgc / 32.0f;
 
  mlx90640->tgc = tgc;
}
 
void MLX90640::extractResolutionParameters(uint16_t *eeData,
                                           thermalParams *mlx90640) {
  uint8_t resolutionEE;
  resolutionEE = (eeData[56] & 0x3000) >> 12;
 
  mlx90640->resolutionEE = resolutionEE;
}
 
void MLX90640::extractKsTaParameters(uint16_t *eeData,
                                     thermalParams *mlx90640) {
  float KsTa;
  KsTa = (eeData[60] & 0xFF00) >> 8;
  if (KsTa > 127) {
    KsTa = KsTa - 256;
  }
  KsTa = KsTa / 8192.0f;
 
  mlx90640->KsTa = KsTa;
}
 
void MLX90640::extractKsToParameters(uint16_t *eeData,
                                     thermalParams *mlx90640) {
  int32_t KsToScale;
  int8_t step;
 
  step = ((eeData[63] & 0x3000) >> 12) * 10;
 
  mlx90640->ct[0] = -40;
  mlx90640->ct[1] = 0;
  mlx90640->ct[2] = (eeData[63] & 0x00F0) >> 4;
  mlx90640->ct[3] = (eeData[63] & 0x0F00) >> 8;
 
  mlx90640->ct[2] = mlx90640->ct[2] * step;
  mlx90640->ct[3] = mlx90640->ct[2] + mlx90640->ct[3] * step;
  mlx90640->ct[4] = 400;
 
  KsToScale = (eeData[63] & 0x000F) + 8;
  KsToScale = 1 << KsToScale;
 
  mlx90640->ksTo[0] = eeData[61] & 0x00FF;
  mlx90640->ksTo[1] = (eeData[61] & 0xFF00) >> 8;
  mlx90640->ksTo[2] = eeData[62] & 0x00FF;
  mlx90640->ksTo[3] = (eeData[62] & 0xFF00) >> 8;
 
  for (int i = 0; i < 4; i++) {
    if (mlx90640->ksTo[i] > 127) {
      mlx90640->ksTo[i] = mlx90640->ksTo[i] - 256;
    }
    mlx90640->ksTo[i] = mlx90640->ksTo[i] / KsToScale;
  }
 
  mlx90640->ksTo[4] = -0.0002;
}
 
void MLX90640::extractAlphaParameters(uint16_t *eeData,
                                      thermalParams *mlx90640) {
  int accRow[24];
  int accColumn[32];
  int p = 0;
  int alphaRef;
  uint8_t alphaScale;
  uint8_t accRowScale;
  uint8_t accColumnScale;
  uint8_t accRemScale;
  float alphaTemp[768];
  float temp;
 
  accRemScale = eeData[32] & 0x000F;
  accColumnScale = (eeData[32] & 0x00F0) >> 4;
  accRowScale = (eeData[32] & 0x0F00) >> 8;
  alphaScale = ((eeData[32] & 0xF000) >> 12) + 30;
  alphaRef = eeData[33];
 
  for (int i = 0; i < 6; i++) {
    p = i * 4;
    accRow[p + 0] = (eeData[34 + i] & 0x000F);
    accRow[p + 1] = (eeData[34 + i] & 0x00F0) >> 4;
    accRow[p + 2] = (eeData[34 + i] & 0x0F00) >> 8;
    accRow[p + 3] = (eeData[34 + i] & 0xF000) >> 12;
  }
 
  for (int i = 0; i < 24; i++) {
    if (accRow[i] > 7) {
      accRow[i] = accRow[i] - 16;
    }
  }
 
  for (int i = 0; i < 8; i++) {
    p = i * 4;
    accColumn[p + 0] = (eeData[40 + i] & 0x000F);
    accColumn[p + 1] = (eeData[40 + i] & 0x00F0) >> 4;
    accColumn[p + 2] = (eeData[40 + i] & 0x0F00) >> 8;
    accColumn[p + 3] = (eeData[40 + i] & 0xF000) >> 12;
  }
 
  for (int i = 0; i < 32; i++) {
    if (accColumn[i] > 7) {
      accColumn[i] = accColumn[i] - 16;
    }
  }
 
  for (int i = 0; i < 24; i++) {
    for (int j = 0; j < 32; j++) {
      p = 32 * i + j;
      alphaTemp[p] = (eeData[64 + p] & 0x03F0) >> 4;
      if (alphaTemp[p] > 31) {
        alphaTemp[p] = alphaTemp[p] - 64;
      }
      alphaTemp[p] = alphaTemp[p] * (1 << accRemScale);
      alphaTemp[p] = (alphaRef + (accRow[i] << accRowScale) +
                      (accColumn[j] << accColumnScale) + alphaTemp[p]);
      alphaTemp[p] = alphaTemp[p] / pow(2, (double)alphaScale);
      alphaTemp[p] =
          alphaTemp[p] -
          mlx90640->tgc * (mlx90640->cpAlpha[0] + mlx90640->cpAlpha[1]) / 2;
      alphaTemp[p] = SCALEALPHA / alphaTemp[p];
    }
  }
 
  temp = alphaTemp[0];
  for (int i = 1; i < 768; i++) {
    if (alphaTemp[i] > temp) {
      temp = alphaTemp[i];
    }
  }
 
  alphaScale = 0;
  while (temp < 32767.4) {
    temp = temp * 2;
    alphaScale = alphaScale + 1;
  }
 
  for (int i = 0; i < 768; i++) {
    temp = alphaTemp[i] * pow(2, (double)alphaScale);
    mlx90640->alpha[i] = (temp + 0.5);
  }
 
  mlx90640->alphaScale = alphaScale;
}
 
void MLX90640::extractOffsetParameters(uint16_t *eeData,
                                       thermalParams *mlx90640) {
  int occRow[24];
  int occColumn[32];
  int p = 0;
  int16_t offsetRef;
  uint8_t occRowScale;
  uint8_t occColumnScale;
  uint8_t occRemScale;
 
  occRemScale = (eeData[16] & 0x000F);
  occColumnScale = (eeData[16] & 0x00F0) >> 4;
  occRowScale = (eeData[16] & 0x0F00) >> 8;
  offsetRef = eeData[17];
  if (offsetRef > 32767) {
    offsetRef = offsetRef - 65536;
  }
 
  for (int i = 0; i < 6; i++) {
    p = i * 4;
    occRow[p + 0] = (eeData[18 + i] & 0x000F);
    occRow[p + 1] = (eeData[18 + i] & 0x00F0) >> 4;
    occRow[p + 2] = (eeData[18 + i] & 0x0F00) >> 8;
    occRow[p + 3] = (eeData[18 + i] & 0xF000) >> 12;
  }
 
  for (int i = 0; i < 24; i++) {
    if (occRow[i] > 7) {
      occRow[i] = occRow[i] - 16;
    }
  }
 
  for (int i = 0; i < 8; i++) {
    p = i * 4;
    occColumn[p + 0] = (eeData[24 + i] & 0x000F);
    occColumn[p + 1] = (eeData[24 + i] & 0x00F0) >> 4;
    occColumn[p + 2] = (eeData[24 + i] & 0x0F00) >> 8;
    occColumn[p + 3] = (eeData[24 + i] & 0xF000) >> 12;
  }
 
  for (int i = 0; i < 32; i++) {
    if (occColumn[i] > 7) {
      occColumn[i] = occColumn[i] - 16;
    }
  }
 
  for (int i = 0; i < 24; i++) {
    for (int j = 0; j < 32; j++) {
      p = 32 * i + j;
      mlx90640->offset[p] = (eeData[64 + p] & 0xFC00) >> 10;
      if (mlx90640->offset[p] > 31) {
        mlx90640->offset[p] = mlx90640->offset[p] - 64;
      }
      mlx90640->offset[p] = mlx90640->offset[p] * (1 << occRemScale);
      mlx90640->offset[p] =
          (offsetRef + (occRow[i] << occRowScale) +
           (occColumn[j] << occColumnScale) + mlx90640->offset[p]);
    }
  }
}
 
void MLX90640::extractKtaPixelParameters(uint16_t *eeData,
                                         thermalParams *mlx90640) {
  int p = 0;
  int8_t KtaRC[4];
  int8_t KtaRoCo;
  int8_t KtaRoCe;
  int8_t KtaReCo;
  int8_t KtaReCe;
  uint8_t ktaScale1;
  uint8_t ktaScale2;
  uint8_t split;
  float ktaTemp[768];
  float temp;
 
  KtaRoCo = (eeData[54] & 0xFF00) >> 8;
  if (KtaRoCo > 127) {
    KtaRoCo = KtaRoCo - 256;
  }
  KtaRC[0] = KtaRoCo;
 
  KtaReCo = (eeData[54] & 0x00FF);
  if (KtaReCo > 127) {
    KtaReCo = KtaReCo - 256;
  }
  KtaRC[2] = KtaReCo;
 
  KtaRoCe = (eeData[55] & 0xFF00) >> 8;
  if (KtaRoCe > 127) {
    KtaRoCe = KtaRoCe - 256;
  }
  KtaRC[1] = KtaRoCe;
 
  KtaReCe = (eeData[55] & 0x00FF);
  if (KtaReCe > 127) {
    KtaReCe = KtaReCe - 256;
  }
  KtaRC[3] = KtaReCe;
 
  ktaScale1 = ((eeData[56] & 0x00F0) >> 4) + 8;
  ktaScale2 = (eeData[56] & 0x000F);
 
  for (int i = 0; i < 24; i++) {
    for (int j = 0; j < 32; j++) {
      p = 32 * i + j;
      split = 2 * (p / 32 - (p / 64) * 2) + p % 2;
      ktaTemp[p] = (eeData[64 + p] & 0x000E) >> 1;
      if (ktaTemp[p] > 3) {
        ktaTemp[p] = ktaTemp[p] - 8;
      }
      ktaTemp[p] = ktaTemp[p] * (1 << ktaScale2);
      ktaTemp[p] = KtaRC[split] + ktaTemp[p];
      ktaTemp[p] = ktaTemp[p] / pow(2, (double)ktaScale1);
      // ktaTemp[p] = ktaTemp[p] * mlx90640->offset[p];
    }
  }
 
  temp = fabs(ktaTemp[0]);
  for (int i = 1; i < 768; i++) {
    if (fabs(ktaTemp[i]) > temp) {
      temp = fabs(ktaTemp[i]);
    }
  }
 
  ktaScale1 = 0;
  while (temp < 63.4) {
    temp = temp * 2;
    ktaScale1 = ktaScale1 + 1;
  }
 
  for (int i = 0; i < 768; i++) {
    temp = ktaTemp[i] * pow(2, (double)ktaScale1);
    if (temp < 0) {
      mlx90640->kta[i] = (temp - 0.5);
    } else {
      mlx90640->kta[i] = (temp + 0.5);
    }
  }
 
  mlx90640->ktaScale = ktaScale1;
}
 
void MLX90640::extractKvPixelParameters(uint16_t *eeData,
                                        thermalParams *mlx90640) {
  int p = 0;
  int8_t KvT[4];
  int8_t KvRoCo;
  int8_t KvRoCe;
  int8_t KvReCo;
  int8_t KvReCe;
  uint8_t kvScale;
  uint8_t split;
  float kvTemp[768];
  float temp;
 
  KvRoCo = (eeData[52] & 0xF000) >> 12;
  if (KvRoCo > 7) {
    KvRoCo = KvRoCo - 16;
  }
  KvT[0] = KvRoCo;
 
  KvReCo = (eeData[52] & 0x0F00) >> 8;
  if (KvReCo > 7) {
    KvReCo = KvReCo - 16;
  }
  KvT[2] = KvReCo;
 
  KvRoCe = (eeData[52] & 0x00F0) >> 4;
  if (KvRoCe > 7) {
    KvRoCe = KvRoCe - 16;
  }
  KvT[1] = KvRoCe;
 
  KvReCe = (eeData[52] & 0x000F);
  if (KvReCe > 7) {
    KvReCe = KvReCe - 16;
  }
  KvT[3] = KvReCe;
 
  kvScale = (eeData[56] & 0x0F00) >> 8;
 
  for (int i = 0; i < 24; i++) {
    for (int j = 0; j < 32; j++) {
      p = 32 * i + j;
      split = 2 * (p / 32 - (p / 64) * 2) + p % 2;
      kvTemp[p] = KvT[split];
      kvTemp[p] = kvTemp[p] / pow(2, (double)kvScale);
      // kvTemp[p] = kvTemp[p] * mlx90640->offset[p];
    }
  }
 
  temp = fabs(kvTemp[0]);
  for (int i = 1; i < 768; i++) {
    if (fabs(kvTemp[i]) > temp) {
      temp = fabs(kvTemp[i]);
    }
  }
 
  kvScale = 0;
  while (temp < 63.4) {
    temp = temp * 2;
    kvScale = kvScale + 1;
  }
 
  for (int i = 0; i < 768; i++) {
    temp = kvTemp[i] * pow(2, (double)kvScale);
    if (temp < 0) {
      mlx90640->kv[i] = (temp - 0.5);
    } else {
      mlx90640->kv[i] = (temp + 0.5);
    }
  }
 
  mlx90640->kvScale = kvScale;
}
 
void MLX90640::extractCPParameters(uint16_t *eeData, thermalParams *mlx90640) {
  float alphaSP[2];
  int16_t offsetSP[2];
  float cpKv;
  float cpKta;
  uint8_t alphaScale;
  uint8_t ktaScale1;
  uint8_t kvScale;
 
  alphaScale = ((eeData[32] & 0xF000) >> 12) + 27;
 
  offsetSP[0] = (eeData[58] & 0x03FF);
  if (offsetSP[0] > 511) {
    offsetSP[0] = offsetSP[0] - 1024;
  }
 
  offsetSP[1] = (eeData[58] & 0xFC00) >> 10;
  if (offsetSP[1] > 31) {
    offsetSP[1] = offsetSP[1] - 64;
  }
  offsetSP[1] = offsetSP[1] + offsetSP[0];
 
  alphaSP[0] = (eeData[57] & 0x03FF);
  if (alphaSP[0] > 511) {
    alphaSP[0] = alphaSP[0] - 1024;
  }
  alphaSP[0] = alphaSP[0] / pow(2, (double)alphaScale);
 
  alphaSP[1] = (eeData[57] & 0xFC00) >> 10;
  if (alphaSP[1] > 31) {
    alphaSP[1] = alphaSP[1] - 64;
  }
  alphaSP[1] = (1 + alphaSP[1] / 128) * alphaSP[0];
 
  cpKta = (eeData[59] & 0x00FF);
  if (cpKta > 127) {
    cpKta = cpKta - 256;
  }
  ktaScale1 = ((eeData[56] & 0x00F0) >> 4) + 8;
  mlx90640->cpKta = cpKta / pow(2, (double)ktaScale1);
 
  cpKv = (eeData[59] & 0xFF00) >> 8;
  if (cpKv > 127) {
    cpKv = cpKv - 256;
  }
  kvScale = (eeData[56] & 0x0F00) >> 8;
  mlx90640->cpKv = cpKv / pow(2, (double)kvScale);
 
  mlx90640->cpAlpha[0] = alphaSP[0];
  mlx90640->cpAlpha[1] = alphaSP[1];
  mlx90640->cpOffset[0] = offsetSP[0];
  mlx90640->cpOffset[1] = offsetSP[1];
}
 
void MLX90640::extractCILCParameters(uint16_t *eeData,
                                     thermalParams *mlx90640) {
  float ilChessC[3];
  uint8_t calibrationModeEE;
 
  calibrationModeEE = (eeData[10] & 0x0800) >> 4;
  calibrationModeEE = calibrationModeEE ^ 0x80;
 
  ilChessC[0] = (eeData[53] & 0x003F);
  if (ilChessC[0] > 31) {
    ilChessC[0] = ilChessC[0] - 64;
  }
  ilChessC[0] = ilChessC[0] / 16.0f;
 
  ilChessC[1] = (eeData[53] & 0x07C0) >> 6;
  if (ilChessC[1] > 15) {
    ilChessC[1] = ilChessC[1] - 32;
  }
  ilChessC[1] = ilChessC[1] / 2.0f;
 
  ilChessC[2] = (eeData[53] & 0xF800) >> 11;
  if (ilChessC[2] > 15) {
    ilChessC[2] = ilChessC[2] - 32;
  }
  ilChessC[2] = ilChessC[2] / 8.0f;
 
  mlx90640->calibrationModeEE = calibrationModeEE;
  mlx90640->ilChessC[0] = ilChessC[0];
  mlx90640->ilChessC[1] = ilChessC[1];
  mlx90640->ilChessC[2] = ilChessC[2];
}
 
int MLX90640::extractDeviatingPixels(uint16_t *eeData,
                                     thermalParams *mlx90640) {
  uint16_t pixCnt = 0;
  uint16_t brokenPixCnt = 0;
  uint16_t outlierPixCnt = 0;
  int warn = 0;
  int i;
 
  for (pixCnt = 0; pixCnt < 5; pixCnt++) {
    mlx90640->brokenPixels[pixCnt] = 0xFFFF;
    mlx90640->outlierPixels[pixCnt] = 0xFFFF;
  }
 
  pixCnt = 0;
  while (pixCnt < 768 && brokenPixCnt < 5 && outlierPixCnt < 5) {
    if (eeData[pixCnt + 64] == 0) {
      mlx90640->brokenPixels[brokenPixCnt] = pixCnt;
      brokenPixCnt = brokenPixCnt + 1;
    } else if ((eeData[pixCnt + 64] & 0x0001) != 0) {
      mlx90640->outlierPixels[outlierPixCnt] = pixCnt;
      outlierPixCnt = outlierPixCnt + 1;
    }
 
    pixCnt = pixCnt + 1;
  }
 
  if (brokenPixCnt > 4) {
    warn = -3;
  } else if (outlierPixCnt > 4) {
    warn = -4;
  } else if ((brokenPixCnt + outlierPixCnt) > 4) {
    warn = -5;
  } else {
    for (pixCnt = 0; pixCnt < brokenPixCnt; pixCnt++) {
      for (i = pixCnt + 1; i < brokenPixCnt; i++) {
        warn = this->checkAdjacentPixels(mlx90640->brokenPixels[pixCnt],
                                         mlx90640->brokenPixels[i]);
        if (warn != 0) {
          return warn;
        }
      }
    }
 
    for (pixCnt = 0; pixCnt < outlierPixCnt; pixCnt++) {
      for (i = pixCnt + 1; i < outlierPixCnt; i++) {
        warn = this->checkAdjacentPixels(mlx90640->outlierPixels[pixCnt],
                                         mlx90640->outlierPixels[i]);
        if (warn != 0) {
          return warn;
        }
      }
    }
 
    for (pixCnt = 0; pixCnt < brokenPixCnt; pixCnt++) {
      for (i = 0; i < outlierPixCnt; i++) {
        warn = this->checkAdjacentPixels(mlx90640->brokenPixels[pixCnt],
                                         mlx90640->outlierPixels[i]);
        if (warn != 0) {
          return warn;
        }
      }
    }
  }
 
  return warn;
}
 
int MLX90640::checkAdjacentPixels(uint16_t pix1, uint16_t pix2) {
  int pixPosDif;
 
  pixPosDif = pix1 - pix2;
  if (pixPosDif > -34 && pixPosDif < -30) {
    return -6;
  }
  if (pixPosDif > -2 && pixPosDif < 2) {
    return -6;
  }
  if (pixPosDif > 30 && pixPosDif < 34) {
    return -6;
  }
 
  return 0;
}
 
float MLX90640::getMedian(float *values, int n) {
  float temp;
 
  for (int i = 0; i < n - 1; i++) {
    for (int j = i + 1; j < n; j++) {
      if (values[j] < values[i]) {
        temp = values[i];
        values[i] = values[j];
        values[j] = temp;
      }
    }
  }
 
  if (n % 2 == 0) {
    return ((values[n / 2] + values[n / 2 - 1]) / 2.0);
 
  } else {
    return values[n / 2];
  }
}
 
int MLX90640::isPixelBad(uint16_t pixel, thermalParams *params) {
  for (int i = 0; i < 5; i++) {
    if (pixel == params->outlierPixels[i] || pixel == params->brokenPixels[i]) {
      return 1;
    }
  }
 
  return 0;
}
 
int MLX90640::validateFrameData(uint16_t *frameData) {
  uint8_t line = 0;
  for (int i = 0; i < 768; i += 32) {
    if ((frameData[i] == 0x7FFF) && frameData[833]) {
      return -8;
    }
    line = line + 1;
  }
 
  return 0;
}
 
int MLX90640::validateAuxData(uint16_t *auxData) {
  if (auxData[0] == 0x7FFF)
    return -8;
 
  for (int i = 8; i < 19; i++) {
    if (auxData[i] == 0x7FFF) {
      return -8;
    }
  }
  for (int i = 20; i < 23; i++) {
    if (auxData[i] == 0x7FFF) {
      return -8;
    }
  }
  for (int i = 24; i < 33; i++) {
    if (auxData[i] == 0x7FFF) {
      return -8;
    }
  }
  for (int i = 40; i < 51; i++) {
    if (auxData[i] == 0x7FFF) {
      return -8;
    }
  }
  for (int i = 52; i < 55; i++) {
    if (auxData[i] == 0x7FFF) {
      return -8;
    }
  }
  for (int i = 56; i < 64; i++) {
    if (auxData[i] == 0x7FFF) {
      return -8;
    }
  }
 
  return 0;
}