mirror of https://github.com/arendst/Tasmota.git
1641 lines
43 KiB
C++
1641 lines
43 KiB
C++
/**
|
|
* @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 <Wire.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 ValidateFrameData(uint16_t *frameData);
|
|
int ValidateAuxData(uint16_t *auxData);
|
|
int MLX90640_I2CRead(uint8_t addr, uint32_t reg, uint16_t len, uint16_t *reg_data);
|
|
int MLX90640_I2CWrite(uint8_t _deviceAddress, unsigned int writeAddress, uint16_t data);
|
|
|
|
// I2C
|
|
#define I2C_BUFFER_LENGTH 128
|
|
|
|
int MLX90640_I2CRead(uint8_t addr, uint32_t reg, uint16_t len, uint16_t *reg_data){
|
|
int bytesRemaining = len * 2;
|
|
int dataSpot = 0; //Start at beginning of array
|
|
while (bytesRemaining > 0)
|
|
{
|
|
Wire.beginTransmission(addr);
|
|
Wire.write(reg >> 8); //MSB
|
|
Wire.write(reg & 0xFF); //LSB
|
|
if (Wire.endTransmission(false) != 0) //Do not release bus
|
|
{
|
|
return (0); //Sensor did not ACK
|
|
}
|
|
int numberOfBytesToRead = bytesRemaining;
|
|
if (numberOfBytesToRead > I2C_BUFFER_LENGTH) numberOfBytesToRead = I2C_BUFFER_LENGTH;
|
|
Wire.requestFrom((int)addr, numberOfBytesToRead);
|
|
if (Wire.available())
|
|
{
|
|
for (uint32_t x = 0 ; x < numberOfBytesToRead / 2; x++)
|
|
{
|
|
reg_data[dataSpot] = Wire.read() << 8; //MSB
|
|
reg_data[dataSpot] |= Wire.read(); //LSB
|
|
dataSpot++;
|
|
}
|
|
}
|
|
bytesRemaining -= numberOfBytesToRead;
|
|
reg += numberOfBytesToRead / 2;
|
|
}
|
|
return (0); //Success
|
|
}
|
|
|
|
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
|
|
return (-1);
|
|
}
|
|
uint16_t dataCheck;
|
|
MLX90640_I2CRead(_deviceAddress, writeAddress, 1, &dataCheck);
|
|
if (dataCheck != data)
|
|
{
|
|
return -2;
|
|
}
|
|
return (0); //Success
|
|
}
|
|
|
|
int MLX90640_DumpEE(uint8_t slaveAddr, uint16_t *eeData)
|
|
{
|
|
return MLX90640_I2CRead(slaveAddr, 0x2400, 832, eeData);
|
|
}
|
|
|
|
int MLX90640_SynchFrame(uint8_t slaveAddr)
|
|
{
|
|
uint16_t dataReady = 0;
|
|
uint16_t statusRegister;
|
|
int error = 1;
|
|
|
|
error = MLX90640_I2CWrite(slaveAddr, 0x8000, 0x0030);
|
|
if(error == -1)
|
|
{
|
|
return error;
|
|
}
|
|
|
|
while(dataReady == 0)
|
|
{
|
|
error = MLX90640_I2CRead(slaveAddr, 0x8000, 1, &statusRegister);
|
|
if(error != 0)
|
|
{
|
|
return error;
|
|
}
|
|
dataReady = statusRegister & 0x0008;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
// int MLX90640_TriggerMeasurement(uint8_t slaveAddr) // ATM not used in Tasmota
|
|
// {
|
|
// int error = 1;
|
|
// uint16_t ctrlReg;
|
|
|
|
// error = MLX90640_I2CRead(slaveAddr, 0x800D, 1, &ctrlReg);
|
|
|
|
// if ( error != 0)
|
|
// {
|
|
// return error;
|
|
// }
|
|
|
|
// ctrlReg |= 0x8000;
|
|
// error = MLX90640_I2CWrite(slaveAddr, 0x800D, ctrlReg);
|
|
|
|
// if ( error != 0)
|
|
// {
|
|
// return error;
|
|
// }
|
|
|
|
// // error = MLX90640_I2CGeneralReset();
|
|
|
|
// // if ( error != 0)
|
|
// // {
|
|
// // return error;
|
|
// // }
|
|
|
|
// error = MLX90640_I2CRead(slaveAddr, 0x800D, 1, &ctrlReg);
|
|
|
|
// if ( error != 0)
|
|
// {
|
|
// return error;
|
|
// }
|
|
|
|
// if ((ctrlReg & 0x8000) != 0)
|
|
// {
|
|
// return -9;
|
|
// }
|
|
|
|
// return 0;
|
|
// }
|
|
|
|
int MLX90640_GetFrameData(uint8_t slaveAddr, uint16_t *frameData)
|
|
{
|
|
uint16_t dataReady = 0;
|
|
uint16_t controlRegister1;
|
|
uint16_t statusRegister;
|
|
int error = 1;
|
|
uint16_t data[64];
|
|
uint8_t cnt = 0;
|
|
|
|
while(dataReady == 0)
|
|
{
|
|
error = MLX90640_I2CRead(slaveAddr, 0x8000, 1, &statusRegister);
|
|
if(error != 0)
|
|
{
|
|
return error;
|
|
}
|
|
dataReady = statusRegister & 0x0008;
|
|
}
|
|
|
|
error = MLX90640_I2CWrite(slaveAddr, 0x8000, 0x0030);
|
|
if(error == -1)
|
|
{
|
|
return error;
|
|
}
|
|
|
|
error = MLX90640_I2CRead(slaveAddr, 0x0400, 768, frameData);
|
|
if(error != 0)
|
|
{
|
|
return error;
|
|
}
|
|
|
|
error = MLX90640_I2CRead(slaveAddr, 0x0700, 64, data);
|
|
if(error != 0)
|
|
{
|
|
return error;
|
|
}
|
|
|
|
error = MLX90640_I2CRead(slaveAddr, 0x800D, 1, &controlRegister1);
|
|
frameData[832] = controlRegister1;
|
|
frameData[833] = statusRegister & 0x0001;
|
|
|
|
if(error != 0)
|
|
{
|
|
return error;
|
|
}
|
|
|
|
error = ValidateAuxData(data);
|
|
if(error == 0)
|
|
{
|
|
for(cnt=0; cnt<64; cnt++)
|
|
{
|
|
frameData[cnt+768] = data[cnt];
|
|
}
|
|
}
|
|
|
|
error = ValidateFrameData(frameData);
|
|
if (error != 0)
|
|
{
|
|
return error;
|
|
}
|
|
|
|
return frameData[833];
|
|
}
|
|
|
|
int ValidateFrameData(uint16_t *frameData)
|
|
{
|
|
uint8_t line = 0;
|
|
|
|
for(int i=0; i<768; i+=32)
|
|
{
|
|
if((frameData[i] == 0x7FFF) && (line%2 == frameData[833])) return -8;
|
|
line = line + 1;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
int ValidateAuxData(uint16_t *auxData)
|
|
{
|
|
|
|
if(auxData[0] == 0x7FFF) return -8;
|
|
|
|
for(int i=8; i<19; i++)
|
|
{
|
|
if(auxData[i] == 0x7FFF) return -8;
|
|
}
|
|
|
|
for(int i=20; i<23; i++)
|
|
{
|
|
if(auxData[i] == 0x7FFF) return -8;
|
|
}
|
|
|
|
for(int i=24; i<33; i++)
|
|
{
|
|
if(auxData[i] == 0x7FFF) return -8;
|
|
}
|
|
|
|
for(int i=40; i<51; i++)
|
|
{
|
|
if(auxData[i] == 0x7FFF) return -8;
|
|
}
|
|
|
|
for(int i=52; i<55; i++)
|
|
{
|
|
if(auxData[i] == 0x7FFF) return -8;
|
|
}
|
|
|
|
for(int i=56; i<64; i++)
|
|
{
|
|
if(auxData[i] == 0x7FFF) return -8;
|
|
}
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
int MLX90640_ExtractParameters(uint16_t *eeData, paramsMLX90640 *mlx90640, int _chunk) // Tasmota
|
|
{
|
|
int error = 0;
|
|
switch(_chunk){
|
|
case 0:
|
|
ExtractVDDParameters(eeData, mlx90640);
|
|
ExtractPTATParameters(eeData, mlx90640);
|
|
ExtractGainParameters(eeData, mlx90640);
|
|
ExtractTgcParameters(eeData, mlx90640);
|
|
ExtractResolutionParameters(eeData, mlx90640);
|
|
ExtractKsTaParameters(eeData, mlx90640);
|
|
ExtractKsToParameters(eeData, mlx90640);
|
|
break;
|
|
case 1:
|
|
ExtractCPParameters(eeData, mlx90640);
|
|
ExtractAlphaParameters(eeData, mlx90640);
|
|
break;
|
|
case 2:
|
|
ExtractOffsetParameters(eeData, mlx90640);
|
|
break;
|
|
case 3:
|
|
ExtractKtaPixelParameters(eeData, mlx90640);
|
|
break;
|
|
case 4:
|
|
ExtractKvPixelParameters(eeData, mlx90640);
|
|
break;
|
|
case 5:
|
|
ExtractCILCParameters(eeData, mlx90640);
|
|
error = ExtractDeviatingPixels(eeData, mlx90640);
|
|
break;
|
|
}
|
|
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, uint8_t _part)
|
|
{
|
|
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));
|
|
}
|
|
|
|
uint32_t _offset = _part*(768/2);
|
|
for( int pixelNumber = _offset; pixelNumber < (_offset+(768/2)); 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 < 32767.4)
|
|
{
|
|
temp = temp*2;
|
|
alphaScale = alphaScale + 1;
|
|
}
|
|
|
|
for(int i = 0; i < 768; i++)
|
|
{
|
|
temp = alphaTemp[i] * pow(2,(double)alphaScale);
|
|
mlx90640->alpha[i] = (temp + 0.5);
|
|
|
|
}
|
|
|
|
mlx90640->alphaScale = alphaScale;
|
|
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
|
|
void 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 < 63.4)
|
|
{
|
|
temp = temp*2;
|
|
ktaScale1 = ktaScale1 + 1;
|
|
}
|
|
|
|
for(int i = 0; i < 768; i++)
|
|
{
|
|
temp = ktaTemp[i] * pow(2,(double)ktaScale1);
|
|
if (temp < 0)
|
|
{
|
|
mlx90640->kta[i] = (temp - 0.5);
|
|
}
|
|
else
|
|
{
|
|
mlx90640->kta[i] = (temp + 0.5);
|
|
}
|
|
|
|
}
|
|
|
|
mlx90640->ktaScale = ktaScale1;
|
|
}
|
|
|
|
|
|
//------------------------------------------------------------------------------
|
|
|
|
void 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 < 63.4)
|
|
{
|
|
temp = temp*2;
|
|
kvScale = kvScale + 1;
|
|
}
|
|
|
|
for(int i = 0; i < 768; i++)
|
|
{
|
|
temp = kvTemp[i] * pow(2,(double)kvScale);
|
|
if (temp < 0)
|
|
{
|
|
mlx90640->kv[i] = (temp - 0.5);
|
|
}
|
|
else
|
|
{
|
|
mlx90640->kv[i] = (temp + 0.5);
|
|
}
|
|
|
|
}
|
|
|
|
mlx90640->kvScale = kvScale;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
|
|
void 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;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|