mirror of
https://github.com/eddyem/stm32samples.git
synced 2025-12-06 10:45:11 +03:00
579 lines
21 KiB
C
579 lines
21 KiB
C
/*
|
|
* This file is part of the MLX90640 project.
|
|
* Copyright 2022 Edward V. Emelianov <edward.emelianoff@gmail.com>.
|
|
*
|
|
* This program is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include <math.h>
|
|
#include "hardware.h"
|
|
#include "i2c.h"
|
|
#include "mlx90640.h"
|
|
#include "mlx90640_regs.h"
|
|
#include "strfunct.h"
|
|
|
|
#ifdef EBUG
|
|
extern volatile uint32_t Tms;
|
|
#endif
|
|
|
|
mlx90640_state mlx_state = M_ERROR;
|
|
|
|
MLX90640_params params;
|
|
|
|
#if REG_CALIBRDATA_LEN > MLX_DMA_MAXLEN || MLX_PIXARRSZ > MLX_DMA_MAXLEN
|
|
#error "MLX_DMA_MAXLEN should be >= REG_CALIBRDATA_LEN"
|
|
#endif
|
|
static uint16_t dataarray[MLX_DMA_MAXLEN]; // array for raw data from sensor
|
|
static int portionlen = 0; // data length in `dataarray`
|
|
float mlx_image[MLX_PIXNO]; // ready image
|
|
|
|
#define CREG_VAL(reg) dataarray[CREG_IDX(reg)]
|
|
#define IMD_VAL(reg) dataarray[IMD_IDX(reg)]
|
|
|
|
static uint8_t simpleimage = 0; // ==1 not to calibrate T
|
|
static uint8_t subpageno = 0; // subpage number
|
|
|
|
// reg_control values for subpage #0 and #1
|
|
static const uint16_t reg_control_val[2] = {
|
|
REG_CONTROL_CHESS | REG_CONTROL_RES18 | REG_CONTROL_REFR_2HZ | REG_CONTROL_SUBPSEL | REG_CONTROL_DATAHOLD | REG_CONTROL_SUBPEN,
|
|
REG_CONTROL_CHESS | REG_CONTROL_RES18 | REG_CONTROL_REFR_2HZ | REG_CONTROL_SUBP1 | REG_CONTROL_SUBPSEL | REG_CONTROL_DATAHOLD | REG_CONTROL_SUBPEN
|
|
};
|
|
|
|
// read register value
|
|
int read_reg(uint16_t reg, uint16_t *val){
|
|
reg = __REV16(reg);
|
|
if(I2C_OK != i2c_7bit_send((uint8_t*)®, 2, 0)){
|
|
DBG("Can't send address");
|
|
return FALSE;
|
|
}
|
|
uint16_t d;
|
|
i2c_status s = i2c_7bit_receive_twobytes((uint8_t*)&d);
|
|
if(I2C_OK != s){
|
|
#ifdef EBUG
|
|
DBG("Can't get info, s=");
|
|
printu(s); NL();
|
|
#endif
|
|
return FALSE;
|
|
}
|
|
*val = __REV16(d);
|
|
return TRUE;
|
|
}
|
|
|
|
// blocking read N uint16_t values starting from `reg`
|
|
// @param reg - register to read
|
|
// @param N (io) - amount of bytes to read / bytes read
|
|
// @return `dataarray` or NULL if failed
|
|
uint16_t *read_data(uint16_t reg, uint16_t *N){
|
|
uint16_t n = *N;
|
|
if(n < 1 || n > MLX_DMA_MAXLEN) return NULL;
|
|
uint16_t i, *data = dataarray;
|
|
#ifdef EBUG
|
|
SEND("Tms="); printu(Tms); newline();
|
|
#endif
|
|
for(i = 0; i < n; ++i){
|
|
if(!read_reg(reg++, data++)){
|
|
DBG("can't read");
|
|
break;
|
|
}
|
|
}
|
|
#ifdef EBUG
|
|
SEND("Tms="); printu(Tms); newline();
|
|
#endif
|
|
*N = i;
|
|
return dataarray;
|
|
}
|
|
|
|
// write register value
|
|
int write_reg(uint16_t reg, uint16_t val){
|
|
// little endian -> big endian
|
|
uint8_t _4bytes[4];
|
|
_4bytes[0] = reg >> 8;
|
|
_4bytes[1] = reg & 0xff;
|
|
_4bytes[2] = val >> 8;
|
|
_4bytes[3] = val & 0xff;
|
|
if(I2C_OK != i2c_7bit_send(_4bytes, 4, 1)) return FALSE;
|
|
return TRUE;
|
|
}
|
|
|
|
/**
|
|
* @brief read_data_dma - read big data buffer by DMA
|
|
* @param reg - starting register number
|
|
* @param N - amount of data (in 16-bit words)
|
|
* @return FALSE if can't run operation
|
|
*/
|
|
int read_data_dma(uint16_t reg, int N){
|
|
if(N < 1 || N > MLX_DMA_MAXLEN) return FALSE;
|
|
/*uint8_t _2bytes[2];
|
|
_2bytes[0] = reg >> 8; // big endian!
|
|
_2bytes[1] = reg & 0xff;*/
|
|
reg = __REV16(reg);
|
|
portionlen = N;
|
|
if(I2C_OK != i2c_7bit_send((uint8_t*)®, 2, 0)){
|
|
DBG("DMA: can't send address");
|
|
return FALSE;
|
|
}
|
|
if(I2C_OK != i2c_7bit_receive_DMA((uint8_t*)dataarray, N*2)) return FALSE;
|
|
return TRUE;
|
|
}
|
|
|
|
/*****************************************************************************
|
|
Calculate parameters & values
|
|
*****************************************************************************/
|
|
// calculate Vdd from vddRAM register
|
|
/*
|
|
static float getVdd(uint16_t vddRAM){
|
|
int16_t ram = (int16_t) vddRAM;
|
|
float vdd = (float)ram - params.vdd25;
|
|
return vdd / params.kVdd + 3.3f;
|
|
}*/
|
|
|
|
// fill OCC/ACC row/col arrays
|
|
static void occacc(int8_t *arr, int l, uint16_t *regstart){
|
|
int n = l >> 2; // divide by 4
|
|
int8_t *p = arr;
|
|
for(int i = 0; i < n; ++i){
|
|
register uint16_t val = *regstart++;
|
|
*p++ = (val & 0x000F) >> 0;
|
|
*p++ = (val & 0x00F0) >> 4;
|
|
*p++ = (val & 0x0F00) >> 8;
|
|
*p++ = (val ) >> 12;
|
|
}
|
|
for(int i = 0; i < l; ++i, ++arr){
|
|
if(*arr > 0x07) *arr -= 0x10;
|
|
}
|
|
}
|
|
|
|
// get all parameters' values from `dataarray`, return FALSE if something failed
|
|
static int get_parameters(){
|
|
#ifdef EBUG
|
|
SEND("0 Tms="); printu(Tms); newline();
|
|
#endif
|
|
int8_t i8;
|
|
int16_t i16;
|
|
uint16_t *pu16;
|
|
uint16_t val = CREG_VAL(REG_VDD);
|
|
i8 = (int8_t) (val >> 8);
|
|
params.kVdd = i8 << 5;
|
|
if(params.kVdd == 0) return FALSE;
|
|
i16 = val & 0xFF;
|
|
params.vdd25 = ((i16 - 0x100) << 5) - (1<<13);
|
|
val = CREG_VAL(REG_KVTPTAT);
|
|
i16 = (val & 0xFC00) >> 10;
|
|
if(i16 > 0x1F) i16 -= 0x40;
|
|
params.KvPTAT = (float)i16 / (1<<12);
|
|
i16 = (val & 0x03FF);
|
|
if(i16 > 0x1FF) i16 -= 0x400;
|
|
params.KtPTAT = (float)i16 / 8.f;
|
|
params.vPTAT25 = (int16_t) CREG_VAL(REG_PTAT);
|
|
val = CREG_VAL(REG_APTATOCCS) >> 12;
|
|
params.alphaPTAT = val / 4.f + 8.f;
|
|
params.gainEE = (int16_t)CREG_VAL(REG_GAIN);
|
|
if(params.gainEE == 0) return FALSE;
|
|
#ifdef EBUG
|
|
SEND("1 Tms="); printu(Tms); newline();
|
|
#endif
|
|
int8_t occRow[MLX_H];
|
|
int8_t occColumn[MLX_W];
|
|
occacc(occRow, MLX_H, &CREG_VAL(REG_OCCROW14));
|
|
occacc(occColumn, MLX_W, &CREG_VAL(REG_OCCCOL14));
|
|
int8_t accRow[MLX_H];
|
|
int8_t accColumn[MLX_W];
|
|
occacc(accRow, MLX_H, &CREG_VAL(REG_ACCROW14));
|
|
occacc(accColumn, MLX_W, &CREG_VAL(REG_ACCCOL14));
|
|
val = CREG_VAL(REG_APTATOCCS);
|
|
// need to do multiplication instead of bitshift, so:
|
|
float occRemScale = 1<<(val&0x0F),
|
|
occColumnScale = 1<<((val>>4)&0x0F),
|
|
occRowScale = 1<<((val>>8)&0x0F);
|
|
int16_t offavg = (int16_t) CREG_VAL(REG_OSAVG);
|
|
// even/odd column/row numbers are for starting from 1, so for starting from 0 we chould swap them:
|
|
// even - for 1,3,5,...; odd - for 0,2,4,... etc
|
|
int8_t ktaavg[4];
|
|
// 0 - odd row, odd col; 1 - odd row even col; 2 - even row, odd col; 3 - even row, even col
|
|
val = CREG_VAL(REG_KTAAVGODDCOL);
|
|
ktaavg[2] = (int8_t)(val & 0xFF); // odd col, even row -> col 0,2,..; row 1,3,..
|
|
ktaavg[0] = (int8_t)(val >> 8);; // odd col, odd row -> col 0,2,..; row 0,2,..
|
|
val = CREG_VAL(REG_KTAAVGEVENCOL);
|
|
ktaavg[3] = (int8_t)(val & 0xFF); // even col, even row -> col 1,3,..; row 1,3,..
|
|
ktaavg[1] = (int8_t)(val >> 8); // even col, odd row -> col 1,3,..; row 0,2,..
|
|
// so index of ktaavg is 2*(row&1)+(col&1)
|
|
val = CREG_VAL(REG_KTAVSCALE);
|
|
uint8_t scale1 = ((val & 0xFF)>>4) + 8, scale2 = (val&0xF);
|
|
if(scale1 == 0 || scale2 == 0) return FALSE;
|
|
float mul = (float)(1<<scale2), div = (float)(1<<scale1); // kta_scales
|
|
uint16_t a_r = CREG_VAL(REG_SENSIVITY); // alpha_ref
|
|
val = CREG_VAL(REG_SCALEACC);
|
|
float *a = params.alpha, diva = (float)(val >> 12);
|
|
diva *= (float)(1<<30); // alpha_scale
|
|
float accRowScale = 1<<((val & 0x0f00)>>8),
|
|
accColumnScale = 1<<((val & 0x00f0)>>4),
|
|
accRemScale = 1<<(val & 0x0f);
|
|
pu16 = &CREG_VAL(REG_OFFAK1);
|
|
float *kta = params.kta, *offset = params.offset;
|
|
#ifdef EBUG
|
|
SEND("2 Tms="); printu(Tms); newline();
|
|
#endif
|
|
for(int row = 0; row < MLX_H; ++row){
|
|
int idx = (row&1)<<1;
|
|
for(int col = 0; col < MLX_W; ++col){
|
|
// offset
|
|
register uint16_t rv = *pu16++;
|
|
i16 = (rv & 0xFC00) >> 10;
|
|
if(i16 > 0x1F) i16 -= 0x40;
|
|
*offset++ = (float)offavg + (float)occRow[row]*occRowScale + (float)occColumn[col]*occColumnScale + (float)i16*occRemScale;
|
|
// kta
|
|
i16 = (rv & 0xF) >> 1;
|
|
if(i16 > 0x03) i16 -= 0x08;
|
|
*kta++ = (ktaavg[idx|(col&1)] + i16*mul) / div;
|
|
// alpha
|
|
i16 = (rv & 0x3F0) >> 4;
|
|
if(i16 > 0x1F) i16 -= 0x40;
|
|
float oft = (float)a_r + accRow[row]*accRowScale + accColumn[col]*accColumnScale +i16*accRemScale;
|
|
*a++ = oft / diva;
|
|
}
|
|
}
|
|
#ifdef EBUG
|
|
SEND("3 Tms="); printu(Tms); newline();
|
|
#endif
|
|
scale1 = (CREG_VAL(REG_KTAVSCALE) >> 8) & 0xF; // kvscale
|
|
div = (float)(1<<scale1);
|
|
val = CREG_VAL(REG_KVAVG);
|
|
i16 = val >> 12; if(i16 > 0x07) i16 -= 0x10;
|
|
ktaavg[0] = i16; // odd col, odd row
|
|
i16 = (val & 0xF0) >> 4; if(i16 > 0x07) i16 -= 0x10;
|
|
ktaavg[1] = i16; // even col, odd row
|
|
i16 = (val & 0x0F00) >> 8; if(i16 > 0x07) i16 -= 0x10;
|
|
ktaavg[2] = i16; // odd col, even row
|
|
i16 = val & 0x0F; if(i16 > 0x07) i16 -= 0x10;
|
|
ktaavg[3] = i16; // even col, even row
|
|
for(int i = 0; i < 4; ++i) params.kv[i] = ktaavg[i] / div;
|
|
val = CREG_VAL(REG_CPOFF);
|
|
params.cpOffset[0] = (val & 0x03ff);
|
|
if(params.cpOffset[0] > 0x1ff) params.cpOffset[0] -= 0x400;
|
|
params.cpOffset[1] = val >> 10;
|
|
if(params.cpOffset[1] > 0x1f) params.cpOffset[1] -= 0x40;
|
|
params.cpOffset[1] += params.cpOffset[0];
|
|
val = ((CREG_VAL(REG_KTAVSCALE) & 0xF0) >> 4) + 8;
|
|
i8 = (int8_t)(CREG_VAL(REG_KVTACP) & 0xFF);
|
|
params.cpKta = (float)i8 / (1<<val);
|
|
val = (CREG_VAL(REG_KTAVSCALE) & 0x0F00) >> 8;
|
|
i16 = CREG_VAL(REG_KVTACP) >> 8;
|
|
if(i16 > 0x7F) i16 -= 0x100;
|
|
params.cpKv = (float)i16 / (1<<val);
|
|
i16 = CREG_VAL(REG_KSTATGC) & 0xFF;
|
|
if(i16 > 0x7F) i16 -= 0x100;
|
|
params.tgc = (float)i16;
|
|
params.tgc /= 32.;
|
|
#ifdef EBUG
|
|
SEND("4 Tms="); printu(Tms); newline();
|
|
#endif
|
|
val = (CREG_VAL(REG_SCALEACC)>>12); // alpha_scale_CP
|
|
i16 = CREG_VAL(REG_ALPHA)>>10; // cp_P1_P0_ratio
|
|
if(i16 > 0x1F) i16 -= 0x40;
|
|
div = (float)(1<<val);
|
|
div *= (float)(1<<27);
|
|
params.cpAlpha[0] = (float)(CREG_VAL(REG_ALPHA) & 0x03FF) / div;
|
|
div = (float)(1<<7);
|
|
params.cpAlpha[1] = params.cpAlpha[0] * (1.f + (float)i16/div);
|
|
i8 = (int8_t)(CREG_VAL(REG_KSTATGC) >> 8);
|
|
params.KsTa = (float)i8/(1<<13);
|
|
div = 1<<((CREG_VAL(REG_CT34) & 0x0F) + 8); // kstoscale
|
|
val = CREG_VAL(REG_KSTO12);
|
|
i8 = (int8_t)(val & 0xFF);
|
|
params.ksTo[0] = 273.15f * i8 / div;
|
|
i8 = (int8_t)(val >> 8);
|
|
params.ksTo[1] = 273.15f * i8 / div;
|
|
val = CREG_VAL(REG_KSTO34);
|
|
i8 = (int8_t)(val & 0xFF);
|
|
params.ksTo[2] = 273.15f * i8 / div;
|
|
i8 = (int8_t)(val >> 8);
|
|
params.ksTo[3] = 273.15f * i8 / div;
|
|
params.CT[0] = 0.f; // 0degr - between ranges 1 and 2
|
|
val = CREG_VAL(REG_CT34);
|
|
mul = ((val & 0x3000)>>12)*10.f; // step
|
|
params.CT[1] = ((val & 0xF0)>>4)*mul; // CT3 - between ranges 2 and 3
|
|
params.CT[2] = ((val & 0x0F00) >> 8)*mul + params.CT[1]; // CT4 - between ranges 3 and 4
|
|
params.alphacorr[0] = 1.f/(1.f + params.ksTo[0] * 40.f);
|
|
params.alphacorr[1] = 1.f;
|
|
params.alphacorr[2] = (1.f + params.ksTo[2] * params.CT[1]);
|
|
params.alphacorr[3] = (1.f + params.ksTo[3] * (params.CT[2] - params.CT[1])) * params.alphacorr[2];
|
|
// Don't forget to check 'outlier' flags for wide purpose
|
|
#ifdef EBUG
|
|
SEND("end Tms="); printu(Tms);
|
|
NL();
|
|
#endif
|
|
return TRUE;
|
|
}
|
|
|
|
/**
|
|
* @brief process_subpage - calculate all parameters from `dataarray` into `mlx_image`
|
|
*/
|
|
static void process_subpage(){
|
|
DBG("process_subpage()");
|
|
SEND("Tms="); printu(Tms); newline();
|
|
SEND("subpage="); printu(subpageno); newline();
|
|
(void)subpageno; (void)simpleimage;
|
|
for(int i = 0; i < MLX_W; ++i){
|
|
printi((int8_t)dataarray[i]); bufputchar(' ');
|
|
} newline();
|
|
SEND("072a="); printuhex(IMD_VAL(REG_IVDDPIX));
|
|
SEND("\n0720="); printuhex(IMD_VAL(REG_ITAPTAT));
|
|
SEND("\n0700="); printuhex(IMD_VAL(REG_ITAVBE));
|
|
SEND("\n070a="); printuhex(IMD_VAL(REG_IGAIN)); newline();
|
|
int16_t i16a = (int16_t)IMD_VAL(REG_IVDDPIX);
|
|
float dvdd = i16a - params.vdd25;
|
|
dvdd = dvdd / params.kVdd;
|
|
SEND("Vd="); float2str(dvdd+3.3f, 2); newline();
|
|
i16a = (int16_t)IMD_VAL(REG_ITAPTAT);
|
|
int16_t i16b = (int16_t)IMD_VAL(REG_ITAVBE);
|
|
float dTa = (float)i16a / (i16a * params.alphaPTAT + i16b); // vptatart
|
|
dTa *= (float)(1<<18);
|
|
dTa = (dTa / (1 + params.KvPTAT*dvdd) - params.vPTAT25);
|
|
dTa = dTa / params.KtPTAT; // without 25degr - Ta0
|
|
SEND("Ta="); float2str(dTa+25., 2); newline();
|
|
i16a = (int16_t)IMD_VAL(REG_IGAIN);
|
|
float Kgain = params.gainEE / (float)i16a;
|
|
SEND("Kgain="); float2str(Kgain, 2); newline();
|
|
// now make first approximation to image
|
|
uint16_t pixno = 0; // current pixel number - for indexing in parameters etc
|
|
for(int row = 0; row < MLX_H; ++row){
|
|
int idx = (row&1)<<1; // index for params.kv
|
|
for(int col = 0; col < MLX_W; ++col, ++pixno){
|
|
uint8_t sp = (row&1)^(col&1); // subpage of current pixel
|
|
if(sp != subpageno) continue;
|
|
register float curval = (float)((int16_t)dataarray[pixno]) * Kgain; // gain compensation
|
|
curval -= params.offset[pixno] * (1.f + params.kta[pixno]*dTa) *
|
|
(1.f + params.kv[idx|(col&1)]*dvdd); // add offset
|
|
float IRcompens = curval; // IR_compensated
|
|
curval -= params.cpOffset[subpageno] * (1.f - params.cpKta * dTa) *
|
|
(1.f + params.cpKv * dvdd); // CP
|
|
if(!simpleimage){
|
|
curval = IRcompens - params.tgc * curval; // IR gradient compens
|
|
float alphaComp = params.alpha[pixno] - params.tgc * params.cpAlpha[subpageno];
|
|
alphaComp /= 1.f + params.KsTa * dTa;
|
|
// calculate To for basic range
|
|
float Tar = dTa + 273.15f + 25.f;
|
|
Tar = Tar*Tar*Tar*Tar;
|
|
float ac3 = alphaComp*alphaComp*alphaComp;
|
|
float Sx = ac3*IRcompens + alphaComp*ac3*Tar;
|
|
Sx = params.KsTa * sqrtf(sqrtf(Sx));
|
|
float To = IRcompens / (alphaComp * (1.f - params.ksTo[1]) + Sx) + Tar;
|
|
curval = sqrtf(sqrtf(To)) - 273.15; // To
|
|
// TODO: extended
|
|
}
|
|
mlx_image[pixno] = curval;
|
|
}
|
|
}
|
|
SEND("Tms="); printu(Tms); newline();
|
|
NL();
|
|
}
|
|
|
|
// start image acquiring for next subpage
|
|
static int startima(){
|
|
DBG("startima()");
|
|
// write `overwrite` flag twice
|
|
if(!write_reg(REG_CONTROL, reg_control_val[subpageno]) ||
|
|
!write_reg(REG_STATUS, REG_STATUS_OVWEN) ||
|
|
!write_reg(REG_STATUS, REG_STATUS_OVWEN)) return FALSE;
|
|
return TRUE;
|
|
}
|
|
|
|
/**
|
|
* @brief parse_buffer - swap bytes in `dataarray` (after receiving or before transmitting data)
|
|
*
|
|
static void parse_buffer(){
|
|
uint16_t *ptr = dataarray;
|
|
DBG("parse_buffer()");
|
|
for(uint16_t i = 0; i < portionlen; ++i, ++ptr){
|
|
*ptr = __REV16(*ptr);
|
|
#if 0
|
|
printu(i);
|
|
addtobuf(" ");
|
|
printuhex(*ptr);
|
|
newline();
|
|
#endif
|
|
}
|
|
#if 0
|
|
sendbuf();
|
|
#endif
|
|
}*/
|
|
|
|
/**
|
|
* @brief mlx90640_process - main finite-state machine
|
|
*/
|
|
void mlx90640_process(){
|
|
#define chstate(s) do{errctr = 0; Tlast = Tms; mlx_state = s;}while(0)
|
|
#define chkerr() do{if(++errctr > MLX_MAXERR_COUNT){chstate(M_ERROR); DBG("-> M_ERROR");}}while(0)
|
|
#define chktmout() do{if(Tms - Tlast > MLX_TIMEOUT){chstate(M_ERROR); DBG("Timeout! -> M_ERROR"); }}while(0)
|
|
static int errctr = 0;
|
|
static uint32_t Tlast = 0;
|
|
uint16_t reg, N;
|
|
/*
|
|
uint8_t gotdata = 0;
|
|
if(i2cDMAr == I2C_DMA_READY){ // convert received data into little-endian
|
|
i2cDMAr = I2C_DMA_RELAX;
|
|
parse_buffer();
|
|
gotdata = 1;
|
|
}*/
|
|
switch(mlx_state){
|
|
case M_FIRSTSTART: // init working mode by request
|
|
if(write_reg(REG_CONTROL, reg_control_val[0])
|
|
&& read_reg(REG_CONTROL, ®)){
|
|
SEND("REG_CTRL="); printuhex(reg); NL();
|
|
if(read_reg(REG_STATUS, ®)){
|
|
SEND("REG_STATUS="); printuhex(reg); NL();}
|
|
/*
|
|
#define PARTD 512
|
|
if(read_data_dma(REG_CALIDATA, PARTD)){
|
|
chstate(M_READCONF);
|
|
DBG("-> M_READCONF");
|
|
}else chkerr();
|
|
*/
|
|
N = REG_CALIDATA_LEN;
|
|
if(read_data(REG_CALIDATA, &N)){
|
|
chstate(M_READCONF);
|
|
DBG("-> M_READCONF");
|
|
}else chkerr();
|
|
}else chkerr();
|
|
break;
|
|
case M_READCONF:
|
|
//if(gotdata){ // calculate calibration parameters
|
|
/* uint16_t *d = &dataarray[PARTD];
|
|
for(uint16_t r = REG_CALIDATA+PARTD; r < REG_CALIDATA + REG_CALIDATA_LEN; ++r){
|
|
if(!read_reg(r, d++)){
|
|
chstate(M_FIRSTSTART);
|
|
DBG("can't read all confdata -> M_FIRSTSTART");
|
|
return;
|
|
}
|
|
}*/
|
|
if(get_parameters()){
|
|
chstate(M_RELAX);
|
|
DBG("-> M_RELAX");
|
|
}else{ // error -> go to M_FIRSTSTART again
|
|
chstate(M_FIRSTSTART);
|
|
DBG("-> M_FIRSTSTART");
|
|
}
|
|
//}else chktmout();
|
|
break;
|
|
case M_STARTIMA:
|
|
if(startima()){
|
|
chstate(M_PROCESS);
|
|
DBG("-> M_PROCESS");
|
|
}else{
|
|
chstate(M_ERROR);
|
|
DBG("can't start subpage -> M_ERROR");
|
|
}
|
|
break;
|
|
case M_PROCESS:
|
|
if(read_reg(REG_STATUS, ®)){
|
|
if(reg & REG_STATUS_NEWDATA){
|
|
if(subpageno != (reg & REG_STATUS_SPNO)){
|
|
chstate(M_ERROR);
|
|
DBG("wrong subpage number -> M_ERROR");
|
|
}else{ // all OK, run image reading
|
|
write_reg(REG_STATUS, 0); // clear rdy bit
|
|
/*
|
|
if(read_data_dma(REG_IMAGEDATA, PARTD)){
|
|
chstate(M_READOUT);
|
|
DBG("-> M_READOUT");
|
|
}else chkerr();
|
|
*/
|
|
N = MLX_PIXARRSZ;
|
|
if(read_data(REG_IMAGEDATA, &N)){
|
|
chstate(M_READOUT);
|
|
DBG("-> M_READOUT");
|
|
}else chkerr();
|
|
}
|
|
}else chktmout();
|
|
}else chkerr();
|
|
break;
|
|
case M_READOUT:
|
|
//if(gotdata){
|
|
/* uint16_t *d = &dataarray[PARTD];
|
|
for(uint16_t r = REG_IMAGEDATA+PARTD; r < REG_IMAGEDATA+MLX_PIXARRSZ; ++r){
|
|
if(!read_reg(r, d++)){
|
|
chstate(M_ERROR);
|
|
DBG("can't read all confdata -> M_ERROR");
|
|
return;
|
|
}
|
|
}*/
|
|
process_subpage();
|
|
subpageno = !subpageno;
|
|
DBG("Subpage ready");
|
|
chstate(M_RELAX);
|
|
/*
|
|
if(++subpageno > 1){ // image ready
|
|
subpageno = 0;
|
|
chstate(M_RELAX);
|
|
DBG("Image READY!");
|
|
}else{
|
|
chstate(M_STARTIMA);
|
|
DBG("-> M_STARTIMA");
|
|
}*/
|
|
//}else chktmout();
|
|
break;
|
|
case M_POWERON:
|
|
if(Tms - Tlast > MLX_POWON_WAIT){
|
|
if(params.kVdd == 0){ // get all parameters
|
|
chstate(M_FIRSTSTART);
|
|
DBG("M_FIRSTSTART");
|
|
}else{ // rewrite settings register
|
|
if(write_reg(REG_CONTROL, reg_control_val[0])){
|
|
chstate(M_RELAX);
|
|
DBG("-> M_RELAX");
|
|
}else chkerr();
|
|
}
|
|
}
|
|
break;
|
|
case M_POWEROFF1:
|
|
MLXPOW_OFF();
|
|
chstate(M_POWEROFF);
|
|
DBG("-> M_POWEROFF");
|
|
break;
|
|
case M_POWEROFF:
|
|
if(Tms - Tlast > MLX_POWOFF_WAIT){
|
|
MLXPOW_ON();
|
|
chstate(M_POWERON);
|
|
DBG("-> M_POWERON");
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
void mlx90640_restart(){
|
|
DBG("restart");
|
|
mlx_state = M_POWEROFF1;
|
|
}
|
|
|
|
// if state of MLX allows, make an image else return error
|
|
// @param simple ==1 for simplest image processing (without T calibration)
|
|
int mlx90640_take_image(uint8_t simple){
|
|
simpleimage = simple;
|
|
if(mlx_state == M_ERROR){
|
|
DBG("Restart I2C");
|
|
i2c_setup(i2cDMAr != I2C_DMA_NOTINIT);
|
|
} else if(mlx_state != M_RELAX) return FALSE;
|
|
if(params.kVdd == 0){ // no parameters -> make first run
|
|
mlx_state = M_FIRSTSTART;
|
|
DBG("no params -> M_FIRSTSTART");
|
|
return TRUE;
|
|
}
|
|
//subpageno = 0;
|
|
mlx_state = M_STARTIMA;
|
|
DBG("-> M_STARTIMA");
|
|
return TRUE;
|
|
}
|