Fix bearing

See merge request !4

include/pubsub.h

@@ -36,11 +36,9 @@ typedef struct {
     int type;
     UA_Byte builtInType;
     union {
-        UA_DateTime (*getTimestamp)(void);
         UA_Double   (*getDouble)(void);
         UA_Float    (*getFloat)(void);
         UA_String   (*getString)(void);
-        UA_UInt32   (*getUint32)(void);
     } getter;
 
 } VariableData;

mavsdk_wrapper.cpp

@@ -37,6 +37,7 @@ static std::future<std::shared_ptr<System>> fut;
 static const double EARTH_RADIUS = 6371000.F;
 
 static bool autocontinue = false;
+static double initialBearing;
 static double previousBearing;
 static double xy_ratio;
 static double target_latitude;
@@ -329,7 +330,8 @@ int mavsdk_setTargetCoordinates(double la, double lo, float a) {
 
     target_latitude = la;
     target_longitude = lo;
-    previousBearing = bearing(position.latitude_deg, position.longitude_deg,
+    initialBearing = previousBearing = bearing(position.latitude_deg,
+                                               position.longitude_deg,
                                                la, lo);
     autocontinue = false;
 
@@ -351,19 +353,35 @@ static int setManualControlInput(float x, float y, float z, float r) {
 }
 
 int mavsdk_setManualControlInput(void) {
-    if (autocontinue)
-        return setManualControlInput(0, 0, 0, 0);
+    double b;
 
+    if (autocontinue) {
+        b = initialBearing;
+    } else {
         Telemetry::Position position = telemetry->position();
-    double b = bearing(position.latitude_deg, position.longitude_deg,
+        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;
         } else {
             previousBearing = b;
         }
+    }
 
     float angle_diff = (float)b - mavsdk_getYaw();
+    if (angle_diff < -180) {
+        angle_diff += 360;
+    } else if (angle_diff > 180) {
+        angle_diff -= 360;
+    }
+
     return setManualControlInput(0, (angle_diff > 0 ? 1 : -1) * std::min(abs(angle_diff), 30.0f) / 30, 0, 0);
 }
 

qjs_wrapper.c

@@ -194,12 +194,6 @@ VariableData pubsub_get_value(UA_String identifier) {
                 case UA_TYPES_FLOAT:
                     *(UA_Float*)var_details.value = droneVariableArray[i].getter.getFloat();
                     break;
-                case UA_TYPES_UINT32:
-                    *(UA_UInt32*)var_details.value = droneVariableArray[i].getter.getUint32();
-                    break;
-                case UA_TYPES_DATETIME:
-                    *(UA_DateTime*)var_details.value = droneVariableArray[i].getter.getTimestamp();
-                    break;
                 case UA_TYPES_STRING:
                     *(UA_String*)var_details.value = droneVariableArray[i].getter.getString();
                     break;