mirror of
https://github.com/azaion/autopilot.git
synced 2026-04-23 06:46:35 +00:00
Added check how many MAVSDK systems are available.
This commit is contained in:
committed by
Tuomas Järvinen
parent
c69a7628c2
commit
ad58ac34b8
@@ -1,5 +1,6 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
// Please check the port name! Also baudrates must match or devices can't communicate with each other.
|
||||||
const char *AZ_CONNECTION_SERIAL = "serial:///dev/ttyS0:115200";
|
const char *AZ_CONNECTION_SERIAL = "serial:///dev/ttyS0:115200";
|
||||||
const char *AZ_CONNECTION_UDP = "udp://:14550";
|
const char *AZ_CONNECTION_UDP = "udp://:14550";
|
||||||
const int AZ_RELATIVE_FLY_ALTITUDE = 50;
|
const int AZ_RELATIVE_FLY_ALTITUDE = 50;
|
||||||
|
|||||||
@@ -73,6 +73,14 @@ bool AzDroneController::stateConnect(void)
|
|||||||
|
|
||||||
bool AzDroneController::stateAutopilot(void)
|
bool AzDroneController::stateAutopilot(void)
|
||||||
{
|
{
|
||||||
|
std::vector<std::shared_ptr<System>> systems = mMavsdk.systems();
|
||||||
|
qDebug() << "AzDroneController::stateAutopilot() Found" << systems.size() << "systems";
|
||||||
|
|
||||||
|
if (systems.size() == 0) {
|
||||||
|
std::cerr << "No system found." << std::endl;
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
|
||||||
std::optional<std::shared_ptr<System>> autopilot = mMavsdk.first_autopilot(AZ_GET_AUTOPILOT_TIMEOUT);
|
std::optional<std::shared_ptr<System>> autopilot = mMavsdk.first_autopilot(AZ_GET_AUTOPILOT_TIMEOUT);
|
||||||
|
|
||||||
if (autopilot.has_value()) {
|
if (autopilot.has_value()) {
|
||||||
|
|||||||
Reference in New Issue
Block a user