Add MLX90620 driver

This commit is contained in:
iabdalkader 2014-08-18 10:26:32 +02:00
parent 563ab52eca
commit 5722436a63
6 changed files with 256 additions and 2 deletions

View File

@ -186,6 +186,7 @@ OBJ += $(addprefix $(BUILD)/$(OMV_DIR)/py/, \
py_select.o \
py_gpio.o \
py_spi.o \
mlx90620.o \
)
ifeq ($(TARGET), OPENMV1)

@ -1 +1 @@
Subproject commit fa1eeeb2d51474016272963083e3b58f6271d59e
Subproject commit 6f78a8b2b3a96f1faa0261f7b044992aa65a39bb

View File

@ -52,6 +52,7 @@ SRCS += $(addprefix py/, \
py_select.c \
py_gpio.c \
py_spi.c \
mlx90620.c \
)
OBJS = $(addprefix $(BUILD)/, $(SRCS:.c=.o))

View File

@ -45,6 +45,8 @@
#include "py_gpio.h"
#include "py_spi.h"
#include "mlx90620.h"
int errno;
static FATFS fatfs0;
static FATFS fatfs1;
@ -172,6 +174,7 @@ static const module_t exported_modules[] ={
// {MP_QSTR_select, py_select_init},
{MP_QSTR_spi, py_spi_init},
{MP_QSTR_gpio, py_gpio_init},
{MP_QSTR_mlx, py_mlx90620_init},
{0, NULL}
};
@ -183,6 +186,7 @@ int main(void) {
- Global MSP (MCU Support Package) initialization
*/
HAL_Init();
#ifdef OPENMV2
if (sdram_init() == false) {
__fatal_error("could not init sdram");
@ -351,7 +355,7 @@ soft_reset:
mp_store_global(qstr_from_str("Image"), mp_make_function_n(1, py_image_load_image));
mp_store_global(qstr_from_str("HaarCascade"), mp_make_function_n(1, py_image_load_cascade));
mp_store_global(qstr_from_str("cpu_freq"), mp_make_function_n(0, py_cpu_freq));
// mp_store_global(qstr_from_str("info"), mp_make_function_n(0, py_info));
//mp_store_global(qstr_from_str("info"), mp_make_function_var(0, py_info));
// mp_store_global(qstr_from_str("gc_collect"), mp_make_function_n(0, py_gc_collect));
/* Export Python modules to the global python namespace */

243
src/omv/py/mlx90620.c Normal file
View File

@ -0,0 +1,243 @@
#include <mp.h>
#include <stdbool.h>
#include <float.h>
#include "systick.h"
#include "soft_i2c.h"
#include "mdefs.h"
#include "fmath.h"
#include "xalloc.h"
#include "py_image.h"
#include "mlx90620.h"
#define MLX_SLAVE_ADDR (0xC0)
#define MLX_EEPROM_ADDR (0xA0)
#define OSC_TRIM_OFFSET (0xF7)
#define REG_EEPROM_DATA (0x00)
// MLX commands
#define WRITE_OSC_TRIM (0x04)
#define SET_CONFIG_DATA (0x03)
#define MLX_READ_REG (0x02)
#define CAL_ACP 0xD4
#define CAL_BCP 0xD5
#define CAL_TGC 0xD8
#define CAL_alphaCP_L 0xD6
#define CAL_alphaCP_H 0xD7
#define CAL_BI_SCALE 0xD9
#define VTH_L 0xDA
#define VTH_H 0xDB
#define KT1_L 0xDC
#define KT1_H 0xDD
#define KT2_L 0xDE
#define KT2_H 0xDF
//Common sensitivity coefficients
#define CAL_A0_L 0xE0
#define CAL_A0_H 0xE1
#define CAL_A0_SCALE 0xE2
#define CAL_DELTA_A_SCALE 0xE3
#define CAL_EMIS_L 0xE4
#define CAL_EMIS_H 0xE5
#define MAP(OldValue, OldMin, OldMax, NewMin, NewMax)\
(((OldValue - OldMin) * (NewMax - NewMin)) / (OldMax - OldMin)) + NewMin
static const float alpha_ij[64] = {
1.60499E-8f, 1.87856E-8f, 1.93677E-8f, 1.87856E-8f, 1.83782E-8f, 2.11139E-8f, 2.21035E-8f, 2.07647E-8f,
2.01826E-8f, 2.30930E-8f, 2.38497E-8f, 2.23363E-8f, 2.19288E-8f, 2.52467E-8f, 2.58287E-8f, 2.46646E-8f,
2.26855E-8f, 2.66436E-8f, 2.68183E-8f, 2.54213E-8f, 2.40825E-8f, 2.72257E-8f, 2.81570E-8f, 2.62362E-8f,
2.48392E-8f, 2.81570E-8f, 2.89720E-8f, 2.68183E-8f, 2.50720E-8f, 2.83899E-8f, 2.87973E-8f, 2.72257E-8f,
2.52467E-8f, 2.85645E-8f, 2.91466E-8f, 2.72257E-8f, 2.52467E-8f, 2.83899E-8f, 2.85645E-8f, 2.72257E-8f,
2.58287E-8f, 2.81570E-8f, 2.83899E-8f, 2.64108E-8f, 2.46646E-8f, 2.74003E-8f, 2.81570E-8f, 2.58287E-8f,
2.42571E-8f, 2.66436E-8f, 2.68183E-8f, 2.54213E-8f, 2.26855E-8f, 2.56541E-8f, 2.56541E-8f, 2.40825E-8f,
2.15214E-8f, 2.38497E-8f, 2.40825E-8f, 2.21035E-8f, 1.99498E-8f, 2.19288E-8f, 2.16960E-8f, 2.01826E-8f,
};
// These are constants calculated from
// the calibration data stored in EEPROM
float k_t1, k_t1_sq, k_t2, emissivity;
int v_th, a_cp, b_cp, tgc, b_i_scale;
int8_t a_ij[64], b_ij[64];
static float calculate_TA(void)
{
uint16_t ptat=0;
uint8_t cmd_buf[4]={MLX_READ_REG, 0x90, 0x00, 0x01};
soft_i2c_write_bytes(MLX_SLAVE_ADDR, cmd_buf, sizeof(cmd_buf), false);
soft_i2c_read_bytes(MLX_SLAVE_ADDR, (uint8_t*)&ptat, 2, true);
return (-k_t1 + fast_sqrtf(k_t1_sq - (4 * k_t2 * (v_th - ptat)))) / (2 * k_t2) + 25;
}
static void mlx90620_read_to(float *t)
{
float v_ir_comp;
float v_ir_off_comp;
float v_ir_tgc_comp;
int16_t cpix;
uint8_t cmd_buf[4];
int16_t ir_data[64];
// static int count=0;
// if (count++ %16 ==0) {
float Ta = calculate_TA();
// (T+273.15f)^4
float Ta4 = (Ta + 273.15f) * (Ta + 273.15f) * (Ta + 273.15f) * (Ta + 273.15f);
// }
// Read IR data
memcpy(cmd_buf, (uint8_t [4]){MLX_READ_REG, 0x00, 0x01, 0x40}, sizeof(cmd_buf)); //read 64*2 bytes
soft_i2c_write_bytes(MLX_SLAVE_ADDR, cmd_buf, sizeof(cmd_buf), false);
soft_i2c_read_bytes(MLX_SLAVE_ADDR, (uint8_t*)ir_data, 128, true);
// Read compensation data
memcpy(cmd_buf, (uint8_t [4]){MLX_READ_REG, 0x91, 0x00, 0x01}, sizeof(cmd_buf));
soft_i2c_write_bytes(MLX_SLAVE_ADDR, cmd_buf, sizeof(cmd_buf), false);
soft_i2c_read_bytes(MLX_SLAVE_ADDR, (uint8_t*)&cpix, 2, true);
//Calculate the offset compensation for the one compensation pixel
//This is a constant in the TO calculation, so calculate it here.
float v_cp_off_comp = (float)cpix - (a_cp + (b_cp/(2<<(b_i_scale-1))) * (Ta - 25));
for (int i=0; i<64; i++) {
//#1: Calculate Offset Compensation
v_ir_off_comp = ir_data[i] - (a_ij[i] + (float)(b_ij[i]/(2<<(b_i_scale-1))) * (Ta - 25));
//#2: Calculate Thermal Gradien Compensation (TGC)
v_ir_tgc_comp = v_ir_off_comp - ( ((float)tgc/32) * v_cp_off_comp);
//#3: Calculate Emissivity Compensation
v_ir_comp = v_ir_tgc_comp / emissivity;
t[i] = fast_sqrtf(fast_sqrtf(v_ir_comp/alpha_ij[i] + Ta4)) - 273.15f;
}
}
mp_obj_t mlx90620_read()
{
float temp[64];
float max_temp = FLT_MIN;
float min_temp = FLT_MAX;
image_t img = {
.w=4,
.h=16,
.bpp=1,
.pixels=xalloc(16*4)
};
// get raw temperatures
mlx90620_read_to(temp);
// normalize temp readings
for (int i=0; i<64; i++) {
if (temp[i] > max_temp) {
max_temp = temp[i];
} else if (temp[i] < min_temp) {
min_temp = temp[i];
}
}
for (int i=0; i<64; i++) {
img.pixels[i] = (uint8_t)(((temp[i]-min_temp)/(max_temp-min_temp))*255.0f);
}
return py_image_from_struct(&img);
}
mp_obj_t mlx90620_read_raw()
{
float *t = m_new(float, 64);
mp_obj_t t_list = mp_obj_new_list(0, NULL);
// get raw temperatures
mlx90620_read_to(t);
// normalize temp readings
for (int i=0; i<64; i++) {
mp_obj_list_append(t_list, mp_obj_new_float(t[i]));
}
return t_list;
}
mp_obj_t mlx90620_init()
{
uint8_t cmd_buf[5];
uint8_t EEPROM_DATA[256];
// Init I2C
soft_i2c_init();
// Read EEPROM data
cmd_buf[0]=REG_EEPROM_DATA;
soft_i2c_write_bytes(MLX_EEPROM_ADDR, cmd_buf, 1, false);
soft_i2c_read_bytes(MLX_EEPROM_ADDR, EEPROM_DATA, 256, true);
// Write oscillator trimming value
uint8_t trim = EEPROM_DATA[OSC_TRIM_OFFSET];
memcpy(cmd_buf, (uint8_t [5]){WRITE_OSC_TRIM, (uint8_t)(trim-0xAA), trim, 0x56, 0x00}, 5);
soft_i2c_write_bytes(MLX_SLAVE_ADDR, cmd_buf, sizeof(cmd_buf), true);
// Write configuration register
uint8_t lsb = 0x0A; //0x09==16Hz
uint8_t msb = 0x74;
memcpy(cmd_buf, (uint8_t [5]){SET_CONFIG_DATA, (uint8_t)(lsb-0x55), lsb, (uint8_t)(msb-0x55), msb}, 5);
soft_i2c_write_bytes(MLX_SLAVE_ADDR, cmd_buf, sizeof(cmd_buf), true);
// Calculate Ta constants
v_th = (256 * EEPROM_DATA[VTH_H] + EEPROM_DATA[VTH_L]);
k_t1 = (256 * EEPROM_DATA[KT1_H] + EEPROM_DATA[KT1_L]) / 1024.0f;
k_t2 = (256 * EEPROM_DATA[KT2_H] + EEPROM_DATA[KT2_L]) / 1048576.0f;
emissivity = ((unsigned int)256 * EEPROM_DATA[CAL_EMIS_H] + EEPROM_DATA[CAL_EMIS_L]) / 32768.0f;
k_t1_sq = k_t1 * k_t1;
a_cp = (int8_t)EEPROM_DATA[CAL_ACP];
b_cp = (int8_t)EEPROM_DATA[CAL_BCP];
tgc = (int8_t)EEPROM_DATA[CAL_TGC];
b_i_scale = EEPROM_DATA[CAL_BI_SCALE];
// Hack
for (int i=0; i<8; i++) {
EEPROM_DATA[i]=EEPROM_DATA[i+8];
EEPROM_DATA[i+64]=EEPROM_DATA[i+8+64];
}
for (int i=0; i<64; i++) {
// Read pixel offsets
a_ij[i] = (int8_t)EEPROM_DATA[i];
// Read slope coefficients
b_ij[i] = (int8_t)EEPROM_DATA[i+4];
}
return mp_const_true;
}
STATIC MP_DEFINE_CONST_FUN_OBJ_0(mlx90620_init_obj, mlx90620_init);
STATIC MP_DEFINE_CONST_FUN_OBJ_0(mlx90620_read_obj, mlx90620_read);
STATIC MP_DEFINE_CONST_FUN_OBJ_0(mlx90620_read_raw_obj, mlx90620_read_raw);
static const mp_map_elem_t globals_dict_table[] = {
{ MP_OBJ_NEW_QSTR(MP_QSTR___name__), MP_OBJ_NEW_QSTR(MP_QSTR_mlx) },
//{ MP_OBJ_NEW_QSTR(MP_QSTR_HZ_8), MP_OBJ_NEW_SMALL_INT(MLX_HZ_8)},
//{ MP_OBJ_NEW_QSTR(MP_QSTR_HZ_16), MP_OBJ_NEW_SMALL_INT(MLX_HZ_16)},
//{ MP_OBJ_NEW_QSTR(MP_QSTR_HZ_32), MP_OBJ_NEW_SMALL_INT(MLX_HZ_32)},
//{ MP_OBJ_NEW_QSTR(MP_QSTR_HZ_64), MP_OBJ_NEW_SMALL_INT(MLX_HZ_64)},
{ MP_OBJ_NEW_QSTR(MP_QSTR_init), (mp_obj_t)&mlx90620_init_obj },
{ MP_OBJ_NEW_QSTR(MP_QSTR_read), (mp_obj_t)&mlx90620_read_obj },
{ MP_OBJ_NEW_QSTR(MP_QSTR_read_raw), (mp_obj_t)&mlx90620_read_raw_obj },
};
STATIC MP_DEFINE_CONST_DICT(globals_dict, globals_dict_table);
static const mp_obj_module_t mlx_module = {
.base = { &mp_type_module },
.name = MP_QSTR_mlx,
.globals = (mp_obj_t)&globals_dict,
};
const mp_obj_module_t *py_mlx90620_init()
{
return &mlx_module;
}

5
src/omv/py/mlx90620.h Normal file
View File

@ -0,0 +1,5 @@
#ifndef __MLX90620_H__
#define __MLX90620_H__
const mp_obj_module_t *py_mlx90620_init();
#endif /* __PY_mlx_H__ */