dont stress device on boot

controller/controller.c

@@ -8,6 +8,7 @@
 #include <string.h>
 #include <stdint.h>
 #include <stdarg.h>
+#include <stdlib.h>
 
 #include "global.h"
 
@@ -58,8 +59,8 @@ const struct device devices[] = {
     "sim/cockpit/autopilot/airspeed"
   }
 };
-
-struct rrefSubscription rrefSubscriptions[2];
+const int numDevices = sizeof(devices) / sizeof(struct device);
+struct rrefSubscription *rrefSubscriptions;
 
 void _log(char *format, ...) {
   va_list args;
@@ -115,6 +116,8 @@ void sendI2CData(struct device d, uint8_t triggerBit, char *data, uint8_t dataLe
 int main() {
   __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);
@@ -127,19 +130,34 @@ int main() {
       setI2CAddress(devices[i].address);
       i2cResponse = i2c_smbus_read_byte_data(i2c, 0);
       if (i2cResponse < 0) {
-        _log("I2C read failed: %s (%d)\n", strerror(errno), errno);
+        _log("I2C read from device %s failed: %s (%d)\n", devices[i].name, strerror(errno), errno);
+        usleep(100 * 1000);
+        continue;
       }
 
       if (i2cResponse == DATA_RESET_BYTE) {
         sendI2CInit(devices[i]);
-        sendI2CData(devices[i], TRIGGER_BIT_1, "xxx", 3);
-        sendI2CData(devices[i], TRIGGER_BIT_2, "xxx", 3);
       } else if (i2cResponse == DATA_STOP_BYTE) {
         // expect no forther data from device
         continue;
       } else {
         // real data
         _log("data: 0x%02x\n", i2cResponse);
+        if (i2cResponse & TRIGGER_BIT_1) {
+          if (i2cResponse & HIGH_BIT_1) {
+            sendI2CData(devices[i], TRIGGER_BIT_1, "+1+", 3);
+          } else {
+            sendI2CData(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);
+          } else {
+            sendI2CData(devices[i], TRIGGER_BIT_2, "-2-", 3);
+          }
+        }
       }
     }
     usleep(10 * 1000);