Added support for switching to AUTO mode in Autopilot.

When the application is started with the command parameter "plane", the
application uses the AzDroneControllerPlane class to control
initialisation. It doesn't arm or takeoff the drone. Instead, it waits
for the user to mode to AUTO (in Ardupilot, Mission in MAVSDK) with the
RC controller. When AUTO mode has been detected, the application will
start a normal mission handling.

src/autopilot.pro

@@ -13,6 +13,7 @@ SOURCES += \
         az_action_point.cpp \
         az_coordinate.cpp \
         az_drone_controller.cpp \
+        az_drone_controller_plane.cpp \
         az_mission.cpp \
         az_mission_controller.cpp \
         az_utils.cpp \
@@ -26,6 +27,7 @@ HEADERS += \
     az_config.h \
     az_coordinate.h \
     az_drone_controller.h \
+    az_drone_controller_plane.h \
     az_mission.h \
     az_mission_controller.h \
     az_utils.h

src/az_drone_controller.cpp

@@ -35,12 +35,14 @@ AzDroneController::AzDroneController(AzMission &mission, QObject *parent)
     connect(this, &AzDroneController::newHealthInfo, this, &AzDroneController::newHealthInfoSlot, Qt::QueuedConnection);
 }
 
+
 void AzDroneController::start()
 {
     // Must wait that main event loop is launched in main()
     delayedStateCallSlot(0);
 }
 
+
 void AzDroneController::delayedStateCallSlot(int ms)
 {
     // MAVSDK examples use blocking sleep() calls for timeouts.
@@ -48,6 +50,7 @@ void AzDroneController::delayedStateCallSlot(int ms)
     QTimer::singleShot(ms, this, &AzDroneController::droneStateMachineSlot);
 }
 
+
 // Connects to the flight controller based on AZ_CONNECTION_XXX defines in AzConfig.
 // Serial port connections is enabled if command line arguments contains "serial"
 // parameter. Otherwise UDP connection is used.
@@ -71,6 +74,7 @@ bool AzDroneController::stateConnect(void)
     }
 }
 
+
 bool AzDroneController::stateAutopilot(void)
 {
     std::vector<std::shared_ptr<System>> systems = mMavsdk.systems();
@@ -93,6 +97,7 @@ bool AzDroneController::stateAutopilot(void)
     }
 }
 
+
 bool AzDroneController::stateTelemetryModule(void)
 {
     if ((mTelemetry = new Telemetry(mSystem)) == nullptr) {
@@ -119,6 +124,7 @@ bool AzDroneController::stateTelemetryModule(void)
     return true;
 }
 
+
 bool AzDroneController::stateActionModule(void)
 {
     mAction = new Action(mSystem);
@@ -133,6 +139,7 @@ bool AzDroneController::stateActionModule(void)
     }
 }
 
+
 bool AzDroneController::stateReadyForArming(void)
 {
     bool result = mTelemetry->health_all_ok();
@@ -144,6 +151,7 @@ bool AzDroneController::stateReadyForArming(void)
     return result;
 }
 
+
 bool AzDroneController::stateArm(void)
 {
     Action::Result result = mAction->arm();
@@ -157,6 +165,7 @@ bool AzDroneController::stateArm(void)
     }
 }
 
+
 bool AzDroneController::stateTakeoff(void)
 {
     // Drone never reaches the target altitude with ArduPilot. This seems
@@ -170,6 +179,7 @@ bool AzDroneController::stateTakeoff(void)
     return result == Action::Result::Success;
 }
 
+
 bool AzDroneController::stateFlyMission(void)
 {
     if (QCoreApplication::arguments().contains("quadcopter") && mCurrentPosition.relative_altitude_m < AZ_RELATIVE_FLY_ALTITUDE * 0.90) {
@@ -182,6 +192,7 @@ bool AzDroneController::stateFlyMission(void)
     return mMissionController->startMissions(flight_altitude_abs);
 }
 
+
 void AzDroneController::missionFinishedSlot(void)
 {
     // TODO!! Maybe use Telemetry::subscribe_landed_state() later to get the state of the landing.
@@ -193,6 +204,7 @@ void AzDroneController::missionFinishedSlot(void)
     }
 }
 
+
 void AzDroneController::droneStateMachineSlot(void)
 {
     static const QString states[] = {
@@ -208,8 +220,7 @@ void AzDroneController::droneStateMachineSlot(void)
         "LAND",
     };
 
-    qDebug() << "";
+    qDebug() << "\nAzDroneController::droneStateMachineSlot() Current state:" << states[(int) mDroneState];
-    qDebug() << "AzDroneController::droneStateMachineSlot() Current state:" << states[(int) mDroneState];
 
     using MethodPtr = bool (AzDroneController::*)();
 
@@ -244,17 +255,7 @@ void AzDroneController::droneStateMachineSlot(void)
 
             if (result) {
                 qDebug() << "AzDroneController::droneStateMachineSlot()" << stateMethods[i].description << "succeeded";
-
                 mDroneState = stateMethods[i + 1].currentState;
-
-                // Quadcopter needs to iterate all states - plane will skip arming and takeoff
-                if (mDroneState == AZ_DRONE_STATE_READY_FOR_ARMING && QCoreApplication::arguments().contains("plane")) {
-                    mDroneState = AZ_DRONE_STATE_TAKE_OFF;
-                    qDebug() << "AzDroneController::droneStateMachineSlot() Skipping arming and takeoff for planes.";
-                }
-                else {
-                    qDebug() << "AzDroneController::droneStateMachineSlot() New state:" << states[(int) mDroneState];
-                }
             }
             else {
                 qDebug() << "AzDroneController::droneStateMachineSlot()" << stateMethods[i].description
@@ -268,6 +269,7 @@ void AzDroneController::droneStateMachineSlot(void)
     }
 }
 
+
 void AzDroneController::newPositionSlot(Telemetry::Position position)
 {
     qDebug() << "AzDroneController::newPositionSlot()" << position.latitude_deg << position.longitude_deg;
@@ -293,11 +295,13 @@ void AzDroneController::newPositionSlot(Telemetry::Position position)
     }
 }
 
+
 void AzDroneController::missionIndexChangedSlot(int currentIndex, int totalIndexes)
 {
     qDebug() << "AzDroneController::missionIndexChanged()" << currentIndex << "/" << totalIndexes;
 }
 
+
 void AzDroneController::newHealthInfoSlot(Telemetry::Health health)
 {
     qDebug() << "AzDroneController::newHealthInfoSlot()";
@@ -308,6 +312,14 @@ void AzDroneController::newHealthInfoSlot(Telemetry::Health health)
     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.
+    // Stop printing healt information if everything in healt is OK.
-    // disconnect(this, &AzDroneController::newHealthInfo, this, &AzDroneController::newHealthInfoSlot);
+    if (health.is_gyrometer_calibration_ok &&
+        health.is_accelerometer_calibration_ok &&
+        health.is_magnetometer_calibration_ok &&
+        health.is_local_position_ok &&
+        health.is_global_position_ok &&
+        health.is_home_position_ok) {
+
+        disconnect(this, &AzDroneController::newHealthInfo, this, &AzDroneController::newHealthInfoSlot);
+    }
 }

src/az_drone_controller.h

@@ -21,6 +21,7 @@ typedef enum {
     AZ_DRONE_STATE_READY_FOR_ARMING,
     AZ_DRONE_STATE_ARMED,
     AZ_DRONE_STATE_TAKE_OFF,
+    AZ_DRONE_STATE_WAITING_HOLD_MODE,
     AZ_DRONE_STATE_FLY_MISSION,
     AZ_DRONE_STATE_LAND,
     AZ_DRONE_STATE_END,
@@ -35,6 +36,7 @@ public:
     // Starts the autopilot.
     void start();
 
+protected:
     // Specify the methods that will be called one at a time to initialise the drone and fly the mission.
     bool stateConnect(void);
     bool stateAutopilot(void);
@@ -46,9 +48,9 @@ public:
     bool stateFlyMission(void);
 
     // Slots that are called by the emitted Qt signals.
-private slots:
+protected slots:
     // Launches a state machine that progressively initialises the drone without blocking.
-    void droneStateMachineSlot(void);
+    virtual void droneStateMachineSlot(void);
 
     // ArduPilot seems to require some delay between calls. This method uses QTimer internally
     // to call each new state method with some delay without blocking the main thread.
@@ -77,7 +79,7 @@ signals:
     // see exactly what is wrong. This signal is emitted to catch updates in the main thread.
     void newHealthInfo(Telemetry::Health);
 
-private:
+protected:
     Mavsdk mMavsdk;
     AzDroneState mDroneState;
     std::shared_ptr<System> mSystem;

src/az_drone_controller_plane.cpp

@@ -0,0 +1,78 @@
+#include <QCoreApplication>
+#include <QDebug>
+#include "az_drone_controller_plane.h"
+
+AzDroneControllerPlane::AzDroneControllerPlane(AzMission &mission, QObject *parent) :
+    AzDroneController(mission, parent)
+{}
+
+
+bool AzDroneControllerPlane::stateWaitingMissionMode(void)
+{
+    Telemetry::FlightMode flight_mode = mTelemetry->flight_mode();
+    std::cout << "azDroneControllerPlane::stateWaitingHoldMode() Current mode: " << flight_mode << std::endl;
+    return flight_mode == Telemetry::FlightMode::Mission;
+}
+
+
+void AzDroneControllerPlane::droneStateMachineSlot(void)
+{
+    static const QString states[] = {
+        "DISCONNECTED",
+        "CONNECTED",
+        "AUTOPILOT",
+        "TELEMETRY_MODULE",
+        "ACTION_MODULE",
+        "READY_FOR_ARMING",
+        "AZ_DRONE_STATE_WAITING_MISSION_MODE",
+        "FLY_MISSION"
+    };
+
+    qDebug() << "\nAzDroneControllerPlane::droneStateMachineSlot() Current state:" << states[(int) mDroneState];
+
+    using MethodPtr = bool (AzDroneControllerPlane::*)();
+
+    struct MethodInfo
+    {
+        AzDroneState currentState;
+        MethodPtr nextStateMethodPtr;
+        QString description;
+        int nextStateDelay;
+    };
+
+    // Plane type will pass arming and taking off states and wait for auto mode from the RC controller instead.
+    static const MethodInfo stateMethods[]
+        = {{AZ_DRONE_STATE_DISCONNECTED, &AzDroneControllerPlane::stateConnect, "stateConnect()", 1000},
+           {AZ_DRONE_STATE_CONNECTED, &AzDroneControllerPlane::stateAutopilot, "stateAutopilot()", 1000},
+           {AZ_DRONE_STATE_AUTOPILOT, &AzDroneControllerPlane::stateTelemetryModule, "stateTelemetryModule()", 1000},
+           {AZ_DRONE_STATE_TELEMETRY_MODULE, &AzDroneControllerPlane::stateActionModule, "stateActionModule()", 1000},
+           {AZ_DRONE_STATE_ACTION_MODULE, &AzDroneControllerPlane::stateReadyForArming, "stateReadyForArming()", 1000},
+           {AZ_DRONE_STATE_ARMED, &AzDroneControllerPlane::stateWaitingMissionMode, "stateWaitingMissionMode()", 1000},
+           {AZ_DRONE_STATE_WAITING_HOLD_MODE, &AzDroneControllerPlane::stateFlyMission, "stateFlyMission()", 1000},
+           {AZ_DRONE_STATE_FLY_MISSION, nullptr, "No function to call. AzMissionControl running", 0}};
+
+    // Iterate through states. If the state function fails, it's repeated until it succeeds.
+    for (uint i = 0; i < sizeof(stateMethods) / sizeof(MethodInfo); i++) {
+        if (stateMethods[i].currentState == mDroneState) {
+            if (mDroneState == AZ_DRONE_STATE_FLY_MISSION) {
+                qDebug() << "AzMissionContoroller running. Waiting signals from it.";
+                return;
+            }
+
+            bool result = (this->*stateMethods[i].nextStateMethodPtr)();
+
+            if (result) {
+                qDebug() << "AzDroneControllerPlane::droneStateMachineSlot()" << stateMethods[i].description << "succeeded";
+                mDroneState = stateMethods[i + 1].currentState;
+            }
+            else {
+                qDebug() << "AzDroneControllerPlane::droneStateMachineSlot()" << stateMethods[i].description
+                         << "failed. Trying again.";
+            }
+
+            delayedStateCallSlot(stateMethods[i].nextStateDelay);
+
+            break;
+        }
+    }
+}

src/az_drone_controller_plane.h

@@ -0,0 +1,15 @@
+#pragma once
+
+#include "az_drone_controller.h"
+#include <QObject>
+
+class AzDroneControllerPlane : public AzDroneController
+{
+    Q_OBJECT
+public:
+    explicit AzDroneControllerPlane(AzMission &mission, QObject *parent = nullptr);
+
+protected:
+    void droneStateMachineSlot(void) override;
+    bool stateWaitingMissionMode(void);
+};

src/main.cpp

@@ -3,6 +3,7 @@
 #include <QFile>
 
 #include "az_drone_controller.h"
+#include "az_drone_controller_plane.h"
 #include "az_mission.h"
 
 int main(int argc, char *argv[])
@@ -39,8 +40,9 @@ int main(int argc, char *argv[])
     cout << mission;
 
     // Launch a drone controller using the MAVSDK for ArduPilot-based flight controllers.
-    AzDroneController droneController(mission);
+    bool isPlane = a.arguments().at(2) == "plane";
-    droneController.start();
+    AzDroneController *controller = isPlane ? new AzDroneControllerPlane(mission) : new AzDroneController(mission);
+    controller->start();
 
     return a.exec();
 }