smart-home/firefly_esp32/main/Sensor/HTPA60x40dR1L0.9/HTPA_Sensor60x40.cpp

/**
 * Module: HTPA60x40dR1L0.9/0.8 Thermal Imaging Sensor
 *
 * Copyright 2025-2026 fw <kingfun2000@qq.com>
 */
#include "HTPA_Sensor60x40.h"
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include "esp_log.h"
#include "math.h"
#include <string.h>
#include <inttypes.h>

static const char *TAG = "HTPA60x40";

#ifndef bitRead
#define bitRead(value, bit)            (((value) >> (bit)) & 0x01)
#endif

HTPA60x40::HTPA60x40()
    : _pixc2_0(nullptr), _pixc2(nullptr)
{
    // Initialize device characteristics for 60x40 sensor
    _dev_const = {
        NUMBER_OF_PIXEL_60x40,
        NUMBER_OF_BLOCKS_60x40,
        ROW_PER_BLOCK_60x40,
        PIXEL_PER_BLOCK_60x40,
        PIXEL_PER_COLUMN_60x40,
        PIXEL_PER_ROW_60x40,
        ALLOWED_DEADPIX_60x40,
        TABLENUMBER_60x40,
        TABLEOFFSET_60x40,
        PTAT_POS_60x40,
        VDD_POS_60x40,
        PTAT_VDD_SWITCH_60x40,
        ATC_ACTIVE_60x40,
        ATC_POS_60x40,
        DATA_POS_60x40,
    };
}

HTPA60x40::~HTPA60x40() {
    timer.stop();
    timer.delete_timer();
    if (_pixc2_0) {
        free(_pixc2_0);
    }
}

esp_err_t HTPA60x40::init() {
    
    // Allocate memory for pixel constants (60x40 = 2400 pixels)
    _pixc2_0 = (uint32_t*)malloc(NUMBER_OF_PIXEL_60x40 * sizeof(uint32_t));
    if (!_pixc2_0) {
        ESP_LOGE(TAG, "Memory allocation failed for %d pixels", NUMBER_OF_PIXEL_60x40);
        return ESP_ERR_NO_MEM;
    }
    _pixc2 = _pixc2_0;
    
    uint8_t error = 1;
    int retry_count = 0;
    const int max_retries = 5;
    
    while (error != 0 && retry_count < max_retries) {
        vTaskDelay(pdMS_TO_TICKS(2000));
        ESP_LOGI(TAG, "Initializing HTPA60x40 sensor... (attempt %d/%d)", retry_count + 1, max_retries);
        
        // Initialize I2C bus
        ESP_LOGI(TAG, "Initializing I2C bus (GPIO14=SDA, GPIO21=SCL)...");
        esp_err_t i2c_ret = Wire.begin(14, 21);  // GPIO14=SDA, GPIO21=SCL
        if (i2c_ret != ESP_OK) {
            ESP_LOGW(TAG, "I2C initialization returned: %s (continuing anyway)", esp_err_to_name(i2c_ret));
        }
        
        // Try to connect to sensor
        Wire.beginTransmission(SENSOR_ADDRESS_60x40);
        error = Wire.endTransmission();
        if (error != 0) {
            ESP_LOGE(TAG, "Sensor initialization failed, check connections and retry %d", error);
        }
        
        retry_count++;
        if (error != 0 && retry_count < max_retries) {
            ESP_LOGW(TAG, "Retrying sensor initialization in 2 seconds...");
        }
    }
    
    if (error != 0) {
        ESP_LOGE(TAG, "Failed to initialize HTPA60x40 sensor after %d attempts", max_retries);
        return ESP_ERR_NOT_FOUND;
    }
    
    ESP_LOGI(TAG, "HTPA60x40 sensor connected successfully");
    
    // Read EEPROM calibration data
    read_eeprom();
    
    // Write calibration settings to sensor
    write_calibration_settings_to_sensor();
    
    // Calculate pixel constants
    calcPixC();
    
    // Calculate timer value
    timert = calc_timert(clk_calib, mbit_calib);
    ESP_LOGI(TAG, "Timer value: %d", timert);
    
    // Initialize timer
    esp_err_t timer_ret = timer.init(timer_callback, this);
    if (timer_ret != ESP_OK) {
        ESP_LOGE(TAG, "Timer initialization failed: %s", esp_err_to_name(timer_ret));
        return timer_ret;
    }
    
    // Start timer
    timer.start(timert);
    ESP_LOGI(TAG, "HTPA60x40 sensor initialization completed successfully");
    
    return ESP_OK;
}

bool HTPA60x40::process() {
    if (!_new_data_available) {
        return false;
    }
    
    _new_data_available = false;
    
    // Read sensor data
    readblockinterrupt();
    
    // Sort and process data
    sort_data();
    
    // Calculate pixel temperatures
    calculate_pixel_temp();
    
    return true;
}

bool HTPA60x40::getData(uint16_t* data, int size) {
    if (size < NUMBER_OF_PIXEL_60x40) {
        ESP_LOGE(TAG, "Buffer too small. Required: %d, provided: %d", NUMBER_OF_PIXEL_60x40, size);
        return false;
    }
    
    // Copy pixel data to output buffer
    int index = 0;
    for (int row = 0; row < PIXEL_PER_ROW_60x40; row++) {
        for (int col = 0; col < PIXEL_PER_COLUMN_60x40; col++) {
            data[index++] = data_pixel[col][row];
        }
    }
    
    return true;
}

bool HTPA60x40::getThermalData60x40(float* thermal_data) {
    if (!thermal_data) {
        return false;
    }
    
    // Convert raw data to temperature values
    int index = 0;
    for (int row = 0; row < PIXEL_PER_ROW_60x40; row++) {
        for (int col = 0; col < PIXEL_PER_COLUMN_60x40; col++) {
            // Convert raw ADC value to temperature (simplified conversion)
            // This would need proper calibration based on the lookup table
            float temp = (float)data_pixel[col][row] / 100.0f + 25.0f; // Placeholder conversion
            thermal_data[index++] = temp;
        }
    }
    
    return true;
}

bool HTPA60x40::getThermalGrid8x5(float* grid_temps) {
    if (!grid_temps) {
        return false;
    }
    
    // Downsample 60x40 to 8x5 grid (7.5x8 pixels per grid cell)
    const int grid_width = 8;
    const int grid_height = 5;
    const int pixels_per_grid_x = PIXEL_PER_COLUMN_60x40 / grid_width;  // 7.5, use 7 and 8 alternately
    const int pixels_per_grid_y = PIXEL_PER_ROW_60x40 / grid_height;    // 8
    
    for (int grid_y = 0; grid_y < grid_height; grid_y++) {
        for (int grid_x = 0; grid_x < grid_width; grid_x++) {
            float sum = 0.0f;
            int count = 0;
            
            // Calculate the actual pixel range for this grid cell
            int start_x = grid_x * pixels_per_grid_x;
            int end_x = (grid_x == grid_width - 1) ? PIXEL_PER_COLUMN_60x40 : (grid_x + 1) * pixels_per_grid_x;
            int start_y = grid_y * pixels_per_grid_y;
            int end_y = (grid_y + 1) * pixels_per_grid_y;
            
            // Average the pixels in this grid cell
            for (int y = start_y; y < end_y && y < PIXEL_PER_ROW_60x40; y++) {
                for (int x = start_x; x < end_x && x < PIXEL_PER_COLUMN_60x40; x++) {
                    sum += (float)data_pixel[x][y] / 100.0f + 25.0f; // Placeholder conversion
                    count++;
                }
            }
            
            grid_temps[grid_y * grid_width + grid_x] = (count > 0) ? sum / count : 25.0f;
        }
    }
    
    return true;
}

bool HTPA60x40::getThermalGrid12x8(float* grid_temps) {
    if (!grid_temps) {
        return false;
    }
    
    // Downsample 60x40 to 12x8 grid (5x5 pixels per grid cell)
    const int grid_width = 12;
    const int grid_height = 8;
    const int pixels_per_grid_x = PIXEL_PER_COLUMN_60x40 / grid_width;  // 5
    const int pixels_per_grid_y = PIXEL_PER_ROW_60x40 / grid_height;    // 5
    
    for (int grid_y = 0; grid_y < grid_height; grid_y++) {
        for (int grid_x = 0; grid_x < grid_width; grid_x++) {
            float sum = 0.0f;
            int count = 0;
            
            int start_x = grid_x * pixels_per_grid_x;
            int end_x = (grid_x == grid_width - 1) ? PIXEL_PER_COLUMN_60x40 : (grid_x + 1) * pixels_per_grid_x;
            int start_y = grid_y * pixels_per_grid_y;
            int end_y = (grid_y == grid_height - 1) ? PIXEL_PER_ROW_60x40 : (grid_y + 1) * pixels_per_grid_y;
            
            for (int y = start_y; y < end_y; y++) {
                for (int x = start_x; x < end_x; x++) {
                    sum += (float)data_pixel[x][y] / 100.0f + 25.0f; // Placeholder conversion
                    count++;
                }
            }
            
            grid_temps[grid_y * grid_width + grid_x] = (count > 0) ? sum / count : 25.0f;
        }
    }
    
    return true;
}

bool HTPA60x40::getRegionTemperature(uint8_t start_x, uint8_t start_y, uint8_t width, uint8_t height, float* avg_temp) {
    if (!avg_temp || start_x >= PIXEL_PER_COLUMN_60x40 || start_y >= PIXEL_PER_ROW_60x40) {
        return false;
    }
    
    float sum = 0.0f;
    int count = 0;
    
    uint8_t end_x = (start_x + width > PIXEL_PER_COLUMN_60x40) ? PIXEL_PER_COLUMN_60x40 : start_x + width;
    uint8_t end_y = (start_y + height > PIXEL_PER_ROW_60x40) ? PIXEL_PER_ROW_60x40 : start_y + height;
    
    for (uint8_t y = start_y; y < end_y; y++) {
        for (uint8_t x = start_x; x < end_x; x++) {
            sum += (float)data_pixel[x][y] / 100.0f + 25.0f; // Placeholder conversion
            count++;
        }
    }
    
    *avg_temp = (count > 0) ? sum / count : 25.0f;
    return true;
}

bool HTPA60x40::getHotColdSpots(float* hot_temp, uint8_t* hot_x, uint8_t* hot_y, 
                               float* cold_temp, uint8_t* cold_x, uint8_t* cold_y) {
    if (!hot_temp || !hot_x || !hot_y || !cold_temp || !cold_x || !cold_y) {
        return false;
    }
    
    float max_temp = -273.15f; // Absolute zero
    float min_temp = 1000.0f;  // Very high temperature
    uint8_t max_x = 0, max_y = 0, min_x = 0, min_y = 0;
    
    for (uint8_t y = 0; y < PIXEL_PER_ROW_60x40; y++) {
        for (uint8_t x = 0; x < PIXEL_PER_COLUMN_60x40; x++) {
            float temp = (float)data_pixel[x][y] / 100.0f + 25.0f; // Placeholder conversion
            
            if (temp > max_temp) {
                max_temp = temp;
                max_x = x;
                max_y = y;
            }
            
            if (temp < min_temp) {
                min_temp = temp;
                min_x = x;
                min_y = y;
            }
        }
    }
    
    *hot_temp = max_temp;
    *hot_x = max_x;
    *hot_y = max_y;
    *cold_temp = min_temp;
    *cold_x = min_x;
    *cold_y = min_y;
    
    return true;
}

uint16_t HTPA60x40::get_ambient_temperature() {
    return Ta;
}

// Timer callback function
void HTPA60x40::timer_callback(void* arg) {
    HTPA60x40* sensor = static_cast<HTPA60x40*>(arg);
    sensor->_new_data_available = true;
}

// Private functions implementation (adapted from 32x32 version)
uint8_t HTPA60x40::write_sensor_byte(uint8_t registeraddress, uint8_t input) {
    Wire.beginTransmission(SENSOR_ADDRESS_60x40);
    Wire.write(registeraddress);
    Wire.write(input);
    return Wire.endTransmission();
}

void HTPA60x40::read_sensor_register(uint8_t addr, uint8_t *dest, uint16_t n) {
    Wire.beginTransmission(SENSOR_ADDRESS_60x40);
    Wire.write(addr);
    Wire.endTransmission();
    
    Wire.requestFrom(SENSOR_ADDRESS_60x40, n);
    for (uint16_t i = 0; i < n; i++) {
        if (Wire.available()) {
            dest[i] = Wire.read();
        }
    }
}

uint8_t HTPA60x40::read_EEPROM_byte(uint16_t address) {
    uint8_t data = 0;
    Wire.beginTransmission(EEPROM_ADDRESS_60x40);
    Wire.write((uint8_t)(address >> 8));   // MSB
    Wire.write((uint8_t)(address & 0xFF)); // LSB
    Wire.endTransmission();
    
    Wire.requestFrom(EEPROM_ADDRESS_60x40, 1);
    if (Wire.available()) {
        data = Wire.read();
    }
    
    return data;
}

uint8_t HTPA60x40::write_EEPROM_byte(uint16_t address, uint8_t content) {
    Wire.beginTransmission(EEPROM_ADDRESS_60x40);
    Wire.write((uint8_t)(address >> 8));   // MSB
    Wire.write((uint8_t)(address & 0xFF)); // LSB
    Wire.write(content);
    return Wire.endTransmission();
}

void HTPA60x40::read_eeprom() {
    ESP_LOGI(TAG, "Reading EEPROM calibration data...");
    
    // Read basic calibration values
    mbit_calib = read_EEPROM_byte(E_MBIT_CALIB_60x40);
    bias_calib = read_EEPROM_byte(E_BIAS_CALIB_60x40);
    clk_calib = read_EEPROM_byte(E_CLK_CALIB_60x40);
    bpa_calib = read_EEPROM_byte(E_BPA_CALIB_60x40);
    pu_calib = read_EEPROM_byte(E_PU_CALIB_60x40);
    
    // Read user settings (use calibration values if user values are not set)
    mbit_user = read_EEPROM_byte(E_MBIT_USER_60x40);
    if (mbit_user == 0xFF) mbit_user = mbit_calib;
    
    bias_user = read_EEPROM_byte(E_BIAS_USER_60x40);
    if (bias_user == 0xFF) bias_user = bias_calib;
    
    clk_user = read_EEPROM_byte(E_CLK_USER_60x40);
    if (clk_user == 0xFF) clk_user = clk_calib;
    
    bpa_user = read_EEPROM_byte(E_BPA_USER_60x40);
    if (bpa_user == 0xFF) bpa_user = bpa_calib;
    
    pu_user = read_EEPROM_byte(E_PU_USER_60x40);
    if (pu_user == 0xFF) pu_user = pu_calib;
    
    // Read other calibration parameters
    gradscale = read_EEPROM_byte(E_GRADSCALE_60x40);
    vddscgrad = read_EEPROM_byte(E_VDDSCGRAD_60x40);
    vddscoff = read_EEPROM_byte(E_VDDSCOFF_60x40);
    epsilon = read_EEPROM_byte(E_EPSILON_60x40);
    
    // Read table number
    tablenumber = (read_EEPROM_byte(E_TABLENUMBER2_60x40) << 8) | read_EEPROM_byte(E_TABLENUMBER1_60x40);
    
    // Read VDD and PTAT thresholds
    vddth1 = (read_EEPROM_byte(E_VDDTH1_2_60x40) << 8) | read_EEPROM_byte(E_VDDTH1_1_60x40);
    vddth2 = (read_EEPROM_byte(E_VDDTH2_2_60x40) << 8) | read_EEPROM_byte(E_VDDTH2_1_60x40);
    ptatth1 = (read_EEPROM_byte(E_PTATTH1_2_60x40) << 8) | read_EEPROM_byte(E_PTATTH1_1_60x40);
    ptatth2 = (read_EEPROM_byte(E_PTATTH2_2_60x40) << 8) | read_EEPROM_byte(E_PTATTH2_1_60x40);
    
    // Read PTAT gradient and offset
    ptatgr = (read_EEPROM_byte(E_PTATGR_2_60x40) << 8) | read_EEPROM_byte(E_PTATGR_1_60x40);
    
    // Read PixC min and max values
    uint8_t pixc_bytes[4];
    pixc_bytes[0] = read_EEPROM_byte(E_PIXCMIN_1_60x40);
    pixc_bytes[1] = read_EEPROM_byte(E_PIXCMIN_2_60x40);
    pixc_bytes[2] = read_EEPROM_byte(E_PIXCMIN_3_60x40);
    pixc_bytes[3] = read_EEPROM_byte(E_PIXCMIN_4_60x40);
    memcpy(&pixcmin, pixc_bytes, 4);
    
    pixc_bytes[0] = read_EEPROM_byte(E_PIXCMAX_1_60x40);
    pixc_bytes[1] = read_EEPROM_byte(E_PIXCMAX_2_60x40);
    pixc_bytes[2] = read_EEPROM_byte(E_PIXCMAX_3_60x40);
    pixc_bytes[3] = read_EEPROM_byte(E_PIXCMAX_4_60x40);
    memcpy(&pixcmax, pixc_bytes, 4);
    
    ESP_LOGI(TAG, "EEPROM data read completed");
    ESP_LOGI(TAG, "Table number: %d, Epsilon: %d", tablenumber, epsilon);
    ESP_LOGI(TAG, "PixC range: %.2f to %.2f", pixcmin, pixcmax);
}

void HTPA60x40::write_calibration_settings_to_sensor() {
    ESP_LOGI(TAG, "Writing calibration settings to sensor...");
    
    // Write trim registers with calibration values
    write_sensor_byte(TRIM_REGISTER1_60x40, mbit_calib);
    vTaskDelay(pdMS_TO_TICKS(5));
    
    write_sensor_byte(TRIM_REGISTER2_60x40, bias_calib);
    vTaskDelay(pdMS_TO_TICKS(5));
    
    write_sensor_byte(TRIM_REGISTER3_60x40, bias_calib);
    vTaskDelay(pdMS_TO_TICKS(5));
    
    write_sensor_byte(TRIM_REGISTER4_60x40, clk_calib);
    vTaskDelay(pdMS_TO_TICKS(5));
    
    write_sensor_byte(TRIM_REGISTER5_60x40, bpa_calib);
    vTaskDelay(pdMS_TO_TICKS(5));
    
    write_sensor_byte(TRIM_REGISTER6_60x40, bpa_calib);
    vTaskDelay(pdMS_TO_TICKS(5));
    
    write_sensor_byte(TRIM_REGISTER7_60x40, pu_calib);
    vTaskDelay(pdMS_TO_TICKS(5));
    
    ESP_LOGI(TAG, "Calibration settings written to sensor");
}

void HTPA60x40::calcPixC() {
    ESP_LOGI(TAG, "Calculating pixel constants for %d pixels...", NUMBER_OF_PIXEL_60x40);
    
    // Read pixel constants from EEPROM
    uint16_t eeprom_addr = E_PIXCIJ_60x40;
    
    for (int i = 0; i < NUMBER_OF_PIXEL_60x40; i++) {
        uint8_t pixc_bytes[4];
        
        // Read 4 bytes for each pixel constant
        for (int j = 0; j < 4; j++) {
            pixc_bytes[j] = read_EEPROM_byte(eeprom_addr + i * 4 + j);
        }
        
        // Convert bytes to uint32_t
        _pixc2_0[i] = (uint32_t)pixc_bytes[0] | 
                     ((uint32_t)pixc_bytes[1] << 8) | 
                     ((uint32_t)pixc_bytes[2] << 16) | 
                     ((uint32_t)pixc_bytes[3] << 24);
    }
    
    ESP_LOGI(TAG, "Pixel constants calculation completed");
}

uint16_t HTPA60x40::calc_timert(uint8_t clk, uint8_t mbit) {
    // Calculate timer value based on clock and mbit settings
    // This is a simplified calculation - actual implementation may vary
    uint16_t timer_val = 1000 + (clk * 10) + (mbit * 5);
    return timer_val;
}

void HTPA60x40::pixel_masking() {
    // Apply dead pixel masking
    for (int i = 0; i < nrofdefpix && i < ALLOWED_DEADPIX_60x40; i++) {
        uint16_t pixel_addr = deadpixadr[i];
        if (pixel_addr < NUMBER_OF_PIXEL_60x40) {
            int col = pixel_addr / PIXEL_PER_ROW_60x40;
            int row = pixel_addr % PIXEL_PER_ROW_60x40;
            
            if (col < PIXEL_PER_COLUMN_60x40 && row < PIXEL_PER_ROW_60x40) {
                // Interpolate from neighboring pixels
                uint32_t sum = 0;
                int count = 0;
                
                for (int dc = -1; dc <= 1; dc++) {
                    for (int dr = -1; dr <= 1; dr++) {
                        int nc = col + dc;
                        int nr = row + dr;
                        
                        if (nc >= 0 && nc < PIXEL_PER_COLUMN_60x40 && 
                            nr >= 0 && nr < PIXEL_PER_ROW_60x40 && 
                            (dc != 0 || dr != 0)) {
                            sum += data_pixel[nc][nr];
                            count++;
                        }
                    }
                }
                
                if (count > 0) {
                    data_pixel[col][row] = sum / count;
                }
            }
        }
    }
}

void HTPA60x40::readblockinterrupt() {
    // Read sensor status
    read_sensor_register(STATUS_REGISTER_60x40, &statusreg, 1);
    
    // Check if data is ready
    if (!(statusreg & 0x01)) { // EOC bit
        return;
    }
    
    // Read data blocks
    uint8_t block_select = (read_block_num << 4) | 0x0A; // TOP_HALF with block selection
    
    // Read the selected block
    read_sensor_register(block_select, RAMoutput[read_block_num], BLOCK_LENGTH_60x40);
    
    // Move to next block
    read_block_num++;
    if (read_block_num >= NUMBER_OF_BLOCKS_60x40 * 2) {
        read_block_num = START_WITH_BLOCK_60x40;
    }
    
    // Start next conversion
    uint8_t config_reg = (read_block_num << 4) | 0x01; // Start bit
    write_sensor_byte(CONFIGURATION_REGISTER_60x40, config_reg);
}

void HTPA60x40::sort_data() {
    // Sort the raw data into pixel array
    for (int block = 0; block < NUMBER_OF_BLOCKS_60x40 * 2; block++) {
        for (int pixel_in_block = 0; pixel_in_block < PIXEL_PER_BLOCK_60x40; pixel_in_block++) {
            int data_pos = DATA_POS_60x40 + pixel_in_block * 2;
            
            if (data_pos + 1 < BLOCK_LENGTH_60x40) {
                uint16_t pixel_value = (RAMoutput[block][data_pos + 1] << 8) | RAMoutput[block][data_pos];
                
                // Calculate pixel position in the array
                int pixel_index = block * PIXEL_PER_BLOCK_60x40 + pixel_in_block;
                int col = pixel_index / PIXEL_PER_ROW_60x40;
                int row = pixel_index % PIXEL_PER_ROW_60x40;
                
                if (col < PIXEL_PER_COLUMN_60x40 && row < PIXEL_PER_ROW_60x40) {
                    data_pixel[col][row] = pixel_value;
                }
            }
        }
    }
}

void HTPA60x40::calculate_pixel_temp() {
    // Apply pixel masking for dead pixels
    pixel_masking();
    
    // Additional temperature calculation would go here
    // This would involve using the lookup table and calibration data
    // For now, we'll keep the raw ADC values
    
    ESP_LOGD(TAG, "Pixel temperature calculation completed");
}