[Spresense] ミルクの温度測定機

Spresense とサーマルカメラセンサーを使い、赤ちゃん用のミルクの温度測定機を作成しました。
最高温度が 39.0度～ 40.0度になるとその旨を LCD に表示します。

私事ですが、昨年子どもが生まれました(第二子)。
現在、絶賛育児中なのですが、ちょっと気になることがあります。
うちの子、最初は多少冷めたミルクでもゴクゴク飲んでいたのですが、最近は適切な温度でないと飲んでくれません。

その適切な温度とは 39.0 ～ 40.0度ぽい。
そこで、ミルクの温度も測れる非接触体温計を使い、以下の作業をしています。

ミルクをお湯で溶かす
水道水で哺乳瓶を冷やす
非接触体温計でミルクの温度を測る
適切な温度でない場合、2 に戻る
適切な温度になったら、赤ちゃんに飲んでもらう

最近の体温計は時間が経つと勝手に電源が落ちてくれます。
普段ならそれで良いのですが、上記の作業中はずっと付いていて欲しいところ。
といいますのも、3 のときにいちいち電源入れて、ミルク温度モードにしないといけないので・・・。

すごい困っているわけではないんですが、この作業がちょっと楽にならないかな？と思ったのが製作の動機です。

Spresense にはサーマルカメラセンサー、LCD が接続されており、モバイルバッテリーで駆動します。

サーマルカメラセンサーは物体から放射される赤外線を検知して温度を測定するセンサーです。

このセンサーの前にミルクの入った哺乳瓶を置き、温度を測定します。
測定情報は LCD に表示され、定期的に更新されます。

サーマルカメラセンサーの温度情報から最高温度、最低温度、平均温度を算出し、表示しています。

今回使用しているサーマルカメラセンサーの解像度は 32x24 で、それを温度ごとに色分けし可視化しています。

補間処理をして拡大してみたのですが、CPU がいっぱいいっぱいか、チューニングが足りないのか結構処理時間が掛かるので結局そのままにしています。
Spresense には HW の拡大縮小が行えるので、それを使う手もありそうです。

最高温度が適切な温度 (動機に書いた 39.0 ～ 40.0度) になると、こちらのインジケーターが点灯します。
でかでかとした表示でユーザ (というか私 ) に手っ取り早くミルクのタイミングを教えてくれます。

インジケーター点灯時の LCD は以下のようになります。
インジケーター用画像は妻に描いてもらいました。困り顔になっていますがそういう顔で、結構似ていると思います。

哺乳瓶にお湯を入れて、それが40度近辺のときの様子が下記の動画で見ることができます。

ここに動画が表示されます

部品名	説明・補足など
Spresense メインボード	-
Spresense 拡張ボード	-
サーマルカメラセンサー	製品名はMLX90640。いろいろ MLX90640 を使ったシールドなどが出ていますが、I2Cで動作させるものならどれでも OK
ディスプレイ	製品名は MSP2807。ドライバに ILI9341 互換のものを使っているなら他でも OK
SD カード	東芝製の micro SD カード(4GB)を使用。インジケーター用の画像を格納するのに使用しています。他のメーカーのものでも問題ないと思います。容量も4GBも不要で、128MBくらいでも十分です
モバイルバッテリー	パナソニック QE-PL201 を使用しています。低電力モードに対応したものを選びましょう。そうでないと Spresense が低消費電力のため、バッテリーが出力を停止してしまいます。

micro USB オス-USB オスのケーブルで Spresense メインボードとモバイルバッテリーを接続します。
(Spresense 側は micro USB のメスであることに注意)

Spresense はサーマルカメラセンサーとは I2C 通信によってセンサーの値を取得します。
ピンの対応としては下記のようになります。

サーマルカメラセンサー	Spresense 拡張ボード
GND	GND
SCL	SCL
SDA	SDA
3V3	3.3V

Spresense はディスプレイとは SPI 通信によって画面の表示を行います。
ピンの対応としては下記のようになります。

ディスプレイ	Spresense 拡張ボード
SDO(MISO)	D12
LED	3.3V
SCK	D13
SDI(MOSI)	D11
DC/RS	D9
RESET	D8
CS	D10
GND	GND
VCC	3.3V


Spresense_milk.ino
/*
  Use Adafruit-GFX-Library 1.5.6
  https://github.com/kzhioki/Adafruit-GFX-Library/archive/spresense.zip

  Use Adafruit-ILI9341 1.5.1
  https://github.com/kzhioki/Adafruit_ILI9341/archive/spresense.zip
*/
#include <Wire.h>
#include <SDHCI.h>
#include <Adafruit_ILI9341.h>
#include "define.h"
#include "util.h"
#include "MLX90640_API.h"
#include "MLX90640_I2C_Driver.h"

#define TA_SHIFT          8  // Default shift for MLX90640 in open air
#define COLS              32
#define ROWS              24
#define pixelsArraySize   (COLS * ROWS)

// 適温判定の上限・下限
#define TEMP_CEIL   40
#define TEMP_FLOOR  39

// 赤ちゃん画像が描かれているかどうか
bool isBabyDrawn = false;

#define DRAW_PIXELS_OFFSET_X  272
#define DRAW_PIXELS_OFFSET_Y  16

byte speed_setting = 1;  // High is 1, Low is 2

Adafruit_ILI9341 tft = Adafruit_ILI9341(&SPI, TFT_DC_PIN, TFT_CS_PIN, TFT_RST_PIN);
paramsMLX90640 mlx90640;

SDClass sd;

float pixels[COLS * ROWS];
float reversePixels[COLS * ROWS];

#define get_pixels(x, y)   (pixels[y * COLS + x])

const byte MLX90640_address = 0x33;  // Default 7-bit unshifted address of the MLX90640

// 最大温度、最低温度、平均温度
float maxTemp = 0.0;
float minTemp = 255.0;
float avgTemp = 0.0;

// low range of the sensor (this will be blue on the screen)
float mintemp   = 24;   // For color mapping
float min_v     = 24;   // Value of current min temp
float min_cam_v = -40;  // Spec in datasheet

// high range of the sensor (this will be red on the screen)
float maxtemp   = 35;   // For color mapping
float max_v     = 35;   // Value of current max temp
float max_cam_v = 300;  // Spec in datasheet

// the colors we will be using
const uint16_t camColors[] = {
    0x480F, 0x400F, 0x400F, 0x400F, 0x4010, 0x3810, 0x3810, 0x3810, 0x3810,
    0x3010, 0x3010, 0x3010, 0x2810, 0x2810, 0x2810, 0x2810, 0x2010, 0x2010,
    0x2010, 0x1810, 0x1810, 0x1811, 0x1811, 0x1011, 0x1011, 0x1011, 0x0811,
    0x0811, 0x0811, 0x0011, 0x0011, 0x0011, 0x0011, 0x0011, 0x0031, 0x0031,
    0x0051, 0x0072, 0x0072, 0x0092, 0x00B2, 0x00B2, 0x00D2, 0x00F2, 0x00F2,
    0x0112, 0x0132, 0x0152, 0x0152, 0x0172, 0x0192, 0x0192, 0x01B2, 0x01D2,
    0x01F3, 0x01F3, 0x0213, 0x0233, 0x0253, 0x0253, 0x0273, 0x0293, 0x02B3,
    0x02D3, 0x02D3, 0x02F3, 0x0313, 0x0333, 0x0333, 0x0353, 0x0373, 0x0394,
    0x03B4, 0x03D4, 0x03D4, 0x03F4, 0x0414, 0x0434, 0x0454, 0x0474, 0x0474,
    0x0494, 0x04B4, 0x04D4, 0x04F4, 0x0514, 0x0534, 0x0534, 0x0554, 0x0554,
    0x0574, 0x0574, 0x0573, 0x0573, 0x0573, 0x0572, 0x0572, 0x0572, 0x0571,
    0x0591, 0x0591, 0x0590, 0x0590, 0x058F, 0x058F, 0x058F, 0x058E, 0x05AE,
    0x05AE, 0x05AD, 0x05AD, 0x05AD, 0x05AC, 0x05AC, 0x05AB, 0x05CB, 0x05CB,
    0x05CA, 0x05CA, 0x05CA, 0x05C9, 0x05C9, 0x05C8, 0x05E8, 0x05E8, 0x05E7,
    0x05E7, 0x05E6, 0x05E6, 0x05E6, 0x05E5, 0x05E5, 0x0604, 0x0604, 0x0604,
    0x0603, 0x0603, 0x0602, 0x0602, 0x0601, 0x0621, 0x0621, 0x0620, 0x0620,
    0x0620, 0x0620, 0x0E20, 0x0E20, 0x0E40, 0x1640, 0x1640, 0x1E40, 0x1E40,
    0x2640, 0x2640, 0x2E40, 0x2E60, 0x3660, 0x3660, 0x3E60, 0x3E60, 0x3E60,
    0x4660, 0x4660, 0x4E60, 0x4E80, 0x5680, 0x5680, 0x5E80, 0x5E80, 0x6680,
    0x6680, 0x6E80, 0x6EA0, 0x76A0, 0x76A0, 0x7EA0, 0x7EA0, 0x86A0, 0x86A0,
    0x8EA0, 0x8EC0, 0x96C0, 0x96C0, 0x9EC0, 0x9EC0, 0xA6C0, 0xAEC0, 0xAEC0,
    0xB6E0, 0xB6E0, 0xBEE0, 0xBEE0, 0xC6E0, 0xC6E0, 0xCEE0, 0xCEE0, 0xD6E0,
    0xD700, 0xDF00, 0xDEE0, 0xDEC0, 0xDEA0, 0xDE80, 0xDE80, 0xE660, 0xE640,
    0xE620, 0xE600, 0xE5E0, 0xE5C0, 0xE5A0, 0xE580, 0xE560, 0xE540, 0xE520,
    0xE500, 0xE4E0, 0xE4C0, 0xE4A0, 0xE480, 0xE460, 0xEC40, 0xEC20, 0xEC00,
    0xEBE0, 0xEBC0, 0xEBA0, 0xEB80, 0xEB60, 0xEB40, 0xEB20, 0xEB00, 0xEAE0,
    0xEAC0, 0xEAA0, 0xEA80, 0xEA60, 0xEA40, 0xF220, 0xF200, 0xF1E0, 0xF1C0,
    0xF1A0, 0xF180, 0xF160, 0xF140, 0xF100, 0xF0E0, 0xF0C0, 0xF0A0, 0xF080,
    0xF060, 0xF040, 0xF020, 0xF800,
};

void drawPixels(float *p, uint8_t rows, uint8_t cols) {
  int colorTemp;
  for (int y = 0; y < rows; y++) {
    for (int x = 0; x < cols; x++) {
      float val = get_point(p, rows, cols, x, y);
      if (val >= maxtemp) {
        colorTemp = maxtemp;
      } else if (val <= mintemp) {
        colorTemp = mintemp;
      } else {
        colorTemp = val;
      }
      
      uint8_t colorIndex = map(colorTemp, mintemp, maxtemp, 0, 255);
      colorIndex = constrain(colorIndex, 0, 255);
      
      // ピクセルを描画
      tft.drawPixel(x + DRAW_PIXELS_OFFSET_X, y + DRAW_PIXELS_OFFSET_Y, camColors[colorIndex]);
    }
  }
}

void draw() {
  bool isBaby = false;
  char temp[16];

  tft.fillRect(16, 16, 144, 48, ILI9341_WHITE);

  sprintf(temp, "max: %.2fC", maxTemp);
  if (TEMP_FLOOR <= maxTemp && maxTemp <= TEMP_CEIL) {
    isBaby = true;
    drawStringCenter(tft, temp, 16, 16, 8, 8, ILI9341_RED, 2);
  } else {
    drawStringCenter(tft, temp, 16, 16, 8, 8, ILI9341_BLACK, 2);
  }

  sprintf(temp, "min: %.2fC", minTemp);
  drawStringCenter(tft, temp, 16, 32, 8, 8, ILI9341_BLACK, 2);

  sprintf(temp, "avg: %.2fC", avgTemp);
  drawStringCenter(tft, temp, 16, 48, 8, 8, ILI9341_BLACK, 2);

  // show tmp image
//  drawPixels(pixels, ROWS, COLS);
  drawPixels(reversePixels, ROWS, COLS);

  if (isBaby != isBabyDrawn) {
    drawBabyGraphic(tft, sd, isBaby);
    isBabyDrawn = isBaby;
  }
}

void setup() {
  Serial.begin(SERIAL_BAUDRATE);

  while (!sd.begin()) { 
    Serial.println("Insert SD card"); 
  }

  Wire.begin();
  Wire.setClock(I2C_CLOCK);

  tft.begin(SPI_CLOCK); 
  tft.setRotation(3);
  tft.fillRect(0, 0, 320, 240, ILI9341_WHITE);

  // Get device parameters - We only have to do this once
  int status;
  uint16_t eeMLX90640[832];  // 32 * 24 = 768

  status = MLX90640_DumpEE(MLX90640_address, eeMLX90640);
  if (status != 0) {
    Serial.println("Failed to load system parameters");
  }

  status = MLX90640_ExtractParameters(eeMLX90640, &mlx90640);
  if (status != 0) {
    Serial.println("Parameter extraction failed");
  }

  // Setting MLX90640 device at slave address 0x33 to work with 2Hz refresh
  MLX90640_SetRefreshRate(0x33, 0x02);

  MLX90640_SetResolution(0x33, 0x03);

  drawBabyGraphic(tft, sd, false);
}

void loop() {
  uint16_t mlx90640Frame[834];
  long startTime, endTime;

  startTime = millis();

  // those fun get tmp array, 32*24, 5fps
  for (byte x = 0; x < speed_setting; x++) {
      int status = MLX90640_GetFrameData(MLX90640_address, mlx90640Frame);
      if (status < 0) {
          Serial.print("GetFrame Error: ");
          Serial.println(status);
          delay(500); // エラー時は長めに待つ (ほぼ意味は無いが・・・)
          return;
      }

      float vdd = MLX90640_GetVdd(mlx90640Frame, &mlx90640);
      float Ta  = MLX90640_GetTa(mlx90640Frame, &mlx90640);
      float tr  = Ta - TA_SHIFT;  // Reflected temperature based on the sensor
                                  // ambient temperature
      float emissivity = 0.95;
      MLX90640_CalculateTo(
          mlx90640Frame, &mlx90640, emissivity, tr,
          reversePixels);  // save pixels temp to array (pixels)
      int mode = MLX90640_GetCurMode(MLX90640_address);
      MLX90640_BadPixelsCorrection(mlx90640.brokenPixels, reversePixels, mode, &mlx90640);
  }

  /*
  // Reverse image (order of Integer array)
  for (int x = 0; x < pixelsArraySize; x++) {
      if (x % COLS == 0) {
          for (int j = 0 + x, k = (COLS - 1) + x; j < COLS + x; j++, k--) {
              pixels[j] = reversePixels[k];
          }
      }
  }
  */

  {
    maxTemp = 0.0;
    minTemp = 255.0;
    avgTemp = 0.0;

    // 最高温度、最低温度、平均温度の算出
    for (int i = 0; i < COLS * ROWS; i++) {
      if (maxTemp < reversePixels[i]) {
        maxTemp = reversePixels[i];
      }
      if (minTemp > reversePixels[i]) {
        minTemp = reversePixels[i];
      }
      avgTemp += reversePixels[i];
    }
    avgTemp /= COLS * ROWS;
  }

  draw();

  endTime = millis();
  {
    char temp[16];
    sprintf(temp, "time: %ld msec", endTime - startTime);
    Serial.println(temp);
  }

  delay(250);
}


define.h
#ifndef __MILK_DEFINE_H__
#define __MILK_DEFINE_H__

// シリアルの Baudrate
#define SERIAL_BAUDRATE  (115200)

// I2C の Clock
#define I2C_CLOCK (400000)

// SPI (TFT) の Clock
#define SPI_CLOCK (40000000)

// ディスプレイ用 CS, RESET, DC ピン
#define TFT_CS_PIN  10
#define TFT_RST_PIN 8
#define TFT_DC_PIN  9

// タッチスクリーン用 CS ピン
#define TOUCH_SCREEN_CS_PIN 7

// Adafruit_ILI9341 ライブラリのデフォルトフォントの幅、高さ (pixel)
#define TEXT_SIZE_BASE_WIDTH  6
#define TEXT_SIZE_BASE_HEIGHT 7

// ポイント座標
typedef struct Point {
  int x;
  int y;
} Point;

// Rect 情報
typedef struct Rect {
  int x;
  int y;
  int w;
  int h;
} Rect;

#endif // __MILK_DEFINE_H__


MLX90640_I2C_Driver.h
/**
   @copyright (C) 2017 Melexis N.V.

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.

*/
#ifndef _MLX90640_I2C_Driver_H_
#define _MLX90640_I2C_Driver_H_

#include <stdint.h>

// Define the size of the I2C buffer based on the platform the user has
//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__)

// I2C_BUFFER_LENGTH is defined in Wire.H
#define I2C_BUFFER_LENGTH BUFFER_LENGTH

#elif defined(__SAMD21G18A__)

// SAMD21 uses RingBuffer.h
#define I2C_BUFFER_LENGTH SERIAL_BUFFER_SIZE

#elif __MK20DX256__
// Teensy

#elif ARDUINO_ARCH_ESP32
// ESP32 based platforms

#else

// The catch-all default is 32
#define I2C_BUFFER_LENGTH 32

#endif
//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=

void MLX90640_I2CInit(void);
int MLX90640_I2CRead(uint8_t slaveAddr, unsigned int startAddress,
                     unsigned int nWordsRead, uint16_t *data);
int MLX90640_I2CWrite(uint8_t slaveAddr, unsigned int writeAddress,
                      uint16_t data);
void MLX90640_I2CFreqSet(int freq);
#endif


MLX90640_I2C_Driver.cpp
/**
   @copyright (C) 2017 Melexis N.V.

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.

*/

#include "MLX90640_I2C_Driver.h"

#include <Wire.h>

#include "Arduino.h"

void MLX90640_I2CInit() {
}

// Read a number of words from startAddress. Store into Data array.
// Returns 0 if successful, -1 if error
int MLX90640_I2CRead(uint8_t _deviceAddress, unsigned int startAddress,
                     unsigned int nWordsRead, uint16_t *data) {
    // Caller passes number of 'unsigned ints to read', increase this to 'bytes
    // to read'
    uint16_t bytesRemaining = nWordsRead * 2;

    // It doesn't look like sequential read works. Do we need to re-issue the
    // address command each time?

    uint16_t dataSpot = 0;  // Start at beginning of array

    // Setup a series of chunked I2C_BUFFER_LENGTH byte reads
    while (bytesRemaining > 0) {
        Wire.beginTransmission(_deviceAddress);
        Wire.write(startAddress >> 8);         // MSB
        Wire.write(startAddress & 0xFF);       // LSB
        if (Wire.endTransmission(false) != 0)  // Do not release bus
        {
            Serial.println("No ack read");
            return (0);  // Sensor did not ACK
        }

        uint16_t numberOfBytesToRead = bytesRemaining;
        if (numberOfBytesToRead > I2C_BUFFER_LENGTH)
            numberOfBytesToRead = I2C_BUFFER_LENGTH;

        Wire.requestFrom(_deviceAddress, (uint8_t)numberOfBytesToRead);
        if (Wire.available()) {
            for (uint16_t x = 0; x < numberOfBytesToRead / 2; x++) {
                // Store data into array
                data[dataSpot] = Wire.read() << 8;  // MSB
                data[dataSpot] |= Wire.read();      // LSB

                dataSpot++;
            }
        }

        bytesRemaining -= numberOfBytesToRead;

        startAddress += numberOfBytesToRead / 2;
    }

    return (0);  // Success
}

// Set I2C Freq, in kHz
// MLX90640_I2CFreqSet(1000) sets frequency to 1MHz
void MLX90640_I2CFreqSet(int freq) {
    // i2c.frequency(1000 * freq);
    Wire.setClock((long)1000 * freq);
}

// Write two bytes to a two byte address
int MLX90640_I2CWrite(uint8_t _deviceAddress, unsigned int writeAddress,
                      uint16_t data) {
    Wire.beginTransmission((uint8_t)_deviceAddress);
    Wire.write(writeAddress >> 8);    // MSB
    Wire.write(writeAddress & 0xFF);  // LSB
    Wire.write(data >> 8);            // MSB
    Wire.write(data & 0xFF);          // LSB
    if (Wire.endTransmission() != 0) {
        // Sensor did not ACK
        Serial.println("Error: Sensor did not ack");
        return (-1);
    }

    uint16_t dataCheck;
    MLX90640_I2CRead(_deviceAddress, writeAddress, 1, &dataCheck);
    if (dataCheck != data) {
        // Serial.println("The write request didn't stick");
        return -2;
    }

    return (0);  // Success
}


MLX90640_API.h
/**
 * @copyright (C) 2017 Melexis N.V.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
#ifndef _MLX640_API_H_
#define _MLX640_API_H_

#include <Arduino.h>

#define SCALEALPHA 0.000001

typedef struct {
    int16_t kVdd;
    int16_t vdd25;
    float KvPTAT;
    float KtPTAT;
    uint16_t vPTAT25;
    float alphaPTAT;
    int16_t gainEE;
    float tgc;
    float cpKv;
    float cpKta;
    uint8_t resolutionEE;
    uint8_t calibrationModeEE;
    float KsTa;
    float ksTo[5];
    int16_t ct[5];
    uint16_t alpha[768];
    uint8_t alphaScale;
    int16_t offset[768];
    int8_t kta[768];
    uint8_t ktaScale;
    int8_t kv[768];
    uint8_t kvScale;
    float cpAlpha[2];
    int16_t cpOffset[2];
    float ilChessC[3];
    uint16_t brokenPixels[5];
    uint16_t outlierPixels[5];
} paramsMLX90640;

int MLX90640_DumpEE(uint8_t slaveAddr, uint16_t *eeData);
int MLX90640_GetFrameData(uint8_t slaveAddr, uint16_t *frameData);
int MLX90640_ExtractParameters(uint16_t *eeData, paramsMLX90640 *mlx90640);
float MLX90640_GetVdd(uint16_t *frameData, const paramsMLX90640 *params);
float MLX90640_GetTa(uint16_t *frameData, const paramsMLX90640 *params);
void MLX90640_GetImage(uint16_t *frameData, const paramsMLX90640 *params,
                       float *result);
void MLX90640_CalculateTo(uint16_t *frameData, const paramsMLX90640 *params,
                          float emissivity, float tr, float *result);
int MLX90640_SetResolution(uint8_t slaveAddr, uint8_t resolution);
int MLX90640_GetCurResolution(uint8_t slaveAddr);
int MLX90640_SetRefreshRate(uint8_t slaveAddr, uint8_t refreshRate);
int MLX90640_GetRefreshRate(uint8_t slaveAddr);
int MLX90640_GetSubPageNumber(uint16_t *frameData);
int MLX90640_GetCurMode(uint8_t slaveAddr);
int MLX90640_SetInterleavedMode(uint8_t slaveAddr);
int MLX90640_SetChessMode(uint8_t slaveAddr);
void MLX90640_BadPixelsCorrection(uint16_t *pixels, float *to, int mode,
                                  paramsMLX90640 *params);

#endif


MLX90640_API.cpp
/**
 * @copyright (C) 2017 Melexis N.V.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
#include "MLX90640_API.h"

#include <math.h>

#include "MLX90640_I2C_Driver.h"

void ExtractVDDParameters(uint16_t *eeData, paramsMLX90640 *mlx90640);
void ExtractPTATParameters(uint16_t *eeData, paramsMLX90640 *mlx90640);
void ExtractGainParameters(uint16_t *eeData, paramsMLX90640 *mlx90640);
void ExtractTgcParameters(uint16_t *eeData, paramsMLX90640 *mlx90640);
void ExtractResolutionParameters(uint16_t *eeData, paramsMLX90640 *mlx90640);
void ExtractKsTaParameters(uint16_t *eeData, paramsMLX90640 *mlx90640);
void ExtractKsToParameters(uint16_t *eeData, paramsMLX90640 *mlx90640);
void ExtractAlphaParameters(uint16_t *eeData, paramsMLX90640 *mlx90640);
void ExtractOffsetParameters(uint16_t *eeData, paramsMLX90640 *mlx90640);
void ExtractKtaPixelParameters(uint16_t *eeData, paramsMLX90640 *mlx90640);
void ExtractKvPixelParameters(uint16_t *eeData, paramsMLX90640 *mlx90640);
void ExtractCPParameters(uint16_t *eeData, paramsMLX90640 *mlx90640);
void ExtractCILCParameters(uint16_t *eeData, paramsMLX90640 *mlx90640);
int ExtractDeviatingPixels(uint16_t *eeData, paramsMLX90640 *mlx90640);
int CheckAdjacentPixels(uint16_t pix1, uint16_t pix2);
float GetMedian(float *values, int n);
int IsPixelBad(uint16_t pixel, paramsMLX90640 *params);

int MLX90640_DumpEE(uint8_t slaveAddr, uint16_t *eeData) {
    return MLX90640_I2CRead(slaveAddr, 0x2400, 832, eeData);
}

int MLX90640_GetFrameData(uint8_t slaveAddr, uint16_t *frameData) {
    uint16_t dataReady = 1;
    uint16_t controlRegister1;
    uint16_t statusRegister;
    int error   = 1;
    uint8_t cnt = 0;

    dataReady = 0;
    while (dataReady == 0) {
        error = MLX90640_I2CRead(slaveAddr, 0x8000, 1, &statusRegister);
        if (error != 0) {
            return error;
        }
        dataReady = statusRegister & 0x0008;
    }

    while (dataReady != 0 && cnt < 5) {
        error = MLX90640_I2CWrite(slaveAddr, 0x8000, 0x0030);
        if (error == -1) {
            return error;
        }

        error = MLX90640_I2CRead(slaveAddr, 0x0400, 832, frameData);
        if (error != 0) {
            return error;
        }

        error = MLX90640_I2CRead(slaveAddr, 0x8000, 1, &statusRegister);
        if (error != 0) {
            return error;
        }
        dataReady = statusRegister & 0x0008;
        cnt       = cnt + 1;
    }

    if (cnt > 4) {
        return -8;
    }

    error          = MLX90640_I2CRead(slaveAddr, 0x800D, 1, &controlRegister1);
    frameData[832] = controlRegister1;
    frameData[833] = statusRegister & 0x0001;

    if (error != 0) {
        return error;
    }

    return frameData[833];
}

int MLX90640_ExtractParameters(uint16_t *eeData, paramsMLX90640 *mlx90640) {
    int error = 0;

    ExtractVDDParameters(eeData, mlx90640);
    ExtractPTATParameters(eeData, mlx90640);
    ExtractGainParameters(eeData, mlx90640);
    ExtractTgcParameters(eeData, mlx90640);
    ExtractResolutionParameters(eeData, mlx90640);
    ExtractKsTaParameters(eeData, mlx90640);
    ExtractKsToParameters(eeData, mlx90640);
    ExtractCPParameters(eeData, mlx90640);
    ExtractAlphaParameters(eeData, mlx90640);
    ExtractOffsetParameters(eeData, mlx90640);
    ExtractKtaPixelParameters(eeData, mlx90640);
    ExtractKvPixelParameters(eeData, mlx90640);
    ExtractCILCParameters(eeData, mlx90640);
    error = ExtractDeviatingPixels(eeData, mlx90640);

    return error;
}

//------------------------------------------------------------------------------

int MLX90640_SetResolution(uint8_t slaveAddr, uint8_t resolution) {
    uint16_t controlRegister1;
    int value;
    int error;

    value = (resolution & 0x03) << 10;

    error = MLX90640_I2CRead(slaveAddr, 0x800D, 1, &controlRegister1);

    if (error == 0) {
        value = (controlRegister1 & 0xF3FF) | value;
        error = MLX90640_I2CWrite(slaveAddr, 0x800D, value);
    }

    return error;
}

//------------------------------------------------------------------------------

int MLX90640_GetCurResolution(uint8_t slaveAddr) {
    uint16_t controlRegister1;
    int resolutionRAM;
    int error;

    error = MLX90640_I2CRead(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;

    error = MLX90640_I2CRead(slaveAddr, 0x800D, 1, &controlRegister1);
    if (error == 0) {
        value = (controlRegister1 & 0xFC7F) | value;
        error = MLX90640_I2CWrite(slaveAddr, 0x800D, value);
    }

    return error;
}

//------------------------------------------------------------------------------

int MLX90640_GetRefreshRate(uint8_t slaveAddr) {
    uint16_t controlRegister1;
    int refreshRate;
    int error;

    error = MLX90640_I2CRead(slaveAddr, 0x800D, 1, &controlRegister1);
    if (error != 0) {
        return error;
    }
    refreshRate = (controlRegister1 & 0x0380) >> 7;

    return refreshRate;
}

//------------------------------------------------------------------------------

int MLX90640_SetInterleavedMode(uint8_t slaveAddr) {
    uint16_t controlRegister1;
    int value;
    int error;

    error = MLX90640_I2CRead(slaveAddr, 0x800D, 1, &controlRegister1);

    if (error == 0) {
        value = (controlRegister1 & 0xEFFF);
        error = MLX90640_I2CWrite(slaveAddr, 0x800D, value);
    }

    return error;
}

//------------------------------------------------------------------------------

int MLX90640_SetChessMode(uint8_t slaveAddr) {
    uint16_t controlRegister1;
    int value;
    int error;

    error = MLX90640_I2CRead(slaveAddr, 0x800D, 1, &controlRegister1);

    if (error == 0) {
        value = (controlRegister1 | 0x1000);
        error = MLX90640_I2CWrite(slaveAddr, 0x800D, value);
    }

    return error;
}

//------------------------------------------------------------------------------

int MLX90640_GetCurMode(uint8_t slaveAddr) {
    uint16_t controlRegister1;
    int modeRAM;
    int error;

    error = MLX90640_I2CRead(slaveAddr, 0x800D, 1, &controlRegister1);
    if (error != 0) {
        return error;
    }
    modeRAM = (controlRegister1 & 0x1000) >> 12;

    return modeRAM;
}

//------------------------------------------------------------------------------

void MLX90640_CalculateTo(uint16_t *frameData, const paramsMLX90640 *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     = MLX90640_GetVdd(frameData, params);
    ta      = MLX90640_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 calculation
    //-----------------------------------
    gain = frameData[778];
    if (gain > 32767) {
        gain = gain - 65536;
    }

    gain = params->gainEE / gain;

    //------------------------- To calculation
    //-------------------------------------
    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;
        }
    }
}

//------------------------------------------------------------------------------

void MLX90640_GetImage(uint16_t *frameData, const paramsMLX90640 *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     = MLX90640_GetVdd(frameData, params);
    ta      = MLX90640_GetTa(frameData, params);

    ktaScale = pow(2, (double)params->ktaScale);
    kvScale  = pow(2, (double)params->kvScale);

    //------------------------- Gain calculation
    //-----------------------------------
    gain = frameData[778];
    if (gain > 32767) {
        gain = gain - 65536;
    }

    gain = params->gainEE / gain;

    //------------------------- Image calculation
    //-------------------------------------
    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;
        }
    }
}

//------------------------------------------------------------------------------

float MLX90640_GetVdd(uint16_t *frameData, const paramsMLX90640 *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 paramsMLX90640 *params) {
    float ptat;
    float ptatArt;
    float vdd;
    float ta;

    vdd = MLX90640_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;
}

//------------------------------------------------------------------------------

int MLX90640_GetSubPageNumber(uint16_t *frameData) {
    return frameData[833];
}

//------------------------------------------------------------------------------
void MLX90640_BadPixelsCorrection(uint16_t *pixels, float *to, int mode,
                                  paramsMLX90640 *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]] = 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 (IsPixelBad(pixels[pix] - 2, params) == 0 &&
                    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 ExtractVDDParameters(uint16_t *eeData, paramsMLX90640 *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 ExtractPTATParameters(uint16_t *eeData, paramsMLX90640 *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 ExtractGainParameters(uint16_t *eeData, paramsMLX90640 *mlx90640) {
    int16_t gainEE;

    gainEE = eeData[48];
    if (gainEE > 32767) {
        gainEE = gainEE - 65536;
    }

    mlx90640->gainEE = gainEE;
}

//------------------------------------------------------------------------------

void ExtractTgcParameters(uint16_t *eeData, paramsMLX90640 *mlx90640) {
    float tgc;
    tgc = eeData[60] & 0x00FF;
    if (tgc > 127) {
        tgc = tgc - 256;
    }
    tgc = tgc / 32.0f;

    mlx90640->tgc = tgc;
}

//------------------------------------------------------------------------------

void ExtractResolutionParameters(uint16_t *eeData, paramsMLX90640 *mlx90640) {
    uint8_t resolutionEE;
    resolutionEE = (eeData[56] & 0x3000) >> 12;

    mlx90640->resolutionEE = resolutionEE;
}

//------------------------------------------------------------------------------

void ExtractKsTaParameters(uint16_t *eeData, paramsMLX90640 *mlx90640) {
    float KsTa;
    KsTa = (eeData[60] & 0xFF00) >> 8;
    if (KsTa > 127) {
        KsTa = KsTa - 256;
    }
    KsTa = KsTa / 8192.0f;

    mlx90640->KsTa = KsTa;
}

//------------------------------------------------------------------------------

void ExtractKsToParameters(uint16_t *eeData, paramsMLX90640 *mlx90640) {
    int 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 ExtractAlphaParameters(uint16_t *eeData, paramsMLX90640 *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 < 32768) {
        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 ExtractOffsetParameters(uint16_t *eeData, paramsMLX90640 *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 ExtractKtaPixelParameters(uint16_t *eeData, paramsMLX90640 *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 < 64) {
        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 ExtractKvPixelParameters(uint16_t *eeData, paramsMLX90640 *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 < 64) {
        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 ExtractCPParameters(uint16_t *eeData, paramsMLX90640 *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 ExtractCILCParameters(uint16_t *eeData, paramsMLX90640 *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 ExtractDeviatingPixels(uint16_t *eeData, paramsMLX90640 *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 = 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 = 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 = CheckAdjacentPixels(mlx90640->brokenPixels[pixCnt],
                                           mlx90640->outlierPixels[i]);
                if (warn != 0) {
                    return warn;
                }
            }
        }
    }

    return warn;
}

//------------------------------------------------------------------------------

int 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 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 IsPixelBad(uint16_t pixel, paramsMLX90640 *params) {
    for (int i = 0; i < 5; i++) {
        if (pixel == params->outlierPixels[i] ||
            pixel == params->brokenPixels[i]) {
            return 1;
        }
    }

    return 0;
}

//------------------------------------------------------------------------------


util.h
#ifndef __MILK_UTIL_H__
#define __MILK_UTIL_H__

#include <SDHCI.h>
#include <Adafruit_ILI9341.h>
#include "define.h"

// 液晶ディスプレイの指定の場所に文字列を表示する
void drawStringCenter(Adafruit_ILI9341& tft, const char* str, int x, int y, int w, int h, int color, int scale = 1);

// 液晶ディスプレイの下部に文字列を表示する
void putStringOnLcd(Adafruit_ILI9341& tft, String str, int color);

// 液晶ディスプレイに LINE_THICKNESS の太さの四角形を描画する
void drawBox(Adafruit_ILI9341& tft, uint16_t* imgBuf, int offset_x, int offset_y, int width, int height, int thickness, int color);

// 指定した行と列から x と y の示す p を返す
float get_point(float *p, uint8_t rows, uint8_t cols, int8_t x, int8_t y);

// 赤ちゃん画像を描画。isBaby == false だと、ただの丸
void drawBabyGraphic(Adafruit_ILI9341& tft, SDClass &sd, bool isBaby);

#endif // __MILK_UTIL_H__


util.cpp
#include "util.h"

#define BABY_GRA_OFFSET_X 82
#define BABY_GRA_OFFSET_Y 82
#define BABY_GRA_WIDTH    156
#define BABY_GRA_HEIGHT   155

// 赤ちゃん画像バッファ
uint16_t babyGraBuffer[BABY_GRA_WIDTH * BABY_GRA_HEIGHT];

void drawStringCenter(Adafruit_ILI9341& tft, const char* str, int x, int y, int w, int h, int color, int scale = 1) {
  int len = strlen(str);
  tft.setTextSize(scale);
  int strWidth = len * TEXT_SIZE_BASE_WIDTH * scale;
  int strHeight = TEXT_SIZE_BASE_HEIGHT * scale;
  int sx = (w - strWidth)/2;
  if (sx < 0) {
    sx = 0;
  }
  int sy = (h - strHeight)/2;
  if (sy < 0) {
    sy = 0;
  }
  tft.setCursor(x + sx, y + sy);
  tft.setTextColor(color);
  tft.print(str);
}

void putStringOnLcd(Adafruit_ILI9341& tft, String str, int color) {
  int len = str.length();
  tft.fillRect(0, 224, 320, 240, ILI9341_BLACK);
  tft.setTextSize(1);
  int sx = 160 - len/2*12;
  if (sx < 0) sx = 0;
  tft.setCursor(sx, 225);
  tft.setTextColor(color);
  tft.println(str);
}

void drawBox(Adafruit_ILI9341& tft, uint16_t* imgBuf, int offset_x, int offset_y, int width, int height, int thickness, int color) {
  /* Draw target line */
  for (int x = offset_x; x < offset_x+width; ++x) {
    for (int n = 0; n < thickness; ++n) {
      *(imgBuf + tft.width()*(offset_y+n) + x)          = color;
      *(imgBuf + tft.width()*(offset_y+height-1-n) + x) = color;
    }
  }
  for (int y = offset_y; y < offset_y+height; ++y) {
    for (int n = 0; n < thickness; ++n) {
      *(imgBuf + tft.width()*y + offset_x+n)           = color;
      *(imgBuf + tft.width()*y + offset_x + width-1-n) = color;
    }
  } 
}

float get_point(float *p, uint8_t rows, uint8_t cols, int8_t x, int8_t y) {
    if (x < 0) x = 0;
    if (y < 0) y = 0;
    if (x >= cols) x = cols - 1;
    if (y >= rows) y = rows - 1;
    return p[y * cols + x];
}

void drawBabyGraphic(Adafruit_ILI9341& tft, SDClass &sd, bool isBaby) {
  char path[32];
  File myFile;

  if (isBaby) {
    sprintf(path, "GRA/baby.raw");
  } else {
    sprintf(path, "GRA/no_baby.raw");    
  }
  myFile = sd.open(path, FILE_READ);

  // Verify file open
  if (!myFile) {
    printf("File open error(%s)\n", path);
    exit(1);
  }

  // ファイルのサイズを取得
  int fileSize = myFile.size();

  // ファイルサイズとバッファのサイズが一致しなかったらエラー
  if (fileSize != sizeof(babyGraBuffer)) {
    printf("fileSize doesn't match buffer size!(fileSize=%d)\n", fileSize);
    exit(1);
  }
    
  int bytesRead = myFile.read(babyGraBuffer, fileSize);
  // 読み込んだサイズとファイルサイズが一致しなかったらエラー
  if (bytesRead != fileSize) {
    printf("bytesRead doesn't match fileSizse!(bytesRead=%d)\n", bytesRead);
    exit(1);
  }
  myFile.close();

  tft.drawRGBBitmap(BABY_GRA_OFFSET_X, BABY_GRA_OFFSET_Y, babyGraBuffer, BABY_GRA_WIDTH, BABY_GRA_HEIGHT);
}