From c8984d8765704099b7d7a227233f198f379a951d Mon Sep 17 00:00:00 2001
From: damage <damage@devloop.de>
Date: Mon, 19 May 2025 20:53:09 +0200
Subject: [PATCH] begin of threaded controller

---
 controller/controller.c | 70 ++++++++++++++++++++++++++++-------------
 1 file changed, 48 insertions(+), 22 deletions(-)

diff --git a/controller/controller.c b/controller/controller.c
index e9d1940..60a9f6d 100644
--- a/controller/controller.c
+++ b/controller/controller.c
@@ -9,10 +9,11 @@
 #include <stdint.h>
 #include <stdarg.h>
 #include <stdlib.h>
+#include <pthread.h>
 
 #include "global.h"
 
-#define I2C_DEVICE "/dev/i2c-7"
+#define IIC_DEVICE "/dev/i2c-7"
 
 #define HEADING_SIM_UP "sim/autopilot/heading_up"
 #define HEADING_SIM_DOWN "sim/autopilot/heading_down"
@@ -21,7 +22,9 @@
 #define AIRSPEED_SIM_UP "sim/autopilot/airspeed_up"
 #define AIRSPEED_SIM_DOWN "sim/autopilot/airspeed_down"
 
-int i2c;
+#define ERROR_IIC_OPEN "open failed"
+
+pthread_mutex_t i2cLock;
 
 struct device {
   uint8_t address;       // I2C address of device
@@ -69,7 +72,7 @@ void _log(char *format, ...) {
   va_end(args);
 }
 
-int setI2CAddress(uint8_t address) {
+int setI2CAddress(int i2c, uint8_t address) {
   if (ioctl(i2c, I2C_SLAVE, address) < 0) {
     _log("setting periphal address 0x%02X failed\n", address);
     return 0;
@@ -78,7 +81,7 @@ int setI2CAddress(uint8_t address) {
   }
 }
 
-void sendI2CInit(struct device d) {
+void sendI2CInit(int i2c, struct device d) {
   _log("Sending init to %s\n", d.name);
   
   int bufLen = 1 + 1 + sizeof(d.name1) + 1 + sizeof(d.name2);
@@ -94,11 +97,11 @@ void sendI2CInit(struct device d) {
   buf[i++] = d.name2[1];
   buf[i++] = d.name2[2];
 
-  setI2CAddress(d.address);
+  setI2CAddress(i2c, d.address);
   i2c_smbus_write_block_data(i2c, 0, bufLen, buf);
 }
 
-void sendI2CData(struct device d, uint8_t triggerBit, char *data, uint8_t dataLen) {
+void sendI2CData(int i2c, struct device d, uint8_t triggerBit, char *data, uint8_t dataLen) {
   int bufLen = 1 + 1 + 1 + dataLen;
   char buf[bufLen];
   int i = 0;
@@ -109,24 +112,23 @@ void sendI2CData(struct device d, uint8_t triggerBit, char *data, uint8_t dataLe
     buf[i++] = data[j];
   }
 
-  setI2CAddress(d.address);
+  setI2CAddress(i2c, d.address);
   i2c_smbus_write_block_data(i2c, 0, bufLen, buf);
 }
 
-int main() {
+void* i2cSend(void* arg) {
+  int i2c;
+}
+
+void* i2cReceive(void* arg) {
+  int i2c;
   __s32 i2cResponse;
 
-  rrefSubscriptions = calloc(sizeof(struct device), numDevices);
-
-  i2c = open(I2C_DEVICE, O_RDWR);
-  if (i2c < 0) {
-    _log("Unable to open I2C device %s\n", I2C_DEVICE);
-    return 1;
-  }
+  i2c = *((int*)arg);
 
   while (1) {
     for (int i = 0; i < numDevices; i++) {
-      setI2CAddress(devices[i].address);
+      setI2CAddress(i2c, devices[i].address);
       i2cResponse = i2c_smbus_read_byte_data(i2c, 0);
       if (i2cResponse < 0) {
         _log("I2C read from device %s failed: %s (%d)\n", devices[i].name, strerror(errno), errno);
@@ -135,7 +137,9 @@ int main() {
       }
 
       if (i2cResponse == DATA_RESET_BYTE) {
-        sendI2CInit(devices[i]);
+        sendI2CInit(i2c, devices[i]);
+        sendI2CData(i2c, devices[i], TRIGGER_BIT_1, "111", 3);
+        sendI2CData(i2c, devices[i], TRIGGER_BIT_2, "222", 3);
       } else if (i2cResponse == DATA_STOP_BYTE) {
         // expect no forther data from device
         usleep(50 * 1000);
@@ -145,25 +149,47 @@ int main() {
         _log("data: 0x%02x\n", i2cResponse);
         /*
         XXX: put data into cache and handle with another core / loop / fork / whatever
+        */
         if (i2cResponse & TRIGGER_BIT_1) {
           if (i2cResponse & HIGH_BIT_1) {
-            sendI2CData(devices[i], TRIGGER_BIT_1, "+1+", 3);
+            sendI2CData(i2c, devices[i], TRIGGER_BIT_1, "+1+", 3);
           } else {
-            sendI2CData(devices[i], TRIGGER_BIT_1, "-1-", 3);
+            sendI2CData(i2c, devices[i], TRIGGER_BIT_1, "-1-", 3);
           }
         }
 
         if (i2cResponse & TRIGGER_BIT_2) {
           if (i2cResponse & HIGH_BIT_2) {
-            sendI2CData(devices[i], TRIGGER_BIT_2, "+2+", 3);
+            sendI2CData(i2c, devices[i], TRIGGER_BIT_2, "+2+", 3);
           } else {
-            sendI2CData(devices[i], TRIGGER_BIT_2, "-2-", 3);
+            sendI2CData(i2c, devices[i], TRIGGER_BIT_2, "-2-", 3);
           }
         }
-        */
       }
     }
   }
 
   close(i2c);
+  pthread_exit(NULL);
+}
+
+int main() {
+  int i2c;
+  pthread_t i2cReceiver;
+
+  rrefSubscriptions = calloc(sizeof(struct device), numDevices);
+
+  i2c = open(IIC_DEVICE, O_RDWR);
+  if (i2c < 0) {
+    _log("Unable to open I2C device %s\n", IIC_DEVICE);
+  }
+
+  pthread_mutex_init(&i2cLock, NULL);
+
+  pthread_create(&i2cReceiver, NULL, i2cReceive, &i2c);
+  pthread_join(i2cReceiver, NULL);
+
+  pthread_mutex_destroy(&i2cLock);
+
+  return 0;
 }