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
2026-04-22 08:06:35 +00:00 · 2024-11-30 17:09:00 +01:00
parent be36fc5c50
commit 37e8cfd3fe
4 changed files with 182 additions and 16 deletions
@@ -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}}
-    , // TODO!! Autopilot or CompanionComputer?
-    mDroneState(AZ_DRONE_STATE_DISCONNECTED)
+    , mMavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}} // TODO!! Autopilot or CompanionComputer?
+    , 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;
-    return mMissionController->startMissions(flight_altitude_abs);
+    //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;
 }


@@ -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;
 }