Deduce roll angle using radius formula

mavsdk_wrapper.cpp

@@ -34,7 +34,11 @@ static std::shared_ptr<System> msystem;
 static auto prom = std::promise<std::shared_ptr<System>>{};
 static std::future<std::shared_ptr<System>> fut;
 
+static const double EARTH_GRAVITY = 9.81;
 static const double EARTH_RADIUS = 6371000.F;
+static const double MIN_YAW_DIFF = 1;
+static const double MAX_ROLL = 35;
+static const double YAW_VELOCITY_COEF = 0.5;
 
 static bool autocontinue = false;
 static double initialBearing;
@@ -93,6 +97,10 @@ static double toRad(double angle) {
     return M_PI * angle / 180;
 }
 
+static double toDeg(double angle) {
+    return 180 * angle / M_PI;
+}
+
 int mavsdk_start(const char * url, const char * log_file, int timeout) {
     std::string connection_url(url);
     ConnectionResult connection_result;
@@ -354,6 +362,7 @@ static int setManualControlInput(float x, float y, float z, float r) {
 
 int mavsdk_setManualControlInput(void) {
     double b;
+    float speed = mavsdk_getSpeed();
 
     if (autocontinue) {
         b = initialBearing;
@@ -370,9 +379,9 @@ int mavsdk_setManualControlInput(void) {
 */
         if (abs(b - previousBearing) > 160) {
             autocontinue = true;
-        } else {
-            previousBearing = b;
+            return 0;
         }
+        previousBearing = b;
     }
 
     float angle_diff = (float)b - mavsdk_getYaw();
@@ -382,7 +391,26 @@ int mavsdk_setManualControlInput(void) {
         angle_diff -= 360;
     }
 
-    return setManualControlInput(0, (angle_diff > 0 ? 1 : -1) * std::min(abs(angle_diff), 70.0f) / 70, 0, 0);
+    if (abs(angle_diff) < MIN_YAW_DIFF) {
+        initialBearing = b;
+        return 0;
+    }
+
+    /*
+    * radius is speed²/g*tan(B) where B is roll angle
+    * let's define yaw angular velocity Ys as speed*360/perimeter
+    * so tan(B) = 2PI*Ys*speed / 360*g
+    */
+    double wanted_yaw_velocity = angle_diff * YAW_VELOCITY_COEF;
+    double roll = toDeg(atan(
+      2 * M_PI * wanted_yaw_velocity * speed / (360 * EARTH_GRAVITY)
+    ));
+    return setManualControlInput(
+      0,
+      (float)((roll > 0 ? 1 : -1) * std::min(abs(roll), MAX_ROLL) / MAX_ROLL),
+      0,
+      0
+    );
 }
 
 // Information functions