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

Improve use of MAVSDK::Telemetry

Browse Source 
Print health information if Telemetry::health_all_ok() fails.

Type: Improvement
Issue: https://denyspopov.atlassian.net/browse/AZ-15
This commit is contained in:
Tuomas Järvinen
2024-05-19 22:06:03 +02:00
parent b0b17d7fcc
commit 09f974c1c9
2 changed files with 38 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/az_drone_controller.cpp
+31 -7
View File
@@ -20,16 +20,18 @@ AzDroneController::AzDroneController(AzMission &mission, QObject *parent)
mMissionController = new AzMissionController(mission, this);
// Connects signals about the mission progress from the AzMissionController to the slot
// in this class. missionIndexChangedSlot() will be used later to find the targets.
// Mission progress from the AzMissionController. Slot will be used to find the targets later.
connect(
mMissionController,
&AzMissionController::missionProgressChanged,
this,
&AzDroneController::missionIndexChangedSlot);
// Connects signal from the mission controller to the slot in this class to finish the flying.
// Mission controller signal to the slot in this class to finish the flying.
connect(mMissionController, &AzMissionController::finished, this, &AzDroneController::missionFinishedSlot);
// Healt info update from MAVSDK.
connect(this, &AzDroneController::newHealthInfo, this, &AzDroneController::newHealthInfoSlot, Qt::QueuedConnection);
}
void AzDroneController::start()
@@ -97,8 +99,15 @@ bool AzDroneController::stateActionModule(void)
bool AzDroneController::stateReadyForArming(void)
{
// TODO!! Check all telemetry components and print warnings and errors.
return mTelemetry->health_all_ok();
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)
@@ -199,8 +208,8 @@ void AzDroneController::droneStateMachineSlot(void)
qDebug() << "AzDroneController::droneStateMachineSlot() New state:" << states[(int) mDroneState];
}
else {
qDebug() << "AzDroneController::droneStateMachineSlot() << stateMethods[i].description failed. Trying "
"again.";
qDebug() << "AzDroneController::droneStateMachineSlot()" << stateMethods[i].description
<< "failed. Trying again.";
}
delayedStateCallSlot(stateMethods[i].nextStateDelay);
@@ -235,3 +244,18 @@ void AzDroneController::missionIndexChangedSlot(int currentIndex, int totalIndex
{
qDebug() << "AzDroneController::missionIndexChanged()" << currentIndex << "/" << totalIndexes;
}
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");
// Not optimal, but disconnect signal immeaditely after getting the first update.
disconnect(this, &AzDroneController::newHealthInfo, this, &AzDroneController::newHealthInfoSlot);
}
src/az_drone_controller.h
+7
View File
@@ -67,12 +67,19 @@ private slots:
// Disarms the drone and exits the application. TODO!! Discuss quitting the application.
void disarmDroneSlot(void);
// Gets Telemetry::Health information in prearming.
void newHealthInfoSlot(Telemetry::Health health);
signals:
// Signal is emitted when Ardupilot sends a new position from the
// MAVSDK thread. Signal goes through the main event loop and is
// captured in the main thread to avoid threading issues.
void newPosition(Telemetry::Position);
// If Telemetry::health_all_ok() fails, then autopilot subscripes for the healt updates to
// see exactly what is wrong. This signal is emitted to catch updates in the main thread.
void newHealthInfo(Telemetry::Health);
private:
Mavsdk mMavsdk;
AzDroneState mDroneState;