azaion/autopilot
1
0
Fork 0
mirror of https://github.com/azaion/autopilot.git synced 2026-04-22 06:56:33 +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_config.h
+2 -2
View File
@@ -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;
src/az_drone_controller.cpp
+64 -59
View File
@@ -1,52 +1,63 @@
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/telemetry/telemetry.h>
#include <QCoreApplication>
#include <QDebug>
#include <QThread>
#include <QTimer>
#include <unistd.h>
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/telemetry/telemetry.h>
#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;
src/az_drone_controller.h
+22 -2
View File
@@ -1,7 +1,6 @@
#pragma once
#include <QObject>
#include <memory.h>
#include <mavsdk/mavsdk.h>
@@ -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:
src/az_mission.cpp
+6 -6
View File
@@ -1,5 +1,3 @@
#include "az_mission.h"
#include <filesystem>
#include <iostream>
#include <string>
@@ -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;
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();
}
src/az_mission_controller.h
+2
View File
@@ -1,7 +1,9 @@
#pragma once
#include <QObject>
#include <mavsdk/plugins/telemetry/telemetry.h>
#include "az_mission.h"
#include "plugins/action/action.h"
src/az_utils.cpp
+7 -4
View File
@@ -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;
}
src/main.cpp
+6 -2
View File
@@ -1,20 +1,24 @@
#include <QCoreApplication>
#include <QDebug>
#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();