Stop relying on gps messages

Use position messages instead of gps ones in case of gps denial.

mavsdk_wrapper.cpp

@@ -43,7 +43,6 @@ static const double COS_ADDED_DISTANCE = cos(ADDED_DISTANCE);
 static const double SIN_ADDED_DISTANCE = sin(ADDED_DISTANCE);
 
 static int mavsdk_started = 0;
-static uint64_t init_timestamp;
 static Telemetry::Position origin;
 
 static Coordinates targeted_destination;
@@ -169,19 +168,22 @@ static void updateProjection(double current_lat, double current_lon) {
 
 int updateLogAndProjection(void) {
     std::ostringstream oss;
-    Telemetry::RawGps gps;
+    Telemetry::Position position;
+    Telemetry::FixedwingMetrics metrics;
 
     if(mavsdk_started) {
-        gps = telemetry->raw_gps ();
-        oss << (gps.timestamp_us - init_timestamp) / 1000 << ";"
-            << gps.latitude_deg << ";" << gps.longitude_deg << ";"
-            << gps.absolute_altitude_m << ";" << telemetry->position().relative_altitude_m << ";"
+        const auto now = std::chrono::system_clock::now();
+        position = telemetry->position();
+        metrics = telemetry->fixedwing_metrics();
+        oss << std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count() << ";"
+            << position.latitude_deg << ";" << position.longitude_deg << ";"
+            << position.absolute_altitude_m << ";" << position.relative_altitude_m << ";"
             << telemetry->attitude_euler().yaw_deg << ";"
-            << gps.velocity_m_s << ";" << telemetry->fixedwing_metrics().climb_rate_m_s;
+            << metrics.velocity_m_s << ";" << metrics.climb_rate_m_s;
         log(oss.str());
 
         if (projected_destination.latitude != 0 || targeted_destination.latitude != 0) {
-            return updateProjection(gps.latitude_deg, gps.longitude_deg);
+            return updateProjection(position.latitude_deg, position.longitude_deg);
         }
     }
 
@@ -234,11 +236,10 @@ int start(const char * ip, int port, const char * log_file, int timeout) {
     } while(isnan(abs_altitude) || abs_altitude == 0);
 
     origin = telemetry->position();
-    init_timestamp = telemetry->raw_gps().timestamp_us;
 
     log("MAVSDK started...");
     mavsdk_started = 1;
-    log("timestamp (ms);latitude (°);longitude (°);AMSL (m);rel altitude (m);yaw (°);ground speed (m/s);climb rate (m/s)");
+    log("timestamp (ms);latitude (°);longitude (°);AMSL (m);rel altitude (m);yaw (°);air speed (m/s);climb rate (m/s)");
 
     return 0;
 }