angle normaliztion bug which caused drone to rotate unnaturally

zwbai · Apr 12, 2018 · 6c3154b · 6c3154b
1 parent 74c8d8b
commit 6c3154b
Show file tree

Hide file tree

Showing 5 changed files with 31 additions and 11 deletions.
diff --git a/AirLib/include/common/VectorMath.hpp b/AirLib/include/common/VectorMath.hpp
@@ -397,13 +397,13 @@ class VectorMathT {
             , 1.0f - 2.0f * (q.x() * q.x() + q.y() * q.y()));
     }
 
-    static RealT normalizeAngleDegrees(RealT angle)
+    static RealT normalizeAngle(RealT angle, RealT max_angle = static_cast<RealT>(360))
     {
-        angle = static_cast<RealT>(std::fmod(angle, 360));
-        if (angle > 180)
-            return angle - 360;
-        else if (angle < -180)
-            return angle + 360;
+        angle = static_cast<RealT>(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;
     }

diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleLevelController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleLevelController.hpp
@@ -64,8 +64,15 @@ class AngleLevelController :
 
         //get response of level PID
         const auto& level_goal = goal_->getGoalValue();
-        pid_->setGoal(level_goal[axis_]);
-        pid_->setMeasured(state_estimator_->getAngles()[axis_]);
+
+        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);
         pid_->update();
 
         //use this to drive rate controller

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,6 +70,18 @@ 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/AirLib/src/vehicles/multirotor/controllers/DroneControllerBase.cpp b/AirLib/src/vehicles/multirotor/controllers/DroneControllerBase.cpp
@@ -292,7 +292,7 @@ bool DroneControllerBase::moveToZ(float z, float velocity, const YawMode& yaw_mo
 
 bool DroneControllerBase::rotateToYaw(float yaw, float margin, CancelableBase& cancelable_action)
 {
-    YawMode yaw_mode(false, VectorMath::normalizeAngleDegrees(yaw));
+    YawMode yaw_mode(false, VectorMath::normalizeAngle(yaw));
     Waiter waiter(getCommandPeriod());
     auto start_pos = getPosition();
     bool is_yaw_reached;
@@ -646,7 +646,7 @@ void DroneControllerBase::adjustYaw(const Vector3r& heading, DrivetrainType driv
     if (drivetrain == DrivetrainType::ForwardOnly && !yaw_mode.is_rate) {
         if (heading.norm() > getDistanceAccuracy()) {
             yaw_mode.yaw_or_rate = yaw_mode.yaw_or_rate + (std::atan2(heading.y(), heading.x()) * 180 / M_PIf);
-            yaw_mode.yaw_or_rate = VectorMath::normalizeAngleDegrees(yaw_mode.yaw_or_rate);
+            yaw_mode.yaw_or_rate = VectorMath::normalizeAngle(yaw_mode.yaw_or_rate);
         }
         else
             yaw_mode.setZeroRate(); //don't change existing yaw if heading is too small because that can generate random result

diff --git a/PythonClient/PythonClient.pyproj b/PythonClient/PythonClient.pyproj
@@ -5,7 +5,7 @@
     <SchemaVersion>2.0</SchemaVersion>
     <ProjectGuid>e2049e20-b6dd-474e-8bca-1c8dc54725aa</ProjectGuid>
     <ProjectHome>.</ProjectHome>
-    <StartupFile>hello_car.py</StartupFile>
+    <StartupFile>orbit.py</StartupFile>
     <SearchPath>
     </SearchPath>
     <WorkingDirectory>.</WorkingDirectory>
@@ -52,6 +52,7 @@
     <Compile Include="objects.py">
       <SubType>Code</SubType>
     </Compile>
+    <Compile Include="orbit.py" />
     <Compile Include="point_cloud.py" />
     <Compile Include="seg_pallete.py">
       <SubType>Code</SubType>