Use MAVSDK::RawMission in AzMissionController

TODO!!
- send mission indexes from AzMissionController to AzDroneController
- handle finishing of the mission

drone_controller/az_drone_controller.cpp

@@ -12,27 +12,16 @@
 #include "az_config.h"
 #include "az_drone_controller.h"
 
-AzDroneController::AzDroneController(AzMission &mission, QObject *parent)
+
+AzDroneController::AzDroneController(AzMission &azMission, QObject *parent)
     : QObject(parent)
     , mMavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}} // TODO!! Autopilot or CompanionComputer?
     , mDroneState(AZ_DRONE_STATE_DISCONNECTED)
-    , mMissionRaw(NULL)
-    , mMissionItemSeqNum(0)
+    , mMissionController(nullptr)
+    , mAzMission(azMission)
 {
     mFirstPosition.relative_altitude_m = -10000;
 
-    mMissionController = new AzMissionController(mission, this);
-
-    // Mission progress from the AzMissionController. Slot will be used to find the targets later.
-    connect(
-        mMissionController,
-        &AzMissionController::missionProgressChanged,
-        this,
-        &AzDroneController::missionIndexChangedSlot);
-
-    // Mission controller signals end of the missions. This will be used to fly to the return point in JSON.
-    connect(mMissionController, &AzMissionController::finished, this, &AzDroneController::missionFinishedSlot);
-
     // Healt info update from MAVSDK.
     connect(this, &AzDroneController::newHealthInfo, this, &AzDroneController::newHealthInfoSlot, Qt::QueuedConnection);
 
@@ -40,40 +29,6 @@ AzDroneController::AzDroneController(AzMission &mission, QObject *parent)
 }
 
 
-MissionRaw::MissionItem AzDroneController::makeRawMissionItem(
-    float latitude_deg1e7,
-    float longitude_deg1e7,
-    int32_t altitude_m,
-    float do_photo,
-    MAV_FRAME frame,
-    MAV_CMD command,
-    float p2,
-    float p3)
-{
-    (void)p2;
-    (void)p3;
-
-    MissionRaw::MissionItem new_item{};
-    new_item.seq = mMissionItemSeqNum;
-    new_item.frame = static_cast<uint32_t>(frame);
-    new_item.command = static_cast<uint32_t>(command);
-    new_item.param1 = do_photo;
-    new_item.x = latitude_deg1e7 * 1e7;
-    new_item.y = longitude_deg1e7 * 1e7;
-    new_item.z = altitude_m;
-    new_item.mission_type = MAV_MISSION_TYPE_MISSION;
-    new_item.autocontinue = 1;
-
-    if (mMissionItemSeqNum == 1) {
-        new_item.current = 1;
-    }
-
-    mMissionItemSeqNum++;
-
-    return new_item;
-}
-
-
 void AzDroneController::start()
 {
     // Must wait that main event loop is launched in main()
@@ -160,12 +115,7 @@ bool AzDroneController::stateGetActionModule(void)
 {
     mAction = new Action(mSystem);
 
-    if (mAction != nullptr) {
-        mMissionController->setAction(mAction);
-        return true;
-    }
-
-    return false;
+    return mAction == nullptr ? false : true;
 }
 
 
@@ -183,93 +133,11 @@ bool AzDroneController::stateHealthOk(void)
 
 bool AzDroneController::stateUploadMission(void)
 {
-    cout << "[CONTROLLER] stateUploadMission() Setting raw mission starts" << endl;
-
-    mMissionItemSeqNum = 0;
-
-    if (mMissionRaw == NULL) {
-        mMissionRaw = new MissionRaw(*mSystem);
+    if (mMissionController == nullptr) {
+        mMissionController = new AzMissionController(*mSystem.get(), mAzMission, this);
     }
 
-    mMissionRaw->clear_mission();
-
-    if (mMissionRaw == NULL) {
-        mMissionRaw = new MissionRaw(*mSystem);
-    }
-
-    auto clearResult = mMissionRaw->clear_mission();
-    if (clearResult != MissionRaw::Result::Success) {
-        std::cout << "[CONTROLLER] stateUploadMission() Clearing mMissionRaw failed" << std::endl;
-        return false;
-    }
-
-    auto downloadResult = mMissionRaw->download_mission();
-    if (downloadResult.first != MissionRaw::Result::Success) {
-        std::cout << "[CONTROLLER] stateUploadMission() Downloading mission failed" << std::endl;
-        return false;
-    }
-
-    // first point in case of ardupilot is always home
-    auto missionPlan = downloadResult.second;
-    MissionRaw::MissionItem homePoint = missionPlan[0];
-    missionPlan.clear();
-
-    // going relative alt mission so we dont care about altitude
-    auto latDeg = homePoint.x * 1e-7;
-    auto lonDeg = homePoint.y * 1e-7;
-
-    // in case of ardupilot we want to set lat lon to 0, to use current position as takeoff position
-    missionPlan.push_back(makeRawMissionItem(
-        0, // lat
-        0, // lon
-        50.0,
-        0,
-        MAV_FRAME_GLOBAL_RELATIVE_ALT,
-        MAV_CMD_NAV_TAKEOFF));
-
-    // setup speed during mission execution
-    missionPlan.push_back(makeRawMissionItem(
-        0, 0, 0, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_DO_CHANGE_SPEED, 9.35f, -1.0f));
-
-    missionPlan.push_back(makeRawMissionItem(
-        latDeg + 0.001,
-        lonDeg + 0.001,
-        50.00,
-        0,
-        MAV_FRAME_GLOBAL_TERRAIN_ALT,
-        MAV_CMD_NAV_WAYPOINT));
-
-    missionPlan.push_back(makeRawMissionItem(
-        latDeg + 0.001,
-        lonDeg - 0.001,
-        50.00,
-        0,
-        MAV_FRAME_GLOBAL_TERRAIN_ALT,
-        MAV_CMD_NAV_WAYPOINT));
-
-    missionPlan.push_back(makeRawMissionItem(
-        latDeg - 0.001, lonDeg, 50.00, 0, MAV_FRAME_GLOBAL_TERRAIN_ALT, MAV_CMD_NAV_WAYPOINT));
-
-    missionPlan.push_back(makeRawMissionItem(
-        latDeg, lonDeg, 50.00, 0, MAV_FRAME_GLOBAL_TERRAIN_ALT, MAV_CMD_NAV_LAND));
-
-    missionPlan.push_back(
-        makeRawMissionItem(0, 0, 0, 0, MAV_FRAME_GLOBAL_INT, MAV_CMD_NAV_RETURN_TO_LAUNCH));
-
-    for (const auto& item : missionPlan) {
-        std::cout << "[CONTROLLER] stateUploadMission() seq: " << (int)item.seq << '\n';
-    }
-
-    auto uploadResult = mMissionRaw->upload_mission(missionPlan);
-    if (uploadResult != MissionRaw::Result::Success) {
-        std::cout << "[CONTROLLER] stateUploadMission() upload failed. Result: " << uploadResult << std::endl;
-        return false;
-    }
-
-    mMissionRaw->set_current_mission_item(0);
-    cout << "[CONTROLLER] stateUploadMission() Setting raw mission ends" << endl;
-
-    return true;
+    return mMissionController->uploadMission();
 }
 
 
@@ -311,13 +179,7 @@ bool AzDroneController::stateFlyMission(void)
     //float flight_altitude_abs = AZ_RELATIVE_FLY_ALTITUDE + mFirstPosition.absolute_altitude_m;
     //return mMissionController->startMissions(flight_altitude_abs);
 
-    // start mission, this set autopilot auto mode.
-    // ignore result, we dont care for now
-    MissionRaw::Result startResult = mMissionRaw->start_mission();
-    cout << "Start mission result:" << startResult << endl;
-    cout << "AzDroneController::stateFlyMission() ends\n";
-
-    return startResult == MissionRaw::Result::Success;
+    return mMissionController->startMission();
 }
 
 
@@ -408,7 +270,7 @@ void AzDroneController::newPositionSlot(Telemetry::Position position)
     mCurrentPosition = position;
 
     // Send new position to the mission controller.
-    if (mDroneState == AZ_DRONE_STATE_FLY_MISSION) {
+    if (mDroneState == AZ_DRONE_STATE_FLY_MISSION && mMissionController) {
         mMissionController->newPosition(position);
     }
 }

drone_controller/az_drone_controller.h

@@ -105,10 +105,10 @@ protected:
     shared_ptr<System> mSystem;
     Telemetry *mTelemetry;
     Action *mAction;
-    MissionRaw *mMissionRaw;
     Telemetry::Position mFirstPosition;
     Telemetry::Position mCurrentPosition;
     AzMissionController *mMissionController;
     bool isCopterType;
     int mMissionItemSeqNum;
+    AzMission &mAzMission;
 };

drone_controller/az_mission_controller.cpp

@@ -1,121 +1,166 @@
 #include <QCoreApplication>
 #include <QDebug>
 
+#include <mavsdk/mavsdk.h>
+#include <mavsdk/plugins/action/action.h>
+#include <mavsdk/plugins/mission/mission.h>
 #include <mavsdk/plugins/telemetry/telemetry.h>
+#include <mavsdk/plugins/mission_raw/mission_raw.h>
+#include <mavsdk/plugins/offboard/offboard.h>
 
 #include "az_mission.h"
 #include "az_mission_controller.h"
 #include "az_utils.h"
 
-AzMissionController::AzMissionController(AzMission &mission, QObject *parent)
+
+AzMissionController::AzMissionController(System &system, AzMission &mission, QObject *parent)
     : QObject(parent)
-    , mMission(mission)
-    , mAction(nullptr)
+    , mAzMission(mission)
     , mMissionStarted(false)
-    , mFlyingToReturnPoint(false)
-    , mAbsoluteAltitude(-10000)
+    , mMissionItemSeqNum(0)
+    , mMissionRaw(nullptr)
+    , mSystem(system)
 {}
 
-void AzMissionController::setAction(Action *action)
+
+MissionRaw::MissionItem AzMissionController::makeRawMissionItem(
+    float latitudeDeg1e7,
+    float longitudeDeg1e7,
+    int32_t altitude,
+    float doPhoto,
+    MAV_FRAME frame,
+    MAV_CMD command,
+    float p2,
+    float p3)
 {
-    mAction = action;
-}
+    (void)p2;
+    (void)p3;
 
-bool AzMissionController::startMissions(float absoluteAltitude)
-{
-    mCurrentMissionIndex = -1;
-    mMissionStarted      = true;
-    mAbsoluteAltitude    = absoluteAltitude; // TODO!! Use altitudes from the JSON file.
-    return flyToNextMissionItem();
-}
+    MissionRaw::MissionItem new_item{};
+    new_item.seq          = mMissionItemSeqNum;
+    new_item.frame        = static_cast<uint32_t>(frame);
+    new_item.command      = static_cast<uint32_t>(command);
+    new_item.param1       = doPhoto;
+    new_item.x            = latitudeDeg1e7 * 1e7;
+    new_item.y            = longitudeDeg1e7 * 1e7;
+    new_item.z            = altitude;
+    new_item.mission_type = MAV_MISSION_TYPE_MISSION;
+    new_item.autocontinue = 1;
 
-void AzMissionController::stopMissions(void)
-{
-    // TODO!! Needs to be improved. At least send signal to AzDroneController.
-    mMissionStarted = false;
-}
-
-bool AzMissionController::flyToNextMissionItem(void)
-{
-    mCurrentMissionIndex++;
-
-    if (mCurrentMissionIndex == (int) mMission.getActionPoints().size()) {
-        mFlyingToReturnPoint = true;
-        qDebug() << "AzMissionController::flyToNextMissionItem() All action points handled.";
-        qDebug() << "AzMissionController::flyToNextMissionItem() Flying to the JSON return point.";
-        const AzCoordinate &coordinate = mMission.getReturnPoint();
-        Action::Result result
-            = mAction->goto_location(coordinate.latitude, coordinate.longitude, mAbsoluteAltitude, 0.0f);
-
-        if (result == Action::Result::Success) {
-            return true;
+    if (mMissionItemSeqNum == 1) {
+        new_item.current = 1;
     }
-        else {
-            cerr << "mAction->goto_location() failed. Reason: " << result << endl;
+
+    mMissionItemSeqNum++;
+
+    return new_item;
+}
+
+
+bool AzMissionController::uploadMission(void)
+{
+    cout << "[MISSION] stateUploadMission() Setting raw mission starts" << endl;
+
+    mMissionItemSeqNum = 0;
+
+    if (mMissionRaw == nullptr) {
+        mMissionRaw = new MissionRaw(mSystem);
+    }
+
+    auto clearResult = mMissionRaw->clear_mission();
+    if (clearResult != MissionRaw::Result::Success) {
+        std::cout << "[MISSION] stateUploadMission() Clearing mMissionRaw failed" << std::endl;
         return false;
     }
-    }
 
-    qDebug() << "AzMissionController::flyToNextMissionItem() flying to the item" << mCurrentMissionIndex + 1 << "/"
-             << mMission.getActionPoints().size();
-
-    const AzCoordinate &coordinate = mMission.getActionPoints().at(mCurrentMissionIndex).getPoint();
-    qDebug() << "AzMissionController::flyToNextMissionItem() navigating to point" << coordinate.latitude
-             << coordinate.longitude;
-
-    Action::Result result = mAction->goto_location(coordinate.latitude, coordinate.longitude, mAbsoluteAltitude, 0.0f);
-
-    // TODO!! Check return value and print warnings and errors.
-    if (result == Action::Result::Success) {
-        emit missionProgressChanged(mCurrentMissionIndex + 1, mMission.getActionPoints().size());
-        return true;
-    }
-    else {
-        cerr << "mAction->goto_location() failed. Reason: " << result << endl;
+    auto downloadResult = mMissionRaw->download_mission();
+    if (downloadResult.first != MissionRaw::Result::Success) {
+        std::cout << "[MISSION] stateUploadMission() Downloading mission failed" << std::endl;
         return false;
     }
+
+    // First point in case of ArduPilot is always home
+    auto missionPlan = downloadResult.second;
+    MissionRaw::MissionItem homePoint = missionPlan[0];
+    missionPlan.clear();
+
+    // Going relative alt mission so we dont care about altitude
+    auto latDeg = homePoint.x * 1e-7;
+    auto lonDeg = homePoint.y * 1e-7;
+    cout << "[MISSION] Home location: " << latDeg << ", " << lonDeg << endl;
+
+    // In case of ardupilot we want to set lat lon to 0, to use current position as takeoff position
+    missionPlan.push_back(makeRawMissionItem(
+        0, // lat
+        0, // lon
+        50.0,
+        0,
+        MAV_FRAME_GLOBAL_RELATIVE_ALT,
+        MAV_CMD_NAV_TAKEOFF));
+
+    // Setup speed during mission execution
+    missionPlan.push_back(makeRawMissionItem(
+        0, 0, 0, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_DO_CHANGE_SPEED, 9.35f, -1.0f));
+
+    // Action points
+    const vector<AzActionPoint> &azMissions = mAzMission.getActionPoints();
+    for (uint i = 0; i < azMissions.size(); i++) {
+        missionPlan.push_back(makeRawMissionItem(
+            azMissions[i].getPoint().latitude,
+            azMissions[i].getPoint().longitude,
+            azMissions[i].getHeight(),
+            0,
+            MAV_FRAME_GLOBAL_TERRAIN_ALT,
+            MAV_CMD_NAV_WAYPOINT));
+    }
+
+    // Landing point
+    missionPlan.push_back(makeRawMissionItem(
+        mAzMission.getReturnPoint().latitude,
+        mAzMission.getReturnPoint().longitude,
+        50.00,
+        0,
+        MAV_FRAME_GLOBAL_TERRAIN_ALT,
+        MAV_CMD_NAV_LAND));
+
+    //missionPlan.push_back(makeRawMissionItem(0, 0, 0, 0, MAV_FRAME_GLOBAL_INT, MAV_CMD_NAV_RETURN_TO_LAUNCH));
+
+    for (const auto& item : missionPlan) {
+        std::cout << "[MISSION] stateUploadMission() seq: " << (int)item.seq << '\n';
+    }
+
+    auto uploadResult = mMissionRaw->upload_mission(missionPlan);
+    if (uploadResult != MissionRaw::Result::Success) {
+        std::cout << "[MISSION] stateUploadMission() upload failed. Result: " << uploadResult << std::endl;
+        return false;
+    }
+
+    mMissionRaw->set_current_mission_item(0);
+    cout << "[MISSION] stateUploadMission() Setting raw mission ends" << endl;
+
+    return true;
 }
 
+
+bool AzMissionController::startMission()
+{
+    MissionRaw::Result startResult = mMissionRaw->start_mission();
+    cout << "[MISSION] Start mission result:" << startResult << endl;
+    return startResult == MissionRaw::Result::Success;
+}
+
+
+void AzMissionController::stopMission(void)
+{
+    mMissionRaw->pause_mission();
+}
+
+
 void AzMissionController::newPosition(Telemetry::Position pos)
 {
+    (void)pos;
+
     if (mMissionStarted == false) {
         return;
     }
-
-    if (mFlyingToReturnPoint == true) {
-        const AzCoordinate &point = mMission.getReturnPoint();
-        float distance = AzUtils::distance(pos.latitude_deg, pos.longitude_deg, point.latitude, point.longitude);
-
-        qDebug() << "AzMissionController::newPosition() distance to return point:" << distance << " km.";
-
-        if (distance <= 0.01) {
-            qDebug() << "AzMissionController::newPosition() return point reached. Mission finished.";
-            mMissionStarted      = false;
-            mFlyingToReturnPoint = false;
-            emit finished();
-        }
-    }
-    else {
-        const AzCoordinate &target = mMission.getActionPoints().at(mCurrentMissionIndex).getPoint();
-        float distance = AzUtils::distance(pos.latitude_deg, pos.longitude_deg, target.latitude, target.longitude);
-
-        qDebug() << "AzMissionController::newPosition() distance to target:" << distance << "km.";
-
-        // TODO!! In final application we need to use the camera to find the target.
-        if (QCoreApplication::arguments().contains("quadcopter") && 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();
-            }
-        }
-    }
 }

drone_controller/az_mission_controller.h

@@ -1,12 +1,12 @@
 #pragma once
 
 #include <QObject>
-#include <QElapsedTimer>
 
+#include <mavsdk/mavsdk.h>
+#include <mavsdk/plugins/mission_raw/mission_raw.h>
 #include <mavsdk/plugins/telemetry/telemetry.h>
 
 #include "az_mission.h"
-#include "plugins/action/action.h"
 
 using namespace mavsdk;
 
@@ -14,10 +14,20 @@ class AzMissionController : public QObject
 {
     Q_OBJECT
 public:
-    explicit AzMissionController(AzMission &mission, QObject *parent = nullptr);
-    void setAction(Action *action);
-    bool startMissions(float absoluteAltitude);
-    void stopMissions(void);
+    explicit AzMissionController(System &system, AzMission &mission, QObject *parent = nullptr);
+    bool startMission();
+    void stopMission(void);
+    bool uploadMission(void);
+
+private:
+    MissionRaw::MissionItem makeRawMissionItem(float latitudeDeg1e7,
+                                               float longitudeDeg1e7,
+                                               int32_t altitude,
+                                               float doPhoto,
+                                               MAV_FRAME frame,
+                                               MAV_CMD command,
+                                               float p2 = 0,
+                                               float p3 = 0);
 
 public slots:
     void newPosition(Telemetry::Position position);
@@ -27,13 +37,10 @@ signals:
     void finished(void);
 
 private:
-    bool flyToNextMissionItem(void);
-
-    AzMission &mMission;
-    Action *mAction;
+    AzMission &mAzMission;
     int mCurrentMissionIndex;
     bool mMissionStarted;
-    bool mFlyingToReturnPoint;
-    float mAbsoluteAltitude;
-    QElapsedTimer mPlaneCirclingTime;
+    int mMissionItemSeqNum;
+    MissionRaw *mMissionRaw;
+    System &mSystem;
 };

drone_controller/mission.json

@@ -7,11 +7,11 @@
     ]
   },
   "action_points": [
-    {"point": [-35.35967231, 149.16146047], "height": 500, "action_enum": "waypoint"},
-    {"point": [-35.35973951, 149.16801175], "height": 400, "action_enum": "search", "action_specific": {"targets": ["artillery"]}},
-    {"point": [-35.36408532, 149.16816283], "height": 300, "action_enum": "search", "action_specific": {"targets": ["artillery", "tank"]}},
-    {"point": [-35.36546294, 149.16479791], "height": 300, "action_enum": "search", "action_specific": {"targets": ["tank"]}},
-    {"point": [-35.36364851, 149.16073255], "height": 500, "action_enum": "return"}
+    {"point": [-35.35967231, 149.16146047], "height": 50, "action_enum": "waypoint"},
+    {"point": [-35.35973951, 149.16801175], "height": 50, "action_enum": "search", "action_specific": {"targets": ["artillery"]}},
+    {"point": [-35.36408532, 149.16816283], "height": 50, "action_enum": "search", "action_specific": {"targets": ["artillery", "tank"]}},
+    {"point": [-35.36546294, 149.16479791], "height": 50, "action_enum": "search", "action_specific": {"targets": ["tank"]}},
+    {"point": [-35.36364851, 149.16073255], "height": 50, "action_enum": "return"}
   ],
   "return_point": [-35.36286449, 149.16534729]
 }