"Failsafe enable, no RC and no offboard" but I'm not turned offboard off

Hello, when I’m running attached code, a system1 drone in potentialField instead to giving their position as in friendTelemetry->subscribe_position is enabling failsafe

How to fix it?

code:

#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"

bool friendDroneOnPosition=false;

inline void handleActionError(Action::Result result, const string& message);

inline void handleOffboardError(Offboard::Result result, const string& message);

Telemetry::LandedStateCallback landedStateCallback(shared_ptr<Telemetry>& telemetry, promise<void>& promise);

static int goToStart(System& system1, System& system2, float heading,int chosenAlgorithm);

static int foeProgram(System& system, float heading);

static int potentialField(System& system1, System& system2);

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);
    }
};

static double computeDistance(Telemetry::Position position1, Telemetry::Position position2) {
    double horizontalDistanceSquared = pow(position1.longitude_deg-position2.longitude_deg, 2.0) + pow(position1.latitude_deg-position2.latitude_deg, 2.0);
    return sqrt(horizontalDistanceSquared + pow(position1.relative_altitude_m-position2.relative_altitude_m, 2.0));
}

static vector<double> computeVector(Telemetry::Position position1, Telemetry::Position position2) {
    vector<double> result;
    result.push_back(position2.longitude_deg - position1.longitude_deg);
    result.push_back(position2.latitude_deg - position1.latitude_deg);
    result.push_back(position2.relative_altitude_m - position1.relative_altitude_m);
    return result;
}

static int potentialField(System& system1, System& system2){

    auto action = make_shared<Action>(system1);
    auto offboard = make_shared<Offboard>(system1);
    auto friendTelemetry = make_shared<Telemetry>(system1);
    auto foeTelemetry = make_shared<Telemetry>(system2);

    auto prom = std::make_shared<std::promise<Telemetry::Result>>();
    auto future_result = prom->get_future();

    const Telemetry::Result set_friend_rate_result = friendTelemetry->set_rate_position(25.0);
    if (set_friend_rate_result != Telemetry::Result::Success) {
        std::cout << "Setting rate failed:" << set_friend_rate_result << std::endl;
    }

    const Telemetry::Result set_foe_rate_result = foeTelemetry->set_rate_position(25.0);
    if (set_foe_rate_result != Telemetry::Result::Success) {
        std::cout << "Setting rate failed:" << set_foe_rate_result << std::endl;
    }

    Telemetry::Position startPosition = friendTelemetry->position();
    Telemetry::Position targetPosition;
    targetPosition.longitude_deg = startPosition.longitude_deg;
    targetPosition.latitude_deg = startPosition.latitude_deg+(30.0/111000);
    Telemetry::Position foePosition = foeTelemetry->position();

    Offboard::VelocityBodyYawspeed velocityBodyYawspeed;
    velocityBodyYawspeed.down_m_s = 0;
    velocityBodyYawspeed.forward_m_s = 0;
    velocityBodyYawspeed.right_m_s = 0;
    velocityBodyYawspeed.yawspeed_deg_s = 0;
    offboard->set_velocity_body(velocityBodyYawspeed);
    /*Offboard::Result obResult = offboard->start();
    handleOffboardError(obResult,"OFFBOARD FAILED\n");*/
    int foeTicks=0;

    foeTelemetry->subscribe_position([&foePosition,&foeTicks](Telemetry::Position position){
       foePosition = position;
       foeTicks++;
    });

    friendTelemetry->subscribe_position([&friendTelemetry,&targetPosition, //this isn't working
                                         &foePosition,&offboard,&action](Telemetry::Position position){
        const double epsilon = -1;
        const double mi = 1;
        if(computeDistance(position,targetPosition)>=0.1){
            double distanceToTarget = computeDistance(position,targetPosition);
            vector<double> distanceToTargetVector = computeVector(position,targetPosition),
                    attractiveForce(3),repulsiveForce(3),forcesVector(3);
            attractiveForce[0] = (epsilon * (1/pow(distanceToTarget,2)) * distanceToTargetVector[0]);
            attractiveForce[1] = (epsilon * (1/pow(distanceToTarget,2)) * distanceToTargetVector[1]);
            attractiveForce[2] = (epsilon * (1/pow(distanceToTarget,2)) * distanceToTargetVector[2]);
            double distanceToFoe = computeDistance(position, foePosition);
            vector<double> distanceToFoeVector = computeVector(position, foePosition);
            repulsiveForce[0] = (mi * (1 / pow(distanceToFoe, 2) * distanceToFoeVector[0]));
            repulsiveForce[1] = (mi * (1 / pow(distanceToFoe, 2) * distanceToFoeVector[1]));
            repulsiveForce[2] = (mi * (1 / pow(distanceToFoe, 2) * distanceToFoeVector[2]));
            forcesVector[0] = attractiveForce[0] + repulsiveForce[0];
            forcesVector[1] = attractiveForce[1] + repulsiveForce[1];
            forcesVector[2] = attractiveForce[2] + repulsiveForce[2];
            Offboard::VelocityBodyYawspeed velocityBodyYawspeed;
            velocityBodyYawspeed.forward_m_s=5;
            velocityBodyYawspeed.right_m_s = 5/(forcesVector[1]/forcesVector[0]);
            velocityBodyYawspeed.down_m_s = 5/(forcesVector[1]/forcesVector[2]);
            offboard->set_velocity_body(velocityBodyYawspeed);
        } else {
            cout<<"We're Arrived"<<endl;
            //Offboard::Result obResult = offboard->stop();
            //handleOffboardError(obResult,"OFFBOARD FAILED\n");
            const Action::Result landResult = action->land();
            handleActionError(landResult,"LANDING FAILED\n");
            while (friendTelemetry->in_air()) {
                cout << "Landing..." << endl;
                sleep_for(seconds(1));
            }
            cout << "Drone 1 Landed!" << endl;
            friendTelemetry->subscribe_position(nullptr);
            sleep_for(seconds(3));
            cout << "Bye! ;)" << endl;

        }
    }
            /*[&action,&targetPosition,&offboard,&foeTelemetry](Telemetry::Position position){
        const double epsilon = -1;
        const double mi = 1;
        if(computeDistance(position,targetPosition)>=0.1){
            double distanceToTarget = computeDistance(position,targetPosition);
            vector<double> distanceToTargetVector = computeVector(position,targetPosition),
                    attractiveForce(3),repulsiveForce(3),forcesVector(3);
            attractiveForce[0] = (epsilon * (1/pow(distanceToTarget,2)) * distanceToTargetVector[0]);
            attractiveForce[1] = (epsilon * (1/pow(distanceToTarget,2)) * distanceToTargetVector[1]);
            attractiveForce[2] = (epsilon * (1/pow(distanceToTarget,2)) * distanceToTargetVector[2]);
            Telemetry::Position foePosition = foeTelemetry->position();
            double distanceToFoe = computeDistance(position, foePosition);
            if(distanceToFoe<=5){
                vector<double> distanceToFoeVector = computeVector(position, foePosition);
                repulsiveForce[0] = (mi * (1 / pow(distanceToFoe, 2) * distanceToFoeVector[0]));
                repulsiveForce[1] = (mi * (1 / pow(distanceToFoe, 2) * distanceToFoeVector[1]));
                repulsiveForce[2] = (mi * (1 / pow(distanceToFoe, 2) * distanceToFoeVector[2]));
            } else {
                repulsiveForce[0] = 0;
                repulsiveForce[1] = 0;
                repulsiveForce[2] = 0;
            }
            forcesVector[0] = attractiveForce[0] + repulsiveForce[0];
            forcesVector[1] = attractiveForce[1] + repulsiveForce[1];
            forcesVector[2] = attractiveForce[2] + repulsiveForce[2];
            Offboard::VelocityBodyYawspeed velocityBodyYawspeed;
            velocityBodyYawspeed.forward_m_s=5;
            velocityBodyYawspeed.right_m_s = 5/(forcesVector[1]/forcesVector[0]);
            velocityBodyYawspeed.down_m_s = 5/(forcesVector[1]/forcesVector[2]);
            offboard->set_velocity_body(velocityBodyYawspeed);
        } else {
            cout<<"We're Arrived"<<endl;
            Offboard::Result obResult = offboard->stop();
            handleOffboardError(obResult,"OFFBOARD FAILED\n");
            const Action::Result landResult = action->land();
            handleActionError(landResult,"LANDING FAILED\n");
        }
    }*/);

    /*while (friendTelemetry->in_air()) {
        cout << "Landing..." << endl;
        sleep_for(seconds(1));
    }
    cout << "Drone 1 Landed!" << endl;
    friendTelemetry->subscribe_position(nullptr);
    sleep_for(seconds(3));
    cout << "Bye! ;)" << endl;*/
    //cout<<"foe ticks: "<<foeTicks<<endl;

    return EXIT_SUCCESS;
}

static int goToStart(System& system1, System& system2, float heading, int chosenAlgorithm){
    double sinus,cosinus;
    if(heading==0) {
        sinus = 0;
        cosinus = 1;
    } else if (heading == 90) {
        sinus = 1;
        cosinus = 0;
    }
    else if (heading == 180) {
        sinus = 0;
        cosinus = -1;
    }
    else if (heading == 270) {
        sinus = -1;
        cosinus = 0;
    }
    else {
        sinus = sin((double)heading / 360 * 2 * M_PI);
        cosinus = cos((double)heading / 360 * 2 * M_PI);
    }

    auto action = make_shared<Action>(system1);
    auto offboard = make_shared<Offboard>(system1);
    auto telemetry = make_shared<Telemetry>(system1);

    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"<<endl;

    const Action::Result takeOffResult = action->takeoff();
    handleActionError(takeOffResult,"TAKEOFF FAILED\n");

    telemetry->subscribe_landed_state(landedStateCallback(telemetry,inAirPromise));
    inAirFuture.wait();

    Offboard::PositionNedYaw point;
    point.north_m = 0;
    point.east_m = 0;
    point.down_m = 0;
    point.yaw_deg = 0;
    offboard->set_position_ned(point);
    Offboard::Result obResult = offboard->start();
    handleOffboardError(obResult,"OFFBOARD FAILED\n");
    point.north_m = -15*cosinus;
    point.east_m = -15*sinus;
    point.down_m = -10;
    point.yaw_deg = heading;
    offboard->set_position_ned(point);
    sleep_for(seconds(15));
    /*obResult = offboard->stop();
    handleOffboardError(obResult,"OFFBOARD FAILED\n");*/

    switch(chosenAlgorithm){
        case 1:
            friendDroneOnPosition=true;
            potentialField(system1,system2);
            break;
        case 2:
            cout << "WIP";
            break;
        case 3:
            cout << "WIP";
            break;
    }

    return EXIT_SUCCESS;
}

static int foeProgram(System& system, float heading){
    double sinus,cosinus;
    if(heading==0) {
        sinus = 0;
        cosinus = 1;
    } else if (heading == 90) {
        sinus = 1;
        cosinus = 0;
    }
    else if (heading == 180) {
        sinus = 0;
        cosinus = -1;
    }
    else if (heading == 270) {
        sinus = -1;
        cosinus = 0;
    }
    else {
        sinus = sin((double)heading / 360 * 2 * M_PI);
        cosinus = cos((double)heading / 360 * 2 * M_PI);
    }

    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 2 arming..."<<endl;

    const Action::Result armResult = action->arm();
    handleActionError(armResult,"ARM FAILED\n");
    cout << "Drone 2 armed"<<endl;

    const Action::Result takeOffResult = action->takeoff();
    handleActionError(takeOffResult,"TAKEOFF FAILED\n");

    telemetry->subscribe_landed_state(landedStateCallback(telemetry,inAirPromise));
    inAirFuture.wait();

    Offboard::PositionNedYaw point;
    point.north_m = 0;
    point.east_m = 0;
    point.down_m = 0;
    point.yaw_deg = 0;
    offboard->set_position_ned(point);
    Offboard::Result obResult = offboard->start();
    handleOffboardError(obResult,"OFFBOARD FAILED\n");
    point.north_m = -15*cosinus;
    point.east_m = -15*sinus;
    point.down_m = -10;
    point.yaw_deg = heading;
    offboard->set_position_ned(point);
    //sleep_for(seconds(15));
    point.north_m = 15*cosinus;
    point.east_m = 15*sinus;
    while(!friendDroneOnPosition){
        cout<<"Waiting on friend drone..."<<endl;
        sleep_for(seconds(1));
    }
    offboard->set_position_ned(point);
    sleep_for(seconds(15));
    //obResult = offboard->stop();
    //handleOffboardError(obResult,"OFFBOARD FAILED\n");


    const Action::Result landResult = action->land();
    handleActionError(landResult,"LANDING FAILED\n");

    while (telemetry->in_air()) {
        cout << "Drone 2 Landing..." << endl;
        sleep_for(seconds(1));
    }
    cout << "Drone 2 Landed!" << endl;

    sleep_for(seconds(3));
    cout << "Bye! ;)" << endl;

    return EXIT_SUCCESS;
}

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;
        }
    };
}

int main() {
    int chosenTest = 0, chosenAlgorithm=0;
    do {
        cout<<"Jaki test chcesz wykonać?\n"<<
            "1) Wrogi dron przelatuje równolegle\n"<<
            "2) Wrogi dron przelatuje prostopadle\n"<<
            "Twój wybór: ";
        cin>>chosenTest;
    } while (chosenTest!=1 && chosenTest!=2);
    do {
        cout<<"Jaki algorytm ma być sprawdzony?\n"<<
            "1) Pól potencjałów\n"<<
            "2) Monte Carlo\n"<<
            "3) Podejście od strony szybkości\n"<<
            "Twój wybór: ";
        cin>>chosenAlgorithm;
    } while (chosenAlgorithm!=1 && chosenAlgorithm!=2 && chosenAlgorithm!=3);
    float foeHeading;
    if(chosenTest==1)
        foeHeading=180;
    else if(chosenTest==2)
        foeHeading=90;

    Mavsdk dc;
    ConnectionResult connectionResult[2];

    connectionResult[0] = dc.add_any_connection("udp://127.0.0.1:14540");
    connectionResult[1] = dc.add_any_connection("udp://127.0.0.1:14541");

    for (auto & i : connectionResult){
        if (i != ConnectionResult::Success) {
            cerr << ERROR_CONSOLE_TEXT
                 << "Connection failed: " << i
                 << NORMAL_CONSOLE_TEXT << endl;
            return 1;
        }
    }

    atomic<signed> systemsCount{0};

    cout << "Waiting to discover systems..." << endl;

    vector<uint64_t> uuids;

    dc.register_on_discover([&systemsCount,&uuids](uint64_t uuid) {
        cout << "Discovered system with UUID: " << uuid << endl;
        uuids.push_back(uuid);
        systemsCount++;
    });

    sleep_for(seconds(5));

    if (systemsCount != 2) {
        cerr << ERROR_CONSOLE_TEXT << "Not all systems found, exiting." << NORMAL_CONSOLE_TEXT
             << endl;
        return 1;
    }

    vector<thread> threads;

    System& system1 = dc.system(uuids[0]);
    System& system2 = dc.system(uuids[1]);

    thread t1(&goToStart,ref(system1),ref(system2),0,chosenAlgorithm);
    thread t2(&foeProgram, ref(system2),foeHeading);

    threads.push_back(move(t1));
    threads.push_back(move(t2));
    for (auto& t : threads){
        t.join();
    }

    return 0;
}