I have code like this:
#include <future>
#include <iostream>
#include <mavsdk.h>
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/offboard/offboard.h>
#include <mavsdk/plugins/telemetry/telemetry.h>
using namespace std;
using namespace mavsdk;
using chrono::seconds;
using this_thread::sleep_for;
#define ERROR_CONSOLE_TEXT "\033[31m"
#define NORMAL_CONSOLE_TEXT "\033[0m"
inline void handleActionError(Action::Result result, const string& message){
if(result != Action::Result::Success){
cerr << ERROR_CONSOLE_TEXT << message << result << NORMAL_CONSOLE_TEXT<< endl;
exit(EXIT_FAILURE);
}
};
inline void handleOffboardError(Offboard::Result result, const string& message){
if(result != Offboard::Result::Success){
cerr << ERROR_CONSOLE_TEXT << message << result << NORMAL_CONSOLE_TEXT<< endl;
exit(EXIT_FAILURE);
}
};
Telemetry::LandedStateCallback landedStateCallback(shared_ptr<Telemetry>& telemetry, promise<void>& promise) {
return [&promise,&telemetry](Telemetry::LandedState landed){
switch (landed){
case Telemetry::LandedState::OnGround:
std::cout << "On ground" << std::endl;
break;
case Telemetry::LandedState::TakingOff:
std::cout << "Taking off..." << std::endl;
break;
case Telemetry::LandedState::Landing:
std::cout << "Landing..." << std::endl;
break;
case Telemetry::LandedState::InAir:
std::cout << "Taking off has finished." << std::endl;
telemetry->subscribe_landed_state(nullptr);
promise.set_value();
break;
case Telemetry::LandedState::Unknown:
std::cout << "Unknown landed state." << std::endl;
break;
}
};
}
Telemetry::PositionCallback positionCallback(){
return [](Telemetry::Position position){
cout<<position.latitude_deg<<"\t"<<position.longitude_deg<<"\t"<<position.relative_altitude_m<<endl;
};
}
static int tellPosition(System& system){
auto telemetry = make_shared<Telemetry>(system);
const Telemetry::Result setRateResult = telemetry->set_rate_position(25);
if(setRateResult!=Telemetry::Result::Success){
cout << ERROR_CONSOLE_TEXT
<< "Setting rate failed:" << setRateResult
<< NORMAL_CONSOLE_TEXT << endl;
return EXIT_FAILURE;
}
telemetry->subscribe_position(positionCallback());
return EXIT_SUCCESS;
}
bool offboardTour(shared_ptr<Offboard> ob){
Offboard::PositionNedYaw point;
point.north_m = 0;
point.east_m = 0;
point.down_m = 0;
point.yaw_deg = 0;
ob->set_position_ned(point);
Offboard::Result obResult = ob->start();
handleOffboardError(obResult,"OFFBOARD FAILED\n");
cout<<"Offboard started"<<endl;
point.down_m= -7.5;
ob->set_position_ned(point);
sleep_for(seconds(10));
point.east_m = 15;
point.yaw_deg = 90;
ob->set_position_ned(point);;
sleep_for(seconds(10));
point.north_m = -15;
point.yaw_deg = 180;
ob->set_position_ned(point);
sleep_for(seconds(10));
point.east_m = 0;
point.yaw_deg = 270;
ob->set_position_ned(point);
sleep_for(seconds(10));
point.north_m = 0;
point.yaw_deg = 0;
ob->set_position_ned(point);
sleep_for(seconds(10));
obResult = ob->stop();
handleOffboardError(obResult,"OFFBOARD FAILED\n");
cout<<"Offboard stopped"<<endl;
return true;
}
static int flyOffboard(System& system){
auto action = make_shared<Action>(system);
auto offboard = make_shared<Offboard>(system);
auto telemetry = make_shared<Telemetry>(system);
const Telemetry::Result setRateResult = telemetry->set_rate_position(1);
if(setRateResult!=Telemetry::Result::Success){
cout << ERROR_CONSOLE_TEXT
<< "Setting rate failed:" << setRateResult
<< NORMAL_CONSOLE_TEXT << endl;
return EXIT_FAILURE;
}
while (!telemetry->health_all_ok()) {
sleep_for(seconds(1));
}
promise<void> inAirPromise;
auto inAirFuture = inAirPromise.get_future();
cout << "Drone 1 arming..."<<endl;
const Action::Result armResult = action->arm();
handleActionError(armResult,"ARM FAILED\n");
cout << "Drone 1 armed";
const Action::Result takeOffResult = action->takeoff();
handleActionError(takeOffResult,"TAKEOFF FAILED\n");
telemetry->subscribe_landed_state(landedStateCallback(telemetry,inAirPromise));
inAirFuture.wait();
bool ret= offboardTour(offboard);
if (!ret){
return EXIT_FAILURE;
}
const Action::Result landResult = action->land();
handleActionError(landResult,"LANDING FAILED\n");
while (telemetry->in_air()) {
cout << "Drone 1 Landing..." << endl;
sleep_for(seconds(1));
}
cout << "Drone 1 Landed!" << endl;
sleep_for(seconds(3));
cout << "Bye! ;)" << endl;
return EXIT_SUCCESS;
}
int main() {
Mavsdk dc;
ConnectionResult connectionResult;
connectionResult = dc.add_any_connection("udp://127.0.0.1:14540");
if (connectionResult != ConnectionResult::Success) {
cerr << ERROR_CONSOLE_TEXT
<< "Connection failed: " << connectionResult
<< NORMAL_CONSOLE_TEXT << endl;
return 1;
}
cout << "Waiting on system..." << endl;
bool systemDiscovered = false;
uint64_t systemUuid;
dc.register_on_discover([&systemDiscovered,&systemUuid](uint64_t uuid) {
cout << "Discovered system with UUID: " << uuid << endl;
systemUuid = uuid;
systemDiscovered = true;
});
sleep_for(seconds(5));
if (!systemDiscovered) {
cerr << ERROR_CONSOLE_TEXT << "Not all systems found, exiting." << NORMAL_CONSOLE_TEXT
<< endl;
return 1;
}
vector<thread> threads;
System& system = dc.system(systemUuid);
thread t1(&flyOffboard, ref(system));
thread t2(&tellPosition, ref(system));
threads.push_back(move(t1));
threads.push_back(move(t2));
for(auto &t: threads){
t.join();
}
return(0);
}
In one thread I’m steering the drone and in other thread I made code, which is intended to give current position of the drone. The problem is the callback is never called (even if I’m not changing telemetry rate)?
How to fix it?