Use raw missions in AutoPilot

- uses MAVSDK::MissionRaw objects for missions
- added new state AZ_DRONE_STATE_MISSION_UPLOADED
- new state is used in AzDroneControllerPlane before waiting for AUTO switch

TODO!!
- move to AzMissionController
- use JSON file instead of hard coded mission items

drone_controller/az_drone_controller.cpp

@@ -5,6 +5,8 @@
 #include <QTimer>
 
 #include <mavsdk/plugins/action/action.h>
+#include <mavsdk/plugins/mission/mission.h>
+#include <mavsdk/plugins/mission_raw/mission_raw.h>
 #include <mavsdk/plugins/telemetry/telemetry.h>
 
 #include "az_config.h"
@@ -12,9 +14,10 @@
 
 AzDroneController::AzDroneController(AzMission &mission, QObject *parent)
     : QObject(parent)
-    , mMavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}
+    , mMavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}} // TODO!! Autopilot or CompanionComputer?
-    , // TODO!! Autopilot or CompanionComputer?
+    , mDroneState(AZ_DRONE_STATE_DISCONNECTED)
-    mDroneState(AZ_DRONE_STATE_DISCONNECTED)
+    , mMissionRaw(NULL)
+    , mMissionItemSeqNum(0)
 {
     mFirstPosition.relative_altitude_m = -10000;
 
@@ -32,6 +35,42 @@ AzDroneController::AzDroneController(AzMission &mission, QObject *parent)
 
     // Healt info update from MAVSDK.
     connect(this, &AzDroneController::newHealthInfo, this, &AzDroneController::newHealthInfoSlot, Qt::QueuedConnection);
+
+    isCopterType = QCoreApplication::arguments().at(2) == "quadcopter";
+}
+
+
+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;
 }
 
 
@@ -142,6 +181,98 @@ 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);
+    }
+
+    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;
+}
+
+
 bool AzDroneController::stateArm(void)
 {
     Action::Result result = mAction->arm();
@@ -158,11 +289,6 @@ bool AzDroneController::stateArm(void)
 
 bool AzDroneController::stateTakeoff(void)
 {
-    // Drone never reaches the target altitude with ArduPilot. This seems
-    // to be a bug in MAVSDK/ArduPilot, because this doesn't happen with PX4.
-    // Added extra check to AzDroneController::stateFlyMission() to not start the
-    // mission before 90% of AZ_RELATIVE_FLY_ALTITUDE is reached.
-
     mAction->set_takeoff_altitude(AZ_RELATIVE_FLY_ALTITUDE);
     Action::Result result = mAction->takeoff();
     cout << "[CONTROLLER] MAVSDK::Action::takeoff() failed. Reason: " << result << endl;
@@ -172,14 +298,26 @@ bool AzDroneController::stateTakeoff(void)
 
 bool AzDroneController::stateFlyMission(void)
 {
-    if (mCurrentPosition.relative_altitude_m < AZ_RELATIVE_FLY_ALTITUDE * 0.90) {
+    // Drone never reaches the target altitude with ArduPilot. This seems
+    // to be a bug in MAVSDK/ArduPilot, because this doesn't happen with PX4.
+    // Added extra check to AzDroneController::stateFlyMission() to not start the
+    // mission before 90% of AZ_RELATIVE_FLY_ALTITUDE is reached.
+    if (isCopterType && mCurrentPosition.relative_altitude_m < AZ_RELATIVE_FLY_ALTITUDE * 0.90) {
         cout << "[CONTROLLER] 90% of the takeoff altitide is not reached yet." << endl;
         return false;
     }
 
     // TODO!! Check with the team about fly altitude. Is altitudes in JSON file absolute or relative?
-    float flight_altitude_abs = AZ_RELATIVE_FLY_ALTITUDE + mFirstPosition.absolute_altitude_m;
+    //float flight_altitude_abs = AZ_RELATIVE_FLY_ALTITUDE + mFirstPosition.absolute_altitude_m;
-    return mMissionController->startMissions(flight_altitude_abs);
+    //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;
 }
 
 
@@ -246,8 +384,19 @@ void AzDroneController::droneStateMachineSlot(void)
 
 void AzDroneController::newPositionSlot(Telemetry::Position position)
 {
-    cout << "[CONTROLLER] GPS position: " << position.latitude_deg << ", " << position.longitude_deg
+    /*
-         << " Altitudes: " << position.absolute_altitude_m << ", " << position.relative_altitude_m << endl;
+    static Telemetry::Position previousPosition;
+
+    // Only print position if it has changed.
+    if (position == previousPosition) {
+        return;
+    }
+
+
+    previousPosition = position;
+    */
+    //cout << "[CONTROLLER] GPS position: " << position.latitude_deg << ", " << position.longitude_deg
+    //     << " Altitudes: " << position.absolute_altitude_m << ", " << position.relative_altitude_m << endl;
 
     // 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.
@@ -267,7 +416,8 @@ void AzDroneController::newPositionSlot(Telemetry::Position position)
 
 void AzDroneController::newHeadingSlot(double heading)
 {
-    cout << "[CONTROLLER] Heading: " << heading << endl;
+    (void)heading;
+    //cout << "[CONTROLLER] Heading: " << heading << endl;
 }
 
 

drone_controller/az_drone_controller.h

@@ -5,6 +5,7 @@
 
 #include <mavsdk/mavsdk.h>
 #include <mavsdk/plugins/action/action.h>
+#include <mavsdk/plugins/mission_raw/mission_raw.h>
 #include <mavsdk/plugins/telemetry/telemetry.h>
 
 #include "az_mission.h"
@@ -19,6 +20,7 @@ typedef enum {
     AZ_DRONE_STATE_GOT_TELEMETRY_MODULE,
     AZ_DRONE_STATE_GOT_ACTION_MODULE,
     AZ_DRONE_STATE_HEALTH_OK,
+    AZ_DRONE_STATE_MISSION_UPLOADED,
     AZ_DRONE_STATE_ARMED,
     AZ_DRONE_STATE_TAKE_OFF,
     AZ_DRONE_STATE_AUTO_MODE_ACTIVATED,
@@ -44,8 +46,18 @@ protected:
     bool stateGetActionModule(void);
     bool stateHealthOk(void);
     bool stateArm(void);
+    bool stateUploadMission(void);
     bool stateTakeoff(void);
     bool stateFlyMission(void);
+    void setRawMission(void);
+    MissionRaw::MissionItem makeRawMissionItem(float latitude_deg1e7,
+                                               float longitude_deg1e7,
+                                               int32_t altitude_m,
+                                               float do_photo,
+                                               MAV_FRAME frame,
+                                               MAV_CMD command,
+                                               float p2 = 0,
+                                               float p3 = 0);
 
     // Slots that are called by the emitted Qt signals.
 protected slots:
@@ -93,7 +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;
 };

drone_controller/az_drone_controller_plane.cpp

@@ -34,7 +34,8 @@ void AzDroneControllerPlane::droneStateMachineSlot(void)
            {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_HEALTH_OK, &AzDroneControllerPlane::stateUploadMission, "Uploading the mission", 1000},
+           {AZ_DRONE_STATE_MISSION_UPLOADED, &AzDroneControllerPlane::stateWaitAutoSwitch, "Waiting user to switch to ArduPilot AUTO mode", 1000},
            {AZ_DRONE_STATE_AUTO_MODE_ACTIVATED, &AzDroneControllerPlane::stateFlyMission, "Starting the mission", 1000},
            {AZ_DRONE_STATE_FLY_MISSION, nullptr, "Mission started", 0}};
 

drone_controller/main.cpp

@@ -25,7 +25,7 @@ int main(int argc, char *argv[])
         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";
+        qCritical() << "\tFor example ./drone_controller mission.json plane udp";
         return 1;
     }