mirror of
https://github.com/azaion/autopilot.git
synced 2026-04-22 08:06:35 +00:00
Changed directory structure and renamed applications
- autopilot -> drone_controller - rtsp_ai_player -> ai_controller - added top level qmake project file - updated documentation - moved small demo applications from tmp/ to misc/
This commit is contained in:
Tuomas Järvinen
@@ -0,0 +1,291 @@
|
||||
#include <QCoreApplication>
|
||||
#include <QDebug>
|
||||
#include <QMetaProperty>
|
||||
#include <QThread>
|
||||
#include <QTimer>
|
||||
|
||||
#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?
|
||||
mDroneState(AZ_DRONE_STATE_DISCONNECTED)
|
||||
{
|
||||
mFirstPosition.relative_altitude_m = -10000;
|
||||
|
||||
mMissionController = new AzMissionController(mission, this);
|
||||
|
||||
// Mission progress from the AzMissionController. Slot will be used to find the targets later.
|
||||
connect(
|
||||
mMissionController,
|
||||
&AzMissionController::missionProgressChanged,
|
||||
this,
|
||||
&AzDroneController::missionIndexChangedSlot);
|
||||
|
||||
// Mission controller signals end of the missions. This will be used to fly to the return point in JSON.
|
||||
connect(mMissionController, &AzMissionController::finished, this, &AzDroneController::missionFinishedSlot);
|
||||
|
||||
// Healt info update from MAVSDK.
|
||||
connect(this, &AzDroneController::newHealthInfo, this, &AzDroneController::newHealthInfoSlot, Qt::QueuedConnection);
|
||||
}
|
||||
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
// Connects to the flight controller based on AZ_CONNECTION_XXX defines in AzConfig.
|
||||
// Serial port connections is enabled if command line arguments contains "serial"
|
||||
// parameter. Otherwise UDP connection is used.
|
||||
bool AzDroneController::stateConnect(void)
|
||||
{
|
||||
ConnectionResult result;
|
||||
|
||||
if (QCoreApplication::arguments().contains("serial")) {
|
||||
result = mMavsdk.add_any_connection(AZ_CONNECTION_SERIAL);
|
||||
}
|
||||
else {
|
||||
result = mMavsdk.add_any_connection(AZ_CONNECTION_UDP);
|
||||
}
|
||||
|
||||
if (result == ConnectionResult::Success) {
|
||||
return true;
|
||||
}
|
||||
else {
|
||||
cout << "[CONTROLLER] MAVSDK::add_any_connection() failed. Reason: " << result << endl;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool AzDroneController::stateGetSystem(void)
|
||||
{
|
||||
vector<shared_ptr<System>> systems = mMavsdk.systems();
|
||||
|
||||
if (systems.size() == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
optional<shared_ptr<System>> autopilot = mMavsdk.first_autopilot(AZ_GET_AUTOPILOT_TIMEOUT);
|
||||
|
||||
if (autopilot.has_value()) {
|
||||
mSystem = autopilot.value();
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
bool AzDroneController::stateGetTelemetryModule(void)
|
||||
{
|
||||
if ((mTelemetry = new Telemetry(mSystem)) == nullptr) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const auto setRateResult = mTelemetry->set_rate_position(1); // 1 Hz
|
||||
if (setRateResult != Telemetry::Result::Success) {
|
||||
cout << "[CONTROLLER] Setting telemetry rate failed: " << setRateResult << endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
// 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.
|
||||
qRegisterMetaType<Telemetry::Position>("Telemetry::Position");
|
||||
connect(this, &AzDroneController::newPosition, this, &AzDroneController::newPositionSlot, Qt::QueuedConnection);
|
||||
mTelemetry->subscribe_position([this](Telemetry::Position position) { emit newPosition(position); });
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
bool AzDroneController::stateGetActionModule(void)
|
||||
{
|
||||
mAction = new Action(mSystem);
|
||||
|
||||
if (mAction != nullptr) {
|
||||
mMissionController->setAction(mAction);
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
bool AzDroneController::stateHealthOk(void)
|
||||
{
|
||||
bool result = mTelemetry->health_all_ok();
|
||||
|
||||
if (result == false) {
|
||||
mTelemetry->subscribe_health([this](Telemetry::Health health) { emit newHealthInfo(health); });
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
bool AzDroneController::stateArm(void)
|
||||
{
|
||||
Action::Result result = mAction->arm();
|
||||
|
||||
if (result == Action::Result::Success) {
|
||||
return true;
|
||||
}
|
||||
else {
|
||||
cout << "[CONTROLLER] MAVSDK::Action::arm() failed. Reason: " << result << endl;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool AzDroneController::stateTakeoff(void)
|
||||
{
|
||||
// Drone never reaches the target altitude with ArduPilot. This seems
|
||||
// to be a bug in MAVSDK/ArduPilot, because this doesn't happen with PX4.
|
||||
// Added extra check to AzDroneController::stateFlyMission() to not start the
|
||||
// mission before 90% of AZ_RELATIVE_FLY_ALTITUDE is reached.
|
||||
|
||||
mAction->set_takeoff_altitude(AZ_RELATIVE_FLY_ALTITUDE);
|
||||
Action::Result result = mAction->takeoff();
|
||||
cout << "[CONTROLLER] MAVSDK::Action::takeoff() failed. Reason: " << result << endl;
|
||||
return result == Action::Result::Success;
|
||||
}
|
||||
|
||||
|
||||
bool AzDroneController::stateFlyMission(void)
|
||||
{
|
||||
if (mCurrentPosition.relative_altitude_m < AZ_RELATIVE_FLY_ALTITUDE * 0.90) {
|
||||
cout << "[CONTROLLER] 90% of the takeoff altitide is not reached yet." << endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
// 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)
|
||||
{
|
||||
// TODO!! Maybe use Telemetry::subscribe_landed_state() later to get the state of the landing.
|
||||
cout << "[CONTROLLER] Mission finished. Landing the drone." << endl;
|
||||
Action::Result result = mAction->land();
|
||||
|
||||
if (result != Action::Result::Success) {
|
||||
cout << "[CONTROLLER] MAVSDK::Action::land() failed. Reason: " << result << endl;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void AzDroneController::droneStateMachineSlot(void)
|
||||
{
|
||||
using MethodPtr = bool (AzDroneController::*)();
|
||||
|
||||
struct MethodInfo
|
||||
{
|
||||
AzDroneState currentState;
|
||||
MethodPtr nextStateMethodPtr;
|
||||
string description;
|
||||
int nextStateDelay;
|
||||
};
|
||||
|
||||
static const MethodInfo stateMethods[]
|
||||
= {{AZ_DRONE_STATE_DISCONNECTED, &AzDroneController::stateConnect, "Connecting to autopilot", 1000},
|
||||
{AZ_DRONE_STATE_CONNECTED, &AzDroneController::stateGetSystem, "Getting MAVSDK system", 1000},
|
||||
{AZ_DRONE_STATE_GOT_SYSTEM, &AzDroneController::stateGetTelemetryModule, "Getting telemetry module", 1000},
|
||||
{AZ_DRONE_STATE_GOT_TELEMETRY_MODULE, &AzDroneController::stateGetActionModule, "Getting action module", 1000},
|
||||
{AZ_DRONE_STATE_GOT_ACTION_MODULE, &AzDroneController::stateHealthOk, "Drone health OK", 1000},
|
||||
{AZ_DRONE_STATE_HEALTH_OK, &AzDroneController::stateArm, "Arming the drone", 1000},
|
||||
{AZ_DRONE_STATE_ARMED, &AzDroneController::stateTakeoff, "Takeoff the drone", 10000},
|
||||
{AZ_DRONE_STATE_TAKE_OFF, &AzDroneController::stateFlyMission, "Starting the mission", 1000},
|
||||
{AZ_DRONE_STATE_FLY_MISSION, nullptr, "Mission started", 0}};
|
||||
|
||||
// Iterate through states. If the state function fails, it's repeated until it succeeds.
|
||||
for (uint i = 0; i < sizeof(stateMethods) / sizeof(MethodInfo); i++) {
|
||||
if (stateMethods[i].currentState == mDroneState) {
|
||||
if (mDroneState == AZ_DRONE_STATE_FLY_MISSION) {
|
||||
cout << "[CONTROLLER] Waiting signals from the mission controller." << endl;
|
||||
return;
|
||||
}
|
||||
|
||||
bool result = (this->*stateMethods[i].nextStateMethodPtr)();
|
||||
|
||||
if (result) {
|
||||
cout << "[CONTROLLER] " << stateMethods[i].description << " succeeded." << endl;
|
||||
mDroneState = stateMethods[i + 1].currentState;
|
||||
}
|
||||
else {
|
||||
cout << "[CONTROLLER] " << stateMethods[i].description << " failed. Trying again." << endl;
|
||||
}
|
||||
|
||||
delayedStateCallSlot(stateMethods[i].nextStateDelay);
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void AzDroneController::newPositionSlot(Telemetry::Position position)
|
||||
{
|
||||
cout << "[CONTROLLER] GPS position " << position.latitude_deg << ", " << position.longitude_deg << endl;
|
||||
|
||||
// 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) {
|
||||
cout << "[CONTROLLER] First altitude " << mFirstPosition.relative_altitude_m << endl;
|
||||
mFirstPosition = position;
|
||||
}
|
||||
|
||||
mCurrentPosition = position;
|
||||
|
||||
// Send new position to the mission controller.
|
||||
if (mDroneState == AZ_DRONE_STATE_FLY_MISSION) {
|
||||
mMissionController->newPosition(position);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void AzDroneController::missionIndexChangedSlot(int currentIndex, int totalIndexes)
|
||||
{
|
||||
cout << "[CONTROLLER] missionIndexChanged() " << currentIndex << "/" << totalIndexes << endl;
|
||||
}
|
||||
|
||||
|
||||
void AzDroneController::newHealthInfoSlot(Telemetry::Health health)
|
||||
{
|
||||
cout << "[CONTROLLER] newHealthInfo:" << endl;
|
||||
cout << "[CONTROLLER] Gyro calibration: " << (health.is_gyrometer_calibration_ok ? "OK" : "FAILED") << endl;
|
||||
cout << "[CONTROLLER] Accel calibration: " << (health.is_accelerometer_calibration_ok ? "OK" : "FAILED") << endl;
|
||||
cout << "[CONTROLLER] Mag calibration: " << (health.is_magnetometer_calibration_ok ? "OK" : "FAILED") << endl;
|
||||
cout << "[CONTROLLER] Local position: " << (health.is_local_position_ok ? "OK" : "FAILED") << endl;
|
||||
cout << "[CONTROLLER] Global position: " << (health.is_global_position_ok ? "OK" : "FAILED") << endl;
|
||||
cout << "[CONTROLLER] Home position: " << (health.is_home_position_ok ? "OK" : "FAILED") << endl;
|
||||
|
||||
// Stop printing healt information if everything in healt is OK.
|
||||
if (health.is_gyrometer_calibration_ok &&
|
||||
health.is_accelerometer_calibration_ok &&
|
||||
health.is_magnetometer_calibration_ok &&
|
||||
health.is_local_position_ok &&
|
||||
health.is_global_position_ok &&
|
||||
health.is_home_position_ok) {
|
||||
|
||||
disconnect(this, &AzDroneController::newHealthInfo, this, &AzDroneController::newHealthInfoSlot);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user