added speed limits, simple_flight rate controls working

ajberchek · Jun 26, 2017 · 4ab1a5e · 4ab1a5e
1 parent d3eaf86
commit 4ab1a5e
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 5 deletions.
diff --git a/AirLib/include/common/EarthUtils.hpp b/AirLib/include/common/EarthUtils.hpp
@@ -352,7 +352,7 @@ class EarthUtils {
     static constexpr float SeaLevelAirDensity = 1.225f; //kg/m^3
     static constexpr float Gravity = 9.80665f;    //m/s^2
     static constexpr float Radius = EARTH_RADIUS; //m
-
+    static constexpr float SpeedOfLight = 299792458.0f; //m
 
 private:
     /* magnetic field */

diff --git a/AirLib/include/controllers/simple_flight/firmware/Mixer.hpp b/AirLib/include/controllers/simple_flight/firmware/Mixer.hpp
@@ -68,10 +68,10 @@ class Mixer {
 
     //only thing that this matrix does is change the sign
     static constexpr motorMixer_t mixerQuadX[] = { //QuadX config
-        { 1.0f, 1.0f, 1.0f,  1.0f },          // FRONT_R
-        { 1.0f, -1.0f, -1.0f,  1.0f },          // REAR_L
-        { 1.0f, -1.0f, 1.0f, -1.0f },          // FRONT_L
-        { 1.0f, 1.0f, -1.0f, -1.0f },          // REAR_R
+        { 1.0f, -1.0f, 1.0f,  1.0f },          // FRONT_R
+        { 1.0f, 1.0f, -1.0f,  1.0f },          // REAR_L
+        { 1.0f, 1.0f, 1.0f, -1.0f },          // FRONT_L
+        { 1.0f, -1.0f, -1.0f, -1.0f },          // REAR_R
     };
 
 };

diff --git a/AirLib/include/physics/FastPhysicsEngine.hpp b/AirLib/include/physics/FastPhysicsEngine.hpp
@@ -306,6 +306,20 @@ class FastPhysicsEngine : public PhysicsEngineBase {
         next.twist.linear = current.twist.linear + (current.accelerations.linear + next.accelerations.linear) * (0.5f * dt_real);
         next.twist.angular = current.twist.angular + (current.accelerations.angular + next.accelerations.angular) * (0.5f * dt_real);
 
+        //if controller has bug, velocities can increase idenfinitely 
+        //so we need to clip this or everything will turn in to infinity/nans
+
+        if (next.twist.linear.squaredNorm() > EarthUtils::SpeedOfLight * EarthUtils::SpeedOfLight) { //speed of light
+            next.twist.linear /= (next.twist.linear.norm() / EarthUtils::SpeedOfLight);
+            next.accelerations.linear = Vector3r::Zero();
+        }
+        //
+        //for disc of 1m radius which angular velocity translates to speed of light on tangent?
+        if (next.twist.angular.squaredNorm() > EarthUtils::SpeedOfLight * EarthUtils::SpeedOfLight) { //speed of light
+            next.twist.angular /= (next.twist.angular.norm() / EarthUtils::SpeedOfLight);
+            next.accelerations.angular = Vector3r::Zero();
+        }
+
         computeNextPose(dt, current.pose, avg_linear, avg_angular, next);
     }