Subscribe to positions from GPS

- use positions provided by the GPS to get an accurate timestamp
- remove telemetry variables

include/mavsdk_wrapper.h

@@ -32,14 +32,8 @@ double mavsdk_getInitialLatitude(void);
 double mavsdk_getInitialLongitude(void);
 double mavsdk_getLatitude(void);
 double mavsdk_getLongitude(void);
-uint64_t mavsdk_getTimestamp(void);
 double mavsdk_getTakeOffAltitude(void);
-float mavsdk_getPitch(void);
-float mavsdk_getRoll(void);
 float mavsdk_getYaw(void);
-float mavsdk_getAirspeed(void);
-float mavsdk_getClimbRate(void);
-float mavsdk_getThrottle(void);
 int mavsdk_healthAllOk(void);
 bool mavsdk_isInManualMode(void);
 int mavsdk_landed(void);

mavsdk_wrapper.cpp

@@ -34,12 +34,6 @@ static std::shared_ptr<System> msystem;
 static auto prom = std::promise<std::shared_ptr<System>>{};
 static std::future<std::shared_ptr<System>> fut;
 
-static Telemetry::FlightMode flight_mode;
-static Telemetry::EulerAngle angle;
-static Telemetry::FixedwingMetrics metric;
-static Telemetry::Position position;
-static uint64_t timestamp_us;
-
 static const double EARTH_RADIUS = 6371000.F;
 
 static bool autocontinue = false;
@@ -101,6 +95,7 @@ static double toRad(double angle) {
 int mavsdk_start(const char * url, const char * log_file, int timeout) {
     std::string connection_url(url);
     ConnectionResult connection_result;
+    float abs_altitude;
     log_file_fd.open(log_file);
 
     connection_result = _mavsdk.add_any_connection(connection_url);
@@ -135,46 +130,27 @@ int mavsdk_start(const char * url, const char * log_file, int timeout) {
     manual_control = new ManualControl(msystem);
     mavlink_passthrough = new MavlinkPassthrough(msystem);
 
-    log("Subscribing to flight mode...");
-    // Subscribe to receive updates on flight mode. You can find out whether FollowMe is active.
-    telemetry->subscribe_flight_mode([](Telemetry::FlightMode _flight_mode) {
-        if(flight_mode != _flight_mode) {
-            flight_mode = _flight_mode;
-        }
-    });
-
-    log("Subscribing to Euler angle...");
-    telemetry->subscribe_attitude_euler([](Telemetry::EulerAngle euler_angle) {
-        angle = euler_angle;
-    });
-
-    log("Subscribing to Fixedwing Metrics...");
-    telemetry->subscribe_fixedwing_metrics([](Telemetry::FixedwingMetrics m) {
-        metric = m;
-    });
-
-    log("Subscribing to position...");
-    telemetry->subscribe_position([](Telemetry::Position pos) {
-        timestamp_us = std::chrono::duration_cast<std::chrono::milliseconds>(
-                          std::chrono::system_clock::now().time_since_epoch()
-                       ).count();
-
+    log("Subscribing to raw GPS...");
+    telemetry->subscribe_raw_gps([](Telemetry::RawGps gps) {
         std::ostringstream oss;
-	      position = pos;
-
-        oss << mavsdk_getTimestamp() << "; "
-            << mavsdk_getLatitude() << "; " << mavsdk_getLongitude() << "; "
-            << mavsdk_getAltitude() << "; " << mavsdk_getAltitudeRel() << "; "
-            << mavsdk_getPitch() << "; " << mavsdk_getRoll() << "; " << mavsdk_getYaw() << "; "
-            << mavsdk_getAirspeed() << "; " << mavsdk_getThrottle() << "; " << mavsdk_getClimbRate();
+        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;
         log(oss.str());
     });
 
-    while(isnan(position.absolute_altitude_m)) {
+    do {
         log("Waiting for first telemetry");
         sleep_for(seconds(1));
-    }
-    origin = position;
+        abs_altitude = mavsdk_getAltitude();
+    } while(isnan(abs_altitude) || abs_altitude == 0);
+    origin = telemetry->position();
     xy_ratio = std::cos(toRad(origin.latitude_deg));
 
     log("MAVSDK started...");
@@ -184,17 +160,16 @@ int mavsdk_start(const char * url, const char * log_file, int timeout) {
 }
 
 int mavsdk_stop() {
-    if(!mavsdk_started)
+    if (!mavsdk_started)
         return -1;
     
-    if(!mavsdk_landed()) {
+    if (!mavsdk_landed()) {
       log_error("You must land first !");
       return -1;
     }
 
-    if(doAction(&Action::shutdown, "Shutdown failed")) {
+    if (doAction(&Action::shutdown, "Shutdown failed"))
         return -1;
-    }
 
     // Delete pointers
     delete action;
@@ -227,46 +202,40 @@ int mavsdk_arm(void) {
 
 int mavsdk_land(void) {
     log("Landing...");
-    if(doAction(&Action::terminate, "Land failed")) {
+    if (doAction(&Action::terminate, "Land failed"))
         return -1;
-    }
 
     // Check if vehicle is still in air
-    while(telemetry->in_air()) {
+    while (telemetry->in_air()) {
         log("Vehicle is landing...");
         sleep_for(seconds(1));
     }
     log("Landed!");
 
-    while(telemetry->armed()) {
+    while (telemetry->armed())
         sleep_for(seconds(1));
-    }
     log("Finished...");
 
     return 0;
 }
 
 int mavsdk_takeOff(void) {
-    if(doAction(&Action::takeoff, "Takeoff failed")) {
+    if (doAction(&Action::takeoff, "Takeoff failed"))
         return -1;
-    }
 
-    while(flight_mode != Telemetry::FlightMode::Takeoff) {
+    while (telemetry->flight_mode() != Telemetry::FlightMode::Takeoff)
         sleep_for(seconds(1));
-    }
 
     log("Taking off...");
     return 0;
 }
 
 int mavsdk_takeOffAndWait(void) {
-    if(mavsdk_takeOff() < 0) {
+    if (mavsdk_takeOff() < 0)
         return -1;
-    }
 
-    while(flight_mode == Telemetry::FlightMode::Takeoff) {
+    while (telemetry->flight_mode() == Telemetry::FlightMode::Takeoff)
         sleep_for(seconds(1));
-    }
     return 0;
 }
 
@@ -304,7 +273,7 @@ int mavsdk_loiter(float radius) {
 }
 
 int mavsdk_setAirspeed(float airspeed) {
-    if(!mavsdk_started)
+    if (!mavsdk_started)
         return -1;
 
     MavlinkPassthrough::CommandLong command;
@@ -402,11 +371,11 @@ int mavsdk_setManualControlInput(void) {
 // Information functions
 
 float mavsdk_getAltitude(void) {
-    return position.absolute_altitude_m;
+    return telemetry->position().absolute_altitude_m;
 }
 
 float mavsdk_getAltitudeRel(void) {
-    return position.relative_altitude_m;
+    return telemetry->position().relative_altitude_m;
 }
 
 float mavsdk_getInitialAltitude(void) {
@@ -422,49 +391,25 @@ double mavsdk_getInitialLongitude(void) {
 }
 
 double mavsdk_getLatitude(void) {
-    return position.latitude_deg;
+    return telemetry->position().latitude_deg;
 }
 
 double mavsdk_getLongitude(void) {
-    return position.longitude_deg;
-}
-
-uint64_t mavsdk_getTimestamp(void) {
-    return timestamp_us;
+    return telemetry->position().longitude_deg;
 }
 
 double mavsdk_getTakeOffAltitude(void) {
     const std::pair<Action::Result, float> response = action->get_takeoff_altitude();
 
-    if(response.first != Action::Result::Success) {
+    if (response.first != Action::Result::Success) {
         log_error_from_result("Get takeoff altitude failed", response.first);
         return -1;
     }
     return response.second;
 }
 
-float mavsdk_getPitch(void) {
-    return angle.pitch_deg;
-}
-
-float mavsdk_getRoll(void) {
-    return angle.roll_deg;
-}
-
 float mavsdk_getYaw(void) {
-    return angle.yaw_deg;
-}
-
-float mavsdk_getAirspeed(void) {
-    return metric.airspeed_m_s;
-}
-
-float mavsdk_getClimbRate(void) {
-    return metric.climb_rate_m_s;
-}
-
-float mavsdk_getThrottle(void) {
-    return metric.throttle_percentage;
+    return telemetry->attitude_euler().yaw_deg;
 }
 
 int mavsdk_healthAllOk(void) {