From b0b17d7fcc95df5be173299f68acae20d20d9eed Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tuomas=20J=C3=A4rvinen?= Date: Sun, 19 May 2024 19:49:39 +0200 Subject: [PATCH] Improve comments in Autopilot Issue: https://denyspopov.atlassian.net/browse/AZ-14 Type: Improvement --- src/az_config.h | 4 +- src/az_drone_controller.cpp | 123 ++++++++++++++++++---------------- src/az_drone_controller.h | 24 ++++++- src/az_mission.cpp | 12 ++-- src/az_mission_controller.cpp | 52 ++++++-------- src/az_mission_controller.h | 2 + src/az_utils.cpp | 11 +-- src/main.cpp | 8 ++- 8 files changed, 131 insertions(+), 105 deletions(-) diff --git a/src/az_config.h b/src/az_config.h index 737390b..5de2d2c 100644 --- a/src/az_config.h +++ b/src/az_config.h @@ -2,5 +2,5 @@ #define FUNCTION_NAME(func) func() -const char *AZ_CONNECTION = "udp://:14550"; -const int AZ_RELATIVE_FLY_ALTITUDE = 50; +const char *AZ_CONNECTION = "udp://:14550"; +const int AZ_RELATIVE_FLY_ALTITUDE = 50; diff --git a/src/az_drone_controller.cpp b/src/az_drone_controller.cpp index db3b1d2..f6ad363 100644 --- a/src/az_drone_controller.cpp +++ b/src/az_drone_controller.cpp @@ -1,52 +1,63 @@ -#include -#include #include #include #include #include -#include + +#include +#include + #include "az_config.h" #include "az_drone_controller.h" -AzDroneController::AzDroneController(AzMission &mission, QObject *parent) : - QObject(parent), - mMavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}, // TODO!! Autopilot or CompanionComputer? +AzDroneController::AzDroneController(AzMission &mission, QObject *parent) + : QObject(parent) + , mMavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}} + , // TODO!! Autopilot or CompanionComputer? mDroneState(AZ_DRONE_STATE_DISCONNECTED) { qDebug() << "AzDroneController::AzDroneController() Thread ID: " << QThread::currentThreadId(); mFirstPosition.relative_altitude_m = -10000; mMissionController = new AzMissionController(mission, this); - connect(mMissionController, &AzMissionController::missionProgressChanged, this, &AzDroneController::missionIndexChangedSlot); + + // Connects signals about the mission progress from the AzMissionController to the slot + // in this class. missionIndexChangedSlot() will be used later to find the targets. + connect( + mMissionController, + &AzMissionController::missionProgressChanged, + this, + &AzDroneController::missionIndexChangedSlot); + + // Connects signal from the mission controller to the slot in this class to finish the flying. connect(mMissionController, &AzMissionController::finished, this, &AzDroneController::missionFinishedSlot); } - void AzDroneController::start() { // Must wait that main event loop is launched in main() delayedStateCallSlot(0); } - void AzDroneController::delayedStateCallSlot(int ms) { + // MAVSDK examples use blocking sleep() calls for timeouts. + // QTimer provides non-blocking state machine operations. QTimer::singleShot(ms, this, &AzDroneController::droneStateMachineSlot); } - bool AzDroneController::stateConnect(void) { + // Connects to the flight controller based on AZ_CONNECTION define in AzConfig. + // TODO!! Add command line option to change between UDP and UART connections. return mMavsdk.add_any_connection(AZ_CONNECTION) == ConnectionResult::Success; } - bool AzDroneController::stateAutopilot(void) { + // Get the first autopilot from the flight controller. return (mSystem = mMavsdk.first_autopilot(3.0).value()) != nullptr; } - bool AzDroneController::stateTelemetryModule(void) { if ((mTelemetry = new Telemetry(mSystem)) == nullptr) { @@ -55,29 +66,26 @@ bool AzDroneController::stateTelemetryModule(void) const auto setRateResult = mTelemetry->set_rate_position(1); // 1 Hz if (setRateResult != Telemetry::Result::Success) { - qCritical() << "Setting rate failed: " << (int)setRateResult; + qCritical() << "Setting rate failed: " << (int) setRateResult; return false; } - // Subscripe to position updates. Updated comes from different MAVSDK thread and so - // we send it as event to this function so that it's handled in the main thread. + // Subscripe to position updates. Updated comes from different MAVSDK thread. Send position + // as signal to this class (Qt::QueuedConnection) so that it's handled in the main thread. connect(this, &AzDroneController::newPosition, this, &AzDroneController::newPositionSlot, Qt::QueuedConnection); - mTelemetry->subscribe_position([this](Telemetry::Position position) { - emit newPosition(position); - }); + mTelemetry->subscribe_position([this](Telemetry::Position position) { emit newPosition(position); }); + // TODO!! This doesn't work. Check how ArduPilot sets home position. Telemetry::Position home = mTelemetry->home(); - qDebug() << "Home: lat:" << home.latitude_deg - << "lon:" << home.longitude_deg - << "altitude abs:" << home.absolute_altitude_m - << "altitude rel:" << home.relative_altitude_m; + qDebug() << "Home: lat:" << home.latitude_deg << "lon:" << home.longitude_deg + << "altitude abs:" << home.absolute_altitude_m << "altitude rel:" << home.relative_altitude_m; return true; } - bool AzDroneController::stateActionModule(void) { + // TODO!! Check return value and print warnings and errors. mAction = new Action(mSystem); if (mAction == nullptr) { return false; @@ -87,55 +95,53 @@ bool AzDroneController::stateActionModule(void) return true; } - bool AzDroneController::stateReadyForArming(void) { + // TODO!! Check all telemetry components and print warnings and errors. return mTelemetry->health_all_ok(); } - bool AzDroneController::stateArm(void) { + // TODO!! Check return value and print warnings and errors. return mAction->arm() == Action::Result::Success; } - bool AzDroneController::stateTakeoff(void) { + // TODO!! Drone never reaches the target altitude with ArduPilot. Investigate and fix. mAction->set_takeoff_altitude(50); - qDebug() << "AzDroneController::stateTakeoff() taking off starts"; - Action::Result result = mAction->takeoff(); //return mAction->takeoff() == Action::Result::Success; - qDebug() << "AzDroneController::stateTakeoff() taking off ends"; + // TODO!! Check return value and print warnings and errors. + Action::Result result = mAction->takeoff(); return result == Action::Result::Success; } - bool AzDroneController::stateFlyMission(void) { + // 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); } - void AzDroneController::missionFinishedSlot(void) { qDebug() << "AzDroneController::missionFinishedSlot() Landing the drone."; + // TODO!! Check return value and print warnings and errors. mAction->land(); - // TODO!! This must be improved a lot. + // TODO!! This must be improved a lot. Disarming should be done when drone lands. QTimer::singleShot(20000, this, &AzDroneController::disarmDroneSlot); } - void AzDroneController::disarmDroneSlot(void) { qDebug() << "AzDroneController::disarmDroneSlot() Disarming the drone and quit."; - // TODO!! This must be improved a lot. + // TODO!! Check return value and print warnings and errors. mAction->disarm(); + // TODO!! Check with the team what to do after the landing. QCoreApplication::instance()->quit(); } - void AzDroneController::droneStateMachineSlot(void) { static QString states[] = { @@ -152,32 +158,33 @@ void AzDroneController::droneStateMachineSlot(void) }; qDebug() << ""; - qDebug() << "AzDroneController::droneStateMachineSlot() Current state:" << states[(int)mDroneState]; + qDebug() << "AzDroneController::droneStateMachineSlot() Current state:" << states[(int) mDroneState]; using MethodPtr = bool (AzDroneController::*)(); - struct MethodInfo { + struct MethodInfo + { AzDroneState currentState; MethodPtr nextStateMethodPtr; QString description; int nextStateDelay; }; - const MethodInfo stateMethods[] = { - { AZ_DRONE_STATE_DISCONNECTED, &AzDroneController::stateConnect, "stateConnect()", 1000 }, - { AZ_DRONE_STATE_CONNECTED, &AzDroneController::stateAutopilot, "stateAutopilot()", 1000 }, - { AZ_DRONE_STATE_AUTOPILOT, &AzDroneController::stateTelemetryModule, "stateTelemetryModule()", 1000 }, - { AZ_DRONE_STATE_TELEMETRY_MODULE, &AzDroneController::stateActionModule, "stateActionModule()", 1000 }, - { AZ_DRONE_STATE_ACTION_MODULE, &AzDroneController::stateReadyForArming, "stateReadyForArming()", 1000 }, - { AZ_DRONE_STATE_READY_FOR_ARMING, &AzDroneController::stateArm, "stateArm()", 1000 }, - { AZ_DRONE_STATE_ARMED, &AzDroneController::stateTakeoff, "stateTakeoff()", 10000 }, - { AZ_DRONE_STATE_TAKE_OFF, &AzDroneController::stateFlyMission, "stateFlyMission()", 1000 }, - { AZ_DRONE_STATE_FLY_MISSION, nullptr, "No function to call. AzMissionControl running", 0 } - }; + const MethodInfo stateMethods[] + = {{AZ_DRONE_STATE_DISCONNECTED, &AzDroneController::stateConnect, "stateConnect()", 1000}, + {AZ_DRONE_STATE_CONNECTED, &AzDroneController::stateAutopilot, "stateAutopilot()", 1000}, + {AZ_DRONE_STATE_AUTOPILOT, &AzDroneController::stateTelemetryModule, "stateTelemetryModule()", 1000}, + {AZ_DRONE_STATE_TELEMETRY_MODULE, &AzDroneController::stateActionModule, "stateActionModule()", 1000}, + {AZ_DRONE_STATE_ACTION_MODULE, &AzDroneController::stateReadyForArming, "stateReadyForArming()", 1000}, + {AZ_DRONE_STATE_READY_FOR_ARMING, &AzDroneController::stateArm, "stateArm()", 1000}, + {AZ_DRONE_STATE_ARMED, &AzDroneController::stateTakeoff, "stateTakeoff()", 10000}, + {AZ_DRONE_STATE_TAKE_OFF, &AzDroneController::stateFlyMission, "stateFlyMission()", 1000}, + {AZ_DRONE_STATE_FLY_MISSION, nullptr, "No function to call. AzMissionControl running", 0}}; + // Iterate through states. If state function fails, then it's tried again until it succeeds. for (uint i = 0; i < sizeof(stateMethods) / sizeof(MethodInfo); i++) { if (stateMethods[i].currentState == mDroneState) { - if ((int)mDroneState == (int)AZ_DRONE_STATE_FLY_MISSION) { + if ((int) mDroneState == (int) AZ_DRONE_STATE_FLY_MISSION) { qDebug() << "AzMissionContoroller running. Waiting signals from it."; return; } @@ -186,36 +193,35 @@ void AzDroneController::droneStateMachineSlot(void) if (result) { qDebug() << "AzDroneController::droneStateMachineSlot()" << stateMethods[i].description << "succeeded"; - qDebug() << "AzDroneController::droneStateMachineSlot() calling next state function in" << stateMethods[i].nextStateDelay << "ms."; + qDebug() << "AzDroneController::droneStateMachineSlot() calling next state function in" + << stateMethods[i].nextStateDelay << "ms."; mDroneState = stateMethods[i + 1].currentState; - qDebug() << "AzDroneController::droneStateMachineSlot() New state:" << states[(int)mDroneState]; + qDebug() << "AzDroneController::droneStateMachineSlot() New state:" << states[(int) mDroneState]; } else { - qDebug() << "AzDroneController::droneStateMachineSlot() << stateMethods[i].description failed. Trying again."; + qDebug() << "AzDroneController::droneStateMachineSlot() << stateMethods[i].description failed. Trying " + "again."; } - //if (stateMethods[i].nextStateDelay > 0 && stateMethods[i].nextStateMethodPtr != nullptr) { - delayedStateCallSlot(stateMethods[i].nextStateDelay); - //} + delayedStateCallSlot(stateMethods[i].nextStateDelay); break; } } } - void AzDroneController::newPositionSlot(Telemetry::Position position) { // 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. if (mFirstPosition.relative_altitude_m < -1000) { qDebug() << "AzDroneController::newPositionSlot()" - << "First altitude" << mFirstPosition.relative_altitude_m - << "lat:" << position.latitude_deg + << "First altitude" << mFirstPosition.relative_altitude_m << "lat:" << position.latitude_deg << "lon:" << position.longitude_deg; mFirstPosition = position; } + // Send new position to the mission controller. if (mDroneState == AZ_DRONE_STATE_FLY_MISSION) { mMissionController->newPosition(position); } @@ -225,7 +231,6 @@ void AzDroneController::newPositionSlot(Telemetry::Position position) } } - void AzDroneController::missionIndexChangedSlot(int currentIndex, int totalIndexes) { qDebug() << "AzDroneController::missionIndexChanged()" << currentIndex << "/" << totalIndexes; diff --git a/src/az_drone_controller.h b/src/az_drone_controller.h index a68b31b..c94a4f6 100644 --- a/src/az_drone_controller.h +++ b/src/az_drone_controller.h @@ -1,7 +1,6 @@ #pragma once #include - #include #include @@ -13,7 +12,7 @@ using namespace mavsdk; -typedef enum { +typedef enum { AZ_DRONE_STATE_DISCONNECTED, AZ_DRONE_STATE_CONNECTED, AZ_DRONE_STATE_AUTOPILOT, @@ -32,7 +31,11 @@ class AzDroneController : public QObject Q_OBJECT public: explicit AzDroneController(AzMission &mission, QObject *parent = nullptr); + + // Starts the autopilot. void start(); + + // Specify the methods that will be called one at a time to initialise the drone and fly the mission. bool stateConnect(void); bool stateAutopilot(void); bool stateTelemetryModule(void); @@ -42,15 +45,32 @@ public: bool stateTakeoff(void); bool stateFlyMission(void); + // Slots that are called by the emitted Qt signals. private slots: + // Launches a state machine that progressively initialises the drone without blocking. void droneStateMachineSlot(void); + + // ArduPilot seems to require some delay between calls. This method uses QTimer internally + // to call each new state method with some delay without blocking the main thread. void delayedStateCallSlot(int ms); + + // Slot that is called when the newPosition() signal below is emitted. void newPositionSlot(Telemetry::Position); + + // A mission file contains several action points. This is called + // when the action point is reached. Indexing starts at 1. void missionIndexChangedSlot(int currentIndex, int totalIndexes); + + // Called at the end of the mission. Lands and disarms the drone. void missionFinishedSlot(void); + + // Disarms the drone and exits the application. TODO!! Discuss quitting the application. void disarmDroneSlot(void); signals: + // Signal is emitted when Ardupilot sends a new position from the + // MAVSDK thread. Signal goes through the main event loop and is + // captured in the main thread to avoid threading issues. void newPosition(Telemetry::Position); private: diff --git a/src/az_mission.cpp b/src/az_mission.cpp index 088f7de..6a58137 100644 --- a/src/az_mission.cpp +++ b/src/az_mission.cpp @@ -1,5 +1,3 @@ -#include "az_mission.h" - #include #include #include @@ -8,13 +6,15 @@ #include "../3rd/rapidjson/include/rapidjson/error/en.h" #include "../3rd/rapidjson/include/rapidjson/filereadstream.h" +#include "az_mission.h" + using namespace rapidjson; using namespace std; -AzMission::AzMission(string filename) : - mFilename(filename), - mOperationalHeight(INVALID_HEIGHT), - mReturnPoint(AzCoordinate(INVALID_LATITUDE, INVALID_LONGITUDE)) +AzMission::AzMission(string filename) + : mFilename(filename) + , mOperationalHeight(INVALID_HEIGHT) + , mReturnPoint(AzCoordinate(INVALID_LATITUDE, INVALID_LONGITUDE)) { if (filesystem::exists(mFilename) == false) { cerr << "JSON file " << mFilename << " doesn't exists." << endl; diff --git a/src/az_mission_controller.cpp b/src/az_mission_controller.cpp index 2202dfe..cfdf917 100644 --- a/src/az_mission_controller.cpp +++ b/src/az_mission_controller.cpp @@ -1,26 +1,25 @@ #include + #include + #include "az_mission_controller.h" #include "az_mission.h" #include "az_utils.h" -AzMissionController::AzMissionController(AzMission &mission, QObject *parent) : - QObject(parent), - mMission(mission), - mAction(nullptr), - mMissionStarted(false), - mAbsoluteAltitude(-10000) -{ -} - +AzMissionController::AzMissionController(AzMission &mission, QObject *parent) + : QObject(parent) + , mMission(mission) + , mAction(nullptr) + , mMissionStarted(false) + , mAbsoluteAltitude(-10000) +{} void AzMissionController::setAction(Action *action) { mAction = action; } - bool AzMissionController::startMissions(float absoluteAltitude) { mCurrentMissionIndex = -1; @@ -29,36 +28,35 @@ bool AzMissionController::startMissions(float absoluteAltitude) return flyToNextMissionItem(); } - 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()) { - qDebug() << "AzMissionController::flyToNextMissionItem() All mission items are handled. Stopping mission handling."; + if (mCurrentMissionIndex >= (int) mMission.getActionPoints().size()) { + qDebug() << "AzMissionController::flyToNextMissionItem() All mission action points are handled."; mMissionStarted = false; emit finished(); return true; } - qDebug() << "AzMissionController::flyToNextMissionItem() flying to the item" - << mCurrentMissionIndex + 1 << "/" << mMission.getActionPoints().size(); + 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; + qDebug() << "AzMissionController::flyToNextMissionItem() navigating to point" << coordinate.latitude + << coordinate.longitude; qDebug() << "AzMissionController::flyToNextMissionItem() MAVSDK::Action::goto_location() starts"; Action::Result result = mAction->goto_location(coordinate.latitude, coordinate.longitude, mAbsoluteAltitude, 0.0f); qDebug() << "AzMissionController::flyToNextMissionItem() MAVSDK::Action::goto_location() ends"; + // TODO!! Check return value and print warnings and errors. if (result == Action::Result::Success) { emit missionProgressChanged(mCurrentMissionIndex + 1, mMission.getActionPoints().size()); return true; @@ -67,25 +65,19 @@ bool AzMissionController::flyToNextMissionItem(void) return false; } - -void AzMissionController::newPosition(Telemetry::Position position) +void AzMissionController::newPosition(Telemetry::Position pos) { - (void)position; - if (mMissionStarted == false) { return; } - const AzCoordinate &coordinate = mMission.getActionPoints().at(mCurrentMissionIndex).getPoint(); - float distanceToTarget = AzUtils::distance(position.latitude_deg, - position.longitude_deg, - coordinate.latitude, - coordinate.longitude); + 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() distanceToTarget: " << distanceToTarget; + qDebug() << "AzMissionController::newPosition() distance to target: " << distance; - // TODO!! In final application we need to - if (distanceToTarget <= 0.01) { + // TODO!! In final application we need to use the camera to find the target. + if (distance <= 0.01) { qDebug() << "AzMissionController::newPosition() target reached. Continuing to the next item."; flyToNextMissionItem(); } diff --git a/src/az_mission_controller.h b/src/az_mission_controller.h index 2435062..424a062 100644 --- a/src/az_mission_controller.h +++ b/src/az_mission_controller.h @@ -1,7 +1,9 @@ #pragma once #include + #include + #include "az_mission.h" #include "plugins/action/action.h" diff --git a/src/az_utils.cpp b/src/az_utils.cpp index ecfc2ab..480f16b 100644 --- a/src/az_utils.cpp +++ b/src/az_utils.cpp @@ -5,14 +5,17 @@ AzUtils::AzUtils() {} #define EARTH_RADIUS 6371.0 // Earth radius in kilometers -double degrees_to_radians(double degrees) { +double degrees_to_radians(double degrees) +{ return degrees * M_PI / 180.0; } -double AzUtils::distance(double lat1, double lon1, double lat2, double lon2) { +double AzUtils::distance(double lat1, double lon1, double lat2, double lon2) +{ double dlat = degrees_to_radians(lat2 - lat1); double dlon = degrees_to_radians(lon2 - lon1); - double a = sin(dlat/2) * sin(dlat/2) + cos(degrees_to_radians(lat1)) * cos(degrees_to_radians(lat2)) * sin(dlon/2) * sin(dlon/2); - double c = 2 * atan2(sqrt(a), sqrt(1-a)); + double a = sin(dlat / 2) * sin(dlat / 2) + + cos(degrees_to_radians(lat1)) * cos(degrees_to_radians(lat2)) * sin(dlon / 2) * sin(dlon / 2); + double c = 2 * atan2(sqrt(a), sqrt(1 - a)); return EARTH_RADIUS * c; } diff --git a/src/main.cpp b/src/main.cpp index 4534228..74f70ee 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,20 +1,24 @@ #include #include -#include "az_mission.h" + #include "az_drone_controller.h" +#include "az_mission.h" int main(int argc, char *argv[]) { + // This is needed to have main event loop and signal-slot events in the AzDroneController. QCoreApplication a(argc, argv); if (argc != 2) { - qCritical() << "Please give mission file as an argument\n"; + qCritical() << "Please give mission JSON file as an argument.\n"; return 1; } + // Reads mission from the JSON file which is given as command line option. AzMission mission(argv[1]); cout << mission; + // Launch a drone controller using the MAVSDK for ArduPilot-based flight controllers. AzDroneController droneController(mission); droneController.start();