azaion/autopilot
1
0
Fork 0
mirror of https://github.com/azaion/autopilot.git synced 2026-04-22 12:06: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_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;