Added initial support for the ArduPlane. Added mandatory command line options.

How to use ArduCopter:
  Launch simulator with command: "./Tools/autotest/sim_vehicle.py --map --console -v ArduCopter"
  Launch autopilot with command: "./autopilot mission.json quadcopter udp"

How to use ArduPlane:
  Launch simulator with command: "./Tools/autotest/sim_vehicle.py --map --console -v ArduPlane"
  Wait 30 seconds and give following commands in the same terminal
    arm throttle
    mode takeoff
  Launch autopilot with command: "./autopilot mission.json plane udp"

Type: New Feature

src/az_drone_controller.cpp

@@ -179,7 +179,7 @@ void AzDroneController::missionFinishedSlot(void)
 
 void AzDroneController::droneStateMachineSlot(void)
 {
-    static QString states[] = {
+    static const QString states[] = {
         "DISCONNECTED",
         "CONNECTED",
         "AUTOPILOT",
@@ -205,7 +205,7 @@ void AzDroneController::droneStateMachineSlot(void)
         int nextStateDelay;
     };
 
-    const MethodInfo stateMethods[]
+    static const MethodInfo stateMethods[]
         = {{AZ_DRONE_STATE_DISCONNECTED, &AzDroneController::stateConnect, "stateConnect()", 1000},
            {AZ_DRONE_STATE_CONNECTED, &AzDroneController::stateAutopilot, "stateAutopilot()", 1000},
            {AZ_DRONE_STATE_AUTOPILOT, &AzDroneController::stateTelemetryModule, "stateTelemetryModule()", 1000},
@@ -216,10 +216,10 @@ void AzDroneController::droneStateMachineSlot(void)
            {AZ_DRONE_STATE_TAKE_OFF, &AzDroneController::stateFlyMission, "stateFlyMission()", 1000},
            {AZ_DRONE_STATE_FLY_MISSION, nullptr, "No function to call. AzMissionControl running", 0}};
 
-    // Iterate through states. If state function fails, then it's tried again until it succeeds.
+    // 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 ((int) mDroneState == (int) AZ_DRONE_STATE_FLY_MISSION) {
+            if (mDroneState == AZ_DRONE_STATE_FLY_MISSION) {
                 qDebug() << "AzMissionContoroller running. Waiting signals from it.";
                 return;
             }
@@ -228,10 +228,17 @@ void AzDroneController::droneStateMachineSlot(void)
 
             if (result) {
                 qDebug() << "AzDroneController::droneStateMachineSlot()" << stateMethods[i].description << "succeeded";
-                qDebug() << "AzDroneController::droneStateMachineSlot() calling next state function in"
+
-                         << stateMethods[i].nextStateDelay << "ms.";
                 mDroneState = stateMethods[i + 1].currentState;
-                qDebug() << "AzDroneController::droneStateMachineSlot() New state:" << states[(int) mDroneState];
+
+                // 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
@@ -247,6 +254,8 @@ void AzDroneController::droneStateMachineSlot(void)
 
 void AzDroneController::newPositionSlot(Telemetry::Position position)
 {
+    qDebug() << "AzDroneController::newPositionSlot()" << position.latitude_deg << position.longitude_deg;
+
     // 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.
     if (mFirstPosition.relative_altitude_m < -1000) {

src/az_mission_controller.cpp

@@ -1,3 +1,4 @@
+#include <QCoreApplication>
 #include <QDebug>
 
 #include <mavsdk/plugins/telemetry/telemetry.h>
@@ -82,9 +83,8 @@ void AzMissionController::newPosition(Telemetry::Position pos)
     }
 
     if (mFlyingToReturnPoint == true) {
-        const AzCoordinate &coordinate = mMission.getReturnPoint();
+        const AzCoordinate &point = mMission.getReturnPoint();
-        float distance
+        float distance = AzUtils::distance(pos.latitude_deg, pos.longitude_deg, point.latitude, point.longitude);
-            = AzUtils::distance(pos.latitude_deg, pos.longitude_deg, coordinate.latitude, coordinate.longitude);
 
         qDebug() << "AzMissionController::newPosition() distance to return point:" << distance << " km.";
 
@@ -102,9 +102,20 @@ void AzMissionController::newPosition(Telemetry::Position pos)
         qDebug() << "AzMissionController::newPosition() distance to target:" << distance << "km.";
 
         // TODO!! In final application we need to use the camera to find the target.
-        if (distance <= 0.01) {
+        if (QCoreApplication::arguments().contains("drone") && distance <= 0.01) {
             qDebug() << "AzMissionController::newPosition() target reached. Continuing to the next item.";
             flyToNextMissionItem();
         }
+        else if (QCoreApplication::arguments().contains("plane")) {
+            if (mPlaneCirclingTime.isValid() == false && distance <= 0.1) {
+                qDebug() << "AzMissionController::newPosition() target reached. Starting circling.";
+                mPlaneCirclingTime.restart();
+            }
+            else if (mPlaneCirclingTime.elapsed() > 35000) {
+                qDebug() << "AzMissionController::newPosition() target reached. Ending circling.";
+                mPlaneCirclingTime.invalidate();
+                flyToNextMissionItem();
+            }
+        }
     }
 }

src/az_mission_controller.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <QObject>
+#include <QElapsedTimer>
 
 #include <mavsdk/plugins/telemetry/telemetry.h>
 
@@ -34,4 +35,5 @@ private:
     bool mMissionStarted;
     bool mFlyingToReturnPoint;
     float mAbsoluteAltitude;
+    QElapsedTimer mPlaneCirclingTime;
 };

src/main.cpp

@@ -1,5 +1,6 @@
 #include <QCoreApplication>
 #include <QDebug>
+#include <QFile>
 
 #include "az_drone_controller.h"
 #include "az_mission.h"
@@ -9,9 +10,27 @@ int main(int argc, char *argv[])
     // This is needed to have main event loop and signal-slot events in the AzDroneController.
     QCoreApplication a(argc, argv);
 
-    if (argc < 2) {
+    if (a.arguments().size() != 4) {
-        qCritical() << "\nPass the mission JSON file as the first argument.";
+        qCritical() << "\nProgram needs three command line parameters.";
-        qCritical() << "A serial port connection can be enabled with \"serial\" as the second argument.\n";
+        qCritical() << "\tFirst argument:  mission JSON file.";
+        qCritical() << "\tSecond argument: \"quadcopter\" or \"plane\".";
+        qCritical() << "\tThird argument:  \"udp\" or \"serial\" for connection type.";
+        qCritical() << "\tFor example ./autopilot mission.json plane udp";
+        return 1;
+    }
+
+    if (QFile::exists(a.arguments().at(1)) == false) {
+        qCritical() << "\nMission file doesn't exist";
+        return 1;
+    }
+
+    if (a.arguments().at(2) != "plane" && a.arguments().at(2) != "quadcopter") {
+        qCritical() << "\nPass \"quadcopter\" or \"plane\" for drone type as second argument";
+        return 1;
+    }
+
+    if (a.arguments().at(3) != "udp" && a.arguments().at(3) != "serial") {
+        qCritical() << "Pass \"udp\" or \"serial\" for connection type as third argument";
         return 1;
     }