diff --git a/src/az_drone_controller.cpp b/src/az_drone_controller.cpp index 619fdda..e455433 100644 --- a/src/az_drone_controller.cpp +++ b/src/az_drone_controller.cpp @@ -16,7 +16,6 @@ AzDroneController::AzDroneController(AzMission &mission, QObject *parent) , // 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); @@ -69,36 +68,32 @@ bool AzDroneController::stateConnect(void) return true; } else { - std::cerr << "MAVSDK::add_any_connection() failed. Reason: " << result << endl; + cout << "[CONTROLLER] MAVSDK::add_any_connection() failed. Reason: " << result << endl; return false; } } -bool AzDroneController::stateAutopilot(void) +bool AzDroneController::stateGetSystem(void) { - std::vector> systems = mMavsdk.systems(); - qDebug() << "AzDroneController::stateAutopilot() Found" << systems.size() << "systems"; + vector> systems = mMavsdk.systems(); if (systems.size() == 0) { - std::cerr << "No system found." << std::endl; return false; } - std::optional> autopilot = mMavsdk.first_autopilot(AZ_GET_AUTOPILOT_TIMEOUT); + optional> autopilot = mMavsdk.first_autopilot(AZ_GET_AUTOPILOT_TIMEOUT); if (autopilot.has_value()) { mSystem = autopilot.value(); return true; } - else { - qCritical() << "MAVSDK::first_autopilot() failed"; - return false; - } + + return false; } -bool AzDroneController::stateTelemetryModule(void) +bool AzDroneController::stateGetTelemetryModule(void) { if ((mTelemetry = new Telemetry(mSystem)) == nullptr) { return false; @@ -106,7 +101,7 @@ 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; + cout << "[CONTROLLER] Setting telemetry rate failed: " << setRateResult << endl; return false; } @@ -116,16 +111,11 @@ bool AzDroneController::stateTelemetryModule(void) connect(this, &AzDroneController::newPosition, this, &AzDroneController::newPositionSlot, Qt::QueuedConnection); 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; - return true; } -bool AzDroneController::stateActionModule(void) +bool AzDroneController::stateGetActionModule(void) { mAction = new Action(mSystem); @@ -133,14 +123,12 @@ bool AzDroneController::stateActionModule(void) mMissionController->setAction(mAction); return true; } - else { - qWarning() << "Creating new MAVSDK::Action failed"; - return false; - } + + return false; } -bool AzDroneController::stateReadyForArming(void) +bool AzDroneController::stateHealthOk(void) { bool result = mTelemetry->health_all_ok(); @@ -160,7 +148,7 @@ bool AzDroneController::stateArm(void) return true; } else { - std::cerr << "MAVSDK::Action::arm() failed. Reason: " << result << endl; + cout << "[CONTROLLER] MAVSDK::Action::arm() failed. Reason: " << result << endl; return false; } } @@ -175,15 +163,15 @@ bool AzDroneController::stateTakeoff(void) mAction->set_takeoff_altitude(AZ_RELATIVE_FLY_ALTITUDE); Action::Result result = mAction->takeoff(); - std::cerr << "MAVSDK::Action::takeoff() failed. Reason: " << result << endl; + cout << "[CONTROLLER] MAVSDK::Action::takeoff() failed. Reason: " << result << endl; return result == Action::Result::Success; } bool AzDroneController::stateFlyMission(void) { - if (QCoreApplication::arguments().contains("quadcopter") && mCurrentPosition.relative_altitude_m < AZ_RELATIVE_FLY_ALTITUDE * 0.90) { - qDebug() << "AzDroneController::stateFlyMission() 90% of set altitide is not reached yet."; + 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; } @@ -196,70 +184,54 @@ bool AzDroneController::stateFlyMission(void) void AzDroneController::missionFinishedSlot(void) { // TODO!! Maybe use Telemetry::subscribe_landed_state() later to get the state of the landing. - qDebug() << "AzDroneController::missionFinishedSlot() Mission finished. Landing the drone."; + cout << "[CONTROLLER] Mission finished. Landing the drone." << endl; Action::Result result = mAction->land(); if (result != Action::Result::Success) { - std::cerr << "mAction->land() failed. Reason: " << result << endl; + cout << "[CONTROLLER] MAVSDK::Action::land() failed. Reason: " << result << endl; } } void AzDroneController::droneStateMachineSlot(void) { - static const QString states[] = { - "DISCONNECTED", - "CONNECTED", - "AUTOPILOT", - "TELEMETRY_MODULE", - "ACTION_MODULE", - "READY_FOR_ARMING", - "ARMED", - "TAKE_OFF", - "FLY_MISSION", - "LAND", - }; - - qDebug() << "\nAzDroneController::droneStateMachineSlot() Current state:" << states[(int) mDroneState]; - using MethodPtr = bool (AzDroneController::*)(); struct MethodInfo { AzDroneState currentState; MethodPtr nextStateMethodPtr; - QString description; + string description; int nextStateDelay; }; 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}, - {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}}; + = {{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) { - qDebug() << "AzMissionContoroller running. Waiting signals from it."; + cout << "[CONTROLLER] Waiting signals from the mission controller." << endl; return; } bool result = (this->*stateMethods[i].nextStateMethodPtr)(); if (result) { - qDebug() << "AzDroneController::droneStateMachineSlot()" << stateMethods[i].description << "succeeded"; + cout << "[CONTROLLER] " << stateMethods[i].description << " succeeded." << endl; mDroneState = stateMethods[i + 1].currentState; } else { - qDebug() << "AzDroneController::droneStateMachineSlot()" << stateMethods[i].description - << "failed. Trying again."; + cout << "[CONTROLLER] " << stateMethods[i].description << " failed. Trying again." << endl; } delayedStateCallSlot(stateMethods[i].nextStateDelay); @@ -272,14 +244,12 @@ void AzDroneController::droneStateMachineSlot(void) void AzDroneController::newPositionSlot(Telemetry::Position position) { - qDebug() << "AzDroneController::newPositionSlot()" << position.latitude_deg << position.longitude_deg; + 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) { - qDebug() << "AzDroneController::newPositionSlot()" - << "First altitude" << mFirstPosition.relative_altitude_m << "lat:" << position.latitude_deg - << "lon:" << position.longitude_deg; + cout << "[CONTROLLER] First altitude " << mFirstPosition.relative_altitude_m << endl; mFirstPosition = position; } @@ -289,28 +259,24 @@ void AzDroneController::newPositionSlot(Telemetry::Position 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; + cout << "[CONTROLLER] missionIndexChanged() " << currentIndex << "/" << totalIndexes << endl; } void AzDroneController::newHealthInfoSlot(Telemetry::Health health) { - qDebug() << "AzDroneController::newHealthInfoSlot()"; - qDebug() << " Gyro calibration: " << (health.is_gyrometer_calibration_ok ? "ok" : "not ok"); - qDebug() << " Accel calibration: " << (health.is_accelerometer_calibration_ok ? "ok" : "not ok"); - qDebug() << " Mag calibration: " << (health.is_magnetometer_calibration_ok ? "ok" : "not ok"); - qDebug() << " Local position: " << (health.is_local_position_ok ? "ok" : "not ok"); - qDebug() << " Global position: " << (health.is_global_position_ok ? "ok" : "not ok"); - qDebug() << " Home position: " << (health.is_home_position_ok ? "ok" : "not ok"); + 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 && diff --git a/src/az_drone_controller.h b/src/az_drone_controller.h index 804660b..edd696c 100644 --- a/src/az_drone_controller.h +++ b/src/az_drone_controller.h @@ -15,13 +15,13 @@ using namespace mavsdk; typedef enum { AZ_DRONE_STATE_DISCONNECTED, AZ_DRONE_STATE_CONNECTED, - AZ_DRONE_STATE_AUTOPILOT, - AZ_DRONE_STATE_TELEMETRY_MODULE, - AZ_DRONE_STATE_ACTION_MODULE, - AZ_DRONE_STATE_READY_FOR_ARMING, + AZ_DRONE_STATE_GOT_SYSTEM, + AZ_DRONE_STATE_GOT_TELEMETRY_MODULE, + AZ_DRONE_STATE_GOT_ACTION_MODULE, + AZ_DRONE_STATE_HEALTH_OK, AZ_DRONE_STATE_ARMED, AZ_DRONE_STATE_TAKE_OFF, - AZ_DRONE_STATE_WAITING_HOLD_MODE, + AZ_DRONE_STATE_AUTO_MODE_ACTIVATED, AZ_DRONE_STATE_FLY_MISSION, AZ_DRONE_STATE_LAND, AZ_DRONE_STATE_END, @@ -39,10 +39,10 @@ public: 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); - bool stateTelemetryModule(void); - bool stateActionModule(void); - bool stateReadyForArming(void); + bool stateGetSystem(void); + bool stateGetTelemetryModule(void); + bool stateGetActionModule(void); + bool stateHealthOk(void); bool stateArm(void); bool stateTakeoff(void); bool stateFlyMission(void); @@ -82,7 +82,7 @@ signals: protected: Mavsdk mMavsdk; AzDroneState mDroneState; - std::shared_ptr mSystem; + shared_ptr mSystem; Telemetry *mTelemetry; Action *mAction; Telemetry::Position mFirstPosition; diff --git a/src/az_drone_controller_plane.cpp b/src/az_drone_controller_plane.cpp index 86ea814..c011fec 100644 --- a/src/az_drone_controller_plane.cpp +++ b/src/az_drone_controller_plane.cpp @@ -7,67 +7,53 @@ AzDroneControllerPlane::AzDroneControllerPlane(AzMission &mission, QObject *pare {} -bool AzDroneControllerPlane::stateWaitingMissionMode(void) +bool AzDroneControllerPlane::stateWaitAutoSwitch(void) { Telemetry::FlightMode flight_mode = mTelemetry->flight_mode(); - std::cout << "azDroneControllerPlane::stateWaitingHoldMode() Current mode: " << flight_mode << std::endl; + cout << "[CONTROLLER] Current flight mode: " << flight_mode << 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; + string 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}}; + = {{AZ_DRONE_STATE_DISCONNECTED, &AzDroneControllerPlane::stateConnect, "Connecting to autopilot", 1000}, + {AZ_DRONE_STATE_CONNECTED, &AzDroneControllerPlane::stateGetSystem, "Getting MAVSDK system", 1000}, + {AZ_DRONE_STATE_GOT_SYSTEM, &AzDroneControllerPlane::stateGetTelemetryModule, "Getting telemetry module", 1000}, + {AZ_DRONE_STATE_GOT_TELEMETRY_MODULE, &AzDroneControllerPlane::stateGetActionModule, "Getting action module", 1000}, + {AZ_DRONE_STATE_GOT_ACTION_MODULE, &AzDroneControllerPlane::stateHealthOk, "Drone health OK", 1000}, + {AZ_DRONE_STATE_HEALTH_OK, &AzDroneControllerPlane::stateWaitAutoSwitch, "User switched to AUTO mode", 1000}, + {AZ_DRONE_STATE_AUTO_MODE_ACTIVATED, &AzDroneControllerPlane::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) { - qDebug() << "AzMissionContoroller running. Waiting signals from it."; + cout << "[CONTROLLER] Waiting signals from the mission controller." << endl; return; } bool result = (this->*stateMethods[i].nextStateMethodPtr)(); if (result) { - qDebug() << "AzDroneControllerPlane::droneStateMachineSlot()" << stateMethods[i].description << "succeeded"; + cout << "[CONTROLLER] " << stateMethods[i].description << " succeeded." << endl; mDroneState = stateMethods[i + 1].currentState; } else { - qDebug() << "AzDroneControllerPlane::droneStateMachineSlot()" << stateMethods[i].description - << "failed. Trying again."; + cout << "[CONTROLLER] " << stateMethods[i].description << " failed. Trying again." << endl; } delayedStateCallSlot(stateMethods[i].nextStateDelay); diff --git a/src/az_drone_controller_plane.h b/src/az_drone_controller_plane.h index fdeadce..55e92b6 100644 --- a/src/az_drone_controller_plane.h +++ b/src/az_drone_controller_plane.h @@ -11,5 +11,5 @@ public: protected: void droneStateMachineSlot(void) override; - bool stateWaitingMissionMode(void); + bool stateWaitAutoSwitch(void); }; diff --git a/src/az_mission.h b/src/az_mission.h index 696616b..b4ae247 100644 --- a/src/az_mission.h +++ b/src/az_mission.h @@ -33,7 +33,7 @@ public: AzCoordinate getReturnPoint(void) const; - friend std::ostream &operator<<(std::ostream &os, const AzMission &obj); + friend ostream &operator<<(ostream &os, const AzMission &obj); private: void parse(void); diff --git a/src/az_mission_controller.cpp b/src/az_mission_controller.cpp index a204193..ecbee77 100644 --- a/src/az_mission_controller.cpp +++ b/src/az_mission_controller.cpp @@ -51,7 +51,7 @@ bool AzMissionController::flyToNextMissionItem(void) return true; } else { - std::cerr << "mAction->goto_location() failed. Reason: " << result << endl; + cerr << "mAction->goto_location() failed. Reason: " << result << endl; return false; } } @@ -71,7 +71,7 @@ bool AzMissionController::flyToNextMissionItem(void) return true; } else { - std::cerr << "mAction->goto_location() failed. Reason: " << result << endl; + cerr << "mAction->goto_location() failed. Reason: " << result << endl; return false; } } diff --git a/src/main.cpp b/src/main.cpp index 3f8775a..694875b 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,3 +1,5 @@ +#include + #include #include #include @@ -8,6 +10,13 @@ int main(int argc, char *argv[]) { + // Add current data and time for easier log handling. + std::cout << "\n#################################################\n"; + std::time_t currentTime = std::time(nullptr); + std::tm* localTime = std::localtime(¤tTime); + std::cout << "Starting autopilot program at " << std::put_time(localTime, "%Y-%m-%d %H:%M:%S") << std::endl; + std::cout << "#################################################\n"; + // This is needed to have main event loop and signal-slot events in the AzDroneController. QCoreApplication a(argc, argv);