From b11608edf80fbb2b9f7b45278f7d0dd4ace4a2db Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Guillaume=20Lain=C3=A9?=
 <44861207+GuillaumeLaine@users.noreply.github.com>
Date: Thu, 16 May 2024 08:16:20 +0200
Subject: [PATCH] control: reduce default control update frequency (#37)

---
 .../px4_ros2/control/setpoint_types/direct_actuators.hpp     | 2 +-
 .../control/setpoint_types/experimental/attitude.hpp         | 2 +-
 .../px4_ros2/control/setpoint_types/experimental/rates.hpp   | 2 +-
 px4_ros2_cpp/src/components/mode.cpp                         | 5 ++++-
 4 files changed, 7 insertions(+), 4 deletions(-)

diff --git a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/direct_actuators.hpp b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/direct_actuators.hpp
index 807ec845..8543c971 100644
--- a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/direct_actuators.hpp
+++ b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/direct_actuators.hpp
@@ -31,7 +31,7 @@ class DirectActuatorsSetpointType : public SetpointBase
   ~DirectActuatorsSetpointType() override = default;
 
   Configuration getConfiguration() override;
-  float desiredUpdateRateHz() override {return 500.f;}
+  float desiredUpdateRateHz() override {return 200.f;}
 
   /**
    * Send servos setpoint
diff --git a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/attitude.hpp b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/attitude.hpp
index bf1d33be..3b27fbb3 100644
--- a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/attitude.hpp
+++ b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/attitude.hpp
@@ -27,7 +27,7 @@ class AttitudeSetpointType : public SetpointBase
   ~AttitudeSetpointType() override = default;
 
   Configuration getConfiguration() override;
-  float desiredUpdateRateHz() override {return 200.f;}
+  float desiredUpdateRateHz() override {return 100.f;}
 
   void update(
     const Eigen::Quaternionf & attitude_setpoint,
diff --git a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rates.hpp b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rates.hpp
index 6dfcc1d4..e9094d52 100644
--- a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rates.hpp
+++ b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rates.hpp
@@ -27,7 +27,7 @@ class RatesSetpointType : public SetpointBase
   ~RatesSetpointType() override = default;
 
   Configuration getConfiguration() override;
-  float desiredUpdateRateHz() override {return 500.f;}
+  float desiredUpdateRateHz() override {return 200.f;}
 
   void update(
     const Eigen::Vector3f & rate_setpoints_ned_rad,
diff --git a/px4_ros2_cpp/src/components/mode.cpp b/px4_ros2_cpp/src/components/mode.cpp
index 82cb704d..6e2a258e 100644
--- a/px4_ros2_cpp/src/components/mode.cpp
+++ b/px4_ros2_cpp/src/components/mode.cpp
@@ -228,7 +228,10 @@ bool ModeBase::onRegistered()
   // TODO: check setpoint types compatibility with current vehicle type
 
   activateSetpointType(*_setpoint_types[0]);
-  setSetpointUpdateRateFromSetpointTypes();
+  if (_setpoint_update_rate_hz < FLT_EPSILON) {
+    // Do not use default setpoint rate if rate was already set by user
+    setSetpointUpdateRateFromSetpointTypes();
+  }
 
   return true;
 }