diff --git a/src/autopilot.pro b/src/autopilot.pro index 740fe94..c1d7a4b 100644 --- a/src/autopilot.pro +++ b/src/autopilot.pro @@ -13,6 +13,7 @@ SOURCES += \ az_action_point.cpp \ az_coordinate.cpp \ az_drone_controller.cpp \ + az_drone_controller_plane.cpp \ az_mission.cpp \ az_mission_controller.cpp \ az_utils.cpp \ @@ -26,6 +27,7 @@ HEADERS += \ az_config.h \ az_coordinate.h \ az_drone_controller.h \ + az_drone_controller_plane.h \ az_mission.h \ az_mission_controller.h \ az_utils.h diff --git a/src/az_drone_controller.cpp b/src/az_drone_controller.cpp index d651b18..619fdda 100644 --- a/src/az_drone_controller.cpp +++ b/src/az_drone_controller.cpp @@ -35,12 +35,14 @@ AzDroneController::AzDroneController(AzMission &mission, QObject *parent) 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. @@ -48,6 +50,7 @@ void AzDroneController::delayedStateCallSlot(int ms) 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. @@ -71,6 +74,7 @@ bool AzDroneController::stateConnect(void) } } + bool AzDroneController::stateAutopilot(void) { std::vector> systems = mMavsdk.systems(); @@ -93,6 +97,7 @@ bool AzDroneController::stateAutopilot(void) } } + bool AzDroneController::stateTelemetryModule(void) { if ((mTelemetry = new Telemetry(mSystem)) == nullptr) { @@ -119,6 +124,7 @@ bool AzDroneController::stateTelemetryModule(void) return true; } + bool AzDroneController::stateActionModule(void) { mAction = new Action(mSystem); @@ -133,6 +139,7 @@ bool AzDroneController::stateActionModule(void) } } + bool AzDroneController::stateReadyForArming(void) { bool result = mTelemetry->health_all_ok(); @@ -144,6 +151,7 @@ bool AzDroneController::stateReadyForArming(void) return result; } + bool AzDroneController::stateArm(void) { Action::Result result = mAction->arm(); @@ -157,6 +165,7 @@ bool AzDroneController::stateArm(void) } } + bool AzDroneController::stateTakeoff(void) { // Drone never reaches the target altitude with ArduPilot. This seems @@ -170,6 +179,7 @@ bool AzDroneController::stateTakeoff(void) return result == Action::Result::Success; } + bool AzDroneController::stateFlyMission(void) { if (QCoreApplication::arguments().contains("quadcopter") && mCurrentPosition.relative_altitude_m < AZ_RELATIVE_FLY_ALTITUDE * 0.90) { @@ -182,6 +192,7 @@ bool AzDroneController::stateFlyMission(void) 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. @@ -193,6 +204,7 @@ void AzDroneController::missionFinishedSlot(void) } } + void AzDroneController::droneStateMachineSlot(void) { static const QString states[] = { @@ -208,8 +220,7 @@ void AzDroneController::droneStateMachineSlot(void) "LAND", }; - qDebug() << ""; - qDebug() << "AzDroneController::droneStateMachineSlot() Current state:" << states[(int) mDroneState]; + qDebug() << "\nAzDroneController::droneStateMachineSlot() Current state:" << states[(int) mDroneState]; using MethodPtr = bool (AzDroneController::*)(); @@ -244,17 +255,7 @@ void AzDroneController::droneStateMachineSlot(void) if (result) { qDebug() << "AzDroneController::droneStateMachineSlot()" << stateMethods[i].description << "succeeded"; - mDroneState = stateMethods[i + 1].currentState; - - // Quadcopter needs to iterate all states - plane will skip arming and takeoff - if (mDroneState == AZ_DRONE_STATE_READY_FOR_ARMING && QCoreApplication::arguments().contains("plane")) { - mDroneState = AZ_DRONE_STATE_TAKE_OFF; - qDebug() << "AzDroneController::droneStateMachineSlot() Skipping arming and takeoff for planes."; - } - else { - qDebug() << "AzDroneController::droneStateMachineSlot() New state:" << states[(int) mDroneState]; - } } else { qDebug() << "AzDroneController::droneStateMachineSlot()" << stateMethods[i].description @@ -268,6 +269,7 @@ void AzDroneController::droneStateMachineSlot(void) } } + void AzDroneController::newPositionSlot(Telemetry::Position position) { qDebug() << "AzDroneController::newPositionSlot()" << position.latitude_deg << position.longitude_deg; @@ -293,11 +295,13 @@ void AzDroneController::newPositionSlot(Telemetry::Position position) } } + void AzDroneController::missionIndexChangedSlot(int currentIndex, int totalIndexes) { qDebug() << "AzDroneController::missionIndexChanged()" << currentIndex << "/" << totalIndexes; } + void AzDroneController::newHealthInfoSlot(Telemetry::Health health) { qDebug() << "AzDroneController::newHealthInfoSlot()"; @@ -308,6 +312,14 @@ void AzDroneController::newHealthInfoSlot(Telemetry::Health health) qDebug() << " Global position: " << (health.is_global_position_ok ? "ok" : "not ok"); qDebug() << " Home position: " << (health.is_home_position_ok ? "ok" : "not ok"); - // Not optimal, but disconnect signal immeaditely after getting the first update. - // disconnect(this, &AzDroneController::newHealthInfo, this, &AzDroneController::newHealthInfoSlot); + // 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); + } } diff --git a/src/az_drone_controller.h b/src/az_drone_controller.h index 4d7af44..804660b 100644 --- a/src/az_drone_controller.h +++ b/src/az_drone_controller.h @@ -21,6 +21,7 @@ typedef enum { AZ_DRONE_STATE_READY_FOR_ARMING, AZ_DRONE_STATE_ARMED, AZ_DRONE_STATE_TAKE_OFF, + AZ_DRONE_STATE_WAITING_HOLD_MODE, AZ_DRONE_STATE_FLY_MISSION, AZ_DRONE_STATE_LAND, AZ_DRONE_STATE_END, @@ -35,6 +36,7 @@ public: // Starts the autopilot. void start(); +protected: // 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); @@ -46,9 +48,9 @@ public: bool stateFlyMission(void); // Slots that are called by the emitted Qt signals. -private slots: +protected slots: // Launches a state machine that progressively initialises the drone without blocking. - void droneStateMachineSlot(void); + virtual 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. @@ -77,7 +79,7 @@ signals: // see exactly what is wrong. This signal is emitted to catch updates in the main thread. void newHealthInfo(Telemetry::Health); -private: +protected: Mavsdk mMavsdk; AzDroneState mDroneState; std::shared_ptr mSystem; diff --git a/src/az_drone_controller_plane.cpp b/src/az_drone_controller_plane.cpp new file mode 100644 index 0000000..86ea814 --- /dev/null +++ b/src/az_drone_controller_plane.cpp @@ -0,0 +1,78 @@ +#include +#include +#include "az_drone_controller_plane.h" + +AzDroneControllerPlane::AzDroneControllerPlane(AzMission &mission, QObject *parent) : + AzDroneController(mission, parent) +{} + + +bool AzDroneControllerPlane::stateWaitingMissionMode(void) +{ + Telemetry::FlightMode flight_mode = mTelemetry->flight_mode(); + std::cout << "azDroneControllerPlane::stateWaitingHoldMode() Current mode: " << flight_mode << std::endl; + return flight_mode == Telemetry::FlightMode::Mission; +} + + +void AzDroneControllerPlane::droneStateMachineSlot(void) +{ + static const QString states[] = { + "DISCONNECTED", + "CONNECTED", + "AUTOPILOT", + "TELEMETRY_MODULE", + "ACTION_MODULE", + "READY_FOR_ARMING", + "AZ_DRONE_STATE_WAITING_MISSION_MODE", + "FLY_MISSION" + }; + + qDebug() << "\nAzDroneControllerPlane::droneStateMachineSlot() Current state:" << states[(int) mDroneState]; + + using MethodPtr = bool (AzDroneControllerPlane::*)(); + + struct MethodInfo + { + AzDroneState currentState; + MethodPtr nextStateMethodPtr; + QString description; + int nextStateDelay; + }; + + // Plane type will pass arming and taking off states and wait for auto mode from the RC controller instead. + static const MethodInfo stateMethods[] + = {{AZ_DRONE_STATE_DISCONNECTED, &AzDroneControllerPlane::stateConnect, "stateConnect()", 1000}, + {AZ_DRONE_STATE_CONNECTED, &AzDroneControllerPlane::stateAutopilot, "stateAutopilot()", 1000}, + {AZ_DRONE_STATE_AUTOPILOT, &AzDroneControllerPlane::stateTelemetryModule, "stateTelemetryModule()", 1000}, + {AZ_DRONE_STATE_TELEMETRY_MODULE, &AzDroneControllerPlane::stateActionModule, "stateActionModule()", 1000}, + {AZ_DRONE_STATE_ACTION_MODULE, &AzDroneControllerPlane::stateReadyForArming, "stateReadyForArming()", 1000}, + {AZ_DRONE_STATE_ARMED, &AzDroneControllerPlane::stateWaitingMissionMode, "stateWaitingMissionMode()", 1000}, + {AZ_DRONE_STATE_WAITING_HOLD_MODE, &AzDroneControllerPlane::stateFlyMission, "stateFlyMission()", 1000}, + {AZ_DRONE_STATE_FLY_MISSION, nullptr, "No function to call. AzMissionControl running", 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) { + qDebug() << "AzMissionContoroller running. Waiting signals from it."; + return; + } + + bool result = (this->*stateMethods[i].nextStateMethodPtr)(); + + if (result) { + qDebug() << "AzDroneControllerPlane::droneStateMachineSlot()" << stateMethods[i].description << "succeeded"; + mDroneState = stateMethods[i + 1].currentState; + } + else { + qDebug() << "AzDroneControllerPlane::droneStateMachineSlot()" << stateMethods[i].description + << "failed. Trying again."; + } + + delayedStateCallSlot(stateMethods[i].nextStateDelay); + + break; + } + } +} diff --git a/src/az_drone_controller_plane.h b/src/az_drone_controller_plane.h new file mode 100644 index 0000000..fdeadce --- /dev/null +++ b/src/az_drone_controller_plane.h @@ -0,0 +1,15 @@ +#pragma once + +#include "az_drone_controller.h" +#include + +class AzDroneControllerPlane : public AzDroneController +{ + Q_OBJECT +public: + explicit AzDroneControllerPlane(AzMission &mission, QObject *parent = nullptr); + +protected: + void droneStateMachineSlot(void) override; + bool stateWaitingMissionMode(void); +}; diff --git a/src/main.cpp b/src/main.cpp index e191bb6..3f8775a 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -3,6 +3,7 @@ #include #include "az_drone_controller.h" +#include "az_drone_controller_plane.h" #include "az_mission.h" int main(int argc, char *argv[]) @@ -39,8 +40,9 @@ int main(int argc, char *argv[]) cout << mission; // Launch a drone controller using the MAVSDK for ArduPilot-based flight controllers. - AzDroneController droneController(mission); - droneController.start(); + bool isPlane = a.arguments().at(2) == "plane"; + AzDroneController *controller = isPlane ? new AzDroneControllerPlane(mission) : new AzDroneController(mission); + controller->start(); return a.exec(); }