use core::slice;
use arrayvec::ArrayVec;
use bytes::Buf;
use embedded_hal::blocking::i2c;
use crate::common::*;
use crate::error::{Error, LibraryError};
use crate::expose_member;
use crate::register::Subpage;
use super::address::EepromAddress;
use super::{NUM_PIXELS, WIDTH};
const NUM_CORNER_TEMPERATURES: usize = 4;
const BASIC_TEMPERATURE_RANGE: usize = 1;
const WORD_SIZE: usize = 16 / 8;
#[derive(Clone, Debug, PartialEq)]
pub struct Mlx90640Calibration {
k_v_dd: i16,
v_dd_25: i16,
resolution: u8,
k_v_ptat: i16,
k_t_ptat: i16,
v_ptat_25: i16,
alpha_ptat: u16,
gain: i16,
k_s_ta: f32,
corner_temperatures: [i16; NUM_CORNER_TEMPERATURES],
k_s_to: [f32; NUM_CORNER_TEMPERATURES],
alpha_correction: [f32; NUM_CORNER_TEMPERATURES],
alpha_pixels: [f32; NUM_PIXELS],
alpha_cp: [f32; 2],
offset_reference_pixels: [i16; NUM_PIXELS],
offset_reference_cp: [i16; 2],
k_v_pattern: [f32; 4],
k_v_cp: f32,
k_ta_pixels: [f32; NUM_PIXELS],
k_ta_cp: f32,
temperature_gradient_coefficient: Option<f32>,
}
impl Mlx90640Calibration {
fn calculate_bulk_pixel_calibration<B: Buf>(data: &mut B) -> ([i16; NUM_PIXELS], u8, u8) {
let (extra_value, row_scale, column_scale, remainder_scale) = {
let scales = word_to_u4s(data);
(scales[0], scales[1], scales[2], scales[3])
};
let offset_average = data.get_i16();
let mut pixel_calibration = [offset_average; NUM_PIXELS];
const VALUES_PER_DATA_ROW: usize = 4;
for row_chunks in pixel_calibration.chunks_exact_mut(WIDTH * VALUES_PER_DATA_ROW) {
let rows_coefficients = word_to_i4s(data);
let rows_coefficients = core::array::IntoIter::new(rows_coefficients)
.map(i16::from)
.map(|coeff| coeff << row_scale)
.rev();
for (row, coefficient) in row_chunks.chunks_exact_mut(WIDTH).zip(rows_coefficients) {
row.iter_mut().for_each(|element| *element += coefficient);
}
}
for column_chunk_index in 0..(WIDTH / VALUES_PER_DATA_ROW) {
let column_coefficients = word_to_i4s(data);
let column_coefficients = column_coefficients
.iter()
.copied()
.map(i16::from)
.map(|coeff| coeff << column_scale)
.rev();
for row in pixel_calibration.chunks_exact_mut(WIDTH) {
let start_index = column_chunk_index * VALUES_PER_DATA_ROW;
let row_range = start_index..(start_index + VALUES_PER_DATA_ROW);
row[row_range]
.iter_mut()
.zip(column_coefficients.clone())
.for_each(|(element, coefficient)| *element += coefficient);
}
}
(pixel_calibration, remainder_scale, extra_value)
}
fn repeat_chessboard<T>(source_values: [i8; 4]) -> impl Iterator<Item = T>
where
T: From<i8> + core::fmt::Debug,
{
let row_even_col_even = source_values[0];
let row_odd_col_even = source_values[1];
let row_even_col_odd = source_values[2];
let row_odd_col_odd = source_values[3];
let even_row_pattern = core::array::IntoIter::new([row_even_col_even, row_even_col_odd]);
let odd_row_pattern = core::array::IntoIter::new([row_odd_col_even, row_odd_col_odd]);
let even_row = even_row_pattern.cycle().take(WIDTH).map(T::from);
let odd_row = odd_row_pattern.cycle().take(WIDTH).map(T::from);
let repeating_rows = even_row.chain(odd_row).cycle();
repeating_rows.take(NUM_PIXELS)
}
fn generate_k_ta_pixels<B: Buf>(data: &mut B) -> impl Iterator<Item = i16> {
let source_data: ArrayVec<i8, 4> = (0..4).map(|_| data.get_i8()).collect();
let source_data = source_data.into_inner().unwrap();
Self::repeat_chessboard(source_data)
}
pub fn from_data(data: &[u8]) -> Result<Self, LibraryError> {
let mut buf = data;
let eeprom_length = usize::from(EepromAddress::End - EepromAddress::Base);
if buf.remaining() < eeprom_length {
return Err(LibraryError::Other(
"Not enough space left in buffer to be a full EEPROM dump",
));
}
buf.advance(WORD_SIZE * 16);
let (mut offset_reference_pixels, offset_correction_remainder_scale, alpha_ptat) =
Self::calculate_bulk_pixel_calibration(&mut buf);
let alpha_ptat = alpha_ptat / 4 + 8;
let (alpha_pixels, alpha_correction_remainder_scale, alpha_scale_exp) =
Self::calculate_bulk_pixel_calibration(&mut buf);
let alpha_pixels: ArrayVec<f32, NUM_PIXELS> = core::array::IntoIter::new(alpha_pixels)
.map(f32::from)
.collect();
let mut alpha_pixels = alpha_pixels.into_inner().unwrap();
let alpha_scale = f32::from(alpha_scale_exp + 30).exp2();
let gain = buf.get_i16();
let v_ptat_25 = buf.get_i16();
let (k_v_ptat, kt_ptat_bytes) = word_6_10_split(&mut buf);
let k_t_ptat = i16_from_bits(&kt_ptat_bytes, 10);
let k_v_dd = (buf.get_i8() as i16) << 5;
let v_dd_25 = ((buf.get_u8() as i16) - 256) * (1 << 5) - (1 << 13);
let k_v_avg = word_to_i4s(&mut buf);
buf.advance(WORD_SIZE);
let lazy_k_ta_pixels = Self::generate_k_ta_pixels(&mut buf);
let unpacked_scales = word_to_u4s(&mut buf);
let resolution = unpacked_scales[0] & 0x3;
let k_v_scale = f32::from(unpacked_scales[1]).exp2();
let k_ta_scale1 = f32::from(unpacked_scales[2] + 8).exp2();
let k_ta_scale2_exp = unpacked_scales[3];
let k_v_pattern: ArrayVec<f32, 4> = core::array::IntoIter::new(k_v_avg)
.map(|v| f32::from(v) / k_v_scale)
.collect();
let k_v_pattern = k_v_pattern.into_inner().unwrap();
let alpha_cp = {
let (alpha_cp_ratio, alpha_cp_bytes) = word_6_10_split(&mut buf);
let alpha_cp_ratio = f32::from(alpha_cp_ratio) / 7f32.exp2();
let alpha_scale_cp = f32::from(alpha_scale_exp + 27).exp2();
let alpha_cp0: f32 = f32::from(u16::from_be_bytes(alpha_cp_bytes)) / alpha_scale_cp;
[alpha_cp0, alpha_cp0 * (1f32 + alpha_cp_ratio)]
};
let offset_reference_cp = {
let (offset_cp_delta, offset_cp_bytes) = word_6_10_split(&mut buf);
let offset_cp0 = i16_from_bits(&offset_cp_bytes, 10);
[offset_cp0, offset_cp0 + i16::from(offset_cp_delta)]
};
let k_v_cp = f32::from(buf.get_i8()) / k_v_scale;
let k_ta_cp = f32::from(buf.get_i8()) / k_ta_scale1;
let k_s_ta = f32::from(buf.get_i8()) / 13f32.exp2();
let temperature_gradient_coefficient = match buf.get_i8() {
0 => None,
n => Some(f32::from(n) / 5f32.exp2()),
};
let mut k_s_to_ranges: ArrayVec<f32, 4> = (0..4).map(|_| f32::from(buf.get_i8())).collect();
k_s_to_ranges.swap(0, 1);
k_s_to_ranges.swap(2, 3);
let mut k_s_to = k_s_to_ranges.into_inner().unwrap();
let unpacked_corner_temps = word_to_u4s(&mut buf);
let corner_temperature_step = i16::from(unpacked_corner_temps[0] & 0x3) * 10;
let ct2 = i16::from(unpacked_corner_temps[1]) * corner_temperature_step;
let ct3 = i16::from(unpacked_corner_temps[2]) * corner_temperature_step + ct2;
let k_s_to_scale = f32::from(unpacked_corner_temps[3] + 8).exp2();
let corner_temperatures = [-40i16, 0, ct2, ct3];
k_s_to.iter_mut().for_each(|k_s_to| *k_s_to /= k_s_to_scale);
let alpha_correction =
alpha_correction_coefficients(BASIC_TEMPERATURE_RANGE, &corner_temperatures, &k_s_to);
let mut k_ta_pixels = [0f32; NUM_PIXELS];
offset_reference_pixels
.iter_mut()
.zip(alpha_pixels.iter_mut())
.zip(lazy_k_ta_pixels)
.zip(k_ta_pixels.iter_mut())
.for_each(|(((offset, alpha), k_ta_numerator), k_ta)| {
let high = buf.get_u8();
let low = buf.get_u8();
let offset_remainder = i16::from(i8::from_ne_bytes([high & 0xFC]) >> 2);
*offset += offset_remainder << offset_correction_remainder_scale;
let alpha_remainder = (i16::from_be_bytes([high & 0x03, low]) << 6) >> 10;
*alpha += f32::from(alpha_remainder << alpha_correction_remainder_scale);
*alpha /= alpha_scale;
let k_ta_remainder = i16::from(i8::from_ne_bytes([low & 0x0E]) >> 1);
let k_ta_numerator =
f32::from(k_ta_numerator + (k_ta_remainder << k_ta_scale2_exp));
*k_ta = k_ta_numerator / k_ta_scale1;
});
Ok(Self {
k_v_dd,
v_dd_25,
resolution,
k_v_ptat: k_v_ptat.into(),
k_t_ptat,
v_ptat_25,
alpha_ptat: alpha_ptat.into(),
gain,
k_s_ta,
corner_temperatures,
k_s_to,
alpha_correction,
alpha_pixels,
alpha_cp,
offset_reference_pixels,
offset_reference_cp,
k_v_pattern,
k_v_cp,
k_ta_pixels,
k_ta_cp,
temperature_gradient_coefficient,
})
}
}
impl<I2C> FromI2C<I2C> for Mlx90640Calibration
where
I2C: i2c::WriteRead + i2c::Write,
{
type Error = Error<I2C>;
type Ok = Self;
fn from_i2c(bus: &mut I2C, i2c_address: u8) -> Result<Self, Error<I2C>> {
const EEPROM_LENGTH: usize =
(EepromAddress::End as usize - EepromAddress::Base as usize + 1) * 2;
let mut eeprom_buf = [0u8; EEPROM_LENGTH];
let eeprom_base: Address = EepromAddress::Base.into();
bus.write_read(i2c_address, &eeprom_base.as_bytes(), &mut eeprom_buf)
.map_err(Error::I2cWriteReadError)?;
Ok(Self::from_data(&eeprom_buf)?)
}
}
impl<'a> CalibrationData<'a> for Mlx90640Calibration {
expose_member!(k_v_dd, i16);
expose_member!(v_dd_25, i16);
expose_member!(resolution, u8);
expose_member!(k_v_ptat, i16);
expose_member!(k_t_ptat, i16);
expose_member!(v_ptat_25, i16);
expose_member!(alpha_ptat, u16);
expose_member!(gain, i16);
expose_member!(k_s_ta, f32);
expose_member!(&corner_temperatures, [i16]);
expose_member!(&k_s_to, [f32]);
expose_member!(&alpha_correction, [f32]);
fn basic_range(&self) -> usize {
BASIC_TEMPERATURE_RANGE
}
type OffsetReferenceIterator = slice::Iter<'a, i16>;
fn offset_reference_pixels(&'a self, _subpage: Subpage) -> Self::OffsetReferenceIterator {
self.offset_reference_pixels.iter()
}
fn offset_reference_cp(&self, subpage: Subpage) -> i16 {
self.offset_reference_cp[subpage as usize]
}
type AlphaIterator = slice::Iter<'a, f32>;
fn alpha_pixels(&'a self, _subpage: Subpage) -> Self::AlphaIterator {
self.alpha_pixels.iter()
}
fn alpha_cp(&self, subpage: Subpage) -> f32 {
self.alpha_cp[subpage as usize]
}
type KvIterator = ChessboardIter<'a, f32>;
fn k_v_pixels(&'a self, _subpage: Subpage) -> Self::KvIterator {
ChessboardIter::new(&self.k_v_pattern)
}
fn k_v_cp(&self, _subpage: Subpage) -> f32 {
self.k_v_cp
}
type KtaIterator = slice::Iter<'a, f32>;
fn k_ta_pixels(&'a self, _subpage: Subpage) -> Self::KtaIterator {
self.k_ta_pixels.iter()
}
fn k_ta_cp(&self, _subpage: Subpage) -> f32 {
self.k_ta_cp
}
expose_member!(temperature_gradient_coefficient, Option<f32>);
}
#[derive(Clone, Debug, PartialEq)]
pub struct ChessboardIter<'a, T: 'a> {
index: usize,
source: &'a [T],
}
impl<'a, T: 'a> ChessboardIter<'a, T> {
fn new(source: &'a [T]) -> Self {
Self { index: 0, source }
}
}
impl<'a, T: 'a> Iterator for ChessboardIter<'a, T> {
type Item = &'a T;
fn next(&mut self) -> Option<Self::Item> {
if self.index < NUM_PIXELS {
let row = self.index / WIDTH;
let column = self.index % WIDTH;
self.index += 1;
Some(match (row % 2 == 0, column % 2 == 0) {
(true, true) => &self.source[0],
(false, true) => &self.source[1],
(true, false) => &self.source[2],
(false, false) => &self.source[3],
})
} else {
None
}
}
}
fn i16_from_bits(mut bytes: &[u8], num_bits: u8) -> i16 {
let num = match bytes.remaining() {
1 => i16::from(bytes.get_i8()),
_ => bytes.get_i16(),
};
let shift_amount = 16 - num_bits;
(num << shift_amount) >> shift_amount
}
fn word_6_10_split<B: Buf>(data: &mut B) -> (i8, [u8; 2]) {
let mut word = [data.get_u8(), data.get_u8()];
let six_bit = i8::from_ne_bytes([word[0]]) >> 2;
word[0] &= 0x03;
(six_bit, word)
}
fn word_to_u4s<B: Buf>(data: &mut B) -> [u8; 4] {
let high = data.get_u8();
let low = data.get_u8();
[(high & 0xF0) >> 4, high & 0xF, (low & 0xF0) >> 4, low & 0xF]
}
fn u8_to_i4s(byte: u8) -> [i8; 2] {
let high = byte & 0xF0;
let low = (byte & 0xF) << 4;
let high = i8::from_ne_bytes([high]);
let low = i8::from_ne_bytes([low]);
[high >> 4, low >> 4]
}
fn word_to_i4s<B: Buf>(data: &mut B) -> [i8; 4] {
let high = data.get_u8();
let low = data.get_u8();
let high = u8_to_i4s(high);
let low = u8_to_i4s(low);
[high[0], high[1], low[0], low[1]]
}
#[cfg(test)]
pub(crate) mod test {
#[cfg(feature = "std")]
extern crate std;
#[cfg(feature = "std")]
use std::{print, println};
use arrayvec::ArrayVec;
use crate::common::CalibrationData;
use crate::mlx90640::{HEIGHT, NUM_PIXELS, WIDTH};
use crate::register::Subpage;
use crate::test::mlx90640_eeprom_data;
use super::Mlx90640Calibration;
pub(crate) fn eeprom() -> Mlx90640Calibration {
let mut eeprom_bytes = mlx90640_eeprom_data();
Mlx90640Calibration::from_data(&mut eeprom_bytes).expect("The EEPROM data to be parsed.")
}
#[test]
fn i16_from_bits() {
assert_eq!(super::i16_from_bits(b"\x00\xff", 8), -1);
assert_eq!(super::i16_from_bits(b"\x03\xff", 10), -1);
assert_eq!(super::i16_from_bits(b"\xf0\xff", 8), -1);
assert_eq!(super::i16_from_bits(b"\xf3\xff", 10), -1);
}
#[test]
fn word_6_10_split() {
fn check(mut data: &[u8], little: i8, remainder: [u8; 2]) {
let split = super::word_6_10_split(&mut data);
assert_eq!(
split.0, little,
"word_6_10_split failed to split the upper 6 bits off"
);
assert_eq!(
split.1, remainder,
"word_6_10_split failed to split the lower 10 bits off"
);
}
check(b"\xfc\x00", -1, [0x00, 0x00]);
check(b"\x03\xff", 0, [0x03, 0xFF]);
check(b"\x83\xff", -32, [0x03, 0xFF]);
}
#[test]
fn word_to_u4s() {
let mut sequence: &[u8] = b"\x12\x34";
assert_eq!(super::word_to_u4s(&mut sequence), [1, 2, 3, 4]);
let mut max_min: &[u8] = b"\xf0\xf0";
assert_eq!(super::word_to_u4s(&mut max_min), [0xF, 0, 0xF, 0]);
let mut min_max: &[u8] = b"\x0f\x0f";
assert_eq!(super::word_to_u4s(&mut min_max), [0, 0xF, 0, 0xF]);
}
#[test]
fn u8_to_i4s() {
assert_eq!(super::u8_to_i4s(0x44), [4, 4]);
assert_eq!(super::u8_to_i4s(0x88), [-8, -8]);
assert_eq!(super::u8_to_i4s(0x48), [4, -8]);
assert_eq!(super::u8_to_i4s(0x84), [-8, 4]);
}
#[test]
fn repeat_chessboard() {
let pattern = [1, 2, 3, 4];
let test_pattern: ArrayVec<i8, NUM_PIXELS> =
Mlx90640Calibration::repeat_chessboard(pattern).collect();
#[cfg(feature = "std")]
for row in 0..HEIGHT {
for column in 0..WIDTH {
let index = row * WIDTH + column;
print!("{} ", test_pattern[index]);
}
println!();
}
for column in 0..WIDTH {
for row in 0..HEIGHT {
let index = row * WIDTH + column;
let expected = match (row % 2, column % 2) {
(0, 0) => 1,
(1, 0) => 2,
(0, 1) => 3,
(1, 1) => 4,
(_, _) => unreachable!(),
};
assert_eq!(
test_pattern[index], expected,
"pattern incorrect at index {}",
index
);
}
}
}
#[test]
fn smoke() {
eeprom();
}
macro_rules! datasheet_test {
($name:ident, $value:literal) => {
#[test]
fn $name() {
assert_eq!(eeprom().$name(), $value);
}
};
}
datasheet_test!(resolution, 2);
datasheet_test!(k_v_dd, -3168);
datasheet_test!(v_dd_25, -13056);
datasheet_test!(v_dd_0, 3.3);
datasheet_test!(k_v_ptat, 22);
datasheet_test!(k_t_ptat, 338);
datasheet_test!(v_ptat_25, 12273);
datasheet_test!(alpha_ptat, 9);
datasheet_test!(gain, 6383);
#[test]
fn pixel_offset() {
let e = eeprom();
let offsets0: ArrayVec<i16, NUM_PIXELS> =
e.offset_reference_pixels(Subpage::One).copied().collect();
let offsets1: ArrayVec<i16, NUM_PIXELS> =
e.offset_reference_pixels(Subpage::Zero).copied().collect();
assert_eq!(offsets0, offsets1);
let index = 11 * WIDTH + 15;
let pixel = offsets0[index];
assert_eq!(pixel, -75);
}
#[test]
fn pixel_k_ta() {
let e = eeprom();
let k_ta0: ArrayVec<f32, NUM_PIXELS> = e.k_ta_pixels(Subpage::One).copied().collect();
let k_ta1: ArrayVec<f32, NUM_PIXELS> = e.k_ta_pixels(Subpage::Zero).copied().collect();
assert_eq!(k_ta0, k_ta1);
let index = 11 * WIDTH + 15;
let pixel = k_ta0[index];
assert_eq!(pixel, 0.005126953125);
}
#[test]
fn k_v_pixels() {
let e = eeprom();
assert_eq!(e.k_v_pixels(Subpage::Zero), e.k_v_pixels(Subpage::One));
let index = 11 * WIDTH + 15;
assert_eq!(e.k_v_pixels(Subpage::Zero).nth(index).unwrap(), &0.5);
}
#[test]
fn emissivity() {
assert_eq!(eeprom().emissivity(), None);
}
#[test]
fn offset_reference_cp() {
let e = eeprom();
assert_eq!(e.offset_reference_cp(Subpage::Zero), -75);
assert_eq!(e.offset_reference_cp(Subpage::One), -77);
}
#[test]
fn k_ta_cp() {
let e = eeprom();
assert_eq!(e.k_ta_cp(Subpage::Zero), e.k_ta_cp(Subpage::One));
assert_eq!(e.k_ta_cp(Subpage::Zero), 0.00457763671875);
}
#[test]
fn k_v_cp() {
let e = eeprom();
assert_eq!(e.k_v_cp(Subpage::Zero), e.k_v_cp(Subpage::One));
assert_eq!(e.k_v_cp(Subpage::Zero), 0.5);
}
#[test]
fn temperature_gradient_coefficient() {
assert_eq!(eeprom().temperature_gradient_coefficient(), Some(1f32));
}
#[test]
fn alpha_cp() {
let e = eeprom();
assert_eq!(e.alpha_cp(Subpage::Zero), 4.07453626394272E-9);
assert_eq!(e.alpha_cp(Subpage::One), 3.851710062200835E-9);
}
datasheet_test!(k_s_ta, -0.001953125);
#[test]
fn pixel_alpha() {
let e = eeprom();
let alpha0: ArrayVec<f32, NUM_PIXELS> = e.alpha_pixels(Subpage::One).copied().collect();
let alpha1: ArrayVec<f32, NUM_PIXELS> = e.alpha_pixels(Subpage::Zero).copied().collect();
assert_eq!(alpha0, alpha1);
let index = 11 * WIDTH + 15;
let pixel = alpha0[index];
assert_eq!(pixel, 1.262233122690854E-7);
}
#[test]
fn k_s_to() {
let e = eeprom();
assert_eq!(e.k_s_to()[1], -0.00080108642578125);
}
#[test]
fn corner_temperatures() {
let e = eeprom();
let ct = e.corner_temperatures();
assert_eq!(ct[0], -40);
assert_eq!(ct[1], 0);
assert_eq!(ct[2], 160);
assert_eq!(ct[3], 320);
}
}