added Magnetometer

BlueAndi · Oct 12, 2023 · bb61648 · bb61648
1 parent bbbb67f
commit bb61648
Show file tree

Hide file tree

Showing 18 changed files with 297 additions and 255 deletions.
diff --git a/lib/HALInterfaces/IAccelerometer.h b/lib/HALInterfaces/IAccelerometer.h
diff --git a/lib/HALInterfaces/IIMU.h b/lib/HALInterfaces/IIMU.h
@@ -89,6 +89,14 @@ class IIMU
      */
     virtual const double getGyroValue() = 0;
 
+
+    /**
+     * Get last Magnetometer values.
+     *
+     * @return Magnetometer values
+     */
+    virtual const double getMagnetometerValue() = 0;
+
     /**
      * Get number of axis used.
      *

diff --git a/lib/HALSim/Board.cpp b/lib/HALSim/Board.cpp
@@ -122,6 +122,9 @@ const char* Board::ACCELEROMETER_NAME = "accelerometer";
 
 /* Name of the gyro in the robot simulation. */
 const char* Board::GYRO_NAME = "gyro";
+
+/* Name of the magnetometer in the robot simulation. */
+const char* Board::MAGNETOMETER_NAME = "magnetometer";
 /******************************************************************************
  * Protected Methods
  *****************************************************************************/

diff --git a/lib/HALSim/Board.h b/lib/HALSim/Board.h
@@ -295,6 +295,9 @@ class Board : public IBoard
     /** Name of the gyro in the robot simulation. */
     static const char* GYRO_NAME;
 
+    /** Name of the Magnetometer in the robot simulation. */
+    static const char* MAGNETOMETER_NAME;
+
     /** Simulated roboter instance. */
     webots::Robot m_robot;
 
@@ -369,7 +372,7 @@ class Board : public IBoard
         m_ledGreen(m_robot.getLED(LED_GREEN_NAME)),
         m_proximitySensors(m_simTime, m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_LEFT_NAME),
                            m_robot.getDistanceSensor(PROXIMITY_SENSOR_FRONT_RIGHT_NAME)),
-        m_imu(m_simTime, m_robot.getAccelerometer(ACCELEROMETER_NAME), m_robot.getGyro(GYRO_NAME))
+        m_imu(m_simTime, m_robot.getAccelerometer(ACCELEROMETER_NAME), m_robot.getGyro(GYRO_NAME), m_robot.getCompass(MAGNETOMETER_NAME))
     {
     }
 

diff --git a/lib/HALSim/IMU.cpp b/lib/HALSim/IMU.cpp
@@ -37,7 +37,8 @@
  *****************************************************************************/
 #include "IMU.h"
 #include <iostream>
-
+#include <cmath> // to do: entfernen
+# define M_PI           3.14159265358979323846
 /******************************************************************************
  * Compiler Switches
  *****************************************************************************/
@@ -71,8 +72,7 @@ void IMU::init()
     m_gyroValue = 0.0;
     m_accelerometer->enable(m_samplingPeriod);
     m_gyro->enable(m_samplingPeriod);
-    //std::cout << "IMU.cpp: Turned on IMU."<< std::endl;
-
+    m_magnetometer->enable(m_samplingPeriod);    
 }
 
 void IMU::calibrate()
@@ -93,7 +93,6 @@ const double* IMU::getAccelerometerValues()
             m_accelerationValues[axisIndex] = accelerationValues[axisIndex];   
         }
     }
-    std::cout<<"IMU.cpp: ACCEL: x = " << m_accelerationValues[0]<< "; y = "<<m_accelerationValues[1]<< "; z = "<<accelerationValues[2]<< std::endl;
 
     return m_accelerationValues;
 }
@@ -104,7 +103,19 @@ const double IMU::getGyroValue()
     if (gyroValues != nullptr) {
         m_gyroValue = gyroValues[INDEX_OF_Z_AXIS];
     }
-    //std::cout<<"IMU.cpp: GYRO: z = " << m_gyroValue<< std::endl;
+
+    return m_gyroValue;
+}
+
+const double IMU::getMagnetometerValue()
+{
+    const double * magnetometerValues = m_magnetometer->getValues();
+
+    if (magnetometerValues != nullptr) {
+        double angleInRadians = atan2(magnetometerValues[INDEX_OF_Y_AXIS], magnetometerValues[INDEX_OF_X_AXIS]);
+        m_gyroValue = angleInRadians;
+        std::cout << "IMU.cpp: Magnetometer = " << m_gyroValue<< std::endl;
+    }
     return m_gyroValue;
 }
 
@@ -135,3 +146,10 @@ bool IMU::isCalibrationSuccessful()
 /******************************************************************************
  * Local Functions
  *****************************************************************************/
+void transformFromUToX(double* result, const double* vectorP, const double* vectorX, const double& angle) {
+    double cosAngle = cos(angle);
+    double sinAngle = sin(angle);
+
+    result[0] = cosAngle * vectorP[0] - sinAngle * vectorP[1] + vectorX[0];
+    result[1] = sinAngle * vectorP[0] + cosAngle * vectorP[1] + vectorX[1];
+}
diff --git a/lib/HALSim/IMU.h b/lib/HALSim/IMU.h
@@ -50,6 +50,7 @@
 
 #include <webots/Accelerometer.hpp>
 #include <webots/Gyro.hpp>
+#include <webots/Compass.hpp>
 
 class IMU : public IIMU
 {
@@ -60,15 +61,16 @@ class IMU : public IIMU
      * @param[in] simTime       Simulation time
      * @param[in] accelerometer The IMU
      */
-    IMU(const SimTime& simTime, webots::Accelerometer* accelerometer, webots::Gyro* gyro) :
+    IMU(const SimTime& simTime, webots::Accelerometer* accelerometer, webots::Gyro* gyro, webots::Compass* compass) :
         IIMU(),
         m_simTime(simTime),
         m_accelerometer(accelerometer),
         m_gyro(gyro),
+        m_magnetometer(compass),
 
         m_sensorCalibSuccessful(false),
         m_sensorCalibStarted(false),
-        m_samplingPeriod(10)
+        m_samplingPeriod(10) // to do: woher kommt wert?
     {
     }
 
@@ -93,21 +95,29 @@ class IMU : public IIMU
 
 
     /**
-     * Get last IMU values.
+     * Get last Accelerometer values.
      *
-     * @return IMU values
+     * @return Accelerometer values
      */ 
     //
     const double* getAccelerometerValues() final;
 
-        /**
-     * Get last IMU values.
+    /**
+     * Get last Gyro values.
      *
-     * @return IMU values
+     * @return Gyro values
      */ 
     //
     const double getGyroValue() final;
 
+    /**
+     * Get last Magnetometer values.
+     *
+     * @return Magnetometer values
+     */ 
+    //
+    const double getMagnetometerValue() final;
+
 
     /**
      * Get number of axis used.
@@ -142,17 +152,21 @@ class IMU : public IIMU
     static const uint8_t NUMBER_OF_AXIS = 2;
 
 
+    static const uint8_t INDEX_OF_X_AXIS = 0;
+    static const uint8_t INDEX_OF_Y_AXIS = 1;
     static const uint8_t INDEX_OF_Z_AXIS = 2;
 
 
-    const SimTime&   m_simTime;                      /**< Simulation time */
+    const SimTime&   m_simTime;                    /**< Simulation time */
     webots::Accelerometer* m_accelerometer;        /**< The accelerometer */
-    webots::Gyro* m_gyro;        /**< The gyro */
+    webots::Gyro*          m_gyro;                 /**< The gyro */
+    webots::Compass*       m_magnetometer;         /**< The magnetometer */
     uint8_t  m_calibErrorInfo; /**< Indicates which sensor failed the calibration, if the calibration failed. */
     bool                    m_sensorCalibSuccessful; /**< Indicates weather the calibration was successfull or not. */
     bool                    m_sensorCalibStarted;     /**< Indicates weather the calibration has started or not. */
     double                  m_accelerationValues[NUMBER_OF_AXIS]; /**< The last value of each sensor */
     double                  m_gyroValue;
+    double                  m_magnetometerValue;
     int                     m_samplingPeriod;
 
     /* Default constructor not allowed. */

diff --git a/lib/HALTarget/Board.h b/lib/HALTarget/Board.h
@@ -210,6 +210,16 @@ class Board : public IBoard
         return m_proximitySensors;
     }
 
+    /**
+     * Get IMU driver.
+     * 
+     * @return IMU driver
+     */
+    IIMU& getIMU() final
+    {
+        return m_imu;
+    }
+
 protected:
 
 private:

diff --git a/lib/HALTarget/IMU.cpp b/lib/HALTarget/IMU.cpp
@@ -122,8 +122,15 @@ const double IMU::getGyroValue()
     m_imu.readGyro();
 
     m_gyroValue = (m_imu.g.z - m_rawGyroOffset) * GYRO_SENSITIVITY_FACTOR;
+    return m_gyroValue;
 }
 
+const double IMU::getMagnetometerValue()
+{
+    return 0.0;
+}
+
+
 const uint8_t IMU::getNumberOfAccelerometerAxis()
 {
     return NUMBER_OF_AXIS;