From c29508df6c0116ec77e666637b83fed507c32af9 Mon Sep 17 00:00:00 2001
From: Edward Emelianov <edward.emelianoff@gmail.com>
Date: Mon, 22 Sep 2025 20:34:14 +0300
Subject: [PATCH] fixed address changing

---
 F3:F303/MLX90640/mlx90640_regs.h |  4 +++-
 F3:F303/MLX90640/mlxproc.c       | 22 ++++++++++++++++++++--
 F3:F303/MLX90640/proto.c         | 12 ++++++++++--
 3 files changed, 33 insertions(+), 5 deletions(-)

diff --git a/F3:F303/MLX90640/mlx90640_regs.h b/F3:F303/MLX90640/mlx90640_regs.h
index 43e9d77..3fe8bcc 100644
--- a/F3:F303/MLX90640/mlx90640_regs.h
+++ b/F3:F303/MLX90640/mlx90640_regs.h
@@ -43,7 +43,6 @@
 #define REG_CONTROL_SUBPSEL     (1<<3)
 #define REG_CONTROL_DATAHOLD    (1<<2)
 #define REG_CONTROL_SUBPEN      (1<<0)
-#define REG_MLXADDR             0x8010
 #define REG_MLXADDR_MASK        (0xff)
 
 // default value
@@ -53,6 +52,9 @@
 #define REG_CALIDATA            0x2400
 #define REG_CALIDATA_LEN        832
 
+// address in EEPROM (writing to 0x8010 will only change address in RAM)
+#define REG_MLXADDR             0x240f
+
 #define REG_APTATOCCS           0x2410
 #define REG_OSAVG               0x2411
 #define REG_OCCROW14            0x2412
diff --git a/F3:F303/MLX90640/mlxproc.c b/F3:F303/MLX90640/mlxproc.c
index 0f0590a..45b4f5d 100644
--- a/F3:F303/MLX90640/mlxproc.c
+++ b/F3:F303/MLX90640/mlxproc.c
@@ -148,9 +148,27 @@ int mlx_sethwaddr(uint8_t addr){
     if(addr > 0x7f) return 0;
     uint16_t data[2], *ptr;
     if(!(ptr = i2c_read_reg16(MLX_address, REG_MLXADDR, 1, 0))) return 0;
-    data[0] = REG_MLXADDR;
-    data[1] = (*ptr & ~REG_MLXADDR_MASK) | addr;
+    //U("Old address: "); USND(uhex2str(*ptr));
+    data[0] = REG_MLXADDR; data[1] = 0;
+    uint16_t oldreg = *ptr;
+    if(!i2c_write(MLX_address, data, 2)) return 0; // clear address
+    uint32_t Told = Tms;
+    while(Tms - Told < 10);
+    ptr = i2c_read_reg16(MLX_address, REG_MLXADDR, 1, 0);
+    // should be zero
+    if(!ptr){
+        data[0] = REG_MLXADDR; data[1] = oldreg;
+        i2c_write(MLX_address, data, 2); // leave old address
+        return 0;
+    }
+    data[0] = REG_MLXADDR; // i2c_write swaps bytes, so we need init data again
+    data[1] = (oldreg & ~REG_MLXADDR_MASK) | addr;
+    //U("Write address: "); U(uhex2str(data[0])); U(", "); USND(uhex2str(data[1]));
     if(!i2c_write(MLX_address, data, 2)) return 0;
+    while(Tms - Told < 10);
+    if(!(ptr = i2c_read_reg16(MLX_address, REG_MLXADDR, 1, 0))) return 0;
+    //U("Got address: "); USND(uhex2str(*ptr));
+    if((*ptr & REG_MLXADDR_MASK) != addr) return 0;
     return 1;
 }
 
diff --git a/F3:F303/MLX90640/proto.c b/F3:F303/MLX90640/proto.c
index 4f6a141..6e2ce99 100644
--- a/F3:F303/MLX90640/proto.c
+++ b/F3:F303/MLX90640/proto.c
@@ -77,8 +77,16 @@ TRUE_INLINE const char *chhwaddr(const char *buf){
     uint32_t a;
     if(buf && *buf){
         const char *nxt = getnum(buf, &a);
-        if(nxt && nxt != buf) if(!mlx_sethwaddr(a)) return ERR;
-    } else return ERR;
+        if(nxt && nxt != buf){
+            if(!mlx_sethwaddr(a)) return ERR;
+        }else{
+            USND("Wrong number");
+            return ERR;
+        }
+    }else{
+        USND("Need address");
+        return ERR;
+    }
     return OK;
 }