diff --git a/drone_controller/az_drone_controller.cpp b/drone_controller/az_drone_controller.cpp
index e455433..3bf4a8a 100644
--- a/drone_controller/az_drone_controller.cpp
+++ b/drone_controller/az_drone_controller.cpp
@@ -105,11 +105,13 @@ bool AzDroneController::stateGetTelemetryModule(void)
         return false;
     }
 
-    // Subscripe to position updates. Updated comes from different MAVSDK thread. Send position
-    // as signal to this class (Qt::QueuedConnection) so that it's handled in the main thread.
+    // Subscripe to position and heading updates. Updated comes from different MAVSDK thread. Send position
+    // or heading as signal to this class (Qt::QueuedConnection) so that it's handled in the main thread.
     qRegisterMetaType<Telemetry::Position>("Telemetry::Position");
     connect(this, &AzDroneController::newPosition, this, &AzDroneController::newPositionSlot, Qt::QueuedConnection);
+    connect(this, &AzDroneController::newHeading, this, &AzDroneController::newHeadingSlot, Qt::QueuedConnection);
     mTelemetry->subscribe_position([this](Telemetry::Position position) { emit newPosition(position); });
+    mTelemetry->subscribe_heading([this](Telemetry::Heading heading) { emit newHeading(heading.heading_deg); });
 
     return true;
 }
@@ -244,7 +246,8 @@ void AzDroneController::droneStateMachineSlot(void)
 
 void AzDroneController::newPositionSlot(Telemetry::Position position)
 {
-    cout << "[CONTROLLER] GPS position " << position.latitude_deg << ", " << position.longitude_deg << endl;
+    cout << "[CONTROLLER] GPS position: " << position.latitude_deg << ", " << position.longitude_deg
+         << " Altitudes: " << position.absolute_altitude_m << ", " << position.relative_altitude_m << endl;
 
     // Save first position. It will be used later to set altitude for missions.
     // TODO!! Probably we want to use rangefinder or at least barometer with altitude from the map later.
@@ -262,6 +265,12 @@ void AzDroneController::newPositionSlot(Telemetry::Position position)
 }
 
 
+void AzDroneController::newHeadingSlot(double heading)
+{
+    cout << "[CONTROLLER] Heading: " << heading << endl;
+}
+
+
 void AzDroneController::missionIndexChangedSlot(int currentIndex, int totalIndexes)
 {
     cout << "[CONTROLLER] missionIndexChanged() " << currentIndex << "/" << totalIndexes << endl;
diff --git a/drone_controller/az_drone_controller.h b/drone_controller/az_drone_controller.h
index edd696c..6fa1cf1 100644
--- a/drone_controller/az_drone_controller.h
+++ b/drone_controller/az_drone_controller.h
@@ -59,6 +59,9 @@ protected slots:
     // Slot that is called when the newPosition() signal below is emitted.
     void newPositionSlot(Telemetry::Position);
 
+    // Slot that is called when the newHeading() signal below is emitted.
+    void newHeadingSlot(double heading);
+
     // A mission file contains several action points. This is called
     // when the action point is reached. Indexing starts at 1.
     void missionIndexChangedSlot(int currentIndex, int totalIndexes);
@@ -75,6 +78,11 @@ signals:
     // captured in the main thread to avoid threading issues.
     void newPosition(Telemetry::Position);
 
+    // Signal is emitted when Ardupilot sends a new heading from the
+    // MAVSDK thread. Signal goes through the main event loop and is
+    // captured in the main thread to avoid threading issues.
+    void newHeading(double heading);
+
     // If Telemetry::health_all_ok() fails, then autopilot subscripes for the healt updates to
     // see exactly what is wrong. This signal is emitted to catch updates in the main thread.
     void newHealthInfo(Telemetry::Health);