diff --git a/drivers/sensor/MLX90640.zig b/drivers/sensor/MLX90640.zig index 026af9102..a05b406b7 100644 --- a/drivers/sensor/MLX90640.zig +++ b/drivers/sensor/MLX90640.zig @@ -291,6 +291,88 @@ pub const MLX90640 = struct { } } + pub fn image(self: *Self, result: []f32) !void { + if (self.params.kVdd == 0) { + try self.extract_parameters(); + + // the initial frame load results in bad data upon restart, so get that bad data out of the way + try self.load_frame(); + } + + try self.load_frame(); + + const subPage: u16 = self.frame[833] & 0x0001; + const vdd = self.get_vdd(); + const ta = self.get_ta(vdd); + + var gain: f32 = @floatFromInt(self.frame[778]); + if (gain > 32767) { + gain = gain - 65536; + } + + const gee: f32 = @floatFromInt(self.params.gainEE); + gain = gee / gain; + + const mode: f32 = @floatFromInt((self.frame[832] & 0x1000) >> 5); + const cmee: f32 = @floatFromInt(self.params.calibrationModeEE); + + var irDataCP = [2]f32{ + @floatFromInt(self.frame[776]), + @floatFromInt(self.frame[808]), + }; + + for (0..2) |i| { + if (irDataCP[i] > 32767) { + irDataCP[i] = irDataCP[i] - 65536; + } + irDataCP[i] = irDataCP[i] * gain; + } + + var cpo: f32 = @floatFromInt(self.params.cpOffset[0]); + irDataCP[0] = irDataCP[0] - cpo * (1 + self.params.cpKta * (ta - 25)) * (1 + self.params.cpKv * (vdd - 3.3)); + + cpo = @floatFromInt(self.params.cpOffset[1]); + if (mode == cmee) { + irDataCP[1] = irDataCP[1] - cpo * (1 + self.params.cpKta * (ta - 25)) * (1 + self.params.cpKv * (vdd - 3.3)); + } else { + irDataCP[1] = irDataCP[1] - (cpo + self.params.ilChessC[0]) * (1 + self.params.cpKta * (ta - 25)) * (1 + self.params.cpKv * (vdd - 3.3)); + } + + const ktaScale: f32 = @floatFromInt(std.math.pow(u16, 2, self.params.ktaScale)); + const kvScale: f32 = @floatFromInt(std.math.pow(u16, 2, self.params.kvScale)); + + var pixelNumber: i32 = 0; + for (0..768) |i| { + pixelNumber = @intCast(i); + const ilPattern: i32 = @divTrunc(pixelNumber, 32) - @divTrunc(pixelNumber, 64) * 2; + const conversionPattern: i32 = (@divTrunc((pixelNumber + 2), 4) - @divTrunc((pixelNumber + 3), 4) + @divTrunc((pixelNumber + 1), 4) - @divTrunc(pixelNumber, 4)) * (1 - 2 * ilPattern); + + var irData: f32 = @floatFromInt(self.frame[i]); + if (irData > 32767) { + irData = irData - 65536; + } + + irData = irData * gain; + + const ktax: f32 = @floatFromInt(self.params.kta[i]); + const kta: f32 = ktax / ktaScale; + const kvx: f32 = @floatFromInt(self.params.kv[i]); + const kv: f32 = kvx / kvScale; + const offsetx: f32 = @floatFromInt(self.params.offset[i]); + irData = irData - offsetx * (1 + kta * (ta - 25)) * (1 + kv * (vdd - 3.3)); + + if (mode != cmee) { + const x: f32 = @floatFromInt(ilPattern); + const y: f32 = @floatFromInt(conversionPattern); + irData = irData + self.params.ilChessC[2] * (2 * x - 1) - self.params.ilChessC[1] * y; + } + + irData = irData - self.params.tgc * irDataCP[subPage]; + + result[i] = irData; + } + } + pub fn load_frame(self: *Self) !void { var ready: bool = false; for (frame_loop) |i| { diff --git a/examples/raspberrypi/rp2xxx/build.zig b/examples/raspberrypi/rp2xxx/build.zig index ea3b8c876..31650bb26 100644 --- a/examples/raspberrypi/rp2xxx/build.zig +++ b/examples/raspberrypi/rp2xxx/build.zig @@ -93,6 +93,8 @@ pub fn build(b: *std.Build) void { .{ .name = "cyw43-wifi-connect", .file = "src/cyw43/wifi_connect.zig" }, .{ .name = "allocator", .file = "src/allocator.zig" }, .{ .name = "mlx90640", .file = "src/mlx90640.zig" }, + .{ .name = "mlx90640-image", .file = "src/mlx90640_image.zig" }, + .{ .name = "mlx90640-hottest-point", .file = "src/mlx90640_hottest_point.zig" }, .{ .name = "ssd1306", .file = "src/ssd1306_oled.zig", .imports = &.{ .{ .name = "font8x8", .module = font8x8_dep.module("font8x8") }, } }, diff --git a/examples/raspberrypi/rp2xxx/src/mlx90640_hottest_point.zig b/examples/raspberrypi/rp2xxx/src/mlx90640_hottest_point.zig new file mode 100644 index 000000000..2113b9b88 --- /dev/null +++ b/examples/raspberrypi/rp2xxx/src/mlx90640_hottest_point.zig @@ -0,0 +1,128 @@ +const std = @import("std"); +const microzig = @import("microzig"); +const sensor = microzig.drivers.sensor; +const display = microzig.drivers.display; +const rp2xxx = microzig.hal; +const gpio = rp2xxx.gpio; +const i2c = rp2xxx.i2c; +const I2C_Device = rp2xxx.drivers.I2C_Device; +const MLX90640 = sensor.MLX90640; +const time = rp2xxx.time; + +const uart = rp2xxx.uart.instance.num(0); +const uart_tx_pin = gpio.num(0); + +var i2c0 = i2c.instance.num(0); +var i2c1 = i2c.instance.num(1); + +const pin_config = rp2xxx.pins.GlobalConfiguration{ + .GPIO0 = .{ .name = "gpio0", .function = .UART0_TX }, +}; + +pub const microzig_options = microzig.Options{ + .log_level = .debug, + .logFn = rp2xxx.uart.log, +}; + +pub fn main() !void { + try init(); + + var i2c_device = I2C_Device.init(i2c1, null); + + var camera = try MLX90640.init(.{ + .i2c = i2c_device.i2c_device(), + .address = @enumFromInt(0x33), + .clock = rp2xxx.drivers.clock_device(), + }); + + try camera.set_refresh_rate(0b101); + + const i2c_dd = rp2xxx.drivers.I2C_Datagram_Device.init(i2c0, @enumFromInt(0x3C), null); + const lcd = try display.ssd1306.init(.i2c, i2c_dd, null); + try lcd.clear_screen(false); + + var fb = display.ssd1306.Framebuffer.init(.black); + var image: [768]f32 = undefined; + + while (true) { + camera.temperature(&image) catch |err| { + std.log.err("unable to read image: {}", .{err}); + time.sleep_ms(100); + continue; + }; + + const hot = find_hottest_pixel(&image); + const pos = camera_to_display(hot.row, hot.col); + + fb.clear(.black); + draw_crosshair(&fb, pos.x, pos.y); + + try lcd.write_full_display(fb.bit_stream()); + time.sleep_ms(50); + } +} + +inline fn camera_to_display(row: usize, col: usize) struct { x: i16, y: i16 } { + return .{ + .x = @intCast((31 - col) * 128 / 32 + 2), + .y = @intCast(row * 64 / 24 + 1), + }; +} + +inline fn find_hottest_pixel(image: *const [768]f32) struct { row: usize, col: usize, temp: f32 } { + var max_temp: f32 = image[0]; + var hot_row: usize = 0; + var hot_col: usize = 0; + for (0..24) |row| { + for (0..32) |col| { + const temp = image[row * 32 + col]; + if (temp > max_temp) { + max_temp = temp; + hot_row = row; + hot_col = col; + } + } + } + return .{ .row = hot_row, .col = hot_col, .temp = max_temp }; +} + +inline fn draw_crosshair(fb: *display.ssd1306.Framebuffer, cx: i16, cy: i16) void { + for (0..10) |d| { + const offset: i16 = @as(i16, @intCast(d)) - 5; + const hx = cx + offset; + const vy = cy + offset; + if (hx >= 0 and hx < 128) fb.set_pixel(@intCast(hx), @intCast(@as(u7, @intCast(cy))), .white); + if (vy >= 0 and vy < 64) fb.set_pixel(@intCast(@as(u7, @intCast(cx))), @intCast(vy), .white); + } +} + +fn init() !void { + uart_tx_pin.set_function(.uart); + uart.apply(.{ + .clock_config = rp2xxx.clock_config, + }); + + i2c0.apply(i2c.Config{ .clock_config = rp2xxx.clock_config }); + i2c1.apply(i2c.Config{ .clock_config = rp2xxx.clock_config }); + + rp2xxx.uart.init_logger(uart); + _ = pin_config.apply(); + + // i2c0: camera (GPIO4=SDA, GPIO5=SCL) + const i2c0_scl = gpio.num(5); + const i2c0_sda = gpio.num(4); + inline for (&.{ i2c0_scl, i2c0_sda }) |pin| { + pin.set_slew_rate(.slow); + pin.set_schmitt_trigger_enabled(true); + pin.set_function(.i2c); + } + + // i2c1: display (GPIO2=SDA, GPIO3=SCL) + const i2c1_scl = gpio.num(3); + const i2c1_sda = gpio.num(2); + inline for (&.{ i2c1_scl, i2c1_sda }) |pin| { + pin.set_slew_rate(.slow); + pin.set_schmitt_trigger_enabled(true); + pin.set_function(.i2c); + } +} diff --git a/examples/raspberrypi/rp2xxx/src/mlx90640_image.zig b/examples/raspberrypi/rp2xxx/src/mlx90640_image.zig new file mode 100644 index 000000000..399a89117 --- /dev/null +++ b/examples/raspberrypi/rp2xxx/src/mlx90640_image.zig @@ -0,0 +1,121 @@ +const std = @import("std"); +const microzig = @import("microzig"); +const sensor = microzig.drivers.sensor; +const display = microzig.drivers.display; +const rp2xxx = microzig.hal; +const gpio = rp2xxx.gpio; +const i2c = rp2xxx.i2c; +const I2C_Device = rp2xxx.drivers.I2C_Device; +const MLX90640 = sensor.MLX90640; +const time = rp2xxx.time; + +const uart = rp2xxx.uart.instance.num(0); +const uart_tx_pin = gpio.num(0); + +var i2c0 = i2c.instance.num(0); +var i2c1 = i2c.instance.num(1); + +const pin_config = rp2xxx.pins.GlobalConfiguration{ + .GPIO0 = .{ .name = "gpio0", .function = .UART0_TX }, +}; + +pub const microzig_options = microzig.Options{ + .log_level = .debug, + .logFn = rp2xxx.uart.log, +}; + +pub fn main() !void { + try init(); + + var i2c_device = I2C_Device.init(i2c1, null); + + var camera = try MLX90640.init(.{ + .i2c = i2c_device.i2c_device(), + .address = @enumFromInt(0x33), + .clock = rp2xxx.drivers.clock_device(), + }); + + try camera.set_refresh_rate(0b101); + + const i2c_dd = rp2xxx.drivers.I2C_Datagram_Device.init(i2c0, @enumFromInt(0x3C), null); + const lcd = try display.ssd1306.init(.i2c, i2c_dd, null); + try lcd.clear_screen(false); + + var fb = display.ssd1306.Framebuffer.init(.black); + var image: [768]f32 = undefined; + + while (true) { + camera.image(&image) catch |err| { + std.log.err("unable to read image: {}", .{err}); + time.sleep_ms(100); + continue; + }; + + const min_max = min_max_temp(&image); + const threshold = min_max.min + (min_max.max - min_max.min) * 0.5; + + fb.clear(.black); + scale_128_x_64(&fb, &image, threshold); + + try lcd.write_full_display(fb.bit_stream()); + time.sleep_ms(50); + } +} + +fn min_max_temp(image: *const [768]f32) struct { min: f32, max: f32 } { + var min: f32 = image[0]; + var max: f32 = image[0]; + for (0..24) |row| { + for (0..32) |col| { + const temp = image[row * 32 + col]; + if (temp < min) min = temp; + if (temp > max) max = temp; + } + } + return .{ .min = min, .max = max }; +} + +// Scale 24×32 thermal image to 128×64 framebuffer +fn scale_128_x_64(fb: *display.ssd1306.Framebuffer, image: *const [768]f32, threshold: f32) void { + for (0..64) |y| { + const cam_row: usize = y * 24 / 64; + for (0..128) |x| { + const cam_col: usize = 31 - (x * 32 / 128); + const temp = image[cam_row * 32 + cam_col]; + if (temp >= threshold) { + fb.set_pixel(@intCast(x), @intCast(y), .white); + } + } + } +} + +fn init() !void { + uart_tx_pin.set_function(.uart); + uart.apply(.{ + .clock_config = rp2xxx.clock_config, + }); + + i2c0.apply(i2c.Config{ .clock_config = rp2xxx.clock_config }); + i2c1.apply(i2c.Config{ .clock_config = rp2xxx.clock_config }); + + rp2xxx.uart.init_logger(uart); + _ = pin_config.apply(); + + // i2c0: camera (GPIO4=SDA, GPIO5=SCL) + const i2c0_scl = gpio.num(5); + const i2c0_sda = gpio.num(4); + inline for (&.{ i2c0_scl, i2c0_sda }) |pin| { + pin.set_slew_rate(.slow); + pin.set_schmitt_trigger_enabled(true); + pin.set_function(.i2c); + } + + // i2c1: display (GPIO2=SDA, GPIO3=SCL) + const i2c1_scl = gpio.num(3); + const i2c1_sda = gpio.num(2); + inline for (&.{ i2c1_scl, i2c1_sda }) |pin| { + pin.set_slew_rate(.slow); + pin.set_schmitt_trigger_enabled(true); + pin.set_function(.i2c); + } +}