Improve use of MAVSDK::Telemetry

Print health information if Telemetry::health_all_ok() fails.

Type: Improvement
Issue: https://denyspopov.atlassian.net/browse/AZ-15

src/az_drone_controller.cpp

@@ -20,16 +20,18 @@ AzDroneController::AzDroneController(AzMission &mission, QObject *parent)
 
     mMissionController = new AzMissionController(mission, this);
 
-    // Connects signals about the mission progress from the AzMissionController to the slot
+    // Mission progress from the AzMissionController. Slot will be used to find the targets later.
-    // in this class. missionIndexChangedSlot() will be used later to find the targets.
     connect(
         mMissionController,
         &AzMissionController::missionProgressChanged,
         this,
         &AzDroneController::missionIndexChangedSlot);
 
-    // Connects signal from the mission controller to the slot in this class to finish the flying.
+    // Mission controller signal to the slot in this class to finish the flying.
     connect(mMissionController, &AzMissionController::finished, this, &AzDroneController::missionFinishedSlot);
+
+    // Healt info update from MAVSDK.
+    connect(this, &AzDroneController::newHealthInfo, this, &AzDroneController::newHealthInfoSlot, Qt::QueuedConnection);
 }
 
 void AzDroneController::start()
@@ -97,8 +99,15 @@ bool AzDroneController::stateActionModule(void)
 
 bool AzDroneController::stateReadyForArming(void)
 {
-    // TODO!! Check all telemetry components and print warnings and errors.
+    bool result = mTelemetry->health_all_ok();
-    return mTelemetry->health_all_ok();
+
+    if (result == false) {
+        mTelemetry->subscribe_health([this](Telemetry::Health health) {
+            emit newHealthInfo(health);
+        });
+    }
+
+    return result;
 }
 
 bool AzDroneController::stateArm(void)
@@ -199,8 +208,8 @@ void AzDroneController::droneStateMachineSlot(void)
                 qDebug() << "AzDroneController::droneStateMachineSlot() New state:" << states[(int) mDroneState];
             }
             else {
-                qDebug() << "AzDroneController::droneStateMachineSlot() << stateMethods[i].description failed. Trying "
+                qDebug() << "AzDroneController::droneStateMachineSlot()" << stateMethods[i].description
-                            "again.";
+                         << "failed. Trying again.";
             }
 
             delayedStateCallSlot(stateMethods[i].nextStateDelay);
@@ -235,3 +244,18 @@ void AzDroneController::missionIndexChangedSlot(int currentIndex, int totalIndex
 {
     qDebug() << "AzDroneController::missionIndexChanged()" << currentIndex << "/" << totalIndexes;
 }
+
+
+void AzDroneController::newHealthInfoSlot(Telemetry::Health health)
+{
+    qDebug() << "AzDroneController::newHealthInfoSlot()";
+    qDebug() << "  Gyro calibration:  " << (health.is_gyrometer_calibration_ok ? "ok" : "not ok");
+    qDebug() << "  Accel calibration: " << (health.is_accelerometer_calibration_ok ? "ok" : "not ok");
+    qDebug() << "  Mag calibration:   " << (health.is_magnetometer_calibration_ok ? "ok" : "not ok");
+    qDebug() << "  Local position:    " << (health.is_local_position_ok ? "ok" : "not ok");
+    qDebug() << "  Global position:   " << (health.is_global_position_ok ? "ok" : "not ok");
+    qDebug() << "  Home position:     " << (health.is_home_position_ok ? "ok" : "not ok");
+
+    // Not optimal, but disconnect signal immeaditely after getting the first update.
+    disconnect(this, &AzDroneController::newHealthInfo, this, &AzDroneController::newHealthInfoSlot);
+}

src/az_drone_controller.h

@@ -67,12 +67,19 @@ private slots:
     // Disarms the drone and exits the application. TODO!! Discuss quitting the application.
     void disarmDroneSlot(void);
 
+    // Gets Telemetry::Health information in prearming.
+    void newHealthInfoSlot(Telemetry::Health health);
+
 signals:
     // Signal is emitted when Ardupilot sends a new position from the
     // MAVSDK thread. Signal goes through the main event loop and is
     // captured in the main thread to avoid threading issues.
     void newPosition(Telemetry::Position);
 
+    // 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);
+
 private:
     Mavsdk mMavsdk;
     AzDroneState mDroneState;