|
| 1 | +const std = @import("std"); |
| 2 | +const microzig = @import("microzig"); |
| 3 | +const sensor = microzig.drivers.sensor; |
| 4 | +const display = microzig.drivers.display; |
| 5 | +const rp2xxx = microzig.hal; |
| 6 | +const gpio = rp2xxx.gpio; |
| 7 | +const i2c = rp2xxx.i2c; |
| 8 | +const I2C_Device = rp2xxx.drivers.I2C_Device; |
| 9 | +const MLX90640 = sensor.MLX90640; |
| 10 | +const time = rp2xxx.time; |
| 11 | + |
| 12 | +const uart = rp2xxx.uart.instance.num(0); |
| 13 | +const uart_tx_pin = gpio.num(0); |
| 14 | + |
| 15 | +var i2c0 = i2c.instance.num(0); |
| 16 | +var i2c1 = i2c.instance.num(1); |
| 17 | + |
| 18 | +const pin_config = rp2xxx.pins.GlobalConfiguration{ |
| 19 | + .GPIO0 = .{ .name = "gpio0", .function = .UART0_TX }, |
| 20 | +}; |
| 21 | + |
| 22 | +pub const microzig_options = microzig.Options{ |
| 23 | + .log_level = .debug, |
| 24 | + .logFn = rp2xxx.uart.log, |
| 25 | +}; |
| 26 | + |
| 27 | +pub fn main() !void { |
| 28 | + try init(); |
| 29 | + |
| 30 | + var i2c_device = I2C_Device.init(i2c1, null); |
| 31 | + |
| 32 | + var camera = try MLX90640.init(.{ |
| 33 | + .i2c = i2c_device.i2c_device(), |
| 34 | + .address = @enumFromInt(0x33), |
| 35 | + .clock = rp2xxx.drivers.clock_device(), |
| 36 | + }); |
| 37 | + |
| 38 | + try camera.set_refresh_rate(0b101); |
| 39 | + |
| 40 | + const i2c_dd = rp2xxx.drivers.I2C_Datagram_Device.init(i2c0, @enumFromInt(0x3C), null); |
| 41 | + const lcd = try display.ssd1306.init(.i2c, i2c_dd, null); |
| 42 | + try lcd.clear_screen(false); |
| 43 | + |
| 44 | + var fb = display.ssd1306.Framebuffer.init(.black); |
| 45 | + var image: [768]f32 = undefined; |
| 46 | + |
| 47 | + while (true) { |
| 48 | + camera.temperature(&image) catch |err| { |
| 49 | + std.log.err("unable to read image: {}", .{err}); |
| 50 | + time.sleep_ms(100); |
| 51 | + continue; |
| 52 | + }; |
| 53 | + |
| 54 | + const centroid = find_hottest_cluster_centroid(&image); |
| 55 | + const pos = camera_to_display(centroid.row, centroid.col); |
| 56 | + |
| 57 | + fb.clear(.black); |
| 58 | + draw_crosshair(&fb, pos.x, pos.y); |
| 59 | + |
| 60 | + try lcd.write_full_display(fb.bit_stream()); |
| 61 | + time.sleep_ms(50); |
| 62 | + } |
| 63 | +} |
| 64 | + |
| 65 | +inline fn camera_to_display(row: f32, col: f32) struct { x: i16, y: i16 } { |
| 66 | + return .{ |
| 67 | + .x = @intFromFloat((31.0 - col) * 128.0 / 32.0 + 2.0), |
| 68 | + .y = @intFromFloat(row * 64.0 / 24.0 + 1.0), |
| 69 | + }; |
| 70 | +} |
| 71 | + |
| 72 | +inline fn find_hottest_cluster_centroid(image: *const [768]f32) struct { row: f32, col: f32 } { |
| 73 | + var max_temp: f32 = image[0]; |
| 74 | + for (image) |temp| { |
| 75 | + if (temp > max_temp) max_temp = temp; |
| 76 | + } |
| 77 | + |
| 78 | + const threshold = max_temp - 2.0; |
| 79 | + |
| 80 | + var hot: [768]bool = undefined; |
| 81 | + for (0..768) |i| { |
| 82 | + hot[i] = image[i] >= threshold; |
| 83 | + } |
| 84 | + |
| 85 | + var visited: [768]bool = .{false} ** 768; |
| 86 | + var queue: [768]u16 = undefined; |
| 87 | + |
| 88 | + var best_sum_row: f32 = 0; |
| 89 | + var best_sum_col: f32 = 0; |
| 90 | + var best_sum_weight: f32 = 0; |
| 91 | + var best_count: usize = 0; |
| 92 | + |
| 93 | + for (0..768) |start| { |
| 94 | + if (!hot[start] or visited[start]) continue; |
| 95 | + |
| 96 | + var sum_row: f32 = 0; |
| 97 | + var sum_col: f32 = 0; |
| 98 | + var sum_weight: f32 = 0; |
| 99 | + var count: usize = 0; |
| 100 | + var head: usize = 0; |
| 101 | + var tail: usize = 0; |
| 102 | + |
| 103 | + queue[tail] = @intCast(start); |
| 104 | + tail += 1; |
| 105 | + visited[start] = true; |
| 106 | + |
| 107 | + while (head < tail) { |
| 108 | + const cur = queue[head]; |
| 109 | + head += 1; |
| 110 | + const r: i32 = @intCast(cur / 32); |
| 111 | + const c: i32 = @intCast(cur % 32); |
| 112 | + const weight = image[cur]; |
| 113 | + sum_row += @as(f32, @floatFromInt(r)) * weight; |
| 114 | + sum_col += @as(f32, @floatFromInt(c)) * weight; |
| 115 | + sum_weight += weight; |
| 116 | + count += 1; |
| 117 | + |
| 118 | + const deltas = [_][2]i32{ .{ 0, 1 }, .{ 0, -1 }, .{ 1, 0 }, .{ -1, 0 } }; |
| 119 | + for (deltas) |d| { |
| 120 | + const nr = r + d[0]; |
| 121 | + const nc = c + d[1]; |
| 122 | + if (nr >= 0 and nr < 24 and nc >= 0 and nc < 32) { |
| 123 | + const ni: usize = @intCast(@as(u32, @intCast(nr)) * 32 + @as(u32, @intCast(nc))); |
| 124 | + if (hot[ni] and !visited[ni]) { |
| 125 | + visited[ni] = true; |
| 126 | + queue[tail] = @intCast(ni); |
| 127 | + tail += 1; |
| 128 | + } |
| 129 | + } |
| 130 | + } |
| 131 | + } |
| 132 | + |
| 133 | + if (count > best_count) { |
| 134 | + best_count = count; |
| 135 | + best_sum_row = sum_row; |
| 136 | + best_sum_col = sum_col; |
| 137 | + best_sum_weight = sum_weight; |
| 138 | + } |
| 139 | + } |
| 140 | + |
| 141 | + if (best_sum_weight > 0) { |
| 142 | + return .{ |
| 143 | + .row = best_sum_row / best_sum_weight, |
| 144 | + .col = best_sum_col / best_sum_weight, |
| 145 | + }; |
| 146 | + } else { |
| 147 | + return .{ .row = 12.0, .col = 16.0 }; |
| 148 | + } |
| 149 | +} |
| 150 | + |
| 151 | +inline fn draw_crosshair(fb: *display.ssd1306.Framebuffer, cx: i16, cy: i16) void { |
| 152 | + for (0..10) |d| { |
| 153 | + const offset: i16 = @as(i16, @intCast(d)) - 5; |
| 154 | + const hx = cx + offset; |
| 155 | + const vy = cy + offset; |
| 156 | + if (hx >= 0 and hx < 128) fb.set_pixel(@intCast(hx), @intCast(@as(u7, @intCast(cy))), .white); |
| 157 | + if (vy >= 0 and vy < 64) fb.set_pixel(@intCast(@as(u7, @intCast(cx))), @intCast(vy), .white); |
| 158 | + } |
| 159 | +} |
| 160 | + |
| 161 | +fn init() !void { |
| 162 | + uart_tx_pin.set_function(.uart); |
| 163 | + uart.apply(.{ |
| 164 | + .clock_config = rp2xxx.clock_config, |
| 165 | + }); |
| 166 | + |
| 167 | + i2c0.apply(i2c.Config{ .clock_config = rp2xxx.clock_config }); |
| 168 | + i2c1.apply(i2c.Config{ .clock_config = rp2xxx.clock_config }); |
| 169 | + |
| 170 | + rp2xxx.uart.init_logger(uart); |
| 171 | + _ = pin_config.apply(); |
| 172 | + |
| 173 | + // i2c0: camera (GPIO4=SDA, GPIO5=SCL) |
| 174 | + const i2c0_scl = gpio.num(5); |
| 175 | + const i2c0_sda = gpio.num(4); |
| 176 | + inline for (&.{ i2c0_scl, i2c0_sda }) |pin| { |
| 177 | + pin.set_slew_rate(.slow); |
| 178 | + pin.set_schmitt_trigger_enabled(true); |
| 179 | + pin.set_function(.i2c); |
| 180 | + } |
| 181 | + |
| 182 | + // i2c1: display (GPIO2=SDA, GPIO3=SCL) |
| 183 | + const i2c1_scl = gpio.num(3); |
| 184 | + const i2c1_sda = gpio.num(2); |
| 185 | + inline for (&.{ i2c1_scl, i2c1_sda }) |pin| { |
| 186 | + pin.set_slew_rate(.slow); |
| 187 | + pin.set_schmitt_trigger_enabled(true); |
| 188 | + pin.set_function(.i2c); |
| 189 | + } |
| 190 | +} |
0 commit comments