mavsdk_wrapper.cpp 14.4 KB
Newer Older
1 2 3 4 5
#include <chrono>
#include <cmath>
#include <cstdint>
#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/action/action.h>
6
#include <mavsdk/plugins/manual_control/manual_control.h>
7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29
#include <mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.h>
#include <mavsdk/plugins/telemetry/telemetry.h>
#include <iostream>
#include <fstream>
#include <functional>
#include <future>
#include <memory>
#include <thread>

#include "mavsdk_wrapper.h"

using namespace mavsdk;
using std::chrono::seconds;
using std::this_thread::sleep_for;

#define ERROR_CONSOLE_TEXT "\033[31m" // Turn text on console red
#define NORMAL_CONSOLE_TEXT "\033[0m" // Restore normal console colour

static std::ofstream log_file_fd;

static Mavsdk _mavsdk;
static Telemetry * telemetry;
static Action * action;
30
static ManualControl * manual_control;
31 32 33 34 35 36
static MavlinkPassthrough * mavlink_passthrough;
static std::shared_ptr<System> msystem;

static auto prom = std::promise<std::shared_ptr<System>>{};
static std::future<std::shared_ptr<System>> fut;

37
static const double EARTH_GRAVITY = 9.81;
38
static const double EARTH_RADIUS = 6371000.F;
39 40 41
static const double MIN_YAW_DIFF = 1;
static const double MAX_ROLL = 35;
static const double YAW_VELOCITY_COEF = 0.5;
42

43
static bool autocontinue = false;
44
static double initialBearing;
45 46 47 48
static double previousBearing;
static double xy_ratio;
static double target_latitude;
static double target_longitude;
49
static int mavsdk_started = 0;
50
static Telemetry::Position origin;
51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95

// Logs functions

static void log(std::string message) {
    log_file_fd << message << std::endl;
}

static void log_error(std::string message) {
    log(ERROR_CONSOLE_TEXT + message + NORMAL_CONSOLE_TEXT);
}

template <class Enumeration>
static void log_error_from_result(std::string message, Enumeration result) {
  std::ostringstream oss;
  oss << message << ": " << result;
  log_error(oss.str());
}

// Action functions

static int doAction(Action::Result (Action::*toDo)(void) const, std::string failure_message) {
    if(!mavsdk_started) {
        log_error("Mavsdk not started");
        return -1;
    }

    const Action::Result result = (action->*toDo)();
    if(result != Action::Result::Success) {
        log_error_from_result(failure_message, result);
        return -1;
    }
    return 0;
}

static int doMavlinkCommand(MavlinkPassthrough::CommandLong command, std::string failure_message) {
    const MavlinkPassthrough::Result result = mavlink_passthrough->send_command_long(command);
    if(result != MavlinkPassthrough::Result::Success) {
        log_error_from_result(failure_message, result);
        return -1;
    }
    return 0;
}

// Connexion management functions

96 97 98 99
static double toRad(double angle) {
    return M_PI * angle / 180;
}

100 101 102 103
static double toDeg(double angle) {
    return 180 * angle / M_PI;
}

104
int mavsdk_start(const char * url, const char * log_file, int timeout) {
105 106
    std::string connection_url(url);
    ConnectionResult connection_result;
107
    float abs_altitude;
108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138
    log_file_fd.open(log_file);

    connection_result = _mavsdk.add_any_connection(connection_url);
    if (connection_result != ConnectionResult::Success) {
        log_error_from_result("Connection failed", connection_result);
        return -1;
    }

    log("Waiting to discover msystem...");
    fut = prom.get_future();

    _mavsdk.subscribe_on_new_system([]() {
        auto msystem_tmp = _mavsdk.systems().back();

        if (msystem_tmp->has_autopilot()) {
            log("Discovered autopilot");

            // Unsubscribe again as we only want to find one system.
            _mavsdk.subscribe_on_new_system(nullptr);
            prom.set_value(msystem_tmp);
        }
    });

    if (fut.wait_for(seconds(timeout)) == std::future_status::timeout) {
        log_error("No autopilot found, exiting.");
        return -1;
    }

    msystem = fut.get();
    telemetry = new Telemetry(msystem);
    action = new Action(msystem);
139
    manual_control = new ManualControl(msystem);
140 141
    mavlink_passthrough = new MavlinkPassthrough(msystem);

142 143
    log("Subscribing to raw GPS...");
    telemetry->subscribe_raw_gps([](Telemetry::RawGps gps) {
144
        std::ostringstream oss;
145 146 147 148 149 150 151 152 153
        Telemetry::EulerAngle euler_angle = telemetry->attitude_euler();
        Telemetry::FixedwingMetrics metrics = telemetry->fixedwing_metrics();
        Telemetry::Position position = telemetry->position();

        oss << gps.timestamp_us << "; "
            << gps.latitude_deg << "; " << gps.longitude_deg << "; "
            << gps.absolute_altitude_m << "; " << position.relative_altitude_m << "; "
            << euler_angle.pitch_deg << "; " << euler_angle.roll_deg << "; " << euler_angle.yaw_deg << "; "
            << metrics.airspeed_m_s << "; " << metrics.throttle_percentage << "; " << metrics.climb_rate_m_s;
154
        log(oss.str());
155
    });
156

157
    do {
158 159
        log("Waiting for first telemetry");
        sleep_for(seconds(1));
160 161 162
        abs_altitude = mavsdk_getAltitude();
    } while(isnan(abs_altitude) || abs_altitude == 0);
    origin = telemetry->position();
163
    xy_ratio = std::cos(toRad(origin.latitude_deg));
164

165 166
    log("MAVSDK started...");
    mavsdk_started = 1;
167
    log("timestamp; latitude; longitude; AMSL (m); rel altitude (m); pitch (°); roll(°); yaw(°); air speed (m/s); throttle(%); climb rate(m/s)");
168 169 170 171
    return 0;
}

int mavsdk_stop() {
172
    if (!mavsdk_started)
173 174
        return -1;
    
175
    if (!mavsdk_landed()) {
176 177 178 179
      log_error("You must land first !");
      return -1;
    }

180
    if (doAction(&Action::shutdown, "Shutdown failed"))
181 182 183 184
        return -1;

    // Delete pointers
    delete action;
185
    delete manual_control;
186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213
    delete mavlink_passthrough;
    delete telemetry;
    log_file_fd.close();

    return 0;
}

int mavsdk_reboot() {
    return doAction(&Action::reboot, "Rebooting failed");
}

// Flight state management functions

int mavsdk_arm(void) {
    if(!mavsdk_started)
        return -1;

    while(!telemetry->health().is_home_position_ok) {
        log("Waiting for home position to be set");
        sleep_for(seconds(1));
    }

    log("Arming...");
    return doAction(&Action::arm, "Arming failed");
}

int mavsdk_land(void) {
    log("Landing...");
214
    if (doAction(&Action::terminate, "Land failed"))
215 216 217
        return -1;

    // Check if vehicle is still in air
218
    while (telemetry->in_air()) {
219 220 221 222 223
        log("Vehicle is landing...");
        sleep_for(seconds(1));
    }
    log("Landed!");

224
    while (telemetry->armed())
225 226 227 228 229 230 231
        sleep_for(seconds(1));
    log("Finished...");

    return 0;
}

int mavsdk_takeOff(void) {
232
    if (doAction(&Action::takeoff, "Takeoff failed"))
233 234
        return -1;

235
    while (telemetry->flight_mode() != Telemetry::FlightMode::Takeoff)
236 237 238 239 240 241 242
        sleep_for(seconds(1));

    log("Taking off...");
    return 0;
}

int mavsdk_takeOffAndWait(void) {
243
    if (mavsdk_takeOff() < 0)
244 245
        return -1;

246
    while (telemetry->flight_mode() == Telemetry::FlightMode::Takeoff)
247 248 249 250
        sleep_for(seconds(1));
    return 0;
}

251 252 253 254 255 256 257 258 259 260 261 262
int mavsdk_triggerParachute(void) {
    if (!mavsdk_started)
        return -1;

    MavlinkPassthrough::CommandLong command;
    command.command = MAV_CMD_DO_PARACHUTE;
    command.param1 = 2; //see https://mavlink.io/en/messages/common.html#PARACHUTE_ACTION
    command.target_sysid = mavlink_passthrough->get_target_sysid();
    command.target_compid = mavlink_passthrough->get_target_compid();
    return doMavlinkCommand(command, "Parachute release failed");
}

263 264
// Flight management functions

265 266
int mavsdk_loiter(float radius) {
    if (!mavsdk_started)
267
        return -1;
268
    Telemetry::Position position = telemetry->position();
269 270 271 272

    MavlinkPassthrough::CommandLong command;
    command.command = MAV_CMD_DO_REPOSITION;
    command.param1 = -1; // Ground speed, -1 for default
273 274 275 276 277
    command.param2 = MAV_DO_REPOSITION_FLAGS_CHANGE_MODE; //will go to navigate mode
    command.param3 = radius; // loiter radius
    command.param5 = (float)position.latitude_deg;
    command.param6 = (float)position.longitude_deg;
    command.param7 = position.absolute_altitude_m; //altitude is ignored, use altitude override package
278 279 280
    command.target_sysid = mavlink_passthrough->get_target_sysid();
    command.target_compid = mavlink_passthrough->get_target_compid();

281
    return doMavlinkCommand(command, "Entering loiter mode failed");
282 283
}

284
int mavsdk_setAirspeed(float airspeed) {
285
    if (!mavsdk_started)
286 287 288 289 290 291
        return -1;

    MavlinkPassthrough::CommandLong command;
    command.command = MAV_CMD_DO_OVERRIDE;
    command.param1 = 1 | 2 | 4 | 8;
    command.param2 = 2 | 4 | 8;
Léo-Paul Géneau's avatar
Léo-Paul Géneau committed
292
    command.param4 = airspeed;
293 294 295 296 297 298
    command.target_sysid = mavlink_passthrough->get_target_sysid();
    command.target_compid = mavlink_passthrough->get_target_compid();

    return doMavlinkCommand(command, "Setting airspeed failed");
}

299
int mavsdk_setAltitude(float altitude) {
300
    if (!mavsdk_started)
301 302 303 304 305 306
        return -1;

    MavlinkPassthrough::CommandLong command;
    command.command = MAV_CMD_DO_OVERRIDE;
    command.param1 = 1 | 2 | 4 | 8;
    command.param2 = 1 | 2 | 8;
307
    command.param3 = altitude;
308 309 310 311 312 313
    command.target_sysid = mavlink_passthrough->get_target_sysid();
    command.target_compid = mavlink_passthrough->get_target_compid();

    return doMavlinkCommand(command, "Setting altitude failed");
}

314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336
static int startAltitudeControl(void) {
    const ManualControl::Result result = manual_control->start_altitude_control();
    if (result != ManualControl::Result::Success) {
        log_error_from_result("Set manual control failed!", result);
        return -1;
    }
    return 0;
}

static double bearing(double lat1, double lon1, double lat2, double lon2) {
    double lat1_rad = toRad(lat1);
    double lat2_rad = toRad(lat2);
    double dL = toRad(lon2 - lon1);
    double x = cos(lat2_rad) * sin(dL);
    double y = cos(lat1_rad) * sin(lat2_rad) - sin(lat1_rad) * cos(lat2_rad) * cos(dL);
    return atan2(x, y) * 180 / M_PI;
}

int mavsdk_setTargetCoordinates(double la, double lo, float a) {
    int result = 0;
    Telemetry::Position position = telemetry->position();

    if (!mavsdk_started)
337 338
        return -1;

339 340
    target_latitude = la;
    target_longitude = lo;
341 342 343
    initialBearing = previousBearing = bearing(position.latitude_deg,
                                               position.longitude_deg,
                                               la, lo);
344 345 346 347 348 349 350 351 352 353 354 355 356 357
    autocontinue = false;

    if (!mavsdk_isInManualMode()) {
        result = mavsdk_setManualControlInput();
        result |= startAltitudeControl();
    }
    result |= mavsdk_setAltitude(a);
    return result;
}

static int setManualControlInput(float x, float y, float z, float r) {
    const ManualControl::Result result = manual_control->set_manual_control_input(x, y, z, r);
    if(result != ManualControl::Result::Success) {
        log_error_from_result("Set manual control input failed!", result);
358 359 360 361 362
        return -1;
    }
    return 0;
}

363
int mavsdk_setManualControlInput(void) {
364
    double b;
365
    float speed = mavsdk_getSpeed();
366

367 368
    if (autocontinue) {
        b = initialBearing;
369
    } else {
370 371 372 373 374 375 376 377 378 379 380 381
        Telemetry::Position position = telemetry->position();
        b = bearing(position.latitude_deg, position.longitude_deg,
                    target_latitude, target_longitude);
/*
  If there is a difference of more than 160° (180° for a half circle and 20° of imprecision)
  between the required bearing and the one some milliseconds ago,
  it means that the target is now behind the drone so the drone just went over the target.

  In this case, we don't follow the target anymore, we simply keep the same trajectory.
*/
        if (abs(b - previousBearing) > 160) {
            autocontinue = true;
382
            return 0;
383
        }
384
        previousBearing = b;
385 386 387
    }

    float angle_diff = (float)b - mavsdk_getYaw();
388 389 390 391 392 393
    if (angle_diff < -180) {
        angle_diff += 360;
    } else if (angle_diff > 180) {
        angle_diff -= 360;
    }

394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413
    if (abs(angle_diff) < MIN_YAW_DIFF) {
        initialBearing = b;
        return 0;
    }

    /*
    * radius is speed²/g*tan(B) where B is roll angle
    * let's define yaw angular velocity Ys as speed*360/perimeter
    * so tan(B) = 2PI*Ys*speed / 360*g
    */
    double wanted_yaw_velocity = angle_diff * YAW_VELOCITY_COEF;
    double roll = toDeg(atan(
      2 * M_PI * wanted_yaw_velocity * speed / (360 * EARTH_GRAVITY)
    ));
    return setManualControlInput(
      0,
      (float)((roll > 0 ? 1 : -1) * std::min(abs(roll), MAX_ROLL) / MAX_ROLL),
      0,
      0
    );
414 415
}

416 417
// Information functions

418
float mavsdk_getAltitude(void) {
419
    return telemetry->position().absolute_altitude_m;
420 421
}

422
float mavsdk_getAltitudeRel(void) {
423
    return telemetry->position().relative_altitude_m;
424 425
}

426 427
float mavsdk_getInitialAltitude(void) {
    return origin.absolute_altitude_m;
428 429 430
}

double mavsdk_getInitialLatitude(void) {
431
    return origin.latitude_deg;
432 433 434
}

double mavsdk_getInitialLongitude(void) {
435
    return origin.longitude_deg;
436 437 438
}

double mavsdk_getLatitude(void) {
439
    return telemetry->position().latitude_deg;
440 441 442
}

double mavsdk_getLongitude(void) {
443
    return telemetry->position().longitude_deg;
444 445
}

446 447 448 449 450 451 452 453 454 455
double *mavsdk_getPositionArray(void) {
    Telemetry::Position position = telemetry->position();
    double *positionArray = (double*) malloc(POSITION_ARRAY_SIZE * sizeof(double));
    positionArray[0] = position.latitude_deg;
    positionArray[1] = position.longitude_deg;
    positionArray[2] = (double) position.absolute_altitude_m;
    positionArray[3] = (double) position.relative_altitude_m;
    return positionArray;
}

456 457 458 459 460 461 462 463 464
float *mavsdk_getSpeedArray(void) {
    Telemetry::FixedwingMetrics metrics = telemetry->fixedwing_metrics();
    float *speedArray = (float*) malloc(SPEED_ARRAY_SIZE * sizeof(float));
    speedArray[0] = mavsdk_getYaw();
    speedArray[1] = metrics.airspeed_m_s;
    speedArray[2] = metrics.climb_rate_m_s;
    return speedArray;
}

465 466 467
double mavsdk_getTakeOffAltitude(void) {
    const std::pair<Action::Result, float> response = action->get_takeoff_altitude();

468
    if (response.first != Action::Result::Success) {
469 470 471 472 473 474
        log_error_from_result("Get takeoff altitude failed", response.first);
        return -1;
    }
    return response.second;
}

475
float mavsdk_getYaw(void) {
476
    return telemetry->attitude_euler().yaw_deg;
477 478
}

479 480 481 482 483 484 485 486
float mavsdk_getSpeed(void) {
    return telemetry->fixedwing_metrics().airspeed_m_s;
}

float mavsdk_getClimbRate(void) {
    return telemetry->fixedwing_metrics().climb_rate_m_s;
}

487 488 489 490
int mavsdk_healthAllOk(void) {
    return telemetry->health_all_ok();
}

491 492 493 494
bool mavsdk_isInManualMode(void) {
    return telemetry->flight_mode() == Telemetry::FlightMode::Altctl;
}

495 496 497
int mavsdk_landed(void) {
    return !telemetry->in_air();
}