Changed directory structure and renamed applications

- autopilot -> drone_controller - rtsp_ai_player -> ai_controller - added top level qmake project file - updated documentation - moved small demo applications from tmp/ to misc/
2026-04-22 08:16:34 +00:00 · 2024-10-19 14:44:34 +02:00
parent 54b7dc41ca
commit 45c19baa45
94 changed files with 149 additions and 204 deletions
@@ -0,0 +1,64 @@
+#include <QCoreApplication>
+#include <QDebug>
+#include "az_drone_controller_plane.h"
+
+AzDroneControllerPlane::AzDroneControllerPlane(AzMission &mission, QObject *parent) :
+    AzDroneController(mission, parent)
+{}
+
+
+bool AzDroneControllerPlane::stateWaitAutoSwitch(void)
+{
+    Telemetry::FlightMode flight_mode = mTelemetry->flight_mode();
+    cout << "[CONTROLLER] Current flight mode: " << flight_mode << endl;
+    return flight_mode == Telemetry::FlightMode::Mission;
+}
+
+
+void AzDroneControllerPlane::droneStateMachineSlot(void)
+{
+    using MethodPtr = bool (AzDroneControllerPlane::*)();
+
+    struct MethodInfo
+    {
+        AzDroneState currentState;
+        MethodPtr nextStateMethodPtr;
+        string 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, "Connecting to autopilot", 1000},
+           {AZ_DRONE_STATE_CONNECTED, &AzDroneControllerPlane::stateGetSystem, "Getting MAVSDK system", 1000},
+           {AZ_DRONE_STATE_GOT_SYSTEM, &AzDroneControllerPlane::stateGetTelemetryModule, "Getting telemetry module", 1000},
+           {AZ_DRONE_STATE_GOT_TELEMETRY_MODULE, &AzDroneControllerPlane::stateGetActionModule, "Getting action module", 1000},
+           {AZ_DRONE_STATE_GOT_ACTION_MODULE, &AzDroneControllerPlane::stateHealthOk, "Drone health OK", 1000},
+           {AZ_DRONE_STATE_HEALTH_OK, &AzDroneControllerPlane::stateWaitAutoSwitch, "User switched to AUTO mode", 1000},
+           {AZ_DRONE_STATE_AUTO_MODE_ACTIVATED, &AzDroneControllerPlane::stateFlyMission, "Starting the mission", 1000},
+           {AZ_DRONE_STATE_FLY_MISSION, nullptr, "Mission started", 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) {
+                cout << "[CONTROLLER] Waiting signals from the mission controller." << endl;
+                return;
+            }
+
+            bool result = (this->*stateMethods[i].nextStateMethodPtr)();
+
+            if (result) {
+                cout << "[CONTROLLER] " << stateMethods[i].description << " succeeded." << endl;
+                mDroneState = stateMethods[i + 1].currentState;
+            }
+            else {
+                cout << "[CONTROLLER] " << stateMethods[i].description << " failed. Trying again." << endl;
+            }
+
+            delayedStateCallSlot(stateMethods[i].nextStateDelay);
+
+            break;
+        }
+    }
+}