prevent multi include

also make buffer void*

.gitignore

@@ -0,0 +1 @@
+config.h

controller/.gitignore

@@ -0,0 +1 @@
+controller

controller/README.md

@@ -0,0 +1,22 @@
+# Controller
+## links
+* https://www.kernel.org/doc/Documentation/i2c/dev-interface
+* https://forum.arduino.cc/t/solved-cannot-get-correct-i2c-data-from-atmega8-into-raspberry-pi/1368337
+
+## PINs
+* interal = physical = usage
+* 5 = 29 = reset peripheral devices
+* 0 = 10 = I2C SDA
+* 1 = 9 = I2C SCL
+
+## I2C
+* somehow the buildin I2C does not work anymore (maybe already damaged?)
+* add additional bus by using GPIOs:
+```
+$ fgrep i2c /boot/firmware/config.txt 
+dtparam=i2c_arm=on
+dtoverlay=i2c-gpio,bus=7,i2c_gpio_sda=10,i2c_gpio_scl=9
+```
+
+## commands
+* reset peripheral devices: `gpioset gpiochip0 5=0`

controller/build.sh

@@ -0,0 +1 @@
+gcc -o controller ringbuffer.c controller.c -li2c

controller/controller.c

@@ -0,0 +1,236 @@
+#define RINGBUFFER_THREAD_SAFE
+
+#include <linux/i2c-dev.h>
+#include <i2c/smbus.h>
+#include <sys/ioctl.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <errno.h>
+#include <string.h>
+#include <stdint.h>
+#include <stdarg.h>
+#include <stdlib.h>
+
+#include "global.h"
+#ifndef RINGBUFFER_H
+#include "ringbuffer.h"
+#endif
+
+#define IIC_DEVICE "/dev/i2c-7"
+#define BUFFER_BLOCKS 128
+
+#define HEADING_SIM_UP "sim/autopilot/heading_up"
+#define HEADING_SIM_DOWN "sim/autopilot/heading_down"
+#define ALTITUDE_SIM_UP "sim/autopilot/altitude_up"
+#define ALTITUDE_SIM_DOWN "sim/autopilot/altitude_down"
+#define AIRSPEED_SIM_UP "sim/autopilot/airspeed_up"
+#define AIRSPEED_SIM_DOWN "sim/autopilot/airspeed_down"
+
+#define ERROR_IIC_OPEN "open failed"
+
+struct ringBuffer i2cDataRead;
+struct ringBuffer i2cDataWrite;
+
+struct blockData {
+  char data[DATA_SIZE];
+  int triggerBit;
+};
+
+struct device {
+  uint8_t address;       // I2C address of device
+  char *name;         // name of this device
+  char name1[3];      // first name to send on init of device
+  char *highCommand1; // command to use if TRIGGER_BIT_1 is set and HIGH_BIT_1 is set
+  char *lowCommand1;  // command to use if TRIGGER_BIT_1 is set and HIGH_BIT_1 is not set
+  int rrefRefresh1;   // refresh rate of subscribing rref
+  char *rref1;        // rref to subscribe to
+  char name2[3];      // second name to send on init of device
+  char *highCommand2; // command to use if TRIGGER_BIT_2 is set and HIGH_BIT_2 is set
+  char *lowCommand2;  // command to use if TRIGGER_BIT_2 is set and HIGH_BIT_2 is not set
+  int rrefRefresh2;   // refresh rate of subscribing rref
+  char *rref2;        // rref to subscribe to
+};
+
+struct rrefSubscription {
+  uint8_t address;
+  uint8_t triggerBit;
+};
+
+const struct device devices[] = {
+  {
+    0x08,
+    "Heading Speed",
+    {'H', 'D', 'G'},
+    HEADING_SIM_UP,
+    HEADING_SIM_DOWN,
+    200,
+    "sim/cockpit/autopilot/heading_mag",
+    {'S', 'P', 'D'},
+    AIRSPEED_SIM_UP,
+    AIRSPEED_SIM_DOWN,
+    200,
+    "sim/cockpit/autopilot/airspeed"
+  }
+};
+const int numDevices = sizeof(devices) / sizeof(struct device);
+struct rrefSubscription *rrefSubscriptions;
+
+void _log(char *format, ...) {
+  va_list args;
+  va_start(args, format);
+  vprintf(format, args);
+  va_end(args);
+}
+
+int setI2CAddress(int i2c, uint8_t address) {
+  if (ioctl(i2c, I2C_SLAVE, address) < 0) {
+    _log("setting i2c address 0x%02X failed\n", address);
+    return 0;
+  } else {
+    return 1;
+  }
+}
+
+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);
+  char buf[bufLen];
+  int i = 0;
+  buf[i++] = DATA_RESET_BYTE;
+  buf[i++] = DATA_INIT_BYTE_1;
+  buf[i++] = d.name1[0];
+  buf[i++] = d.name1[1];
+  buf[i++] = d.name1[2];
+  buf[i++] = DATA_INIT_BYTE_2;
+  buf[i++] = d.name2[0];
+  buf[i++] = d.name2[1];
+  buf[i++] = d.name2[2];
+
+  setI2CAddress(i2c, d.address);
+  i2c_smbus_write_block_data(i2c, 0, bufLen, buf);
+}
+
+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;
+  buf[i++] = DATA_BYTE;
+  buf[i++] = triggerBit;
+  buf[i++] = dataLen;
+  for (int j = 0; j < dataLen; j++) {
+    buf[i++] = data[j];
+  }
+
+  setI2CAddress(i2c, d.address);
+  i2c_smbus_write_block_data(i2c, 0, bufLen, buf);
+}
+
+int tempCounter = 0;
+int tempGauge = 0;
+void* mqttHandler(void* arg) {
+  struct blockData *block = malloc(sizeof(struct blockData));
+
+  while (1) {
+    sprintf(block->data, "%03d", tempCounter);
+    block->triggerBit = TRIGGER_BIT_1;
+    ringBufferWrite(&i2cDataWrite, block);
+    tempCounter++;
+    sleep(1);
+  }
+
+  free(block);
+}
+
+void* i2cHandler(void* arg) {
+  int i2c, tmp, buzy;
+  __s32 i2cResponse;
+  struct blockData *block = malloc(sizeof(struct blockData));
+
+  i2c = open(IIC_DEVICE, O_RDWR);
+  if (i2c < 0) {
+    _log("Unable to open I2C device %s\n", IIC_DEVICE);
+  }
+
+  while (1) {
+    buzy = 0;
+    for (int i = 0; i < numDevices; i++) {
+      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);
+        continue;
+      }
+
+      if (i2cResponse == DATA_RESET_BYTE) {
+        sendI2CInit(i2c, devices[i]);
+        buzy = 1;
+      } else if (i2cResponse == DATA_STOP_BYTE) {
+        // expect no forther data from device
+        // send data to device
+        do {
+          tmp = ringBufferRead(&i2cDataWrite, block);
+          if (tmp == 0) {
+            // we got something to send
+            sendI2CData(i2c, devices[i], block->triggerBit, block->data, 3);
+            buzy = 1;
+          }
+        } while (tmp == 0);
+        continue;
+      } else {
+        // real data
+        _log("data: 0x%02x\n", i2cResponse);
+        if (i2cResponse & TRIGGER_BIT_2) {
+          if (i2cResponse & HIGH_BIT_2) {
+            tempGauge++;
+            if (tempGauge > 999) {
+              tempGauge = 999;
+            }
+          } else {
+            tempGauge--;
+            if (tempGauge < 0) {
+              tempGauge = 0;
+            }
+          }
+          sprintf(block->data, "%03d", tempGauge);
+          block->triggerBit = TRIGGER_BIT_2;
+          ringBufferWrite(&i2cDataWrite, block);
+        }
+        buzy = 1;
+      }
+    }
+
+    if (buzy == 0) {
+      usleep(50 * 1000);
+    }
+  }
+
+  close(i2c);
+  free(block);
+  pthread_exit(NULL);
+}
+
+int main() {
+  pthread_t i2cThread, mqttThread;
+
+  // create the ring buffers for i2c devices
+  ringBufferCreate(BUFFER_BLOCKS, sizeof(struct blockData), &i2cDataRead);
+  ringBufferCreate(BUFFER_BLOCKS, sizeof(struct blockData), &i2cDataWrite);
+
+  // alloc
+  rrefSubscriptions = calloc(sizeof(struct device), numDevices);
+
+  // start and join threads
+  pthread_create(&i2cThread, NULL, i2cHandler, NULL);
+  pthread_create(&mqttThread, NULL, mqttHandler, NULL);
+  pthread_join(i2cThread, NULL);
+  pthread_join(mqttThread, NULL);
+
+  // free resources
+  free(rrefSubscriptions);
+  ringBufferDestroy(&i2cDataRead);
+  ringBufferDestroy(&i2cDataWrite);
+
+  return 0;
+}

controller/global.h

@@ -0,0 +1,11 @@
+#define DATA_STOP_BYTE 0x00
+#define DATA_INIT_BYTE_1 0x01
+#define DATA_INIT_BYTE_2 0x02
+#define DATA_BYTE 0x03
+#define DATA_RESET_BYTE 0xFF
+#define TRIGGER_BIT_1 1
+#define HIGH_BIT_1 2
+#define TRIGGER_BIT_2 4
+#define HIGH_BIT_2 8
+#define I2C_ADDRESS_START 0x08
+#define DATA_SIZE 8

controller/ringbuffer.c

@@ -0,0 +1,75 @@
+#ifndef RINGBUFFER_H
+#include "ringbuffer.h"
+#endif
+
+void _ringBufferIncReader(struct ringBuffer *buf) {
+  buf->reader += buf->blockSize;
+  if (buf->reader >= buf->buffer + buf->blocks * buf->blockSize) {
+    buf->reader = buf->buffer;
+  }
+}
+
+void _ringBufferIncWriter(struct ringBuffer *buf) {
+  buf->writer += buf->blockSize;
+  if (buf->writer == buf->buffer + buf->blocks * buf->blockSize) {
+    buf->writer = buf->buffer;
+  }
+
+  if (buf->writer == buf->reader) {
+    // we incremented the writer and now it is equal to reader
+    // this means, the writer is overtaking the reader
+    // so push the reader forward, even if we are
+    // loosing data but this is how ring buffers work, eh?
+    _ringBufferIncReader(buf);
+  }
+}
+
+void ringBufferCreate(int blocks, size_t blockSize, struct ringBuffer *out) {
+  out->buffer = malloc(blocks * blockSize);
+  out->blocks = blocks;
+  out->blockSize = blockSize;
+  out->reader = out->buffer;
+  out->writer = out->buffer;
+#ifdef RINGBUFFER_THREAD_SAFE
+  pthread_mutex_init(&out->mutex, NULL);
+#endif
+}
+
+void ringBufferDestroy(struct ringBuffer *buf) {
+  free(buf->buffer);
+#ifdef RINGBUFFER_THREAD_SAFE
+  pthread_mutex_destroy(&buf->mutex);
+#endif
+}
+
+int ringBufferRead(struct ringBuffer *buf, void *out) {
+#ifdef RINGBUFFER_THREAD_SAFE
+  pthread_mutex_lock(&buf->mutex);
+#endif
+  if (buf->reader != buf->writer) {
+    // we have data to read
+    memcpy(out, buf->reader, buf->blockSize);
+    _ringBufferIncReader(buf);
+#ifdef RINGBUFFER_THREAD_SAFE
+    pthread_mutex_unlock(&buf->mutex);
+#endif
+    return 0;
+  } else {
+    // nothing to read
+#ifdef RINGBUFFER_THREAD_SAFE
+    pthread_mutex_unlock(&buf->mutex);
+#endif
+    return 1;
+  }
+}
+
+void ringBufferWrite(struct ringBuffer *buf, void *in) {
+#ifdef RINGBUFFER_THREAD_SAFE
+  pthread_mutex_lock(&buf->mutex);
+#endif
+  memcpy(buf->writer, in, buf->blockSize);
+  _ringBufferIncWriter(buf);
+#ifdef RINGBUFFER_THREAD_SAFE
+  pthread_mutex_unlock(&buf->mutex);
+#endif
+}

controller/ringbuffer.h

@@ -0,0 +1,25 @@
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+
+#ifdef RINGBUFFER_THREAD_SAFE
+#include <pthread.h>
+#endif
+
+#define RINGBUFFER_H
+
+struct ringBuffer {
+  void *buffer;
+  int blocks;
+  size_t blockSize;
+  void *reader;
+  void *writer;
+#ifdef RINGBUFFER_THREAD_SAFE
+  pthread_mutex_t mutex;
+#endif
+};
+
+void ringBufferCreate(int blocks, size_t blockSize, struct ringBuffer *out);
+void ringBufferDestroy(struct ringBuffer *buf);
+int ringBufferRead(struct ringBuffer *buf, void *out);
+void ringBufferWrite(struct ringBuffer *buf, void *in);

master/global.h

@@ -0,0 +1 @@
+../controller/global.h

master/master.ino

@@ -0,0 +1,213 @@
+#include <Wire.h>
+#include <ArduinoMqttClient.h>
+#include <Ethernet.h>
+
+#include "config.h"
+#include "global.h"
+
+#define READ_BYTES_FROM_WIRE 1
+
+#define HEADING_SIM_UP "sim/autopilot/heading_up"
+#define HEADING_SIM_DOWN "sim/autopilot/heading_down"
+#define ALTITUDE_SIM_UP "sim/autopilot/altitude_up"
+#define ALTITUDE_SIM_DOWN "sim/autopilot/altitude_down"
+#define AIRSPEED_SIM_UP "sim/autopilot/airspeed_up"
+#define AIRSPEED_SIM_DOWN "sim/autopilot/airspeed_down"
+
+#define MQTT_SIM_COMMAND_TOPIC "/xplane/meta/cmnd"
+#define MQTT_LOG_TOPIC "/xplane/meta/log"
+#define MQTT_SIM_VALUE_TOPIC "/xplane/rref/#"
+#define MQTT_SIM_VALUE_SUBSCRIBE_TOPIC_PREFIX "/xplane/rref/"
+#define MQTT_SIM_VALUE_SUBSCRIBE_TOPIC "/xplane/meta/rref"
+
+#define MQTT_KEEPALIVE_INTERVAL_MS 15000
+
+#define SLAVE_RESET_PIN 7
+
+struct device {
+  byte address;       // I2C address of device
+  char *name;         // name of this device
+  char name1[3];      // first name to send on init of device
+  char *highCommand1; // command to use if TRIGGER_BIT_1 is set and HIGH_BIT_1 is set
+  char *lowCommand1;  // command to use if TRIGGER_BIT_1 is set and HIGH_BIT_1 is not set
+  int rrefRefresh1;   // refresh rate of subscribing rref
+  char *rref1;        // rref to subscribe to
+  char name2[3];      // second name to send on init of device
+  char *highCommand2; // command to use if TRIGGER_BIT_2 is set and HIGH_BIT_2 is set
+  char *lowCommand2;  // command to use if TRIGGER_BIT_2 is set and HIGH_BIT_2 is not set
+  int rrefRefresh2;   // refresh rate of subscribing rref
+  char *rref2;        // rref to subscribe to
+};
+
+struct device devices[] = {
+  {
+    0x08,
+    "Heading Speed",
+    {'H', 'D', 'G'},
+    HEADING_SIM_UP,
+    HEADING_SIM_DOWN,
+    200,
+    "sim/cockpit/autopilot/heading_mag",
+    {'S', 'P', 'D'},
+    AIRSPEED_SIM_UP,
+    AIRSPEED_SIM_DOWN,
+    200,
+    "sim/cockpit/autopilot/airspeed"
+  }
+};
+const int numDevices = sizeof(devices) / sizeof(device);
+
+struct rrefSubscription {
+  byte address;
+  byte triggerBit;
+};
+
+struct rrefSubscription rrefSubscriptions[numDevices * 2];
+
+byte mac[] = { 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED };
+EthernetClient client;
+
+MqttClient mqttClient(client);
+
+void onMqttMessage(int messageSize) {
+  Serial.println("Received MQTT message");
+  
+  // assume, that we always receive topic beginning with MQTT_SIM_VALUE_SUBSCRIBE_TOPIC_PREFIX
+  // otherwise, this will _HORRIBLY_ fail
+
+  char *rrefNoStr = &mqttClient.messageTopic()[13];
+  int rrefNo = atoi(rrefNoStr);
+  struct rrefSubscription rref = rrefSubscriptions[rrefNo - 1];
+
+  Wire.beginTransmission(rref.address);
+  Wire.write(DATA_BYTE);
+  Wire.write(rref.triggerBit);
+  Wire.write(messageSize);
+  while (mqttClient.available()) {
+    Wire.write((char)mqttClient.read());
+  }
+  Wire.endTransmission(rref.address);
+}
+
+void sendMqttMessage(char *topic, int n, ...) {
+  mqttClient.beginMessage(topic);
+  va_list args;
+  va_start(args, n);
+  for (int i = 0; i < n; i++) {
+    mqttClient.print(va_arg(args, char*));
+  }
+  va_end(args);
+  mqttClient.endMessage();
+}
+
+void sendI2CInit(struct device d) {
+  sendMqttMessage(MQTT_LOG_TOPIC, 2, "Sending init to ", d.name);
+  Wire.beginTransmission(d.address);
+  Wire.write(DATA_RESET_BYTE);
+  Wire.write(DATA_INIT_BYTE_1);
+  Wire.write(d.name1, 3);
+  Wire.write(DATA_INIT_BYTE_2);
+  Wire.write(d.name2, 3);
+  Wire.endTransmission(d.address);
+}
+
+void subscribeRrefs(struct device *d, int deviceNo) {
+  char subscribeMsg[256];
+
+  // subscribe no 1
+  sprintf(subscribeMsg, "%03d %03d %s", deviceNo + 1, d->rrefRefresh1, d->rref1); // subscription no is index-1 based
+  sendMqttMessage(MQTT_SIM_VALUE_SUBSCRIBE_TOPIC, 1, subscribeMsg);
+  rrefSubscriptions[deviceNo * 2] = { d->address, TRIGGER_BIT_1 };
+  
+  // subscribe no 2
+  sprintf(subscribeMsg, "%03d %03d %s", deviceNo + 2, d->rrefRefresh2, d->rref2); // subscription no is index-1 based
+  sendMqttMessage(MQTT_SIM_VALUE_SUBSCRIBE_TOPIC, 1, subscribeMsg);
+  rrefSubscriptions[(deviceNo * 2) + 1] = { d->address, TRIGGER_BIT_2 };
+}
+
+void setup() {
+  // setup slave reset pin and pull it down for a reset
+  pinMode(SLAVE_RESET_PIN, OUTPUT);
+  digitalWrite(SLAVE_RESET_PIN, LOW);
+  
+  // start the Ethernet connection:
+  if (Ethernet.begin(mac) == 0) {
+    Serial.println("Failed to configure Ethernet using DHCP");
+    // no point in carrying on, so do nothing forevermore:
+    while (1);
+  }
+  // print your local IP address:
+  char localIP[16];
+  sprintf(localIP, "%d.%d.%d.%d", Ethernet.localIP()[0], Ethernet.localIP()[1], Ethernet.localIP()[2], Ethernet.localIP()[3]);
+  Serial.println(localIP);
+
+  // MQTT auth
+  mqttClient.setUsernamePassword(MQTT_USER, MQTT_PASS);
+
+  // MQTT connection
+  if (!mqttClient.connect(MQTT_SERVER, MQTT_PORT)) {
+    Serial.print("MQTT connection failed! Error code = ");
+    Serial.println(mqttClient.connectError());
+
+    while (1);
+  } else {
+    Serial.println("MQTT connected");
+    sendMqttMessage(MQTT_LOG_TOPIC, 2, "Master online ", localIP);
+  }
+
+  // subscribe to MQTT topic
+  mqttClient.onMessage(onMqttMessage);
+  mqttClient.subscribe(MQTT_SIM_VALUE_TOPIC);
+  mqttClient.setKeepAliveInterval(MQTT_KEEPALIVE_INTERVAL_MS);
+
+  // switch on the slaves and give them a moment to boot
+  digitalWrite(SLAVE_RESET_PIN, HIGH);
+  delay(5000);
+
+  // start I2C
+  Wire.begin();
+}
+
+void loop() {
+  // loop through devices and ask for data
+  for (int i = 0; i < numDevices; i++) {
+    Wire.requestFrom(devices[i].address, READ_BYTES_FROM_WIRE);
+    while (Wire.available()) {
+      // device has data, ask as long for data as it sends
+      // if it never stops sending, we are fucked :)
+      byte data = Wire.read();
+      if (data == DATA_STOP_BYTE) {
+        // do not request further data
+        continue;
+      } else if (data == DATA_RESET_BYTE) {
+        // device needs initialization
+        sendMqttMessage(MQTT_LOG_TOPIC, 2, "got reset byte from ", devices[i].name);
+        sendI2CInit(devices[i]);
+        subscribeRrefs(&devices[i], i);
+      } else {
+        // handle as payload
+        if (data & TRIGGER_BIT_1) {
+          if (data & HIGH_BIT_1) {
+            sendMqttMessage(MQTT_SIM_COMMAND_TOPIC, 1, devices[i].highCommand1);
+          } else {
+            sendMqttMessage(MQTT_SIM_COMMAND_TOPIC, 1, devices[i].lowCommand1);
+          }
+        } else if (data & TRIGGER_BIT_2) {
+          if (data & HIGH_BIT_2) {
+            sendMqttMessage(MQTT_SIM_COMMAND_TOPIC, 1, devices[i].highCommand2);
+          } else {
+            sendMqttMessage(MQTT_SIM_COMMAND_TOPIC, 1, devices[i].lowCommand2);
+          }
+        } else {
+          // who are you?
+        }
+      }
+
+      Wire.requestFrom(devices[i].address, READ_BYTES_FROM_WIRE);
+    }
+  }
+
+  // call poll() regularly to allow the library to receive MQTT messages and
+  // send MQTT keep alives which avoids being disconnected by the broker
+  mqttClient.poll();
+}

rotator/README.md

@@ -0,0 +1,37 @@
+# rotator device
+* two rotators
+* two displays
+
+## required libraries
+* https://github.com/adafruit/Adafruit_SSD1306/
+* https://github.com/adafruit/Adafruit-GFX-Library
+* https://github.com/adafruit/Adafruit_BusIO
+
+## PINs
+* IDE = Physical = Usage
+* x = 1 = reset (connecto to master)
+* 2 = 4 = rotator 1 white
+* 3 = 5 = rotator 2 white
+* 4 = 6 = rotator 1 green
+* 5 = 11 = rotator 2 green
+* 6 = 12 = display 2 dc
+* 7 = 13 = display 2 reset
+* 8 = 14 = display 1 dc
+* 9 = 15 = display 1 reset
+* 10 = 16 = display 1 cs
+* 11 = 17 = SPI MOSI
+* 12 = 18 = (unused) SPI MISO
+* 13 = 19 = SPI SCK
+* 14 / A0 = 23 = address selector bit 1
+* 15 / A1 = 24 = address selector bit 2
+* 16 / A2 = 25 = address selector bit 3
+* 17 / A3 = 26 = display 2 cs
+* 18 / A4 = 27 = I2C SDA (connect to master)
+* 19 / A5 = 28 = I2C SCL (connect to master)
+
+## address selection
+* no address selector pin set to low = 0x08
+* address selector bit 1 = low = 0x09
+* address selector bit 2 = low = 0x0A
+* address selector bit 1 and 2 = low = 0x0B
+...

rotator/global.h

@@ -0,0 +1 @@
+../controller/global.h

rotator/rotator.ino

@@ -1,160 +1,283 @@
+#include <Wire.h>
 
-#define PIN_HEADING_CLK D7
-#define PIN_HEADING_DT D5
-#define PIN_HEADING_SW D6
+#include <SPI.h>
+#include <Adafruit_GFX.h>
+#include <Fonts/FreeMonoBold24pt7b.h>
+#include <Fonts/FreeMonoBold18pt7b.h>
+#include <Fonts/FreeMonoBold12pt7b.h>
+#include <Adafruit_SSD1306.h>
 
-#define PIN_ALTITUDE_CLK D1
-#define PIN_ALTITUDE_DT D2
-#define PIN_ALTITUDE_SW D3
+#include "global.h"
 
-#define PIN_LED D4
+#define SCREEN_WIDTH 128 // OLED display width, in pixels
+#define SCREEN_HEIGHT 64 // OLED display height, in pixels
+
+#define PIN_ROTATOR1_WHITE 2
+#define PIN_ROTATOR1_GREEN 4
+
+#define PIN_ROTATOR2_WHITE 3
+#define PIN_ROTATOR2_GREEN 5
 
 #define VALUE_BUFFER 30
 
-#define HEADING_DT_TRIGGER 1
-#define HEADING_DT_HIGH 2
-#define HEADING_SW_TRIGGER 4
+#define SKIP_ROTARY_INPUTS 50
 
-#define ALTITUDE_DT_TRIGGER 8
-#define ALTITUDE_DT_HIGH 16
-#define ALTITUDE_SW_TRIGGER 32
+#define INTERNAL_STATE_UNKNOWN 0
+#define INTERNAL_STATE_INITIALIZED 1
 
-#define MIN_DT_INPUT_DELAY 1000    // 1ms
-#define MIN_SW_INPUT_DELAY 500000 // 500ms
+#define PIN_ADDRESS_1 14
+#define PIN_ADDRESS_2 15
+#define PIN_ADDRESS_3 16
 
-#define MIN_ALTITUDE 1
-#define MAX_ALTITUDE 60000
+#define PIN_DISPLAY1_DC 8
+#define PIN_DISPLAY1_RST 9
+#define PIN_DISPLAY1_CS 10
+#define PIN_DISPLAY2_DC 6
+#define PIN_DISPLAY2_RST 7
+#define PIN_DISPLAY2_CS 17
 
-#define MIN_HEADING 1
-#define MAX_HEADING 360
+Adafruit_SSD1306 display1(SCREEN_WIDTH, SCREEN_HEIGHT,
+  &SPI, PIN_DISPLAY1_DC, PIN_DISPLAY1_RST, PIN_DISPLAY1_CS);
+Adafruit_SSD1306 display2(SCREEN_WIDTH, SCREEN_HEIGHT,
+  &SPI, PIN_DISPLAY2_DC, PIN_DISPLAY2_RST, PIN_DISPLAY2_CS);
+
+char name1[] = {'X', 'X', 'X'};
+char name2[] = {'Y', 'Y', 'Y'};
+int internalState = INTERNAL_STATE_UNKNOWN;
 
 uint8_t lastClk = HIGH;
-uint16_t heading = 360;
-uint16_t altitude = 0;
 
 // ringbuffer of trigger and direction values
-uint8_t valueBuffer[VALUE_BUFFER] = { 0 };
+byte valueBuffer[VALUE_BUFFER] = { 0 };
 uint8_t readerPos, writerPos = 0;
 
-long lastInput = 0;
-
-// prevent bouncing input
-bool useInput(long minInputDelay) {
-  long now = micros();
-  long diff = now - lastInput;
-
-  // < 0 because of long overruns
-  bool ret = diff < 0 || diff > minInputDelay;
-  if (ret) {
-    lastInput = now;
-  }
-
-  return ret;
+// ringbuffer of display data
+struct {
+  
 }
 
 void addValue(uint8_t value) {
-    valueBuffer[writerPos++] = value;
-    if (writerPos >= VALUE_BUFFER) {
-      writerPos = 0;
-    }  
+  valueBuffer[writerPos++] = value;
+
+  // check for writer position overflow
+  if (writerPos >= VALUE_BUFFER) {
+    writerPos = 0;
+  }
+
+  // check if writer overtooks reader
+  // if so, increment reader and check
+  // for reader position overflow
+  if (writerPos == readerPos) {
+    readerPos++;
+    if (readerPos >= VALUE_BUFFER) {
+      readerPos = 0;
+    }
+  }
 }
 
-void clkFalling(uint8_t PIN_DT, uint8_t DT_TRIGGER, uint8_t DT_HIGH) {
-  if (useInput(MIN_DT_INPUT_DELAY)) {
-    uint8_t dt = digitalRead(PIN_DT);
-    uint8_t value = DT_TRIGGER;
-    if (dt) {
-      value |= DT_HIGH;
-    } else {
-      // value is already "DT_LOW"
-    }
+void falling(uint8_t pin, byte triggerBit, byte highBit) {
+  uint8_t dt = digitalRead(pin);
+  byte value = triggerBit;
 
+  // read direction of pin
+  if (dt) {
+    value |= highBit;
+  } else {
+    // value is already "lowBit"
+  }
+
+  if (useInput(value)) {
     addValue(value);
   }
 }
 
-void swFalling(uint8_t SW_TRIGGER) {
-  if (useInput(MIN_SW_INPUT_DELAY)) {
-    addValue(SW_TRIGGER);
-  }
+void rotator1Falling() {
+  falling(PIN_ROTATOR1_GREEN, TRIGGER_BIT_1, HIGH_BIT_1);
 }
 
-void ICACHE_RAM_ATTR headingClkFalling();
-void headingClkFalling() {
-  clkFalling(PIN_HEADING_DT, HEADING_DT_TRIGGER, HEADING_DT_HIGH);
+void rotator2Falling() {
+  falling(PIN_ROTATOR2_GREEN, TRIGGER_BIT_2, HIGH_BIT_2);
 }
 
-void ICACHE_RAM_ATTR altitudeClkFalling();
-void altitudeClkFalling() {
-  clkFalling(PIN_ALTITUDE_DT, ALTITUDE_DT_TRIGGER, ALTITUDE_DT_HIGH);
-}
-
-void ICACHE_RAM_ATTR headingSwFalling();
-void headingSwFalling() {
-  swFalling(HEADING_SW_TRIGGER);
-}
-
-void ICACHE_RAM_ATTR altitudeSwFalling();
-void altitudeSwFalling() {
-  swFalling(ALTITUDE_SW_TRIGGER);
-}
-
-void dtTriggered(uint16_t &cnt, uint8_t high, uint8_t fast, uint16_t miV, uint16_t miVR, uint16_t maV, uint16_t mavR) {
-  if (high) {
-    cnt++;
+int eventCount = 0;
+byte lastValue = 0;
+// this method blocks the parallel use of rotators
+// if parallel use should be possible
+// lastValue and eventCount must be stored per rotator
+bool useInput(byte value) {
+  if (lastValue == value) {
+    // same event as last event
+    // check if already SKIP_ROTARY_INPUTS happend
+    if (eventCount > SKIP_ROTARY_INPUTS) {
+      eventCount = 0;
+      return true;
+    } else {
+      eventCount++;
+      return false;
+    }
   } else {
-    cnt--;
-  }
+    // not same event as last event
+    // reset counter
+    lastValue = value;
+    eventCount = 0;
+    return false;
+  }  
+}
 
-  if (cnt > maV) {
-    cnt = mavR;
-  }
-
-  if (cnt < miV) {
-    cnt = miVR;
+void i2cRequest() {
+  if (internalState == INTERNAL_STATE_UNKNOWN) {
+    // looks like we were restarted
+    // request initialization first
+    Serial.println("Requesting init");
+    Wire.write(DATA_RESET_BYTE);
+    internalState = INTERNAL_STATE_INITIALIZED;
+  } else {
+    // if write is ahead, send data
+    if (writerPos != readerPos) {
+      byte value = valueBuffer[readerPos++];
+   
+      if (readerPos >= VALUE_BUFFER) {
+        readerPos = 0;
+      }
+  
+      Wire.write(value);
+    } else {
+      Wire.write(DATA_STOP_BYTE);
+    }
   }
 }
 
-void setup() {
-  Serial.begin(115200);
-  while (!Serial) {
-    ; // wait for serial port to connect. Needed for native USB port only
+void i2cReceive(int bytes) {
+  while (Wire.available()) {
+    byte data = Wire.read();
+    if (data == DATA_RESET_BYTE) {
+      Serial.println("Received reset request");
+      writerPos = readerPos = 0;
+    } else if (data == DATA_INIT_BYTE_1) {
+      Serial.println("Received init 1");
+      // next three bytes are the name of the first rotator
+      for (int i = 0; i < 3 && Wire.available(); i++) {
+        name1[i] = Wire.read();
+      }
+    } else if (data == DATA_INIT_BYTE_2) {
+      Serial.println("Received init 2");
+      // next three bytes are the name of the second rotator
+      for (int i = 0; i < 3 && Wire.available(); i++) {
+        name2[i] = Wire.read();
+      }
+    } else if (data == DATA_BYTE) {
+      // next byte will be trigger bit
+      byte triggerBit = Wire.read();
+      
+      // next byte will be length of data
+      byte dataLength = Wire.read();
+            
+      // next byte(s) will be the data itself
+      char dataBytes[dataLength];
+      for (int i = 0; i < dataLength && Wire.available(); i++) {
+        dataBytes[i] = Wire.read();
+      }
+
+      if (triggerBit & TRIGGER_BIT_1) {
+        updateDisplay1(dataBytes, dataLength);
+      } else if (triggerBit & TRIGGER_BIT_2) {
+        updateDisplay2(dataBytes, dataLength);
+      } else {
+        Serial.println("Unknown trigger bit");
+      }
+    } else {
+      // not yet implemented?
+      Serial.println("Received unknown");
+    }
   }
+}
 
-  pinMode(PIN_HEADING_SW, INPUT_PULLUP);
-  pinMode(PIN_HEADING_DT, INPUT_PULLUP);
-  pinMode(PIN_HEADING_CLK, INPUT_PULLUP);
+void displayTestScreen(Adafruit_SSD1306 display) {
+  display.clearDisplay();
+  for (int x = 0; x < SCREEN_WIDTH; x++) {
+    for (int y = 0; y < SCREEN_HEIGHT; y++) {
+      display.drawPixel(x, y, SSD1306_WHITE);
+    }
+  }
+  display.display();
+}
 
-  attachInterrupt(digitalPinToInterrupt(PIN_HEADING_CLK), headingClkFalling, FALLING);
-  attachInterrupt(digitalPinToInterrupt(PIN_HEADING_SW), headingSwFalling, FALLING);
-  attachInterrupt(digitalPinToInterrupt(PIN_ALTITUDE_CLK), altitudeClkFalling, FALLING);
-  attachInterrupt(digitalPinToInterrupt(PIN_ALTITUDE_SW), altitudeSwFalling, FALLING);
+void updateDisplay1(char *data, byte len) {
+  updateDisplay(&display1, name1, data, len);
+}
+
+void updateDisplay2(char *data, byte len) {
+  updateDisplay(&display2, name2, data, len);
+}
+
+void updateDisplay(Adafruit_SSD1306 *display, char *name, char *data, byte len) {
+  display->clearDisplay();
+  display->setTextColor(SSD1306_WHITE); // Draw white text
+  display->cp437(true);         // Use full 256 char 'Code Page 437' font
+
+  display->setTextSize(1);      // Normal 1:1 pixel scale
+  display->setCursor(0, 8);     // Start at top-left corner
+  display->setFont(NULL);
+  display->write(name[0]);
+  display->write(name[1]);
+  display->write(name[2]);
+  
+  display->setTextSize(2);      // Normal 1:1 pixel scale
+  display->setCursor(0, 56);     // Start at top-left corner
+  display->setFont(&FreeMonoBold18pt7b);
+  for (byte i = 0; i < len; i++) {
+    display->write(data[i]);
+  }
+  display->display();
+}
+
+uint8_t address = I2C_ADDRESS_START;
+void setup() {
+  // https://support.arduino.cc/hc/en-us/articles/4839084114460-If-your-board-runs-the-sketch-twice
+  delay(150);
+  Serial.begin(115200);
+
+  // setup rotator GPIOs
+  pinMode(PIN_ROTATOR1_WHITE, INPUT_PULLUP);
+  pinMode(PIN_ROTATOR1_GREEN, INPUT_PULLUP);
+  pinMode(PIN_ROTATOR2_WHITE, INPUT_PULLUP);
+  pinMode(PIN_ROTATOR2_GREEN, INPUT_PULLUP);
+  
+  // setup address selector GPIOs
+  pinMode(PIN_ADDRESS_1, INPUT_PULLUP);
+  pinMode(PIN_ADDRESS_2, INPUT_PULLUP);
+  pinMode(PIN_ADDRESS_3, INPUT_PULLUP);
+
+  // calculate address by LOW GPIOs
+  address |= (digitalRead(PIN_ADDRESS_1) == LOW) << 1;
+  address |= (digitalRead(PIN_ADDRESS_2) == LOW) << 2;
+  address |= (digitalRead(PIN_ADDRESS_3) == LOW) << 3;
+
+  attachInterrupt(digitalPinToInterrupt(PIN_ROTATOR1_WHITE), rotator1Falling, FALLING);
+  attachInterrupt(digitalPinToInterrupt(PIN_ROTATOR2_WHITE), rotator2Falling, FALLING);
+
+  Wire.begin(address);
+  Wire.onRequest(i2cRequest);
+  Wire.onReceive(i2cReceive);
+  
+  if(!display1.begin(SSD1306_SWITCHCAPVCC)) {
+    Serial.println(F("SSD1306 allocation failed (1)"));
+    for(;;); // Don't proceed, loop forever
+  }
+  displayTestScreen(display1);
+  
+  if(!display2.begin(SSD1306_SWITCHCAPVCC)) {
+    Serial.println(F("SSD1306 allocation failed (2)"));
+    for(;;); // Don't proceed, loop forever
+  }
+  displayTestScreen(display2);
+  // make sure, test screen is at least displayed 2 seconds
+  delay(2000);
+
+  updateDisplay1("---", 3);
+  updateDisplay2("---", 3);
 }
 
 void loop() {
-  while (writerPos != readerPos) {
-    uint8_t value = valueBuffer[readerPos++];
-
-    if (readerPos >= VALUE_BUFFER) {
-      readerPos = 0;
-    }
-
-    if (value & HEADING_DT_TRIGGER) {
-      uint8_t dtHigh = value & HEADING_DT_HIGH;
-      dtTriggered(heading, dtHigh, 5, MIN_HEADING, MAX_HEADING, MAX_HEADING, MIN_HEADING);
-      Serial.print("Heading: ");
-      Serial.println(heading);
-    } else if (value & HEADING_SW_TRIGGER) {
-      Serial.println("Heading: pushed");
-    } else if (value & ALTITUDE_DT_TRIGGER) {
-      uint8_t dtHigh = value & ALTITUDE_DT_HIGH;
-      dtTriggered(altitude, dtHigh, 5, MIN_ALTITUDE, MIN_ALTITUDE, MAX_ALTITUDE, MAX_ALTITUDE);
-      Serial.print("Altitude: ");
-      Serial.println(altitude);
-    } else if (value & ALTITUDE_SW_TRIGGER) {
-      Serial.println("Altitude: pushed");
-    } else {
-      // never happen error...
-    }
-  }
+  delay(1000);
 }

simulatesimulator/.gitignore

@@ -0,0 +1,2 @@
+main
+stress

simulatesimulator/build.sh

@@ -0,0 +1,2 @@
+gcc -o main mqtt.c -lmosquitto main.c
+gcc -o stress mqtt.c -lmosquitto stress.c

simulatesimulator/main.c

@@ -0,0 +1,104 @@
+#include <unistd.h>
+#include <stdio.h>
+#include <mosquitto.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "mqtt.h"
+#include "config.h"
+
+#define MAX_AIRSPEED 385
+#define MAX_MQTT_PAYLOAD_SIZE 512
+
+struct mosquitto *mqtt;
+
+int airspeed = 0;
+int heading = 0;
+
+struct test {
+	char *foo;
+};
+
+void onMessage(struct mosquitto *mqtt, void *obj, const struct mosquitto_message *message) {
+	char *mqttTopic;
+	char mqttPayload[MAX_MQTT_PAYLOAD_SIZE];
+
+	if (strcmp(message->topic, "/xplane/meta/rref") == 0) {
+		//requestRrefFromXPlane(message);
+	} else if (strcmp(message->topic, "/xplane/meta/cmnd") == 0) {
+		mqttTopic = NULL;
+		memset(mqttPayload, 0, sizeof(mqttPayload));
+
+		printf("CMD: %s\n", (char *)message->payload);
+		if (strcmp((char *)message->payload, "sim/autopilot/airspeed_up") == 0) {
+			airspeed++;
+			if (airspeed > MAX_AIRSPEED) {
+				airspeed = MAX_AIRSPEED;
+			} else {
+				// airspeed changed
+				mqttTopic = "/xplane/rref/2";
+				sprintf(mqttPayload, "%03d", airspeed);
+			}
+			printf("Airspeed: %d\n", airspeed);
+		} else if (strcmp((char *)message->payload, "sim/autopilot/airspeed_down") == 0) {
+			airspeed--;
+			if (airspeed < 0) {
+				airspeed = 0;
+			} else {
+				mqttTopic = "/xplane/rref/2";
+				sprintf(mqttPayload, "%03d", airspeed);
+			}
+			printf("Airspeed: %d\n", airspeed);
+		} else if (strcmp((char *)message->payload, "sim/autopilot/heading_up") == 0) {
+			heading++;
+			if (heading > 360) {
+				heading = 1;
+			}
+			mqttTopic = "/xplane/rref/1";
+			sprintf(mqttPayload, "%03d", heading);
+			printf("Heading: %d\n", heading);
+		} else if (strcmp((char *)message->payload, "sim/autopilot/heading_down") == 0) {
+			heading--;
+			if (heading < 0) {
+				heading = 359;
+			}
+			mqttTopic = "/xplane/rref/1";
+			sprintf(mqttPayload, "%03d", heading);
+			printf("Heading: %d\n", heading);
+		} else {
+			printf("unkown command %s\n", (char *)message->payload);
+		}
+
+		if (mqttTopic != NULL && mqttPayload != NULL) {
+			sendMqtt(mqtt, mqttTopic, mqttPayload, strlen(mqttPayload));
+		}
+	} else if (strcmp(message->topic, "/xplane/meta/log") == 0) {
+		printf("Log: %s\n", (char *)message->payload);
+	} else {
+		fprintf(stderr, "Unknown topic %s\n", message->topic);
+	}
+}
+
+void doit(struct test *blah) {
+	blah->foo = "moep";
+}
+
+int main() {
+	struct test bar = { "baz" };
+	doit(&bar);
+	printf("foo is %s\n", bar.foo);
+
+	mqtt = connectMqtt(MQTT_USER1, MQTT_PASS, onMessage);
+	if (mqtt == NULL) {
+		return -1;
+	}
+
+	recvMqtt(mqtt, "/xplane/meta/cmnd");
+
+	while (1) {
+		mosquitto_loop(mqtt, 0, 1);
+		usleep(10 * 1000);
+	}
+
+	disconnectMqtt(mqtt);
+}

simulatesimulator/mqtt.c

@@ -0,0 +1,93 @@
+#include <stdio.h>
+
+#include <errno.h>
+#include <string.h>
+
+// https://mosquitto.org/api/files/mosquitto-h.html
+#include <mosquitto.h>
+
+struct mosquitto *connectMqtt(char *user, char *pass, void (*messageCallback)(struct mosquitto *, void *, const struct mosquitto_message *))
+{
+    int err;
+    struct mosquitto *mqtt;
+
+    err = mosquitto_lib_init();
+    if (err)
+    {
+        fprintf(stderr, "Error initalizing mosquitto lib (%d): %s\n", err, mosquitto_strerror(err));
+        return NULL;
+    }
+
+    mqtt = mosquitto_new(user, true, NULL);
+    if (mqtt == NULL)
+    {
+        fprintf(stderr, "Error creating mosquitto instance (%d): %s\n", errno, strerror(errno));
+        return NULL;
+    }
+
+    err = mosquitto_username_pw_set(mqtt, user, pass);
+    if (err)
+    {
+        fprintf(stderr, "Error setting username & password (%d): %s\n", err, mosquitto_strerror(err));
+        return NULL;
+    }
+
+    // FIXME: I am hard coded :(
+    err = mosquitto_connect(mqtt, "openhab.sugarland.lan", 1883, 10);
+    if (err)
+    {
+        fprintf(stderr, "Error on connection of type ");
+        if (err == MOSQ_ERR_ERRNO)
+        {
+            fprintf(stderr, "system (%d): %s\n", errno, strerror(errno));
+        }
+        else
+        {
+            fprintf(stderr, "mosquitto (%d): %s\n", err, mosquitto_strerror(err));
+        }
+        return NULL;
+    }
+
+    mosquitto_message_callback_set(mqtt, messageCallback);
+
+    return mqtt;
+}
+
+int sendMqtt(struct mosquitto *mqtt, char *topic, char *payload, int payloadLength)
+{
+    int err;
+
+    err = mosquitto_publish(mqtt, NULL, topic, payloadLength, payload, 0, false);
+    if (err)
+    {
+        fprintf(stderr, "Error publishing message to %s (%d): %s\n", topic, err, mosquitto_strerror(err));
+        return -1;
+    }
+    else
+    {
+        return 0;
+    }
+}
+
+int recvMqtt(struct mosquitto *mqtt, char *topic)
+{
+    int err;
+    
+    err = mosquitto_subscribe(mqtt, NULL, topic, 0);
+    if (err) {
+        fprintf(stderr, "Error subscribing to %s (%d): %s", topic, err, mosquitto_strerror(err));
+        return -1;
+    }
+    else
+    {
+        return 0;
+    }
+}
+
+void disconnectMqtt(struct mosquitto *mqtt)
+{
+    // ignore errors
+    mosquitto_disconnect(mqtt);
+    mosquitto_destroy(mqtt);
+    mosquitto_lib_cleanup();
+}

simulatesimulator/mqtt.h

@@ -0,0 +1,4 @@
+struct mosquitto * connectMqtt(char *user, char *pass, void (*messageCallback)(struct mosquitto *, void *, const struct mosquitto_message *));
+int sendMqtt(struct mosquitto *mqtt, char *topic, char *payload, int payloadLength);
+int recvMqtt(struct mosquitto *mqtt, char *topic);
+void disconnectMqtt(struct mosquitto *mqtt);

simulatesimulator/stress.c

@@ -0,0 +1,114 @@
+#include <unistd.h>
+#include <stdio.h>
+#include <mosquitto.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "mqtt.h"
+#include "config.h"
+
+#define MAX_AIRSPEED 385
+#define MAX_MQTT_PAYLOAD_SIZE 512
+#define LOOPS 1000000
+
+struct mosquitto *mqtt;
+
+int airspeed = 100;
+int heading = 0;
+
+void onMessage(struct mosquitto *mqtt, void *obj, const struct mosquitto_message *message) {
+	char *mqttTopic;
+	char mqttPayload[MAX_MQTT_PAYLOAD_SIZE];
+
+	if (strcmp(message->topic, "/xplane/meta/rref") == 0) {
+		//requestRrefFromXPlane(message);
+	} else if (strcmp(message->topic, "/xplane/meta/cmnd") == 0) {
+		mqttTopic = NULL;
+		memset(mqttPayload, 0, sizeof(mqttPayload));
+
+		printf("CMD: %s\n", (char *)message->payload);
+		if (strcmp((char *)message->payload, "sim/autopilot/airspeed_up") == 0) {
+			airspeed++;
+			if (airspeed > MAX_AIRSPEED) {
+				airspeed = MAX_AIRSPEED;
+			} else {
+				// airspeed changed
+				mqttTopic = "/xplane/rref/2";
+				sprintf(mqttPayload, "%03d", airspeed);
+			}
+			printf("Airspeed: %d\n", airspeed);
+		} else if (strcmp((char *)message->payload, "sim/autopilot/airspeed_down") == 0) {
+			airspeed--;
+			if (airspeed < 0) {
+				airspeed = 0;
+			} else {
+				mqttTopic = "/xplane/rref/2";
+				sprintf(mqttPayload, "%03d", airspeed);
+			}
+			printf("Airspeed: %d\n", airspeed);
+		} else if (strcmp((char *)message->payload, "sim/autopilot/heading_up") == 0) {
+			heading++;
+			if (heading > 360) {
+				heading = 1;
+			}
+			mqttTopic = "/xplane/rref/1";
+			sprintf(mqttPayload, "%03d", heading);
+			printf("Heading: %d\n", heading);
+		} else if (strcmp((char *)message->payload, "sim/autopilot/heading_down") == 0) {
+			heading--;
+			if (heading < 0) {
+				heading = 359;
+			}
+			mqttTopic = "/xplane/rref/1";
+			sprintf(mqttPayload, "%03d", heading);
+			printf("Heading: %d\n", heading);
+		} else {
+			printf("unkown command %s\n", (char *)message->payload);
+		}
+
+		if (mqttTopic != NULL && mqttPayload != NULL) {
+			sendMqtt(mqtt, mqttTopic, mqttPayload, strlen(mqttPayload));
+		}
+	} else if (strcmp(message->topic, "/xplane/meta/log") == 0) {
+		printf("Log: %s\n", (char *)message->payload);
+	} else {
+		fprintf(stderr, "Unknown topic %s\n", message->topic);
+	}
+}
+
+int loop = 0;
+int main() {
+	char mqttPayload[MAX_MQTT_PAYLOAD_SIZE];
+
+	mqtt = connectMqtt(MQTT_USER1, MQTT_PASS, onMessage);
+	if (mqtt == NULL) {
+		return -1;
+	}
+
+	recvMqtt(mqtt, "/xplane/meta/cmnd");
+
+	while (1) {
+		loop++;
+		if (loop > LOOPS) {
+			sprintf(mqttPayload, "%03d", heading);
+			sendMqtt(mqtt, "/xplane/rref/1", mqttPayload, strlen(mqttPayload));
+			sprintf(mqttPayload, "%03d", airspeed);
+			sendMqtt(mqtt, "/xplane/rref/2", mqttPayload, strlen(mqttPayload));
+
+			heading++;
+			if (heading >= 360) {
+				heading = 0;
+			}
+
+			airspeed++;
+			if (airspeed >= MAX_AIRSPEED) {
+				airspeed = 100;
+			}
+			loop = 0;
+		}
+
+		mosquitto_loop(mqtt, 0, 1);
+	}
+
+	disconnectMqtt(mqtt);
+}