hello people, i am unable to control the velocity of the vtol. i am trying to direct the vtol by using set_velocity_body().
//
// Example showing how to make a VTOL takeoff, transition to fixedwing, loiter
// for a bit, and then transition back and return to launch.
//
#include <chrono>
#include <cstdint>
#include <iostream>
#include <thread>
#include <cmath>
#include <future>
#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/telemetry/telemetry.h>
#include <mavsdk/plugins/offboard/offboard.h>
using std::this_thread::sleep_for;
using std::chrono::seconds;
using namespace mavsdk;
double position_m;
void usage(const std::string& bin_name)
{
std::cerr << "Usage : " << bin_name << " <connection_url>\n"
<< "Connection URL format should be :\n"
<< " For TCP : tcp://[server_host][:server_port]\n"
<< " For UDP : udp://[bind_host][:bind_port]\n"
<< " For Serial : serial:///path/to/serial/dev[:baudrate]\n"
<< "For example, to connect to the simulator use URL: udp://:14540\n";
}
std::shared_ptr<System> get_system(Mavsdk& mavsdk)
{
std::cout << "Waiting to discover system...\n";
auto prom = std::promise<std::shared_ptr<System>>{};
auto fut = prom.get_future();
// We wait for new systems to be discovered, once we find one that has an
// autopilot, we decide to use it.
mavsdk.subscribe_on_new_system([&mavsdk, &prom]() {
auto system = mavsdk.systems().back();
if (system->has_autopilot()) {
std::cout << "Discovered autopilot\n";
// Unsubscribe again as we only want to find one system.
mavsdk.subscribe_on_new_system(nullptr);
prom.set_value(system);
}
});
// We usually receive heartbeats at 1Hz, therefore we should find a
// system after around 3 seconds max, surely.
if (fut.wait_for(seconds(3)) == std::future_status::timeout) {
std::cerr << "No autopilot found.\n";
return {};
}
// Get discovered system now.
return fut.get();
}
int main(int argc, char** argv)
{
if (argc != 2) {
usage(argv[0]);
return 1;
}
Mavsdk mavsdk;
ConnectionResult connection_result = mavsdk.add_any_connection(argv[1]);
if (connection_result != ConnectionResult::Success) {
std::cerr << "Connection failed: " << connection_result << '\n';
return 1;
}
auto system = get_system(mavsdk);
if (!system) {
return 1;
}
// Instantiate plugins.
auto telemetry = Telemetry{system};
auto action = Action{system};
// We want to listen to the altitude of the drone at 1 Hz.
const Telemetry::Result set_rate_result = telemetry.set_rate_position(1.0);
if (set_rate_result != Telemetry::Result::Success) {
std::cerr << "Setting rate failed: " << set_rate_result << '\n';
return 1;
}
// Set up callback to monitor altitude.
telemetry.subscribe_position([](Telemetry::Position position) {
std::cout << "Altitude: " << position.relative_altitude_m << " m\n";
position_m = position.relative_altitude_m;
});
// Wait until we are ready to arm.
while (!telemetry.health_all_ok()) {
std::cout << "Waiting for vehicle to be ready to arm...\n";
sleep_for(seconds(1));
}
// Arm vehicle
std::cout << "Arming.\n";
const Action::Result arm_result = action.arm();
if (arm_result != Action::Result::Success) {
std::cerr << "Arming failed: " << arm_result << '\n';
return 1;
}
// Take off
float desired_altitude = 20.0f;
action.set_takeoff_altitude(desired_altitude);
std::cout << "Taking off.\n";
const Action::Result takeoff_result = action.takeoff();
if (takeoff_result != Action::Result::Success) {
std::cerr << "Takeoff failed:n" << takeoff_result << '\n';
return 1;
}
while (true)
{
// Wait while it takes off.
if (position_m >= desired_altitude * 0.95){
std::cout << "Transition to fixedwing.\n";
sleep_for(seconds(1));
const Action::Result fw_result = action.transition_to_fixedwing();
sleep_for(seconds(1));
if (fw_result != Action::Result::Success) {
std::cerr << "Transition to fixed wing failed: " << fw_result << '\n';
return 1;
}
else if (fw_result == Action::Result::Success)
{
break;
}
}else {
sleep_for(seconds(1));
}
}
// Let it transition and start loitering.
sleep_for(seconds(10));
auto offboard = Offboard{system};
Offboard::VelocityBodyYawspeed stay{};
offboard.set_velocity_body(stay);
// Offboard modunu başlatmadan önce, offboard kontrolünün başarılı olacağından emin olun
Offboard::Result offboard_result = offboard.start();
if (offboard_result != Offboard::Result::Success) {
std::cerr << "Offboard modu baslatilamadi: " << offboard_result << std::endl;
return 1;
}
std::cout << "Offboard modu baslatildi. Saga donud yapiliyor..." << std::endl;
// Sağa dönüş için y hızını pozitif yapın
Offboard::VelocityBodyYawspeed saga_donus{0.0f, 5.0f, 0.0f, 0.0f};
offboard.set_velocity_body(saga_donus);
sleep_for(seconds(5)); // 5 saniye boyunca sağa dönüş yap
std::cout << "Şimdi sola dönüş yapılıyor..." << std::endl;
// Sola dönüş için y hızını negatif yapın
Offboard::VelocityBodyYawspeed sola_donus{0.0f, -5.0f, 0.0f, 0.0f};
offboard.set_velocity_body(sola_donus);
sleep_for(seconds(5)); // 5 saniye boyunca sola dönüş yap
// Yukarı hareket için down_m_s negatif yapılır
Offboard::VelocityBodyYawspeed yukari_hareket{0.0f, 0.0f, 2.0f, 0.0f};
std::cout << "Yukarı hareket ediliyor..." << std::endl;
offboard.set_velocity_body(yukari_hareket);
sleep_for(seconds(5)); // 5 saniye boyunca yukarı hareket et
// Aşağı hareket için down_m_s pozitif yapılır
Offboard::VelocityBodyYawspeed asagi_hareket{0.0f, 0.0f, -2.0f, 0.0f};
std::cout << "Aşağı hareket ediliyor..." << std::endl;
offboard.set_velocity_body(asagi_hareket);
sleep_for(seconds(5)); // 5 saniye boyunca aşağı hareket et
// Offboard modunu durdur
offboard_result = offboard.stop();
if (offboard_result != Offboard::Result::Success) {
std::cerr << "Offboard modu durdurulamadı: " << offboard_result << std::endl;
return 1;
}
std::cout << "Offboard modu durduruldu." << std::endl;
std::cout << "Burada1 \n";
// Let's stop before reaching the goto point and go back to hover.
std::cout << "Transition back to multicopter...\n";
const Action::Result mc_result = action.transition_to_multicopter();
sleep_for(seconds(1));
std::cout << "Burada2 \n";
if (mc_result != Action::Result::Success) {
std::cerr << "Transition to multi copter failed: " << mc_result << '\n';
return 1;
}
// Let's stop before reaching the goto point and go back to hover.
std::cout << "Transition back to multicopter...\n";
const Action::Result mc_result2 = action.transition_to_multicopter();
sleep_for(seconds(1));
if (mc_result != Action::Result::Success) {
std::cerr << "Transition to multi copter failed: " << mc_result2 << '\n';
return 1;
}
// Wait for the transition to be carried out.
sleep_for(seconds(5));
// Now just land here.
std::cout << "Landing...\n";
const Action::Result land_result = action.land();
if (land_result != Action::Result::Success) {
std::cerr << "Land failed: " << land_result << '\n';
return 1;
}
// Wait until disarmed.
while (telemetry.armed()) {
std::cout << "Waiting for vehicle to land and disarm\n.";
sleep_for(seconds(1));
}
std::cout << "Disarmed, exiting.\n";
return 0;
}
px4 version: 1.16.0alpha
mavsdk version: 1.4.17
ros noetic