azaion/autopilot
1
0
Fork 0
mirror of https://github.com/azaion/autopilot.git synced 2026-04-22 05:26:34 +00:00
Code Issues Packages Projects Releases Wiki Activity

Reduced and improved logging

Browse Source 
- removed unnecessary logging
- print start date and time when application starts
- use std::cout instead of qDebug()
- better logging in DroneController classes
- renamed Controller states for better readability
This commit is contained in:
Tuomas Järvinen
2024-10-18 21:45:20 +02:00
parent 91257d0ce7
commit 54b7dc41ca
7 changed files with 78 additions and 117 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/az_drone_controller.cpp
+41 -75
View File
@@ -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<std::shared_ptr<System>> systems = mMavsdk.systems();
qDebug() << "AzDroneController::stateAutopilot() Found" << systems.size() << "systems";
vector<shared_ptr<System>> systems = mMavsdk.systems();
if (systems.size() == 0) {
std::cerr << "No system found." << std::endl;
return false;
}
std::optional<std::shared_ptr<System>> autopilot = mMavsdk.first_autopilot(AZ_GET_AUTOPILOT_TIMEOUT);
optional<shared_ptr<System>> 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 &&
src/az_drone_controller.h
+10 -10
View File
@@ -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<System> mSystem;
shared_ptr<System> mSystem;
Telemetry *mTelemetry;
Action *mAction;
Telemetry::Position mFirstPosition;
src/az_drone_controller_plane.cpp
+14 -28
View File
@@ -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);
src/az_drone_controller_plane.h
+1 -1
View File
@@ -11,5 +11,5 @@ public:
protected:
void droneStateMachineSlot(void) override;
bool stateWaitingMissionMode(void);
bool stateWaitAutoSwitch(void);
};
src/az_mission.h
+1 -1
View File
@@ -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);
src/az_mission_controller.cpp
+2 -2
View File
@@ -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;
}
}
src/main.cpp
+9
View File
@@ -1,3 +1,5 @@
#include <ctime>
#include <QCoreApplication>
#include <QDebug>
#include <QFile>
@@ -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(&currentTime);
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);