diff --git a/test/test.ino b/test/test.ino
index 3aad67d..05502b1 100644
--- a/test/test.ino
+++ b/test/test.ino
@@ -1,14 +1,22 @@
+#include <U8g2lib.h>
+
 #include <MCP23017.h>
 #include <ArduinoMqttClient.h>
 #include <Ethernet.h>
 #include "config.h"
 
-#define HEADING_PORT A
-#define HEADING_ADDR 0x20
-#define HEADING_RESET 9
+#define ROT_PORT_A A
+#define ROT_ADDR_1 0x20
+
 #define HEADING_CLK 0b00000001
 #define HEADING_DT 0b00000010
 #define HEADING_SW 0b00000100
+#define SPEED_CLK 0b00100000
+#define SPEED_DT 0b01000000
+#define SPEED_SW 0b10000000
+
+
+// https://github.com/olikraus/u8g2/wiki/fntgrpfreeuniversal
 
 byte mac[] = { 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED };
 
@@ -19,9 +27,10 @@ const char topic[]  = "/xplane/rref/#";
 EthernetClient client;
 MqttClient mqttClient(client);
 
-byte lastHeadingInput;
+byte lastRotInput_1A;
 
-MCP23017 mcp1 = MCP23017(HEADING_ADDR, HEADING_RESET);
+MCP23017 mcp1 = MCP23017(ROT_ADDR_1, 100);
+U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* clock=*/ SCL, /* data=*/ SDA, /* reset=*/ U8X8_PIN_NONE);  // High speed I2C
 
 void setup() {
   // put your setup code here, to run once:
@@ -62,60 +71,127 @@ void setup() {
     Serial.println("MCP connected");
   }
   
-  mcp1.setPortMode(0b00000000, HEADING_PORT);
-  mcp1.setPortPullUp(0b00000000, HEADING_PORT);
+  mcp1.setPortMode(0b00000000, ROT_PORT_A);
+  mcp1.setPortPullUp(0b00000000, ROT_PORT_A);
 
-  lastHeadingInput = mcp1.getPort(HEADING_PORT);
+  lastRotInput_1A = mcp1.getPort(ROT_PORT_A);
+
+  u8g2.begin();
+
+  mqttClient.beginMessage("/xplane/meta/rref");
+  mqttClient.print("001 010 sim/cockpit/autopilot/heading_mag");
+  mqttClient.endMessage();
+  mqttClient.beginMessage("/xplane/meta/rref");
+  mqttClient.print("002 010 sim/cockpit/autopilot/airspeed");
+  mqttClient.endMessage();
 }
 
 bool isBitDown(byte input, byte reference) {
   return !(input & reference);
 }
 
+int getRotation(bool newClk, bool lastClk, bool newDt, bool lastDt) {
+  if (lastClk && !newClk) {
+    if (newDt) {
+      // CW
+      return 1;
+    } else {
+      // CCW
+      return -1;
+    }
+  } else {
+    return 0;
+  }
+}
+
+void sendMqttByRotation(int rotation, char *topic, char *cw, char *ccw) {
+  if (rotation != 0) {
+    mqttClient.beginMessage(topic);
+    if (rotation == 1) {
+      mqttClient.print(cw);
+    } else {
+      mqttClient.print(ccw);
+    }
+    mqttClient.endMessage();
+  }  
+}
+
 void loop() {
   // 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();
 
-  byte newHeadingInput = mcp1.getPort(HEADING_PORT);
+  byte newRotInput_1A = mcp1.getPort(ROT_PORT_A);
 
-  if (isBitDown(newHeadingInput, HEADING_SW) && !isBitDown(lastHeadingInput, HEADING_SW)) {
+  if (isBitDown(newRotInput_1A, HEADING_SW) && !isBitDown(lastRotInput_1A, HEADING_SW)) {
     mqttClient.beginMessage("/xplane/meta/cmnd");
     mqttClient.print("sim/autopilot/heading_push");
     mqttClient.endMessage();
   }
 
-  bool newHeadingClk = isBitDown(newHeadingInput, HEADING_CLK);
-  bool lastHeadingClk = isBitDown(lastHeadingInput, HEADING_CLK);
+  int rotation = 0;
 
-  if (lastHeadingClk && !newHeadingClk) {
-    mqttClient.beginMessage("/xplane/meta/cmnd");
-    if (isBitDown(newHeadingInput, HEADING_DT)) {
-      // CW
-      mqttClient.print("sim/autopilot/heading_up");
-    } else {
-      // CCW
-      mqttClient.print("sim/autopilot/heading_down");
-    }
-    mqttClient.endMessage();
-  }
+  bool newHeadingClk = isBitDown(newRotInput_1A, HEADING_CLK);
+  bool lastHeadingClk = isBitDown(lastRotInput_1A, HEADING_CLK);
+  bool newHeadingDt = isBitDown(newRotInput_1A, HEADING_DT);
+  bool lastHeadingDt = isBitDown(lastRotInput_1A, HEADING_DT);
+  rotation = getRotation(newHeadingClk, lastHeadingClk, newHeadingDt, lastHeadingDt);
+  sendMqttByRotation(rotation, "/xplane/meta/cmnd", "sim/autopilot/heading_up", "sim/autopilot/heading_down");
 
-  lastHeadingInput = newHeadingInput;
+  bool newSpeedClk = isBitDown(newRotInput_1A, SPEED_CLK);
+  bool lastSpeedClk = isBitDown(lastRotInput_1A, SPEED_CLK);
+  bool newSpeedDt = isBitDown(newRotInput_1A, SPEED_DT);
+  bool lastSpeedDt = isBitDown(lastRotInput_1A, SPEED_DT);
+  rotation = getRotation(newSpeedClk, lastSpeedClk, newSpeedDt, lastSpeedDt);
+  sendMqttByRotation(rotation, "/xplane/meta/cmnd", "sim/autopilot/airspeed_up", "sim/autopilot/airspeed_down");
+
+  lastRotInput_1A = newRotInput_1A;
 }
 
 void onMqttMessage(int messageSize) {
-  // we received a message, print out the topic and contents
-  Serial.println("Received a message with topic '");
-  Serial.print(mqttClient.messageTopic());
-  Serial.print("', length ");
-  Serial.print(messageSize);
-  Serial.println(" bytes:");
+  if (mqttClient.messageTopic() == "/xplane/rref/1") {
+    char buf[3] = "";
+    char c = '\0';
+    for (int i = 0; i < 3 && mqttClient.available() && c != '.'; i++) {
+      c = (char)mqttClient.read();
+      if (c != '.') {
+        buf[i] = c;
+      }
+    }
 
-  // use the Stream interface to print the contents
-  while (mqttClient.available()) {
-    Serial.print((char)mqttClient.read());
+    if (buf[1] == '\0') {
+      buf[1] = buf[0];
+      buf[0] = '0';
+    }
+  
+    if (buf[2] == '\0') {
+      buf[2] = buf[1];
+      buf[1] = buf[0];
+      buf[0] = '0';
+    }
+  
+    u8g2.clearBuffer();
+    u8g2.setFont(u8g2_font_fub11_tf);
+    u8g2.drawStr(0, 11, "HDG");
+    u8g2.setFont(u8g2_font_fub42_tn);
+    u8g2.drawStr(10, 63, buf);
+    u8g2.sendBuffer();
   }
-  Serial.println();
 
-  Serial.println();
+  // TODO: speed display
+
+//  // we received a message, print out the topic and contents
+//  Serial.println("Received a message with topic '");
+//  Serial.print(mqttClient.messageTopic());
+//  Serial.print("', length ");
+//  Serial.print(messageSize);
+//  Serial.println(" bytes:");
+//
+//  // use the Stream interface to print the contents
+//  while (mqttClient.available()) {
+//    Serial.print((char)mqttClient.read());
+//  }
+//  Serial.println();
+//
+//  Serial.println();
 }