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()) {