Implemented findings from BlueAndi

doc/architecture/SENSORFUSION.md

@@ -25,16 +25,16 @@ On simulation the physical communication uses a socket connection.
 ### Tx channel "SENSOR_DATA"
 This channel is used to send raw sensor data used for Sensor Fusion on the ZumoComSystem.
 
-* The dataypes int16_t is used.
+* The datatypes can be found in SerialMuxChannel.h.
 * Order:
-  * Acceleration in X
-  * Acceleration in Y
-  * turnRateZ around Z
-  * Magnetometer value in X 
-  * Magnetometer value in Y 
-  * Angle calculated by Odometry
-  * Position in X calculated by Odometry
-  * Position in Y calculated by Odometry
+  * Acceleration in X (raw sensor value)
+  * Acceleration in Y (raw sensor value)
+  * turnRateZ around Z (raw sensor value)
+  * Magnetometer value in X (raw sensor value)
+  * Magnetometer value in Y (raw sensor value)
+  * Angle calculated by Odometry (in mrad)
+  * Position in X calculated by Odometry (in mm)
+  * Position in Y calculated by Odometry (in mm)
 * Endianess: Big endian
 
 # SW Architecture

lib/HALInterfaces/IBoard.h

@@ -136,7 +136,7 @@ class IBoard
     virtual ILed& getGreenLed() = 0;
 
     /**
-     * Get IMU driver.
+     * Get IMU (=Inertial Measurement Unit) driver.
      * 
      * @return IMU driver
      */

lib/HALInterfaces/IIMU.h

@@ -1,7 +1,7 @@
 /* MIT License
  *
  * Copyright (c) 2023 Andreas Merkle <[email protected]>
- * 
+ *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
  * in the Software without restriction, including without limitation the rights
@@ -25,9 +25,9 @@
     DESCRIPTION
 *******************************************************************************/
 /**
- * @brief  Abstract IMU interface, depicts Zumo32U4IMU library
+ * @brief  Abstract IMU (Inertial Measurement Unit) interface, depicts Zumo32U4IMU library
  * @author Juliane Kerpe <[email protected]>
- * 
+ *
  * @addtogroup HALInterfaces
  *
  * @{
@@ -55,12 +55,14 @@
 /** Struct of the IMU data in x, y and z direction. */
 typedef struct _IMUData
 {
-    int16_t valueX;
-    int16_t valueY;
-    int16_t valueZ;
+    int16_t valueX; /* Raw sensor value in x direction (unitless) */
+    int16_t valueY; /* Raw sensor value in y direction (unitless) */
+    int16_t valueZ; /* Raw sensor value in z direction (unitless) */
 } __attribute__((packed)) IMUData;
 
-/** The abstract IMU interface. */
+/** The abstract IMU interface.
+ * IMU stands for Inertial Measurement Unit.
+ */
 class IIMU
 {
 public:
@@ -77,49 +79,49 @@ class IIMU
      * @return True if the sensor type was detected succesfully; false otherwise.
      */
     virtual bool init() = 0;
-    
+
     /**
      * Enables all of the inertial sensors with a default configuration.
      */
     virtual void enableDefault() = 0;
-    
+
     /**
      * 	Configures the sensors with settings optimized for turn sensing.
      */
     virtual void configureForTurnSensing() = 0;
 
     /**
      * Takes a reading from the accelerometer and makes the measurements available in a.
-     */ 
+     */
     virtual void readAccelerometer() = 0;
-        
+
     /**
      * Takes a reading from the gyro and makes the measurements available in g.
      */
     virtual void readGyro() = 0;
-        
+
     /**
      * Takes a reading from the magnetometer and makes the measurements available in m.
      */
     virtual void readMagnetometer() = 0;
-        
+
     /**
      * Indicates whether the accelerometer has new measurement data ready.
-     * 
-     * @return True if there is new accelerometer data available; false otherwise. 
+     *
+     * @return True if there is new accelerometer data available; false otherwise.
      */
     virtual bool accelerometerDataReady() = 0;
-        
+
     /**
      * Indicates whether the gyro  has new measurement data ready.
-     * 
+     *
      * @return True if there is new gyro data available; false otherwise.
      */
     virtual bool gyroDataReady() = 0;
-        
+
     /**
      * Indicates whether the magnetometer  has new measurement data ready.
-     * 
+     *
      * @return True if there is new magnetometer data available; false otherwise.
      */
     virtual bool magnetometerDataReady() = 0;
@@ -129,21 +131,21 @@ class IIMU
      *
      * @param[in] accelerationValues  Pointer to IMUData struct.
      */
-    virtual void getAccelerationValues(IMUData* accelerationValues) = 0;
+    virtual const void getAccelerationValues(IMUData* accelerationValues) = 0;
 
     /**
      * Get last raw Gyroscope values as a IMUData struct containing values in x, y and z.
      *
      * @param[in] turnRates  Pointer to IMUData struct.
      */
-    virtual void getTurnRates(IMUData* turnRates) = 0;
+    virtual const void getTurnRates(IMUData* turnRates) = 0;
 
     /**
      * Get last raw Magnetometer values as a IMUData struct containing values in x, y and z.
      *
      * @param[in] magnetometerValues  Pointer to IMUData struct.
      */
-    virtual void getMagnetometerValues(IMUData* magnetometerValues) = 0;
+    virtual const void getMagnetometerValues(IMUData* magnetometerValues) = 0;
 
     /**
      * Calibrate the IMU.

lib/HALSim/Board.cpp

@@ -138,7 +138,7 @@ void Board::init()
     m_keyboard.init();
     m_lineSensors.init();
     m_motors.init();
-    // To Do: TD084	React if IMU initialization fails
+    /*  TODO: TD084	React if IMU initialization fails */
     (void)m_imu.init();
     m_imu.configureForTurnSensing();
 }

lib/HALSim/Board.h

@@ -172,8 +172,8 @@ class Board : public IBoard
     }
 
     /**
-     * Get IMU.
-     *
+     * Get IMU (=Inertial Measurement Unit) driver.
+     * 
      * @return IMU driver
      */
     IIMU& getIMU() final

lib/HALSim/IMU.cpp

@@ -78,9 +78,9 @@ void IMU::readAccelerometer()
         const double* accelerationValues = m_accelerometer->getValues();
         if (nullptr != accelerationValues)
         {
-            m_accelerationValues.valueX = convertFromDoubleToInt16(accelerationValues[0]); /** Index 0 is the x-axis. */
-            m_accelerationValues.valueY = convertFromDoubleToInt16(accelerationValues[1]); /** Index 1 is the y-axis. */
-            m_accelerationValues.valueZ = convertFromDoubleToInt16(accelerationValues[2]); /** Index 2 is the z-axis. */
+            m_accelerationValues.valueX = convertFromDoubleToInt16(accelerationValues[0]); /* Index 0 is the x-axis. */
+            m_accelerationValues.valueY = convertFromDoubleToInt16(accelerationValues[1]); /* Index 1 is the y-axis. */
+            m_accelerationValues.valueZ = convertFromDoubleToInt16(accelerationValues[2]); /* Index 2 is the z-axis. */
         }
     }
 }
@@ -93,9 +93,9 @@ void IMU::readGyro()
 
         if (nullptr != gyroValues)
         {
-            m_gyroValues.valueX = convertFromDoubleToInt16(gyroValues[0]); /** Index 0 is the x-axis. */
-            m_gyroValues.valueY = convertFromDoubleToInt16(gyroValues[1]); /** Index 1 is the y-axis. */
-            m_gyroValues.valueZ = convertFromDoubleToInt16(gyroValues[2]); /** Index 2 is the z-axis. */
+            m_gyroValues.valueX = convertFromDoubleToInt16(gyroValues[0]); /* Index 0 is the x-axis. */
+            m_gyroValues.valueY = convertFromDoubleToInt16(gyroValues[1]); /* Index 1 is the y-axis. */
+            m_gyroValues.valueZ = convertFromDoubleToInt16(gyroValues[2]); /* Index 2 is the z-axis. */
         }
     }
 }
@@ -108,14 +108,14 @@ void IMU::readMagnetometer()
 
         if (nullptr != magnetometerValues)
         {
-            m_magnetometerValues.valueX = convertFromDoubleToInt16(magnetometerValues[0]); /** Index 0 is the x-axis. */
-            m_magnetometerValues.valueY = convertFromDoubleToInt16(magnetometerValues[1]); /** Index 1 is the y-axis. */
-            m_magnetometerValues.valueZ = convertFromDoubleToInt16(magnetometerValues[2]); /** Index 2 is the z-axis. */
+            m_magnetometerValues.valueX = convertFromDoubleToInt16(magnetometerValues[0]); /* Index 0 is the x-axis. */
+            m_magnetometerValues.valueY = convertFromDoubleToInt16(magnetometerValues[1]); /* Index 1 is the y-axis. */
+            m_magnetometerValues.valueZ = convertFromDoubleToInt16(magnetometerValues[2]); /* Index 2 is the z-axis. */
         }
     }
 }
 
-void IMU::getAccelerationValues(IMUData* accelerationValues)
+const void IMU::getAccelerationValues(IMUData* accelerationValues)
 {
     if (nullptr != accelerationValues)
     {
@@ -125,7 +125,7 @@ void IMU::getAccelerationValues(IMUData* accelerationValues)
     }
 }
 
-void IMU::getTurnRates(IMUData* turnRates)
+const void IMU::getTurnRates(IMUData* turnRates)
 {
     if (nullptr != turnRates)
     {
@@ -135,7 +135,7 @@ void IMU::getTurnRates(IMUData* turnRates)
     }
 }
 
-void IMU::getMagnetometerValues(IMUData* magnetometerValues)
+const void IMU::getMagnetometerValues(IMUData* magnetometerValues)
 {
     if (nullptr != magnetometerValues)
     {
@@ -147,22 +147,20 @@ void IMU::getMagnetometerValues(IMUData* magnetometerValues)
 
 void IMU::calibrate()
 {
-    // TODO: implement TD067
+    /* TODO: implement TD067 */
 }
 
 int16_t IMU::convertFromDoubleToInt16(double originalValue)
 {
-    int16_t       convertedValue;
-    const int16_t minimumValue = std::numeric_limits<int16_t>::min();
-    const int16_t maximumValue = std::numeric_limits<int16_t>::max();
+    int16_t convertedValue;
 
-    if (originalValue <= minimumValue)
+    if (originalValue <= INT16_MIN)
     {
-        convertedValue = minimumValue;
+        convertedValue = INT16_MIN;
     }
-    else if (originalValue >= maximumValue)
+    else if (originalValue >= INT16_MAX)
     {
-        convertedValue = maximumValue;
+        convertedValue = INT16_MAX;
     }
     else
     {

lib/HALSim/IMU.h

@@ -72,6 +72,9 @@ class IMU : public IIMU
      */
     IMU(const SimTime& simTime, webots::Accelerometer* accelerometer, webots::Gyro* gyro, webots::Compass* compass) :
         IIMU(),
+        m_accelerationValues{0, 0, 0},
+        m_gyroValues{0, 0, 0},
+        m_magnetometerValues{0, 0, 0},
         m_simTime(simTime),
         m_accelerometer(accelerometer),
         m_gyro(gyro),
@@ -157,21 +160,21 @@ class IMU : public IIMU
      *
      * @param[in] accelerationValues  Pointer to IMUData struct.
      */
-    void getAccelerationValues(IMUData* accelerationValues);
+    void const getAccelerationValues(IMUData* accelerationValues);
 
     /**
      * Get last raw Gyroscope values as a IMUData struct containing values in x, y and z.
      *
      * @param[in] turnRates  Pointer to IMUData struct.
      */
-    void getTurnRates(IMUData* turnRates);
+    void const getTurnRates(IMUData* turnRates);
 
     /**
      * Get last raw Magnetometer values as a IMUData struct containing values in x, y and z.
      *
      * @param[in] magnetometerValues  Pointer to IMUData struct.
      */
-    void getMagnetometerValues(IMUData* magnetometerValues);
+    void const getMagnetometerValues(IMUData* magnetometerValues);
 
     /**
      * Calibrate the IMU.
@@ -188,16 +191,14 @@ class IMU : public IIMU
      */
     int16_t convertFromDoubleToInt16(double originalValue);
 
-private:
-    IMUData m_accelerationValues = {0, 0, 0};
-    IMUData m_gyroValues         = {0, 0, 0};
-    IMUData m_magnetometerValues = {0, 0, 0};
+    IMUData m_accelerationValues; /* Struct to store the current raw accelerometer data. */
+    IMUData m_gyroValues;         /* Struct to store the current gyro data. */
+    IMUData m_magnetometerValues; /* Struct to store the current magnotometer data. */
 
-private:
-    const SimTime&         m_simTime;       /** Simulation time. */
-    webots::Accelerometer* m_accelerometer; /** The accelerometer of Webots. */
-    webots::Gyro*          m_gyro;          /** The gyro of Webots. */
-    webots::Compass*       m_magnetometer;  /** The magnetometer of Webots. */
+    const SimTime&         m_simTime;       /* Simulation time. */
+    webots::Accelerometer* m_accelerometer; /* The accelerometer of Webots. */
+    webots::Gyro*          m_gyro;          /* The gyro of Webots. */
+    webots::Compass*       m_magnetometer;  /* The magnetometer of Webots. */
 };
 
 /******************************************************************************

lib/HALTarget/Board.h

@@ -167,7 +167,7 @@ class Board : public IBoard
     }
 
     /**
-     * Get IMU driver.
+     * Get IMU (=Inertial Measurement Unit) driver.
      * 
      * @return IMU driver
      */