diff --git a/rokubimini/include/rokubimini/Rokubimini.hpp b/rokubimini/include/rokubimini/Rokubimini.hpp
index 65af34facfe6f3c511698f9cf079f3cf5eede5c5..ba54db61541c3b3f73a00b7404f8b3af7984b3e5 100644
--- a/rokubimini/include/rokubimini/Rokubimini.hpp
+++ b/rokubimini/include/rokubimini/Rokubimini.hpp
@@ -454,6 +454,17 @@ public:
   */
   virtual bool setSensorCalibration(const calibration::SensorCalibration& sensorCalibration) = 0;
 
+  /**
+   * @fn virtual bool saveConfigParameter()
+   *
+   * @brief Saves the current configuration to the device. It's virtual since it's implementation-specific.
+   *
+   * @return True if the configuration was
+   * successfully saved in the device.
+   *
+  */
+  virtual bool saveConfigParameter() = 0;
+
   /**
    * @fn Reading getReading() const
    *
diff --git a/rokubimini/include/rokubimini/configuration/Configuration.hpp b/rokubimini/include/rokubimini/configuration/Configuration.hpp
index 8eac5ea71723335fbaa0fd72c07a3ccabe3e5a28..9b0f125d26df3dc3d0a50f4fc20d00ea31b2131d 100644
--- a/rokubimini/include/rokubimini/configuration/Configuration.hpp
+++ b/rokubimini/include/rokubimini/configuration/Configuration.hpp
@@ -241,6 +241,24 @@ public:
   */
   uint8_t getImuAngularRateRange() const;
 
+  /**
+   * @fn void setSaveConfiguration(const bool saveConfiguration)
+   *
+   * @brief Sets the \a saveConfiguration variable.
+   * @param saveConfiguration The value to set.
+   *
+  */
+  void setSaveConfiguration(const bool saveConfiguration);
+
+  /**
+   * @fn bool getSaveConfiguration() const
+   *
+   * @brief Gets the \a saveConfiguration variable.
+   * @return The value of \a saveConfiguration to get.
+   *
+  */
+  bool getSaveConfiguration() const;
+
   /**
    * @fn bool hasImuAngularRateRange() const
    *
@@ -358,6 +376,17 @@ public:
 
   bool hasImuAngularRateFilter() const;
 
+  /**
+   * @fn bool hasSaveConfiguration() const
+   *
+   * @brief Checks if the value of the \a saveConfiguration_ variable has been set by the user in the configuration
+   * file.
+   *
+   * @return True If the variable has been set by the user.
+   *
+  */
+
+  bool hasSaveConfiguration() const;
   /**
    * @fn void printConfiguration() const
    *
@@ -534,6 +563,22 @@ protected:
    *
   */
   bool hasForceTorqueOffset_;
+
+  /**
+   * @var bool saveConfiguration_
+   *
+   * @brief The saveConfiguration_ variable.
+   *
+  */
+  bool saveConfiguration_;
+
+  /**
+   * @var bool hasSaveConfiguration_
+   *
+   * @brief Flag indicating if \a saveConfiguration_ is set.
+   *
+  */
+  bool hasSaveConfiguration_;
 };
 
 }  // namespace configuration
diff --git a/rokubimini/src/rokubimini/Rokubimini.cpp b/rokubimini/src/rokubimini/Rokubimini.cpp
index 49e2eb866c03c42b467071eef16f591367c93142..ce7c80095b93262446f9382b7a1fe680b1ce492b 100644
--- a/rokubimini/src/rokubimini/Rokubimini.cpp
+++ b/rokubimini/src/rokubimini/Rokubimini.cpp
@@ -50,6 +50,14 @@ void Rokubimini::startupWithCommunication()
   {
     setSensorCalibration(configuration_.getSensorCalibration());
   }
+
+  if (configuration_.hasSaveConfiguration())
+  {
+    if (configuration_.getSaveConfiguration())
+    {
+      saveConfigParameter();
+    }
+  }
   doStartupWithCommunication();
 }
 void Rokubimini::startupWithoutCommunication()
diff --git a/rokubimini/src/rokubimini/configuration/Configuration.cpp b/rokubimini/src/rokubimini/configuration/Configuration.cpp
index b07d60b8d793db1fcc097398813cec5684b7f2ad..ec7474a1be52aa6af64ff8d0c434d669c0f0314f 100644
--- a/rokubimini/src/rokubimini/configuration/Configuration.cpp
+++ b/rokubimini/src/rokubimini/configuration/Configuration.cpp
@@ -127,6 +127,15 @@ void Configuration::fromFile(const std::string& path)
     {
       hasSensorCalibration_ = false;
     }
+    if (yaml_node.hasKey("save_configuration"))
+    {
+      setSaveConfiguration(yaml_node["save_configuration"].as<bool>());
+      hasSaveConfiguration_ = true;
+    }
+    else
+    {
+      hasSaveConfiguration_ = false;
+    }
   }
 }
 
@@ -134,6 +143,7 @@ void Configuration::printConfiguration() const
 {
   MELO_INFO_STREAM("setReadingToNanOnDisconnect_: " << setReadingToNanOnDisconnect_);
   MELO_INFO_STREAM("useCustomCalibration_: " << useCustomCalibration_);
+  MELO_INFO_STREAM("saveConfiguration_: " << saveConfiguration_);
   MELO_INFO_STREAM("imuAccelerationRange_: " << imuAccelerationRange_);
   MELO_INFO_STREAM("imuAngularRateRange_: " << imuAngularRateRange_);
   MELO_INFO_STREAM("imuAccelerationFilter_: " << imuAccelerationFilter_);
@@ -150,6 +160,7 @@ Configuration& Configuration::operator=(const Configuration& other)
   forceTorqueFilter_ = other.getForceTorqueFilter();
   forceTorqueOffset_ = other.getForceTorqueOffset();
   useCustomCalibration_ = other.getUseCustomCalibration();
+  saveConfiguration_ = other.getSaveConfiguration();
   sensorCalibration_ = other.getSensorCalibration();
   imuAccelerationRange_ = other.imuAccelerationRange_;
   imuAngularRateRange_ = other.imuAngularRateRange_;
@@ -160,6 +171,7 @@ Configuration& Configuration::operator=(const Configuration& other)
   hasForceTorqueFilter_ = other.hasForceTorqueFilter();
   hasForceTorqueOffset_ = other.hasForceTorqueOffset();
   hasUseCustomCalibration_ = other.hasUseCustomCalibration();
+  hasSaveConfiguration_ =  other.hasSaveConfiguration();
   hasSensorCalibration_ = other.hasSensorCalibration();
   hasImuAccelerationRange_ = other.hasImuAccelerationRange();
   hasImuAngularRateRange_ = other.hasImuAngularRateRange();
@@ -204,6 +216,18 @@ bool Configuration::getUseCustomCalibration() const
   return useCustomCalibration_;
 }
 
+void Configuration::setSaveConfiguration(const bool saveConfiguration)
+{
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
+  saveConfiguration_ = saveConfiguration;
+}
+
+bool Configuration::getSaveConfiguration() const
+{
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
+  return saveConfiguration_;
+}
+
 void Configuration::setSetReadingToNanOnDisconnect(const bool setReadingToNanOnDisconnect)
 {
   std::lock_guard<std::recursive_mutex> lock(mutex_);
@@ -324,6 +348,12 @@ bool Configuration::hasUseCustomCalibration() const
   return hasUseCustomCalibration_;
 }
 
+bool Configuration::hasSaveConfiguration() const
+{
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
+  return hasSaveConfiguration_;
+}
+
 bool Configuration::hasSensorCalibration() const
 {
   std::lock_guard<std::recursive_mutex> lock(mutex_);
diff --git a/rokubimini_ethercat/include/rokubimini_ethercat/ObjectDictionary.hpp b/rokubimini_ethercat/include/rokubimini_ethercat/ObjectDictionary.hpp
index bdb56fcfc14bbc727fc4b9da191db343546a4696..705dc9d1e2b343e5587d9d8c211e1b2ccf310cfc 100644
--- a/rokubimini_ethercat/include/rokubimini_ethercat/ObjectDictionary.hpp
+++ b/rokubimini_ethercat/include/rokubimini_ethercat/ObjectDictionary.hpp
@@ -60,6 +60,10 @@
 #define OD_SENSOR_CONFIGURATION_SID_INERTIA_COMPENSATION (uint8_t(0x05))
 #define OD_SENSOR_CONFIGURATION_SID_ORIENTATION_ESTIMATION (uint8_t(0x06))
 
+#define OD_CONTROL_ID (uint16_t(0x8030))
+#define OD_CONTROL_SID_COMMAND (uint8_t(0x01))
+#define OD_CONTROL_SID_STATUS (uint8_t(0x02))
+
 #define OD_SENSOR_CALIBRATION_ID (uint16_t(0x2000))
 #define OD_SENSOR_CALIBRATION_SID_PASSPHRASE (uint8_t(0x01))
 #define OD_SENSOR_CALIBRATION_SID_CALIBRATION_MATRIX (uint8_t(0x02))
diff --git a/rokubimini_ethercat/include/rokubimini_ethercat/RokubiminiEthercat.hpp b/rokubimini_ethercat/include/rokubimini_ethercat/RokubiminiEthercat.hpp
index 55e1aff45331a9390f40ba09a7478dafa674e113..b36c2e6de947a6e743a1e8670a8d3080eab0a030 100644
--- a/rokubimini_ethercat/include/rokubimini_ethercat/RokubiminiEthercat.hpp
+++ b/rokubimini_ethercat/include/rokubimini_ethercat/RokubiminiEthercat.hpp
@@ -259,6 +259,17 @@ public:
 
   bool setSensorCalibration(const calibration::SensorCalibration& sensorCalibration) override;
 
+  /**
+   * @fn bool saveConfigParameter()
+   *
+   * @brief Saves the current configuration to the device.
+   *
+   * @return True if the configuration was
+   * successfully saved in the device.
+   *
+  */
+  bool saveConfigParameter() override;
+
   /**
    * @fn template <typename Value>
   bool sendSdoRead(const uint16_t index, const uint8_t subindex, const bool completeAccess, Value &value)
diff --git a/rokubimini_ethercat/include/rokubimini_ethercat/RokubiminiEthercatSlave.hpp b/rokubimini_ethercat/include/rokubimini_ethercat/RokubiminiEthercatSlave.hpp
index ebfddb9c491aae4eb40a9608ac67129e67751ff7..3a91f69d035ca0cf7eb484160ae854348e7109ae 100644
--- a/rokubimini_ethercat/include/rokubimini_ethercat/RokubiminiEthercatSlave.hpp
+++ b/rokubimini_ethercat/include/rokubimini_ethercat/RokubiminiEthercatSlave.hpp
@@ -297,6 +297,17 @@ public:
 
   bool setSensorCalibration(const calibration::SensorCalibration& sensorCalibration);
 
+  /**
+   * @fn bool saveConfigParameter()
+   *
+   * @brief Saves the current configuration to the device.
+   *
+   * @return True if the configuration was
+   * successfully saved in the device.
+   *
+  */
+  bool saveConfigParameter();
+
   /**
    * @fn void getReading(rokubimini::Reading &reading)
    *
diff --git a/rokubimini_ethercat/src/rokubimini_ethercat/RokubiminiEthercat.cpp b/rokubimini_ethercat/src/rokubimini_ethercat/RokubiminiEthercat.cpp
index 25b27b9e411f463e99e2d84e68777b32e6a0f555..ab55273cabe9590ed68f7109effe14137dfb5393 100644
--- a/rokubimini_ethercat/src/rokubimini_ethercat/RokubiminiEthercat.cpp
+++ b/rokubimini_ethercat/src/rokubimini_ethercat/RokubiminiEthercat.cpp
@@ -127,6 +127,11 @@ bool RokubiminiEthercat::setSensorCalibration(const calibration::SensorCalibrati
   return true;
 }
 
+bool RokubiminiEthercat::saveConfigParameter()
+{
+  return slavePtr_->saveConfigParameter();
+}
+
 bool RokubiminiEthercat::sendSdoReadGeneric(const std::string& indexString, const std::string& subindexString,
                                             const std::string& valueTypeString, std::string& valueString)
 {
diff --git a/rokubimini_ethercat/src/rokubimini_ethercat/RokubiminiEthercatSlave.cpp b/rokubimini_ethercat/src/rokubimini_ethercat/RokubiminiEthercatSlave.cpp
index e6a1835015ccbc05bbefb9ad53c2e1be0c6f6fed..b460165254ffb09303a5c256d24990a80963fa21 100644
--- a/rokubimini_ethercat/src/rokubimini_ethercat/RokubiminiEthercatSlave.cpp
+++ b/rokubimini_ethercat/src/rokubimini_ethercat/RokubiminiEthercatSlave.cpp
@@ -387,6 +387,23 @@ bool RokubiminiEthercatSlave::setSensorCalibration(const calibration::SensorCali
   return success;
 }
 
+bool RokubiminiEthercatSlave::saveConfigParameter()
+{
+  std::lock_guard<std::recursive_mutex> lock(mutex_);
+  MELO_DEBUG("[%s] Saving configuration parameters", name_.c_str());
+  bool success = true;
+  uint8_t retain_configuration = 0x01;
+  success &= sendSdoWrite(OD_CONTROL_ID, OD_CONTROL_SID_COMMAND, false, retain_configuration);
+  uint8_t status;
+  success &= sendSdoRead(OD_CONTROL_ID, OD_CONTROL_SID_STATUS, false, status);
+  if (status != 0)
+  {
+    MELO_ERROR("[%s] Could not save configuration parameters on device. Status value is: %u", name_.c_str(), status);
+    return false;
+  }
+  return success;
+}
+
 void RokubiminiEthercatSlave::getReading(rokubimini::Reading& reading) const
 {
   std::lock_guard<std::recursive_mutex> lock(mutex_);
diff --git a/rokubimini_serial/include/rokubimini_serial/RokubiminiSerial.hpp b/rokubimini_serial/include/rokubimini_serial/RokubiminiSerial.hpp
index 2a039c39ad45d20f5e5d21bb831b8f02adb366ca..17faf3c7e641d2dbb91e7aa7fa070dba1f669a08 100644
--- a/rokubimini_serial/include/rokubimini_serial/RokubiminiSerial.hpp
+++ b/rokubimini_serial/include/rokubimini_serial/RokubiminiSerial.hpp
@@ -321,14 +321,17 @@ public:
   */
   bool setCommunicationSetup(const configuration::SensorConfiguration& sensorConfiguration, const uint8_t& dataFormat,
                              const uint32_t& baudRate);
+  
   /**
-   * @fn bool saveConfig()
+   * @fn bool saveConfigParameter()
    *
-   * @brief Sets the device in run mode.
+   * @brief Saves the current configuration to the device.
+   *
+   * @return True if the configuration was
+   * successfully saved in the device.
    *
-   * @return True if the operation was successful.
   */
-  bool saveConfig();
+  bool saveConfigParameter() override;
 
   /**
    * @fn bool loadConfig()
diff --git a/rokubimini_serial/include/rokubimini_serial/RokubiminiSerialImpl.hpp b/rokubimini_serial/include/rokubimini_serial/RokubiminiSerialImpl.hpp
index b0bce2dd1b277504d4e71fd51b74fad4463bf19f..f9472bc94de039ba626b2717a2e9dd2a3b7e2004 100644
--- a/rokubimini_serial/include/rokubimini_serial/RokubiminiSerialImpl.hpp
+++ b/rokubimini_serial/include/rokubimini_serial/RokubiminiSerialImpl.hpp
@@ -461,13 +461,15 @@ public:
   bool setCommunicationSetup(const configuration::SensorConfiguration& sensorConfiguration, const uint8_t& dataFormat,
                              const uint32_t& baudRate);
   /**
-   * @fn bool saveConfig()
+   * @fn bool saveConfigParameter()
    *
-   * @brief Sets the device in run mode.
+   * @brief Saves the current configuration to the device.
+   *
+   * @return True if the configuration was
+   * successfully saved in the device.
    *
-   * @return True if the operation was successful.
   */
-  bool saveConfig();
+  bool saveConfigParameter();
 
   /**
    * @fn bool loadConfig()
diff --git a/rokubimini_serial/src/rokubimini_serial/RokubiminiSerial.cpp b/rokubimini_serial/src/rokubimini_serial/RokubiminiSerial.cpp
index be5bbdd3dbd7932ea1d5e6cf39e659ae85bf4f33..a0732d9919902b18498447111645d330f168e87d 100644
--- a/rokubimini_serial/src/rokubimini_serial/RokubiminiSerial.cpp
+++ b/rokubimini_serial/src/rokubimini_serial/RokubiminiSerial.cpp
@@ -161,9 +161,9 @@ bool RokubiminiSerial::setCommunicationSetup(const configuration::SensorConfigur
   return implPtr_->setCommunicationSetup(sensorConfiguration, dataFormat, baudRate);
 }
 
-bool RokubiminiSerial::saveConfig()
+bool RokubiminiSerial::saveConfigParameter()
 {
-  return implPtr_->saveConfig();
+  return implPtr_->saveConfigParameter();
 }
 
 bool RokubiminiSerial::loadConfig()
diff --git a/rokubimini_serial/src/rokubimini_serial/RokubiminiSerialImpl.cpp b/rokubimini_serial/src/rokubimini_serial/RokubiminiSerialImpl.cpp
index 2ddb024d2cf390030e571370ccd86011ebd37da1..df66332f4d4484a0000471bdcbfcc7a3c045b475 100644
--- a/rokubimini_serial/src/rokubimini_serial/RokubiminiSerialImpl.cpp
+++ b/rokubimini_serial/src/rokubimini_serial/RokubiminiSerialImpl.cpp
@@ -945,7 +945,7 @@ bool RokubiminiSerialImpl::setCommunicationSetup(const configuration::SensorConf
   return success;
 }
 
-bool RokubiminiSerialImpl::saveConfig()
+bool RokubiminiSerialImpl::saveConfigParameter()
 {
   if (!isInConfigMode())
   {