azaion/autopilot
1
0
Fork 0
mirror of https://github.com/azaion/autopilot.git synced 2026-04-22 16:56:34 +00:00
Code Issues Packages Projects Releases Wiki Activity

Improve comments in Autopilot

Browse Source 
Issue: https://denyspopov.atlassian.net/browse/AZ-14
Type: Improvement
This commit is contained in:
Tuomas Järvinen
2024-05-19 19:49:39 +02:00
parent f0c8a1334a
commit b0b17d7fcc
8 changed files with 131 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/az_mission_controller.cpp
+22 -30
View File
@@ -1,26 +1,25 @@
#include <QDebug>
#include <mavsdk/plugins/telemetry/telemetry.h>
#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();
}