mirror of
https://github.com/azaion/autopilot.git
synced 2026-04-22 12:06:34 +00:00
Initial version of ArduPilot compatible autopilot
- removed PX4 and MAVSDK git submodules
This commit is contained in:
Tuomas Järvinen
@@ -0,0 +1,232 @@
|
||||
#include <mavsdk/plugins/action/action.h>
|
||||
#include <mavsdk/plugins/telemetry/telemetry.h>
|
||||
#include <QCoreApplication>
|
||||
#include <QDebug>
|
||||
#include <QThread>
|
||||
#include <QTimer>
|
||||
#include <unistd.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?
|
||||
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);
|
||||
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)
|
||||
{
|
||||
QTimer::singleShot(ms, this, &AzDroneController::droneStateMachineSlot);
|
||||
}
|
||||
|
||||
|
||||
bool AzDroneController::stateConnect(void)
|
||||
{
|
||||
return mMavsdk.add_any_connection(AZ_CONNECTION) == ConnectionResult::Success;
|
||||
}
|
||||
|
||||
|
||||
bool AzDroneController::stateAutopilot(void)
|
||||
{
|
||||
return (mSystem = mMavsdk.first_autopilot(3.0).value()) != nullptr;
|
||||
}
|
||||
|
||||
|
||||
bool AzDroneController::stateTelemetryModule(void)
|
||||
{
|
||||
if ((mTelemetry = new Telemetry(mSystem)) == nullptr) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const auto setRateResult = mTelemetry->set_rate_position(1); // 1 Hz
|
||||
if (setRateResult != Telemetry::Result::Success) {
|
||||
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.
|
||||
connect(this, &AzDroneController::newPosition, this, &AzDroneController::newPositionSlot, Qt::QueuedConnection);
|
||||
mTelemetry->subscribe_position([this](Telemetry::Position position) {
|
||||
emit newPosition(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;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
bool AzDroneController::stateActionModule(void)
|
||||
{
|
||||
mAction = new Action(mSystem);
|
||||
if (mAction == nullptr) {
|
||||
return false;
|
||||
}
|
||||
|
||||
mMissionController->setAction(mAction);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
bool AzDroneController::stateReadyForArming(void)
|
||||
{
|
||||
return mTelemetry->health_all_ok();
|
||||
}
|
||||
|
||||
|
||||
bool AzDroneController::stateArm(void)
|
||||
{
|
||||
return mAction->arm() == Action::Result::Success;
|
||||
}
|
||||
|
||||
|
||||
bool AzDroneController::stateTakeoff(void)
|
||||
{
|
||||
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";
|
||||
|
||||
return result == Action::Result::Success;
|
||||
}
|
||||
|
||||
|
||||
bool AzDroneController::stateFlyMission(void)
|
||||
{
|
||||
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.";
|
||||
mAction->land();
|
||||
// TODO!! This must be improved a lot.
|
||||
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.
|
||||
mAction->disarm();
|
||||
QCoreApplication::instance()->quit();
|
||||
}
|
||||
|
||||
|
||||
void AzDroneController::droneStateMachineSlot(void)
|
||||
{
|
||||
static QString states[] = {
|
||||
"DISCONNECTED",
|
||||
"CONNECTED",
|
||||
"AUTOPILOT",
|
||||
"TELEMETRY_MODULE",
|
||||
"ACTION_MODULE",
|
||||
"READY_FOR_ARMING",
|
||||
"ARMED",
|
||||
"TAKE_OFF",
|
||||
"FLY_MISSION",
|
||||
"LAND",
|
||||
};
|
||||
|
||||
qDebug() << "";
|
||||
qDebug() << "AzDroneController::droneStateMachineSlot() Current state:" << states[(int)mDroneState];
|
||||
|
||||
using MethodPtr = bool (AzDroneController::*)();
|
||||
|
||||
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 }
|
||||
};
|
||||
|
||||
for (uint i = 0; i < sizeof(stateMethods) / sizeof(MethodInfo); i++) {
|
||||
if (stateMethods[i].currentState == mDroneState) {
|
||||
if ((int)mDroneState == (int)AZ_DRONE_STATE_FLY_MISSION) {
|
||||
qDebug() << "AzMissionContoroller running. Waiting signals from it.";
|
||||
return;
|
||||
}
|
||||
|
||||
bool result = (this->*stateMethods[i].nextStateMethodPtr)();
|
||||
|
||||
if (result) {
|
||||
qDebug() << "AzDroneController::droneStateMachineSlot()" << stateMethods[i].description << "succeeded";
|
||||
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];
|
||||
}
|
||||
else {
|
||||
qDebug() << "AzDroneController::droneStateMachineSlot() << stateMethods[i].description failed. Trying again.";
|
||||
}
|
||||
|
||||
//if (stateMethods[i].nextStateDelay > 0 && stateMethods[i].nextStateMethodPtr != nullptr) {
|
||||
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
|
||||
<< "lon:" << position.longitude_deg;
|
||||
mFirstPosition = position;
|
||||
}
|
||||
|
||||
if (mDroneState == AZ_DRONE_STATE_FLY_MISSION) {
|
||||
mMissionController->newPosition(position);
|
||||
}
|
||||
else {
|
||||
qDebug() << "AzDroneController::newPositionSlot() altitude rel:" << position.relative_altitude_m
|
||||
<< "altitude abs:" << position.absolute_altitude_m;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void AzDroneController::missionIndexChangedSlot(int currentIndex, int totalIndexes)
|
||||
{
|
||||
qDebug() << "AzDroneController::missionIndexChanged()" << currentIndex << "/" << totalIndexes;
|
||||
}
|
||||
Reference in New Issue
Block a user