diff --git a/drone_controller/az_drone_controller.cpp b/drone_controller/az_drone_controller.cpp index 5074a84..e50b4d7 100644 --- a/drone_controller/az_drone_controller.cpp +++ b/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(frame); - new_item.command = static_cast(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); } } diff --git a/drone_controller/az_drone_controller.h b/drone_controller/az_drone_controller.h index eaac073..fa42144 100644 --- a/drone_controller/az_drone_controller.h +++ b/drone_controller/az_drone_controller.h @@ -105,10 +105,10 @@ protected: shared_ptr mSystem; Telemetry *mTelemetry; Action *mAction; - MissionRaw *mMissionRaw; Telemetry::Position mFirstPosition; Telemetry::Position mCurrentPosition; AzMissionController *mMissionController; bool isCopterType; int mMissionItemSeqNum; + AzMission &mAzMission; }; diff --git a/drone_controller/az_mission_controller.cpp b/drone_controller/az_mission_controller.cpp index ecbee77..302a25f 100644 --- a/drone_controller/az_mission_controller.cpp +++ b/drone_controller/az_mission_controller.cpp @@ -1,121 +1,166 @@ #include #include +#include +#include +#include #include +#include +#include #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(frame); + new_item.command = static_cast(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; - } - else { - cerr << "mAction->goto_location() failed. Reason: " << result << endl; - return false; - } + if (mMissionItemSeqNum == 1) { + new_item.current = 1; } - qDebug() << "AzMissionController::flyToNextMissionItem() flying to the item" << mCurrentMissionIndex + 1 << "/" - << mMission.getActionPoints().size(); + mMissionItemSeqNum++; - const AzCoordinate &coordinate = mMission.getActionPoints().at(mCurrentMissionIndex).getPoint(); - qDebug() << "AzMissionController::flyToNextMissionItem() navigating to point" << coordinate.latitude - << coordinate.longitude; + return new_item; +} - 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; +bool AzMissionController::uploadMission(void) +{ + cout << "[MISSION] stateUploadMission() Setting raw mission starts" << endl; + + mMissionItemSeqNum = 0; + + if (mMissionRaw == nullptr) { + mMissionRaw = new MissionRaw(mSystem); } - else { - cerr << "mAction->goto_location() failed. Reason: " << result << endl; + + auto clearResult = mMissionRaw->clear_mission(); + if (clearResult != MissionRaw::Result::Success) { + std::cout << "[MISSION] stateUploadMission() Clearing mMissionRaw failed" << std::endl; return false; } + + 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 &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(); - } - } - } } diff --git a/drone_controller/az_mission_controller.h b/drone_controller/az_mission_controller.h index 764481c..304cc61 100644 --- a/drone_controller/az_mission_controller.h +++ b/drone_controller/az_mission_controller.h @@ -1,12 +1,12 @@ #pragma once #include -#include +#include +#include #include #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; }; diff --git a/drone_controller/mission.json b/drone_controller/mission.json index 7b39789..cc68cec 100644 --- a/drone_controller/mission.json +++ b/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] }