From 05722c0e096e192d883a0a101d6ffc041aa10c21 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tuomas=20J=C3=A4rvinen?= Date: Sun, 2 Jun 2024 08:33:57 +0200 Subject: [PATCH] Added initial support for the ArduPlane. Added mandatory command line options. How to use ArduCopter: Launch simulator with command: "./Tools/autotest/sim_vehicle.py --map --console -v ArduCopter" Launch autopilot with command: "./autopilot mission.json quadcopter udp" How to use ArduPlane: Launch simulator with command: "./Tools/autotest/sim_vehicle.py --map --console -v ArduPlane" Wait 30 seconds and give following commands in the same terminal arm throttle mode takeoff Launch autopilot with command: "./autopilot mission.json plane udp" Type: New Feature --- src/az_drone_controller.cpp | 23 ++++++++++++++++------- src/az_mission_controller.cpp | 19 +++++++++++++++---- src/az_mission_controller.h | 2 ++ src/main.cpp | 25 ++++++++++++++++++++++--- 4 files changed, 55 insertions(+), 14 deletions(-) diff --git a/src/az_drone_controller.cpp b/src/az_drone_controller.cpp index 887dc32..f85a167 100644 --- a/src/az_drone_controller.cpp +++ b/src/az_drone_controller.cpp @@ -179,7 +179,7 @@ void AzDroneController::missionFinishedSlot(void) void AzDroneController::droneStateMachineSlot(void) { - static QString states[] = { + static const QString states[] = { "DISCONNECTED", "CONNECTED", "AUTOPILOT", @@ -205,7 +205,7 @@ void AzDroneController::droneStateMachineSlot(void) int nextStateDelay; }; - const MethodInfo stateMethods[] + static 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}, @@ -216,10 +216,10 @@ void AzDroneController::droneStateMachineSlot(void) {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. + // 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 ((int) mDroneState == (int) AZ_DRONE_STATE_FLY_MISSION) { + if (mDroneState == AZ_DRONE_STATE_FLY_MISSION) { qDebug() << "AzMissionContoroller running. Waiting signals from it."; return; } @@ -228,10 +228,17 @@ 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."; + mDroneState = stateMethods[i + 1].currentState; - qDebug() << "AzDroneController::droneStateMachineSlot() New state:" << states[(int) mDroneState]; + + // 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 @@ -247,6 +254,8 @@ void AzDroneController::droneStateMachineSlot(void) void AzDroneController::newPositionSlot(Telemetry::Position position) { + qDebug() << "AzDroneController::newPositionSlot()" << position.latitude_deg << position.longitude_deg; + // 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) { diff --git a/src/az_mission_controller.cpp b/src/az_mission_controller.cpp index 52588e1..0260b3b 100644 --- a/src/az_mission_controller.cpp +++ b/src/az_mission_controller.cpp @@ -1,3 +1,4 @@ +#include #include #include @@ -82,9 +83,8 @@ void AzMissionController::newPosition(Telemetry::Position pos) } if (mFlyingToReturnPoint == true) { - const AzCoordinate &coordinate = mMission.getReturnPoint(); - float distance - = AzUtils::distance(pos.latitude_deg, pos.longitude_deg, coordinate.latitude, coordinate.longitude); + const AzCoordinate &point = mMission.getReturnPoint(); + float distance = AzUtils::distance(pos.latitude_deg, pos.longitude_deg, point.latitude, point.longitude); qDebug() << "AzMissionController::newPosition() distance to return point:" << distance << " km."; @@ -102,9 +102,20 @@ void AzMissionController::newPosition(Telemetry::Position pos) qDebug() << "AzMissionController::newPosition() distance to target:" << distance << "km."; // TODO!! In final application we need to use the camera to find the target. - if (distance <= 0.01) { + if (QCoreApplication::arguments().contains("drone") && distance <= 0.01) { qDebug() << "AzMissionController::newPosition() target reached. Continuing to the next item."; flyToNextMissionItem(); } + else if (QCoreApplication::arguments().contains("plane")) { + if (mPlaneCirclingTime.isValid() == false && distance <= 0.1) { + qDebug() << "AzMissionController::newPosition() target reached. Starting circling."; + mPlaneCirclingTime.restart(); + } + else if (mPlaneCirclingTime.elapsed() > 35000) { + qDebug() << "AzMissionController::newPosition() target reached. Ending circling."; + mPlaneCirclingTime.invalidate(); + flyToNextMissionItem(); + } + } } } diff --git a/src/az_mission_controller.h b/src/az_mission_controller.h index a17617f..764481c 100644 --- a/src/az_mission_controller.h +++ b/src/az_mission_controller.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include @@ -34,4 +35,5 @@ private: bool mMissionStarted; bool mFlyingToReturnPoint; float mAbsoluteAltitude; + QElapsedTimer mPlaneCirclingTime; }; diff --git a/src/main.cpp b/src/main.cpp index c723c3a..e191bb6 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,5 +1,6 @@ #include #include +#include #include "az_drone_controller.h" #include "az_mission.h" @@ -9,9 +10,27 @@ 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() << "\nPass the mission JSON file as the first argument."; - qCritical() << "A serial port connection can be enabled with \"serial\" as the second argument.\n"; + if (a.arguments().size() != 4) { + qCritical() << "\nProgram needs three command line parameters."; + qCritical() << "\tFirst argument: mission JSON file."; + qCritical() << "\tSecond argument: \"quadcopter\" or \"plane\"."; + qCritical() << "\tThird argument: \"udp\" or \"serial\" for connection type."; + qCritical() << "\tFor example ./autopilot mission.json plane udp"; + return 1; + } + + if (QFile::exists(a.arguments().at(1)) == false) { + qCritical() << "\nMission file doesn't exist"; + return 1; + } + + if (a.arguments().at(2) != "plane" && a.arguments().at(2) != "quadcopter") { + qCritical() << "\nPass \"quadcopter\" or \"plane\" for drone type as second argument"; + return 1; + } + + if (a.arguments().at(3) != "udp" && a.arguments().at(3) != "serial") { + qCritical() << "Pass \"udp\" or \"serial\" for connection type as third argument"; return 1; }