   int32_t newLed = messenger.readInt32Arg();
   if (newLed == 1)
   {
-    digitalWrite(ledPin, HIGH);
+    setLED( HIGH);
     messenger.sendCmd(kDebug, "LED COMMAND ON");
+
+	// For Demo Purpose:
+	// If we received a LED ON command, we set the heading in the sim to 180
+	messenger.sendCmd(kHeading,180);
+
   }
   else
   {
-    digitalWrite(ledPin, LOW);
+    setLED( LOW);
     messenger.sendCmd(kDebug, "LED COMMAND OFF");
 
     // When we turned it off we unsubscribe from the heading lock for demo purpose

  // initialize the digital pin as an output.
  pinMode(ledPin, OUTPUT);

  // Make sure led is turned off after start or reset
  digitalWrite(ledPin, LOW);
}

// Loop function

#include <CmdMessenger.h>  // CmdMessenger

// Internal led
const int ledPin = LED_BUILTIN;

// Listen on serial connection for messages from the pc
CmdMessenger messenger(Serial);
