EmbeddedCAM/changes.patch

diff --git a/.vscode/settings.json b/.vscode/settings.json
new file mode 100644
index 0000000..9e4a950
--- /dev/null
+++ b/.vscode/settings.json
@@ -0,0 +1,23 @@
+{
+    "files.associations": {
+        "**/debian/*.install": "plain",
+        "charconv": "cpp",
+        "chrono": "cpp",
+        "optional": "cpp",
+        "format": "cpp",
+        "ratio": "cpp",
+        "system_error": "cpp",
+        "array": "cpp",
+        "functional": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "utility": "cpp",
+        "__bit_reference": "cpp",
+        "__node_handle": "cpp",
+        "deque": "cpp",
+        "limits": "cpp",
+        "locale": "cpp",
+        "vector": "cpp",
+        "algorithm": "cpp"
+    }
+}
\ No newline at end of file
diff --git a/CameraWebServer.ino b/EmbeddedCAM.ino
similarity index 79%
rename from CameraWebServer.ino
rename to EmbeddedCAM.ino
index 6ad04cd..4f58df6 100644
--- a/CameraWebServer.ino
+++ b/EmbeddedCAM.ino
@@ -3,7 +3,7 @@
 
 #include <stdbool.h>
 #include "esp_system.h"
-#include "driver/i2c.h"
+#include "driver/i2c_master.h"
 
 #include "src/lipton/cci.h"
 #include "src/lipton/vospi.h"
@@ -28,27 +28,26 @@
 // ===================
 // Select camera model
 // ===================
-//#define CAMERA_MODEL_WROVER_KIT // Has PSRAM
-//#define CAMERA_MODEL_ESP_EYE  // Has PSRAM
+// #define CAMERA_MODEL_WROVER_KIT // Has PSRAM
+// #define CAMERA_MODEL_ESP_EYE  // Has PSRAM
 #define CAMERA_MODEL_ESP32S3_EYE // Has PSRAM
-//#define CAMERA_MODEL_M5STACK_PSRAM // Has PSRAM
-//#define CAMERA_MODEL_M5STACK_V2_PSRAM // M5Camera version B Has PSRAM
-//#define CAMERA_MODEL_M5STACK_WIDE // Has PSRAM
-//#define CAMERA_MODEL_M5STACK_ESP32CAM // No PSRAM
-//#define CAMERA_MODEL_M5STACK_UNITCAM // No PSRAM
-//#define CAMERA_MODEL_M5STACK_CAMS3_UNIT  // Has PSRAM
-//#define CAMERA_MODEL_AI_THINKER // Has PSRAM
-//#define CAMERA_MODEL_TTGO_T_JOURNAL // No PSRAM
-//#define CAMERA_MODEL_XIAO_ESP32S3 // Has PSRAM
-// ** Espressif Internal Boards **
-//#define CAMERA_MODEL_ESP32_CAM_BOARD
-//#define CAMERA_MODEL_ESP32S2_CAM_BOARD
-//#define CAMERA_MODEL_ESP32S3_CAM_LCD
-//#define CAMERA_MODEL_DFRobot_FireBeetle2_ESP32S3 // Has PSRAM
-//#define CAMERA_MODEL_DFRobot_Romeo_ESP32S3 // Has PSRAM
+// #define CAMERA_MODEL_M5STACK_PSRAM // Has PSRAM
+// #define CAMERA_MODEL_M5STACK_V2_PSRAM // M5Camera version B Has PSRAM
+// #define CAMERA_MODEL_M5STACK_WIDE // Has PSRAM
+// #define CAMERA_MODEL_M5STACK_ESP32CAM // No PSRAM
+// #define CAMERA_MODEL_M5STACK_UNITCAM // No PSRAM
+// #define CAMERA_MODEL_M5STACK_CAMS3_UNIT  // Has PSRAM
+// #define CAMERA_MODEL_AI_THINKER // Has PSRAM
+// #define CAMERA_MODEL_TTGO_T_JOURNAL // No PSRAM
+// #define CAMERA_MODEL_XIAO_ESP32S3 // Has PSRAM
+//  ** Espressif Internal Boards **
+// #define CAMERA_MODEL_ESP32_CAM_BOARD
+// #define CAMERA_MODEL_ESP32S2_CAM_BOARD
+// #define CAMERA_MODEL_ESP32S3_CAM_LCD
+// #define CAMERA_MODEL_DFRobot_FireBeetle2_ESP32S3 // Has PSRAM
+// #define CAMERA_MODEL_DFRobot_Romeo_ESP32S3 // Has PSRAM
 #include "camera_pins.h"
 
-
 //
 // LEP Task constants
 //
@@ -69,25 +68,25 @@ int64_t vsyncDetectedUsec;
 
 WiFiServer lipSrv(8078);
 
-
 // ===========================
 // Enter your WiFi credentials
 // ===========================
-const char *ssid = "ssid";
-const char *password = "password";
+const char *ssid = "CansatDev";
+const char *password = "plechovka";
 
 void startCameraServer();
 void setupLedFlash(int pin);
 
-void setup() {
+void setup()
+{
   gpio_config_t usb_phy_conf = {
-    .pin_bit_mask = (1ULL << 19) | (1ULL << 20),
-    .mode = GPIO_MODE_OUTPUT,
-    .pull_up_en = GPIO_PULLUP_ENABLE,
-    .pull_down_en = GPIO_PULLDOWN_DISABLE,
-    .intr_type = GPIO_INTR_DISABLE,
-};
-gpio_config(&usb_phy_conf);
+      .pin_bit_mask = (1ULL << 19) | (1ULL << 20),
+      .mode = GPIO_MODE_OUTPUT,
+      .pull_up_en = GPIO_PULLUP_ENABLE,
+      .pull_down_en = GPIO_PULLDOWN_DISABLE,
+      .intr_type = GPIO_INTR_DISABLE,
+  };
+  gpio_config(&usb_phy_conf);
 
   printf("[MAIN] Start task\n");
 
@@ -142,6 +141,8 @@ gpio_config(&usb_phy_conf);
   // disable data from lepton
   digitalWrite(LEP_CSN_PIN, 1);
 
+  printf("LEPTON INIT DONE");
+
   camera_config_t config;
   config.ledc_channel = LEDC_CHANNEL_0;
   config.ledc_timer = LEDC_TIMER_0;
@@ -160,14 +161,12 @@ gpio_config(&usb_phy_conf);
   config.pin_sccb_sda = -1;
   config.pin_sccb_scl = -1;
   config.sccb_i2c_port = I2C_MASTER_NUM;
-  config.pin_sccb_sda = -1;
-  config.sccb_i2c_port = 1;
   config.pin_pwdn = PWDN_GPIO_NUM;
   config.pin_reset = RESET_GPIO_NUM;
   config.xclk_freq_hz = 20000000;
   config.frame_size = FRAMESIZE_UXGA;
-  config.pixel_format = PIXFORMAT_JPEG;  // for streaming
-  //config.pixel_format = PIXFORMAT_RGB565; // for face detection/recognition
+  config.pixel_format = PIXFORMAT_JPEG; // for streaming
+  // config.pixel_format = PIXFORMAT_RGB565; // for face detection/recognition
   config.grab_mode = CAMERA_GRAB_WHEN_EMPTY;
   config.fb_location = CAMERA_FB_IN_PSRAM;
   config.jpeg_quality = 12;
@@ -175,17 +174,23 @@ gpio_config(&usb_phy_conf);
 
   // if PSRAM IC present, init with UXGA resolution and higher JPEG quality
   //                      for larger pre-allocated frame buffer.
-  if (config.pixel_format == PIXFORMAT_JPEG) {
-    if (psramFound()) {
+  if (config.pixel_format == PIXFORMAT_JPEG)
+  {
+    if (psramFound())
+    {
       config.jpeg_quality = 10;
       config.fb_count = 2;
       config.grab_mode = CAMERA_GRAB_LATEST;
-    } else {
+    }
+    else
+    {
       // Limit the frame size when PSRAM is not available
       config.frame_size = FRAMESIZE_SVGA;
       config.fb_location = CAMERA_FB_IN_DRAM;
     }
-  } else {
+  }
+  else
+  {
     // Best option for face detection/recognition
     config.frame_size = FRAMESIZE_240X240;
 #if CONFIG_IDF_TARGET_ESP32S3
@@ -200,20 +205,23 @@ gpio_config(&usb_phy_conf);
 
   // camera init
   esp_err_t err = esp_camera_init(&config);
-  if (err != ESP_OK) {
+  if (err != ESP_OK)
+  {
     printf("Camera init failed with error 0x%x", err);
     return;
   }
 
   sensor_t *s = esp_camera_sensor_get();
   // initial sensors are flipped vertically and colors are a bit saturated
-  if (s->id.PID == OV3660_PID) {
-    s->set_vflip(s, 1);        // flip it back
-    s->set_brightness(s, 1);   // up the brightness just a bit
-    s->set_saturation(s, -2);  // lower the saturation
+  if (s->id.PID == OV3660_PID)
+  {
+    s->set_vflip(s, 1);       // flip it back
+    s->set_brightness(s, 1);  // up the brightness just a bit
+    s->set_saturation(s, -2); // lower the saturation
   }
   // drop down frame size for higher initial frame rate
-  if (config.pixel_format == PIXFORMAT_JPEG) {
+  if (config.pixel_format == PIXFORMAT_JPEG)
+  {
     s->set_framesize(s, FRAMESIZE_QVGA);
   }
 
@@ -235,7 +243,8 @@ gpio_config(&usb_phy_conf);
   WiFi.setSleep(false);
 
   printf("WiFi connecting\n");
-  while (WiFi.status() != WL_CONNECTED) {
+  while (WiFi.status() != WL_CONNECTED)
+  {
     delay(500);
     printf(".");
   }
@@ -245,12 +254,11 @@ gpio_config(&usb_phy_conf);
   startCameraServer();
 
   printf("Camera Ready! Use 'http://");
-  printf(WiFi.localIP());
+  printf(WiFi.localIP().toString().c_str());
   lipSrv.begin();
   printf("' to connect\n");
 }
 
-
 uint8_t *take_picture()
 {
   digitalWrite(LEP_CSN_PIN, 0);
@@ -314,12 +322,12 @@ void lepton_restart()
   {
     delay(LEP_RESET_FAIL_RETRY_SECS * 1000);
     printf("LEPRESET\n\a");
-    
-    #ifdef LEP_RESET_PIN
+
+#ifdef LEP_RESET_PIN
     digitalWrite(LEP_RESET_PIN, LEP_RESET_ON);
     delay(10);
     digitalWrite(LEP_RESET_PIN, LEP_RESET_OFF);
-    #endif
+#endif
 
     // Delay for Lepton internal initialization (max 950 mSec)
     delay(1000);
diff --git a/src/lipton/cci.cpp b/src/lipton/cci.cpp
index 9100263..9559271 100644
--- a/src/lipton/cci.cpp
+++ b/src/lipton/cci.cpp
@@ -26,6 +26,7 @@
 #include <stdbool.h>
 #include <stdio.h>
 #include <Arduino.h>
+#include "lepton_system.h"
 
 #define CCI_MAX_WAIT_TICKS 5000
 
@@ -39,95 +40,101 @@ static uint16_t cci_last_status;
 // plus register starting address
 static uint8_t burst_buf[1026];
 
-
-
 //
 // Forward declarations for primitive access methods
 //
 static int cci_write_register(uint16_t reg, uint16_t value);
-static int cci_write_burst(uint16_t start, uint16_t word_len, uint16_t* buf);
+static int cci_write_burst(uint16_t start, uint16_t word_len, uint16_t *buf);
 static uint16_t cci_read_register(uint16_t reg);
-static int cci_read_burst(uint16_t start, uint16_t word_len, uint16_t* buf);
+static int cci_read_burst(uint16_t start, uint16_t word_len, uint16_t *buf);
 static uint32_t cci_wait_busy_clear();
-static void cci_wait_busy_clear_check(char* cmd);
-
-
+static void cci_wait_busy_clear_check(char *cmd);
 
 //
 // CCI API
 //
 
-
 /**
  * Write 0 (equivalent to run_cmd) to 512 16-bit words to the lepton and issue
  * the specified command.  Lengths > 16 words are written to the BLOCK data buffer.
  */
-void cci_set_reg(uint16_t cmd, int len, uint16_t* buf)
+void cci_set_reg(uint16_t cmd, int len, uint16_t *buf)
 {
-	char cmd_buf[11];   // sized for 'cmd 0xNNNN<NULL>'
+	char cmd_buf[11]; // sized for 'cmd 0xNNNN<NULL>'
 	int ret = 1;
-	
+
 	cci_last_status_error = false;
-	
+
 	cci_wait_busy_clear();
-	
-	if ((len > 0) && (len <= 16)) {
+
+	if ((len > 0) && (len <= 16))
+	{
 		ret = cci_write_burst(CCI_REG_DATA_0, len, buf);
-	} else if ((len > 16) && (len <= 512)) {
+	}
+	else if ((len > 16) && (len <= 512))
+	{
 		ret = cci_write_burst(CCI_BLOCK_BUF_0, len, buf);
-	} else if (len > 512) {
+	}
+	else if (len > 512)
+	{
 		ret = 0;
 	}
-	
-	if (ret == 1) {
-		if (len > 0) {
+
+	if (ret == 1)
+	{
+		if (len > 0)
+		{
 			sprintf(cmd_buf, "CMD 0x%4x\n", cmd);
 			cci_write_register(CCI_REG_DATA_LENGTH, len);
-		} else {
+		}
+		else
+		{
 			sprintf(cmd_buf, "RUN 0x%4x\n", cmd);
 		}
-		
+
 		cci_write_register(CCI_REG_COMMAND, cmd);
 		cci_wait_busy_clear_check(cmd_buf);
-	} else {
+	}
+	else
+	{
 		cci_last_status = 0;
 		cci_last_status_error = true;
 	}
 }
 
-
 /**
  * Read up to 512 16-bit words form the lepton with the specified command.  Lengths > 16
  * words are read from the BLOCK data buffer.
  */
-void cci_get_reg(uint16_t cmd, int len, uint16_t* buf)
+void cci_get_reg(uint16_t cmd, int len, uint16_t *buf)
 {
-	char cmd_buf[11];   // sized for 'cmd 0xNNNN<NULL>'
+	char cmd_buf[11]; // sized for 'cmd 0xNNNN<NULL>'
 
 	sprintf(cmd_buf, "CMD 0x%4x", cmd);
-	
+
 	cci_wait_busy_clear();
 	cci_write_register(CCI_REG_DATA_LENGTH, len);
 	cci_write_register(CCI_REG_COMMAND, cmd);
 	cci_wait_busy_clear_check(cmd_buf);
-	if ((len > 0) && (len <= 16)) {
-		(void) cci_read_burst(CCI_REG_DATA_0, len, buf);
-	} else if ((len > 16) && (len <= 512)) {
-		(void) cci_read_burst(CCI_BLOCK_BUF_0, len, buf);
+	if ((len > 0) && (len <= 16))
+	{
+		(void)cci_read_burst(CCI_REG_DATA_0, len, buf);
+	}
+	else if ((len > 16) && (len <= 512))
+	{
+		(void)cci_read_burst(CCI_BLOCK_BUF_0, len, buf);
 	}
 }
 
-
 /**
  * Return true if previous command succeeded as detected by cci_wait_busy_clear_check
  */
-bool cci_command_success(uint16_t* status)
+bool cci_command_success(uint16_t *status)
 {
 	*status = cci_last_status;
 	return !cci_last_status_error;
 }
 
-
 /**
  * Ping the camera.
  *   Returns 0 for a successful ping
@@ -138,24 +145,28 @@ uint32_t cci_run_ping()
 {
 	uint32_t res;
 	uint8_t lep_res;
-	
+
 	cci_wait_busy_clear();
 	cci_write_register(CCI_REG_COMMAND, CCI_CMD_SYS_RUN_PING);
 	res = cci_wait_busy_clear();
 
 	lep_res = (res & 0x000FF00) >> 8; // 8-bit Response Error Code: 0=LEP_OK
-	if (res == 0x00010000) {
+	if (res == 0x00010000)
+	{
 		return 0x100;
-	} else if (lep_res == 0x00) {
+	}
+	else if (lep_res == 0x00)
+	{
 		return 0;
-	} else {
+	}
+	else
+	{
 		// Convert negative Lepton Response Error Code to a positive number to return
 		lep_res = ~lep_res + 1;
 		return lep_res;
 	}
 }
 
-
 /**
  * Request that a flat field correction occur immediately.
  */
@@ -166,7 +177,6 @@ void cci_run_ffc()
 	cci_wait_busy_clear_check("CCI_CMD_SYS_RUN_FFC");
 }
 
-
 /**
  * Get the system uptime.
  */
@@ -176,13 +186,12 @@ uint32_t cci_get_uptime()
 	cci_write_register(CCI_REG_DATA_LENGTH, 2);
 	cci_write_register(CCI_REG_COMMAND, CCI_CMD_SYS_GET_UPTIME);
 	cci_wait_busy_clear_check("CCI_CMD_SYS_GET_UPTIME");
-	
+
 	uint16_t ls_word = cci_read_register(CCI_REG_DATA_0);
 	uint16_t ms_word = cci_read_register(CCI_REG_DATA_1);
 	return ms_word << 16 | ls_word;
 }
 
-
 /**
  * Get the AUX (case) temperature in Kelvin x 100 (16-bit result).
  */
@@ -192,13 +201,12 @@ uint32_t cci_get_aux_temp()
 	cci_write_register(CCI_REG_DATA_LENGTH, 2);
 	cci_write_register(CCI_REG_COMMAND, CCI_CMD_SYS_GET_AUX_TEMP);
 	cci_wait_busy_clear_check("CCI_CMD_SYS_GET_AUX_TEMP");
-	
+
 	uint16_t ls_word = cci_read_register(CCI_REG_DATA_0);
 	uint16_t ms_word = cci_read_register(CCI_REG_DATA_1);
 	return ms_word << 16 | ls_word;
 }
 
-
 /**
  * Get the FPA (sensor) temperature in Kelvin x 100 (16-bit result).
  */
@@ -208,20 +216,19 @@ uint32_t cci_get_fpa_temp()
 	cci_write_register(CCI_REG_DATA_LENGTH, 2);
 	cci_write_register(CCI_REG_COMMAND, CCI_CMD_SYS_GET_FPA_TEMP);
 	cci_wait_busy_clear_check("CCI_CMD_SYS_GET_FPA_TEMP");
-	
+
 	uint16_t ls_word = cci_read_register(CCI_REG_DATA_0);
 	uint16_t ms_word = cci_read_register(CCI_REG_DATA_1);
 	return ms_word << 16 | ls_word;
 }
 
-
 /**
  * Change the telemetry enable state.
  */
 void cci_set_telemetry_enable_state(cci_telemetry_enable_state_t state)
 {
 	uint32_t value = state;
-	
+
 	cci_wait_busy_clear();
 	cci_write_register(CCI_REG_DATA_0, value & 0xffff);
 	cci_write_register(CCI_REG_DATA_1, value >> 16 & 0xffff);
@@ -230,7 +237,6 @@ void cci_set_telemetry_enable_state(cci_telemetry_enable_state_t state)
 	cci_wait_busy_clear_check("CCI_CMD_SYS_SET_TELEMETRY_ENABLE_STATE");
 }
 
-
 /**
  * Get the telemetry enable state.
  */
@@ -240,20 +246,19 @@ uint32_t cci_get_telemetry_enable_state()
 	cci_write_register(CCI_REG_DATA_LENGTH, 2);
 	cci_write_register(CCI_REG_COMMAND, CCI_CMD_SYS_GET_TELEMETRY_ENABLE_STATE);
 	cci_wait_busy_clear_check("CCI_CMD_SYS_GET_TELEMETRY_ENABLE_STATE");
-	
+
 	uint16_t ls_word = cci_read_register(CCI_REG_DATA_0);
 	uint16_t ms_word = cci_read_register(CCI_REG_DATA_1);
 	return ms_word << 16 | ls_word;
 }
 
-
 /**
  * Change the telemetry location.
  */
 void cci_set_telemetry_location(cci_telemetry_location_t location)
 {
 	uint32_t value = location;
-	
+
 	cci_wait_busy_clear();
 	cci_write_register(CCI_REG_DATA_0, value & 0xffff);
 	cci_write_register(CCI_REG_DATA_1, value >> 16 & 0xffff);
@@ -262,7 +267,6 @@ void cci_set_telemetry_location(cci_telemetry_location_t location)
 	cci_wait_busy_clear_check("CCI_CMD_SYS_SET_TELEMETRY_LOCATION");
 }
 
-
 /**
  * Get the telemetry location.
  */
@@ -272,20 +276,19 @@ uint32_t cci_get_telemetry_location()
 	cci_write_register(CCI_REG_DATA_LENGTH, 2);
 	cci_write_register(CCI_REG_COMMAND, CCI_CMD_SYS_GET_TELEMETRY_LOCATION);
 	cci_wait_busy_clear_check("CCI_CMD_SYS_GET_TELEMETRY_LOCATION");
-	
+
 	uint16_t ls_word = cci_read_register(CCI_REG_DATA_0);
 	uint16_t ms_word = cci_read_register(CCI_REG_DATA_1);
 	return ms_word << 16 | ls_word;
 }
 
-
 /**
  * Get the Gain Mode
  */
 void cci_set_gain_mode(cc_gain_mode_t mode)
 {
 	uint32_t value = mode;
-	
+
 	cci_wait_busy_clear();
 	cci_write_register(CCI_REG_DATA_0, value & 0xffff);
 	cci_write_register(CCI_REG_DATA_1, value >> 16 & 0xffff);
@@ -294,7 +297,6 @@ void cci_set_gain_mode(cc_gain_mode_t mode)
 	cci_wait_busy_clear_check("CCI_CMD_SYS_SET_GAIN_MODE");
 }
 
-
 /**
  * Set the gain mode
  */
@@ -304,20 +306,19 @@ uint32_t cci_get_gain_mode()
 	cci_write_register(CCI_REG_DATA_LENGTH, 2);
 	cci_write_register(CCI_REG_COMMAND, CCI_CMD_SYS_GET_GAIN_MODE);
 	cci_wait_busy_clear_check("CCI_CMD_SYS_GET_GAIN_MODE");
-	
+
 	uint16_t ls_word = cci_read_register(CCI_REG_DATA_0);
 	uint16_t ms_word = cci_read_register(CCI_REG_DATA_1);
 	return ms_word << 16 | ls_word;
 }
 
-
 /**
  * Change the radiometry enable state.
  */
 void cci_set_radiometry_enable_state(cci_radiometry_enable_state_t state)
 {
 	uint32_t value = state;
-	
+
 	cci_wait_busy_clear();
 	cci_write_register(CCI_REG_DATA_0, value & 0xffff);
 	cci_write_register(CCI_REG_DATA_1, value >> 16 & 0xffff);
@@ -326,7 +327,6 @@ void cci_set_radiometry_enable_state(cci_radiometry_enable_state_t state)
 	cci_wait_busy_clear_check("CCI_CMD_RAD_SET_RADIOMETRY_ENABLE_STATE");
 }
 
-
 /**
  * Get the radiometry enable state.
  */
@@ -336,17 +336,16 @@ uint32_t cci_get_radiometry_enable_state()
 	cci_write_register(CCI_REG_DATA_LENGTH, 2);
 	cci_write_register(CCI_REG_COMMAND, CCI_CMD_RAD_GET_RADIOMETRY_ENABLE_STATE);
 	cci_wait_busy_clear_check("CCI_CMD_RAD_GET_RADIOMETRY_ENABLE_STATE");
-	
+
 	uint16_t ls_word = cci_read_register(CCI_REG_DATA_0);
 	uint16_t ms_word = cci_read_register(CCI_REG_DATA_1);
 	return ms_word << 16 | ls_word;
 }
 
-
 /**
  * Set the radiometry flux parameters
  */
-void cci_set_radiometry_flux_linear_params(cci_rad_flux_linear_params_t* params)
+void cci_set_radiometry_flux_linear_params(cci_rad_flux_linear_params_t *params)
 {
 	cci_wait_busy_clear();
 	cci_write_register(CCI_REG_DATA_0, params->sceneEmissivity);
@@ -362,11 +361,10 @@ void cci_set_radiometry_flux_linear_params(cci_rad_flux_linear_params_t* params)
 	cci_wait_busy_clear_check("CCI_CMD_RAD_SET_RADIOMETRY_FLUX_LINEAR_PARAMS");
 }
 
-
 /**
  * Get the radiometry flux parameters
  */
-bool cci_get_radiometry_flux_linear_params(cci_rad_flux_linear_params_t* params)
+bool cci_get_radiometry_flux_linear_params(cci_rad_flux_linear_params_t *params)
 {
 	cci_wait_busy_clear();
 	cci_write_register(CCI_REG_DATA_LENGTH, 8);
@@ -380,18 +378,17 @@ bool cci_get_radiometry_flux_linear_params(cci_rad_flux_linear_params_t* params)
 	params->TAtmK = cci_read_register(CCI_REG_DATA_5);
 	params->reflWindow = cci_read_register(CCI_REG_DATA_6);
 	params->TReflK = cci_read_register(CCI_REG_DATA_7);
-	
+
 	return !cci_last_status_error;
 }
 
-
 /**
  * Change the radiometry TLinear enable state.
  */
 void cci_set_radiometry_tlinear_enable_state(cci_radiometry_tlinear_enable_state_t state)
 {
 	uint32_t value = state;
-	
+
 	cci_wait_busy_clear();
 	cci_write_register(CCI_REG_DATA_0, value & 0xffff);
 	cci_write_register(CCI_REG_DATA_1, value >> 16 & 0xffff);
@@ -400,7 +397,6 @@ void cci_set_radiometry_tlinear_enable_state(cci_radiometry_tlinear_enable_state
 	cci_wait_busy_clear_check("CCI_CMD_RAD_SET_RADIOMETRY_TLINEAR_ENABLE_STATE");
 }
 
-
 /**
  * Get the radiometry TLinear enable state.
  */
@@ -410,20 +406,19 @@ uint32_t cci_get_radiometry_tlinear_enable_state()
 	cci_write_register(CCI_REG_DATA_LENGTH, 2);
 	cci_write_register(CCI_REG_COMMAND, CCI_CMD_RAD_GET_RADIOMETRY_TLINEAR_ENABLE_STATE);
 	cci_wait_busy_clear_check("CCI_CMD_RAD_GET_RADIOMETRY_TLINEAR_ENABLE_STATE");
-	
+
 	uint16_t ls_word = cci_read_register(CCI_REG_DATA_0);
 	uint16_t ms_word = cci_read_register(CCI_REG_DATA_1);
 	return ms_word << 16 | ls_word;
 }
 
-
 /**
  * Set the radiometry TLinear Auto Resolution
  */
 void cci_set_radiometry_tlinear_auto_res(cci_radiometry_tlinear_auto_res_state_t state)
 {
 	uint32_t value = state;
-	
+
 	cci_wait_busy_clear();
 	cci_write_register(CCI_REG_DATA_0, value & 0xffff);
 	cci_write_register(CCI_REG_DATA_1, value >> 16 & 0xffff);
@@ -432,7 +427,6 @@ void cci_set_radiometry_tlinear_auto_res(cci_radiometry_tlinear_auto_res_state_t
 	cci_wait_busy_clear_check("CCI_CMD_RAD_SET_RADIOMETRY_TLINEAR_AUTO_RES");
 }
 
-
 /**
  * Get the radiometry TLinear Auto Resolution
  */
@@ -442,13 +436,12 @@ uint32_t cci_get_radiometry_tlinear_auto_res()
 	cci_write_register(CCI_REG_DATA_LENGTH, 2);
 	cci_write_register(CCI_REG_COMMAND, CCI_CMD_RAD_GET_RADIOMETRY_TLINEAR_AUTO_RES);
 	cci_wait_busy_clear_check("CCI_CMD_RAD_GET_RADIOMETRY_TLINEAR_AUTO_RES");
-	
+
 	uint16_t ls_word = cci_read_register(CCI_REG_DATA_0);
 	uint16_t ms_word = cci_read_register(CCI_REG_DATA_1);
 	return ms_word << 16 | ls_word;
 }
 
-
 /**
  * Set the Radiometry Spotmeter Region-of-interest
  */
@@ -464,11 +457,10 @@ void cci_set_radiometry_spotmeter(uint16_t r1, uint16_t c1, uint16_t r2, uint16_
 	cci_wait_busy_clear_check("CCI_CMD_RAD_SET_RADIOMETRY_SPOT_ROI");
 }
 
-
 /**
  * Get the Radiometry Spotmeter Region-of-interest
  */
-bool cci_get_radiometry_spotmeter(uint16_t* r1, uint16_t* c1, uint16_t* r2, uint16_t* c2)
+bool cci_get_radiometry_spotmeter(uint16_t *r1, uint16_t *c1, uint16_t *r2, uint16_t *c2)
 {
 	cci_wait_busy_clear();
 	cci_write_register(CCI_REG_DATA_LENGTH, 4);
@@ -478,11 +470,10 @@ bool cci_get_radiometry_spotmeter(uint16_t* r1, uint16_t* c1, uint16_t* r2, uint
 	*c1 = cci_read_register(CCI_REG_DATA_1);
 	*r2 = cci_read_register(CCI_REG_DATA_2);
 	*c2 = cci_read_register(CCI_REG_DATA_3);
-	
+
 	return !cci_last_status_error;
 }
 
-
 /**
  * Get the AGC enable state.
  */
@@ -492,20 +483,19 @@ uint32_t cci_get_agc_enable_state()
 	cci_write_register(CCI_REG_DATA_LENGTH, 2);
 	cci_write_register(CCI_REG_COMMAND, CCI_CMD_AGC_GET_AGC_ENABLE_STATE);
 	cci_wait_busy_clear_check("CCI_CMD_AGC_GET_AGC_ENABLE_STATE");
-	
+
 	uint16_t ls_word = cci_read_register(CCI_REG_DATA_0);
 	uint16_t ms_word = cci_read_register(CCI_REG_DATA_1);
 	return ms_word << 16 | ls_word;
 }
 
-
 /**
  * Set the AGC enable state.
  */
 void cci_set_agc_enable_state(cci_agc_enable_state_t state)
 {
 	uint32_t value = state;
-	
+
 	cci_wait_busy_clear();
 	cci_write_register(CCI_REG_DATA_0, value & 0xffff);
 	cci_write_register(CCI_REG_DATA_1, value >> 16 & 0xffff);
@@ -514,7 +504,6 @@ void cci_set_agc_enable_state(cci_agc_enable_state_t state)
 	cci_wait_busy_clear_check("CCI_CMD_AGC_SET_AGC_ENABLE_STATE");
 }
 
-
 /**
  * Get the AGC calc enable state.
  */
@@ -524,20 +513,19 @@ uint32_t cci_get_agc_calc_enable_state()
 	cci_write_register(CCI_REG_DATA_LENGTH, 2);
 	cci_write_register(CCI_REG_COMMAND, CCI_CMD_AGC_GET_CALC_ENABLE_STATE);
 	cci_wait_busy_clear_check("CCI_CMD_AGC_GET_CALC_ENABLE_STATE");
-	
+
 	uint16_t ls_word = cci_read_register(CCI_REG_DATA_0);
 	uint16_t ms_word = cci_read_register(CCI_REG_DATA_1);
 	return ms_word << 16 | ls_word;
 }
 
-
 /**
  * Set the AGC calc enable state.
  */
 void cci_set_agc_calc_enable_state(cci_agc_enable_state_t state)
 {
 	uint32_t value = state;
-	
+
 	cci_wait_busy_clear();
 	cci_write_register(CCI_REG_DATA_0, value & 0xffff);
 	cci_write_register(CCI_REG_DATA_1, value >> 16 & 0xffff);
@@ -558,7 +546,6 @@ void cc_run_oem_reboot()
 	cci_wait_busy_clear_check("CCI_CMD_OEM_RUN_REBOOT");
 }
 
-
 /**
  * Get the GPIO mode.
  */
@@ -566,22 +553,21 @@ uint32_t cci_get_gpio_mode()
 {
 	cci_wait_busy_clear();
 	cci_write_register(CCI_REG_DATA_LENGTH, 2);
- 	cci_write_register(CCI_REG_COMMAND, CCI_CMD_OEM_GET_GPIO_MODE);
+	cci_write_register(CCI_REG_COMMAND, CCI_CMD_OEM_GET_GPIO_MODE);
 	cci_wait_busy_clear_check("CCI_CMD_OEM_GET_GPIO_MODE");
-	
+
 	uint16_t ls_word = cci_read_register(CCI_REG_DATA_0);
 	uint16_t ms_word = cci_read_register(CCI_REG_DATA_1);
 	return ms_word << 16 | ls_word;
 }
 
-
 /**
  * Set the GPIO mode.
  */
 void cci_set_gpio_mode(cci_gpio_mode_t mode)
 {
 	uint32_t value = mode;
-	
+
 	cci_wait_busy_clear();
 	cci_write_register(CCI_REG_DATA_0, value & 0xffff);
 	cci_write_register(CCI_REG_DATA_1, value >> 16 & 0xffff);
@@ -590,12 +576,11 @@ void cci_set_gpio_mode(cci_gpio_mode_t mode)
 	cci_wait_busy_clear_check("CCI_CMD_OEM_SET_GPIO_MODE");
 }
 
-
 /**
  * Get the FLIR systems part number
  *   - call with a 32-character buffer
  */
-void cci_get_part_number(char* pn)
+void cci_get_part_number(char *pn)
 {
 	bool low_half = true;
 	int i = 0;
@@ -604,21 +589,25 @@ void cci_get_part_number(char* pn)
 	int t = 0; // maximum tick count
 
 	cci_get_reg(CCI_CMD_OEM_GET_PART_NUM, 16, cci_buf);
-	
-	*pn = (char) (cci_buf[0] & 0xFF);
-	while ((*pn != 0) && (i<16) && (t++ < CCI_MAX_WAIT_TICKS)) {
+
+	*pn = (char)(cci_buf[0] & 0xFF);
+	while ((*pn != 0) && (i < 16) && (t++ < CCI_MAX_WAIT_TICKS))
+	{
 		low_half = !low_half;
-		if (low_half) {
-			*(++pn) = (char) (cci_buf[i] & 0xFF);
-		} else {
-			*(++pn) = (char) (cci_buf[i] >> 8);
+		if (low_half)
+		{
+			*(++pn) = (char)(cci_buf[i] & 0xFF);
+		}
+		else
+		{
+			*(++pn) = (char)(cci_buf[i] >> 8);
 			i++;
 		}
 	}
 	*(++pn) = 0;
 }
 
-
+#define TAG_CCI "[CCI]"
 
 //
 // Primitive access methods
@@ -634,98 +623,122 @@ static int cci_write_register(uint16_t reg, uint16_t value)
 		reg >> 8 & 0xff,
 		reg & 0xff,
 		value >> 8 & 0xff,
-		value & 0xff
-	};
-	if (i2c_master_write_slave(CCI_ADDRESS, write_buf, sizeof(write_buf)) != ESP_OK) {
-		printf("[CCI] Error: failed to write CCI register %02x with value %02x\n", reg, value);
+		value & 0xff};
+	esp_err_t ret = i2c_master_transmit(LEPTON_DEV_HANDLE, write_buf, sizeof(write_buf), I2C_TIMEOUT_MS_VALUE);
+	if (ret == ESP_OK)
+	{
+		ESP_LOGV(TAG_CCI, "Write to register 0x%04X successful (Value: 0x%04X)", reg, value);
+		return 1;
+	}
+	else
+	{
+		ESP_LOGE(TAG_CCI, "Write to register 0x%04X failed: %s", reg, esp_err_to_name(ret));
 		return -1;
-	};
-
-	return 1;
+	}
 }
 
-
 /**
  * Burst write a group of CCI data registers
  */
-static int cci_write_burst(uint16_t start, uint16_t word_len, uint16_t* buf)
+static int cci_write_burst(uint16_t start, uint16_t word_len, uint16_t *buf)
 {
 	int i;
-	
-	// Create the i2c transaction buffer 
-	burst_buf[0] = start >> 8;
-	burst_buf[1] = start & 0xFF;
-	for (i=1; i<=word_len; i++) {
-		burst_buf[i*2] = *buf >> 8;
-		burst_buf[i*2 + 1] = *buf++ & 0xFF;
+
+	size_t bufSize = sizeof(*buf) * (1 + word_len);
+	unsigned char *buffer = (unsigned char *)malloc(bufSize);
+
+	// Create the i2c transaction buffer
+	buffer[0] = start >> 8;
+	buffer[1] = start & 0xFF;
+	for (i = 1; i <= word_len; i++)
+	{
+		buffer[i * 2] = *buf >> 8;
+		buffer[i * 2 + 1] = *buf++ & 0xFF;
 	}
-	
-	// Execute the burs
-	if (i2c_master_write_slave(CCI_ADDRESS, burst_buf, word_len*2 + 2) != ESP_OK) {
 
-		printf("[CCI] Error: failed to burst write CCI register %02x with length %d\n", start, word_len);
+	esp_err_t ret = i2c_master_transmit(LEPTON_DEV_HANDLE, buffer, bufSize, I2C_TIMEOUT_MS_VALUE);
+	free(buffer);
+	if (ret == ESP_OK)
+	{
+		ESP_LOGV(TAG_CCI, "Burst write to register 0x%04X successful", start);
+		return 1;
+	}
+	else
+	{
+		ESP_LOGE(TAG_CCI, "Burst write to register 0x%04X failed: %s", start, esp_err_to_name(ret));
 		return -1;
-	};
-	
-	return 1;
+	}
 }
 
-
 /**
  * Read a CCI register.
  */
-static uint16_t cci_read_register(uint16_t reg)
-{
-	uint8_t buf[2];
 
-	// Write the register address
-	buf[0] = reg >> 8;
-	buf[1] = reg & 0xff;
+/**
+ * @brief Reads a 16-bit value from a specific register of an I2C device.
+ *
+ * @param dev_handle I2C device handle
+ * @param reg_addr Register address to read from
+ * @return value The register value
+ */
+uint16_t cci_read_register(uint16_t reg_addr)
+{
+	uint8_t buffer[2];
 
-	if (i2c_master_write_slave(CCI_ADDRESS, buf, sizeof(buf)) != ESP_OK) {
-		printf("[CCI] Error: failed to write CCI register %02x\n", reg);
-		return -1;
+	esp_err_t ret = i2c_master_transmit_receive(LEPTON_DEV_HANDLE, (uint8_t *)&reg_addr, sizeof(reg_addr), buffer, sizeof(buffer), I2C_TIMEOUT_MS_VALUE);
+	if (ret == ESP_OK)
+	{
+		uint16_t value = (((uint16_t)(buffer[0] & 0xFF)) << 8) | (buffer[1] & 0xFF);
+		ESP_LOGV(TAG_CCI, "Read from register 0x%04X successful (Value: 0x%04X)", reg_addr, value);
+		return value;
 	}
-
-	// Read
-	if (i2c_master_read_slave(CCI_ADDRESS, buf, sizeof(buf)) != ESP_OK) {
-		printf("[CCI] Error: failed to read from CCI register %02x\n", reg);
+	else
+	{
+		ESP_LOGE(TAG_CCI, "Read from register 0x%04X failed: %s", reg_addr, esp_err_to_name(ret));
+		return -1;
 	}
-
-	return buf[0] << 8 | buf[1];
 }
 
-
 /**
- * Burst read a group of CCI data registers
+ * @brief Reads a burst of 16-bit values from specific registers of an I2C device.
+ *
+ * @param dev_handle I2C device handle
+ * @param start Register address to read from
+ * @param word_len Register address to read from
+ * @return esp_err_t ESP_OK on success, or an error code
  */
-static int cci_read_burst(uint16_t start, uint16_t word_len, uint16_t* buf)
+int cci_read_burst(uint16_t start, uint16_t word_len, uint16_t *buf)
 {
-	int i;
-	
-	// Write the starting address
-	burst_buf[0] = start >> 8;
-	burst_buf[1] = start & 0xFF;
-	if (i2c_master_write_slave(CCI_ADDRESS, burst_buf, 2) != ESP_OK) {
-		printf("[CCI] Error: failed to write CCI register %02x\n", start);
+
+	if (!buf)
+	{
 		return -1;
 	}
-	
-	// Read
-	if (i2c_master_read_slave(CCI_ADDRESS, burst_buf, word_len*2) != ESP_OK) {
-		printf("[CCI] Error: failed to burst read from CCI register %02x with length %d\n", start, word_len);
-		return -1;
+	size_t bufSize = word_len * sizeof(*buf);
+	unsigned char *buffer = (unsigned char *)malloc(bufSize);
+	esp_err_t ret = i2c_master_transmit_receive(LEPTON_DEV_HANDLE, (uint8_t *)&start, sizeof(start), buffer, bufSize, I2C_TIMEOUT_MS_VALUE);
+	if (ret == ESP_OK)
+	{
+		ESP_LOGV(TAG_CCI, "Read from register 0x%04X successful", start);
+		for (int i = 0; i < word_len; i++)
+		{ // CHECK IF THIS IS NEEDED
+			*buf++ = buffer[i * 2] << 8 | buffer[i * 2 + 1];
+		}
+		free(buffer);
+		return 1;
 	}
-	
-	// Copy data out
-	for (i=0; i<word_len; i++) {
-		*buf++ = burst_buf[i*2] << 8 | burst_buf[i*2 + 1];
+	else
+	{
+		ESP_LOGE(TAG_CCI, "Read from register 0x%04X failed: %s", start, esp_err_to_name(ret));
+		return -1;
 	}
-	
-	return 1;
 }
 
-
+/**
+ * Wait for busy to be clear in the status register
+ *   Returns the 16-bit STATUS
+ *   Returns 0x00010000 if there is a communication failure
+ */
 /**
  * Wait for busy to be clear in the status register
  *   Returns the 16-bit STATUS
@@ -734,38 +747,44 @@ static int cci_read_burst(uint16_t start, uint16_t word_len, uint16_t* buf)
 static uint32_t cci_wait_busy_clear()
 {
 	bool err = false;
-	uint8_t buf[2] = {0x00, 0x07};
+	uint8_t tx_buf[2]; // Register address to read from
+	uint8_t rx_buf[2]; // Value buffer
 
-	int t = 0; // maximum tick count
+	uint16_t reg_addr = 0x0002; // STATUS register
+	int t = 0;
 
-	// Wait for booted, not busy
-	while (((buf[1] & 0x07) != 0x06) && !err) {
-		if (t++ >= CCI_MAX_WAIT_TICKS) {
+	while (!err)
+	{
+		if (t++ >= CCI_MAX_WAIT_TICKS)
+		{
 			err = true;
 			break;
 		}
 
-		// Write STATUS register address
-		buf[0] = 0x00;
-		buf[1] = 0x02;
-		
-		if (i2c_master_write_slave(CCI_ADDRESS, buf, sizeof(buf)) != ESP_OK) {
-			printf("[CCI] Error: failed to set STATUS register\n");
-			err = true;
-		};
+		// Prepare register address in big-endian
+		tx_buf[0] = (reg_addr >> 8) & 0xFF;
+		tx_buf[1] = reg_addr & 0xFF;
 
-		// Read register - low bits in buf[1]
-		if (i2c_master_read_slave(CCI_ADDRESS, buf, sizeof(buf)) != ESP_OK) {
-			printf("[CCI] Error: failed to read STATUS register\n");
-			err = true;
+		esp_err_t ret = i2c_master_transmit_receive(LEPTON_DEV_HANDLE, tx_buf, sizeof(tx_buf), rx_buf, sizeof(rx_buf), I2C_TIMEOUT_MS_VALUE);
+		if (ret != ESP_OK)
+		{
+			ESP_LOGE(TAG_CCI, "Failed to read STATUS register: %s", esp_err_to_name(ret));
+			return 0x00010000;
+		}
+
+		uint16_t status = ((uint16_t)rx_buf[0] << 8) | rx_buf[1];
+
+		ESP_LOGV(TAG_CCI, "STATUS register read: 0x%04X", status);
+
+		// Check bits [2:0] == 0b110 (0x06)
+		if ((rx_buf[1] & 0x07) == 0x06)
+		{
+			return status;
 		}
 	}
-	
-	if (err) {
-		return 0x00010000;
-	} else {
-		return (buf[0] << 8) | buf[1];
-	}
+
+	// Timed out
+	return 0x00010000;
 }
 
 
@@ -773,21 +792,25 @@ static uint32_t cci_wait_busy_clear()
  * Wait for busy to be clear in the status register and check the result
  * printing an error if detected
  */
-static void cci_wait_busy_clear_check(char* cmd)
+static void cci_wait_busy_clear_check(char *cmd)
 {
-	int8_t   response;
+	int8_t response;
 	uint32_t t32;
-	
+
 	cci_last_status_error = false;
-	
+
 	t32 = cci_wait_busy_clear();
 	cci_last_status = t32 & 0xFFFF;
-	if (t32 == 0x00010000) {
+	if (t32 == 0x00010000)
+	{
 		printf("[CCI] Error: cmd: %s failed wait_busy_clear\n", cmd);
 		cci_last_status_error = true;
-	} else {
-		response = (int8_t) ((t32 & 0x0000FF00) >> 8);
-		if (response < 0) {
+	}
+	else
+	{
+		response = (int8_t)((t32 & 0x0000FF00) >> 8);
+		if (response < 0)
+		{
 			printf("[CCI] Error: %s returned %d\n", cmd, response);
 		}
 	}
diff --git a/src/lipton/cci.h b/src/lipton/cci.h
index 997d998..24d1165 100644
--- a/src/lipton/cci.h
+++ b/src/lipton/cci.h
@@ -33,7 +33,6 @@
 
 // Device characteristics
 #define CCI_WORD_LENGTH 0x02
-#define CCI_ADDRESS 0x2A
 
 // CCI register locations
 #define CCI_REG_STATUS 0x0002
diff --git a/src/lipton/i2c.cpp b/src/lipton/i2c.cpp
index d86969e..ef21c66 100644
--- a/src/lipton/i2c.cpp
+++ b/src/lipton/i2c.cpp
@@ -24,11 +24,13 @@
  */
 // #include "system_config.h"
 #include "i2c.h"
-#include "driver/i2c.h"
+#include "driver/i2c_master.h"
 #include "lepton_system.h"
 // #include "freertos/FreeRTOS.h"
 // #include "freertos/semphr.h"
 
+i2c_master_bus_handle_t i2c0_bus_hdl; // migrate to newer
+i2c_master_dev_handle_t LEPTON_DEV_HANDLE;
 
 //
 // I2C API
@@ -37,74 +39,61 @@
 /**
  * i2c master initialization
  */
-esp_err_t i2c_master_init(int scl_pin, int sda_pin)
+
+esp_err_t i2c_master_bus_detect_devices(i2c_master_bus_handle_t handle)
 {
-    int i2c_master_port = I2C_MASTER_NUM;
-    i2c_config_t conf;
+    const uint16_t probe_timeout_ms = 50; // timeout in milliseconds
+    uint8_t address;
+    bool found_any = false;
 
-    // Configure the I2C controller in master mode using the pins provided
-    conf.mode = I2C_MODE_MASTER;
-    conf.sda_io_num = sda_pin;
-    conf.sda_pullup_en = GPIO_PULLUP_ENABLE;
-    conf.scl_io_num = scl_pin;
-    conf.scl_pullup_en = GPIO_PULLUP_ENABLE;
-    conf.master.clk_speed = I2C_MASTER_FREQ_HZ;
-    conf.clk_flags = 0;
-    esp_err_t err = i2c_param_config((i2c_port_t)i2c_master_port, &conf);
-    if (err != ESP_OK) {
-        return err;
-    }
+    printf("Scanning I2C bus...\n");
 
-    // Install the I2C driver
-    return i2c_driver_install((i2c_port_t)i2c_master_port, conf.mode,
-                              I2C_MASTER_RX_BUF_LEN,
-                              I2C_MASTER_TX_BUF_LEN, 0);
-}
+    for (int i = 0; i < 128; i++)
+    {
+        fflush(stdout);
+        address = i;
 
+        esp_err_t ret = i2c_master_probe(handle, address, probe_timeout_ms);
 
-/**
- * Read esp-i2c-slave
- *
- * _______________________________________________________________________________________
- * | start | slave_addr + rd_bit +ack | read n-1 bytes + ack | read 1 byte + nack | stop |
- * --------|--------------------------|----------------------|--------------------|------|
- *
- */
-esp_err_t i2c_master_read_slave(uint8_t addr7, uint8_t *data_rd, size_t size)
-{
-    if (size == 0) {
-        return ESP_OK;
+        if (ret == ESP_OK)
+        {
+            printf("Found device at 0x%02X\n", address);
+            found_any = true;
+        }
     }
-    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
-    i2c_master_start(cmd);
-    i2c_master_write_byte(cmd, (addr7 << 1) | I2C_MASTER_READ, ACK_CHECK_EN);
-    if (size > 1) {
-        i2c_master_read(cmd, data_rd, size - 1, ACK_VAL);
+
+    if (!found_any)
+    {
+        printf("No I2C devices found.\n");
     }
-    i2c_master_read_byte(cmd, data_rd + size - 1, NACK_VAL);
-    i2c_master_stop(cmd);
-    esp_err_t ret = i2c_master_cmd_begin((i2c_port_t)I2C_MODE_MASTER, cmd, 1000 / portTICK_RATE_MS);
-    i2c_cmd_link_delete(cmd);
-    return ret;
-}
 
+    return ESP_OK;
+}
 
-/**
- * Write esp-i2c-slave
- *
- * ___________________________________________________________________
- * | start | slave_addr + wr_bit + ack | write n bytes + ack  | stop |
- * --------|---------------------------|----------------------|------|
- *
- */
-esp_err_t i2c_master_write_slave(uint8_t addr7, uint8_t *data_wr, size_t size)
+esp_err_t i2c_master_init(gpio_num_t scl_pin, gpio_num_t sda_pin)
 {
-    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
-    i2c_master_start(cmd);
-    i2c_master_write_byte(cmd, (addr7 << 1) | I2C_MASTER_WRITE, ACK_CHECK_EN);
-    i2c_master_write(cmd, data_wr, size, ACK_CHECK_EN);
-    i2c_master_stop(cmd);
-    esp_err_t ret = i2c_master_cmd_begin((i2c_port_t)I2C_MODE_MASTER, cmd, 1000 / portTICK_RATE_MS);
-    i2c_cmd_link_delete(cmd);
-    return ret;
-}
+
+    i2c_master_bus_config_t i2c0_bus_cfg = {
+        .i2c_port = I2C_MASTER_NUM,
+        .sda_io_num = sda_pin,
+        .scl_io_num = scl_pin,
+        .clk_source = I2C_CLK_SRC_DEFAULT,
+        .glitch_ignore_cnt = 7,
+        .flags = {
+            .enable_internal_pullup = true,
+            .allow_pd = false,
+        }
+    };
+    ESP_ERROR_CHECK(i2c_new_master_bus(&i2c0_bus_cfg, &i2c0_bus_hdl));
+
+    i2c_master_bus_detect_devices(i2c0_bus_hdl);
+
+    i2c_device_config_t LEPTON_DEV_CFG = {
+        .dev_addr_length = I2C_ADDR_BIT_LEN_7,
+        .device_address = CCI_ADDRESS,
+        .scl_speed_hz = I2C_MASTER_FREQ_HZ,
+    };
+
+    ESP_ERROR_CHECK(i2c_master_bus_add_device(i2c0_bus_hdl, &LEPTON_DEV_CFG, &LEPTON_DEV_HANDLE));
+    return ESP_OK;
+}
\ No newline at end of file
diff --git a/src/lipton/i2c.h b/src/lipton/i2c.h
index 1c91689..905a8f1 100644
--- a/src/lipton/i2c.h
+++ b/src/lipton/i2c.h
@@ -22,11 +22,13 @@
  * along with tCam.  If not, see <https://www.gnu.org/licenses/>.
  *
  */
-#pragma once
+
+#ifndef TCAM_I2C
+#define TCAM_I2C
 
 #include <stdint.h>
 #include "esp_system.h"
-#include "driver/i2c.h"
+#include "driver/i2c_master.h"
 
 //
 // I2C constants
@@ -41,10 +43,11 @@
 #define ACK_VAL (i2c_ack_type_t)0x0
 #define NACK_VAL (i2c_ack_type_t)0x1
 
+extern i2c_master_bus_handle_t i2c0_bus_hdl; //migrate to newer
+extern i2c_master_dev_handle_t LEPTON_DEV_HANDLE;
 
 //
 // I2C API
 //
-esp_err_t i2c_master_init(int scl_pin, int sda_pin);
-esp_err_t i2c_master_read_slave(uint8_t addr7, uint8_t *data_rd, size_t size);
-esp_err_t i2c_master_write_slave(uint8_t addr7, uint8_t *data_wr, size_t size);
\ No newline at end of file
+esp_err_t i2c_master_init(gpio_num_t scl_pin, gpio_num_t sda_pin);
+#endif
\ No newline at end of file
diff --git a/src/lipton/lepton_system.cpp b/src/lipton/lepton_system.cpp
index 04a1de0..cc32342 100644
--- a/src/lipton/lepton_system.cpp
+++ b/src/lipton/lepton_system.cpp
@@ -12,7 +12,6 @@
 
 // lepton image and telem buffer
 lep_buffer_t rsp_lep_buffer;
-
 /**
  * Initialize the ESP32 GPIO and internal peripherals
  */
@@ -28,8 +27,7 @@ bool lepton_io_init()
 	if (ret != ESP_OK) {
 		printf("[LEP SYS] Error: I2C Master initialization failed\n");
 		return false;
-	}
-	
+	}	
 	// Attempt to initialize the SPI Master used by the lep_task
 	memset(&spi_buscfg, 0, sizeof(spi_bus_config_t));
 	spi_buscfg.miso_io_num=LEP_MISO_PIN;
diff --git a/src/lipton/lepton_system.h b/src/lipton/lepton_system.h
index 0920608..51e94d1 100644
--- a/src/lipton/lepton_system.h
+++ b/src/lipton/lepton_system.h
@@ -19,8 +19,11 @@
 #define LEP_CSN_PIN        14  // SPI_CS       10
 #define LEP_RESET_PIN      20  // RESET_L      17
 
-#define I2C_MASTER_SDA_PIN 4  // SDA          5
-#define I2C_MASTER_SCL_PIN 5  // SCL          8
+#define I2C_TIMEOUT_MS_VALUE 2000
+
+#define I2C_MASTER_SDA_PIN GPIO_NUM_4  // SDA
+#define I2C_MASTER_SCL_PIN GPIO_NUM_5  // SCL 
+#define CCI_ADDRESS 0x2A
 
 // I2C
 #define I2C_MASTER_NUM I2C_NUM_1