Subcribe to GLOBAL_POSITION_INT messages

Fetch position from GLOBAL_POSITION_INT to get timestamped positions

mavsdk_wrapper.cpp

@@ -44,6 +44,8 @@ static const double SIN_ADDED_DISTANCE = sin(ADDED_DISTANCE);
 
 static int mavsdk_started = 0;
 static Telemetry::Position origin;
+static int64_t current_position[POSITION_ARRAY_SIZE] = {0};
+static int64_t timestamp_difference = 0;
 
 static Coordinates targeted_destination;
 static Coordinates projected_destination;
@@ -165,14 +167,12 @@ static void doReposition_async(float la, float lo, float radius, float y) {
     std::thread(doReposition, la, lo, radius, y).detach();
 }
 
-static void updateProjection(double current_lat, double current_lon) {
+static void updateProjection(int64_t current_lat, int64_t current_lon) {
     projected_destination = project(
         targeted_destination,
-        bearing(current_lat, current_lon,
+        bearing((double)current_lat / 1e7, (double)current_lon / 1e7,
                 targeted_destination.latitude, targeted_destination.longitude)
     );
-    printf("distance between follower and projection in C %f\n", distance(current_lat, current_lon, targeted_destination.latitude, targeted_destination.longitude));
-    // printf("bearing %f\n", toDeg(bearing(targeted_destination.latitude, targeted_destination.longitude, projected_destination.latitude, projected_destination.longitude)));
 
     doReposition_async(
         (float)projected_destination.latitude,
@@ -189,22 +189,19 @@ static long long int getTimestamp() {
 
 void updateLogAndProjection(void) {
     std::ostringstream oss;
-    Telemetry::Position position;
     Telemetry::FixedwingMetrics metrics;
-    long long int timestamp = getTimestamp();
 
     if(mavsdk_started) {
-        position = telemetry->position();
         metrics = telemetry->fixedwing_metrics();
-        oss << timestamp << ";"
-            << position.latitude_deg << ";" << position.longitude_deg << ";"
-            << position.absolute_altitude_m << ";" << position.relative_altitude_m << ";"
+        oss << current_position[4] << ";"
+            << current_position[0] << ";" << current_position[1] << ";"
+            << current_position[2] << ";" << current_position[3] << ";"
             << telemetry->attitude_euler().yaw_deg << ";"
             << metrics.airspeed_m_s << ";" << metrics.climb_rate_m_s;
         log(oss.str());
 
         if (projected_destination.latitude != 0 || targeted_destination.latitude != 0) {
-            updateProjection(position.latitude_deg, position.longitude_deg);
+            updateProjection(current_position[0], current_position[1]);
         }
     }
 }
@@ -248,6 +245,20 @@ int start(const char * ip, int port, const char * log_file, int timeout) {
     action = new Action(msystem);
     mavlink_passthrough = new MavlinkPassthrough(msystem);
 
+    mavlink_passthrough->subscribe_message_async(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, [](const mavlink_message_t& message) {
+        mavlink_global_position_int_t position;
+        mavlink_msg_global_position_int_decode(&message, &position);
+
+        if (timestamp_difference == 0)
+            timestamp_difference = getTimestamp() - position.time_boot_ms;
+
+        current_position[0] = position.lat;
+        current_position[1] = position.lon;
+        current_position[2] = position.alt;
+        current_position[3] = position.relative_alt;
+        current_position[4] = position.time_boot_ms + timestamp_difference;
+    });
+
     do {
         log("Waiting for first telemetry");
         sleep_for(seconds(1));
@@ -260,7 +271,7 @@ int start(const char * ip, int port, const char * log_file, int timeout) {
 
     log("MAVSDK started...");
     mavsdk_started = 1;
-    log("timestamp (ms);latitude (°);longitude (°);AMSL (m);rel altitude (m);yaw (°);air speed (m/s);climb rate (m/s)");
+    log("timestamp (ms);latitude (°E7);longitude (°E7);AMSL (mm);rel altitude (mm);yaw (°);air speed (m/s);climb rate (m/s)");
 
     return 0;
 }
@@ -382,18 +393,17 @@ void setTargetCoordinates(double la, double lo, float a) {
         return;
     }
 
-    Telemetry::Position position = telemetry->position();
     targeted_destination.latitude = la;
     targeted_destination.longitude = lo;
     
-    updateProjection(position.latitude_deg, position.longitude_deg);
+    updateProjection(current_position[0], current_position[1]);
     setAltitude_async(a);
 }
 
 // Information functions
 
 float getAltitude(void) {
-    return telemetry->position().absolute_altitude_m;
+    return (float)(current_position[2] / 1000);
 }
 
 float getInitialAltitude(void) {
@@ -408,15 +418,9 @@ double getInitialLongitude(void) {
     return origin.longitude_deg;
 }
 
-double *getPositionArray(void) {
-    double timestamp = (double) getTimestamp();
-    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;
-    positionArray[4] = timestamp;
+int64_t *getPositionArray(void) {
+    int64_t *positionArray = (int64_t*) malloc(POSITION_ARRAY_SIZE * sizeof(int64_t));
+    memcpy(positionArray, current_position, POSITION_ARRAY_SIZE * sizeof(int64_t));
     return positionArray;
 }