Merge pull request CCNYRoboticsLab#60 from mistoll/ahrs_enu

Add ENU support, fix orientation init

imu_filter_madgwick/CMakeLists.txt

@@ -24,12 +24,12 @@ include_directories(
 
 
 # create imu_filter library
-add_library (imu_filter src/imu_filter.cpp  src/imu_filter_ros.cpp  include/imu_filter_madgwick/imu_filter.h)
+add_library (imu_filter src/imu_filter.cpp  src/imu_filter_ros.cpp src/stateless_orientation.cpp)
 add_dependencies(imu_filter ${PROJECT_NAME}_gencfg)
 target_link_libraries(imu_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 
 # create imu_filter_nodelet library
-add_library (imu_filter_nodelet src/imu_filter_nodelet.cpp include/imu_filter_madgwick/imu_filter_nodelet.h)
+add_library (imu_filter_nodelet src/imu_filter_nodelet.cpp)
 add_dependencies(imu_filter_nodelet ${PROJECT_NAME}_gencfg)
 target_link_libraries(imu_filter_nodelet imu_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 
@@ -52,4 +52,17 @@ install(DIRECTORY include/${PROJECT_NAME}/
 
 install(FILES imu_filter_nodelet.xml
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+
+
+if(CATKIN_ENABLE_TESTING)
+  catkin_add_gtest(${PROJECT_NAME}-madgwick_test
+    test/stateless_orientation_test.cpp
+    test/madgwick_test.cpp
+  )
+  target_link_libraries(${PROJECT_NAME}-madgwick_test
+    imu_filter
+    ${catkin_LIBRARIES}
   )
+endif()

imu_filter_madgwick/include/imu_filter_madgwick/earth_frame.h

@@ -0,0 +1,8 @@
+#ifndef IMU_FILTER_MADGWICK_EARTH_FRAME_H_
+#define IMU_FILTER_MADGWICK_EARTH_FRAME_H_
+
+namespace EarthFrame {
+  enum EarthFrame { ENU, NED, NWU };
+}
+
+#endif

imu_filter_madgwick/include/imu_filter_madgwick/imu_filter.h

@@ -25,6 +25,9 @@
 #ifndef IMU_FILTER_MADWICK_IMU_FILTER_H
 #define IMU_FILTER_MADWICK_IMU_FILTER_H
 
+#include <imu_filter_madgwick/earth_frame.h>
+#include <iostream>
+
 class ImuFilter
 {
   public:
@@ -34,32 +37,14 @@ class ImuFilter
 
   private:
     // **** paramaters
-    double gain_;     // algorithm gain
-    double zeta_;	  // gyro drift bias gain
+    double gain_;    // algorithm gain
+    double zeta_;    // gyro drift bias gain
+    EarthFrame::EarthFrame earth_frame_;    // NWU, ENU, NED
 
     // **** state variables
     double q0, q1, q2, q3;  // quaternion
     float w_bx_, w_by_, w_bz_; // 
 
-    // **** member functions
-
-    // Fast inverse square-root
-    // See: http://en.wikipedia.org/wiki/Methods_of_computing_square_roots#Reciprocal_of_the_square_root
-    static float invSqrt(float x)
-    {
-      float xhalf = 0.5f * x;
-      union
-      {
-        float x;
-        int i;
-      } u;
-      u.x = x;
-      u.i = 0x5f3759df - (u.i >> 1);
-      /* The next line can be repeated any number of times to increase accuracy */
-      u.x = u.x * (1.5f - xhalf * u.x * u.x);
-      return u.x;
-    }
-
 public:
     void setAlgorithmGain(double gain)
     {
@@ -71,6 +56,11 @@ class ImuFilter
         zeta_ = zeta;
     }
 
+    void setEarthFrame(EarthFrame::EarthFrame frame)
+    {
+        earth_frame_ = frame;
+    }
+
     void getOrientation(double& q0, double& q1, double& q2, double& q3)
     {
         q0 = this->q0;

imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_ros.h

@@ -81,8 +81,10 @@ class ImuFilterRos
     boost::shared_ptr<FilterConfigServer> config_server_;
 
     // **** paramaters
+    EarthFrame::EarthFrame earth_frame_;
     bool use_mag_;
     bool use_magnetic_field_msg_;
+    bool stateless_;
     bool publish_tf_;
     bool reverse_tf_;
     std::string fixed_frame_;
@@ -115,10 +117,6 @@ class ImuFilterRos
                        float roll, float pitch, float yaw);
 
     void reconfigCallback(FilterConfig& config, uint32_t level);
-
-    void computeRPY(float ax, float ay, float az,
-                    float mx, float my, float mz,
-                    float& roll, float& pitch, float& yaw);
 };
 
 #endif // IMU_FILTER_IMU_MADWICK_FILTER_ROS_H

imu_filter_madgwick/include/imu_filter_madgwick/stateless_orientation.h

@@ -0,0 +1,48 @@
+/*
+ *  Copyright (C) 2010, CCNY Robotics Lab
+ *  Ivan Dryanovski <[email protected]>
+ *
+ *  http://robotics.ccny.cuny.edu
+ *
+ *  Based on implementation of Madgwick's IMU and AHRS algorithms.
+ *  http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
+ *
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef IMU_FILTER_MADWICK_STATELESS_ORIENTATION_H
+#define IMU_FILTER_MADWICK_STATELESS_ORIENTATION_H
+
+#include <geometry_msgs/Vector3.h>
+#include <geometry_msgs/Quaternion.h>
+#include <imu_filter_madgwick/earth_frame.h>
+
+class StatelessOrientation
+{
+public:
+  static bool computeOrientation(
+    EarthFrame::EarthFrame frame,
+    geometry_msgs::Vector3 acceleration,
+    geometry_msgs::Vector3 magneticField,
+    geometry_msgs::Quaternion& orientation);
+
+  static bool computeOrientation(
+    EarthFrame::EarthFrame frame,
+    geometry_msgs::Vector3 acceleration,
+    geometry_msgs::Quaternion& orientation);
+
+};
+
+#endif // IMU_FILTER_MADWICK_STATELESS_ORIENTATION_H

imu_filter_madgwick/package.xml

@@ -32,6 +32,8 @@
   <run_depend>message_filters</run_depend>
   <run_depend>dynamic_reconfigure</run_depend>
 
+  <test_depend>rosunit</test_depend>
+
   <export>
     <nodelet plugin="${prefix}/imu_filter_nodelet.xml" />
   </export>