compute min angle distance along circle

zwbai · Apr 12, 2018 · 129df5c · 129df5c
1 parent 6c3154b
commit 129df5c
Show file tree

Hide file tree

Showing 3 changed files with 32 additions and 18 deletions.
diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleLevelController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleLevelController.hpp
@@ -68,11 +68,10 @@ class AngleLevelController :
         TReal goal_angle = level_goal[axis_];
         TReal measured_angle = state_estimator_->getAngles()[axis_];
 
-        TReal goal_angle_normd = Axis3r::normalizeAngle(goal_angle, 2*M_PI);
-        TReal measured_angle_normd = Axis3r::normalizeAngle(measured_angle, 2*M_PI);
-
-        pid_->setGoal(goal_angle_normd);
-        pid_->setMeasured(measured_angle_normd);
+        adjustToMinDistanceAngles(measured_angle, goal_angle);
+
+        pid_->setGoal(goal_angle);
+        pid_->setMeasured(measured_angle);
         pid_->update();
 
         //use this to drive rate controller
@@ -83,6 +82,7 @@ class AngleLevelController :
         output_ = rate_controller_->getOutput();
     }
 
+
     virtual TReal getOutput() override
     {
         return output_;
@@ -99,6 +99,32 @@ class AngleLevelController :
         return rate_mode_;
     }
 
+private:
+    static void adjustToMinDistanceAngles(TReal& angle1, TReal& angle2)
+    {
+        static constexpr TReal TwoPi = 2 * M_PIf;
+
+        //first make sure both angles are restricted from -360 to +360
+        angle1 = static_cast<TReal>(std::fmod(angle1, TwoPi));
+        angle2 = static_cast<TReal>(std::fmod(angle2, TwoPi));
+
+        //now make sure both angles are restricted from 0 to 360
+        if (angle1 < 0)
+            angle1 = TwoPi + angle1;
+        if (angle2 < 0)
+            angle2 = TwoPi + angle2;
+
+        //measure distance between two angles
+        auto dist = angle1 - angle2;
+
+        //if its > 180 then invert first angle
+        if (dist > M_PIf)
+            angle1 = angle1 - TwoPi;
+        //if two much on other side then invert second angle
+        else if (dist < -M_PIf)
+            angle2 = angle2 - TwoPi;
+    }
+
 private:
     unsigned int axis_;
     const IGoal* goal_;

diff --git a/...include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/CommonStructs.hpp b/...include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/CommonStructs.hpp
@@ -70,18 +70,6 @@ class Axis3 {
         return 3;
     }
 
-    //duplicate version of function in VectorMath
-    static T normalizeAngle(T angle, T max_angle = static_cast<T>(360))
-    {
-        angle = static_cast<T>(std::fmod(angle, max_angle));
-        if (angle > max_angle/2)
-            return angle - max_angle;
-        else if (angle < -max_angle/2)
-            return angle + max_angle;
-        else
-            return angle;
-    }
-
 
 private:
     T vals_[3];

diff --git a/AirLibUnitTests/main.cpp b/AirLibUnitTests/main.cpp
@@ -12,13 +12,13 @@ int main()
     using namespace msr::airlib;
 
     std::unique_ptr<TestBase> tests[] = {
+        std::unique_ptr<TestBase>(new QuaternionTest()),
         std::unique_ptr<TestBase>(new CelestialTest()),
         std::unique_ptr<TestBase>(new SettingsTest()),
         std::unique_ptr<TestBase>(new SimpleFlightTest())
         //,
         //std::unique_ptr<TestBase>(new PixhawkTest()),
         //std::unique_ptr<TestBase>(new RosFlightTest()),
-        //std::unique_ptr<TestBase>(new QuaternionTest()),
         //std::unique_ptr<TestBase>(new WorkerThreadTest())
     };