Remove setTargetCoordinates' `going_through` param

Now `setTargetCoordinates` always go through the desired point by updating
regulary the projected coordinates.
Function `loiter` is now defined for the purpose where `setTargetCoordinates`
was called with `going_through` parameter set to `false` (to loiter around a
point without going through it).

mavsdk_wrapper.cpp

@@ -20,6 +20,11 @@ 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
 
+struct Coordinates {
+  double latitude;
+  double longitude;
+};
+
 static std::ofstream log_file_fd;
 
 static Mavsdk _mavsdk;
@@ -33,11 +38,17 @@ static std::future<std::shared_ptr<System>> fut;
 
 static const float DEFAULT_RADIUS = 100;
 static const float EARTH_RADIUS = 6371000;
+static const float ADDED_DISTANCE = (2 * DEFAULT_RADIUS) / EARTH_RADIUS;
+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;
+static Coordinates projected_destination;
+
 // Logs functions
 
 static void log(std::string message) {
@@ -80,7 +91,7 @@ static int doMavlinkCommand(MavlinkPassthrough::CommandLong command, std::string
     return 0;
 }
 
-// Connexion management functions
+// Coordinates related functions
 
 static double toRad(double angle) {
     return M_PI * angle / 180;
@@ -90,6 +101,74 @@ static double toDeg(double angle) {
     return 180 * angle / M_PI;
 }
 
+/*
+ * compute the bearing angle between point 1 and point 2
+ * the bearing angle is the direction to take to go from point 1 to point 2
+ * values are in [-π; π] where 0 is North
+*/
+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);
+}
+
+/*
+ * To ensure that a drone goes through the target point and to avoid loitering
+ * trajectory, the target point is projected to a distance equal to twice the
+ * default loiter radius.
+*/
+static Coordinates getProjectionCoordinates(Coordinates destination, Coordinates position) {
+    double laRad = toRad(destination.latitude);
+    double sinLa = sin(laRad);
+    double cosLa = cos(laRad);
+    double loRad = toRad(destination.longitude);
+    double bearing_angle = bearing(position.latitude, position.longitude,
+                                   targeted_destination.latitude,
+                                   targeted_destination.longitude);
+    double newLa = asin(sinLa * COS_ADDED_DISTANCE
+                        + cosLa * SIN_ADDED_DISTANCE * cos(bearing_angle));
+    double newLo = loRad + atan2(sin(bearing_angle) * SIN_ADDED_DISTANCE * cosLa,
+                                 COS_ADDED_DISTANCE - sinLa * sin(newLa));
+    Coordinates projected{ toDeg(newLa), toDeg(newLo) };
+    return projected;
+}
+
+static int doReposition(float la, float lo, float radius, float y) {
+    if (!mavsdk_started)
+        return -1;
+
+    MavlinkPassthrough::CommandLong command;
+    command.command = MAV_CMD_DO_REPOSITION;
+    command.param1 = -1; // Ground speed, -1 for default
+    command.param2 = MAV_DO_REPOSITION_FLAGS_CHANGE_MODE; //will go to navigate mode
+    command.param3 = radius; // loiter radius
+    command.param4 = y; // loiter direction, 0: clockwise 1: counter clockwise
+    command.param5 = la;
+    command.param6 = lo;
+    command.target_sysid = mavlink_passthrough->get_target_sysid();
+    command.target_compid = mavlink_passthrough->get_target_compid();
+
+    return doMavlinkCommand(command, "Entering loiter mode failed");
+}
+
+static void updateProjection(double current_lat, double current_lon) {
+    Coordinates position{ current_lat, current_lon };
+    projected_destination = getProjectionCoordinates(targeted_destination,
+                                                     position);
+
+    return doReposition(
+        (float)projected_destination.latitude,
+        (float)projected_destination.longitude,
+        DEFAULT_RADIUS,
+        0
+    );
+}
+
+// Connexion management functions
+
 int start(const char * ip, int port, const char * log_file, int timeout) {
     std::string remote_ip(ip);
     ConnectionResult connection_result;
@@ -149,6 +228,9 @@ int start(const char * ip, int port, const char * log_file, int timeout) {
             << telemetry->attitude_euler().yaw_deg << ";"
             << gps.velocity_m_s << ";" << telemetry->fixedwing_metrics().climb_rate_m_s;
         log(oss.str());
+
+        if (projected_destination.latitude != 0 || targeted_destination.latitude != 0)
+            updateProjection(gps.latitude_deg, gps.longitude_deg);
     });
     return 0;
 }
@@ -218,32 +300,25 @@ int triggerParachute(void) {
 
 // Flight management functions
 
-static int doReposition(float la, float lo, float radius, float y) {
+static int setAltitude(float altitude) {
     if (!mavsdk_started)
         return -1;
 
     MavlinkPassthrough::CommandLong command;
-    command.command = MAV_CMD_DO_REPOSITION;
-    command.param1 = -1; // Ground speed, -1 for default
-    command.param2 = MAV_DO_REPOSITION_FLAGS_CHANGE_MODE; //will go to navigate mode
-    command.param3 = radius; // loiter radius
-    command.param4 = y; // loiter direction, 0: clockwise 1: counter clockwise
-    command.param5 = la;
-    command.param6 = lo;
+    command.command = MAV_CMD_DO_OVERRIDE;
+    command.param1 = 1 | 2 | 4 | 8;
+    command.param2 = 1 | 2 | 8;
+    command.param3 = altitude;
     command.target_sysid = mavlink_passthrough->get_target_sysid();
     command.target_compid = mavlink_passthrough->get_target_compid();
 
-    return doMavlinkCommand(command, "Entering loiter mode failed");
+    return doMavlinkCommand(command, "Setting altitude failed");
 }
 
-int loiter(float radius) {
-    Telemetry::Position position = telemetry->position();
-    return doReposition(
-        (float)position.latitude_deg,
-        (float)position.longitude_deg,
-        radius,
-        0
-    );
+int loiter(double la, double lo, float a, float radius) {
+    int result = doReposition((float)la, (float)lo, radius, 0);
+    result |= setAltitude(a);
+    return result;
 }
 
 int setAirspeed(float airspeed) {
@@ -261,65 +336,16 @@ int setAirspeed(float airspeed) {
     return doMavlinkCommand(command, "Setting airspeed failed");
 }
 
-static int setAltitude(float altitude) {
-    if (!mavsdk_started)
-        return -1;
-
-    MavlinkPassthrough::CommandLong command;
-    command.command = MAV_CMD_DO_OVERRIDE;
-    command.param1 = 1 | 2 | 4 | 8;
-    command.param2 = 1 | 2 | 8;
-    command.param3 = altitude;
-    command.target_sysid = mavlink_passthrough->get_target_sysid();
-    command.target_compid = mavlink_passthrough->get_target_compid();
-
-    return doMavlinkCommand(command, "Setting altitude failed");
-}
-
-/*
- * compute the bearing angle between point 1 and point 2
- * the bearing angle is the direction to take to go from point 1 to point 2
- * values are in [-π; π] where 0 is North
-*/
-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);
-}
-
-int setTargetCoordinates(double la, double lo, float a, bool going_through) {
+int setTargetCoordinates(double la, double lo, float a) {
     int result;
     if (!mavsdk_started)
         return -1;
 
-    if (going_through) {
-      double b, laRad = toRad(la), loRad = toRad(lo), newLa, newLo;
-      float addedDistance = (2 * DEFAULT_RADIUS) / EARTH_RADIUS;
     Telemetry::Position position = telemetry->position();
+    targeted_destination.latitude = la;
+    targeted_destination.longitude = lo;
     
-      b = bearing(position.latitude_deg, position.longitude_deg, la, lo);
-      newLa = asin(
-        sin(laRad) * cos(addedDistance)
-        + cos(laRad) * sin(addedDistance) * cos(b)
-      );
-      newLo = loRad + atan2(
-        sin(b) * sin(addedDistance) * cos(laRad),
-        cos(addedDistance) - sin(laRad) * sin(newLa)
-      );
-
-      result = doReposition(
-          (float)toDeg(newLa),
-          (float)toDeg(newLo),
-          DEFAULT_RADIUS,
-          0
-      );
-    } else {
-      result = doReposition((float)la, (float)lo, DEFAULT_RADIUS, 0);
-    }
-
+    result = updateProjection(position.latitude_deg, position.longitude_deg);
     result |= setAltitude(a);
     return result;
 }