use core::marker::PhantomData;
use arrayvec::ArrayVec;
use embedded_hal::blocking::i2c;
use paste::paste;
use crate::calculations::*;
use crate::common::*;
use crate::error::Error;
use crate::register::*;
macro_rules! set_register_field {
{ $register_access:ident, $field:ident, $doc:literal } => {
set_register_field! {
$register_access,
$field,
bool,
$doc
}
};
{ $register_access:ident, $field:ident, $typ:ty, $doc:literal } => {
paste! {
#[doc = $doc]
pub fn [< set_ $field >](&mut self, new_value: $typ) -> Result<(), Error<I2C>> {
let mut current = self.$register_access()?;
if current.$field() != new_value {
current.[< set_ $field >](new_value);
self.[< set_ $register_access >](current)
} else {
Ok(())
}
}
}};
}
#[derive(Clone, Debug)]
pub struct CameraDriver<
Cam,
Clb,
I2C,
const HEIGHT: usize,
const WIDTH: usize,
const NUM_BYTES: usize,
> {
bus: I2C,
address: u8,
calibration: Clb,
pixel_buffer: [u8; NUM_BYTES],
resolution_correction: f32,
ambient_temperature: Option<f32>,
emissivity: f32,
access_pattern: AccessPattern,
reflected_temperature: Option<f32>,
_camera: PhantomData<Cam>,
}
impl<'a, Cam, Clb, I2C, const HEIGHT: usize, const WIDTH: usize, const BUFFER_SIZE: usize>
CameraDriver<Cam, Clb, I2C, HEIGHT, WIDTH, BUFFER_SIZE>
where
Cam: MelexisCamera,
Clb: CalibrationData<'a>,
I2C: i2c::WriteRead + i2c::Write,
{
pub fn new(bus: I2C, address: u8) -> Result<Self, Error<I2C>>
where
Clb: FromI2C<I2C, Ok = Clb, Error = Error<I2C>>,
{
let mut bus = bus;
let calibration = Clb::from_i2c(&mut bus, address)?;
Self::new_with_calibration(bus, address, calibration)
}
pub fn new_with_calibration(
bus: I2C,
address: u8,
calibration: Clb,
) -> Result<Self, Error<I2C>> {
let mut bus = bus;
let control: ControlRegister = read_register(&mut bus, address)?;
let resolution_correction = Cam::resolution_correction(
calibration.resolution(),
control.resolution().as_raw() as u8,
);
let access_pattern = control.access_pattern();
let emissivity = calibration.emissivity().unwrap_or(1f32);
Ok(Self {
bus,
address,
calibration,
pixel_buffer: [0u8; BUFFER_SIZE],
resolution_correction,
ambient_temperature: None,
emissivity,
access_pattern,
reflected_temperature: None,
_camera: PhantomData,
})
}
fn status_register(&mut self) -> Result<StatusRegister, Error<I2C>> {
let register = read_register(&mut self.bus, self.address)?;
Ok(register)
}
fn set_status_register(&mut self, register: StatusRegister) -> Result<(), Error<I2C>> {
write_register(&mut self.bus, self.address, register)
}
fn update_control_register(&mut self, register: &ControlRegister) {
let calibrated_resolution = self.calibration.resolution();
self.resolution_correction =
Cam::resolution_correction(calibrated_resolution, register.resolution().as_raw() as u8);
self.access_pattern = register.access_pattern();
}
fn control_register(&mut self) -> Result<ControlRegister, Error<I2C>> {
let register: ControlRegister = read_register(&mut self.bus, self.address)?;
self.update_control_register(®ister);
Ok(register)
}
fn set_control_register(&mut self, register: ControlRegister) -> Result<(), Error<I2C>> {
self.update_control_register(®ister);
write_register(&mut self.bus, self.address, register)?;
Ok(())
}
pub fn last_measured_subpage(&mut self) -> Result<Subpage, Error<I2C>> {
Ok(self.status_register()?.last_updated_subpage())
}
pub fn data_available(&mut self) -> Result<Option<Subpage>, Error<I2C>> {
let register = self.status_register()?;
Ok(if register.new_data() {
Some(register.last_updated_subpage())
} else {
None
})
}
pub fn reset_data_available(&mut self) -> Result<(), Error<I2C>> {
let mut current = self.status_register()?;
current.reset_new_data();
self.set_status_register(current)
}
pub fn overwrite_enabled(&mut self) -> Result<bool, Error<I2C>> {
Ok(self.status_register()?.overwrite_enabled())
}
set_register_field! {
status_register,
overwrite_enabled,
"Enabled (or disable) overwriting of data in RAM with new data."
}
pub fn subpages_enabled(&mut self) -> Result<bool, Error<I2C>> {
Ok(self.control_register()?.use_subpages())
}
set_register_field! {
control_register,
use_subpages,
"Enabled (or disable) the use of subpages."
}
pub fn data_hold_enabled(&mut self) -> Result<bool, Error<I2C>> {
Ok(self.control_register()?.data_hold())
}
set_register_field! {
control_register,
data_hold,
"Enabled (or disable) data holding."
}
pub fn subpage_repeat(&mut self) -> Result<bool, Error<I2C>> {
Ok(self.control_register()?.subpage_repeat())
}
set_register_field! {
control_register,
subpage_repeat,
"Enabled (or disable) subpage repeat mode."
}
pub fn selected_subpage(&mut self) -> Result<Subpage, Error<I2C>> {
Ok(self.control_register()?.subpage())
}
set_register_field! {
control_register,
subpage,
Subpage,
"Set the currently selected subpage when [subpage repeat][CameraDriver::subpage_repeat] is enabled."
}
pub fn frame_rate(&mut self) -> Result<FrameRate, Error<I2C>> {
Ok(self.control_register()?.frame_rate())
}
set_register_field! {
control_register,
frame_rate,
FrameRate,
"Set camera's frame rate."
}
pub fn resolution(&mut self) -> Result<Resolution, Error<I2C>> {
Ok(self.control_register()?.resolution())
}
set_register_field! {
control_register,
resolution,
Resolution,
"Set ADC resolution within the camera."
}
pub fn access_pattern(&mut self) -> Result<AccessPattern, Error<I2C>> {
Ok(self.control_register()?.access_pattern())
}
set_register_field! {
control_register,
access_pattern,
AccessPattern,
"Set the access pattern used by the camera."
}
pub fn effective_emissivity(&self) -> f32 {
self.emissivity
}
pub fn override_emissivity(&mut self, new_value: f32) {
self.emissivity = new_value;
}
pub fn use_default_emissivity(&mut self) {
let default_emissivity = self.calibration.emissivity();
self.emissivity = default_emissivity.unwrap_or(1f32);
}
pub fn reflected_temperature(&self) -> Option<f32> {
self.reflected_temperature
}
pub fn set_reflected_temperature(&mut self, new_value: Option<f32>) {
self.reflected_temperature = new_value;
}
pub fn ambient_temperature(&self) -> Option<f32> {
self.ambient_temperature
}
pub fn height(&self) -> usize {
Cam::HEIGHT
}
pub fn width(&self) -> usize {
Cam::WIDTH
}
fn read_ram(&mut self, subpage: Subpage) -> Result<RamData, Error<I2C>> {
read_ram::<Cam, I2C, HEIGHT>(
&mut self.bus,
self.address,
self.access_pattern,
subpage,
&mut self.pixel_buffer,
)
}
pub fn generate_raw_image_subpage_to(
&'a mut self,
subpage: Subpage,
destination: &mut [f32],
) -> Result<(), Error<I2C>> {
let ram = self.read_ram(subpage)?;
let mut valid_pixels = Cam::pixels_in_subpage(subpage, self.access_pattern).into_iter();
let t_a = raw_pixels_to_ir_data(
&self.calibration,
self.emissivity,
self.resolution_correction,
&self.pixel_buffer,
ram,
subpage,
self.access_pattern,
&mut valid_pixels,
destination,
);
self.ambient_temperature = Some(t_a);
Ok(())
}
pub fn generate_image_subpage_to(
&'a mut self,
subpage: Subpage,
destination: &mut [f32],
) -> Result<(), Error<I2C>> {
let ram = self.read_ram(subpage)?;
let mut valid_pixels = Cam::pixels_in_subpage(subpage, self.access_pattern).into_iter();
let t_a = raw_pixels_to_temperatures(
&self.calibration,
self.emissivity,
self.reflected_temperature,
self.resolution_correction,
&self.pixel_buffer,
ram,
subpage,
self.access_pattern,
&mut valid_pixels,
destination,
);
self.ambient_temperature = Some(t_a);
Ok(())
}
pub fn generate_image_to<'b: 'a>(
&'b mut self,
destination: &mut [f32],
) -> Result<(), Error<I2C>> {
let subpage = self.last_measured_subpage()?;
self.generate_image_subpage_to(subpage, destination)
}
pub fn generate_image_if_ready(
&'a mut self,
destination: &mut [f32],
) -> Result<bool, Error<I2C>> {
let address = self.address;
let bus = &mut self.bus;
let pixel_buffer = &mut self.pixel_buffer;
let mut status_register: StatusRegister = read_register(bus, address)?;
if status_register.new_data() {
let subpage = status_register.last_updated_subpage();
let mut valid_pixels = Cam::pixels_in_subpage(subpage, self.access_pattern).into_iter();
let ram = read_ram::<Cam, I2C, HEIGHT>(
bus,
address,
self.access_pattern,
subpage,
pixel_buffer,
)?;
let ambient_temperature = raw_pixels_to_temperatures(
&self.calibration,
self.emissivity,
self.reflected_temperature,
self.resolution_correction,
&self.pixel_buffer,
ram,
subpage,
self.access_pattern,
&mut valid_pixels,
destination,
);
self.ambient_temperature = Some(ambient_temperature);
status_register.reset_new_data();
write_register(bus, address, status_register)?;
Ok(true)
} else {
Ok(false)
}
}
pub fn synchronize(&mut self) -> Result<(), Error<I2C>> {
let mut status_register = self.status_register()?;
status_register.reset_new_data();
status_register.set_overwrite_enabled(true);
status_register.set_start_measurement();
write_register(&mut self.bus, self.address, status_register)?;
while !status_register.new_data() {
status_register = read_register(&mut self.bus, self.address)?;
core::hint::spin_loop();
}
Ok(())
}
}
fn read_ram<Cam, I2C, const HEIGHT: usize>(
bus: &mut I2C,
i2c_address: u8,
access_pattern: AccessPattern,
subpage: Subpage,
pixel_data_buffer: &mut [u8],
) -> Result<RamData, Error<I2C>>
where
Cam: MelexisCamera,
I2C: i2c::WriteRead + i2c::Write,
{
let pixel_ranges: ArrayVec<PixelAddressRange, HEIGHT> =
Cam::pixel_ranges(subpage, access_pattern)
.into_iter()
.collect();
for range in pixel_ranges.iter() {
let offset = range.buffer_offset;
let address_bytes = range.start_address.as_bytes();
bus.write_read(
i2c_address,
&address_bytes[..],
&mut pixel_data_buffer[offset..(offset + range.length)],
)
.map_err(Error::I2cWriteReadError)?;
}
RamData::from_i2c::<I2C, Cam>(bus, i2c_address, subpage).map_err(Error::I2cWriteReadError)
}
fn read_register<R, I2C>(bus: &mut I2C, address: u8) -> Result<R, Error<I2C>>
where
I2C: i2c::WriteRead + i2c::Write,
R: Register,
{
fn read_register<I2C: i2c::WriteRead>(
bus: &mut I2C,
i2c_address: u8,
register_address: Address,
) -> Result<[u8; 2], I2C::Error> {
let register_address_bytes = register_address.as_bytes();
let mut register_bytes = [0u8; 2];
bus.write_read(i2c_address, ®ister_address_bytes, &mut register_bytes)?;
Ok(register_bytes)
}
let register_address = R::address();
let register_value =
read_register(bus, address, register_address).map_err(Error::I2cWriteReadError)?;
let register = R::from(®ister_value[..]);
Ok(register)
}
fn write_register<R, I2C>(bus: &mut I2C, address: u8, register: R) -> Result<(), Error<I2C>>
where
I2C: i2c::Write + i2c::WriteRead,
R: Register,
{
let register_address = R::address();
let register_bytes: [u8; 2] = register.into();
write_raw_register(bus, address, register_address.as_bytes(), register_bytes)
.map_err(Error::I2cWriteError)?;
Ok(())
}
fn write_raw_register<I2C: i2c::Write>(
bus: &mut I2C,
i2c_address: u8,
register_address: [u8; 2],
register_data: [u8; 2],
) -> Result<(), I2C::Error> {
let combined: [u8; 4] = [
register_address[0],
register_address[1],
register_data[0],
register_data[1],
];
bus.write(i2c_address, &combined)?;
Ok(())
}
#[cfg(test)]
#[allow(clippy::excessive_precision)]
mod test {
extern crate std;
use float_cmp::{approx_eq, assert_approx_eq};
use crate::{mlx90640, mlx90641};
use crate::{test::*, Subpage};
use crate::{I2cRegister, MelexisCamera, Mlx90640Driver, Mlx90641Driver, StatusRegister};
fn create_mlx90640() -> Mlx90640Driver<MockCameraBus<MLX90640_RAM_LENGTH>> {
let address: u8 = 0x30;
let mock_bus = datasheet_mlx90640_at_address(address);
Mlx90640Driver::new(mock_bus, address)
.expect("A MLX90640 camera should be created after loading its data")
}
fn create_mlx90641() -> Mlx90641Driver<MockCameraBus<MLX90641_RAM_LENGTH>> {
let address: u8 = 0x28;
let mock_bus = mock_mlx90641_at_address(address);
Mlx90641Driver::new(mock_bus, address)
.expect("A MLX90641 camera should be created after loading its data")
}
#[test]
fn smoke_test() {
create_mlx90640();
create_mlx90641();
}
#[test]
fn read_register() {
let address = 0x10;
let mut mock_bus = datasheet_mlx90640_at_address(address);
mock_bus.clear_recent_operations();
let register: I2cRegister = super::read_register(&mut mock_bus, address).unwrap();
assert_eq!(register, I2cRegister::default());
let ops = mock_bus.recent_operations();
assert_eq!(
ops.len(),
1,
"Only one operation should be performed to read a register"
)
}
#[test]
fn read_write_register() {
let address = 0x42;
let mut mock_bus = datasheet_mlx90640_at_address(address);
mock_bus.clear_recent_operations();
let mut status_register: StatusRegister =
super::read_register(&mut mock_bus, address).unwrap();
assert_eq!(mock_bus.recent_operations().len(), 1);
assert!(!status_register.overwrite_enabled());
status_register.set_overwrite_enabled(true);
super::write_register(&mut mock_bus, address, status_register).unwrap();
assert_eq!(mock_bus.recent_operations().len(), 2);
}
#[test]
fn default_emissivity() {
let mut cam = create_mlx90640();
assert_eq!(cam.effective_emissivity(), 1f32);
cam.override_emissivity(0.92);
assert_eq!(cam.effective_emissivity(), 0.92);
cam.use_default_emissivity();
assert_eq!(cam.effective_emissivity(), 1f32);
}
#[test]
fn set_reflected_temperature() {
let mut cam = create_mlx90640();
assert!(cam.reflected_temperature().is_none());
const T_R: f32 = 10.5;
cam.set_reflected_temperature(Some(T_R));
assert_eq!(cam.reflected_temperature(), Some(T_R));
cam.set_reflected_temperature(None);
assert!(cam.reflected_temperature().is_none());
}
#[test]
fn mlx90640_datasheet_integration() {
let mut cam = create_mlx90640();
let mut temperatures = [0f32; mlx90640::Mlx90640::NUM_PIXELS];
let res = cam.generate_image_subpage_to(Subpage::Zero, &mut temperatures);
assert!(res.is_ok());
const PIXEL_INDEX: usize = 11 * mlx90640::Mlx90640::WIDTH + 15;
assert_approx_eq!(f32, temperatures[PIXEL_INDEX], 80.36331, epsilon = 0.5);
}
#[test]
fn mlx90641_datasheet_integration() {
let mut cam = create_mlx90641();
let mut temperatures = [0f32; mlx90641::Mlx90641::NUM_PIXELS];
let res = cam.generate_image_if_ready(&mut temperatures);
assert!(res.is_ok());
assert!(res.unwrap());
const PIXEL_INDEX: usize = 5 * mlx90641::Mlx90641::WIDTH + 8;
assert_approx_eq!(f32, temperatures[PIXEL_INDEX], 80.129812, epsilon = 0.5);
}
#[test]
fn mlx90640_example_integration() {
let i2c_address = 0x43;
let mut mocked = example_mlx90640_at_address(i2c_address);
mocked.set_data_available(false);
let mut cam = Mlx90640Driver::new(mocked.clone(), i2c_address).unwrap();
let mut temperatures = [f32::NAN; mlx90640::Mlx90640::NUM_PIXELS];
let not_ready = cam.generate_image_if_ready(&mut temperatures);
assert!(!not_ready.unwrap());
for temperature in temperatures.iter() {
assert!(temperature.is_nan());
}
mocked.set_data_available(true);
let ready = cam.generate_image_if_ready(&mut temperatures);
assert!(ready.unwrap());
mocked.update_frame(
mlx90640_example_data::FRAME_1_DATA,
mlx90640_example_data::FRAME_1_STATUS_REGISTER,
);
mocked.set_data_available(true);
assert!(cam.generate_image_if_ready(&mut temperatures).unwrap());
let paired = temperatures
.iter()
.zip(mlx90640_example_data::TEMPERATURES.iter());
for (index, (actual, expected)) in paired.enumerate() {
assert!(
approx_eq!(f32, *actual, *expected, epsilon = 0.001),
"[pixel {:?}]:\n{:>10}: `{:?}`,\n{:>10}: `{:?}`,",
index,
"expected",
*expected,
"actual",
*actual
);
}
}
fn create_sentinel_buffer() -> [u8; mlx90641::Mlx90641::NUM_PIXELS * 2] {
let mut buf = [0u8; mlx90641::Mlx90641::NUM_PIXELS * 2];
buf.chunks_exact_mut(4).for_each(|chunk| {
chunk[0] = 0xDE;
chunk[1] = 0xAD;
chunk[2] = 0xBE;
chunk[3] = 0xEF;
});
buf
}
fn check_sentinel_buffer(buf: &[u8]) {
assert!(buf.len() % 4 == 0);
for (index, chunk) in buf.chunks_exact(4).enumerate() {
assert_ne!(
chunk[0..2],
[0xDE, 0xAD],
"Failed at byte index {}",
index * 4
);
assert_ne!(
chunk[2..4],
[0xBE, 0xEF],
"Failed at byte index {}",
index * 4 + 2
);
}
}
#[test]
fn mlx90641_ram_access_subpage_0() {
let mut buf = create_sentinel_buffer();
let i2c_address = 0x47;
let mut mock_bus = mock_mlx90641_at_address(i2c_address);
let ram_data_result =
super::read_ram::<mlx90641::Mlx90641, _, { mlx90641::Mlx90641::HEIGHT }>(
&mut mock_bus,
i2c_address,
crate::AccessPattern::Interleave,
crate::Subpage::Zero,
&mut buf,
);
assert!(ram_data_result.is_ok());
check_sentinel_buffer(&buf);
}
#[test]
fn mlx90641_ram_access_subpage_1() {
let mut buf = create_sentinel_buffer();
let i2c_address = 0x49;
let mut mock_bus = mock_mlx90641_at_address(i2c_address);
let ram_data_result =
super::read_ram::<mlx90641::Mlx90641, _, { mlx90641::Mlx90641::HEIGHT }>(
&mut mock_bus,
i2c_address,
crate::AccessPattern::Interleave,
crate::Subpage::One,
&mut buf,
);
assert!(ram_data_result.is_ok());
check_sentinel_buffer(&buf);
}
#[test]
fn get_register_flag_minimal_operations() {
let i2c_address = 0x49;
let mut mocked = example_mlx90640_at_address(i2c_address);
mocked.set_data_available(false);
let mut cam = Mlx90640Driver::new(mocked.clone(), i2c_address).unwrap();
mocked.clear_recent_operations();
cam.frame_rate().unwrap();
let ops = mocked.recent_operations();
assert_eq!(
ops.len(),
1,
"There should only be one operation to check a register"
);
}
#[test]
fn set_register_flag_minimal_operations() {
let i2c_address = 0x49;
let mut mocked = example_mlx90640_at_address(i2c_address);
mocked.set_data_available(false);
let mut cam = Mlx90640Driver::new(mocked.clone(), i2c_address).unwrap();
mocked.clear_recent_operations();
cam.set_frame_rate(crate::FrameRate::SixtyFour).unwrap();
let ops = mocked.recent_operations();
assert_eq!(
ops.len(),
2,
"There should only be two operations to update a register"
);
}
}